Publish planned footprints after smoothing

[tonynajjar]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #1)
Primary OS tested on	(Ubuntu, MacOS, Windows)
Robotic platform tested on	(Steve's Robot, gazebo simulation of Tally, hardware turtlebot)
Does this PR contain AI generated software?	(No; Yes and it is marked inline in the code)

---

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

---

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 this PR is fatally flawed. The smoother does not consider the full SE2 footprints and the planned footprints is to debug visualize the planner itself, not including the smoother stage. I don't disagree this is useful to move for analyzing your current issue, but I do not believe this is the correct action in general for merging this PR.

Please let me know what you think :-)

[tonynajjar]

I get what you mean but I would argue that both could be useful to debug depending on the scenario.
Actually most of the time as a user, I don't really care about the inner workings of SMAC, I rather care to see what my planner is feeding my controller, and that is the smoothed path.
So do you think we should add another '/planned_footprints_smoothed'?

[SteveMacenski]

Sure :)

[codecov]

Codecov Report

Attention: Patch coverage is 33.33333% with 34 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_smac_planner/src/smac_planner_lattice.cpp	33.33%	18 Missing ⚠️
nav2_smac_planner/src/smac_planner_hybrid.cpp	33.33%	16 Missing ⚠️

Files with missing lines	Coverage Δ
nav2_smac_planner/src/smac_planner_hybrid.cpp	87.10% <33.33%> (-2.05%)	⬇️
nav2_smac_planner/src/smac_planner_lattice.cpp	86.81% <33.33%> (-2.76%)	⬇️

... and 9 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[SteveMacenski]

All comments repeated for the Lattice case :-)

[SteveMacenski]

Suggested change

	"planned_footprints_smoothed", 1);
	"smoothed_footprints", 1);

Pedantic preference. If you think the other is better, we can use that instead. No required change.

An aside, but would it be good to have these be ~ topics so that they all group together under the node?

[SteveMacenski]

This doesn't need to be a standalone message, it can just be the first marker in the markerarray I believe.

[tonynajjar]

Not sure I get it, the msg has to be a visualization_msgs::msg::MarkerArray, can't be a visualization_msgs::msg::Marker (because that's what the publisher is initialized with)

[SteveMacenski]

Store the clock outside of the for loop and use here (so that all poses have the exact same timestamp). I could see that causing someone a subtle annoyance in the future.

[tonynajjar]

I added this to clear the previous footprints because as it was before, the footprints would just superimpose

[mergify]

@tonynajjar, 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).