[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") {

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -27,7 +27,6 @@
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
 
 using std::hypot;
-using std::min;
 using std::max;
 using std::abs;
 using namespace nav2_costmap_2d;  // NOLINT
@@ -324,7 +323,11 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin
 
   // If the no pose is not far enough, take the last pose
   if (goal_pose_it == transformed_plan.poses.end()) {
-    goal_pose_it = std::prev(transformed_plan.poses.end());
+    if (params_->project_carrot_past_goal) {
+      return projectCarrotPastGoal(lookahead_dist, transformed_plan);
+    } else {
+      goal_pose_it = std::prev(transformed_plan.poses.end());
+    }
   } else if (goal_pose_it != transformed_plan.poses.begin()) {
     // Find the point on the line segment between the two poses
     // that is exactly the lookahead distance away from the robot pose (the origin)
@@ -345,6 +348,43 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin
   return *goal_pose_it;
 }
 
+geometry_msgs::msg::PoseStamped
+RegulatedPurePursuitController::projectCarrotPastGoal(
+  const double & lookahead_dist, const nav_msgs::msg::Path & transformed_plan)
+{
+  if (transformed_plan.poses.size() == 1) {
+    // We cannot recover the direction of the last segment
+    // Put the lookahead point directly in front of the robot so that we don't
+    // turn
+    auto goal_pose = transformed_plan.poses.back();
+    geometry_msgs::msg::PoseStamped pose;
+    pose.header.frame_id = goal_pose.header.frame_id;
+    pose.header.stamp = goal_pose.header.stamp;
+    pose.pose.position.x =
+      goal_pose.pose.position.x > 0.0 ? lookahead_dist : -lookahead_dist;
+    pose.pose.position.y = 0.0;
+    return pose;
+  }
+
+  // Calculate the direction of the last segment
+  const auto goal_pose = transformed_plan.poses.end()[-1];
+  const auto prev_pose = transformed_plan.poses.end()[-2];
+  const auto dx = goal_pose.pose.position.x - prev_pose.pose.position.x;
+  const auto dy = goal_pose.pose.position.y - prev_pose.pose.position.y;
+  const auto d = std::hypot(dx, dy);
+
+  // Create a carrot the lookahead distance from the goal pose
+  auto pose = goal_pose;
+  pose.pose.position.x += dx / d * lookahead_dist;
+  pose.pose.position.y += dy / d * lookahead_dist;
+
+  // Interpolate as usual
+  auto point = circleSegmentIntersection(
+    prev_pose.pose.position, pose.pose.position, lookahead_dist);
+  pose.pose.position = point;
+  return pose;
+}
+
 void RegulatedPurePursuitController::applyConstraints(
   const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/,
   const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign)
@@ -378,7 +418,63 @@ void RegulatedPurePursuitController::applyConstraints(
 
 void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path)
 {
-  path_handler_->setPlan(path);
+  if (params_->project_carrot_past_goal && path.poses.size() >= 2) {
+    // Insert an additional goal pose extremely close to the goal pose so that
+    // the returned plan has at least two poses to allow projection of the
+    // carrot past the goal pose if requested
+    // We also want to do this at cusp points
+
+    nav_msgs::msg::Path augmented_plan;
+    augmented_plan.header = path.header;
+
+    auto cusp_idx = getIndexOfNextCusp(path, 1);
+    unsigned int last_cusp = 0;
+    while (true) {
+      // Loop until the cusp function returns the index to the goal pose
+      // This might happen on the first loop and we want to make sure we copy
+      // the poses up to that point, so use an infinite loop and break out at
+      // the end
+
+      // Copy from the last copied pose up to the cusp
+      std::copy(
+        path.poses.begin() + last_cusp, path.poses.begin() + cusp_idx,
+        std::back_inserter(augmented_plan.poses));
+      const auto cusp_pose_stamped = path.poses[cusp_idx];
+      const auto prev_pose_stamped = path.poses[cusp_idx - 1];
+      const auto retracted_pos = retractPoint(
+        cusp_pose_stamped.pose.position,
+        prev_pose_stamped.pose.position);
+      auto retracted_pose_stamped{cusp_pose_stamped};
+      retracted_pose_stamped.pose.position = retracted_pos;
+      augmented_plan.poses.push_back(retracted_pose_stamped);
+      last_cusp = cusp_idx;
+      if (cusp_idx == path.poses.size() - 1) {
+        break;
+      }
+      cusp_idx = getIndexOfNextCusp(path, last_cusp + 1);
+    }
+    // Add the goal pose at the end
+    augmented_plan.poses.push_back(path.poses.back());
+    path_handler_->setPlan(augmented_plan);
+  } else {
+    path_handler_->setPlan(path);
+  }
+}
+
+geometry_msgs::msg::Point RegulatedPurePursuitController::retractPoint(
+  const geometry_msgs::msg::Point & origin,
+  const geometry_msgs::msg::Point & towards)
+{
+  auto retracted = origin;
+  auto dx = origin.x - towards.x;
+  auto dy = origin.y - towards.y;
+  auto d = std::hypot(dx, dy);
+  // Retract the point a tiny amount -> 0.001 will mean the points are
+  // indistiguishable to the progress checking/pruning algorithm
+  retracted.x -= 0.001 * dx / d;
+  retracted.y -= 0.001 * dy / d;
+
+  return retracted;
 }
 
 void RegulatedPurePursuitController::setSpeedLimit(
@@ -401,51 +497,62 @@ void RegulatedPurePursuitController::setSpeedLimit(
   }
 }
 
-double RegulatedPurePursuitController::findVelocitySignChange(
-  const nav_msgs::msg::Path & transformed_plan)
+unsigned int RegulatedPurePursuitController::getIndexOfNextCusp(
+  const nav_msgs::msg::Path & transformed_plan,
+  const unsigned int start_index)
 {
-  // Iterating through the transformed global path to determine the position of the cusp
-  for (unsigned int pose_id = 1; pose_id < transformed_plan.poses.size() - 1; ++pose_id) {
-    // We have two vectors for the dot product OA and AB. Determining the vectors.
-    double oa_x = transformed_plan.poses[pose_id].pose.position.x -
-      transformed_plan.poses[pose_id - 1].pose.position.x;
-    double oa_y = transformed_plan.poses[pose_id].pose.position.y -
-      transformed_plan.poses[pose_id - 1].pose.position.y;
-    double ab_x = transformed_plan.poses[pose_id + 1].pose.position.x -
-      transformed_plan.poses[pose_id].pose.position.x;
-    double ab_y = transformed_plan.poses[pose_id + 1].pose.position.y -
-      transformed_plan.poses[pose_id].pose.position.y;
+  // Make sure we don't start earlier than the end of the first segment!
+  for (unsigned int pose_id = std::max(start_index, 1u);
+    pose_id < transformed_plan.poses.size() - 1; ++pose_id)
+  {
+    // We have two vectors for the dot product OA and AB. Determining the
+    // vectors.
+    const auto prev_pos = transformed_plan.poses[pose_id - 1].pose.position;
+    const auto this_pos = transformed_plan.poses[pose_id].pose.position;
+    const auto next_pos = transformed_plan.poses[pose_id + 1].pose.position;
+
+    double oa_x = this_pos.x - prev_pos.x;
+    double oa_y = this_pos.y - prev_pos.y;
+    double ab_x = next_pos.x - this_pos.x;
+    double ab_y = next_pos.y - this_pos.y;
 
     /* Checking for the existance of cusp, in the path, using the dot product
-    and determine it's distance from the robot. If there is no cusp in the path,
-    then just determine the distance to the goal location. */
+    and determine it's index in the path. If there is no cusp in the path,
+    then just return the index of the goal location. */
     const double dot_prod = (oa_x * ab_x) + (oa_y * ab_y);
     if (dot_prod < 0.0) {
-      // returning the distance if there is a cusp
+      // returning the pose index if there is a cusp
       // The transformed path is in the robots frame, so robot is at the origin
-      return hypot(
-        transformed_plan.poses[pose_id].pose.position.x,
-        transformed_plan.poses[pose_id].pose.position.y);
+      return pose_id;
     }
-
-    if (
-      (hypot(oa_x, oa_y) == 0.0 &&
+    if ((hypot(oa_x, oa_y) == 0.0 &&
       transformed_plan.poses[pose_id - 1].pose.orientation !=
       transformed_plan.poses[pose_id].pose.orientation)
       ||
       (hypot(ab_x, ab_y) == 0.0 &&
       transformed_plan.poses[pose_id].pose.orientation !=
       transformed_plan.poses[pose_id + 1].pose.orientation))
     {
-      // returning the distance since the points overlap
+      // returning the pose index since the points overlap
       // but are not simply duplicate points (e.g. in place rotation)
-      return hypot(
-        transformed_plan.poses[pose_id].pose.position.x,
-        transformed_plan.poses[pose_id].pose.position.y);
+      return pose_id;
     }
   }
+  // Return the index of the last point if no cusps found
+  return transformed_plan.poses.size() - 1;
+}
 
-  return std::numeric_limits<double>::max();
+double RegulatedPurePursuitController::findVelocitySignChange(
+  const nav_msgs::msg::Path & transformed_plan)
+{
+  const auto cusp_idx = getIndexOfNextCusp(transformed_plan, 1);
+  if (cusp_idx != transformed_plan.poses.size() - 1) {
+    return hypot(
+      transformed_plan.poses[cusp_idx].pose.position.x,
+      transformed_plan.poses[cusp_idx].pose.position.y);
+  } else {
+    return std::numeric_limits<double>::max();
+  }
 }
 }  // namespace nav2_regulated_pure_pursuit_controller