Improve collision while undocking and test

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

nav2_docking/opennav_docking/include/opennav_docking/controller.hpp

@@ -22,11 +22,11 @@
 
 #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 "nav_msgs/msg/path.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 
 namespace opennav_docking
@@ -59,22 +59,25 @@ class 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 backward = false);
+    const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward = false);
 
   /**
    * @brief Callback executed when a parameter change is detected.
@@ -116,6 +119,7 @@ class Controller
   double projection_time_;
   double simulation_time_step_;
   double collision_tolerance_;
+  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_;

nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp

@@ -112,12 +112,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.

nav2_docking/opennav_docking/src/controller.cpp

@@ -85,15 +85,14 @@ Controller::Controller(
   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_) {
-    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);
+    configureCollisionChecker(node, costmap_topic, footprint_topic, transform_tolerance_);
   }
 
   trajectory_pub_ =
@@ -104,21 +103,22 @@ Controller::~Controller()
 {
   control_law_.reset();
   trajectory_pub_.reset();
+  collision_checker_.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)
+  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, backward);
+  return isTrajectoryCollisionFree(pose, is_docking, backward);
 }
 
 bool Controller::isTrajectoryCollisionFree(
-  const geometry_msgs::msg::Pose & target_pose, bool backward)
+  const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward)
 {
   // Visualization of the trajectory
   nav_msgs::msg::Path trajectory;
@@ -128,14 +128,14 @@ bool Controller::isTrajectoryCollisionFree(
   // 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);
+    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",
@@ -157,12 +157,15 @@ bool Controller::isTrajectoryCollisionFree(
     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);
+    // 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 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_ &&

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -449,7 +449,9 @@ 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");
     }
     vel_publisher_->publish(std::move(command));
@@ -515,7 +517,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;
@@ -533,7 +535,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;
@@ -556,7 +558,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");
   }
 
@@ -643,7 +645,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");