Fix backward motion for graceful controller (#4527) (#4554)

* Fix backward motion for graceful controller

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Update smooth_control_law.cpp

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

---------

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
(cherry picked from commit d1ad640)

Co-authored-by: Alberto Tudela <ajtudela@gmail.com>

nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp

@@ -33,7 +33,6 @@ namespace nav2_graceful_controller
 struct EgocentricPolarCoordinates
 {
   float r;       // Radial distance between the robot pose and the target pose.
-                 // Negative value if the robot is moving backwards.
   float phi;     // Orientation of target with respect to the line of sight
                  // from the robot to the target.
   float delta;   // Steering angle of the robot with respect to the line of sight.
@@ -68,21 +67,6 @@ struct EgocentricPolarCoordinates
     r = sqrt(dX * dX + dY * dY);
     phi = angles::normalize_angle(tf2::getYaw(target.orientation) + line_of_sight);
     delta = angles::normalize_angle(tf2::getYaw(current.orientation) + line_of_sight);
-    // If the robot is moving backwards, flip the sign of the radial distance
-    r *= backward ? -1.0 : 1.0;
-  }
-
-  /**
-   * @brief Construct a new egocentric polar coordinates for the target pose.
-   *
-   * @param target Target pose.
-   * @param backward If true, the robot is moving backwards. Defaults to false.
-   */
-  explicit EgocentricPolarCoordinates(
-    const geometry_msgs::msg::Pose & target,
-    bool backward = false)
-  {
-    EgocentricPolarCoordinates(target, geometry_msgs::msg::Pose(), backward);
   }
 };
 

nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp

@@ -79,8 +79,7 @@ class SmoothControlLaw
    * @param v_angular_max The maximum absolute velocity in the angular direction.
    */
   void setSpeedLimit(
-    const double v_linear_min, const double v_linear_max,
-    const double v_angular_max);
+    const double v_linear_min, const double v_linear_max, const double v_angular_max);
 
   /**
    * @brief Compute linear and angular velocities command using the curvature.
@@ -103,8 +102,7 @@ class SmoothControlLaw
    * @return Velocity command.
    */
   geometry_msgs::msg::Twist calculateRegularVelocity(
-    const geometry_msgs::msg::Pose & target,
-    const bool & backward = false);
+    const geometry_msgs::msg::Pose & target, const bool & backward = false);
 
   /**
    * @brief Calculate the next pose of the robot by generating a velocity command using the

nav2_graceful_controller/src/smooth_control_law.cpp

@@ -43,8 +43,7 @@ void SmoothControlLaw::setSlowdownRadius(double slowdown_radius)
 }
 
 void SmoothControlLaw::setSpeedLimit(
-  const double v_linear_min, const double v_linear_max,
-  const double v_angular_max)
+  const double v_linear_min, const double v_linear_max, const double v_angular_max)
 {
   v_linear_min_ = v_linear_min;
   v_linear_max_ = v_linear_max;
@@ -59,6 +58,8 @@ geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
   auto ego_coords = EgocentricPolarCoordinates(target, current, backward);
   // Calculate the curvature
   double curvature = calculateCurvature(ego_coords.r, ego_coords.phi, ego_coords.delta);
+  // Invert the curvature if the robot is moving backwards
+  curvature = backward ? -curvature : curvature;
 
   // Adjust the linear velocity as a function of the path curvature to
   // slowdown the controller as it approaches its target

nav2_graceful_controller/test/test_egopolar.cpp

@@ -31,8 +31,7 @@ TEST(EgocentricPolarCoordinatesTest, constructorWithValues) {
   float phi_value = 1.2;
   float delta_value = -0.5;
 
-  nav2_graceful_controller::EgocentricPolarCoordinates coords(r_value, phi_value,
-    delta_value);
+  nav2_graceful_controller::EgocentricPolarCoordinates coords(r_value, phi_value, delta_value);
 
   EXPECT_FLOAT_EQ(r_value, coords.r);
   EXPECT_FLOAT_EQ(phi_value, coords.phi);
@@ -57,6 +56,19 @@ TEST(EgocentricPolarCoordinatesTest, constructorFromPoses) {
   EXPECT_FLOAT_EQ(-1.1827937483787536, coords.delta);
 }
 
+TEST(EgocentricPolarCoordinatesTest, constructorFromPose) {
+  geometry_msgs::msg::Pose target;
+  target.position.x = 3.0;
+  target.position.y = 3.0;
+  target.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), M_PI));
+
+  nav2_graceful_controller::EgocentricPolarCoordinates coords(target);
+
+  EXPECT_FLOAT_EQ(4.2426405, coords.r);
+  EXPECT_FLOAT_EQ(2.3561945, coords.phi);
+  EXPECT_FLOAT_EQ(-M_PI / 4, coords.delta);
+}
+
 TEST(EgocentricPolarCoordinatesTest, constructorFromPosesBackward) {
   geometry_msgs::msg::Pose target;
   target.position.x = -3.0;
@@ -70,7 +82,7 @@ TEST(EgocentricPolarCoordinatesTest, constructorFromPosesBackward) {
 
   nav2_graceful_controller::EgocentricPolarCoordinates coords(target, current, true);
 
-  EXPECT_FLOAT_EQ(-6.4031243, coords.r);
+  EXPECT_FLOAT_EQ(6.4031243, coords.r);
   EXPECT_FLOAT_EQ(-0.096055523, coords.phi);
   EXPECT_FLOAT_EQ(-1.0960555, coords.delta);
 }