ROS2 Support

[Divelix]

First of all, thank you for open sourcing wavemap! I just launched it inside container on newer college and panoptic mapping datasets and it seems to me like a neat tool for real-time local mapping tool for navigation and manipulation tasks, but our tech stack is fully based on ROS2 and I would like to avoid using ros1_bridge in such data intensive task so ROS2 wrapper is highly appreciated.

I thought it could be implemented as separate branch like rtabmap does it or to separate both ROS1 and ROS2 wrappers from core wavemap repo into their own repos.

What do you think about ros2 integration in general? I'm currently exploring wavemap's codebase and probably would make fork for experiments with ros2.

[victorreijgwart]

Hi @Divelix, I'm glad to hear you like the project! A ROS2 interface would be useful to a lot of people, and we would be keen to add it. Our lab is still almost exclusively on ROS1, so I'm not very familiar with ROS2's new mechanisms and wouldn't get to test the new interface on real robots myself. Would you be able to do this testing, to ensure the ROS2 interface is reliable and has good performance?

In terms of implementation, I think it should be possible to maintain a single branch that is compatible with both ROS versions. All of wavemap's core functionality is already packaged in ROS agnostic libraries (in the libraries folder). Porting these should mainly be a matter of updating their CMakeLists to work with both catkin and colcon. For the ROS interfaces, we could rename the ros folder to ros1, create a second ros2 folder and port ROS2 versions of wavemap's ROS messages, ROS server, and Rviz plugin into that.
In the coming weeks I have quite a few deadlines coming up, but I'd be happy to help and can also extend the CI to include ROS2.

A colleague of ours created a ROS2 fork of wavemap about half a year ago during a hackathon and got it to build. Wavemap has changed quite a lot since then, but it's probably still a good starting point :)

[victorreijgwart]

The KISS-ICP package implemented its ROS1&2 support really nicely. We could use that as a reference as well.

[Divelix]

The KISS-ICP file structure looks great. However, the issue lies in the fact that the wavemap core library is not truly ROS-agnostic, as it is built via Catkin. Even worse, it depends on Catkin-based libraries such as glog_catkin, eigen_catkin, tracy_catkin, and minkindr. Therefore, it seems that the first step in transitioning to a KISS-ICP-like structure is to rebuild the core module (located in the 'libraries' folder) using plain CMake instead of Catkin.

[victorreijgwart]

Hi @Divelix, I'm currently travelling but will get back to you by next week.

[Divelix]

I managed to build wavemap library in CMake (without Tracy for now):

1. Install glog, boost and Eigen via apt (easy to integrate into docker):

sudo apt install \
    libgoogle-glog-dev \
    libboost-all-dev \
    libeigen3-dev

2. Copy kindr conversions from here, preserving file structure to remain all #include statements working without editing them. kindr is on the same level as wavemap and wavemap_io.
3. Add these lines to CMakeLists.txt

find_package(Eigen3 3.3 REQUIRED NO_MODULE)
find_package(glog 0.4.0 REQUIRED)

Also cmake complained on optional, so I added #include <optional> to param.h and set CXX standard to 17.

[victorreijgwart]

Pulling apt packages and expecting them to correspond to specific versions will likely not work across Ubuntu releases, but we can try this for now and extend CI later to test additional Ubuntu versions. If it breaks, an alternative would be only to set the minimum version or pull in specific versions through FetchContent/ExternalProject_Add.

[Divelix]

I managed to install glog, eigen and tracy via FetchContent. Boost is apt installed in my setup for now as temporary solution.
About Boost: wavemap uses only preprocessor from it which is headers only, so libboost-dev is enough (libboost-all-dev takes more than 500 Mb on disk).

[victorreijgwart]

Nice! Pulling them in with FetchContent seems like a nice solution. Then the framework is easy setup on new computers, portable across distros and we'd be sure the versions we test in CI match what the users experience. Regarding boost that sounds good. As a note for the future, we could remove the dependency on its preprocessor too. But lets focus on the bigger picture of getting wavemap to build with colcon and making its ROS2 interface feature complete first.

[Divelix]

I managed to add Tracy support in CMake (found solution in Tracy docs). Just add these lines to CMakeLists.txt:

include(FetchContent)
FetchContent_Declare(
    tracy
    GIT_REPOSITORY https://github.com/wolfpld/tracy.git
    GIT_TAG v0.10
    GIT_SHALLOW TRUE
    GIT_PROGRESS TRUE
)
FetchContent_MakeAvailable(tracy)

# After add_library()
target_link_libraries(${PROJECT_NAME} PUBLIC TracyClient)

It builds really fast, so I assume it pulls only Tracy's thin client, which is cool.

[edwinpha]

Hi @victorreijgwart and @Divelix,
I was going to ask about the ROS2 support in an issue when I saw this discussion. Do you need any help with the development?

Btw, thanks for the open-sourcing wavemap!

[edwinpha]

Sorry, @victorreijgwart could you try it again? I had some commits in local that I forgot to push.
Just to be 100% sure, the build folder should have 12 pkgs instead of 17 (rm catkin_simple, eigen_catkin, gflags_catkin, glog_catkin and tracy_catkin)

• Sure! I thought of it as a temporal and fast solution to fork it in my repo. Once we are ready with minkindr we can create a PR without catkin_simple and another for ROS2
• Well, I decided to remove catkin_simple because of what you mentioned. It is a really nice pkg but not the best if you need to debug and have a clean installation (ROS2 transition). Also, I guess we should create a kind of "colcon_simple" to make it work on ROS2. So it seemed like the cleaner thing to do to remove it from the dependencies!

[edwinpha]

Ok! That seems good for eigen_checks and minkindr! I will try it and let you know!

[victorreijgwart]

Your newest changes also build. CMake and GCC generate some warnings (mainly related to Tracy). But they don't look critical, so we can ignore them for now. I should have some time to address them toward the end of this week :)

[edwinpha]

That would be great! I recently added minkindr with FetchContent as we were saying...and it worked!

• The only thing is I had to explicitly remove the gtests in the CMakeLists.txt of minkindr because it crashed when looking for eigen_checks (expected).
• Besides this fact, minkindr has been added as a header-only library!

I will continue in a couple of days with the transition to ROS2 because I am a little busy right now; however, it seems to me that we are at a good point. Let me now of any updates regarding Tracy.

[victorreijgwart]

Awesome! I'll let you know once I get to work on Tracy/GCC's warnings.

[Divelix]

I don't rush with commits, because it seems to me that such change can't be done incrementally. From my perspective there should be one monster commit with project refactoring, then we add ros2 dummy colcon package which we will fill with features incrementally (catching up with current features in ros1 wrapper).

[Divelix]

I managed to assemble wavemap (with dummy ros wrappers for now) in kiss-icp folder structure and build it with both colcon and catkin without errors.
I had to add such lines into common.h to ignore many warning treated as errors:

#pragma GCC diagnostic ignored "-Wfloat-conversion"
#pragma GCC diagnostic ignored "-Wconversion"
#pragma GCC diagnostic ignored "-Wsign-compare"

@victorreijgwart I had to assemble wavemap from scratch, so I'm not sure if I should create monster commit in feature/ros2 branch (which would probably erase all current progress in this branch) or create temporary github repo to let you take a look at the code?

[victorreijgwart]

It would be great if you could share it as a fork, then I can take a look :)

[Divelix]

@victorreijgwart ok, there is my fork. I added dockerfiles for ros1/ros2 and README.md with instructions.

My next step is to integrate your ROS1 wrapper code, then launch files and rviz plugin. I think we should first implement and merge project refactoring with full current functionality and after that start ROS2 wrapper.

Moreover, kiss-icp structure allows easy integration of python bindings, which is also interesting to me.

[Divelix]

I created draft PR from this fork to make it more visible for interested people. At some point of development of this fork, when full picture of applying new file structure to wavemap would be more clear, we could make another PR that would make such massive change incremental (less disruptive to git history) and delete this one,

There is a lot of work to do to make it conform current main branch in both tooling and functionality, but it is a good place to start. It would be nice if @victorreijgwart, @edwinpha and other people who will read this would help to make this massive change as fast and clean as possible.

[edwinpha]

@Divelix oh wow, that is an impressive fork.
@victorreijgwart I am working locally to translate the ROS1 to ROS2 pkg, with the current pkg structure. For now, I have managed to build wavemap_msgs, wavemap, and wavemap_ros_conversions. In the last one, I had to add this repo because xmlrpcpp is not on ROS2.
I will push some commits as soon as I have a cleaner ws.

[victorreijgwart]

@edwinpha Great that you managed to already get all those packages to build. Regarding xmlrpcpp, I don't think we actually need it on ROS2. In ROS1, we need it to load the relevant parameters from the ROS param server. But internally, wavemap uses its own ROS-agnostic types to handle configurations. To interact with ROS2's parameter system, we probably need to rewrite the config_conversions methods and the dependency on xmlrpcpp will probably get replaced with what ROS2 uses.

[edwinpha]

Hi @victorreijgwart, I have pushed in the branch some commits to build the following packages from wavemap in ROS2:

• wavemap_msgs
• wavemap
• wavemap_ros_conversions
• wavemap_io
  TODO:
• wavemap_rviz_plugins (currently working on - some compilation errors related to OGRE and others)
• wavemap_ros
• wavemap_examples
• wavemap_all
• wavemap_utils

I am testing it locally on my PC (ROS2 Humble); probably a good idea is to use a docker container for replicability if anyone else wants to collaborate. Also, related to the config_conversions_methods, I am currently using the mentioned xmlrpcpp repo to make it build. Maybe later, it can be replaced with what ROS2 uses.

This week is going to be a little tricky for me to work on this because I will be attending the ICAR conference, so I wanted to update the current position of the branch.

[victorreijgwart]

Thanks for the update and all your work! Developing locally is also good and probably faster. In case Docker eventually becomes needed for collaboration, this Dockerfile could be a useful reference. It's a bit complicated because it uses many optimizations to build faster in our CI system. If it helps, I can make a simplified version and/or take care of porting it to ROS2. Let me know if you run into any questions or issues with wavemap's config system or Rviz plugin. Although I'm quite tight on time, I would be happy to help where I can.
Have a great time at ICAR :)