RPP Dexory tweaks (ros-navigation#4140)

* RPP Dexory tweaks

Signed-off-by: Guillaume Doisy <[email protected]>

* Update nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

Signed-off-by: Steve Macenski <[email protected]>

* Update nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

Signed-off-by: Steve Macenski <[email protected]>

* projectCarrotPastGoal test

Signed-off-by: Guillaume Doisy <[email protected]>

* Use circleSegmentIntersection when projecting past end of path

This guarantees that the look ahead distance is maintained

Signed-off-by: James Ward <[email protected]>

* lint

Signed-off-by: Guillaume Doisy <[email protected]>

---------

Signed-off-by: Guillaume Doisy <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: James Ward <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
Co-authored-by: James Ward <[email protected]>

nav2_regulated_pure_pursuit_controller/CMakeLists.txt

@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_regulated_pure_pursuit_controller)
 
 find_package(ament_cmake REQUIRED)
+find_package(nav2_amcl REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
@@ -26,6 +27,7 @@ set(dependencies
   nav2_costmap_2d
   pluginlib
   nav_msgs
+  nav2_amcl
   nav2_util
   nav2_core
   tf2

nav2_regulated_pure_pursuit_controller/README.md

@@ -90,6 +90,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin
 | `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | 
 | `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | 
 | `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | 
+| `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvate calculation (to avoid oscilaltions at the end of the path) | 
 
 Example fully-described XML with default parameter values:
 
@@ -137,6 +138,7 @@ controller_server:
       rotate_to_heading_min_angle: 0.785
       max_angular_accel: 3.2
       max_robot_pose_search_dist: 10.0
+      interpolate_curvature_after_goal: false
       cost_scaling_dist: 0.3
       cost_scaling_gain: 1.0
       inflation_cost_scaling_factor: 3.0

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp

@@ -56,6 +56,7 @@ struct Parameters
   double rotate_to_heading_min_angle;
   bool allow_reversing;
   double max_robot_pose_search_dist;
+  bool interpolate_curvature_after_goal;
   bool use_collision_detection;
   double transform_tolerance;
 };

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp

@@ -58,11 +58,12 @@ class PathHandler
    * - Outside the local_costmap (collision avoidance cannot be assured)
    * @param pose pose to transform
    * @param max_robot_pose_search_dist Distance to search for matching nearest path point
+   * @param reject_unit_path If true, fail if path has only one pose
    * @return Path in new frame
    */
   nav_msgs::msg::Path transformGlobalPlan(
     const geometry_msgs::msg::PoseStamped & pose,
-    double max_robot_pose_search_dist);
+    double max_robot_pose_search_dist, bool reject_unit_path = false);
 
   /**
    * @brief Transform a pose to another frame.

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -131,10 +131,12 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    * @brief Whether robot should rotate to rough path heading
    * @param carrot_pose current lookahead point
    * @param angle_to_path Angle of robot output relatie to carrot marker
+   * @param x_vel_sign Velocoty sign (forward or backward)
    * @return Whether should rotate to path heading
    */
   bool shouldRotateToPath(
-    const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path);
+    const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path,
+    double & x_vel_sign);
 
   /**
    * @brief Whether robot should rotate to final goal orientation
@@ -185,9 +187,13 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    * @brief Get lookahead point
    * @param lookahead_dist Optimal lookahead distance
    * @param path Current global path
+   * @param interpolate_after_goal If true, interpolate the lookahead point after the goal based
+   * on the orientation given by the position of the last two pose of the path
    * @return Lookahead point
    */
-  geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &);
+  geometry_msgs::msg::PoseStamped getLookAheadPoint(
+    const double &, const nav_msgs::msg::Path &,
+    bool interpolate_after_goal = false);
 
   /**
    * @brief checks for the cusp position
@@ -210,6 +216,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
   carrot_pub_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
+  curvature_carrot_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> carrot_arc_pub_;
   std::unique_ptr<nav2_regulated_pure_pursuit_controller::PathHandler> path_handler_;
   std::unique_ptr<nav2_regulated_pure_pursuit_controller::ParameterHandler> param_handler_;

nav2_regulated_pure_pursuit_controller/package.xml

@@ -10,6 +10,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>nav2_amcl</depend>
   <depend>nav2_common</depend>
   <depend>nav2_core</depend>
   <depend>nav2_util</depend>

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

@@ -90,6 +90,9 @@ ParameterHandler::ParameterHandler(
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".max_robot_pose_search_dist",
     rclcpp::ParameterValue(costmap_size_x / 2.0));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".interpolate_curvature_after_goal",
+    rclcpp::ParameterValue(false));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".use_collision_detection",
     rclcpp::ParameterValue(true));
@@ -159,6 +162,15 @@ ParameterHandler::ParameterHandler(
     params_.max_robot_pose_search_dist = std::numeric_limits<double>::max();
   }
 
+  node->get_parameter(
+    plugin_name_ + ".interpolate_curvature_after_goal",
+    params_.interpolate_curvature_after_goal);
+  if (!params_.use_fixed_curvature_lookahead && params_.interpolate_curvature_after_goal) {
+    RCLCPP_WARN(
+      logger_, "For interpolate_curvature_after_goal to be set to true, "
+      "use_fixed_curvature_lookahead should be true, it is currently set to false. Disabling.");
+    params_.interpolate_curvature_after_goal = false;
+  }
   node->get_parameter(
     plugin_name_ + ".use_collision_detection",
     params_.use_collision_detection);
@@ -170,16 +182,6 @@ ParameterHandler::ParameterHandler(
     params_.use_cost_regulated_linear_velocity_scaling = false;
   }
 
-  /** Possible to drive in reverse direction if and only if
-   "use_rotate_to_heading" parameter is set to false **/
-
-  if (params_.use_rotate_to_heading && params_.allow_reversing) {
-    RCLCPP_WARN(
-      logger_, "Disabling reversing. Both use_rotate_to_heading and allow_reversing "
-      "parameter cannot be set to true. By default setting use_rotate_to_heading true");
-    params_.allow_reversing = false;
-  }
-
   dyn_params_handler_ = node->add_on_set_parameters_callback(
     std::bind(
       &ParameterHandler::dynamicParametersCallback,
@@ -250,12 +252,6 @@ ParameterHandler::dynamicParametersCallback(
       } else if (name == plugin_name_ + ".use_collision_detection") {
         params_.use_collision_detection = parameter.as_bool();
       } else if (name == plugin_name_ + ".use_rotate_to_heading") {
-        if (parameter.as_bool() && params_.allow_reversing) {
-          RCLCPP_WARN(
-            logger_, "Both use_rotate_to_heading and allow_reversing "
-            "parameter cannot be set to true. Rejecting parameter update.");
-          continue;
-        }
         params_.use_rotate_to_heading = parameter.as_bool();
       } else if (name == plugin_name_ + ".allow_reversing") {
         if (params_.use_rotate_to_heading && parameter.as_bool()) {

nav2_regulated_pure_pursuit_controller/src/path_handler.cpp

@@ -47,12 +47,17 @@ double PathHandler::getCostmapMaxExtent() const
 
 nav_msgs::msg::Path PathHandler::transformGlobalPlan(
   const geometry_msgs::msg::PoseStamped & pose,
-  double max_robot_pose_search_dist)
+  double max_robot_pose_search_dist,
+  bool reject_unit_path)
 {
   if (global_plan_.poses.empty()) {
     throw nav2_core::InvalidPath("Received plan with zero length");
   }
 
+  if (reject_unit_path && global_plan_.poses.size() == 1) {
+    throw nav2_core::InvalidPath("Received plan with length of one");
+  }
+
   // let's get the pose of the robot in the frame of the plan
   geometry_msgs::msg::PoseStamped robot_pose;
   if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) {
@@ -73,6 +78,15 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan(
       return euclidean_distance(robot_pose, ps);
     });
 
+  // Make sure we always have at least 2 points on the transformed plan and that we don't prune
+  // the global plan below 2 points in order to have always enough point to interpolate the
+  // end of path direction
+  if (global_plan_.poses.begin() != closest_pose_upper_bound && global_plan_.poses.size() > 1 &&
+    transformation_begin == std::prev(closest_pose_upper_bound))
+  {
+    transformation_begin = std::prev(std::prev(closest_pose_upper_bound));
+  }
+
   // We'll discard points on the plan that are outside the local costmap
   const double max_costmap_extent = getCostmapMaxExtent();
   auto transformation_end = std::find_if(

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -20,6 +20,7 @@
 #include <vector>
 #include <utility>
 
+#include "nav2_amcl/angleutils.hpp"
 #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp"
 #include "nav2_core/controller_exceptions.hpp"
 #include "nav2_util/node_utils.hpp"
@@ -73,6 +74,8 @@ void RegulatedPurePursuitController::configure(
 
   global_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("received_global_plan", 1);
   carrot_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>("lookahead_point", 1);
+  curvature_carrot_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>(
+    "curvature_lookahead_point", 1);
 }
 
 void RegulatedPurePursuitController::cleanup()
@@ -84,6 +87,7 @@ void RegulatedPurePursuitController::cleanup()
     plugin_name_.c_str());
   global_path_pub_.reset();
   carrot_pub_.reset();
+  curvature_carrot_pub_.reset();
 }
 
 void RegulatedPurePursuitController::activate()
@@ -95,6 +99,7 @@ void RegulatedPurePursuitController::activate()
     plugin_name_.c_str());
   global_path_pub_->on_activate();
   carrot_pub_->on_activate();
+  curvature_carrot_pub_->on_activate();
 }
 
 void RegulatedPurePursuitController::deactivate()
@@ -106,6 +111,7 @@ void RegulatedPurePursuitController::deactivate()
     plugin_name_.c_str());
   global_path_pub_->on_deactivate();
   carrot_pub_->on_deactivate();
+  curvature_carrot_pub_->on_deactivate();
 }
 
 std::unique_ptr<geometry_msgs::msg::PointStamped> RegulatedPurePursuitController::createCarrotMsg(
@@ -171,7 +177,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 
   // Transform path to robot base frame
   auto transformed_plan = path_handler_->transformGlobalPlan(
-    pose, params_->max_robot_pose_search_dist);
+    pose, params_->max_robot_pose_search_dist, params_->interpolate_curvature_after_goal);
   global_path_pub_->publish(transformed_plan);
 
   // Find look ahead distance and point on path and publish
@@ -190,6 +196,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 
   // Get the particular point on the path at the lookahead distance
   auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan);
+  auto rotate_to_path_carrot_pose = carrot_pose;
   carrot_pub_->publish(createCarrotMsg(carrot_pose));
 
   double linear_vel, angular_vel;
@@ -200,33 +207,39 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   if (params_->use_fixed_curvature_lookahead) {
     auto curvature_lookahead_pose = getLookAheadPoint(
       params_->curvature_lookahead_dist,
-      transformed_plan);
+      transformed_plan, params_->interpolate_curvature_after_goal);
+    rotate_to_path_carrot_pose = curvature_lookahead_pose;
     regulation_curvature = calculateCurvature(curvature_lookahead_pose.pose.position);
+    curvature_carrot_pub_->publish(createCarrotMsg(curvature_lookahead_pose));
   }
 
   // Setting the velocity direction
-  double sign = 1.0;
+  double x_vel_sign = 1.0;
   if (params_->allow_reversing) {
-    sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0;
+    x_vel_sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0;
   }
 
   linear_vel = params_->desired_linear_vel;
 
   // Make sure we're in compliance with basic constraints
+  // For shouldRotateToPath, using x_vel_sign in order to support allow_reversing
+  // and rotate_to_path_carrot_pose for the direction carrot pose:
+  //        - equal to "normal" carrot_pose when curvature_lookahead_pose = false
+  //        - otherwise equal to curvature_lookahead_pose (which can be interpolated after goal)
   double angle_to_heading;
   if (shouldRotateToGoalHeading(carrot_pose)) {
     double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation);
     rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed);
-  } else if (shouldRotateToPath(carrot_pose, angle_to_heading)) {
+  } else if (shouldRotateToPath(rotate_to_path_carrot_pose, angle_to_heading, x_vel_sign)) {
     rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed);
   } else {
     applyConstraints(
       regulation_curvature, speed,
       collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan,
-      linear_vel, sign);
+      linear_vel, x_vel_sign);
 
     // Apply curvature to angular velocity after constraining linear velocity
-    angular_vel = linear_vel * lookahead_curvature;
+    angular_vel = linear_vel * regulation_curvature;
   }
 
   // Collision checking on this velocity heading
@@ -246,10 +259,15 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 }
 
 bool RegulatedPurePursuitController::shouldRotateToPath(
-  const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path)
+  const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path,
+  double & x_vel_sign)
 {
   // Whether we should rotate robot to rough path heading
   angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x);
+  // In case we are reversing
+  if (x_vel_sign < 0.0) {
+    angle_to_path = nav2_amcl::angleutils::normalize(angle_to_path + M_PI);
+  }
   return params_->use_rotate_to_heading &&
          fabs(angle_to_path) > params_->rotate_to_heading_min_angle;
 }
@@ -314,7 +332,8 @@ geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersect
 
 geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint(
   const double & lookahead_dist,
-  const nav_msgs::msg::Path & transformed_plan)
+  const nav_msgs::msg::Path & transformed_plan,
+  bool interpolate_after_goal)
 {
   // Find the first pose which is at a distance greater than the lookahead distance
   auto goal_pose_it = std::find_if(
@@ -324,7 +343,32 @@ 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 (interpolate_after_goal) {
+      auto last_pose_it = std::prev(transformed_plan.poses.end());
+      auto prev_last_pose_it = std::prev(last_pose_it);
+
+      double end_path_orientation = atan2(
+        last_pose_it->pose.position.y - prev_last_pose_it->pose.position.y,
+        last_pose_it->pose.position.x - prev_last_pose_it->pose.position.x);
+
+      // Project the last segment out to guarantee it is beyond the look ahead
+      // distance
+      auto projected_position = last_pose_it->pose.position;
+      projected_position.x += cos(end_path_orientation) * lookahead_dist;
+      projected_position.y += sin(end_path_orientation) * lookahead_dist;
+
+      // Use the circle intersection to find the position at the correct look
+      // ahead distance
+      const auto interpolated_position = circleSegmentIntersection(
+        last_pose_it->pose.position, projected_position, lookahead_dist);
+
+      geometry_msgs::msg::PoseStamped interpolated_pose;
+      interpolated_pose.header = last_pose_it->header;
+      interpolated_pose.pose.position = interpolated_position;
+      return interpolated_pose;
+    } 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)