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 de899a21f9b..efa4d16b517 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp @@ -114,13 +114,29 @@ class GracefulController : public nav2_core::Controller * @brief Validate a given target pose for calculating command velocity * @param target_pose Target pose to validate * @param dist_to_target Distance to target pose - * @param dist_to_goal Distance to navigation goal * @param trajectory Trajectory to validate in simulation * @param costmap_transform Transform between global and local costmap * @param cmd_vel Initial command velocity to validate in simulation * @return true if target pose is valid, false otherwise */ bool validateTargetPose( + geometry_msgs::msg::PoseStamped & target_pose, + double dist_to_target, + nav_msgs::msg::Path & trajectory, + geometry_msgs::msg::TransformStamped & costmap_transform, + geometry_msgs::msg::TwistStamped & cmd_vel); + + /** + * @brief Validate a given target pose for calculating command velocity on approach to goal + * @param target_pose Target pose to validate + * @param dist_to_target Distance to target pose + * @param dist_to_goal Distance to navigation goal + * @param trajectory Trajectory to validate in simulation + * @param costmap_transform Transform between global and local costmap + * @param cmd_vel Initial command velocity to validate in simulation + * @return true if target pose is valid, false otherwise + */ + bool validateTargetPoseOnApproach( geometry_msgs::msg::PoseStamped & target_pose, double dist_to_target, double dist_to_goal, @@ -128,6 +144,24 @@ class GracefulController : public nav2_core::Controller geometry_msgs::msg::TransformStamped & costmap_transform, geometry_msgs::msg::TwistStamped & cmd_vel); + /** + * @brief Find the best approach trajectory by searching multiple orientations + * @param target_pose Base target pose (position will be used) + * @param dist_to_target Distance to target + * @param costmap_transform Transform + * @param safety_cost Safety cost threshold + * @param best_trajectory Reference to store best trajectory + * @param best_cmd_vel Reference to store best cmd_vel + * @return true if a valid trajectory was found + */ + bool findBestApproachTrajectory( + geometry_msgs::msg::PoseStamped & target_pose, + double dist_to_target, + geometry_msgs::msg::TransformStamped & costmap_transform, + double safety_cost, + nav_msgs::msg::Path & best_trajectory, + geometry_msgs::msg::TwistStamped & best_cmd_vel); + /** * @brief Simulate trajectory calculating in every step the new velocity command based on * a new curvature value and checking for collisions. @@ -166,6 +200,16 @@ class GracefulController : public nav2_core::Controller const double & x, const double & y, const double & theta, double inflation_scale = 1.0); + /** + * @brief Get the maximum cost of a path + * @param path Path to check + * @param costmap_transform Transform between global and local costmap + * @return Maximum cost encountered + */ + double getMaxCost( + const nav_msgs::msg::Path & path, + geometry_msgs::msg::TransformStamped & costmap_transform); + /** * @brief Compute the distance to each pose in a path * @param poses Poses to compute distances with @@ -195,6 +239,8 @@ class GracefulController : public nav2_core::Controller // True from the time a new path arrives until we have completed an initial rotation bool do_initial_rotation_; + std::optional safe_approach_angle_; + nav2::Publisher::SharedPtr local_plan_pub_; nav2::Publisher::SharedPtr motion_target_pub_; nav2::Publisher::SharedPtr slowdown_pub_; 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 1d3312dc6f5..cf4573ba34a 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp @@ -53,6 +53,8 @@ struct Parameters double footprint_scaling_linear_vel; double footprint_scaling_factor; double footprint_scaling_step; + int obstacle_cost_margin; + double final_rotation_search_step; }; /** diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index 073933a77b4..81a141d31f9 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -58,6 +58,16 @@ void GracefulController::configure( FootprintCollisionChecker>(costmap_ros_->getCostmap()); } + double max_valid_cost = costmap_ros_->getUseRadius() ? + static_cast(nav2_costmap_2d::MAX_NON_OBSTACLE) : + static_cast(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + if (max_valid_cost - static_cast(params_->obstacle_cost_margin) < 0.0) { + RCLCPP_WARN( + logger_, "obstacle_cost_margin (%d) is higher than max cost (%d).", + params_->obstacle_cost_margin, nav2_costmap_2d::MAX_NON_OBSTACLE); + throw nav2_core::ControllerException("obstacle_cost_margin is higher than max cost."); + } + // Publishers local_plan_pub_ = node->create_publisher("local_plan"); motion_target_pub_ = node->create_publisher("motion_target"); @@ -222,8 +232,10 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( } // Compute velocity at this moment if valid target pose is found - if (validateTargetPose( - target_pose, dist_to_target, dist_to_goal, local_plan, costmap_transform, cmd_vel)) + if ( + validateTargetPoseOnApproach(target_pose, dist_to_target, dist_to_goal, local_plan, + costmap_transform, cmd_vel) || + validateTargetPose(target_pose, dist_to_target, local_plan, costmap_transform, cmd_vel)) { // Publish the selected target_pose motion_target_pub_->publish(std::make_unique(target_pose)); @@ -246,6 +258,7 @@ void GracefulController::newPathReceived(const nav_msgs::msg::Path & /*raw_globa { goal_reached_ = false; do_initial_rotation_ = true; + safe_approach_angle_.reset(); } void GracefulController::setSpeedLimit( @@ -273,11 +286,8 @@ void GracefulController::setSpeedLimit( } bool GracefulController::validateTargetPose( - geometry_msgs::msg::PoseStamped & target_pose, - double dist_to_target, - double dist_to_goal, - nav_msgs::msg::Path & trajectory, - geometry_msgs::msg::TransformStamped & costmap_transform, + geometry_msgs::msg::PoseStamped & target_pose, double dist_to_target, + nav_msgs::msg::Path & trajectory, geometry_msgs::msg::TransformStamped & costmap_transform, geometry_msgs::msg::TwistStamped & cmd_vel) { // Continue if target_pose is too far away from robot @@ -285,18 +295,6 @@ bool GracefulController::validateTargetPose( return false; } - if (dist_to_goal < params_->max_lookahead) { - if (params_->prefer_final_rotation) { - // Avoid instability and big sweeping turns at the end of paths by - // ignoring final heading - double yaw = std::atan2(target_pose.pose.position.y, target_pose.pose.position.x); - target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); - } - } else if (dist_to_target < params_->min_lookahead) { - // Make sure target is far enough away to avoid instability - return false; - } - // Flip the orientation of the motion target if the robot is moving backwards bool reversing = false; if (params_->allow_backward && target_pose.pose.position.x < 0.0) { @@ -308,8 +306,7 @@ bool GracefulController::validateTargetPose( // Actually simulate the path double sim_linear_velocity = params_->v_linear_max; do { - control_law_->setSpeedLimit(params_->v_linear_min, sim_linear_velocity, - params_->v_angular_max); + control_law_->setSpeedLimit(params_->v_linear_min, sim_linear_velocity, params_->v_angular_max); if (simulateTrajectory(target_pose, costmap_transform, trajectory, cmd_vel, reversing)) { // Successfully simulated to target_pose return true; @@ -322,6 +319,40 @@ bool GracefulController::validateTargetPose( return false; } +bool GracefulController::validateTargetPoseOnApproach( + geometry_msgs::msg::PoseStamped & target_pose, double dist_to_target, double dist_to_goal, + nav_msgs::msg::Path & trajectory, geometry_msgs::msg::TransformStamped & costmap_transform, + geometry_msgs::msg::TwistStamped & cmd_vel) +{ + // Not approaching goal with large lookahead and don't evaluate shortcut trajectories + // when we do not prefer rotating to goal at the end. + if (dist_to_goal >= params_->max_lookahead || !params_->prefer_final_rotation) { + return false; + } + // Avoid instability and big sweeping turns at the end of paths by + // ignoring final heading + double yaw = std::atan2(target_pose.pose.position.y, target_pose.pose.position.x); + target_pose.pose.orientation = + nav2_util::geometry_utils::orientationAroundZAxis(yaw); + + if (validateTargetPose(target_pose, dist_to_target, trajectory, costmap_transform, cmd_vel)) { + // Determine the maximum valid cost based on robot footprint type + double max_valid_cost = + costmap_ros_->getUseRadius() ? static_cast(nav2_costmap_2d::MAX_NON_OBSTACLE) : + static_cast(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + + // Check if the final rotation path is risky + double safety_threshold = max_valid_cost - static_cast(params_->obstacle_cost_margin); + if (getMaxCost(trajectory, costmap_transform) >= safety_threshold) { + // Try to find a better approach by searching spiral curves + findBestApproachTrajectory( + target_pose, dist_to_target, costmap_transform, max_valid_cost, trajectory, cmd_vel); + } + return true; + } + return false; +} + bool GracefulController::simulateTrajectory( const geometry_msgs::msg::PoseStamped & motion_target, const geometry_msgs::msg::TransformStamped & costmap_transform, @@ -426,6 +457,25 @@ geometry_msgs::msg::Twist GracefulController::rotateToTarget(double angle_to_tar return vel; } +double GracefulController::getMaxCost( + const nav_msgs::msg::Path & path, geometry_msgs::msg::TransformStamped & costmap_transform) +{ + double max_cost = 0.0; + + for (const auto & pose : path.poses) { + geometry_msgs::msg::PoseStamped costmap_pose; + tf2::doTransform(pose, costmap_pose, costmap_transform); + unsigned int mx, my; + if (costmap_ros_->getCostmap()->worldToMap(costmap_pose.pose.position.x, + costmap_pose.pose.position.y, mx, my)) + { + max_cost = std::max(max_cost, collision_checker_->pointCost(mx, my)); + } + } + + return max_cost; +} + bool GracefulController::inCollision( const double & x, const double & y, const double & theta, double inflation_scale) @@ -515,6 +565,76 @@ void GracefulController::validateOrientations( } } +bool GracefulController::findBestApproachTrajectory( + geometry_msgs::msg::PoseStamped & target_pose, double dist_to_target, + geometry_msgs::msg::TransformStamped & costmap_transform, double safety_cost, + nav_msgs::msg::Path & best_trajectory, geometry_msgs::msg::TwistStamped & best_cmd_vel) +{ + bool found_valid = false; + double best_eta = std::numeric_limits::max(); + + for (int i = 0; i < 2 * M_PI / params_->final_rotation_search_step; ++i) { + double angle = static_cast(i) * params_->final_rotation_search_step; + // Prioritize previously selected approach angles + if (safe_approach_angle_.has_value()) { + angle += safe_approach_angle_.value(); + } + + // Create candidate pose + auto candidate_pose = target_pose; + candidate_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(angle); + + nav_msgs::msg::Path candidate_path = best_trajectory; + geometry_msgs::msg::TwistStamped candidate_cmd_vel = best_cmd_vel; + + // Validate the candidate + if (validateTargetPose( + candidate_pose, dist_to_target, candidate_path, costmap_transform, + candidate_cmd_vel)) + { + double candidate_cost = getMaxCost(candidate_path, costmap_transform); + + bool reversing = false; + if (params_->allow_backward && target_pose.pose.position.x < 0.0) { + reversing = true; + } + // Calculate ETA + double eta = 0.0; + for (size_t j = 1; j < candidate_path.poses.size(); ++j) { + auto current_pose = candidate_path.poses[j - 1]; + auto next_pose = candidate_path.poses[j]; + auto cmd = control_law_->calculateRegularVelocity(candidate_pose.pose, current_pose.pose, + reversing); + double speed = std::abs(cmd.linear.x); + // Avoid division by zero + speed = std::max(speed, 1e-3); + double step_dist = nav2_util::geometry_utils::euclidean_distance( + current_pose.pose, next_pose.pose); + double step_time = step_dist / speed; + eta += step_time; + } + + // Selection logic: Pick the fastest among the safe ones + if (eta < best_eta) { + best_eta = eta; + if (candidate_cost < safety_cost) { + best_trajectory = candidate_path; + best_cmd_vel = candidate_cmd_vel; + target_pose = candidate_pose; + found_valid = true; + // Reuse known safe approach angle if still valid + if (safe_approach_angle_.value_or(1e3 /*Never in (-PI, PI]*/) == angle) { + break; + } + safe_approach_angle_ = angle; + } + } + } + } + + return found_valid; +} + } // namespace nav2_graceful_controller // Register this controller as a nav2_core plugin diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp index 99d65bb598e..40d83cf8d5a 100644 --- a/nav2_graceful_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -76,6 +76,10 @@ ParameterHandler::ParameterHandler( plugin_name_ + ".footprint_scaling_factor", 0.25); params_.footprint_scaling_step = node->declare_or_get_parameter( plugin_name_ + ".footprint_scaling_step", 0.1); + params_.obstacle_cost_margin = node->declare_or_get_parameter( + plugin_name_ + ".obstacle_cost_margin", 1); + params_.final_rotation_search_step = node->declare_or_get_parameter( + plugin_name_ + ".final_rotation_search_step", 0.1); if (params_.initial_rotation && params_.allow_backward) { RCLCPP_WARN( logger_, "Initial rotation and allow backward parameters are both true, " @@ -172,6 +176,8 @@ ParameterHandler::updateParametersCallback( params_.footprint_scaling_factor = parameter.as_double(); } else if (param_name == plugin_name_ + ".footprint_scaling_step") { params_.footprint_scaling_step = parameter.as_double(); + } else if (param_name == plugin_name_ + ".final_rotation_search_step") { + params_.final_rotation_search_step = parameter.as_double(); } } else if (param_type == ParameterType::PARAMETER_BOOL) { if (param_name == plugin_name_ + ".initial_rotation") { @@ -183,6 +189,10 @@ ParameterHandler::updateParametersCallback( } else if (param_name == plugin_name_ + ".use_collision_detection") { params_.use_collision_detection = parameter.as_bool(); } + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == plugin_name_ + ".obstacle_cost_margin") { + params_.obstacle_cost_margin = parameter.as_int(); + } } } } diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index 0f87c0f52d1..445f65b9c2a 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -252,7 +252,9 @@ TEST(GracefulControllerTest, dynamicParameters) { rclcpp::Parameter("test.in_place_collision_resolution", 15.0), rclcpp::Parameter("test.footprint_scaling_linear_vel", 16.0), rclcpp::Parameter("test.footprint_scaling_factor", 17.0), - rclcpp::Parameter("test.footprint_scaling_step", 18.0)}); + rclcpp::Parameter("test.footprint_scaling_step", 18.0), + rclcpp::Parameter("test.obstacle_cost_margin", 19), + rclcpp::Parameter("test.final_rotation_search_step", 20.0)}); // Spin rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); @@ -279,6 +281,8 @@ TEST(GracefulControllerTest, dynamicParameters) { EXPECT_EQ(node->get_parameter("test.footprint_scaling_linear_vel").as_double(), 16.0); EXPECT_EQ(node->get_parameter("test.footprint_scaling_factor").as_double(), 17.0); EXPECT_EQ(node->get_parameter("test.footprint_scaling_step").as_double(), 18.0); + EXPECT_EQ(node->get_parameter("test.obstacle_cost_margin").as_int(), 19); + EXPECT_EQ(node->get_parameter("test.final_rotation_search_step").as_double(), 20.0); // Set allow backward to true results = params->set_parameters_atomically( @@ -913,6 +917,147 @@ TEST(GracefulControllerTest, slowDownForObstacle) { EXPECT_LT(cmd_vel.twist.linear.x, max_vel); } +TEST(GracefulControllerTest, computeVelocityCommandObstacleMargin) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + + // Create a costmap of 10x10 meters + auto costmap_ros = std::make_shared("test_costmap"); + costmap_ros->declare_parameter("global_frame", rclcpp::ParameterValue("test_global_frame")); + costmap_ros->declare_parameter("robot_base_frame", rclcpp::ParameterValue("test_robot_frame")); + costmap_ros->declare_parameter("width", rclcpp::ParameterValue(10)); + costmap_ros->declare_parameter("height", rclcpp::ParameterValue(10)); + costmap_ros->declare_parameter("resolution", rclcpp::ParameterValue(0.1)); + costmap_ros->declare_parameter( + "footprint", rclcpp::ParameterValue("[[-0.2, -0.2], [0.2, -0.2], [0.2, 0.2], [-0.2, 0.2]]")); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + // Create controller + auto controller = std::make_shared(); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + nav2_controller::FeasiblePathHandler path_handler; + path_handler.initialize(node, node->get_logger(), "path_handler", costmap_ros, tf); + nav2_controller::SimpleGoalChecker checker; + checker.initialize(node, "checker", costmap_ros); + + auto params = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + int cost_margin = 10; + auto fut = params->set_parameters_atomically( + {rclcpp::Parameter("test.prefer_final_rotation", true), + rclcpp::Parameter("test.obstacle_cost_margin", cost_margin), + rclcpp::Parameter("test.slowdown_radius", 0.1), + rclcpp::Parameter("test.footprint_scaling_linear_vel", 0.2), + rclcpp::Parameter("test.footprint_scaling_factor", 1.0), + rclcpp::Parameter("test.footprint_scaling_step", 0.1), + rclcpp::Parameter("test.max_lookahead", 2.0)}); + rclcpp::spin_until_future_complete(node->get_node_base_interface(), fut); + + // Create the robot pose + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = "test_robot_frame"; + robot_pose.pose.position.x = 0.0; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + robot_pose.pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + + // Set transform between global and robot frame + geometry_msgs::msg::TransformStamped global_to_robot; + global_to_robot.header.frame_id = "test_global_frame"; + global_to_robot.header.stamp = node->get_clock()->now(); + global_to_robot.child_frame_id = "test_robot_frame"; + global_to_robot.transform.translation.x = 5.0; + global_to_robot.transform.translation.y = 5.0; + global_to_robot.transform.translation.z = 0.0; + tf->setTransform(global_to_robot, "test", false); + + // Set a plan in a straight line from the robot + nav_msgs::msg::Path plan; + plan.header.frame_id = "test_global_frame"; + plan.poses.resize(5); + plan.poses[0].header.frame_id = "test_global_frame"; + plan.poses[0].pose.position.x = 5.0; + plan.poses[0].pose.position.y = 5.0; + plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[1].header.frame_id = "test_global_frame"; + plan.poses[1].pose.position.x = 5.25; + plan.poses[1].pose.position.y = 5.0; + plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[2].header.frame_id = "test_global_frame"; + plan.poses[2].pose.position.x = 5.5; + plan.poses[2].pose.position.y = 5.0; + plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[3].header.frame_id = "test_global_frame"; + plan.poses[3].pose.position.x = 5.75; + plan.poses[3].pose.position.y = 5.0; + plan.poses[3].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[4].header.frame_id = "test_global_frame"; + plan.poses[4].pose.position.x = 6.0; + plan.poses[4].pose.position.y = 5.0; + plan.poses[4].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + path_handler.setPlan(plan); + + controller->newPathReceived(plan); + path_handler.setPlan(plan); + + auto costmap = costmap_ros->getCostmap(); + unsigned int mx, my; + for (int i = 0; i < 5; i++) { + for (int j = 0; j < 5; j++) { + costmap->worldToMap(5.25 + i * 0.1, 4.75 + j * 0.1, mx, my); + // Case 1: Safe Cost + // Safety threshold = 252 - 10 = 242. + // We set obstacle cost to 230 (< 242). + costmap->setCost(mx, my, 230); + } + } + + auto [closest_point1, pruned_plan_end1] = path_handler.findPlanSegment(robot_pose); + nav_msgs::msg::Path transformed_plan1 = path_handler.transformLocalPlan(closest_point1, + pruned_plan_end1); + + geometry_msgs::msg::Twist robot_velocity; + robot_velocity.linear.x = 0.0; + robot_velocity.linear.y = 0.0; + geometry_msgs::msg::PoseStamped goal; + auto cmd_vel_safe = controller->computeVelocityCommands( + robot_pose, robot_velocity, &checker, transformed_plan1, goal); + + // Expectation: Go straight. + EXPECT_GT(cmd_vel_safe.twist.linear.x, 0.0); + EXPECT_NEAR(cmd_vel_safe.twist.angular.z, 0.0, 1e-3); + + for (int i = 0; i < 5; i++) { + for (int j = 0; j < 5; j++) { + costmap->worldToMap(5.25 + i * 0.1, 4.75 + j * 0.1, mx, my); + // Case 2: Risky Cost (but valid) + // We set obstacle cost to 250 (>= 242). + // This should trigger the avoidance logic. + costmap->setCost(mx, my, 250); + } + } + costmap->worldToMap(5.5, 5.4, mx, my); + costmap->setCost(mx, my, nav2_costmap_2d::LETHAL_OBSTACLE); + + auto [closest_point2, pruned_plan_end2] = path_handler.findPlanSegment(robot_pose); + nav_msgs::msg::Path transformed_plan2 = path_handler.transformLocalPlan(closest_point2, + pruned_plan_end2); + + auto cmd_vel_risky = controller->computeVelocityCommands( + robot_pose, robot_velocity, &checker, transformed_plan2, goal); + + bool is_same = (std::abs(cmd_vel_safe.twist.linear.x - cmd_vel_risky.twist.linear.x) < 1e-4) && + (std::abs(cmd_vel_safe.twist.angular.z - cmd_vel_risky.twist.angular.z) < 1e-4); + + EXPECT_FALSE(is_same); + + // It is likely to turn, so angular z should be non-zero (or larger than safe case) + EXPECT_GT(std::abs(cmd_vel_risky.twist.angular.z), 1e-3); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv);