Added collision detection for docking

[ajtudela]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4404
Primary OS tested on	Ubuntu 24.04
Robotic platform tested on	Morphia
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• Added optional collision checking on docking / undocking.

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]

Only addition I would like to see (which I think is the intent of the collision_tolerance_ param) is a parameter for a tolerance to the dock or from the dock to ignore collisions. If we're checking within N distance of the dock (or proxy of distance along the trajectory control law), ignore collisions since we're literally going to collide with the dock.

So while docking, its at the end (and at the start of the trajectory for undocking)

Otherwise, this looks very sensible!

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Files with missing lines	Coverage Δ
...docking/include/opennav_docking/docking_server.hpp	100.00% <ø> (ø)
nav2_docking/opennav_docking/src/controller.cpp	100.00% <100.00%> (+1.66%)	⬆️
...av2_docking/opennav_docking/src/docking_server.cpp	89.63% <100.00%> (ø)

... and 1 file with indirect coverage changes

---

🚨 Try these New Features:

• Flaky Tests Detection - Detect and resolve failed and flaky tests

[SteveMacenski]

@ajtudela do you want me to review again? I see updates but wasn't sure if we came to agreement on the trajectory checking for dock/undock (or if what I'm asking for isn't sensible)

[ajtudela]

@ajtudela do you want me to review again? I see updates but wasn't sure if we came to agreement on the trajectory checking for dock/undock (or if what I'm asking for isn't sensible)

Yes, please. I came to the same conclusion as you when doing the test: skip the last part of the trajectory when docking and the first part when undocking.

It's implemented in the latest commit, and I've added new tests to check for collisions in the 4 scenarios: dock/undock forward/backward.

If you can review it, I'll fix the python test and your possible changes tomorrow.

Thanks!

[SteveMacenski]

Otherwise, pretty much ready to rumble!

[ajtudela]

Update docs here: ros-navigation/docs.nav2.org#610

[ajtudela]

Can be backported to humble?

[SteveMacenski]

That's a nice solution to the docking/undocking problem - very elegant

Can be backported to humble?

I'm going to say 'yes'. It technically breaks the controller's constructor & computeVelocityCommand, but that's not a public facing object and this is an important update that Humble users will want over the next ~3 years of LTS support. This is still a pretty new package, so I'm willing to make the exception here

[ajtudela]

That's a nice solution to the docking/undocking problem - very elegant

Can be backported to humble?

I'm going to say 'yes'. It technically breaks the controller's constructor & computeVelocityCommand, but that's not a public facing object and this is an important update that Humble users will want over the next ~3 years of LTS support. This is still a pretty new package, so I'm willing to make the exception here

Great. I'll make another PR in the other repo.

[SteveMacenski]

Thanks, this is great work!