From 7362da7370163331e13ceea7ebeabf17484404b7 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Mon, 8 Jul 2024 20:40:26 +0200 Subject: [PATCH 1/2] Fix backward motion for graceful controller Signed-off-by: Alberto Tudela --- .../ego_polar_coords.hpp | 16 ---------------- .../smooth_control_law.hpp | 6 ++---- .../src/smooth_control_law.cpp | 7 ++++--- .../test/test_egopolar.cpp | 18 +++++++++++++++--- 4 files changed, 21 insertions(+), 26 deletions(-) diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp index c9123c510f..eadf608844 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp +++ b/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); } }; diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp index ab68f75f68..eb553d3927 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp +++ b/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 diff --git a/nav2_graceful_controller/src/smooth_control_law.cpp b/nav2_graceful_controller/src/smooth_control_law.cpp index 545a7dd6a8..41a75e3a5a 100644 --- a/nav2_graceful_controller/src/smooth_control_law.cpp +++ b/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,13 +58,15 @@ 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 double v = v_linear_max_ / (1.0 + beta_ * std::pow(fabs(curvature), lambda_)); // Slowdown when the robot is near the target to remove singularity - v = std::min(v_linear_max_ * (ego_coords.r / slowdown_radius_), v); + v = std::min(v_linear_max_ * fabs(ego_coords.r / slowdown_radius_), v); // Set some small v_min when far away from origin to promote faster // turning motion when the curvature is very high diff --git a/nav2_graceful_controller/test/test_egopolar.cpp b/nav2_graceful_controller/test/test_egopolar.cpp index 2bd3c4c244..db01e26a88 100644 --- a/nav2_graceful_controller/test/test_egopolar.cpp +++ b/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); } From 2e926121b55fcf93bc111b7e56791f874e389dc6 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Mon, 22 Jul 2024 23:27:58 +0200 Subject: [PATCH 2/2] Update smooth_control_law.cpp Signed-off-by: Alberto Tudela --- nav2_graceful_controller/src/smooth_control_law.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_graceful_controller/src/smooth_control_law.cpp b/nav2_graceful_controller/src/smooth_control_law.cpp index 41a75e3a5a..7a04fce71e 100644 --- a/nav2_graceful_controller/src/smooth_control_law.cpp +++ b/nav2_graceful_controller/src/smooth_control_law.cpp @@ -66,7 +66,7 @@ geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity( double v = v_linear_max_ / (1.0 + beta_ * std::pow(fabs(curvature), lambda_)); // Slowdown when the robot is near the target to remove singularity - v = std::min(v_linear_max_ * fabs(ego_coords.r / slowdown_radius_), v); + v = std::min(v_linear_max_ * (ego_coords.r / slowdown_radius_), v); // Set some small v_min when far away from origin to promote faster // turning motion when the curvature is very high