Port obstacle layer in nav2_costmap_2d

[bpwilcox]

The obstacle layer plugin (and related dependencies) remains to be ported to ROS2 in the costmap_2d package. The package currently compiles without including this layer. There is, though, a dependency on the tf2_sensor_msgs package in geometry2 which has yet to be ported to ROS2.

[SteveMacenski]

What about tf2_sensor_msgs is not ported? It looks complete to me. Once dependencies are done, I can commit to porting this and the voxel layer

[bpwilcox]

What about tf2_sensor_msgs is not ported? It looks complete to me. Once dependencies are done, I can commit to porting this and the voxel layer

tf2_sensor_msgs is not yet ported to ROS2. It currently has an AMENT_IGNORE and has ros1 dependencies. It should be a simple port, however, but being outside of the navigation stack means we need to sort it out as a PR to the geometry2 package. That's on my short list, so I'll post here when that is complete.

[SteveMacenski]

I took a look at this last night, nothing in obstacle layer actually needs tf2_sensor_msgs. There's 1 function the observation buffer uses that is defined in an include upstream of tf2_sensor_msgs. I do not think this port is required.

But let me get the observation buffer done and PRed before I bite my tongue

tongue bitten. Is needed. Will port sometime this week. It's such a small header though I'll just fork geometry2 and fix then add to the ros2_dependencies.repos file until geometry2 upstream merges.

[mkhansenbot]

@SteveMacenski - Chris Ye of Intel is working on #132, also linked to ros2/geometry2#59

I've asked for an update on the latter issue. I'd suggest you wait to hear from him first before starting your own port.

[SteveMacenski]

@mkhansen-intel that appears to be the message filter in tf2_ros not the tf2_sensor_msgs package/header. I'm already pretty deep in it - if he gets to it before me then all good. I'm mostly using this as an exercise to learn all the new ament tools anyhow

[bpwilcox]

@SteveMacenski Do you have any update on porting the obstacle layer? Depending on where you are, if we can coordinate with @prasenjitdan who would like to help with the voxel layer, that would be great. For the porting of the dynamic reconfigure functionality, you can take a look at PR #256. Because achieving full costmap_2d functionality is very high priority this first half of November, let me know if there is anything blocking now and how quickly we can push on this. I can dedicate myself to this if needed, but glad to have your contributions.

[SteveMacenski]

@prasenjitdan @bpwilcox My branch is listed below.

The state is:

• ported message types, headers, and logging
• ported the simple parameters

Needed (just parameter stuff):

• validation of parameter nodehandles picking up needed params
• Deal with getting a node object to work with in the stringstream unpacking + search parameter replacement
• some message filter issues that appears to be upstream in tf2_ros message filters concerning _1

https://github.com/SteveMacenski/navigation2/tree/port_obstacle

The massive plus side if that this same code is used for the voxel layer by in large so if its done once it'll be good across the board and voxel layer porting will be straight forward.

I would absolutely welcome @prasenjitdan to take over and bring the rest across the finish line. You'll see TODO STEVE's in those 3 areas.

Please let me know if you're actively working on it, as it would free me up to work on other things

[SteveMacenski]

The branch as it exists does not compile due to the tf2_ros _1 error. If you comment out the while (ss << string) loop + tf2_ros/messagefilter header it should compile fine.

[prasenjitdan]

@SteveMacenski and @bpwilcox,

At this time, Team is working on 6 packages that we are porting and testing. We have also lined up the next 6 packages for next batch. I can include Voxel Layer replacing one of the identified packages as priorities. I have been given 42 packages to start with, where Obstacle Layer is not part of it. I'm going to check internally and externally to see if we can include " Obstacle Layer". I'll keep you posted.

Thanks,
Dan

[SteveMacenski]

Ok, given that the voxel layer derives from the obstacle layer, in order to port the voxel layer, it requires the obstacle layer. I think it should be easy to tie them together :)

[prasenjitdan]

Hi Steve,

I was able to prioritize obstacle layer under our next porting targets. Can you please confirm that the package name corresponding to Obstacle Layer is costmap_2d or not? If so, I'm assuming that the request is actually port costmap_2d. Correct?

Thanks,
Dan

[SteveMacenski]

This ticket was to cover just the costmap_2d/obstacle_layer port, as part of the larger costmap_2d port. But if you can port everything, that's a major plus.

If you look at the CMakeLists for it, you'll see the commented out sections are the parts still needing a port. This shows obstacle_layer, voxel_layer being the major 2 things needed done, and then the costmap_2d_{markers, cloud} scripts which are auxiliary capabilities and very simple to port over but haven't been because they're dependent on the voxel_layer

[bpwilcox]

@prasenjitdan As @SteveMacenski mentioned, the commented out sections in the CMakeLists are the ones yet to be fully ported (with exception of voxel_grid package which was recently ported in #243). If you can commit to porting the obstacle/voxel layer, then could you also provide us an estimate on when you believe you'd be able to dedicate work and submit a PR for us? We're currently debugging some integration issues at the moment, but once we're stable running the full stack (ideally before end of next week), we're going to want to push full functionality with costmap.

[bpwilcox]

Since I haven't seen a response from @prasenjitdan, I decided to take this myself and finish @SteveMacenski's port of obstacle_layer on my own branch. I'm already almost done (it compiles fully), with exception of re-enabling the dynamic parameter callback and testing the code run-time.

[SteveMacenski]

Not sure what you mean by dynamic parameter callback but thanks for taking on the effort! I'm going to be slammed by work until after Christmas, hopefully I can take back on a more significant chunk in the new year.
Until then.. bunches of small PRs :-)

[bpwilcox]

Not sure what you mean by dynamic parameter callback but thanks for taking on the effort! I'm going to be slammed by work until after Christmas, hopefully I can take back on a more significant chunk in the new year.
Until then.. bunches of small PRs :-)

I was referring to the replacement for dynamic reconfigure with the reconfigureCB callback that I believe you removed temporarily.

[prasenjitdan]

@bpwilcox, Team is still working on the previous set of packages that includes pcl-ros/perception, xacro, diagnostics-updater/aggregator, libccd, etc. Though we really wanted to extend support in porting obstacle layer and Voxel layer, it will happen only after previous packages are completely ported/tested in ROS2. I really appreciate your initiative on porting Obstacle Layer, and considering porting Voxel layer as well. If you have time, please go ahead with your plan on porting Voxel Layer as well. We are aiming to finish porting of current packages by Nov 16. We can start testing Obstacle Layer and Voxel Layer in ROS2 after that if that helps.

[bpwilcox]

Resolved with #318