[RTAB-Map] How to connect custom code from core to ROS 2 wrapper (Ubuntu 22.04, ROS 2 Humble)

[GGYU]

Dear Mathieu,

I’m currently using RTAB-Map on Ubuntu 22.04 with ROS 2 Humble, and I’m looking to customize the system by adding a feature into the RTAB-Map core (standalone) and making it accessible via the ROS 2 wrapper (rtabmap_ros).

To proceed effectively, I’d appreciate some guidance on how to approach this integration.

❓Questions
Which part of the RTAB-Map core should be modified to add custom processing or features?
For example, is Rtabmap.cpp, RtabmapEvent.cpp, or some other entry point generally used for this purpose?

What’s the recommended way to expose this new core functionality to the ROS 2 wrapper?
Should this be done by extending CoreWrapper.cpp, or would it be better to define a new topic/service interface?

Are there any existing examples of RTAB-Map core features being exposed to the ROS 2 wrapper that I could refer to?
For instance, how were functions like get_map, set_goal, or reset connected between core and wrapper?

🧩 Environment
OS: Ubuntu 22.04

ROS: ROS 2 Humble

RTAB-Map: Built from latest GitHub source

Any advice or guidance would be highly appreciated. Thank you for your time and your amazing work on RTAB-Map!

If the development is successful, I would also like to publish a paper based on this work.

Thank you always for your dedication and hard work.

Best regards,
GGYU

[matlabbe]

Hi GGYU,

It depends what kind of new feature you want to add.

For feature that extends current functionality, like "I want to integrate a new LiDAR registration approach", I would say create a new file here: https://github.com/introlab/rtabmap/tree/master/corelib/src/icp with the new approach following same interface that the current approaches. In this case, you would need to add a new option to "Icp/Strategy" parameter and update the "if" in RegistrationIcp class to use your function with your strategy is selected.

Param: Icp/Strategy = "1"  [ICP implementation: 0=Point Cloud Library, 1=libpointmatcher, 2=CCCoreLib (CloudCompare).]

In rtabmap_ros, you would then only need to set parameter Icp/Strategy=3 to use your new lidar registration approach.

If you want to integrate a new graph optimization approach (like g2o, GTSAM), you can follow how parameter "Optimizer/Strategy" is used to select which strategy you want to use.

Param: Optimizer/Strategy = "2"  [Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.]

In same way, in rtabmap_ros we just need to change "Optimizer/Strategy" parameter to switch between the approach used.

To integrate a new Odometry approach, follow how "Odom/Strategy" parameter is used. You can see already many different implementations under https://github.com/introlab/rtabmap/tree/master/corelib/src/odometry, which can be used under ROS just by changing "Odom/Strategy" parameter.

Param: Odom/Strategy = "0" [0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D]

Another example is the Visual feature approach:

Param: Kp/DetectorStrategy = "6" [0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector]
Param: Vis/FeatureType = "6"     [0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector]

They are defined in https://github.com/introlab/rtabmap/blob/master/corelib/src/Features2d.cpp, and from ROS, we just need to set appropriate parameters to select them and/or changing their own parameters.

Overall, if you can expose a new parameter in Parameters.h, it would make it a lot easier to use it from rtabmap_ros.

For mapping approaches, we follow the same architecture design (https://github.com/introlab/rtabmap/tree/master/corelib/src/global_map), though in ROS MapsManager we call them individually based on subscribed topics (they can all be used at the same time). That's because the internal rtabmap library doesn't generate maps on its own, that was most a ROS specific feature.

Last note: for a new loop closure detector, it would be harder to replace the current one, as the whole code is made around the original loop closure detection approach. We however added some API functions to be able to use your own loop closure detector as a separate ROS node and call appropriate services on rtabmap node to add new loop closures in the map's graph. That's how CMR loop closure detector have been integrated.

cheers,
Mathieu