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..da880c7dc8e 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -56,8 +56,10 @@ 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); @@ -177,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 (inCollision( + if (params_->use_collision_detection && inCollision( costmap_pose.pose.position.x, costmap_pose.pose.position.y, tf2::getYaw(costmap_pose.pose.orientation))) { @@ -348,7 +350,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..7b2f7c42936 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( @@ -187,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(); } } } 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