ICP odometry ROS2 with realsense-like sensor on Gazebo

Hi, I would like to add the icp_odometry node to the simulation. I use the rtabmap binaries from OSRF repos for now and I'm on foxy ros2.

I try to fix the configuration but here is the error I get

[icp_odometry-1] [pcl::NormalEstimationOMP::compute] Both radius (1.000000) and K (5) defined! Set one of them to zero first and then re-run compute ().
[icp_odometry-1] [pcl::concatenateFields] The number of points in the two input datasets differs!

The full log

[INFO] [launch]: All log files can be found below /home/devcyclair/.ros/log/2022-01-12-15-30-10-569780-MSI-6926-563355
[INFO] [launch]: Default logging verbosity is set to INFO
/opt/ros/foxy/lib/python3.8/site-packages/launch_ros/actions/node.py:196: UserWarning: The parameter 'node_namespace' is deprecated, use 'namespace' instead
  warnings.warn("The parameter 'node_namespace' is deprecated, use 'namespace' instead")
[INFO] [icp_odometry-1]: process started with pid [563363]
[icp_odometry-1] 1641997810.979368 [0] icp_odomet: using network interface wlo1 (udp/192.168.1.36) selected arbitrarily from: wlo1, docker0
[icp_odometry-1] [INFO] [1641997811.001957199] [rtabmap.icp_odometry]: Odometry: frame_id               = camera_link
[icp_odometry-1] [INFO] [1641997811.002115614] [rtabmap.icp_odometry]: Odometry: odom_frame_id          = odom
[icp_odometry-1] [INFO] [1641997811.002142019] [rtabmap.icp_odometry]: Odometry: publish_tf             = false
[icp_odometry-1] [INFO] [1641997811.002151031] [rtabmap.icp_odometry]: Odometry: wait_for_transform     = 0.100000
[icp_odometry-1] [INFO] [1641997811.002198216] [rtabmap.icp_odometry]: Odometry: initial_pose           = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
[icp_odometry-1] [INFO] [1641997811.002214816] [rtabmap.icp_odometry]: Odometry: ground_truth_frame_id  = 
[icp_odometry-1] [INFO] [1641997811.002226989] [rtabmap.icp_odometry]: Odometry: ground_truth_base_frame_id = camera_link
[icp_odometry-1] [INFO] [1641997811.002239942] [rtabmap.icp_odometry]: Odometry: config_path            = 
[icp_odometry-1] [INFO] [1641997811.002250389] [rtabmap.icp_odometry]: Odometry: publish_null_when_lost = true
[icp_odometry-1] [INFO] [1641997811.002258557] [rtabmap.icp_odometry]: Odometry: guess_frame_id         = 
[icp_odometry-1] [INFO] [1641997811.002266062] [rtabmap.icp_odometry]: Odometry: guess_min_translation  = 0.000000
[icp_odometry-1] [INFO] [1641997811.002274662] [rtabmap.icp_odometry]: Odometry: guess_min_rotation     = 0.000000
[icp_odometry-1] [INFO] [1641997811.002284929] [rtabmap.icp_odometry]: Odometry: guess_min_time         = 0.000000
[icp_odometry-1] [INFO] [1641997811.002293291] [rtabmap.icp_odometry]: Odometry: expected_update_rate   = 0.000000 Hz
[icp_odometry-1] [INFO] [1641997811.002301562] [rtabmap.icp_odometry]: Odometry: max_update_rate        = 0.000000 Hz
[icp_odometry-1] [INFO] [1641997811.002309671] [rtabmap.icp_odometry]: Odometry: wait_imu_to_init       = false
[icp_odometry-1] [INFO] [1641997811.002334728] [rtabmap.icp_odometry]: Odometry: stereoParams_=0 visParams_=0 icpParams_=1
[icp_odometry-1] [WARN] [1641997811.014582599] [rtabmap.icp_odometry]: IcpOdometry: Transferring value 0.05 of "Icp/VoxelSize" to ros parameter "scan_voxel_size" for convenience. "Icp/VoxelSize" is set to 0.
[icp_odometry-1] [WARN] [1641997811.014649166] [rtabmap.icp_odometry]: IcpOdometry: Transferring value 5 of "Icp/PointToPlaneK" to ros parameter "scan_normal_k" for convenience.
[icp_odometry-1] [WARN] [1641997811.014672416] [rtabmap.icp_odometry]: IcpOdometry: Transferring value 1.0 of "Icp/PointToPlaneRadius" to ros parameter "scan_normal_radius" for convenience.
[icp_odometry-1] [ WARN] (2022-01-12 15:30:11.014) OdometryF2M.cpp:159::OdometryF2M() OdomF2M/BundleAdjustment=1 cannot be used with registration not done only with images (Reg/Strategy=1), disabling bundle adjustment.
[icp_odometry-1] [INFO] [1641997811.020662931] [rtabmap.icp_odometry]: IcpOdometry: queue_size             = 3
[icp_odometry-1] [INFO] [1641997811.020716646] [rtabmap.icp_odometry]: IcpOdometry: qos                    = 0
[icp_odometry-1] [INFO] [1641997811.020739853] [rtabmap.icp_odometry]: IcpOdometry: scan_cloud_max_points  = 500
[icp_odometry-1] [INFO] [1641997811.020755165] [rtabmap.icp_odometry]: IcpOdometry: scan_downsampling_step = 1
[icp_odometry-1] [INFO] [1641997811.020769928] [rtabmap.icp_odometry]: IcpOdometry: scan_range_min         = 0.200000 m
[icp_odometry-1] [INFO] [1641997811.020782504] [rtabmap.icp_odometry]: IcpOdometry: scan_range_max         = 3.000000 m
[icp_odometry-1] [INFO] [1641997811.020791579] [rtabmap.icp_odometry]: IcpOdometry: scan_voxel_size        = 0.050000 m
[icp_odometry-1] [INFO] [1641997811.020805056] [rtabmap.icp_odometry]: IcpOdometry: scan_normal_k          = 5
[icp_odometry-1] [INFO] [1641997811.020815237] [rtabmap.icp_odometry]: IcpOdometry: scan_normal_radius     = 1.000000 m
[icp_odometry-1] [INFO] [1641997811.020823840] [rtabmap.icp_odometry]: IcpOdometry: scan_normal_ground_up  = 0.000000

The launch file

...
    parameters_icp=[{
        'frame_id':'camera_link',
        'subscribe_depth':True,
        'subscribe_scan_cloud':True, 
        'scan_cloud_max_points': 500,
        'scan_range_max': 3.0,
        'scan_range_min': 0.2,
        'subscribe_scan':False,         
        'subscribe_rgbd':False,
        'subscribe_rgb':True,
        'approx_sync':True,
        'use_sim_time':True,
        'publish_tf':False,
        'odom_frame_id':'odom',
        # # 'odom_topic':'odom'
        'queue_size': 3,
        'map_always_update': True,
        'map_empty_ray_tracing': True,
          }]
...
    remappings=[
           ('rgb/image', '/camera/segmented/image_raw'),
           ('rgb/camera_info', '/color/camera_info'),
           ('depth/image', '/depth/image_raw' ),
          ('scan_cloud', '/depth/color/points' )]
...
        Node(
            package='rtabmap_ros', executable='icp_odometry', output='screen',
            parameters=parameters_icp,
            node_namespace='rtabmap',
            arguments=['-d  --uinfo ' ], #--uinfo --Icp/Strategy 0
            remappings=remappings),

Is there a param I forgot ?

introlab / rtabmap_ros

ICP odometry ROS2 with realsense-like sensor on Gazebo #701