VINS Fusion + RTABMap with ROS2 Humble

classic Classic list List threaded Threaded
22 messages Options
12
Reply | Threaded
Open this post in threaded view
|

Re: VINS Fusion + RTABMap with ROS2 Humble

f0rward
This post was updated on .
Hi,

Yes, I did.

First, I sync the cameras' RGB and depth.
  sync_node = ComposableNode(
                package='rtabmap_sync',
                plugin='rtabmap_sync::RGBDSync',
                name=f'{camera_name}_sync',
                parameters=[sync_params],
                remappings=[
                    ('rgb/image', cam.color),
                    ('depth/image', cam.depth),
                    ('rgb/camera_info', cam.color_info),
                    ('depth/camera_info', cam.depth_info),
                    ('rgbd_image', f'{cam.name}/rgbd_image'),
                ],
            )

Then I use the inputs to pass it to RGBDOdometry
    params = {
            'frame_id': robot_base_link,
            'use_sim_time': use_sim_time,
            'publish_tf': publish_tf,
            'imu_frame_id': imu_frame_id,
            'publish_sensor_data': True,
            'subscribe_rgbd': True,
            'rgbd_cameras': len(camera_names),
            'approx_sync': True,
            'qos_image': 1,
            'qos_camera_info': 1,
        }

    # Create remappings
    remappings = [
        ("rgbd_image0", '/cam_front/rgbd_image'),
        ("rgbd_image1", '/cam_left/rgbd_image'),
        ("rgbd_image2", '/cam_right/rgbd_image'),
        ('odom', '/odom'),
    ]

    plugin_name = 'rtabmap_odom::StereoOdometry' if mode == 'stereo' else 'rtabmap_odom::RGBDOdometry'

    node = ComposableNode(
        package='rtabmap_odom',
        plugin=plugin_name,
        name='visual_odometry',
        parameters=[params],
        remappings=remappings,
        extra_arguments=[{'use_intra_process_comms': not sim_mode}],
    )

Rtabmap and rtabmapviz subscribe to sensor data.

ros2 topic echo /cam_right/rgbd_image --once | grep encoding
  encoding: rgb8
  encoding: 16UC1

ros2 topic echo /odom_sensor_data/raw --once | grep encoding
  encoding: mono8
  encoding: 16UC1

Edit:
Apparently, there is a parameter for that.
RGBDOdometry: keep_color     = false

I set it to true, and it now works as expected.

What is the difference between the parameters I pass through `params` and those passed as arguments (arguments=["--delete_db_on_start"])? (They should achieve the same things)

Can you achieve everything only with `params`?


Reply | Threaded
Open this post in threaded view
|

Re: VINS Fusion + RTABMap with ROS2 Humble

matlabbe
Administrator
You can do everything with parameters. For "--delete_db_on_start" (also called "-d") argument, there is a parameter called "delete_db_on_start" that can be used instead.

For RTAB-Map's library parameters, those shown when you do "rtabmap --params" should be passed as "string" value when using as ros parameters.

Because we are still parsing RTAB-Map's parameters inside the library (not ros), we also have the option to pass them as arguments to the node. This can be useful if we don't want to explicitly make a roslaunch argument for every rtabmap parameters. For example with rtabmap.launch.py, without changing the launch file, we can adjust the RTAB-Map's internal parameters by doing this for example:
ros2 launch rtabmap_launch rtabmap.launch.py args:="--Vis/MinInliers 10 --Vis/FeatureType 1 --Kp/DetectorStrategy 1"

However, when we "ros2 run" directly the node, there is not a huge difference between using arguments or ros parameters:
$ros2 run rtabmap_slam rtabmap -d  --Vis/MinInliers 10 --Vis/FeatureType 1 --Kp/DetectorStrategy 1

[INFO] [1755139282.535938086] [rtabmap]: Update RTAB-Map parameter "Kp/DetectorStrategy"="1" from arguments
[INFO] [1755139282.535968442] [rtabmap]: Update RTAB-Map parameter "Vis/FeatureType"="1" from arguments
[INFO] [1755139282.535973960] [rtabmap]: Update RTAB-Map parameter "Vis/MinInliers"="10" from arguments
[INFO] [1755139282.536251202] [rtabmap]: RTAB-Map detection rate = 1.000000 Hz
[INFO] [1755139282.536444358] [rtabmap]: rtabmap: Deleted database "/home/mathieu/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[INFO] [1755139282.536460977] [rtabmap]: rtabmap: Using database from "/home/mathieu/.ros/rtabmap.db" (0 MB).
[INFO] [1755139282.575346127] [rtabmap]: rtabmap: Database version = "0.22.1".

----------------------------

$ ros2 run rtabmap_slam rtabmap --ros-args -p delete_db_on_start:=true  -p Vis/MinInliers:="'10'" -p Vis/FeatureType:="'1'" -p Kp/DetectorStrategy:="'1'"

[INFO] [1755139363.332874544] [rtabmap]: Setting RTAB-Map parameter "Kp/DetectorStrategy"="1" (rosparam)
[INFO] [1755139363.335673199] [rtabmap]: Setting RTAB-Map parameter "Vis/FeatureType"="1" (rosparam)
[INFO] [1755139363.335765468] [rtabmap]: Setting RTAB-Map parameter "Vis/MinInliers"="10" (rosparam)
[INFO] [1755139363.336387898] [rtabmap]: RTAB-Map detection rate = 1.000000 Hz
[INFO] [1755139363.336528024] [rtabmap]: rtabmap: Deleted database "/home/mathieu/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[INFO] [1755139363.336546337] [rtabmap]: rtabmap: Using database from "/home/mathieu/.ros/rtabmap.db" (0 MB).
[INFO] [1755139363.370205899] [rtabmap]: rtabmap: Database version = "0.22.1".

12