* Parametrize collision checking in nav2_graceful_controller (ros-navigation#5006)

* * Parametrize collision checking in nav2_graceful_controller

Signed-off-by: suchetanrs <[email protected]>

* * Fix linting errors

Signed-off-by: suchetanrs <[email protected]>

* * Address PR comments
* Add parameter to dynamic reconfigure

Signed-off-by: suchetanrs <[email protected]>

* * Add test for the use_collision_detection parameter

Signed-off-by: suchetanrs <[email protected]>

---------

Signed-off-by: suchetanrs <[email protected]>

Author: suchetanrs

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;
 };
 
 /**

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<nav2_costmap_2d::
-      FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>(costmap_ros_->getCostmap());
+  if(params_->use_collision_detection) {
+    collision_checker_ = std::make_unique<nav2_costmap_2d::
+        FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>(costmap_ros_->getCostmap());
+  }
 
   // Publishers
   transformed_plan_pub_ = node->create_publisher<nav_msgs::msg::Path>("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)))
     {

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<rclcpp::Parameter> param
           continue;
         }
         params_.allow_backward = parameter.as_bool();
+      } else if (name == plugin_name_ + ".use_collision_detection") {
+        params_.use_collision_detection = parameter.as_bool();
       }
     }
   }

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<GMControllerFixture>();
@@ -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