Add cancel_decelleration to RegulatedPurePursuitController

Signed-off-by: Ramon Wijnands <[email protected]>

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp

@@ -53,6 +53,7 @@ struct Parameters
   double curvature_lookahead_dist;
   bool use_rotate_to_heading;
   double max_angular_accel;
+  double cancel_decelleration;
   double rotate_to_heading_min_angle;
   bool allow_reversing;
   double max_robot_pose_search_dist;

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -96,6 +96,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller
     const geometry_msgs::msg::Twist & velocity,
     nav2_core::GoalChecker * /*goal_checker*/) override;
 
+  bool cancel() override;
+
   /**
    * @brief nav2_core setPlan - Sets the global plan
    * @param path The global plan
@@ -111,6 +113,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    */
   void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
 
+  void reset() override;
+
 protected:
   /**
    * @brief Get lookahead distance
@@ -212,6 +216,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   Parameters * params_;
   double goal_dist_tol_;
   double control_duration_;
+  bool cancelling_ = false;
+  bool finished_cancelling_ = false;
 
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

@@ -85,6 +85,8 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".cancel_decelleration", rclcpp::ParameterValue(1.0));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false));
   declare_parameter_if_not_declared(
@@ -151,6 +153,7 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(
     plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle);
   node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel);
+  node->get_parameter(plugin_name_ + ".cancel_decelleration", params_.cancel_decelleration);
   node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing);
   node->get_parameter(
     plugin_name_ + ".max_robot_pose_search_dist",
@@ -237,6 +240,8 @@ ParameterHandler::dynamicParametersCallback(
         params_.regulated_linear_scaling_min_speed = parameter.as_double();
       } else if (name == plugin_name_ + ".max_angular_accel") {
         params_.max_angular_accel = parameter.as_double();
+      } else if (name == plugin_name_ + ".cancel_decelleration") {
+        params_.cancel_decelleration = parameter.as_double();
       } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") {
         params_.rotate_to_heading_min_angle = parameter.as_double();
       }

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -238,10 +238,37 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
       collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan,
       linear_vel, x_vel_sign);
 
+    if (cancelling_) {
+      const double & dt = control_duration_;
+      auto linear_delta = speed.linear.x - linear_vel;
+      // Make sure we are decelerating with at least the cancel_decelleration
+      if (x_vel_sign > 0.0) {
+        linear_delta = std::max(linear_delta, dt * params_->cancel_decelleration);
+      } else {
+        linear_delta = std::min(linear_delta, -dt * params_->cancel_decelleration);
+      }
+      linear_vel = speed.linear.x - linear_delta;
+
+      if (x_vel_sign > 0) {
+        if (linear_vel < 0) {
+          linear_vel = 0;
+          finished_cancelling_ = true;
+        }
+      } else {
+        if (linear_vel > 0) {
+          linear_vel = 0;
+          finished_cancelling_ = true;
+        }
+      }
+
+      RCLCPP_INFO(logger_, "cancelling to %f", linear_vel);
+    }
+
     // Apply curvature to angular velocity after constraining linear velocity
     angular_vel = linear_vel * regulation_curvature;
   }
 
+
   // Collision checking on this velocity heading
   const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y);
   if (params_->use_collision_detection &&
@@ -258,6 +285,12 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   return cmd_vel;
 }
 
+bool RegulatedPurePursuitController::cancel()
+{
+  cancelling_ = true;
+  return finished_cancelling_;
+}
+
 bool RegulatedPurePursuitController::shouldRotateToPath(
   const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path,
   double & x_vel_sign)
@@ -445,6 +478,11 @@ void RegulatedPurePursuitController::setSpeedLimit(
   }
 }
 
+void RegulatedPurePursuitController::reset()
+{
+  cancelling_ = false;
+}
+
 double RegulatedPurePursuitController::findVelocitySignChange(
   const nav_msgs::msg::Path & transformed_plan)
 {