Backward docking without sensors #4749

Jakubach · 2024-11-09T21:41:09Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	ticket1 ticket2
Primary OS tested on	Ubuntu
Robotic platform tested on	Gazebo simulation
Does this PR contain AI generated software?	No

Description of contribution in a few bullet points

Fixed an issue with backward docking described in issue
Added an option for backward docking without continious pose refining (to use only initial detected dock pose when robot is docking backward but it has sensors for detection only on front)
Added an initial rotation functionality
Added a minimal angular velocity parameter which is used for initial rotation
Updated readme file

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

Check that any new parameters added are updated in docs.nav2.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

SteveMacenski

I think we can do this a bit better - but I appreciate the first prototype and your contributions!

nav2_docking/opennav_docking/src/docking_server.cpp

nav2_docking/README.md

nav2_docking/opennav_docking/include/opennav_docking/controller.hpp

nav2_docking/opennav_docking/src/docking_server.cpp

mergify · 2024-11-13T18:10:59Z

@Jakubach, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-11-13T18:22:33Z

@Jakubach, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-11-13T18:31:23Z

@Jakubach, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-11-13T18:37:48Z

@Jakubach, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-11-13T18:41:18Z

@Jakubach, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

SteveMacenski

Did you test this on your robot platforms as well to validate this works?

I also see some new lines or removal of previous empty lines, try to revert those changes please :-)

SteveMacenski · 2024-11-13T20:19:48Z

nav2_docking/opennav_docking/test/test_controller.cpp

@@ -63,6 +64,7 @@ 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),


dock backwards + dock blind checks

SteveMacenski · 2024-11-13T20:22:29Z

nav2_docking/opennav_docking/src/controller.cpp

 {
  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
-  cmd = control_law_->calculateRegularVelocity(pose, backward);
+  cmd = control_law_->calculateRegularVelocity(pose, robot_pose, backward);


Is this related to the other item on reverse issues? I'd like @ajtudela's thoughts on this but I don't have a problem with it. I'm just surprised none of us thought about this if it was that simple. It removes the other work in the other file with the movement of target points

In my case (humble, Ubuntu 22.04) that was a part of fixing docking problem posted previously.

I'll try to explain them in detail here:

calculateRegularVelocity(target_pose, current_pose, backward) is used to calculate the velocity from the current_pose.

calculateRegularVelocity(target_pose, backward) is used to calculate the velocity from the origin (or base frame of the robot). This function internally calls calculateRegularVelocity(target_pose, {0,0,0}, backward). If current_pose is (0,0,0), the result function will be the same.

Internally, the velocitywould be calculated using ~ the difference between the two poses.

So if your target_pose is different from the robot_frame, you could do the following

robot_pose = getRobotPoseInFrame(target_frame) calculateRegularVelocity(target_pose, robot_pose, backward)

OR

transform(target_pose, base_frame_) calculateRegularVelocity(target_pose, backward)

The speed would be the same.

Because @Jakubach has removed the backward projection and the later transformation, they should use the 'full' calculateRegularVelocity instead of the 'simpler' one.

I hope my explanation is clear enough.

SteveMacenski · 2024-11-13T20:24:28Z

nav2_docking/opennav_docking/src/docking_server.cpp

+    // If you have backward_blind and dock_backward then
+    // we know we need to do the initial rotation to go from forward to reverse
+    // before doing the rest of the procedure. The initial_rotation would thus always be true.
+    initial_rotation_ = true;


Does initial_rotation_ even need to be a parameter? If backward_blind_ = true don't we always do the rotation?

It also seems unused in the code itself :-)

SteveMacenski · 2024-11-13T20:25:31Z

nav2_docking/opennav_docking/src/docking_server.cpp

+      }
+      if (name == "backward_blind") {
+        backward_blind_ = parameter.as_bool();
+        initial_rotation_ = parameter.as_bool();


You shouldn't update other parameters -- remove dock backward / initial rotation setting

nav2_docking/opennav_docking/src/docking_server.cpp

SteveMacenski · 2024-11-13T20:29:47Z

nav2_docking/opennav_docking/src/controller.cpp

+  vel.linear.x = 0.0;
+  vel.angular.z = 0.0;
+  if(angle_to_target > 0) {
+    vel.angular.z = std::clamp(1.0 * angle_to_target * v_angular_max_,


1.0 * in both expressions seems unnecessary. I think the other one might mean -1.0.

What is the derivation of this rotation code for angle_to_target * v_angular_max? Might it be better to steal the rotation code from something like the rotation shim controller / spin behavior to make this consistent?

Odometric error is often large with high acceleration, so it would probably be wise to have acceleration / decelleration in the rotation action such that we minimize slippage -- since especially in this case, we're fully dead reckoning

@SteveMacenski i am almost done with that, but how should I take the current angular velocity inside docking_server in the good way?
shim controller code requires it and I am trying to take that

navigation2/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

Line 288 in f426bda

RotationShimController::computeRotateToHeadingCommand(

You could get it the same way we do in the controller server with an odometry subscriber:

navigation2/nav2_controller/src/controller_server.cpp

Line 626 in f426bda

nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());

Thank you @SteveMacenski, it's already implemented and I hope I managed it well.

I just noticed that from time to time (not in 100% of situations) robot slows down during performing an initial rotation. The angular distance is correct but robot the generatated velocities are like 0.003 rad/s
Now I am inspecting why that happens. I am testing with rotate_to_heading_angular_vel: 0.5 and rotate_to_heading_max_angular_accel: 0.5

EDIT: It's not related with that code. The code is fine but I have a strange problem with velocities during rotation (angular velocity goes to 0 in half of the turn, then the controller couldn't go out from this situation, but it's something from smoother or odometry maybe).

SteveMacenski · 2024-11-13T20:33:23Z

nav2_docking/opennav_docking/src/docking_server.cpp

+  double angle_to_goal;
+  auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
+  while(rclcpp::ok()){
+    angle_to_goal = angles::shortest_angular_distance(tf2::getYaw(robot_pose.pose.orientation),


A question to ask: would we rather (1) rotate purely 180 from the staging pose and have the control law handle any deviation in the staging pose or (2) rotate approximately 180, taking into account any angular offsets present in the staging pose so that its as straight-backward as possible?

If (1), then we can just take the robot orientation and the target is just PI from that, no need to look at the dock pose.

If (2), then I think we need to look at the dock pose's orientation, not its relative position angle. But, I would agree this is up for debate about what's better. Another area @ajtudela's opinion would be appreciated :-)

Same here, I'd say (2) and just go straight to the dock but I'll have it tested the two versions in the lab tomorrow with our robots.

SteveMacenski · 2024-11-13T20:33:54Z

nav2_docking/opennav_docking/src/docking_server.cpp

+    angle_to_goal = angles::shortest_angular_distance(tf2::getYaw(robot_pose.pose.orientation),
+    atan2(robot_pose.pose.position.y - dock_pose.pose.position.y,
+    robot_pose.pose.position.x - dock_pose.pose.position.x));
+    if(fabs(angle_to_goal) > 0.1){


This should either be (1) a configurable threshold or (2) wait until its exceeded so its at least that much of a turn

I made a mistake here, there should be an opposite sign (<). I will reimplement with 1 or 2. Thanks

I called it tolerance in a hurry, but tell me please if it better should be "threshold"

SteveMacenski · 2024-11-13T20:36:58Z

nav2_docking/opennav_docking/src/docking_server.cpp

-    // Thus, we backward project the controller's target pose a little bit after the
-    // dock so that the robot never gets to the end of the spiral before its in contact
-    // with the dock to stop the docking procedure.
-    const double backward_projection = 0.25;


Wait a moment... even if we have the reversing signs handled another way, this is strictly necessary so that at the end of the trajectory near the dock, we continue to have a smooth motion rather than hitting the end of the trajectory singularity! We need this strictly speaking even for forward docking as the comment above mentions

Handled in and reverted that code.

I believe your PR outright removes this bit, not adjust it. I'm on board with the adjustment if @ajtudela is

I see the change in your recent pushes. Can we make (1 - 2 * dock_backwards_) * a little more clear? I don't like to use boolean properties in math expressions in this way, its a bit obtuse to parse.

You could just use an if/else for setting a negative sign

I share your opinion

You need to flip the orientation before you backward project it, no? That way we always apply the point behind the docked pose, instead of 0.25m in front of the docked pose. ... or was the order incorrect beforehand (but I'm thinking not)

If you flip the orientation before the backward projection, the sign of the translation is inverted and the pose gets closer to the robot.

It seems it was in the wrong order beforehand. It's odd that no one notice it.

A couple of images:

Flip + Project

Project + Flip

I had similar observation. Putting that code before conditional statement was a fix but I was not sure about that. Now it's confirmed I think.

Huh, that is interesting that everyone doing backward docking missed this -- sounds good

Huh, that is interesting that everyone doing backward docking missed this -- sounds good

Yeah, the only thing I can think of is that they set the dock_pose much farther than the real dock, so the robot will bump the dock but never reach the target pose.

It would be nice to ask someone with such a robot what their settings are.

codecov · 2024-11-13T22:04:43Z

Codecov Report

Attention: Patch coverage is 40.81633% with 29 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...av2_docking/opennav_docking/src/docking_server.cpp	44.11%	19 Missing ⚠️
nav2_docking/opennav_docking/src/controller.cpp	33.33%	10 Missing ⚠️

Files with missing lines	Coverage Δ
...docking/include/opennav_docking/docking_server.hpp	`100.00% <ø> (ø)`
nav2_docking/opennav_docking/src/controller.cpp	`85.33% <33.33%> (-13.00%)`	⬇️
...av2_docking/opennav_docking/src/docking_server.cpp	`85.99% <44.11%> (-3.91%)`	⬇️

... and 3 files with indirect coverage changes

SteveMacenski · 2024-11-13T22:59:27Z

Let me know when you want me to review again - I see that you're pushing frequent updates

ajtudela · 2024-11-12T16:31:17Z

nav2_docking/opennav_docking/src/docking_server.cpp

-    // The control law can get jittery when close to the end when atan2's can explode.
-    // Thus, we backward project the controller's target pose a little bit after the
-    // dock so that the robot never gets to the end of the spiral before its in contact
-    // with the dock to stop the docking procedure.
-    const double backward_projection = 0.25;
-    const double yaw = tf2::getYaw(target_pose.pose.orientation);
-    target_pose.pose.position.x += cos(yaw) * backward_projection;
-    target_pose.pose.position.y += sin(yaw) * backward_projection;
-    tf2_buffer_->transform(target_pose, target_pose, base_frame_);
-


You need to project the motion target a little further. Otherwise the control law will be very unstable at very short distances from the target.

In the graceful controller, there are similar tricks to avoid this unstability.

ajtudela · 2024-11-12T16:41:20Z

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -557,7 +571,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,robot_pose.pose, cmd, backward)) {


You don't need to pass the robot pose to calculate the velocity, because the underlying control law generates it in the base frame.

ajtudela · 2024-11-12T16:41:57Z

nav2_docking/opennav_docking/src/docking_server.cpp

+    if(initial_rotation_ && fabs(angle_to_goal) > initial_rotation_min_angle_){
+      command->twist = controller_->rotateToTarget(angle_to_goal);
+    }
+    else if (!controller_->computeVelocityCommand(target_pose.pose, robot_pose.pose, command->twist, dock_backwards_)) {


ajtudela · 2024-11-13T21:06:17Z

nav2_docking/opennav_docking/src/controller.cpp

 {
  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
-  cmd = control_law_->calculateRegularVelocity(pose, backward);
+  cmd = control_law_->calculateRegularVelocity(pose, robot_pose, backward);


I'll try to explain them in detail here:

calculateRegularVelocity(target_pose, current_pose, backward) is used to calculate the velocity from the current_pose.

calculateRegularVelocity(target_pose, backward) is used to calculate the velocity from the origin (or base frame of the robot). This function internally calls calculateRegularVelocity(target_pose, {0,0,0}, backward). If current_pose is (0,0,0), the result function will be the same.

Internally, the velocitywould be calculated using ~ the difference between the two poses.

So if your target_pose is different from the robot_frame, you could do the following

robot_pose = getRobotPoseInFrame(target_frame) calculateRegularVelocity(target_pose, robot_pose, backward)

OR

transform(target_pose, base_frame_) calculateRegularVelocity(target_pose, backward)

The speed would be the same.

Because @Jakubach has removed the backward projection and the later transformation, they should use the 'full' calculateRegularVelocity instead of the 'simpler' one.

I hope my explanation is clear enough.

… (ros-navigation#4711) * mppi parameters_handler: Improve verbose handling (ros-navigation#4704) The "verbose" parameter of the parameters_handler is a special case that needs registration before the dynamic parameter handler callback is registered. In verbose mode make the parameter handler info/warn/debug messages more expressive. Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Address review comments. (ros-navigation#4704) * remove comments. * Use RCLCPP_DEBUG instead of INFO for low level messages. * Add test for trying to access parameters that are not declared. Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * mppi parameters_handler: Improve static/dynamic/not defined logging (ros-navigation#4704) Attempts to change undefined parameters will not be successful and will log an error. Attempts to change static parameters will be ignored, a debug message is logged if a change in parameters is attempted. Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * mppi parameters_handler: populate SetParametersResult (ros-navigation#4704) Provide a mechanism to populate an rcl_interfaces::msg::SetParametersResult with the reasons for unsuccessful parameter setting, so that it may be propogated back to a set parameter client. The mechanism provides feedback when attempting to set undefined parameters, static parameters and could be used to validate dynamic parameters and provide a reason for rejection. NOTE: This changes public interface of ParametersHandler class. s/setDynamicParamCallback/setParamCallback s/addDynamicParamCallback/addParamCallback which takes a callback function that is able to populate a rcl_interfaces::msg::SetParametersResult. In order to indicate an unsuccessful parameter change and the reason, callback functions should append a "\n" to the reason before appending to if it is not empty. Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * mppi parameters_handler: fix reason handling and improve tests Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> --------- Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> Signed-off-by: Jakubach <jakubach@gmail.com>

* 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>

Signed-off-by: Jakubach <jakubach@gmail.com>

mergify · 2024-11-22T15:25:05Z

This pull request is in conflict. Could you fix it @Jakubach?

Signed-off-by: Jakubach <jakubach@gmail.com>

SteveMacenski · 2024-11-22T20:15:05Z

There's some test failures / linting issues reported in CI: https://app.circleci.com/pipelines/github/ros-navigation/navigation2/13004/workflows/13cb1e43-ec84-477d-9253-5d4464f09fb2/jobs/39130/tests

Please build / test locally not in our CI, it makes this very difficult to review when we're collapsing threads due to the volume of mergify warnings about build failures :-)

Jakubach · 2024-11-24T14:57:00Z

I've noticed that there exist a new bug during undock (after backward docking). The staging pose is in a wrong pose. I will upload a fix. The question is, should robot after docking go back to the real staging pose or it could go back to the rotated pose (that after initial rotation).

Screencast.from.11-24-2024.03.54.41.PM.webm

My first try was to add two rotations and that solved the problem. Robot after undocking is in a "after-initial-rotation-staging-pose". But maybe should getStagingPose method handle this?

void DockingServer::undockRobot()
{
  std::lock_guard<std::mutex> lock(*mutex_);
  action_start_time_ = this->now();
  rclcpp::Rate loop_rate(controller_frequency_);

  auto goal = undocking_action_server_->get_current_goal();
  auto result = std::make_shared<UndockRobot::Result>();
  result->success = false;

  if (!undocking_action_server_ || !undocking_action_server_->is_server_active()) {
    RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping.");
    return;
  }

  if (checkAndWarnIfCancelled(undocking_action_server_, "undock_robot")) {
    undocking_action_server_->terminate_all(result);
    return;
  }

  getPreemptedGoalIfRequested(goal, undocking_action_server_);
  auto max_duration = rclcpp::Duration::from_seconds(goal->max_undocking_time);

  try {
    // Get dock plugin information from request or docked state, reset state.
    std::string dock_type = curr_dock_type_;
    if (!goal->dock_type.empty()) {
      dock_type = goal->dock_type;
    }

    ChargingDock::Ptr dock = dock_db_->findDockPlugin(dock_type);
    if (!dock) {
      throw opennav_docking_core::DockNotValid("No dock information to undock from!");
    }
    RCLCPP_INFO(
      get_logger(),
      "Attempting to undock robot of dock type %s.", dock->getName().c_str());

    // Check if the robot is docked before proceeding
    if (dock->isCharger() && (!dock->isDocked() && !dock->isCharging())) {
      RCLCPP_INFO(get_logger(), "Robot is not in the dock, no need to undock");
      return;
    }

    // Get "dock pose" by finding the robot pose
    geometry_msgs::msg::PoseStamped dock_pose = getRobotPoseInFrame(fixed_frame_);

    if (dock_backwards_) {
      dock_pose.pose.orientation = 
        nav2_util::geometry_utils::orientationAroundZAxis(
          tf2::getYaw(dock_pose.pose.orientation) + M_PI);
    }

    // Get staging pose (in fixed frame)
    geometry_msgs::msg::PoseStamped staging_pose =
      dock->getStagingPose(dock_pose.pose, dock_pose.header.frame_id);

    if (dock_backwards_) {
      staging_pose.pose.orientation = 
        nav2_util::geometry_utils::orientationAroundZAxis(
          tf2::getYaw(staging_pose.pose.orientation) + M_PI);
    }

    // Control robot to staging pose
    rclcpp::Time loop_start = this->now();
    while (rclcpp::ok()) {
      // Stop if we exceed max duration
      auto timeout = rclcpp::Duration::from_seconds(goal->max_undocking_time);
      if (this->now() - loop_start > timeout) {
        throw opennav_docking_core::FailedToControl("Undocking timed out");
      }

      // Stop if cancelled/preempted
      if (checkAndWarnIfCancelled(undocking_action_server_, "undock_robot") ||
        checkAndWarnIfPreempted(undocking_action_server_, "undock_robot"))
      {
        publishZeroVelocity();
        undocking_action_server_->terminate_all(result);
        return;
      }

      // Don't control the robot until charging is disabled
      if (dock->isCharger() && !dock->disableCharging()) {
        loop_rate.sleep();
        continue;
      }

      // Get command to approach staging pose
      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_, false,
          !dock_backwards_))
      {
        RCLCPP_INFO(get_logger(), "Robot has reached staging pose");
        // Have reached staging_pose
        vel_publisher_->publish(std::move(command));
        if (!dock->isCharger() || dock->hasStoppedCharging()) {
          RCLCPP_INFO(get_logger(), "Robot has undocked!");
          result->success = true;
          curr_dock_type_.clear();
          publishZeroVelocity();
          undocking_action_server_->succeeded_current(result);
          return;
        }
        // Haven't stopped charging?
        throw opennav_docking_core::FailedToControl("Failed to control off dock");
      }

      // Publish command and sleep
      vel_publisher_->publish(std::move(command));
      loop_rate.sleep();
    }
  } catch (const tf2::TransformException & e) {
    RCLCPP_ERROR(get_logger(), "Transform error: %s", e.what());
    result->error_code = DockRobot::Result::UNKNOWN;
  } catch (opennav_docking_core::DockNotValid & e) {
    RCLCPP_ERROR(get_logger(), "%s", e.what());
    result->error_code = DockRobot::Result::DOCK_NOT_VALID;
  } catch (opennav_docking_core::FailedToControl & e) {
    RCLCPP_ERROR(get_logger(), "%s", e.what());
    result->error_code = DockRobot::Result::FAILED_TO_CONTROL;
  } catch (opennav_docking_core::DockingException & e) {
    RCLCPP_ERROR(get_logger(), "%s", e.what());
    result->error_code = DockRobot::Result::UNKNOWN;
  } catch (std::exception & e) {
    RCLCPP_ERROR(get_logger(), "Internal error: %s", e.what());
    result->error_code = DockRobot::Result::UNKNOWN;
  }

  publishZeroVelocity();
  undocking_action_server_->terminate_current(result);
}

ajtudela · 2024-11-24T15:58:38Z

I've noticed that there exist a new bug during undock (after backward docking). The staging pose is in a wrong pose. I will upload a fix. The question is, should robot after docking go back to the real staging pose or it could go back to the rotated pose (that after initial rotation).

Could you describe the bug, please? I don't see it clearly in the video.

Jakubach · 2024-11-24T16:08:21Z

I've noticed that there exist a new bug during undock (after backward docking). The staging pose is in a wrong pose. I will upload a fix. The question is, should robot after docking go back to the real staging pose or it could go back to the rotated pose (that after initial rotation).

Could you describe the bug, please? I don't see it clearly in the video.

If dock_backwards is enabled then a 180 degree rotation of dock_pose if performed. Then if you try to undock the robot, a staging pose will be moved by offset in X axis of a dock framed. The moved frame should be in a -X direction if backward docking was performed. If you maximize the video (it's covered if not maximized on the bottom) then there is a visible staging pose frame for docking and the incorrect staging pose frame for undocking.

Jakubach · 2024-11-25T13:59:28Z

There is one more thing to consider probably.
I made a test where I started from undockRobot method and I was getting an error message:

[coverage_navigation.py-1] Waiting for 'UndockRobot' action server
[coverage_navigation.py-1] Undocking from dock of type: robot_dock...
[coverage_navigation.py-1] Undocking request was rejected!

Is it caused by backward_blind parameter with use_external_detection_pose_?

The problem inside undockRobot method was with checking the condition dock->isDocked(). Inside isDocked method
below condition doesn't allow for procedure of undocking (even if I set the frame in my params file similarly to the nova_carter_demo).

  if (dock_pose_.header.frame_id.empty()) {
    // Dock pose is not yet valid
    return false;
  }

I though that the problem is inside undockRobot method because isDocked was tested before execution of getStagingPose method. But that didn't solve problem fully.

    // Get "dock pose" by finding the robot pose
    geometry_msgs::msg::PoseStamped dock_pose = getRobotPoseInFrame(fixed_frame_);

    // Get staging pose (in fixed frame)
    geometry_msgs::msg::PoseStamped staging_pose =
      dock->getStagingPose(dock_pose.pose, dock_pose.header.frame_id);

  // Check if the robot is docked before proceeding
    if (/*dock->isCharger() && */(!dock->isDocked() && !dock->isCharging())) {
      RCLCPP_INFO(get_logger(), "Robot is not in the dock, no need to undock");
      return;
    }

Inside getStagingPose I had also to comment the condition about use_external_detection_pose_ allowing then to always set frame. It's a quick fix but I am not sure yet how to fix that properly.

geometry_msgs::msg::PoseStamped SimpleChargingDock::getStagingPose(
  const geometry_msgs::msg::Pose & pose, const std::string & frame)
{
  // If not using detection, set the dock pose as the given dock pose estimate
  // if (!use_external_detection_pose_) {
    // This gets called at the start of docking
    // Reset our internally tracked dock pose
  //   dock_pose_.header.frame_id = frame;
  //   dock_pose_.pose = pose;
  // }
  dock_pose_.header.frame_id = frame;
  dock_pose_.pose = pose;
 // ...
}

SteveMacenski · 2024-11-26T20:40:08Z

The question is, should robot after docking go back to the real staging pose or it could go back to the rotated pose (that after initial rotation).

I'd say it should go back to the "real" staging pose, so backup and then rotate so its back in the same state as it started in before calling the docking behavior.

I'll admit I'm in the same boat as @ajtudela, I'm not really sure what I'm looking at in the video, but doesn't look good obviously :-)

I made a test where I started from undockRobot method and I was getting an error message:

Its hard to say, that logging wasn't very descriptive. Was the dock type in your dock database and was that specified in your undock request? Undock as an empty action doesn't work unless you previously docked using it for the current dock to be stored in the server's state. It seems to be being rejected early and that logging is from the simple commander API, not the server itself. This makes me think you're sending it an empty request without a dock plugin specified.

Can you summarize your findings into what change you think should be made and why current behavior is incorrect? That would be easier for me to review and validate :-)

Jakubach · 2024-11-27T13:35:48Z

I've tried to describe what is the problem on the video, it shows docking then undocking. The undocking procedure is not completed correctly. There is visible a frame of the staging_pose for undocking and it's placed behind the wall in that case.

Below you can find the docking pose configuration and undock_robot method which I call with a dock_type argument equal to simulated_dock. The file is loaded correctly by docking_server in my opinion.

docking_server:
  ros__parameters:
    docks:
    - simulated_dock
    simulated_dock:
      type: simulated_dock
      frame: map
      pose:
      - 6.5530412207692734e-09
      - 0.0017732545966282487
      - 3.1415889543540354


  def undock_robot(self, dock_type):
      """Send a `UndockRobot` action request."""
      print("Waiting for 'UndockRobot' action server")
      while not self.undocking_client.wait_for_server(timeout_sec=1.0):
          print('"UndockRobot" action server not available, waiting...')

      goal_msg = UndockRobot.Goal()
      goal_msg.dock_type = dock_type

      print('Undocking from dock of type: ' + str(dock_type) + '...')
      send_goal_future = self.undocking_client.send_goal_async(goal_msg,
                                                               self._feedbackCallback)
      rclpy.spin_until_future_complete(self, send_goal_future)
      self.goal_handle = send_goal_future.result()

      if not self.goal_handle.accepted:
          print('Undocking request was rejected!')
          return False

      self.result_future = self.goal_handle.get_result_async()
      return True

Issues summary:

dock->isDocked() does not pass the condition with dock_pose_.header.frame_id.empty() when backward_blind is True and use_external_detection_pose_ is True. Setting the frame is in the getStagingPose method.
When dock_backwards is True and when performing undocking then the staging pose frame is placed in the incorrect X direction.

ajtudela · 2024-11-27T15:04:19Z

The getStagingPose method depends on the type of dock you are using. If you're using the SimpleChargingDock class and your dock is backwards, you need to set your staging_x_offset to positive and your staging_yaw_offset to Pi. Otherwise you are telling the docking_server that your staging pose is behind your robot. Not in front of it.

You can see it here: https://github.com/ros-navigation/navigation2/blob/main/nav2_docking/opennav_docking/src/simple_charging_dock.cpp#L142

Jakubach · 2024-11-27T18:28:56Z

Thank you. That fixed the second issue. The first issue is still there and when I start simulation with undockRobot with dock type (provided in previous post) in nav2_simple_command then I get a message that robot is not in docking station. The method isDocked returns False in that if statement:

  if (dock_pose_.header.frame_id.empty()) {
    // Dock pose is not yet valid
    return false;
  }

Logs from docking server:

[component_container_isolated-1] [INFO] [1732732032.923337819] [docking_server]: Attempting to undock robot of dock type simulated_dock. 
[component_container_isolated-1] [INFO] [1732732032.923834298] [docking_server]: Robot is not in the dock, no need to undock
[component_container_isolated-1] [WARN] [1732732032.923910400] [docking_server]: [undock_robot] [ActionServer] Current goal was not completed successfully.
[component_container_isolated-1] [WARN] [1732732032.923947093] [docking_server]: [undock_robot] [ActionServer] Aborting handle.

Is that still a problem with configuration?

Jakubach force-pushed the backward-docking-without-sensors branch from c915ebd to 14faf82 Compare November 9, 2024 21:47

SteveMacenski requested changes Nov 12, 2024

View reviewed changes

SteveMacenski mentioned this pull request Nov 13, 2024

Fixed issue with backward docking and added initial in-place rotation open-navigation/opennav_docking#70

Open

7 tasks

Jakubach force-pushed the backward-docking-without-sensors branch from ad0982d to 4345fc5 Compare November 13, 2024 18:19

Jakubach force-pushed the backward-docking-without-sensors branch from 80ba35f to e1a6598 Compare November 13, 2024 18:28

Jakubach force-pushed the backward-docking-without-sensors branch from d357701 to d292774 Compare November 13, 2024 18:34

Jakubach force-pushed the backward-docking-without-sensors branch from 96b2730 to d2a10df Compare November 13, 2024 18:38

Jakubach force-pushed the backward-docking-without-sensors branch 4 times, most recently from dca434d to 0376cf5 Compare November 13, 2024 19:46

SteveMacenski reviewed Nov 13, 2024

View reviewed changes

SteveMacenski requested changes Nov 13, 2024

View reviewed changes

Jakubach force-pushed the backward-docking-without-sensors branch 3 times, most recently from 2f116b7 to 034b258 Compare November 13, 2024 21:42

Jakubach force-pushed the backward-docking-without-sensors branch 2 times, most recently from 2b47cb8 to 3ab0010 Compare November 13, 2024 22:46

ajtudela reviewed Nov 13, 2024

View reviewed changes

Jakubach force-pushed the backward-docking-without-sensors branch 4 times, most recently from 7d62a43 to e02d9d7 Compare November 15, 2024 14:35

aosmw and others added 4 commits November 22, 2024 16:24

Fixes after merging Signed-off-by: Jakub Chudziński <jakubach@gmail.com>

932a318

Signed-off-by: Jakubach <jakubach@gmail.com>

Fixes after merging Signed-off-by: Jakub Chudziński <jakubach@gmail.com>

40a2b0f

Signed-off-by: Jakubach <jakubach@gmail.com>

Jakubach force-pushed the backward-docking-without-sensors branch from 413bec7 to 40a2b0f Compare November 22, 2024 15:24

Jakubach added 2 commits November 22, 2024 16:27

Merge branch 'main' into backward-docking-without-sensors

4f367af

Signed-off-by: Jakubach <jakubach@gmail.com>

Fixes

4ddb38e

ros-navigation deleted a comment from mergify bot Nov 22, 2024

Backward docking without sensors #4749

Are you sure you want to change the base?

Backward docking without sensors #4749

Conversation

Jakubach commented Nov 9, 2024

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

SteveMacenski left a comment

Choose a reason for hiding this comment

mergify bot commented Nov 13, 2024

mergify bot commented Nov 13, 2024

mergify bot commented Nov 13, 2024

mergify bot commented Nov 13, 2024

mergify bot commented Nov 13, 2024

SteveMacenski left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

SteveMacenski Nov 13, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ajtudela Nov 13, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Jakubach Nov 14, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Jakubach Nov 15, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Jakubach Nov 13, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Jakubach Nov 13, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

SteveMacenski Nov 18, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Jakubach Nov 19, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

codecov bot commented Nov 13, 2024 • edited Loading

Codecov Report

SteveMacenski commented Nov 13, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ajtudela Nov 13, 2024 • edited Loading

Choose a reason for hiding this comment

mergify bot commented Nov 22, 2024

SteveMacenski commented Nov 22, 2024 • edited Loading

Jakubach commented Nov 24, 2024 • edited Loading

ajtudela commented Nov 24, 2024 • edited Loading

Jakubach commented Nov 24, 2024 • edited Loading

Jakubach commented Nov 25, 2024 • edited Loading

SteveMacenski commented Nov 26, 2024 • edited Loading

Jakubach commented Nov 27, 2024 • edited Loading

ajtudela commented Nov 27, 2024 • edited Loading

Jakubach commented Nov 27, 2024 • edited Loading

SteveMacenski Nov 13, 2024 •

edited

Loading

ajtudela Nov 13, 2024 •

edited

Loading

Jakubach Nov 14, 2024 •

edited

Loading

Jakubach Nov 15, 2024 •

edited

Loading

Jakubach Nov 13, 2024 •

edited

Loading

Jakubach Nov 13, 2024 •

edited

Loading

SteveMacenski Nov 18, 2024 •

edited

Loading

Jakubach Nov 19, 2024 •

edited

Loading

codecov bot commented Nov 13, 2024 •

edited

Loading

SteveMacenski commented Nov 13, 2024 •

edited

Loading

ajtudela Nov 13, 2024 •

edited

Loading

SteveMacenski commented Nov 22, 2024 •

edited

Loading

Jakubach commented Nov 24, 2024 •

edited

Loading

ajtudela commented Nov 24, 2024 •

edited

Loading

Jakubach commented Nov 24, 2024 •

edited

Loading

Jakubach commented Nov 25, 2024 •

edited

Loading

SteveMacenski commented Nov 26, 2024 •

edited

Loading

Jakubach commented Nov 27, 2024 •

edited

Loading

ajtudela commented Nov 27, 2024 •

edited

Loading

Jakubach commented Nov 27, 2024 •

edited

Loading