From 6412d2d09349692493fc785e7aa696202f69455d Mon Sep 17 00:00:00 2001
From: Jakubach <jakubach@gmail.com>
Date: Fri, 22 Nov 2024 15:46:10 +0100
Subject: [PATCH] Added collision detection for docking (#4752)

* Added collision detection for docking

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Minor fixes

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Improve collision  while undocking and test

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Fix smoke testing

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Rename dock_collision_threshold

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Added docs and params

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Minor changes in README

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

---------

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Jakubach <jakubach@gmail.com>
---
 nav2_bringup/params/nav2_params.yaml          |   7 +
 nav2_docking/README.md                        |  18 +
 .../include/opennav_docking/controller.hpp    |  72 ++-
 .../opennav_docking/docking_server.hpp        |   3 +-
 .../opennav_docking/src/controller.cpp        | 144 +++++-
 .../opennav_docking/src/docking_server.cpp    |  17 +-
 .../opennav_docking/test/test_controller.cpp  | 476 +++++++++++++++++-
 .../test/test_docking_server.py               |  14 +-
 8 files changed, 721 insertions(+), 30 deletions(-)

diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index 2027f4c0e1c..69fe42195eb 100644
--- a/nav2_bringup/params/nav2_params.yaml
+++ b/nav2_bringup/params/nav2_params.yaml
@@ -421,6 +421,13 @@ docking_server:
       k_delta: 2.0
       v_linear_min: 0.15
       v_linear_max: 0.15
+      use_collision_detection: true
+      costmap_topic: "/local_costmap/costmap_raw"
+      footprint_topic: "/local_costmap/published_footprint"
+      transform_tolerance: 0.1
+      projection_time: 5.0
+      simulation_step: 0.1
+      dock_collision_threshold: 0.3
 
 loopback_simulator:
   ros__parameters:
diff --git a/nav2_docking/README.md b/nav2_docking/README.md
index 0a18ae6cccb..6eeb56e8afa 100644
--- a/nav2_docking/README.md
+++ b/nav2_docking/README.md
@@ -209,6 +209,7 @@ For debugging purposes, there are several publishers which can be used with RVIZ
 | dock_database  |  The filepath to the dock database to use for this environment | string |  N/A  |
 | docks  |  Instead of `dock_database`, the set of docks specified in the params file itself | vector<string> | N/A     |
 | navigator_bt_xml  | BT XML to use for Navigator, if non-default | string | ""     |
+<<<<<<< HEAD
 | controller.k_phi  | Ratio of the rate of change of angle relative to distance from the target. Much be > 0. | double | 3.0  |
 | controller.k_delta  |  Higher values result in converging to the target more quickly. | double | 2.0     |
 | controller.beta  |  Parameter to reduce linear velocity proportional to path curvature. Increasing this linearly reduces the velocity (v(t) = v_max / (1 + beta * |curv|^lambda)). | double | 0.4  |
@@ -218,6 +219,23 @@ For debugging purposes, there are several publishers which can be used with RVIZ
 | controller.v_angular_min |  Minimum angular velocity for approaching dock. | double | 0.15    |
 | controller.v_angular_max |  Maximum angular velocity for approaching dock. | double | 0.75    |
 | controller.slowdown_radius |  Radius to end goal to commense slow down. | double | 0.25     |
+=======
+| controller.k_phi  | Ratio of the rate of change in phi to the rate of change in r. Controls the convergence of the slow subsystem  | double | 3.0  |
+| controller.k_delta  | Constant factor applied to the heading error feedback. Controls the convergence of the fast subsystem | double | 2.0     |
+| controller.beta  | Constant factor applied to the path curvature. This value must be positive. Determines how fast the velocity drops when the curvature increases | double | 0.4  |
+| controller.lambda  | Constant factor applied to the path curvature. This value must be greater or equal to 1. Determines the sharpness of the curve: higher lambda implies sharper curves | double | 2.0     |
+| controller.v_linear_min | Minimum linear velocity (m/s) | double | 0.1     |
+| controller.v_linear_max | Maximum linear velocity (m/s) | double | 0.25    |
+| controller.v_angular_max | Maximum angular velocity (rad/s) produced by the control law | double | 0.75    |
+| controller.slowdown_radius | Radius (m) around the goal pose in which the robot will start to slow down | double | 0.25     |
+| controller.use_collision_detection | Whether to use collision detection to avoid obstacles | bool | true     |
+| controller.costmap_topic | The topic to use for the costmap | string | "local_costmap/costmap_raw"     |
+| controller.footprint_topic | The topic to use for the robot's footprint | string | "local_costmap/published_footprint"     |
+| controller.transform_tolerance | Time with which to post-date the transform that is published, to indicate that this transform is valid into the future. | double | 0.1     |
+| controller.projection_time | Time to look ahead for collisions (s). | double | 5.0     |
+| controller.simulation_time_step | Time step for projections (s). | double | 0.1     |
+| controller.dock_collision_threshold | Distance (m) from the dock pose to ignore collisions. | double | 0.3     |
+>>>>>>> 90a6c8d8 (Added collision detection for docking (#4752))
 
 Note: `dock_plugins` and either `docks` or `dock_database` are required.
 
diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp
index 354522cbc44..b279e57f3ac 100644
--- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp
+++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp
@@ -1,4 +1,5 @@
 // Copyright (c) 2024 Open Navigation LLC
+// Copyright (c) 2024 Alberto J. Tudela Roldán
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -16,11 +17,16 @@
 #define OPENNAV_DOCKING__CONTROLLER_HPP_
 
 #include <memory>
+#include <string>
 #include <vector>
 
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/twist.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+#include "nav2_costmap_2d/footprint_subscriber.hpp"
+#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
 #include "nav2_graceful_controller/smooth_control_law.hpp"
+#include "nav_msgs/msg/path.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 
 namespace opennav_docking
@@ -34,27 +40,65 @@ class Controller
 public:
   /**
    * @brief Create a controller instance. Configure ROS 2 parameters.
+   *
+   * @param node Lifecycle node
+   * @param tf tf2_ros TF buffer
+   * @param fixed_frame Fixed frame
+   * @param base_frame Robot base frame
    */
-  explicit Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node);
+  Controller(
+    const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, std::shared_ptr<tf2_ros::Buffer> tf,
+    std::string fixed_frame, std::string base_frame);
+
+  /**
+   * @brief A destructor for opennav_docking::Controller
+   */
+  ~Controller();
 
   /**
    * @brief Compute a velocity command using control law.
    * @param pose Target pose, in robot centric coordinates.
    * @param cmd Command velocity.
+   * @param is_docking If true, robot is docking. If false, robot is undocking.
    * @param backward If true, robot will drive backwards to goal.
    * @returns True if command is valid, false otherwise.
    */
   bool computeVelocityCommand(
-    const geometry_msgs::msg::Pose & pose,
-    geometry_msgs::msg::Twist & cmd, bool backward = false);
+    const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool is_docking,
+    bool backward = false);
+
+protected:
+  /**
+   * @brief Check if a trajectory is collision free.
+   *
+   * @param target_pose Target pose, in robot centric coordinates.
+   * @param is_docking If true, robot is docking. If false, robot is undocking.
+   * @param backward If true, robot will drive backwards to goal.
+   * @return True if trajectory is collision free.
+   */
+  bool isTrajectoryCollisionFree(
+    const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward = false);
 
   /**
-   * @brief Callback executed when a parameter change is detected
+   * @brief Callback executed when a parameter change is detected.
+   *
    * @param event ParameterEvent message
    */
   rcl_interfaces::msg::SetParametersResult
   dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
 
+  /**
+   * @brief Configure the collision checker.
+   *
+   * @param node Lifecycle node
+   * @param costmap_topic Costmap topic
+   * @param footprint_topic Footprint topic
+   * @param transform_tolerance Transform tolerance
+   */
+  void configureCollisionChecker(
+    const rclcpp_lifecycle::LifecycleNode::SharedPtr & node,
+    std::string costmap_topic, std::string footprint_topic, double transform_tolerance);
+
   // Dynamic parameters handler
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
   std::mutex dynamic_params_lock_;
@@ -72,11 +116,29 @@ class Controller
     const double & dt);
 
 protected:
-  std::unique_ptr<nav2_graceful_controller::SmoothControlLaw> control_law_;
+  rclcpp::Logger logger_{rclcpp::get_logger("Controller")};
+  rclcpp::Clock::SharedPtr clock_;
 
+  // Smooth control law
+  std::unique_ptr<nav2_graceful_controller::SmoothControlLaw> control_law_;
   double k_phi_, k_delta_, beta_, lambda_;
   double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_;
   double rotate_to_heading_angular_vel_, rotate_to_heading_max_angular_accel_;
+
+  // The trajectory of the robot while dock / undock for visualization / debug purposes
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr trajectory_pub_;
+
+  // Used for collision checking
+  bool use_collision_detection_;
+  double projection_time_;
+  double simulation_time_step_;
+  double dock_collision_threshold_;
+  double transform_tolerance_;
+  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
+  std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
+  std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
+  std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
+  std::string fixed_frame_, base_frame_;
 };
 
 }  // namespace opennav_docking
diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
index 7e834aab670..ea3f288a34a 100644
--- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
+++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
@@ -118,12 +118,13 @@ class DockingServer : public nav2_util::LifecycleNode
    * @param pose The pose to command towards.
    * @param linear_tolerance Pose is reached when linear distance is within this tolerance.
    * @param angular_tolerance Pose is reached when angular distance is within this tolerance.
+   * @param is_docking If true, the robot is docking. If false, the robot is undocking.
    * @param backward If true, the robot will drive backwards.
    * @returns True if pose is reached.
    */
   bool getCommandToPose(
     geometry_msgs::msg::Twist & cmd, const geometry_msgs::msg::PoseStamped & pose,
-    double linear_tolerance, double angular_tolerance, bool backward);
+    double linear_tolerance, double angular_tolerance, bool is_docking, bool backward);
 
   /**
    * @brief Get the robot pose (aka base_frame pose) in another frame.
diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp
index 20fbbe9d2cc..76a429ee651 100644
--- a/nav2_docking/opennav_docking/src/controller.cpp
+++ b/nav2_docking/opennav_docking/src/controller.cpp
@@ -1,4 +1,5 @@
 // Copyright (c) 2024 Open Navigation LLC
+// Copyright (c) 2024 Alberto J. Tudela Roldán
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -16,13 +17,22 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "opennav_docking/controller.hpp"
+#include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/node_utils.hpp"
+#include "nav_2d_utils/conversions.hpp"
+#include "tf2/utils.h"
 
 namespace opennav_docking
 {
 
-Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
+Controller::Controller(
+  const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, std::shared_ptr<tf2_ros::Buffer> tf,
+  std::string fixed_frame, std::string base_frame)
+: 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));
   nav2_util::declare_parameter_if_not_declared(
@@ -35,8 +45,6 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
     node, "controller.v_linear_min", rclcpp::ParameterValue(0.1));
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.v_linear_max", rclcpp::ParameterValue(0.25));
-  nav2_util::declare_parameter_if_not_declared(
-    node, "controller.v_angular_min", rclcpp::ParameterValue(0.15));
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.v_angular_max", rclcpp::ParameterValue(0.75));
   nav2_util::declare_parameter_if_not_declared(
@@ -45,6 +53,22 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
     node, "controller.rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.0));
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.rotate_to_heading_max_angular_accel", rclcpp::ParameterValue(3.2));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.use_collision_detection", rclcpp::ParameterValue(true));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.costmap_topic",
+    rclcpp::ParameterValue(std::string("local_costmap/costmap_raw")));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.footprint_topic", rclcpp::ParameterValue(
+      std::string("local_costmap/published_footprint")));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.transform_tolerance", rclcpp::ParameterValue(0.1));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.projection_time", rclcpp::ParameterValue(5.0));
+  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.dock_collision_threshold", rclcpp::ParameterValue(0.3));
 
   node->get_parameter("controller.k_phi", k_phi_);
   node->get_parameter("controller.k_delta", k_delta_);
@@ -63,17 +87,120 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
   // Add callback for dynamic parameters
   dyn_params_handler_ = node->add_on_set_parameters_callback(
     std::bind(&Controller::dynamicParametersCallback, this, std::placeholders::_1));
+
+  node->get_parameter("controller.use_collision_detection", use_collision_detection_);
+  node->get_parameter("controller.projection_time", projection_time_);
+  node->get_parameter("controller.simulation_time_step", simulation_time_step_);
+  node->get_parameter("controller.transform_tolerance", transform_tolerance_);
+
+  if (use_collision_detection_) {
+    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.dock_collision_threshold", dock_collision_threshold_);
+    configureCollisionChecker(node, costmap_topic, footprint_topic, transform_tolerance_);
+  }
+
+  trajectory_pub_ =
+    node->create_publisher<nav_msgs::msg::Path>("docking_trajectory", 1);
+}
+
+Controller::~Controller()
+{
+  control_law_.reset();
+  trajectory_pub_.reset();
+  collision_checker_.reset();
+  costmap_sub_.reset();
+  footprint_sub_.reset();
 }
 
 bool Controller::computeVelocityCommand(
-  const geometry_msgs::msg::Pose & pose,
-  geometry_msgs::msg::Twist & cmd, bool backward)
+  const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool is_docking,
+  bool backward)
 {
   std::lock_guard<std::mutex> lock(dynamic_params_lock_);
   cmd = control_law_->calculateRegularVelocity(pose, backward);
+  return isTrajectoryCollisionFree(pose, is_docking, backward);
+}
+
+bool Controller::isTrajectoryCollisionFree(
+  const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward)
+{
+  // 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_;
+  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_, trajectory.header.stamp,
+      tf2::durationFromSec(transform_tolerance_));
+  } 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
+    next_pose.pose = control_law_->calculateNextPose(
+      simulation_time_step_, target_pose, next_pose.pose, backward);
+
+    // Add the pose to the trajectory for visualization
+    trajectory.poses.push_back(next_pose);
+
+    // Transform pose from base_frame into fixed_frame
+    geometry_msgs::msg::PoseStamped local_pose = next_pose;
+    local_pose.header.stamp = trajectory.header.stamp;
+    tf2::doTransform(local_pose, local_pose, base_to_fixed_transform);
+
+    // Determine the distance at which to check for collisions
+    // Skip the final segment of the trajectory for docking
+    // and the initial segment for undocking
+    // This avoids false positives when the robot is at the dock
+    double dock_collision_distance = is_docking ?
+      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 dock_collision_threshold, check for collisions
+    if (use_collision_detection_ &&
+      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",
+        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;
+    }
+  }
+
+  trajectory_pub_->publish(trajectory);
+
   return true;
 }
 
+void Controller::configureCollisionChecker(
+  const rclcpp_lifecycle::LifecycleNode::SharedPtr & node,
+  std::string costmap_topic, std::string footprint_topic, double transform_tolerance)
+{
+  costmap_sub_ = std::make_unique<nav2_costmap_2d::CostmapSubscriber>(node, costmap_topic);
+  footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
+    node, footprint_topic, *tf2_buffer_, base_frame_, transform_tolerance);
+  collision_checker_ = std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
+    *costmap_sub_, *footprint_sub_, node->get_name());
+}
+
 rcl_interfaces::msg::SetParametersResult
 Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
 {
@@ -105,6 +232,13 @@ Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
         rotate_to_heading_angular_vel_ = parameter.as_double();
       } else if (name == "controller.rotate_to_heading_max_angular_accel") {
         rotate_to_heading_max_angular_accel_ = parameter.as_double();
+      } else if (name == "controller.projection_time") {
+        projection_time_ = parameter.as_double();
+      } else if (name == "controller.simulation_time_step") {
+        simulation_time_step_ = 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
       control_law_->setCurvatureConstants(k_phi_, k_delta_, beta_, lambda_);
diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp
index d4fa4910828..2bab3407f4c 100644
--- a/nav2_docking/opennav_docking/src/docking_server.cpp
+++ b/nav2_docking/opennav_docking/src/docking_server.cpp
@@ -100,7 +100,7 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state)
 
   // Create composed utilities
   mutex_ = std::make_shared<std::mutex>();
-  controller_ = std::make_unique<Controller>(node);
+  controller_ = std::make_unique<Controller>(node, tf2_buffer_, fixed_frame_, base_frame_);
   navigator_ = std::make_unique<Navigator>(node);
   dock_db_ = std::make_unique<DockDatabase>(mutex_);
   if (!dock_db_->initialize(node, tf2_buffer_)) {
@@ -263,8 +263,7 @@ void DockingServer::dockRobot()
     // Send robot to its staging pose
     publishDockingFeedback(DockRobot::Feedback::NAV_TO_STAGING_POSE);
     const auto initial_staging_pose = dock->getStagingPose();
-    const auto robot_pose = getRobotPoseInFrame(
-      initial_staging_pose.header.frame_id);
+    const auto robot_pose = getRobotPoseInFrame(initial_staging_pose.header.frame_id);
     if (!goal->navigate_to_staging_pose ||
       utils::l2Norm(robot_pose.pose, initial_staging_pose.pose) < dock_prestaging_tolerance_)
     {
@@ -499,8 +498,8 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped &
     // Compute and publish controls
     auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
     command->header.stamp = now();
-    if (!controller_->computeVelocityCommand(target_pose.pose,
-    command->twist, dock_backwards_))
+    if (!controller_->computeVelocityCommand(target_pose.pose, command->twist, true,
+        dock_backwards_))
     {
       throw opennav_docking_core::FailedToControl("Failed to get control");
     }
@@ -567,7 +566,7 @@ bool DockingServer::resetApproach(const geometry_msgs::msg::PoseStamped & stagin
     auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
     command->header.stamp = now();
     if (getCommandToPose(
-        command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_,
+        command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, false,
         !dock_backwards_))
     {
       return true;
@@ -585,7 +584,7 @@ bool DockingServer::resetApproach(const geometry_msgs::msg::PoseStamped & stagin
 
 bool DockingServer::getCommandToPose(
   geometry_msgs::msg::Twist & cmd, const geometry_msgs::msg::PoseStamped & pose,
-  double linear_tolerance, double angular_tolerance, bool backward)
+  double linear_tolerance, double angular_tolerance, bool is_docking, bool backward)
 {
   // Reset command to zero velocity
   cmd.linear.x = 0;
@@ -608,7 +607,7 @@ bool DockingServer::getCommandToPose(
   tf2_buffer_->transform(target_pose, target_pose, base_frame_);
 
   // Compute velocity command
-  if (!controller_->computeVelocityCommand(target_pose.pose, cmd, backward)) {
+  if (!controller_->computeVelocityCommand(target_pose.pose, cmd, is_docking, backward)) {
     throw opennav_docking_core::FailedToControl("Failed to get control");
   }
 
@@ -695,7 +694,7 @@ void DockingServer::undockRobot()
       auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
       command->header.stamp = now();
       if (getCommandToPose(
-          command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_,
+          command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, false,
           !dock_backwards_))
       {
         RCLCPP_INFO(get_logger(), "Robot has reached staging pose");
diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp
index 9ad599fd72e..d1fb6ffbe01 100644
--- a/nav2_docking/opennav_docking/test/test_controller.cpp
+++ b/nav2_docking/opennav_docking/test/test_controller.cpp
@@ -1,4 +1,5 @@
 // Copyright (c) 2024 Open Navigation LLC
+// Copyright (c) 2024 Alberto J. Tudela Roldán
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -17,6 +18,10 @@
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 #include "opennav_docking/controller.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "tf2_ros/buffer.h"
 #include "ament_index_cpp/get_package_share_directory.hpp"
 
 // Testing the controller at high level; the nav2_graceful_controller
@@ -33,21 +38,187 @@ RosLockGuard g_rclcpp;
 namespace opennav_docking
 {
 
+class ControllerFixture : public opennav_docking::Controller
+{
+public:
+  ControllerFixture(
+    const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, std::shared_ptr<tf2_ros::Buffer> tf,
+    std::string fixed_frame, std::string base_frame)
+  : Controller(node, tf, fixed_frame, base_frame)
+  {
+  }
+
+  ~ControllerFixture() = default;
+
+  bool isTrajectoryCollisionFree(
+    const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward = false)
+  {
+    return opennav_docking::Controller::isTrajectoryCollisionFree(
+      target_pose, is_docking, backward);
+  }
+
+  void setCollisionTolerance(double tolerance)
+  {
+    dock_collision_threshold_ = tolerance;
+  }
+};
+
+class TestCollisionChecker : public nav2_util::LifecycleNode
+{
+public:
+  explicit TestCollisionChecker(std::string name)
+  : LifecycleNode(name)
+  {
+  }
+
+  ~TestCollisionChecker()
+  {
+    footprint_pub_.reset();
+    costmap_pub_.reset();
+  }
+
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & /*state*/)
+  {
+    RCLCPP_INFO(this->get_logger(), "Configuring");
+
+    costmap_ = std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, -5.0, -5.0);
+
+    footprint_pub_ = create_publisher<geometry_msgs::msg::PolygonStamped>(
+      "test_footprint", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+    costmap_pub_ = std::make_shared<nav2_costmap_2d::Costmap2DPublisher>(
+      shared_from_this(), costmap_.get(), "test_base_frame", "test_costmap", true);
+
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/)
+  {
+    RCLCPP_INFO(this->get_logger(), "Activating");
+    costmap_pub_->on_activate();
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+  {
+    RCLCPP_INFO(this->get_logger(), "Deactivating");
+    costmap_pub_->on_deactivate();
+    costmap_.reset();
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  void publishFootprint(
+    const double radius, const double center_x, const double center_y,
+    std::string base_frame, const rclcpp::Time & stamp)
+  {
+    std::unique_ptr<geometry_msgs::msg::PolygonStamped> msg =
+      std::make_unique<geometry_msgs::msg::PolygonStamped>();
+
+    msg->header.frame_id = base_frame;
+    msg->header.stamp = stamp;
+
+    geometry_msgs::msg::Point32 p;
+
+    p.x = center_x + radius;
+    p.y = center_y + radius;
+    msg->polygon.points.push_back(p);
+
+    p.x = center_x + radius;
+    p.y = center_y - radius;
+    msg->polygon.points.push_back(p);
+
+    p.x = center_x - radius;
+    p.y = center_y - radius;
+    msg->polygon.points.push_back(p);
+
+    p.x = center_x - radius;
+    p.y = center_y + radius;
+    msg->polygon.points.push_back(p);
+
+    footprint_pub_->publish(std::move(msg));
+  }
+
+  void publishCostmap()
+  {
+    costmap_pub_->publishCostmap();
+  }
+
+  geometry_msgs::msg::Pose setPose(double x, double y, double theta)
+  {
+    geometry_msgs::msg::Pose pose;
+    pose.position.x = x;
+    pose.position.y = y;
+    pose.position.z = 0.0;
+    pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta);
+    return pose;
+  }
+
+  void setRectangle(
+    double width, double height, double center_x, double center_y, unsigned char cost)
+  {
+    unsigned int mx, my;
+    if (!costmap_->worldToMap(center_x, center_y, mx, my)) {
+      RCLCPP_ERROR(this->get_logger(), "Failed to convert world coordinates to map coordinates");
+      return;
+    }
+
+    unsigned int width_cell = static_cast<unsigned int>(width / costmap_->getResolution());
+    unsigned int height_cell = static_cast<unsigned int>(height / costmap_->getResolution());
+
+    for (unsigned int i = 0; i < width_cell; ++i) {
+      for (unsigned int j = 0; j < height_cell; ++j) {
+        costmap_->setCost(mx + i, my + j, cost);
+      }
+    }
+  }
+
+  void clearCostmap()
+  {
+    if (!costmap_) {
+      RCLCPP_ERROR(this->get_logger(), "Costmap is not initialized");
+      return;
+    }
+
+    unsigned int size_x = costmap_->getSizeInCellsX();
+    unsigned int size_y = costmap_->getSizeInCellsY();
+
+    for (unsigned int i = 0; i < size_x; ++i) {
+      for (unsigned int j = 0; j < size_y; ++j) {
+        costmap_->setCost(i, j, nav2_costmap_2d::FREE_SPACE);
+      }
+    }
+  }
+
+private:
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_;
+
+  rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_pub_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DPublisher> costmap_pub_;
+};
+
 TEST(ControllerTests, ObjectLifecycle)
 {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
-  auto controller = std::make_unique<opennav_docking::Controller>(node);
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  tf->setUsingDedicatedThread(true);  // One-thread broadcasting-listening model
+
+  // Skip collision detection
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.use_collision_detection", rclcpp::ParameterValue(false));
+
+  auto controller = std::make_unique<opennav_docking::Controller>(
+    node, tf, "test_base_frame", "test_base_frame");
 
   geometry_msgs::msg::Pose pose;
   geometry_msgs::msg::Twist cmd_out, cmd_init;
-  EXPECT_TRUE(controller->computeVelocityCommand(pose, cmd_out));
+  EXPECT_TRUE(controller->computeVelocityCommand(pose, cmd_out, true));
   EXPECT_NE(cmd_init, cmd_out);
   controller.reset();
 }
 
 TEST(ControllerTests, DynamicParameters) {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
-  auto controller = std::make_shared<opennav_docking::Controller>(node);
+  auto controller = std::make_unique<opennav_docking::Controller>(
+    node, nullptr, "test_base_frame", "test_base_frame");
 
   auto params = std::make_shared<rclcpp::AsyncParametersClient>(
     node->get_node_base_interface(), node->get_node_topics_interface(),
@@ -63,8 +234,10 @@ TEST(ControllerTests, DynamicParameters) {
       rclcpp::Parameter("controller.v_linear_min", 5.0),
       rclcpp::Parameter("controller.v_linear_max", 6.0),
       rclcpp::Parameter("controller.v_angular_max", 7.0),
-      rclcpp::Parameter("controller.v_angular_min", 2.0),
-      rclcpp::Parameter("controller.slowdown_radius", 8.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.dock_collision_threshold", 11.0)});
 
   // Spin
   rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
@@ -79,6 +252,299 @@ TEST(ControllerTests, DynamicParameters) {
   EXPECT_EQ(node->get_parameter("controller.v_angular_max").as_double(), 7.0);
   EXPECT_EQ(node->get_parameter("controller.v_angular_min").as_double(), 2.0);
   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.dock_collision_threshold").as_double(), 11.0);
 }
 
+TEST(ControllerTests, TFException)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  tf->setUsingDedicatedThread(true);  // One-thread broadcasting-listening model
+
+  auto controller = std::make_unique<opennav_docking::ControllerFixture>(
+    node, tf, "test_fixed_frame", "test_base_frame");
+
+  geometry_msgs::msg::Pose pose;
+  EXPECT_FALSE(controller->isTrajectoryCollisionFree(pose, false));
+  controller.reset();
+}
+
+TEST(ControllerTests, CollisionCheckerDockForward) {
+  auto collision_tester = std::make_shared<TestCollisionChecker>("collision_test");
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  tf->setUsingDedicatedThread(true);  // One-thread broadcasting-listening model
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.footprint_topic", rclcpp::ParameterValue("test_footprint"));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.costmap_topic", rclcpp::ParameterValue("test_costmap_raw"));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.projection_time", rclcpp::ParameterValue(10.0));
+  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.dock_collision_threshold", rclcpp::ParameterValue(0.3));
+
+  auto controller = std::make_unique<opennav_docking::ControllerFixture>(
+    node, tf, "test_base_frame", "test_base_frame");
+  collision_tester->configure();
+  collision_tester->activate();
+
+  // Set the pose of the dock at 1.75m in front of the robot
+  auto dock_pose = collision_tester->setPose(1.75, 0.0, 0.0);
+
+  // Publish a footprint of 0.5m "radius" at origin
+  auto radius = 0.5;
+  collision_tester->publishFootprint(radius, 0.0, 0.0, "test_base_frame", node->now());
+
+  // Publish an empty costmap
+  // It should not hit anything in an empty costmap
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, false));
+
+  // Set a dock in the costmap of 0.2x1.5m at 2m in front of the robot
+  // It should hit the dock because the robot is 0.5m wide and the dock pose is at 1.75
+  // But it does not hit because the collision tolerance is 0.3m
+  collision_tester->setRectangle(0.2, 1.5, 2.0, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, false));
+
+  // Set an object between the robot and the dock
+  // It should hit the object
+  collision_tester->clearCostmap();
+  collision_tester->setRectangle(0.2, 0.2, 1.0, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE);
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, false));
+
+  // Set the collision tolerance to 0 to ensure all obstacles in the path are detected
+  controller->setCollisionTolerance(0.0);
+
+  // Set a dock in the costmap of 0.2x1.5m at 2m in front of the robot
+  // Now it should hit the dock
+  collision_tester->clearCostmap();
+  collision_tester->setRectangle(0.2, 1.5, 2.0, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, false));
+
+  collision_tester->deactivate();
+}
+
+TEST(ControllerTests, CollisionCheckerDockBackward) {
+  auto collision_tester = std::make_shared<TestCollisionChecker>("collision_test");
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  tf->setUsingDedicatedThread(true);  // One-thread broadcasting-listening model
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.footprint_topic", rclcpp::ParameterValue("test_footprint"));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.costmap_topic", rclcpp::ParameterValue("test_costmap_raw"));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.projection_time", rclcpp::ParameterValue(10.0));
+  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.dock_collision_threshold", rclcpp::ParameterValue(0.3));
+
+  auto controller = std::make_unique<opennav_docking::ControllerFixture>(
+    node, tf, "test_base_frame", "test_base_frame");
+  collision_tester->configure();
+  collision_tester->activate();
+
+  // Set the pose of the dock at 1.75m behind the robot
+  auto dock_pose = collision_tester->setPose(-1.75, 0.0, 0.0);
+
+  // Publish a footprint of 0.5m "radius" at origin
+  auto radius = 0.5;
+  collision_tester->publishFootprint(radius, 0.0, 0.0, "test_base_frame", node->now());
+
+  // Publish an empty costmap
+  // It should not hit anything in an empty costmap
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, true));
+
+  // Set a dock in the costmap of 0.2x1.5m at 2m behind the robot
+  // It should hit the dock because the robot is 0.5m wide and the dock pose is at -1.75
+  // But it does not hit because the collision tolerance is 0.3m
+  collision_tester->setRectangle(0.2, 1.5, -2.1, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, true));
+
+  // Set an object between the robot and the dock
+  // It should hit the object
+  collision_tester->clearCostmap();
+  collision_tester->setRectangle(0.2, 0.2, -1.0, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE);
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, true));
+
+  // Set the collision tolerance to 0 to ensure all obstacles in the path are detected
+  controller->setCollisionTolerance(0.0);
+
+  // Set a dock in the costmap of 0.2x1.5m at 2m behind the robot
+  // Now it should hit the dock
+  collision_tester->clearCostmap();
+  collision_tester->setRectangle(0.2, 1.5, -2.1, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, true));
+
+  collision_tester->deactivate();
+}
+
+TEST(ControllerTests, CollisionCheckerUndockBackward) {
+  auto collision_tester = std::make_shared<TestCollisionChecker>("collision_test");
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  tf->setUsingDedicatedThread(true);  // One-thread broadcasting-listening model
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.footprint_topic", rclcpp::ParameterValue("test_footprint"));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.costmap_topic", rclcpp::ParameterValue("test_costmap_raw"));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.projection_time", rclcpp::ParameterValue(10.0));
+  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.dock_collision_threshold", rclcpp::ParameterValue(0.3));
+
+  auto controller = std::make_unique<opennav_docking::ControllerFixture>(
+    node, tf, "test_base_frame", "test_base_frame");
+  collision_tester->configure();
+  collision_tester->activate();
+
+  // Set the staging pose at 1.75m behind the robot
+  auto staging_pose = collision_tester->setPose(-1.75, 0.0, 0.0);
+
+  // Publish a footprint of 0.5m "radius" at origin
+  auto radius = 0.5;
+  collision_tester->publishFootprint(radius, 0.0, 0.0, "test_base_frame", node->now());
+
+  // Publish an empty costmap
+  // It should not hit anything in an empty costmap
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, true));
+
+  // Set a dock in the costmap of 0.2x1.5m in front of the robot. The robot is docked
+  // It should hit the dock because the robot is 0.5m wide and the robot pose is at 1.75
+  // But it does not hit because the collision tolerance is 0.3m
+  collision_tester->setRectangle(0.2, 1.5, 0.25, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, true));
+
+  // Set an object beyond the staging pose
+  // It should hit the object
+  collision_tester->clearCostmap();
+  collision_tester->setRectangle(0.2, 0.2, -1.75 - 0.5, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE);
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true));
+
+  // Set an object between the robot and the staging pose
+  // It should hit the object
+  collision_tester->clearCostmap();
+  collision_tester->setRectangle(0.2, 0.2, -1.0, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE);
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true));
+
+  // Set the collision tolerance to 0 to ensure all obstacles in the path are detected
+  controller->setCollisionTolerance(0.0);
+
+  // Set a dock in the costmap of 0.2x1.5m in front of the robot. The robot is docked
+  // Now it should hit the dock
+  collision_tester->clearCostmap();
+  collision_tester->setRectangle(0.2, 1.5, 0.25, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true));
+
+  collision_tester->deactivate();
+}
+
+TEST(ControllerTests, CollisionCheckerUndockForward) {
+  auto collision_tester = std::make_shared<TestCollisionChecker>("collision_test");
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  tf->setUsingDedicatedThread(true);  // One-thread broadcasting-listening model
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.footprint_topic", rclcpp::ParameterValue("test_footprint"));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.costmap_topic", rclcpp::ParameterValue("test_costmap_raw"));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "controller.projection_time", rclcpp::ParameterValue(10.0));
+  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.dock_collision_threshold", rclcpp::ParameterValue(0.3));
+
+  auto controller = std::make_unique<opennav_docking::ControllerFixture>(
+    node, tf, "test_base_frame", "test_base_frame");
+  collision_tester->configure();
+  collision_tester->activate();
+
+  // Set the staging pose at 1.75m in the front of the robot
+  auto staging_pose = collision_tester->setPose(1.75, 0.0, 0.0);
+
+  // Publish a footprint of 0.5m "radius"
+  auto radius = 0.5;
+  collision_tester->publishFootprint(radius, 0.0, 0.0, "test_base_frame", node->now());
+
+  // Publish an empty costmap
+  // It should not hit anything in an empty costmap
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, false));
+
+  // Set a dock in the costmap of 0.2x1.5m at 0.5m behind the robot. The robot is docked
+  // It should not hit anything because the robot is docked and the trajectory is backward
+  collision_tester->setRectangle(0.2, 1.5, -0.35, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, false));
+
+  // Set an object beyond the staging pose
+  // It should hit the object
+  collision_tester->clearCostmap();
+  collision_tester->setRectangle(0.2, 0.3, 1.75 + 0.5, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE);
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false));
+
+  // Set an object between the robot and the staging pose
+  // It should hit the object
+  collision_tester->clearCostmap();
+  collision_tester->setRectangle(0.2, 0.2, 1.0, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE);
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false));
+
+  // Set the collision tolerance to 0 to ensure all obstacles in the path are detected
+  controller->setCollisionTolerance(0.0);
+
+  // Set a dock in the costmap of 0.2x1.5m at 0.5m behind the robot. The robot is docked
+  // Now it should hit the dock
+  collision_tester->clearCostmap();
+  collision_tester->setRectangle(0.2, 1.5, -0.35, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
+  collision_tester->publishCostmap();
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false));
+
+  collision_tester->deactivate();
+}
+
+
 }  // namespace opennav_docking
diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py
index 563e810ce36..5b76f6095ce 100644
--- a/nav2_docking/opennav_docking/test/test_docking_server.py
+++ b/nav2_docking/opennav_docking/test/test_docking_server.py
@@ -41,15 +41,19 @@ def generate_test_description():
             executable='opennav_docking',
             name='docking_server',
             parameters=[{'wait_charge_timeout': 1.0,
+                         'controller': {
+                             'use_collision_detection': False,
+                             'transform_tolerance': 0.5,
+                         },
                          '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',
         ),