How to set "advance" parameters for each node in Python

[Rodhaaret]

Hey everyone!

In short my problem is understanding:

1. Which parameters can be set for rtabmap in Python (ex. Maker off/on, Laser: off/on etc.)
2. How do i set them correctly in Python?

Just to help you understand what im trying to do, my code is below. I'm then trying to turn things on and off to improve the performance.

• P.s. any suggestion for making it run better is also much appreciated!

import os
import sys
import launch
import launch_ros
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition, UnlessCondition
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():     

    config_dir = os.path.join(get_package_share_directory('command_center'), 'config')         
    map_dir = os.path.join(get_package_share_directory('command_center'), 'maps')

    rviz_config = os.path.join(config_dir,"rtabmap_spot_4cam.rviz")
    map = os.path.join(map_dir, 'rtabmap_spot_4cam.db') 

    rviz_node = launch_ros.actions.Node(
        package='rviz2', executable='rviz2', output='screen',
        arguments=[["-d"], [rviz_config]]
    )

    # Camera remapping nodes
    spotcam_sync1_node = launch_ros.actions.Node(
        package='rtabmap_sync', executable='rgbd_sync', name='rgbd_sync1', output="screen",
        parameters=[{
            "approx_sync": True,
            "queue_size":100,
            }],
        remappings=[
            ("rgb/image", "/camera/frontleft/image"),
            ("depth/image", "/depth_registered/frontleft/image"),
            ("rgb/camera_info", "/camera/frontleft/camera_info"),
            ("rgbd_image", 'rgbd_image')], #Not sure what is meant here
        namespace='spot_camera_frontleft'
    )
    spotcam_sync2_node = launch_ros.actions.Node(
        package='rtabmap_sync', executable='rgbd_sync', name='rgbd_sync2', output="screen",
        parameters=[{
            "approx_sync": True,
            "queue_size":100,
            }],
        remappings=[
            ("rgb/image", "/camera/frontright/image"),
            ("depth/image", "/depth_registered/frontright/image"),
            ("rgb/camera_info", "/camera/frontright/camera_info"),
            ("rgbd_image", 'rgbd_image')], #Not sure what is meant here
        namespace='spot_camera_frontright'
    )
    spotcam_sync3_node = launch_ros.actions.Node(
        package='rtabmap_sync', executable='rgbd_sync', name='rgbd_sync3', output="screen",
        parameters=[{
            "approx_sync": True,
            "queue_size":100,
            }],
        remappings=[
            ("rgb/image", "/camera/left/image"),
            ("depth/image", "/depth_registered/left/image"),
            ("rgb/camera_info", "/camera/left/camera_info"),
            ("rgbd_image", 'rgbd_image')], #Not sure what is meant here
        namespace='spot_camera_left'
    )
    spotcam_sync4_node = launch_ros.actions.Node(
        package='rtabmap_sync', executable='rgbd_sync', name='rgbd_sync4', output="screen",
        parameters=[{
            "approx_sync": True,
            "queue_size":100,
            }],
        remappings=[
            ("rgb/image", "/camera/right/image"),
            ("depth/image", "/depth_registered/right/image"),
            ("rgb/camera_info", "/camera/right/camera_info"),
            ("rgbd_image", 'rgbd_image')], #Not sure what is meant here
        namespace='spot_camera_right'
    )

    # RGB-D odometry
    rgbd_odometry_node = launch_ros.actions.Node(
        package='rtabmap_odom', executable='rgbd_odometry', output="screen",
        parameters=[{
            "frame_id": 'body',
            "odom_frame_id": 'odom',
            "publish_tf": True, 
            "approx_sync": True,
            "subscribe_rgbd": True,
            "queue_size":100,
            "RGBD/StartAtOrigin": "false" ,
            #"RGBD/MarkerDetection":"True",

            #"Odom/FilteringStrategy": 2, #"0=No filtering 1=Kalman filtering 2=Particle filtering.
            }],
        remappings=[
            ("rgbd_image", '/spot_camera_frontleft/rgbd_image'),
            ("odom", '/odometry')],
        arguments=["--delete_db_on_start", ''],
        prefix='',
        namespace='rtabmap'
    )

    # Voxelcloud nodes
    voxelcloud1_node = launch_ros.actions.Node(
        package='rtabmap_util', executable='point_cloud_xyzrgb', name='point_cloud_xyzrgb1', output='screen',
        parameters=[{
            "approx_sync": True,
            "queue_size":100,
            "max_depth":2.0,
            "min_depth":0.1,
            "noise_filter_radius": 0.1,
            "filter_nans":True,
            "voxel_size":0.2,
            "decimation":4,
        }],
        remappings=[
            ("rgb/image", "/camera/frontleft/image"),
            ("depth/image", "/depth_registered/frontleft/image"),
            ("rgb/camera_info", "/camera/frontleft/camera_info"),
            ('rgbd_image', 'rgbd_image'),
            ('cloud', 'voxel_cloud1')]
    )

    voxelcloud2_node = launch_ros.actions.Node(
        package='rtabmap_util', executable='point_cloud_xyzrgb', name='point_cloud_xyzrgb2', output='screen',
        parameters=[{
            "approx_sync": True,
            "queue_size":100,
            "max_depth":2.0,
            "min_depth":0.1,
            "noise_filter_radius": 0.1,
            "filter_nans":True,
            "voxel_size":0.2,
            "decimation":4,
        }],
        remappings=[
            ("rgb/image", "/camera/frontright/image"),
            ("depth/image", "/depth_registered/frontright/image"),
            ("rgb/camera_info", "/camera/frontright/camera_info"),
            ('rgbd_image', 'rgbd_image'),
            ('cloud', 'voxel_cloud2')]
    )

    voxelcloud3_node = launch_ros.actions.Node(
        package='rtabmap_util', executable='point_cloud_xyzrgb', name='point_cloud_xyzrgb3', output='screen',
        parameters=[{
            "approx_sync": True,
            "queue_size":100,
            "max_depth":2.0,
            "min_depth":0.1,
            "noise_filter_radius": 0.1,
            "filter_nans":True,
            "voxel_size":0.2,
            "decimation":4,
        }],
        remappings=[
            ("rgb/image", "/camera/left/image"),
            ("depth/image", "/depth_registered/left/image"),
            ("rgb/camera_info", "/camera/left/camera_info"),
            ('rgbd_image', 'rgbd_image'),
            ('cloud', 'voxel_cloud3')]
    )   
    voxelcloud4_node = launch_ros.actions.Node(
        package='rtabmap_util', executable='point_cloud_xyzrgb', name='point_cloud_xyzrgb4', output='screen',
        parameters=[{
            "approx_sync": True,
            "queue_size":100,
            "max_depth":2.0,
            "min_depth":0.1,
            "noise_filter_radius": 0.1,
            "filter_nans":True,
            "voxel_size":0.2,
            "decimation":4,
        }],
        remappings=[
            ("rgb/image", "/camera/right/image"),
            ("depth/image", "/depth_registered/right/image"),
            ("rgb/camera_info", "/camera/right/camera_info"),
            ('rgbd_image', 'rgbd_image'),
            ('cloud', 'voxel_cloud4')]
    )  

    # SLAM 
    slam_node = launch_ros.actions.Node(
        package='rtabmap_slam', executable='rtabmap', output="screen",
        parameters=[{
            "rgbd_cameras":4,
            "subscribe_depth": True,
            "subscribe_rgbd": True,
            "subscribe_rgb": True,
            "subscribe_odom_info": True,
            "approx_sync_max_interval":0.7, 
            "queue_size":100,
            "odom_sensor_sync": True, #added by me
            "frame_id": 'body',
            "map_frame_id": 'map',
            "publish_tf": True,
            "database_path": map,
            "approx_sync": True,
            "Mem/IncrementalMemory": "true",
            "Mem/InitWMWithAllNodes": "true",
            "RGBD/MarkerDetection":"True",
            'Reg/Force3DoF':'true', # Maybe try 6dof (have to be in the mapping aswell)
            'RGBD/NeighborLinkRefining':'True',
            #"icp_odometry": True,
            "Reg/Strategy": '2', #0=TORO, 1=g2o and 2=GTSAM
            'Optimizer/GravitySigma':'0.3', # Disable imu constraints (we are already in 2D)
            'proj_max_ground_height':'0.1', #ncreasing the proj_max_ground_height will make the algorithm ignore points that are under this threshold while projection. All points below this threshold will be ignored:
            "proj_max_height":"2.0",

            #"Odom/FilteringStrategy": 2, #"0=No filtering 1=Kalman filtering 2=Particle filtering.
        }],
        remappings=[
            # Has to start from 0 and go up
            ("rgbd_image0", '/spot_camera_frontleft/rgbd_image'),
            ("rgbd_image1", '/spot_camera_frontright/rgbd_image'),
            ("rgbd_image2", '/spot_camera_left/rgbd_image'),
            ("rgbd_image3", '/spot_camera_right/rgbd_image'),
            ("odom", '/odometry')],
        arguments=["--delete_db_on_start"],
        prefix='',
        namespace='rtabmap'
    )

    return launch.LaunchDescription(
        [
        spotcam_sync1_node,
        spotcam_sync2_node,
        spotcam_sync3_node,
        spotcam_sync4_node,
        rgbd_odometry_node,
        slam_node,
        voxelcloud1_node,
        voxelcloud2_node,
        voxelcloud3_node,
        voxelcloud4_node,
        rviz_node,
        ]
    )

[matlabbe]

If you build rtabmap with OpenGV support, you could feed your 4 cameras to rgbd_odometry like you did for rtabmap node. This would result in more robust visual odometry.

To set rtabmap's parameters, you can do what you did:

parameters=[{
  'Reg/Force3DoF':'true'
  ...
}],

You can do:

ros2 run rtabmap_odom rgbd_odometry --params
or
ros2 run rtabmap_slam rtabmap --params

to see parameters used by those nodes.

Otherwise, if you have questions on more specific parts you want to change/try, I could try to advise.

[Rodhaaret]

Argh nice the ros2 run rtabmap_odom rgbd_odometry --params was exactly what i needed!

Okay that will be nice! Because we did experience some slight inconsistency when running it in localization mode. So i will try adding the following code to the rgbd_odom_node next time:

   remappings=[
            # Has to start from 0 and go up
            ("rgbd_image0", '/spot_camera_frontleft/rgbd_image'),
            ("rgbd_image1", '/spot_camera_frontright/rgbd_image'),
            ("rgbd_image2", '/spot_camera_left/rgbd_image'),
            ("rgbd_image3", '/spot_camera_right/rgbd_image'),
            ("odom", '/odometry')],
        arguments=["--delete_db_on_start"],
        prefix='',
        namespace='rtabmap'
    )

Thanks a lot. If you have any suggestion on how to make rtabmap run faster in localization mode it would be lovely. We are basicly just feeding the /obstacle topic and /map generated by rtabmap to topics used by Nav2. Though it is running a bit slow, especially for reliable obstacle avoidance. I should mention that it is running on a Intel Nuc with no GPU.

Our idea was to look into turn off unused parameter (eg. Octomap etc) to hopefully increase the speed. Maybe you have some suggestions how performance could be increased?

And again, thanks so much for the help!

[matlabbe]

Note that --delete_db_on_start is used only by rtabmap node.

a bit slow

How slow is it? if you can have frame rate statistics (ros2 topic hz ...). In particular, what is the frame rate of the generated visual odometry topic? or the generated point clouds used for obstacle detection? Don't know the resolution of the depth images, but you may be able to increase even more the decimation from 4 to 8, which could be still enough for obstacle detection. For VO, you can use param Odom/ImageDecimation to reduce size of the images, which may be handy if you combine 4 cameras.

For localization (rtabmap node), you can increase default update rate of 1 Hz to something higher with Rtabmap/DetectionRate parameter. You may also disable local grid generation by setting RGBD/CreateOccupancyGrid to false. You can also save time on compression with Mem/BinDataKept set to false.

For mapping, if you just want a 2D map, you may set Grid/3D to false and Grid/RayTracing to true. If you expect flat floor, you could set Grid/NormalsSegmentation to false to use a fast passthrough filter directly (then you will need to set at least Grid/MaxGroundHeight to segment the floor).

cheers, Mathieu