Dock target pose offset sign

[redvinaa]

I noticed that when docking backwards (the plug is on the back of the robot, with the detected maker pose of the docking station pointing forward towards the robot), the target pose offset is applied in the wrong direction. Here is the change that solves it for me, but I'm not sure if it's a real bug or a problem with my setup.

diff --git a/opennav_docking/src/docking_server.cpp b/opennav_docking/src/docking_server.cpp
index 75d428e..6146d25 100644
--- a/opennav_docking/src/docking_server.cpp
+++ b/opennav_docking/src/docking_server.cpp
@@ -412,7 +412,8 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped &
     // 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;
+    float sign = dock_backwards_ ? -1.0 : 1.0;
+    const double backward_projection = 0.25 * sign;
     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;

[SteveMacenski]

First, I'm not sure we actually support the case where you have the sensor in front and then need to back into the dock. That's what ros-navigation/navigation2#4403 covers the need to add. So I'm wondering a bit if some of the issue you have is w.r.t. that. How are you doing your 180 turn now or is the smooth control law doing the swing around?

Here is the change that solves it for me, but I'm not sure if it's a real but or a problem with my setup.

I think this is pointing to the fact that your dock orientation is incorrect or at least unexpected from the output of your detector (or using non-detection based semantic labels?). If using the simple charging dock, we have some fields that help with applying rotations from the detected-frame to the dock-frame for the Apriltags case by default (

opennav_docking/nova_carter_docking/params/nova_carter_docking.yaml

Lines 29 to 31 in a3b14ff

	external_detection_rotation_roll: -1.57
	external_detection_rotation_pitch: -1.57
	external_detection_rotation_yaw: 0.0

), but could be used to correct your issue as well. You should be able to visualize these topics in rviz.

Since we're applying the backward projection distance to the pose at the yaw angle, it seems like your main issue is that your yaw angle is inverted. That should be an easy 180 fix and I don't think that's a bug in whether you dock forward or backward at first and second looks.

[ajtudela]

You're right Steve.

All of the motion is handled by the controller (smooth control law). The dock_backwards_ param simply inverts the coordinates (r, phi, delta) inside to generate a curvature that and a velocity that allows the robot to move backwards. But the motion target (in this case, the dock pose) must always point in the direction of the motion, regardless of whether the robot is moving forward or backward.

Don't look the robot, just the orientation of the dock.

In fact, if the pose is reversed (pointing towards the robot), the controller will generate a curvature with a large arc and the robot will probably collide with an obstacle.

TL;DR: Just flip the yaw of your dock pose 180º. ;)

[SteveMacenski]

Sounds like we're done here then :-)

Definitely having the setting for "observe forward -> rotate 180 -> dock backward" is a useful feature and discussed in ros-navigation/navigation2#4403 for that reason!

[redvinaa]

First, I'm not sure we actually support the case where you have the sensor in front and then need to back into the dock.

It's not what I meant. The camera on the robot is facing backwards (in the direction of the docking station and in the direction of movement). The marker pose is pointing forward (from the docking station torwards the robot).

TL;DR: Just flip the yaw of your dock pose 180º. ;)

I have tried this before opening the issue, but for some reason the controller fails to lead the robot to the target pose.
It seems that with the original code the controller always turns the robot right before the goal.

I have recorded the behavior of the robot failing with the intended settings (e.g. detected pose pointing in the direction of movement)...

original.mp4

...and also the opposite (with the diff above) which does succeed.

fixed.mp4

(Arrow colors legend: light blue: staging pose, light green: detected pose, orange: dock pose, darker green: actual target pose, which is the dock pose plus 0.25 m offset)

[SteveMacenski]

It's not what I meant. The camera on the robot is facing backwards

Ah ok! Then not a problem for you.

---

Did you set dock_backwards for both?

Its possible that there's a bug here... But it seems to me that

• (1) your dock orientation was wrong which you fixed in your second video. But you didn't how the second video without your backward_projection patch.
• Did you set dock_backwards for both?

Can you explain why you made that change to backward_projection? I don't see the sense in it or how that would address the problem at hand. I did add that later into the process, so its possible that there's error in it that I didn't anticipate, but even looking now I can't sense why. I could be being obtuse.

The dock_pose is the dock's pose in the odom frame. Assuming you have the correct orientation set for the dock in your detector (:wink:) it seems to me to be independent to the robot's forward/reversing into the dock selection; its an attribute of the dock itself as the odom->dock_frame transform? It seems to me that just fixing your detector's orientation should also resolve this issue for you as well!

[ajtudela]

I checked again and I found the problem.

The equations for the smooth control law weren't originally designed for backward motion. So, we had to 'reverse' them to allow this kind of motion.

But the motion target (in this case, the dock pose) must always point in the direction of the motion, regardless of whether the robot is moving forward or backward.

I said this thinking in the graceful controller, where the backward motion works, as it shown in the picture:

I looked at the code and found where the backwards motion is used:
https://github.com/ros-navigation/navigation2/blob/f837a97cad828b6310b9468b2b663a7986b20a78/nav2_graceful_controller/src/graceful_controller.cpp#L162-L168

And... yes, both the coordinates inside the equations and the motion target are flipped so that the trajectory is generated in the right way. This flipping behaviour is inside the graceful controller instead of the smooth control law library where I should put it, so that anyone (like the docking server) can use forward and backward motion.

I've found another minor bug in checking this, so give me a few of days to fix it.

[SteveMacenski]

OK, sounds good to me! Thanks for the investigation, always good to find and understand root cause so that we know the solution won't mask other more subtle issues.

I look forward to it!

[redvinaa]

So @ajtudela's fix will probably solve this, but for the sake of being complete I'll answer the questions.

Did you set dock_backwards for both?

Yes, I didn't touch it.

(1) your dock orientation was wrong which you fixed in your second video. But you didn't how the second video without your backward_projection patch.

I guess you could say the orientation was right and I "made it wrong" in the second video. What happens without the "fix" is that the actual target pose is placed closer to the robot, so the dock pose is never reached and the tolerance checker never triggers. Here's a video to show this.

third.mp4

[SteveMacenski]

What happens without the "fix" is that the actual target pose is placed closer to the robot, so the dock pose is never reached and the tolerance checker never triggers.

That looks familiar - that's the end of the curve (the reason we added that additional 0.25m in the first place to avoid that behavior right up against the dock!). OK looking forward to the PR!

Also, reopening this, I didn't mean to leave it closed and sounds like actions are coming of it that we should track!

[ajtudela]

Hi @redvinaa

I've fixed the backward motion for the graceful controller here: ros-navigation/navigation2#4527

Because I don't have a robot with sensors in the back, could you try this branch: https://github.com/grupo-avispa/opennav_docking/tree/fix_backward when the PR is merged?

Thanks a lot!

[redvinaa]

I cherry-picked these to my humble branch, and it works now!

[SteveMacenski]

@ajtudela that looks good to me, can you open a PR for that against the 2x repositories? Should be easy for me to merge!