[DRAFT] ROS2 Wrapper

[nhewitt99]

Hi there, thanks for the exciting research and excellent python implementation. I've been plugging away at a minimal ROS2 wrapper (#5) over the last few days and want to share what I have so far for feedback from the authors / the larger community. It's still a long way from complete as a drop-in SLAM package and I don't usually do python ROS development, so I'm sure there's a lot to be improved.

Please feel free to close this if there's a more appropriate place for this ongoing development.

Implementation so far

This adds a subdirectory which contains a ROS 2 package, kiss_slam_ros with a single node. The node kiss_slam_node listens for PointCloud2 messages and runs them through KISS-SLAM to provide a map -> lidar transform, odometry, and a 2d occupancy grid.

Here's a crunchy demo gif:

Big questions

I'd appreciate some advice about approaching a few major challenges:

• Global occupancy mapping: I currently provide a 2d OccupancyGrid message by continually fusing local point clouds into an OccupancyGridMapper. This means that the grid is not updated to reflect global loop closures. This should likely be changed to a frontend/backend system, with a backend doing something like the SlamPipeline._global_mapping function after loop closures. However, this is costly and I'm unsure how we could make the SLAM objects thread-safe to do this asynchronously.
• Map to odom: SLAM systems in the ROS 2 ecosystem commonly correct drift in a mobile robot's odometry by providing the map -> odom transform (which can have discontinuities) while allowing odom -> base_link to remain smooth. In contrast, I currently implemented this as a monolithic map -> scan localization, providing only a potentially-discontinuous localization estimate, which is not easy to incorporate into developers' existing odometry workflows. I suppose we could add a tf listener for odom -> base_link, compare the difference with KISS-SLAM's localization, then publish the difference as map -> odom? Is this a sensible approach? Potentially we can also add a parameter to continue allowing using KISS-SLAM for the whole localization/odom solution in one for users not bringing external odom.

Smaller points

• Licensure: I want to be sure to fairly credit the authors' work but am new to FOSS contributions. Please let me know if the licensure and copyright metadata needs any changes, and if copyright headers should be added to source files.
• Config: There are currently no parameters for non-default KISS-SLAM configurations. At the very least, a parameter to point to config files should be added. (It would be nice to have a tight integration with the ros2 param system so that individual params can be configured from launchfiles and include 1 or 2 yaml examples in the package installation.)
• Formatting: Please let me know if there's an auto-formatter that I should run to unify style between the kiss-slam pip package and this wrapper. It looks like python formatting in ROS 2 is in a weird state...
• base_link: Transforms are published to the lidar frame right now rather than to base_link. This isn't a very difficult change to make (listen for a base_link -> lidar tf, and transform incoming point clouds) but I'm waiting until determining how to approach map->odom before more tf rework.
• Organization: Where should this live? I mimicked the directory structure in KISS-ICP but didn't add colcon ignore files yet. Happy to move the files or break out to a separate repo if appropriate.

[benemer]

Thanks for the PR! Happy to see that the community picks up on this :)

It may take some time to go through this and think about everything. In the meantime, everyone should feel invited to comment on this and share some ideas/use-cases, such that we can see what exactly the community wants to use this for.

Regarding some of your minor points:

• License: I am also not an expert, for now, I would handle it like this: For the files you created, put an MIT license with your name + co-authors.
• Config: Totally agree. It would be nice to have one set of default parameters, for example I don't really like the way we do it in KISS-ICP where we have (possibly different) default parameters for Python, C++, and ROS.
• Formatting: Can you use the pre-commit config?
• Organization: Good question. For now, I would keep it in this main repo as we do in KISS-ICP.

@nachovizzo, @tizianoGuadagnino, @saurabh1002 feel free to hop on, I think we all should be involved in the ROS 2 madness :)

[nhewitt99]

Thanks for the feedback! Not sure how I missed the pre-commit hooks! I ran linting and added license strings. I also took a first-pass at exposing config as node parameters.

edit: My approach in 8da02b1 was silly. d9c3ded creates and then fetches Parameters at runtime based on the pydantic config. This way, we don't have to worry about multiple sources of truth for default values, and can modify parameters with usual ROS tooling (s.a. ros2 run kiss_slam_ros kiss_slam_node --ros-args -p slam_config.local_mapper.voxel_size:=0.2)

Old ramblings about 8da02b1 collapsed

In the interest of avoiding duplicate (potentially different) default settings as mentioned, I took an approach that creates a default file from the `write_config` script at the package build time. This won't end up out of sync with upstream changes made to pydantic defaults in KISS-SLAM, etc. But it's opaque during development and a little hacky by running during setup.py. It also still doesn't expose individual SLAM parameters to the ROS parameter server. I think I have an idea for declaring params programatically from pydantic.

Also sorry for the force-pushes; made some mistakes in my original commit metadata. Shouldn't be any more.