[RPP] Project carrot beyond end of path if interpolating

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp

@@ -34,6 +34,7 @@ struct Parameters
 {
   double desired_linear_vel, base_desired_linear_vel;
   double lookahead_dist;
+  bool project_carrot_past_goal;
   double rotate_to_heading_angular_vel;
   double max_lookahead_dist;
   double min_lookahead_dist;

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -191,11 +191,42 @@ class RegulatedPurePursuitController : public nav2_core::Controller
 
   /**
    * @brief checks for the cusp position
-   * @param pose Pose input to determine the cusp position
-   * @return robot distance from the cusp
+   * @param transformed_plan Transformed plan to search for the velocity sign change
+   * @return robot distance from the velocity sign change
    */
   double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan);
 
+/**
+   * @brief Gets the index of the next cusp position
+   * @param transformed_plan Path to search for the cusp position
+   * @param start_index Index of the pose in the path to begin the search
+   * @return index of the pose of the cusp, or last pose if no cusp found
+   */
+  unsigned int getIndexOfNextCusp(
+    const nav_msgs::msg::Path & transformed_plan,
+    const unsigned int start_index);
+
+  /**
+   * @brief Finds the position a very small distance from the initial point in the direction of the target point
+   * @param origin Position of the origin
+   * @param towards Position of the target to retract towards
+   * @return position of the retracted point
+   */
+  geometry_msgs::msg::Point retractPoint(
+    const geometry_msgs::msg::Point & origin,
+    const geometry_msgs::msg::Point & towards);
+
+  /**
+   * @brief Project the carrot past the end of the path to maintain the lookahead distance
+   * @param lookahead_dist Lookahead distance
+   * @param transformed_plan Path to project carrot off the end of
+   * @return pose of the lookahead point
+   */
+  geometry_msgs::msg::PoseStamped projectCarrotPastGoal(
+    const double & lookahead_dist,
+    const nav_msgs::msg::Path & transformed_plan);
+
+
   rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::string plugin_name_;

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

@@ -39,6 +39,8 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".project_carrot_past_goal", rclcpp::ParameterValue(false));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3));
   declare_parameter_if_not_declared(
@@ -97,7 +99,13 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel);
   params_.base_desired_linear_vel = params_.desired_linear_vel;
   node->get_parameter(plugin_name_ + ".lookahead_dist", params_.lookahead_dist);
+  node->get_parameter(plugin_name_ + ".project_carrot_past_goal", params_.project_carrot_past_goal);
   node->get_parameter(plugin_name_ + ".min_lookahead_dist", params_.min_lookahead_dist);
+  if (params_.min_lookahead_dist > 0.0 && !params_.project_carrot_past_goal) {
+    RCLCPP_WARN(
+      logger_, "min_lookahead_dist is non-zero but project_carrot_past_goal is not set "
+      "- min_lookahead_dist will not be respected at end of the path");
+  }
   node->get_parameter(plugin_name_ + ".max_lookahead_dist", params_.max_lookahead_dist);
   node->get_parameter(plugin_name_ + ".lookahead_time", params_.lookahead_time);
   node->get_parameter(
@@ -241,6 +249,8 @@ ParameterHandler::dynamicParametersCallback(
     } else if (type == ParameterType::PARAMETER_BOOL) {
       if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") {
         params_.use_velocity_scaled_lookahead_dist = parameter.as_bool();
+      } else if (name == plugin_name_ + ".project_carrot_past_goal") {
+        params_.project_carrot_past_goal = parameter.as_bool();
       } else if (name == plugin_name_ + ".use_regulated_linear_velocity_scaling") {
         params_.use_regulated_linear_velocity_scaling = parameter.as_bool();
       } else if (name == plugin_name_ + ".use_fixed_curvature_lookahead") {