Feature addition: capability for the RRP to drive the robot backwards (#2443)

* Feature addition: capability for the RRP to drive the robot backwards

* findDirectionChange(): returns maximum double value + removing the size check for the indexer in for loop

* satisfying ament_uncrustify and ament_cpplint

* removing the unnecessary condition in the cusp determination

* Satisfying ament_uncrustify - 2

Author: padhupradheep

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -210,7 +210,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   void applyConstraints(
     const double & dist_error, const double & lookahead_dist,
     const double & curvature, const geometry_msgs::msg::Twist & speed,
-    const double & pose_cost, double & linear_vel);
+    const double & pose_cost, double & linear_vel, double & sign);
 
   /**
    * @brief Get lookahead point
@@ -220,6 +220,13 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    */
   geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &);
 
+  /**
+   * @brief checks for the cusp position
+   * @param pose Pose input to determine the cusp position
+   * @return robot distance from the cusp
+   */
+  double findDirectionChange(const geometry_msgs::msg::PoseStamped & pose);
+
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::string plugin_name_;
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
@@ -251,6 +258,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   double max_angular_accel_;
   double rotate_to_heading_min_angle_;
   double goal_dist_tol_;
+  bool allow_reversing_;
 
   nav_msgs::msg::Path global_plan_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -15,6 +15,7 @@
 
 #include <algorithm>
 #include <string>
+#include <limits>
 #include <memory>
 #include <utility>
 
@@ -103,6 +104,8 @@ void RegulatedPurePursuitController::configure(
     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_ + ".allow_reversing", rclcpp::ParameterValue(false));
 
   node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_);
   base_desired_linear_vel_ = desired_linear_vel_;
@@ -148,6 +151,7 @@ void RegulatedPurePursuitController::configure(
   node->get_parameter(plugin_name_ + ".use_rotate_to_heading", use_rotate_to_heading_);
   node->get_parameter(plugin_name_ + ".rotate_to_heading_min_angle", rotate_to_heading_min_angle_);
   node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_);
+  node->get_parameter(plugin_name_ + ".allow_reversing", allow_reversing_);
   node->get_parameter("controller_frequency", control_frequency);
 
   transform_tolerance_ = tf2::durationFromSec(transform_tolerance);
@@ -160,6 +164,16 @@ void RegulatedPurePursuitController::configure(
     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 (use_rotate_to_heading_ && 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");
+    allow_reversing_ = false;
+  }
+
   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);
   carrot_arc_pub_ = node->create_publisher<nav_msgs::msg::Path>("lookahead_collision_arc", 1);
@@ -243,7 +257,19 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   auto transformed_plan = transformGlobalPlan(pose);
 
   // Find look ahead distance and point on path and publish
-  const double lookahead_dist = getLookAheadDistance(speed);
+  double lookahead_dist = getLookAheadDistance(speed);
+
+  // Check for reverse driving
+  if (allow_reversing_) {
+    // Cusp check
+    double dist_to_direction_change = findDirectionChange(pose);
+
+    // if the lookahead distance is further than the cusp, use the cusp distance instead
+    if (dist_to_direction_change < lookahead_dist) {
+      lookahead_dist = dist_to_direction_change;
+    }
+  }
+
   auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan);
   carrot_pub_->publish(createCarrotMsg(carrot_pose));
 
@@ -261,6 +287,12 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
     curvature = 2.0 * carrot_pose.pose.position.y / carrot_dist2;
   }
 
+  // Setting the velocity direction
+  double sign = 1.0;
+  if (allow_reversing_) {
+    sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0;
+  }
+
   linear_vel = desired_linear_vel_;
 
   // Make sure we're in compliance with basic constraints
@@ -274,7 +306,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
     applyConstraints(
       fabs(lookahead_dist - sqrt(carrot_dist2)),
       lookahead_dist, curvature, speed,
-      costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel);
+      costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel, sign);
 
     // Apply curvature to angular velocity after constraining linear velocity
     angular_vel = linear_vel * curvature;
@@ -427,7 +459,7 @@ double RegulatedPurePursuitController::costAtPose(const double & x, const double
 void RegulatedPurePursuitController::applyConstraints(
   const double & dist_error, const double & lookahead_dist,
   const double & curvature, const geometry_msgs::msg::Twist & curr_speed,
-  const double & pose_cost, double & linear_vel)
+  const double & pose_cost, double & linear_vel, double & sign)
 {
   double curvature_vel = linear_vel;
   double cost_vel = linear_vel;
@@ -474,11 +506,13 @@ void RegulatedPurePursuitController::applyConstraints(
   }
 
   // Limit linear velocities to be valid and kinematically feasible, v = v0 + a * dt
+  linear_vel = sign * linear_vel;
   double & dt = control_duration_;
   const double max_feasible_linear_speed = curr_speed.linear.x + max_linear_accel_ * dt;
   const double min_feasible_linear_speed = curr_speed.linear.x - max_linear_decel_ * dt;
   linear_vel = std::clamp(linear_vel, min_feasible_linear_speed, max_feasible_linear_speed);
-  linear_vel = std::clamp(linear_vel, 0.0, desired_linear_vel_);
+  linear_vel = std::clamp(fabs(linear_vel), 0.0, desired_linear_vel_);
+  linear_vel = sign * linear_vel;
 }
 
 void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path)
@@ -568,6 +602,34 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan(
   return transformed_plan;
 }
 
+double RegulatedPurePursuitController::findDirectionChange(
+  const geometry_msgs::msg::PoseStamped & pose)
+{
+  // Iterating through the global path to determine the position of the cusp
+  for (unsigned int pose_id = 1; pose_id < global_plan_.poses.size(); ++pose_id) {
+    // We have two vectors for the dot product OA and AB. Determining the vectors.
+    double oa_x = global_plan_.poses[pose_id].pose.position.x -
+      global_plan_.poses[pose_id - 1].pose.position.x;
+    double oa_y = global_plan_.poses[pose_id].pose.position.y -
+      global_plan_.poses[pose_id - 1].pose.position.y;
+    double ab_x = global_plan_.poses[pose_id + 1].pose.position.x -
+      global_plan_.poses[pose_id].pose.position.x;
+    double ab_y = global_plan_.poses[pose_id + 1].pose.position.y -
+      global_plan_.poses[pose_id].pose.position.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. */
+    if ( (oa_x * ab_x) + (oa_y * ab_y) < 0.0) {
+      auto x = global_plan_.poses[pose_id].pose.position.x - pose.pose.position.x;
+      auto y = global_plan_.poses[pose_id].pose.position.y - pose.pose.position.y;
+      return hypot(x, y);  // returning the distance if there is a cusp
+    }
+  }
+
+  return std::numeric_limits<double>::max();
+}
+
 bool RegulatedPurePursuitController::transformPose(
   const std::string frame,
   const geometry_msgs::msg::PoseStamped & in_pose,

nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp

@@ -85,11 +85,11 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure
   void applyConstraintsWrapper(
     const double & dist_error, const double & lookahead_dist,
     const double & curvature, const geometry_msgs::msg::Twist & curr_speed,
-    const double & pose_cost, double & linear_vel)
+    const double & pose_cost, double & linear_vel, double & sign)
   {
     return applyConstraints(
       dist_error, lookahead_dist, curvature, curr_speed, pose_cost,
-      linear_vel);
+      linear_vel, sign);
   }
 };
 
@@ -286,6 +286,7 @@ TEST(RegulatedPurePursuitTest, applyConstraints)
   geometry_msgs::msg::Twist curr_speed;
   double pose_cost = 0.0;
   double linear_vel = 0.0;
+  double sign = 1.0;
 
   // since costmaps here are bogus, we can't access them
   ctrl->resetVelocityApproachScaling();
@@ -294,23 +295,23 @@ TEST(RegulatedPurePursuitTest, applyConstraints)
   curr_speed.linear.x = 0.25;
   ctrl->applyConstraintsWrapper(
     dist_error, lookahead_dist, curvature, curr_speed, pose_cost,
-    linear_vel);
+    linear_vel, sign);
   EXPECT_EQ(linear_vel, 0.25);  // min set speed
 
   linear_vel = 1.0;
   curvature = 0.7407;
   curr_speed.linear.x = 0.5;
   ctrl->applyConstraintsWrapper(
     dist_error, lookahead_dist, curvature, curr_speed, pose_cost,
-    linear_vel);
+    linear_vel, sign);
   EXPECT_NEAR(linear_vel, 0.5, 0.01);  // lower by curvature
 
   linear_vel = 1.0;
   curvature = 1000.0;
   curr_speed.linear.x = 0.25;
   ctrl->applyConstraintsWrapper(
     dist_error, lookahead_dist, curvature, curr_speed, pose_cost,
-    linear_vel);
+    linear_vel, sign);
   EXPECT_NEAR(linear_vel, 0.25, 0.01);  // min out by curvature