Merge branch 'main' into feat/optional-publish-zero-velocity

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

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

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"

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

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -272,6 +272,7 @@ class ControllerServer : public nav2_util::LifecycleNode
   double failure_tolerance_;
   bool use_realtime_priority_;
   bool publish_zero_velocity_;
+  rclcpp::Duration costmap_update_timeout_;
 
   // Whether we've published the single controller warning yet
   geometry_msgs::msg::PoseStamped end_pose_;

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");
 
@@ -64,6 +65,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("publish_zero_velocity", rclcpp::ParameterValue(true));
+  declare_parameter("costmap_update_timeout", 0.30);  // 300ms
 
   // The costmap node is used in the implementation of the controller
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
@@ -224,6 +226,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 {
@@ -482,7 +488,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();
       }
 
@@ -545,6 +555,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<Action::Result> result = std::make_shared<Action::Result>();
+    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());
     onGoalExit();

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_

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.

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<double> 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);

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

nav2_planner/include/nav2_planner/planner_server.hpp

@@ -242,6 +242,7 @@ class PlannerServer : public nav2_util::LifecycleNode
   std::vector<std::string> planner_ids_;
   std::vector<std::string> planner_types_;
   double max_planner_duration_;
+  rclcpp::Duration costmap_update_timeout_;
   std::string planner_ids_concat_;
 
   // TF buffer

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<ActionServerToPose>(
     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();
   }
 }