Undocking doesn't seem to take into account backwards docking in staging pose retrieval

[AlessandroPozzoni]

Hello,

I've been having some issues with undocking after successful backwards docking with external detection.
In particular I noticed that when the staging pose for the undocking is retrieved, the pose used to get the staging pose is the one of the docked robot.

In my case I have:

• a robot docking backwards
• a dock, with pose with yaw = 0, staging_x_offset = -2.0 and staging_yaw_offset = 3.14

This means that at the end of the docking phase, the robot pose has yaw = 3.14 .

When getting the staging pose in the undocking phase, the robot pose is passed as parameter pose. This also means that:

• yaw = tf2::getYaw(pose.orientation) = 3.14
• cos(yaw) = -1
• cos(yaw) * staging_x_offset_ = -staging_x_offset_

Therefore the staging offset gets inverted in sign, bringing the staging pose on the other side of the dock (in my case not reachable).

I feel like the function to get the staging pose should take into account the fact that the robot was docking backwards, in order to retrieve the staging pose on the correct side of the dock.

I hope I didn't make any mistakes along the way and that this is an actual issue (being my first one ever), thank you for the amazing resource.

[AlessandroPozzoni]

Flipping the dock (assigning it yaw = 3.14) and inverting the offset (staging_x_offset = 2.0, staging_yaw_offset = 0.0) seem to fix it (it's more of a workaround) when the external detection is in use.

• The staging pose is the same (2.0 "on the left" of the dock)
• The final yaw of the robot is 3.14
• cos(yaw) = -1
• The offset applied to the robot pose is cos(yaw) * staging_x_offset_ = -2.0 (2.0 "on the left" of the dock)

This happens because the dock pose gets estimated with yaw = 0.0 anyway, meaning that the robot approaches the dock from the same side as before (even though theoretically it is approaching the dock from the "wrong" side), therefore ending up docking backwards and with the same yaw as the dock defined in the database (3.14).

However, if the external detection is not used, the same issue as before occurs, with the staging pose ending up on the opposite side of the dock.

[SteveMacenski]

I'm just surprised to hear an issue with reversing since I think this was tested at length by the developer that created the backwards docking add-on. What's your external pose detection frames look like? Maybe those aren't set correctly (or maybe this wasn't tested as well as I was lead to believe)

The slide at 17:10 in my roscon talk shows the frames how they should appear to you in rviz using the external detection offsets (X axis pointed into the dock, regardless of docking in reverse or forward).

Flipping the dock (assigning it yaw = 3.14) and inverting the offset (staging_x_offset = 2.0, staging_yaw_offset = 0.0) seem to fix it (it's more of a workaround) when the external detection is in use.
However, if the external detection is not used, the same issue as before occurs, with the staging pose ending up on the opposite side of the dock.

This fixing both the docking and undocking case & that now the non-detection case is broken makes me think that perhaps your detection frames / offsets aren't setup correctly.

PS also worth testing the version in Nav2 itself. This is broadly deprecated and development has been occurring in Nav2's version. It could be that this has been resolved in Nav2's version with some other changes and isn't reflected here.

---

Under the assumption that there is a bug, which I'm thinking there maybe isn't (?):

We have some block we use in the approach docking for the reverse case, maybe we should be applying similarly to the undock case in the appropriate place (somewhere around here)?

https://github.com/ros-navigation/navigation2/blob/main/nav2_docking/opennav_docking/src/docking_server.cpp#L434-L437

I think that would also need to be done in resetApproach, no?

[ajtudela]

If there is indeed a bug, it may be related to the lines you've mentioned: L434-L437.

The fix is here: ros-navigation/navigation2#4749 (comment)

But that PR is stalled :/.

P.S. I can do a new PR with these lines if we can't wait for the #4749

[SteveMacenski]

Isn't that PR for docking backards by staging forwards, reversing automatically, and docking totally blind?

Is there's a portion in there that's a bug in general for backward docking (nonspecific to that application), then another PR to resolve just that issue in the interim would definitely be mergable.

Edit: Yes, that would be great!

[AlessandroPozzoni]

Thank you for the replies.
I'll attach a test I did just with the detection off, to exclude the issue of the detection frames, and the relative frames.

These are the relevant settings of the server:

docking_server:
  ros__parameters:
    dock_database: /path/to/docks.yaml # Correctly set in the actual project
    dock_plugins: ['nova_carter_dock']
    dock_backwards: True
    undock_linear_tolerance: 0.3
    undock_angular_tolerance: 0.5
    nova_carter_dock:
      plugin: 'opennav_docking::SimpleChargingDock'
      docking_threshold: 0.3
      staging_x_offset: -2.0
      staging_yaw_offset: 3.14
      use_external_detection_pose: false

This is docks.yaml:

docks:
  aruco_dock:
    type: "nova_carter_dock"
    frame: map
    pose: [6.5, 0.0, 0.0]

In the staging phase, the robot navigates to the staging pose, which has staging_x_offset: -2.0 from the frame of the dock

Then, the robot correctly gets to staging_yaw_offset: 3.14 and correctly performs the docking

When undocking, the staging pose is moved on the other side of the dock frame

[AlessandroPozzoni]

When getting the staging pose in the undocking phase, the robot pose is passed as parameter pose. This also means that:

* `yaw = tf2::getYaw(pose.orientation) = 3.14`

* `cos(yaw) = -1`

* `cos(yaw) * staging_x_offset_ = -staging_x_offset_`

Therefore the staging offset gets inverted in sign, bringing the staging pose on the other side of the dock (in my case not reachable).

I still have a feeling it's about this. I double checked and my odom frame is set correctly too (in this case, with the same yaw as the dock frame), meaning that when the robot pose is retrieved to find the staging pose, it should have yaw = 3.14.

This means that when the offset gets added to the robot pose used as the dock pose x component, it will be inverted in sign.

If it is assumed that the dock has the same pose as the robot after docking, then the fact that it's docking backwards should probably be taken into account.
Maybe something like what you guys mentioned:

// 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);

Thank you again for the support, I hope I'm not missing something in my configuration that's introducing this problem.

[ajtudela]

I don't think there's a bug when undocking backwards, but please bear with me.

These are the frames when the robot is docked backwards: left is base_frame and right is dock_frame. Both are in the opposite direction.

When undocking:

• First, getRobotPoseInFrame(fixed_frame_) is called to get the robot pose in the fixed frame (odom).
• Then, dock->getStagingPose() is called in the same frame as the robot.
• If we look inside:

yaw = tf2::getYaw(pose.orientation) = 3.14
cos(yaw) = -1
staging_pose.pose.position.x += cos(yaw) * staging_x_offset_ = -staging_x_offset_ = - (-2.0) = 2.0

The staging_pose.pose.position.x is in front of the robot.

So I'm not sure if I'm missing something here or if your frames are not set correctly.

P.S.: In any case, as your robot is docking backwards, could you please checkout to this branch and dock your robot. I want to make sure that this piece of code is fully functional before merging it. I would do it myself but my robots dock forward. Thanks a lot!

[AlessandroPozzoni]

Thank you for the insights.

After doing some more testing I added two logs to the implementation of getRobotPoseInFrame(fixed_frame_):

geometry_msgs::msg::PoseStamped DockingServer::getRobotPoseInFrame(const std::string & frame)
{
  geometry_msgs::msg::PoseStamped robot_pose;
  robot_pose.header.frame_id = base_frame_;
  RCLCPP_INFO(get_logger(), "[TRANSF_BEFORE] Robot pose frame: %s\nBase frame: %s\tFixed frame: %s\t Param frame: %s\n", robot_pose.header.frame_id.c_str(), base_frame_.c_str(), fixed_frame_.c_str(), frame.c_str());

  robot_pose.header.stamp = rclcpp::Time(0);
  tf2_buffer_->transform(robot_pose, robot_pose, frame);
  RCLCPP_INFO(get_logger(), "[TRANSF_AFTER] Robot pose frame: %s\nBase frame: %s\tFixed frame: %s\t Param frame: %s\n", robot_pose.header.frame_id.c_str(), base_frame_.c_str(), fixed_frame_.c_str(), frame.c_str());

  return robot_pose;
}

This brought to the following log piece:

[opennav_docking-20] [INFO] [...] [docking_server]: Attempting to undock robot of dock type nova_carter_dock.
[opennav_docking-20] [INFO] [...] [docking_server]: [TRANSF_BEFORE] Robot pose frame: base_link
[opennav_docking-20] Base frame: base_link	Fixed frame: odom	 Param frame: odom

[opennav_docking-20] [INFO] [...] [docking_server]: [TRANSF_AFTER] Robot pose frame: odom
[opennav_docking-20] Base frame: base_link	Fixed frame: odom	 Param frame: odom

From what I gather:

• the transformation changed the frame_id of robot_pose from base_link to odom
  -> the returned robot_pose has frame_id: odom.
• base_frame_ is correctly set to base_link
• fixed_frame_ is correctly set to odom (so the parameter frame that was passed to the function is odom)

Therefore in this snippet

    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);

• dock_pose will have the same frame_id as the pose returned from getRobotPoseInFrame(fixed_frame_);
  ->meaning odom
• dock->getStagingPose() is called with dock_pose.header.frame_id, which is odom.

I then added more logs to dock->getStagingPose()

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;
  }

  RCLCPP_INFO(node_->get_logger(), "[STAGING_POSE] pose: x: %f, y: %f, z: %f, yaw: %f, frame: %s",
     pose.position.x, pose.position.y, pose.position.z, tf2::getYaw(pose.orientation), frame.c_str());

  // Compute the staging pose with given offsets
  const double yaw = tf2::getYaw(pose.orientation);
  geometry_msgs::msg::PoseStamped staging_pose;
  staging_pose.header.frame_id = frame;
  staging_pose.header.stamp = node_->now();
  staging_pose.pose = pose;

  RCLCPP_INFO(node_->get_logger(), "[STAGING_POSE] This is the staging position before the offsets: x: %f, y: %f, z: %f, frame: %s",
     staging_pose.pose.position.x, staging_pose.pose.position.y, staging_pose.pose.position.z, staging_pose.header.frame_id.c_str());

  staging_pose.pose.position.x += cos(yaw) * staging_x_offset_;
  staging_pose.pose.position.y += sin(yaw) * staging_x_offset_;

  RCLCPP_INFO(node_->get_logger(), "[STAGING_POSE] This is the staging position after the offsets: x: %f, y: %f, z: %f, frame: %s",
     staging_pose.pose.position.x, staging_pose.pose.position.y, staging_pose.pose.position.z, staging_pose.header.frame_id.c_str());
  
  tf2::Quaternion orientation;
  orientation.setEuler(0.0, 0.0, yaw + staging_yaw_offset_);
  staging_pose.pose.orientation = tf2::toMsg(orientation);

  // Publish staging pose for debugging purposes
  staging_pose_pub_->publish(staging_pose);
  return staging_pose;
}

Getting this:

[opennav_docking-20] [INFO] [...] [docking_server]: [STAGING_POSE] pose: x: 6.204902, y: -0.004166, z: 0.000000, yaw: -2.953945, frame: odom
[opennav_docking-20] [INFO] [...] [docking_server]: [STAGING_POSE] This is the staging position before the offsets: x: 6.204902, y: -0.004166, z: 0.000000, frame: odom
[opennav_docking-20] [INFO] [...] [docking_server]: [STAGING_POSE] This is the staging position after the offsets: x: 8.169793, y: 0.368931, z: 0.000000, frame: odom

The offset does indeed make a positive contribution, having changed in sign along the x axis, but the position is defined (and the offset is calculated) with respect to the odom frame, bringing it "behind" the robot.

For completeness: the same messages, in the docking phase, are the following:

[opennav_docking-20] [INFO] [...] [docking_server]: [STAGING_POSE] pose: x: 6.500000, y: 0.000000, z: 0.000000, yaw: 0.000000, frame: odom
[opennav_docking-20] [INFO] [...] [docking_server]: [STAGING_POSE] This is the staging position before the offsets: x: 6.500000, y: 0.000000, z: 0.000000, frame: odom
[opennav_docking-20] [INFO] [...] [docking_server]: [STAGING_POSE] This is the staging position after the offsets: x: 4.500000, y: 0.000000, z: 0.000000, frame: odom

the staging pose is still computed based on the odom frame, but the pose of the dock is retrieved from the dock database, and not by assuming it equal to the robot pose, yielding yaw: 0.000000 and a correct offset contribution.

---

I know it's a lot to read, but I hope it makes my situation clearer. If I missed anything along the way, and if you have the time to look into it, please let me know.

P.S.: In any case, as your robot is docking backwards, could you please checkout to this branch and dock your robot. I want to make sure that this piece of code is fully functional before merging it. I would do it myself but my robots dock forward. Thanks a lot!

Regarding this I'll check it out as soon as I can and I'll get back to you! @ajtudela