From 271d2d60e65167a960f6935d1f3cc7b4d2b0b576 Mon Sep 17 00:00:00 2001 From: Huiyu Leong <26198479+huiyulhy@users.noreply.github.com> Date: Thu, 12 Sep 2024 08:28:07 -0700 Subject: [PATCH 1/2] [DWB] Option to limit velocity commands in trajectory generator (#4663) * Option to limit vel cmd through traj generator Signed-off-by: huiyulhy * Cleanup Signed-off-by: huiyulhy * fix linting Signed-off-by: huiyulhy * Update linting Signed-off-by: huiyulhy * uncrustify Signed-off-by: huiyulhy * uncrustify Signed-off-by: huiyulhy --------- Signed-off-by: huiyulhy --- .../include/dwb_plugins/standard_traj_generator.hpp | 3 +++ .../dwb_plugins/src/standard_traj_generator.cpp | 10 ++++++++++ 2 files changed, 13 insertions(+) diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp index 87ba77baf0..db5467cd48 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp @@ -147,6 +147,9 @@ class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator /// @brief the name of the overlying plugin ID std::string plugin_name_; + /// @brief Option to limit velocity in the trajectory generator by using current velocity + bool limit_vel_cmd_in_traj_; + /* Backwards Compatibility Parameter: include_last_point * * dwa had an off-by-one error built into it. diff --git a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp index 49a62de302..3b57df08e1 100644 --- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp @@ -75,6 +75,10 @@ void StandardTrajectoryGenerator::initialize( nh, plugin_name + ".include_last_point", rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + nh, + plugin_name + ".limit_vel_cmd_in_traj", rclcpp::ParameterValue(false)); + /* * If discretize_by_time, then sim_granularity represents the amount of time that should be between * two successive points on the trajectory. @@ -89,6 +93,7 @@ void StandardTrajectoryGenerator::initialize( nh->get_parameter(plugin_name + ".linear_granularity", linear_granularity_); nh->get_parameter(plugin_name + ".angular_granularity", angular_granularity_); nh->get_parameter(plugin_name + ".include_last_point", include_last_point_); + nh->get_parameter(plugin_name + ".limit_vel_cmd_in_traj", limit_vel_cmd_in_traj_); } void StandardTrajectoryGenerator::initializeIterator( @@ -156,9 +161,14 @@ dwb_msgs::msg::Trajectory2D StandardTrajectoryGenerator::generateTrajectory( double running_time = 0.0; std::vector steps = getTimeSteps(cmd_vel); traj.poses.push_back(start_pose); + bool first_vel = false; for (double dt : steps) { // calculate velocities vel = computeNewVelocity(cmd_vel, vel, dt); + if (!first_vel && limit_vel_cmd_in_traj_) { + traj.velocity = vel; + first_vel = true; + } // update the position of the robot using the velocities passed in pose = computeNewPosition(pose, vel, dt); From b71cccd8c47d4cb472db1c3a509af3821336933a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 12 Sep 2024 11:08:18 -0700 Subject: [PATCH 2/2] Adding planner server timeout for costmap waiting (#4673) * Adding planner server timeout for costmap waiting Signed-off-by: Steve Macenski * Adding controller server's costmap timeout as well Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- .../params/nav2_multirobot_params_1.yaml | 2 ++ .../params/nav2_multirobot_params_2.yaml | 2 ++ .../params/nav2_multirobot_params_all.yaml | 2 ++ nav2_bringup/params/nav2_params.yaml | 2 ++ .../nav2_controller/controller_server.hpp | 1 + nav2_controller/src/controller_server.cpp | 19 ++++++++++++++++++- .../nav2_core/controller_exceptions.hpp | 7 +++++++ nav2_msgs/action/FollowPath.action | 1 + .../include/nav2_planner/planner_server.hpp | 1 + nav2_planner/src/planner_server.cpp | 10 ++++++++++ 10 files changed, 46 insertions(+), 1 deletion(-) diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 493c53fc2b..319ebd41ac 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -68,6 +68,7 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -198,6 +199,7 @@ map_server: planner_server: ros__parameters: planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 605c86c66b..86e2b78e5e 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -68,6 +68,7 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -197,6 +198,7 @@ map_server: planner_server: ros__parameters: planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index 4bde87b47d..b6b27887a3 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -68,6 +68,7 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -230,6 +231,7 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 + costmap_update_timeout: 1.0 planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 2581635e7f..2027f4c0e1 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -68,6 +68,7 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -276,6 +277,7 @@ planner_server: ros__parameters: expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 0f250dee14..e2d3fd497a 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -267,6 +267,7 @@ class ControllerServer : public nav2_util::LifecycleNode double failure_tolerance_; bool use_realtime_priority_; + rclcpp::Duration costmap_update_timeout_; // Whether we've published the single controller warning yet geometry_msgs::msg::PoseStamped end_pose_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 8ea2d9cfb1..a990c3a400 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -44,7 +44,8 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"}, lp_loader_("nav2_core", "nav2_core::Controller"), default_ids_{"FollowPath"}, - default_types_{"dwb_core::DWBLocalPlanner"} + default_types_{"dwb_core::DWBLocalPlanner"}, + costmap_update_timeout_(300ms) { RCLCPP_INFO(get_logger(), "Creating controller server"); @@ -63,6 +64,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0)); declare_parameter("use_realtime_priority", rclcpp::ParameterValue(false)); + declare_parameter("costmap_update_timeout", 0.30); // 300ms // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( @@ -222,6 +224,10 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + double costmap_update_timeout_dbl; + get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); + costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); + // Create the action server that we implement with our followPath method // This may throw due to real-time prioritzation if user doesn't have real-time permissions try { @@ -480,7 +486,11 @@ void ControllerServer::computeControl() // Don't compute a trajectory until costmap is valid (after clear costmap) rclcpp::Rate r(100); + auto waiting_start = now(); while (!costmap_ros_->isCurrent()) { + if (now() - waiting_start > costmap_update_timeout_) { + throw nav2_core::ControllerTimedOut("Costmap timed out waiting for update"); + } r.sleep(); } @@ -543,6 +553,13 @@ void ControllerServer::computeControl() result->error_code = Action::Result::INVALID_PATH; action_server_->terminate_current(result); return; + } catch (nav2_core::ControllerTimedOut & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Result::CONTROLLER_TIMED_OUT; + action_server_->terminate_current(result); + return; } catch (nav2_core::ControllerException & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); diff --git a/nav2_core/include/nav2_core/controller_exceptions.hpp b/nav2_core/include/nav2_core/controller_exceptions.hpp index 5f87c4c287..2d75836fa5 100644 --- a/nav2_core/include/nav2_core/controller_exceptions.hpp +++ b/nav2_core/include/nav2_core/controller_exceptions.hpp @@ -69,6 +69,13 @@ class NoValidControl : public ControllerException : ControllerException(description) {} }; +class ControllerTimedOut : public ControllerException +{ +public: + explicit ControllerTimedOut(const std::string & description) + : ControllerException(description) {} +}; + } // namespace nav2_core #endif // NAV2_CORE__CONTROLLER_EXCEPTIONS_HPP_ diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 49f04e2139..80b0a1d4ef 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -16,6 +16,7 @@ uint16 INVALID_PATH=103 uint16 PATIENCE_EXCEEDED=104 uint16 FAILED_TO_MAKE_PROGRESS=105 uint16 NO_VALID_CONTROL=106 +uint16 CONTROLLER_TIMED_OUT=107 std_msgs/Empty result uint16 error_code diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 12e364c710..2810a89b06 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -242,6 +242,7 @@ class PlannerServer : public nav2_util::LifecycleNode std::vector planner_ids_; std::vector planner_types_; double max_planner_duration_; + rclcpp::Duration costmap_update_timeout_; std::string planner_ids_concat_; // TF buffer diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index d572360fc9..bb59b0b023 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -44,6 +44,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) gp_loader_("nav2_core", "nav2_core::GlobalPlanner"), default_ids_{"GridBased"}, default_types_{"nav2_navfn_planner::NavfnPlanner"}, + costmap_update_timeout_(1s), costmap_(nullptr) { RCLCPP_INFO(get_logger(), "Creating"); @@ -52,6 +53,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) declare_parameter("planner_plugins", default_ids_); declare_parameter("expected_planner_frequency", 1.0); declare_parameter("action_server_result_timeout", 10.0); + declare_parameter("costmap_update_timeout", 1.0); get_parameter("planner_plugins", planner_ids_); if (planner_ids_ == default_ids_) { @@ -150,6 +152,10 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + double costmap_update_timeout_dbl; + get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); + costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); + // Create the action servers for path planning to a pose and through poses action_server_pose_ = std::make_unique( shared_from_this(), @@ -283,7 +289,11 @@ void PlannerServer::waitForCostmap() { // Don't compute a plan until costmap is valid (after clear costmap) rclcpp::Rate r(100); + auto waiting_start = now(); while (!costmap_ros_->isCurrent()) { + if (now() - waiting_start > costmap_update_timeout_) { + throw nav2_core::PlannerTimedOut("Costmap timed out waiting for update"); + } r.sleep(); } }