Replaced sensor_msgs/PointCloud to sensor_msgs/PointCloud2

[saching13]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#1967
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo simulation of Turtlebot3 Waffle

---

Description of contribution in a few bullet points

• Replaced PointCloud Msg and publishers with PointCloud2
• Changed a rclcpp publisher created from lifecycle node to lifecycle publisher
• Modified the base_obstacle_test.cpp to remove PointCloud as arg to `addCriticVisualization

Description of documentation updates required from your changes

• Change the documentation of addCriticVisualization(...) under TrajectoryCritic, BaseObstacleCritic and MapGridCritic

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration 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

[SteveMacenski]

I can see why this took a little bit, that DWB visualization was convoluted as hell before you even stepped foot in here and the complexities of working with PC2's certainly made that harder.

This overall looks very good, only superficial things we should work on

[SteveMacenski]

I also noticed you didn't use the PC2 Modifier object: https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.hpp#L47-L70

Is there a reason for that? It may have made some things simpler, it'll handle some of the field stuff for you. An example of how people use it right now is in this post https://discourse.ros.org/t/proposal-for-a-new-pointcloud2iterator-pointcloud2modifier/19122 (though note the general post is about adding a new PC2 util which does not yet exist)

It also deals with the offsets for you! https://github.com/ros2/common_interfaces/blob/975f1ce52645a9eabe27a222ceb43bbbba268956/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.hpp#L101

I think it could largely simplify all 3 of the major places you have to populate pointclouds

[saching13]

I can see why this took a little bit, that DWB visualization was convoluted as hell before you even stepped foot in here and the complexities of working with PC2's certainly made that harder.

Actually, it didn't take much time to implement.
I took some time initially to understand how to test.
I spent a lot of time trying to figure out why clearing_endpoints_ is not working during testing.

I saw the modifier but I don't recall why I chose not to use it.
But now that I look at it again and the post. it makes more sense to use it.
Should I change it?

[SteveMacenski]

I'd recommend it, I think it would simplify this code alot more for later modification / reading since it would abstract out alot of the field / offsets work

[saching13]

I changed to use Modifier for voxel_layer and costmap_2d_cloud.
But It looks like I cannot call setPointCloud2Fields repetitively in a loop from publisher in dwb_core.
Any suggestions?

if only offset was private member here :p

[SteveMacenski]

Can you point specifically to where you have the issue with DWB and what the error is?

[saching13]

https://github.com/saching13/navigation2/blob/e50a00697502557214cca58338f3dc938bf626a0/nav2_dwb_controller/dwb_core/src/publisher.cpp#L289-L301

Here. I have the PointFields names in an array.

[codecov]

Codecov Report

Merging #2263 (ccb27e1) into main (3bf1f7a) will decrease coverage by 0.54%.
The diff coverage is 13.79%.

❗ Current head ccb27e1 differs from pull request most recent head da008d1. Consider uploading reports for the commit da008d1 to get more accurate results

@@            Coverage Diff             @@
##             main    #2263      +/-   ##
==========================================
- Coverage   85.06%   84.51%   -0.55%     
==========================================
  Files         254      254              
  Lines       12708    12738      +30     
==========================================
- Hits        10810    10766      -44     
- Misses       1898     1972      +74     

Flag	Coverage Δ
project	84.51% <13.79%> (-0.55%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Impacted Files	Coverage Δ
...costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp	92.30% <ø> (ø)
nav2_costmap_2d/src/costmap_2d_cloud.cpp	0.00% <0.00%> (ø)
...controller/dwb_core/include/dwb_core/publisher.hpp	100.00% <ø> (ø)
...er/dwb_core/include/dwb_core/trajectory_critic.hpp	90.00% <0.00%> (ø)
...oller/dwb_critics/include/dwb_critics/map_grid.hpp	100.00% <ø> (ø)
nav2_dwb_controller/dwb_critics/src/map_grid.cpp	58.22% <0.00%> (ø)
nav2_dwb_controller/dwb_core/src/publisher.cpp	44.72% <2.17%> (-7.03%)	⬇️
nav2_costmap_2d/plugins/voxel_layer.cpp	78.99% <45.00%> (-1.01%)	⬇️
...2_dwb_controller/dwb_critics/src/base_obstacle.cpp	97.36% <100.00%> (ø)
...re/include/dwb_core/illegal_trajectory_tracker.hpp	40.00% <0.00%> (-60.00%)	⬇️
... and 8 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 3bf1f7a...da008d1. Read the comment docs.

[SteveMacenski]

Fair point, I don't think you can either. Each of the adding fields clears the existing. We could add a new function to PointCloud2Modifiers to enable adding additional ones, but I think that's overkill for this PR. You can ignore the DWB stuff then

[saching13]

We could add a new function to PointCloud2Modifiers

I would be happy to add it as it improves the PointCloud2Modifiers :)

[SteveMacenski]

That's a whole other process itself that could take easily several weeks to months, I think we should focus here first ;-)

[saching13]

When I run test locally using colcon test Everything passes sometimes and some different packages fails somethimes. Which is wierd ?
And on specific tests that I modifed. it failed sometimes and when I clear and rebuild it from scratch it works.
This is the command I used.
colcon test --packages-select nav2_system_tests --ctest-args -R test_speed_filter_local test_speed_filter_global test_keepout_filter

All three passes which contains my modifications. But fails on the CI..
Any suggestion on why and how I can fix this ?

[SteveMacenski]

[tester_node-17] [INFO] [1616959351.970963788] [nav2_tester]: ?[1;37;44mReceived cost_cloud points?[0m
[tester_node-17] [INFO] [1616959351.972979746] [nav2_tester]: ?[1;37;44mReceived Voxel Marked Points?[0m
[tester_node-17] [INFO] [1616959351.979771551] [nav2_tester]: ?[1;37;44mReceived Voxel unknown points?[0m

It may be due to excessive logging, that does take up bandwidth. The root cause is something else I've seen before that isn't your fault, but you should clean up the logs, that's too much logging (only log the first time you get something on each topic perhaps?)

[SteveMacenski]

If you submit a PR against https://github.com/ros-planning/navigation.ros.org/blob/master/migration/Foxy.rst for adding PC->PC2 conversion, that would be great. I think that's the last major thing

[saching13]

Yes. Working on it now.

[SteveMacenski]

If/when CI passes and docs PR, this is ready to go!

[saching13]

release_test passed but debug_test failed. any idea why ?
these tests seems to pass locally though

[SteveMacenski]

[bt_navigator-13] [ERROR] [1617171465.576777190] [bt_navigator]: Action server failed while executing action callback: "send_goal failed"

Isn't your fault, its unrelated

[SteveMacenski]

Thanks for the hard work!