From ba0bb489ad727144372cefe626900e21eedb8243 Mon Sep 17 00:00:00 2001 From: suchetanrs Date: Sun, 23 Mar 2025 09:26:47 +0530 Subject: [PATCH 1/4] * Parametrize collision checking in nav2_graceful_controller Signed-off-by: suchetanrs --- .../nav2_graceful_controller/parameter_handler.hpp | 1 + nav2_graceful_controller/src/graceful_controller.cpp | 12 ++++++++---- nav2_graceful_controller/src/parameter_handler.cpp | 4 ++++ 3 files changed, 13 insertions(+), 4 deletions(-) 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 dd4d4df09ce..3d6ad16f33b 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,7 @@ struct Parameters double rotation_scaling_factor; bool allow_backward; double in_place_collision_resolution; + bool use_collision_detection; }; /** diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index 3e6857d0287..2ffba55c3c2 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -56,8 +56,11 @@ void GracefulController::configure( params_->v_linear_min, params_->v_linear_max, params_->v_angular_max); // Initialize footprint collision checker - collision_checker_ = std::make_unique>(costmap_ros_->getCostmap()); + if(params_->use_collision_detection) + { + collision_checker_ = std::make_unique>(costmap_ros_->getCostmap()); + } // Publishers transformed_plan_pub_ = node->create_publisher("transformed_global_plan", 1); @@ -169,6 +172,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( // Need to check at least the end pose num_steps = std::max(static_cast(1), num_steps); bool collision_free = true; + bool use_collision_detection = params_->use_collision_detection; for (size_t i = 1; i <= num_steps; ++i) { double step = static_cast(i) / static_cast(num_steps); double yaw = step * angle_to_goal; @@ -177,7 +181,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( next_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); geometry_msgs::msg::PoseStamped costmap_pose; tf2::doTransform(next_pose, costmap_pose, costmap_transform); - if (inCollision( + if (use_collision_detection && inCollision( costmap_pose.pose.position.x, costmap_pose.pose.position.y, tf2::getYaw(costmap_pose.pose.orientation))) { @@ -348,7 +352,7 @@ bool GracefulController::simulateTrajectory( // Check for collision geometry_msgs::msg::PoseStamped global_pose; tf2::doTransform(next_pose, global_pose, costmap_transform); - if (inCollision( + if (params_->use_collision_detection && inCollision( global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation))) { diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp index f8ba2aa075c..a06ece609fa 100644 --- a/nav2_graceful_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -70,6 +70,8 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".allow_backward", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, plugin_name_ + ".in_place_collision_resolution", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true)); node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); node->get_parameter(plugin_name_ + ".min_lookahead", params_.min_lookahead); @@ -103,6 +105,8 @@ ParameterHandler::ParameterHandler( node->get_parameter(plugin_name_ + ".allow_backward", params_.allow_backward); node->get_parameter( plugin_name_ + ".in_place_collision_resolution", params_.in_place_collision_resolution); + node->get_parameter( + plugin_name_ + ".use_collision_detection", params_.use_collision_detection); if (params_.initial_rotation && params_.allow_backward) { RCLCPP_WARN( From bb8ea0d14a4cbaf47cd33cc3d21bfba9a31f94a9 Mon Sep 17 00:00:00 2001 From: suchetanrs Date: Sun, 23 Mar 2025 09:51:42 +0530 Subject: [PATCH 2/4] * Fix linting errors Signed-off-by: suchetanrs --- nav2_graceful_controller/src/graceful_controller.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index 2ffba55c3c2..d4caa8a6ae5 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -56,10 +56,9 @@ void GracefulController::configure( params_->v_linear_min, params_->v_linear_max, params_->v_angular_max); // Initialize footprint collision checker - if(params_->use_collision_detection) - { + if(params_->use_collision_detection) { collision_checker_ = std::make_unique>(costmap_ros_->getCostmap()); + FootprintCollisionChecker>(costmap_ros_->getCostmap()); } // Publishers From ff306b5080476fc0d4bc9a957404ffc126352b9d Mon Sep 17 00:00:00 2001 From: suchetanrs Date: Tue, 25 Mar 2025 13:00:57 +0530 Subject: [PATCH 3/4] * Address PR comments * Add parameter to dynamic reconfigure Signed-off-by: suchetanrs --- nav2_graceful_controller/src/graceful_controller.cpp | 3 +-- nav2_graceful_controller/src/parameter_handler.cpp | 2 ++ 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index d4caa8a6ae5..da880c7dc8e 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -171,7 +171,6 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( // Need to check at least the end pose num_steps = std::max(static_cast(1), num_steps); bool collision_free = true; - bool use_collision_detection = params_->use_collision_detection; for (size_t i = 1; i <= num_steps; ++i) { double step = static_cast(i) / static_cast(num_steps); double yaw = step * angle_to_goal; @@ -180,7 +179,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( next_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); geometry_msgs::msg::PoseStamped costmap_pose; tf2::doTransform(next_pose, costmap_pose, costmap_transform); - if (use_collision_detection && inCollision( + if (params_->use_collision_detection && inCollision( costmap_pose.pose.position.x, costmap_pose.pose.position.y, tf2::getYaw(costmap_pose.pose.orientation))) { diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp index a06ece609fa..7b2f7c42936 100644 --- a/nav2_graceful_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -191,6 +191,8 @@ ParameterHandler::dynamicParametersCallback(std::vector param continue; } params_.allow_backward = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_collision_detection") { + params_.use_collision_detection = parameter.as_bool(); } } } From 03a78b13fd15e856be2137e6e2b2c8dc880b832d Mon Sep 17 00:00:00 2001 From: suchetanrs Date: Tue, 25 Mar 2025 16:19:51 +0530 Subject: [PATCH 4/4] * Add test for the use_collision_detection parameter Signed-off-by: suchetanrs --- nav2_graceful_controller/test/test_graceful_controller.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index de95598bb0f..98e9dbd4e60 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -248,6 +248,8 @@ TEST(GracefulControllerTest, dynamicParameters) { node, "test.initial_rotation", rclcpp::ParameterValue(true)); nav2_util::declare_parameter_if_not_declared( node, "test.allow_backward", rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + node, "test.use_collision_detection", rclcpp::ParameterValue(true)); // Create controller auto controller = std::make_shared(); @@ -282,6 +284,7 @@ TEST(GracefulControllerTest, dynamicParameters) { rclcpp::Parameter("test.prefer_final_rotation", false), rclcpp::Parameter("test.rotation_scaling_factor", 13.0), rclcpp::Parameter("test.allow_backward", true), + rclcpp::Parameter("test.use_collision_detection", false), rclcpp::Parameter("test.in_place_collision_resolution", 15.0)}); // Spin @@ -305,6 +308,7 @@ TEST(GracefulControllerTest, dynamicParameters) { EXPECT_EQ(node->get_parameter("test.prefer_final_rotation").as_bool(), false); EXPECT_EQ(node->get_parameter("test.rotation_scaling_factor").as_double(), 13.0); EXPECT_EQ(node->get_parameter("test.allow_backward").as_bool(), true); + EXPECT_EQ(node->get_parameter("test.use_collision_detection").as_bool(), false); EXPECT_EQ(node->get_parameter("test.in_place_collision_resolution").as_double(), 15.0); // Set initial rotation to true