Rename dock_collision_threshold

Signed-off-by: Alberto Tudela <[email protected]>

nav2_docking/opennav_docking/include/opennav_docking/controller.hpp

@@ -118,7 +118,7 @@ class Controller
   bool use_collision_detection_;
   double projection_time_;
   double simulation_time_step_;
-  double collision_tolerance_;
+  double dock_collision_threshold_;
   double transform_tolerance_;
   std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
   std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;

nav2_docking/opennav_docking/src/controller.cpp

@@ -64,7 +64,7 @@ Controller::Controller(
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1));
   nav2_util::declare_parameter_if_not_declared(
-    node, "controller.collision_tolerance", rclcpp::ParameterValue(0.30));
+    node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3));
 
   node->get_parameter("controller.k_phi", k_phi_);
   node->get_parameter("controller.k_delta", k_delta_);
@@ -91,7 +91,7 @@ Controller::Controller(
     std::string costmap_topic, footprint_topic;
     node->get_parameter("controller.costmap_topic", costmap_topic);
     node->get_parameter("controller.footprint_topic", footprint_topic);
-    node->get_parameter("controller.collision_tolerance", collision_tolerance_);
+    node->get_parameter("controller.dock_collision_threshold", dock_collision_threshold_);
     configureCollisionChecker(node, costmap_topic, footprint_topic, transform_tolerance_);
   }
 
@@ -165,15 +165,15 @@ bool Controller::isTrajectoryCollisionFree(
       nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose) :
       std::hypot(next_pose.pose.position.x, next_pose.pose.position.y);
 
-    // If this distance is greater than the collision_tolerance, check for collisions
-    auto projected_pose = nav_2d_utils::poseToPose2D(local_pose.pose);
+    // If this distance is greater than the dock_collision_threshold, check for collisions
     if (use_collision_detection_ &&
-      dock_collision_distance > collision_tolerance_ &&
-      !collision_checker_->isCollisionFree(projected_pose))
+      dock_collision_distance > dock_collision_threshold_ &&
+      !collision_checker_->isCollisionFree(nav_2d_utils::poseToPose2D(local_pose.pose)))
     {
       RCLCPP_WARN(
         logger_, "Collision detected at pose: (%.2f, %.2f, %.2f) in frame %s",
-        projected_pose.x, projected_pose.y, projected_pose.theta, fixed_frame_.c_str());
+        local_pose.pose.position.x, local_pose.pose.position.y, local_pose.pose.position.z,
+        local_pose.header.frame_id.c_str());
       trajectory_pub_->publish(trajectory);
       return false;
     }
@@ -226,8 +226,8 @@ Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
         projection_time_ = parameter.as_double();
       } else if (name == "controller.simulation_time_step") {
         simulation_time_step_ = parameter.as_double();
-      } else if (name == "controller.collision_tolerance") {
-        collision_tolerance_ = parameter.as_double();
+      } else if (name == "controller.dock_collision_threshold") {
+        dock_collision_threshold_ = parameter.as_double();
       }
 
       // Update the smooth control law with the new params

nav2_docking/opennav_docking/test/test_controller.cpp

@@ -59,7 +59,7 @@ class ControllerFixture : public opennav_docking::Controller
 
   void setCollisionTolerance(double tolerance)
   {
-    collision_tolerance_ = tolerance;
+    dock_collision_threshold_ = tolerance;
   }
 };
 
@@ -237,7 +237,7 @@ TEST(ControllerTests, DynamicParameters) {
       rclcpp::Parameter("controller.slowdown_radius", 8.0),
       rclcpp::Parameter("controller.projection_time", 9.0),
       rclcpp::Parameter("controller.simulation_time_step", 10.0),
-      rclcpp::Parameter("controller.collision_tolerance", 11.0)});
+      rclcpp::Parameter("controller.dock_collision_threshold", 11.0)});
 
   // Spin
   rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
@@ -253,7 +253,7 @@ TEST(ControllerTests, DynamicParameters) {
   EXPECT_EQ(node->get_parameter("controller.slowdown_radius").as_double(), 8.0);
   EXPECT_EQ(node->get_parameter("controller.projection_time").as_double(), 9.0);
   EXPECT_EQ(node->get_parameter("controller.simulation_time_step").as_double(), 10.0);
-  EXPECT_EQ(node->get_parameter("controller.collision_tolerance").as_double(), 11.0);
+  EXPECT_EQ(node->get_parameter("controller.dock_collision_threshold").as_double(), 11.0);
 }
 
 TEST(ControllerTests, TFException)
@@ -285,7 +285,7 @@ TEST(ControllerTests, CollisionCheckerDockForward) {
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1));
   nav2_util::declare_parameter_if_not_declared(
-    node, "controller.collision_tolerance", rclcpp::ParameterValue(0.3));
+    node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3));
 
   auto controller = std::make_unique<opennav_docking::ControllerFixture>(
     node, tf, "test_base_frame", "test_base_frame");
@@ -350,7 +350,7 @@ TEST(ControllerTests, CollisionCheckerDockBackward) {
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1));
   nav2_util::declare_parameter_if_not_declared(
-    node, "controller.collision_tolerance", rclcpp::ParameterValue(0.3));
+    node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3));
 
   auto controller = std::make_unique<opennav_docking::ControllerFixture>(
     node, tf, "test_base_frame", "test_base_frame");
@@ -415,7 +415,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) {
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1));
   nav2_util::declare_parameter_if_not_declared(
-    node, "controller.collision_tolerance", rclcpp::ParameterValue(0.3));
+    node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3));
 
   auto controller = std::make_unique<opennav_docking::ControllerFixture>(
     node, tf, "test_base_frame", "test_base_frame");
@@ -488,7 +488,7 @@ TEST(ControllerTests, CollisionCheckerUndockForward) {
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1));
   nav2_util::declare_parameter_if_not_declared(
-    node, "controller.collision_tolerance", rclcpp::ParameterValue(0.3));
+    node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3));
 
   auto controller = std::make_unique<opennav_docking::ControllerFixture>(
     node, tf, "test_base_frame", "test_base_frame");