Added collision detection for docking

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,10 +17,15 @@
 #define OPENNAV_DOCKING__CONTROLLER_HPP_
 
 #include <memory>
+#include <string>
 #include <vector>
 
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/twist.hpp"
+#include "nav_msgs/msg/path.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 "nav2_util/lifecycle_node.hpp"
 
@@ -34,8 +40,20 @@ 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.
@@ -45,25 +63,64 @@ class Controller
    * @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 backward = false);
+
+protected:
+  /**
+   * @brief Check if a trajectory is collision free.
+   *
+   * @param target_pose Target pose, in robot centric coordinates.
+   * @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 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_;
 
-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_;
+
+  // 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 collision_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

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(
@@ -39,6 +49,22 @@
     node, "controller.v_angular_max", rclcpp::ParameterValue(0.75));
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.slowdown_radius", rclcpp::ParameterValue(0.25));
+  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.collision_tolerance", rclcpp::ParameterValue(0.30));
 
   node->get_parameter("controller.k_phi", k_phi_);
   node->get_parameter("controller.k_delta", k_delta_);
@@ -55,16 +81,117 @@
   // 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_);
+
+  if (use_collision_detection_) {
+    double transform_tolerance;
+    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.transform_tolerance", transform_tolerance);
+    node->get_parameter("controller.collision_tolerance", collision_tolerance_);
+    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();
+  costmap_sub_.reset();
+  footprint_sub_.reset();
+  collision_checker_.reset();
 }
 
 bool Controller::computeVelocityCommand(
   const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool backward)
 {
   std::lock_guard<std::mutex> lock(dynamic_params_lock_);
   cmd = control_law_->calculateRegularVelocity(pose, backward);
+  return isTrajectoryCollisionFree(pose, backward);
+}
+
+bool Controller::isTrajectoryCollisionFree(
+  const geometry_msgs::msg::Pose & target_pose, 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_;
+  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
+    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);
+
+    // 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);
+
+    // 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);
+    if (use_collision_detection_ &&
+      dock_collision_distance > collision_tolerance_ &&
+      !collision_checker_->isCollisionFree(projected_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());
+      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)
 {
@@ -92,6 +219,12 @@
         v_angular_max_ = parameter.as_double();
       } else if (name == "controller.slowdown_radius") {
         slowdown_radius_ = 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.collision_tolerance") {
+        collision_tolerance_ = parameter.as_double();
       }
 
       // Update the smooth control law with the new params

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -86,7 +86,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_)) {
@@ -248,8 +248,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_)
     {

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.
@@ -16,7 +17,10 @@
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/pose.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/node_utils.hpp"
 #include "opennav_docking/controller.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
@@ -36,7 +40,25 @@ namespace opennav_docking
 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_fixed_frame", "test_base_frame");
+
+  // Set a transform between fixed_frame and base_frame
+  geometry_msgs::msg::TransformStamped fixed_to_base;
+  fixed_to_base.header.frame_id = "test_fixed_frame";
+  fixed_to_base.header.stamp = node->get_clock()->now();
+  fixed_to_base.child_frame_id = "test_base_frame";
+  fixed_to_base.transform.translation.x = 0.0;
+  fixed_to_base.transform.translation.y = 0.0;
+  fixed_to_base.transform.translation.z = 0.0;
+  tf->setTransform(fixed_to_base, "test", false);
 
   geometry_msgs::msg::Pose pose;
   geometry_msgs::msg::Twist cmd_out, cmd_init;
@@ -47,7 +69,8 @@ TEST(ControllerTests, ObjectLifecycle)
 
 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_shared<opennav_docking::Controller>(
+    node, nullptr, "test_fixed_frame", "test_base_frame");
 
   auto params = std::make_shared<rclcpp::AsyncParametersClient>(
     node->get_node_base_interface(), node->get_node_topics_interface(),
@@ -63,7 +86,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.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.collision_tolerance", 11.0)});
 
   // Spin
   rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
@@ -77,6 +103,9 @@ TEST(ControllerTests, DynamicParameters) {
   EXPECT_EQ(node->get_parameter("controller.v_linear_max").as_double(), 6.0);
   EXPECT_EQ(node->get_parameter("controller.v_angular_max").as_double(), 7.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.collision_tolerance").as_double(), 11.0);
 }
 
 }  // namespace opennav_docking