Fix backward moving while docking

[iradionov]

Basic Info

| Primary OS tested on | Ubuntu |
| Robotic platform tested on | simulation |
| Does this PR contain AI generated software? | No |

---

Description of contribution in a few bullet points

This commit ruins the backward docking process because the phi angle is rotated (+M_PI) and a robot tries to approach with wrong direction.

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

[ajtudela]

Hi,

This is already fixed in the main branch. Your dock detector must publish a pose pointing 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.

Please, check this issue for more info: open-navigation/opennav_docking#47

[SteveMacenski]

@ajtudela go on vacation! Is that the direction of the dock arrow the same as forward docking before your PR? Maybe my memory is wrong, but I thought it was pointing 'out' before like a surface normal for forward docking - but that may have been when I was doing dock detection output testing. I don't care which way it does (and pointing like your image makes sense to me from a control perspective), as long as its consistently the same with forward and reverse docking orientations.

If that's all good, I need to make sure that I update the documentation to mention that so that its not ambiguous

@iradionov please verify with the updated orientation

[ajtudela]

@SteveMacenski Two days left!! 😃

Yes, the direction of the dock arrow must be the same for both forward and backwards. (If the docking is backwards, the controller will reverse it internally, so the trajectory is generated correctly).

I agree, it should be clear from the documentation that the orientation of the dock has to be the same, regardless of whether the docking is forward or backwards.

[iradionov]

Hi, guys!
Thank you for the response and the details. It seems that it's not only me have a misunderstanding in terms of goal direction.
We tried this algorithm in simple cases (moving on a straight line) with a "wrong" direction and it worked somehow. Yesterday I tried this old version on complex cases and yes, the goal direction should be rotated when movement is backward.
For us, this point wasn't clear and in our case (a not small Ackermann-type robot) a more natural way is looking at the docking process as a parking. So the goal direction should be matched with the car's direction at this point.
Something like this

What is the reason why we should invert the goal direction for backward moving?

[SteveMacenski]

I don't understand the question fully or what I'm looking at in terms of the images, the frames are unlabeled and not described. They also look both like adequate solutions so I don't see 'the problem' and 'the solution' curves.

the goal direction should be rotated when movement is backward

Precision here would be helpful. You shouldn't need to rotate anything specially for reversing. It sounds like your dock's detected orientation need to be rotated but that would have been required for forward or backward. It was just the wrong orientation - but to be fair - I don't document what that should be anywhere, so this is a very useful conversation for a gap in docs that I should fill. Then, inline in the software, we handle any inversions required without your need for modification to make reversing work properly.

Is that correct? If so, I think that makes this PR not needed - anything else we need to discuss?

What is the reason why we should invert the goal direction for backward moving?

Precision would be helpful, I don't know if you mean your dock's detected orientation or the inversion actually in the software.

[iradionov]

As I understand @ajtudela, on this image

the arrow (docking pose) should be rotated; this is a wrong setup. Am I right?
If so, this PR is not actual. But for our team, this representation seems more natural for a car-like robot.
Thanks for your replies.

[ajtudela]

the arrow (docking pose) should be rotated; this is a wrong setup. Am I right? If so, this PR is not actual. But for our team, this representation seems more natural for a car-like robot. Thanks for your replies.

Sorry for the late replay. You shouldn't have to rotate anything for backwards or forward docking, because the pose of the dock doesn't depend on the movement of the robot, it's always the same.

The server, internally, will reverse the dock orientation if the movement is backwards, but you only have to worry about publishing the pose correctly.

I'm not sure if this answers your questions, please let me know.

[SteveMacenski]

@iradionov to be clear, are you saying with the current state of main, setting the orientation of the dock as suggested, is still having a problem for you?

[iradionov]

It's not a problem. I understand this idea, but as mentioned ahead this representation seems more natural for our team since we work with a car-like robot.
Thanks for your time.