Create iterator for custom height field in pointcloud.

[greganderson-vermeer]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5585
Primary OS tested on	Ubuntu
Robotic platform tested on	Vermeer Balehawk
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

• Added iterator in for a custom height field in our point cloud iterator

Description of documentation updates required from your changes

• If completed in a configurable method it would require new parameters, so would need to add that to default configs and documentation page
• I added some capabilities, need to document them

Description of how this change was tested

• No unit tests have been written or updated, but this has been tested on my physical robot platform in production for 6 weeks.

---

Future work that may be required in bullet points

• I know in this form this is not going to be commonly useful and would need to be setup to be configurable for end users. (see issue description) I would be willing do the configuration setup work but guidance or direction to the best example of how this is typically handled in the nav2 Eco-system would be appreciated.

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
• Should this be backported to current distributions? If so, tag with backport-*.

[SteveMacenski]

See #5585 (comment)

Basically: Just have the height_field be a parameter with a string type default to "z". Then you can set this in your yaml to "height" and no other code needs to change (i.e. no new iterators or changes to the check, just have iter_z use that parameterized string value)

[codecov]

Codecov Report

❌ Patch coverage is 85.71429% with 2 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_collision_monitor/src/pointcloud.cpp	85.71%	2 Missing ⚠️

Files with missing lines	Coverage Δ
nav2_collision_monitor/src/pointcloud.cpp	80.55% <85.71%> (+0.89%)	⬆️

... and 4 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]

Beyond that, we just need the new parameter introduced into the configuration guide https://docs.nav2.org/configuration/packages/configuring-collision-monitor.html

Can this be removed from draft mode?

[greganderson-vermeer]

Beyond that, we just need the new parameter introduced into the configuration guide https://docs.nav2.org/configuration/packages/configuring-collision-monitor.html

Can this be removed from draft mode?

What about updating test cases? I would also like to field test this on our machine this week.

[SteveMacenski]

Sure thing! I don't know how easy it would be considering we don't use the height field in our simulations, but you could create a unit test that instantiates a server, spins its node in another thread, and then pass in some manually created pointcloud that contains height data & check the output topic for the behavior you expect given those height fields. If you check the unit tests for the collision monitor already, you should be able to find some like this (but you can make more simple / standalone if you like instead of the fixtures to save time).

Can you open the docs PR?

[SteveMacenski]

@greganderson-vermeer any update? I'd love to get this done this week before leaving for ROSCon :_)

[mergify]

This pull request is in conflict. Could you fix it @greganderson-vermeer?

[SteveMacenski]

any reason this is still a draft? I think this is good to merge once you open that docs PR! we just need the new parameter introduced into the configuration guide https://docs.nav2.org/configuration/packages/configuring-collision-monitor.html

[greganderson-vermeer]

any reason this is still a draft? I think this is good to merge once you open that docs PR! we just need the new parameter introduced into the configuration guide https://docs.nav2.org/configuration/packages/configuring-collision-monitor.html

I wanted to get tests implemented. I am having problems with the 3rd test case I added having inconsistent results, not sure what to do with it. I think its a race condition between the next action pub and the polygon error in velocity_polygon.cpp on line 136 since the polygon is set to a nullptr at line 424 of the collision monitor node.

Ill get the documentation PR open.

[SteveMacenski]

@greganderson-vermeer please sign off with DCO

Also a couple of your new tests failed

/opt/overlay_ws/src/navigation2/nav2_collision_monitor/test/collision_monitor_node_test.cpp:1698
Value of: waitActionState(500ms)
  Actual: false
Expected: true

/opt/overlay_ws/src/navigation2/nav2_collision_monitor/test/collision_monitor_node_test.cpp:1698
Value of: waitActionState(500ms)
  Actual: false
Expected: true

[SteveMacenski]

Docs PR linked: ros-navigation/docs.nav2.org#796

[greganderson-vermeer]

@SteveMacenski It looks like to add sign offs on two of my older commits I will have to do a rebase with a force push. Since I have already merged with dev I believe this is going to change a large chunk of commit history going back to main. How would you like me to proceed?

[SteveMacenski]

Rebasing or pulling in a merge commit from main should be fine as we squash all PRs into a single commit into the main branch anyway. As long as the diff of the PR doesn't show work outside of the scope of your commits, its all fine. I'm not particularly a git-purist. As long as it works & doesn't contain outside work (and since we squash all commits), I'm OK with it.

You still have a testing issue:

collision_monitor_node_test.gtest.missing_result

Means something probably hung:

11: [ RUN      ] Tester.testIncorrectSourceType
11: [INFO] [1761053788.532223430] [collision_monitor]: 
11: 	collision_monitor lifecycle node launched. 
11: 	Waiting on external lifecycle transitions to activate
11: 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
11: [INFO] [1761053788.534206716] [collision_monitor]: Configuring
11: [ERROR] [1761053788.534661910] [collision_monitor]: [UnknownSource]: Unknown source type: unknown
11: [INFO] [1761053788.534692874] [collision_monitor]: Cleaning up
11/12 Test #11: collision_monitor_node_test ......***Timeout  60.06 sec
test 12

[mergify]

This pull request is in conflict. Could you fix it @greganderson-vermeer?

[SteveMacenski]

You didn't fix DCO

[mergify]

This pull request is in conflict. Could you fix it @greganderson-vermeer?

[greganderson-vermeer]

You didn't fix DCO

I've not been able to find a way to do that without all the previously merged files (102) from main also showing up in the diff.

Can you overide the DCO check? Or I could close this PR and manually merge my changes locally to a new branch and PR.

[SteveMacenski]

This is something unfortunately that you should do. So a new PR would work. I don't think you should need to do that though, I don't see any main changes in your git history for the 15 commits in this PR. The instructions in the DCO job should do this correctly, I think?

git rebase HEAD~15 --signoff
git push --force-with-lease origin feat/CustomCollisionPointFields

[mergify]

This pull request is in conflict. Could you fix it @greganderson-vermeer?

[greganderson-vermeer]

I just updated and pulled the upstream main, merged that main with my branch again, and tried the DCO commands and still end up with 104 changes in the diff. I'm going to manually build a single commit locally on top of the latest of main and force push that to this branch and see what happens.

[greganderson-vermeer]

This tested OK locally. Diff looks good. DCO is happy. Fingers crossed!

[SteveMacenski]

Great thanks for the iteration (on iterators)! 😄

[greganderson-vermeer]

@SteveMacenski Is there anything I need to do to backport this into Jazzy?

[SteveMacenski]

@greganderson-vermeer yes, please cherry pick and open the Jazzy PR. This won't be auto-backportable for my mergify bot to do the work for us unfortunately. But it should be an easy 1-2 conflicts to deal with manually. I can merge that once its open pretty easily