diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp index 28bec7eddd8..ef65c380d17 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp @@ -131,6 +131,7 @@ class GracefulController : public nav2_core::Controller * * @param motion_target Motion target point (in costmap local frame?) * @param costmap_transform Transform between global and local costmap + * @param target_distance Path distance to target point * @param trajectory Simulated trajectory * @param cmd_vel Initial command velocity during simulation * @param backward Flag to indicate if the robot is moving backward @@ -139,6 +140,7 @@ class GracefulController : public nav2_core::Controller bool simulateTrajectory( const geometry_msgs::msg::PoseStamped & motion_target, const geometry_msgs::msg::TransformStamped & costmap_transform, + const double & target_distance, nav_msgs::msg::Path & trajectory, geometry_msgs::msg::TwistStamped & cmd_vel, bool backward); diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp index 8d22841f817..e1bb936f6e0 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp @@ -46,6 +46,7 @@ struct Parameters double v_angular_max_initial; double v_angular_min_in_place; double slowdown_radius; + double deceleration_max; bool initial_rotation; double initial_rotation_tolerance; bool prefer_final_rotation; 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 eb553d3927c..9111b916651 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 @@ -32,6 +32,24 @@ namespace nav2_graceful_controller class SmoothControlLaw { public: + /** + * @brief Constructor for nav2_graceful_controller::SmoothControlLaw + * + * @param k_phi Ratio of the rate of change in phi to the rate of change in r. + * @param k_delta Constant factor applied to the heading error feedback. + * @param beta Constant factor applied to the path curvature: dropping velocity. + * @param lambda Constant factor applied to the path curvature for sharpness. + * @param slowdown_radius Radial threshold applied to the slowdown rule. + * @param deceleration_max Maximum deceleration. + * @param v_linear_min Minimum linear velocity. + * @param v_linear_max Maximum linear velocity. + * @param v_angular_max Maximum angular velocity. + */ + SmoothControlLaw( + double k_phi, double k_delta, double beta, double lambda, + double slowdown_radius, double deceleration_max, + double v_linear_min, double v_linear_max, double v_angular_max); + /** * @brief Constructor for nav2_graceful_controller::SmoothControlLaw * @@ -71,6 +89,13 @@ class SmoothControlLaw */ void setSlowdownRadius(const double slowdown_radius); + /** + * @brief Set the max deceleration + * + * @param deceleration_max Maximum deceleration possible. + */ + void setMaxDeceleration(const double deceleration_max); + /** * @brief Update the velocity limits. * @@ -86,6 +111,22 @@ class SmoothControlLaw * * @param target Pose of the target in the robot frame. * @param current Current pose of the robot in the robot frame. + * @param target_distance Path distance from current to target frame. + * @param backward If true, the robot is moving backwards. Defaults to false. + * @return Velocity command. + */ + geometry_msgs::msg::Twist calculateRegularVelocity( + const geometry_msgs::msg::Pose & target, + const geometry_msgs::msg::Pose & current, + const double & target_distance, + const bool & backward = false); + + /** + * @brief Compute linear and angular velocities command using the curvature. + * + * @param target Pose of the target in the robot frame. + * @param current Current pose of the robot in the robot frame. + * @param target_distance Path distance from current to target frame. * @param backward If true, the robot is moving backwards. Defaults to false. * @return Velocity command. */ @@ -111,6 +152,25 @@ class SmoothControlLaw * @param dt Time step. * @param target Pose of the target in the robot frame. * @param current Current pose of the robot in the robot frame. + * @param target_distance Path distance from current to target frame. + * @param backward If true, the robot is moving backwards. Defaults to false. + * @return geometry_msgs::msg::Pose + */ + geometry_msgs::msg::Pose calculateNextPose( + const double dt, + const geometry_msgs::msg::Pose & target, + const geometry_msgs::msg::Pose & current, + const double & target_distance, + const bool & backward = false); + + /** + * @brief Calculate the next pose of the robot by generating a velocity command using the + * curvature and the current pose. + * + * @param dt Time step. + * @param target Pose of the target in the robot frame. + * @param current Current pose of the robot in the robot frame. + * @param target_distance Path distance from current to target frame. * @param backward If true, the robot is moving backwards. Defaults to false. * @return geometry_msgs::msg::Pose */ @@ -170,6 +230,11 @@ class SmoothControlLaw */ double slowdown_radius_; + /** + * @brief Maximum deceleration. + */ + double deceleration_max_; + /** * @brief Minimum linear velocity. */ diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index b8cce0b0aed..ee0550a1eea 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -53,7 +53,8 @@ void GracefulController::configure( // Handles the control law to generate the velocity commands control_law_ = std::make_unique( - params_->k_phi, params_->k_delta, params_->beta, params_->lambda, params_->slowdown_radius, + params_->k_phi, params_->k_delta, params_->beta, params_->lambda, + params_->slowdown_radius, params_->deceleration_max, params_->v_linear_min, params_->v_linear_max, params_->v_angular_max); // Initialize footprint collision checker @@ -134,6 +135,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( control_law_->setCurvatureConstants( params_->k_phi, params_->k_delta, params_->beta, params_->lambda); control_law_->setSlowdownRadius(params_->slowdown_radius); + control_law_->setMaxDeceleration(params_->deceleration_max); control_law_->setSpeedLimit(params_->v_linear_min, params_->v_linear_max, params_->v_angular_max); // Transform path to robot base frame @@ -310,7 +312,9 @@ bool GracefulController::validateTargetPose( } // Actually simulate the path - if (simulateTrajectory(target_pose, costmap_transform, trajectory, cmd_vel, reversing)) { + if (simulateTrajectory(target_pose, costmap_transform, dist_to_target, trajectory, cmd_vel, + reversing)) + { // Successfully simulated to target_pose return true; } @@ -322,6 +326,7 @@ bool GracefulController::validateTargetPose( bool GracefulController::simulateTrajectory( const geometry_msgs::msg::PoseStamped & motion_target, const geometry_msgs::msg::TransformStamped & costmap_transform, + const double & target_distance, nav_msgs::msg::Path & trajectory, geometry_msgs::msg::TwistStamped & cmd_vel, bool backward) @@ -372,12 +377,12 @@ bool GracefulController::simulateTrajectory( // If this is first iteration, this is our current target velocity if (trajectory.poses.empty()) { cmd_vel.twist = control_law_->calculateRegularVelocity( - motion_target.pose, next_pose.pose, backward); + motion_target.pose, next_pose.pose, target_distance, backward); } // Apply velocities to calculate next pose next_pose.pose = control_law_->calculateNextPose( - dt, motion_target.pose, next_pose.pose, backward); + dt, motion_target.pose, next_pose.pose, target_distance, backward); } // Add the pose to the trajectory for visualization diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp index 95fdda9f82b..8babb79e80e 100644 --- a/nav2_graceful_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -58,6 +58,8 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".v_angular_min_in_place", rclcpp::ParameterValue(0.25)); declare_parameter_if_not_declared( node, plugin_name_ + ".slowdown_radius", rclcpp::ParameterValue(1.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".deceleration_max", rclcpp::ParameterValue(3.0)); declare_parameter_if_not_declared( node, plugin_name_ + ".initial_rotation", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( @@ -97,6 +99,7 @@ ParameterHandler::ParameterHandler( node->get_parameter( plugin_name_ + ".v_angular_min_in_place", params_.v_angular_min_in_place); node->get_parameter(plugin_name_ + ".slowdown_radius", params_.slowdown_radius); + node->get_parameter(plugin_name_ + ".deceleration_max", params_.deceleration_max); node->get_parameter(plugin_name_ + ".initial_rotation", params_.initial_rotation); node->get_parameter( plugin_name_ + ".initial_rotation_tolerance", params_.initial_rotation_tolerance); @@ -168,6 +171,8 @@ ParameterHandler::dynamicParametersCallback(std::vector param params_.v_angular_min_in_place = parameter.as_double(); } else if (param_name == plugin_name_ + ".slowdown_radius") { params_.slowdown_radius = parameter.as_double(); + } else if (param_name == plugin_name_ + ".deceleration_max") { + params_.deceleration_max = parameter.as_double(); } else if (param_name == plugin_name_ + ".initial_rotation_tolerance") { params_.initial_rotation_tolerance = parameter.as_double(); } else if (param_name == plugin_name_ + ".rotation_scaling_factor") { diff --git a/nav2_graceful_controller/src/smooth_control_law.cpp b/nav2_graceful_controller/src/smooth_control_law.cpp index 7a04fce71ef..fdf6f671b5b 100644 --- a/nav2_graceful_controller/src/smooth_control_law.cpp +++ b/nav2_graceful_controller/src/smooth_control_law.cpp @@ -20,6 +20,16 @@ namespace nav2_graceful_controller { +SmoothControlLaw::SmoothControlLaw( + double k_phi, double k_delta, double beta, double lambda, + double slowdown_radius, double deceleration_max, + double v_linear_min, double v_linear_max, double v_angular_max) +: k_phi_(k_phi), k_delta_(k_delta), beta_(beta), lambda_(lambda), + slowdown_radius_(slowdown_radius), deceleration_max_(deceleration_max), + v_linear_min_(v_linear_min), v_linear_max_(v_linear_max), v_angular_max_(v_angular_max) +{ +} + SmoothControlLaw::SmoothControlLaw( double k_phi, double k_delta, double beta, double lambda, double slowdown_radius, double v_linear_min, double v_linear_max, double v_angular_max) @@ -42,6 +52,11 @@ void SmoothControlLaw::setSlowdownRadius(double slowdown_radius) slowdown_radius_ = slowdown_radius; } +void SmoothControlLaw::setMaxDeceleration(double deceleration_max) +{ + deceleration_max_ = deceleration_max; +} + void SmoothControlLaw::setSpeedLimit( const double v_linear_min, const double v_linear_max, const double v_angular_max) { @@ -50,6 +65,48 @@ void SmoothControlLaw::setSpeedLimit( v_angular_max_ = v_angular_max; } +geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity( + const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, + const double & target_distance, const bool & backward) +{ + // Convert the target to polar coordinates + 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_ * (target_distance / slowdown_radius_), v); + + // Constraint robot velocity within deceleration limits + v = std::min(sqrt(2 * target_distance * deceleration_max_), v); + + // Set some small v_min when far away from origin to promote faster + // turning motion when the curvature is very high + v = std::clamp(v, v_linear_min_, v_linear_max_); + + // Set the velocity to negative if the robot is moving backwards + v = backward ? -v : v; + + // Compute the angular velocity + double w = curvature * v; + // Bound angular velocity between [-max_angular_vel, max_angular_vel] + double w_bound = std::clamp(w, -v_angular_max_, v_angular_max_); + // And linear velocity to follow the curvature + v = (curvature != 0.0) ? (w_bound / curvature) : v; + + // Return the velocity command + geometry_msgs::msg::Twist cmd_vel; + cmd_vel.linear.x = v; + cmd_vel.angular.z = w_bound; + return cmd_vel; +} + geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity( const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, const bool & backward) @@ -95,6 +152,24 @@ geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity( return calculateRegularVelocity(target, geometry_msgs::msg::Pose(), backward); } +geometry_msgs::msg::Pose SmoothControlLaw::calculateNextPose( + const double dt, + const geometry_msgs::msg::Pose & target, + const geometry_msgs::msg::Pose & current, + const double & target_distance, + const bool & backward) +{ + geometry_msgs::msg::Twist vel = calculateRegularVelocity(target, current, target_distance, + backward); + geometry_msgs::msg::Pose next; + double yaw = tf2::getYaw(current.orientation); + next.position.x = current.position.x + vel.linear.x * dt * cos(yaw); + next.position.y = current.position.y + vel.linear.x * dt * sin(yaw); + yaw += vel.angular.z * dt; + next.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); + return next; +} + geometry_msgs::msg::Pose SmoothControlLaw::calculateNextPose( const double dt, const geometry_msgs::msg::Pose & target, diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index 799daa8d8b3..fa8426b4317 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -27,15 +27,17 @@ class SCLFixture : public nav2_graceful_controller::SmoothControlLaw public: SCLFixture( double k_phi, double k_delta, double beta, double lambda, - double slowdown_radius, double v_linear_min, double v_linear_max, double v_angular_max) + double slowdown_radius, double deceleration_max, + double v_linear_min, double v_linear_max, double v_angular_max) : nav2_graceful_controller::SmoothControlLaw(k_phi, k_delta, beta, lambda, - slowdown_radius, v_linear_min, v_linear_max, v_angular_max) {} + slowdown_radius, deceleration_max, v_linear_min, v_linear_max, v_angular_max) {} double getCurvatureKPhi() {return k_phi_;} double getCurvatureKDelta() {return k_delta_;} double getCurvatureBeta() {return beta_;} double getCurvatureLambda() {return lambda_;} double getSlowdownRadius() {return slowdown_radius_;} + double getMaxDeceleration() {return deceleration_max_;} double getSpeedLinearMin() {return v_linear_min_;} double getSpeedLinearMax() {return v_linear_max_;} double getSpeedAngularMax() {return v_angular_max_;} @@ -84,7 +86,7 @@ class GMControllerFixture : public nav2_graceful_controller::GracefulController TEST(SmoothControlLawTest, setCurvatureConstants) { // Initialize SmoothControlLaw - SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0, 0); + SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); // Set curvature constants scl.setCurvatureConstants(1.0, 2.0, 3.0, 4.0); @@ -100,9 +102,20 @@ TEST(SmoothControlLawTest, setCurvatureConstants) { EXPECT_EQ(scl.getSlowdownRadius(), 5.0); } +TEST(SmoothControlLawTest, setMaxDeceleration) { + // Initialize SmoothControlLaw + SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + + // Set max deceleration + scl.setMaxDeceleration(3.0); + + // Check results + EXPECT_EQ(scl.getMaxDeceleration(), 3.0); +} + TEST(SmoothControlLawTest, setSpeedLimits) { // Initialize SmoothControlLaw - SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); // Set speed limits scl.setSpeedLimit(1.0, 2.0, 3.0); @@ -115,7 +128,7 @@ TEST(SmoothControlLawTest, setSpeedLimits) { TEST(SmoothControlLawTest, calculateCurvature) { // Initialize SmoothControlLaw - SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 0.0, 1.0, 1.0); + SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 3.0, 0.0, 1.0, 1.0); // Initialize target geometry_msgs::msg::Pose target; @@ -150,7 +163,7 @@ TEST(SmoothControlLawTest, calculateCurvature) { TEST(SmoothControlLawTest, calculateRegularVelocity) { // Initialize SmoothControlLaw - SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 0.0, 1.0, 1.0); + SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 3.0, 0.0, 1.0, 1.0); // Initialize target geometry_msgs::msg::Pose target; @@ -163,7 +176,8 @@ TEST(SmoothControlLawTest, calculateRegularVelocity) { current.position.y = 0.0; current.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); // Calculate velocity - auto cmd_vel = scl.calculateRegularVelocity(target, current); + double target_distance = 5.0; + auto cmd_vel = scl.calculateRegularVelocity(target, current, target_distance); // Check results: both linear and angular velocity must be positive EXPECT_NEAR(cmd_vel.linear.x, 0.1460446, 0.0001); @@ -178,7 +192,8 @@ TEST(SmoothControlLawTest, calculateRegularVelocity) { current.position.y = 0.0; current.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); // Calculate velocity - cmd_vel = scl.calculateRegularVelocity(target, current); + target_distance = 4.995; + cmd_vel = scl.calculateRegularVelocity(target, current, target_distance); // Check results: linear velocity must be positive and angular velocity must be negative EXPECT_NEAR(cmd_vel.linear.x, 0.96651200, 0.0001); @@ -187,7 +202,7 @@ TEST(SmoothControlLawTest, calculateRegularVelocity) { TEST(SmoothControlLawTest, calculateNextPose) { // Initialize SmoothControlLaw - SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 0.0, 1.0, 1.0); + SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 3.0, 0.0, 1.0, 1.0); // Initialize target geometry_msgs::msg::Pose target; @@ -201,7 +216,8 @@ TEST(SmoothControlLawTest, calculateNextPose) { current.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); // Calculate next pose float dt = 0.1; - auto next_pose = scl.calculateNextPose(dt, target, current); + double target_distance = 5.0; + auto next_pose = scl.calculateNextPose(dt, target, current, target_distance); // Check results EXPECT_NEAR(next_pose.position.x, 0.1, 0.1);