[Draft] [nav2_controller] Add controller_patience param

nav2_bringup/bringup/params/nav2_params.yaml

@@ -93,6 +93,7 @@ controller_server:
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
     progress_checker_plugin: "progress_checker"
     goal_checker_plugin: "goal_checker"
     controller_plugins: ["FollowPath"]

nav2_controller/include/nav2_controller/nav2_controller.hpp

@@ -229,9 +229,14 @@ class ControllerServer : public nav2_util::LifecycleNode
   double min_y_velocity_threshold_;
   double min_theta_velocity_threshold_;
 
+  double failure_tolerance_;
+
   // Whether we've published the single controller warning yet
   geometry_msgs::msg::Pose end_pose_;
 
+  // Last time the controller generated a valid command
+  rclcpp::Time last_valid_cmd_time_;
+
 private:
   /**
     * @brief Callback for speed limiting messages

nav2_controller/src/nav2_controller.cpp

@@ -55,6 +55,8 @@ ControllerServer::ControllerServer()
 
   declare_parameter("speed_limit_topic", rclcpp::ParameterValue("speed_limit"));
 
+  declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0));
+
   // The costmap node is used in the implementation of the controller
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "local_costmap", std::string{get_namespace()}, "local_costmap");
@@ -109,6 +111,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
 
   std::string speed_limit_topic;
   get_parameter("speed_limit_topic", speed_limit_topic);
+  get_parameter("failure_tolerance", failure_tolerance_);
 
   costmap_ros_->on_configure(state);
 
@@ -297,6 +300,7 @@ void ControllerServer::computeControl()
     setPlannerPath(action_server_->get_current_goal()->path);
     progress_checker_->reset();
 
+    last_valid_cmd_time_ = now();
     rclcpp::WallRate loop_rate(controller_frequency_);
     while (rclcpp::ok()) {
       if (action_server_ == nullptr || !action_server_->is_server_active()) {
@@ -385,11 +389,35 @@ void ControllerServer::computeAndPublishVelocity()
 
   nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());
 
-  auto cmd_vel_2d =
-    controllers_[current_controller_]->computeVelocityCommands(
-    pose,
-    nav_2d_utils::twist2Dto3D(twist),
-    goal_checker_.get());
+  geometry_msgs::msg::TwistStamped cmd_vel_2d;
+
+  try {
+    cmd_vel_2d =
+      controllers_[current_controller_]->computeVelocityCommands(
+      pose,
+      nav_2d_utils::twist2Dto3D(twist),
+      goal_checker_.get());
+    last_valid_cmd_time_ = now();
+  } catch (nav2_core::PlannerException & e) {
+    if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) {
+      RCLCPP_WARN(this->get_logger(), e.what());
+      cmd_vel_2d.twist.angular.x = 0;
+      cmd_vel_2d.twist.angular.y = 0;
+      cmd_vel_2d.twist.angular.z = 0;
+      cmd_vel_2d.twist.linear.x = 0;
+      cmd_vel_2d.twist.linear.y = 0;
+      cmd_vel_2d.twist.linear.z = 0;
+      cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID();
+      cmd_vel_2d.header.stamp = now();
+      if ((now() - last_valid_cmd_time_).seconds() > failure_tolerance_ &&
+        failure_tolerance_ != -1.0)
+      {
+        throw nav2_core::PlannerException("Controller patience exceeded");
+      }
+    } else {
+      throw nav2_core::PlannerException(e.what());
+    }
+  }
 
   std::shared_ptr<Action::Feedback> feedback = std::make_shared<Action::Feedback>();
   feedback->speed = std::hypot(cmd_vel_2d.twist.linear.x, cmd_vel_2d.twist.linear.y);