Minor fixes

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

nav2_docking/opennav_docking/include/opennav_docking/controller.hpp

@@ -101,6 +101,7 @@ class Controller
   std::mutex dynamic_params_lock_;
 
   rclcpp::Logger logger_{rclcpp::get_logger("Controller")};
+  rclcpp::Clock::SharedPtr clock_;
 
   // Smooth control law
   std::unique_ptr<nav2_graceful_controller::SmoothControlLaw> control_law_;

nav2_docking/opennav_docking/src/controller.cpp

@@ -31,6 +31,7 @@ Controller::Controller(
 : tf2_buffer_(tf), fixed_frame_(fixed_frame), base_frame_(base_frame)
 {
   logger_ = node->get_logger();
+  clock_ = node->get_clock();
 
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.k_phi", rclcpp::ParameterValue(3.0));
@@ -96,7 +97,7 @@ Controller::Controller(
   }
 
   trajectory_pub_ =
-    node->create_publisher<nav_msgs::msg::Path>(node->get_name() + std::string("/trajectory"), 1);
+    node->create_publisher<nav_msgs::msg::Path>("docking_trajectory", 1);
 }
 
 Controller::~Controller()
@@ -122,13 +123,26 @@ bool Controller::isTrajectoryCollisionFree(
   // Visualization of the trajectory
   nav_msgs::msg::Path trajectory;
   trajectory.header.frame_id = base_frame_;
+  trajectory.header.stamp = clock_->now();
 
   // First pose
   geometry_msgs::msg::PoseStamped next_pose;
   next_pose.header.frame_id = base_frame_;
   next_pose.pose.orientation.w = 1.0;
   trajectory.poses.push_back(next_pose);
 
+  // Get the transform from base_frame to fixed_frame
+  geometry_msgs::msg::TransformStamped base_to_fixed_transform;
+  try {
+    base_to_fixed_transform =
+      tf2_buffer_->lookupTransform(fixed_frame_, base_frame_, tf2::TimePointZero);
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR(
+      logger_, "Could not get transform from %s to %s: %s",
+      base_frame_.c_str(), fixed_frame_.c_str(), ex.what());
+    return false;
+  }
+
   // Generate path
   for (double t = 0; t < projection_time_; t += simulation_time_step_) {
     // Apply velocities to calculate next pose
@@ -140,18 +154,24 @@ bool Controller::isTrajectoryCollisionFree(
 
     // Transform pose from base_frame into fixed_frame
     geometry_msgs::msg::PoseStamped local_pose = next_pose;
-    local_pose.header.stamp = rclcpp::Time(0);
-    tf2_buffer_->transform(local_pose, local_pose, fixed_frame_);
+    local_pose.header.stamp = trajectory.header.stamp;
+    tf2::doTransform(local_pose, local_pose, base_to_fixed_transform);
+
+    // Compute the distance between the end of the current trajectory and the dock pose
+    double dock_collision_distance =
+      nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose);
 
-    // Check for collisions at the projected pose
+    // If this distance is greater than the collision_tolerance, check for collisions
+    // Skipping the last part of the trajectory where the dock should be
     auto projected_pose = nav_2d_utils::poseToPose2D(local_pose.pose);
-    double distance = nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose);
     if (use_collision_detection_ &&
-      !collision_checker_->isCollisionFree(projected_pose) && distance > collision_tolerance_)
+      dock_collision_distance > collision_tolerance_ &&
+      !collision_checker_->isCollisionFree(projected_pose))
     {
-      RCLCPP_INFO(
+      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());
+      trajectory_pub_->publish(trajectory);
       return false;
     }
   }
@@ -203,6 +223,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();
       }
 
       // Update the smooth control law with the new params

nav2_docking/opennav_docking/test/test_controller.cpp

@@ -88,7 +88,8 @@ TEST(ControllerTests, DynamicParameters) {
       rclcpp::Parameter("controller.v_angular_max", 7.0),
       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.simulation_time_step", 10.0),
+      rclcpp::Parameter("controller.collision_tolerance", 11.0)});
 
   // Spin
   rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
@@ -104,6 +105,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);
 }
 
 }  // namespace opennav_docking

nav2_docking/opennav_docking/test/test_docking_server.py

@@ -41,15 +41,16 @@ def generate_test_description():
             executable='opennav_docking',
             name='docking_server',
             parameters=[{'wait_charge_timeout': 1.0,
+                         'controller': {'use_collision_detection': False},
                          'dock_plugins': ['test_dock_plugin'],
                          'test_dock_plugin': {
-                            'plugin': 'opennav_docking::SimpleChargingDock',
-                            'use_battery_status': True},
+                             'plugin': 'opennav_docking::SimpleChargingDock',
+                             'use_battery_status': True},
                          'docks': ['test_dock'],
                          'test_dock': {
-                            'type': 'test_dock_plugin',
-                            'frame': 'odom',
-                            'pose': [10.0, 0.0, 0.0]
+                             'type': 'test_dock_plugin',
+                             'frame': 'odom',
+                             'pose': [10.0, 0.0, 0.0]
                          }}],
             output='screen',
         ),