search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
986 stars 555 forks source link

[BUG] RGBD_Odometry node subscribes to incorrect topic when rgbd_cameras = 0 #1216

Closed shivamtrip closed 2 weeks ago

shivamtrip commented 3 weeks ago

Configuration: Ubuntu 22.04, ROS2 Humble

Details: I am testing multi-camera visual odometry. I set the rgbd_cameras parameter to 0, but the RGBD_Odometry node is still subscribed to the rtabmap/rgbd_image topic instead of the rtabmap/rgbd_images topic. However, the RTABMapViz node subscribes to the correct topic.

Refer to terminal output below. Even though the RGBD_Odometry node prints that rgbd_cameras = 1, I confirmed that rgbd_cameras = 0 for the node by verifying the parameter value on the parameter tree (ros2 param get /rtabmap/rtabmap_viz rgbd_cameras).

[rgbd_sync-2] [INFO] [1727969194.305150319] [camera_2.rgbd_sync_2]: rgbd_sync_2: approx_sync = false
[rgbd_sync-2] [INFO] [1727969194.305254483] [camera_2.rgbd_sync_2]: rgbd_sync_2: topic_queue_size  = 1
[rgbd_sync-2] [INFO] [1727969194.305263040] [camera_2.rgbd_sync_2]: rgbd_sync_2: sync_queue_size  = 10
[rgbd_sync-2] [INFO] [1727969194.305267471] [camera_2.rgbd_sync_2]: rgbd_sync_2: qos             = 1
[rgbd_sync-2] [INFO] [1727969194.305271939] [camera_2.rgbd_sync_2]: rgbd_sync_2: qos_camera_info = 1
[rgbd_sync-2] [INFO] [1727969194.305276540] [camera_2.rgbd_sync_2]: rgbd_sync_2: depth_scale = 1.000000
[rgbd_sync-2] [INFO] [1727969194.305289956] [camera_2.rgbd_sync_2]: rgbd_sync_2: decimation = 1
[rgbd_sync-2] [INFO] [1727969194.305294497] [camera_2.rgbd_sync_2]: rgbd_sync_2: compressed_rate = 0.000000
[rgbd_sync-1] [INFO] [1727969194.306040160] [camera_1.rgbd_sync_1]: rgbd_sync_1: approx_sync = false
[rgbd_sync-1] [INFO] [1727969194.306146512] [camera_1.rgbd_sync_1]: rgbd_sync_1: topic_queue_size  = 1
[rgbd_sync-1] [INFO] [1727969194.306155174] [camera_1.rgbd_sync_1]: rgbd_sync_1: sync_queue_size  = 10
[rgbd_sync-1] [INFO] [1727969194.306159587] [camera_1.rgbd_sync_1]: rgbd_sync_1: qos             = 1
[rgbd_sync-1] [INFO] [1727969194.306163956] [camera_1.rgbd_sync_1]: rgbd_sync_1: qos_camera_info = 1
[rgbd_sync-1] [INFO] [1727969194.306169687] [camera_1.rgbd_sync_1]: rgbd_sync_1: depth_scale = 1.000000
[rgbd_sync-1] [INFO] [1727969194.306182662] [camera_1.rgbd_sync_1]: rgbd_sync_1: decimation = 1
[rgbd_sync-1] [INFO] [1727969194.306187067] [camera_1.rgbd_sync_1]: rgbd_sync_1: compressed_rate = 0.000000
[rgbd_sync-2] [INFO] [1727969194.317703674] [camera_2.rgbd_sync_2]: 
[rgbd_sync-2] rgbd_sync_2 subscribed to (exact sync):
[rgbd_sync-2]    /camera_image_2,
[rgbd_sync-2]    /stereo_depths_2,
[rgbd_sync-2]    /camera_info_2
[rgbd_sync-1] [INFO] [1727969194.319000134] [camera_1.rgbd_sync_1]: 
[rgbd_sync-1] rgbd_sync_1 subscribed to (exact sync):
[rgbd_sync-1]    /camera_image_1,
[rgbd_sync-1]    /stereo_depths_1,
[rgbd_sync-1]    /camera_info_1
[rgbd_odometry-3] [INFO] [1727969194.320283351] [rtabmap.rgbd_odometry]: Odometry: frame_id               = right_stereo_camera_1
[rgbd_odometry-3] [INFO] [1727969194.320394350] [rtabmap.rgbd_odometry]: Odometry: odom_frame_id          = odom
[rgbd_odometry-3] [INFO] [1727969194.320402207] [rtabmap.rgbd_odometry]: Odometry: publish_tf             = true
[rgbd_odometry-3] [INFO] [1727969194.320406643] [rtabmap.rgbd_odometry]: Odometry: wait_for_transform     = 0.100000
[rgbd_odometry-3] [INFO] [1727969194.320419369] [rtabmap.rgbd_odometry]: Odometry: log_to_rosout_level    = 4
[rgbd_odometry-3] [INFO] [1727969194.320436979] [rtabmap.rgbd_odometry]: Odometry: initial_pose           = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
[rgbd_odometry-3] [INFO] [1727969194.320441897] [rtabmap.rgbd_odometry]: Odometry: ground_truth_frame_id  = 
[rgbd_odometry-3] [INFO] [1727969194.320446220] [rtabmap.rgbd_odometry]: Odometry: ground_truth_base_frame_id = right_stereo_camera_1
[rgbd_odometry-3] [INFO] [1727969194.320450600] [rtabmap.rgbd_odometry]: Odometry: config_path            = 
[rgbd_odometry-3] [INFO] [1727969194.320454719] [rtabmap.rgbd_odometry]: Odometry: publish_null_when_lost = true
[rgbd_odometry-3] [INFO] [1727969194.320458911] [rtabmap.rgbd_odometry]: Odometry: publish_compressed_sensor_data = false
[rgbd_odometry-3] [INFO] [1727969194.320464502] [rtabmap.rgbd_odometry]: Odometry: guess_frame_id         = 
[rgbd_odometry-3] [INFO] [1727969194.320468676] [rtabmap.rgbd_odometry]: Odometry: guess_min_translation  = 0.000000
[rgbd_odometry-3] [INFO] [1727969194.320473323] [rtabmap.rgbd_odometry]: Odometry: guess_min_rotation     = 0.000000
[rgbd_odometry-3] [INFO] [1727969194.320478593] [rtabmap.rgbd_odometry]: Odometry: guess_min_time         = 0.000000
[rgbd_odometry-3] [INFO] [1727969194.320483277] [rtabmap.rgbd_odometry]: Odometry: expected_update_rate   = 0.000000 Hz
[rgbd_odometry-3] [INFO] [1727969194.320487951] [rtabmap.rgbd_odometry]: Odometry: max_update_rate        = 0.000000 Hz
[rgbd_odometry-3] [INFO] [1727969194.320492521] [rtabmap.rgbd_odometry]: Odometry: min_update_rate        = 0.000000 Hz
[rgbd_odometry-3] [INFO] [1727969194.320497087] [rtabmap.rgbd_odometry]: Odometry: wait_imu_to_init       = false
[rgbd_odometry-3] [INFO] [1727969194.320501377] [rtabmap.rgbd_odometry]: Odometry: sensor_data_compression_format = .jpg
[rgbd_odometry-3] [INFO] [1727969194.320505625] [rtabmap.rgbd_odometry]: Odometry: sensor_data_parallel_compression = true
[rgbd_odometry-3] [INFO] [1727969194.320512566] [rtabmap.rgbd_odometry]: Odometry: stereoParams_=0 visParams_=1 icpParams_=0
[rgbd_odometry-3] [INFO] [1727969194.340824355] [rtabmap.rgbd_odometry]: RGBDOdometry: approx_sync    = false
[rgbd_odometry-3] [INFO] [1727969194.340859913] [rtabmap.rgbd_odometry]: RGBDOdometry: topic_queue_size = 1
[rgbd_odometry-3] [INFO] [1727969194.340865935] [rtabmap.rgbd_odometry]: RGBDOdometry: sync_queue_size  = 5
[rgbd_odometry-3] [INFO] [1727969194.340870388] [rtabmap.rgbd_odometry]: RGBDOdometry: qos             = 1
[rgbd_odometry-3] [INFO] [1727969194.340874739] [rtabmap.rgbd_odometry]: RGBDOdometry: qos_camera_info = 1
[rgbd_odometry-3] [INFO] [1727969194.340879264] [rtabmap.rgbd_odometry]: RGBDOdometry: subscribe_rgbd = true
**[rgbd_odometry-3] [INFO] [1727969194.340883754] [rtabmap.rgbd_odometry]: RGBDOdometry: rgbd_cameras   = 1**
[rgbd_odometry-3] [INFO] [1727969194.340888175] [rtabmap.rgbd_odometry]: RGBDOdometry: keep_color     = false
[rgbd_odometry-3] [INFO] [1727969194.341246863] [rtabmap.rgbd_odometry]: 
[rgbd_odometry-3] rgbd_odometry subscribed to:
**[rgbd_odometry-3]    /rtabmap/rgbd_image**
[rtabmap_viz-4] [INFO] [1727969194.486432639] [rtabmap.rtabmap_viz]: rtabmap_viz: Using configuration from "/home/shivam/.ros/rtabmapGUI.ini"
[rtabmap_viz-4] libpng warning: iCCP: known incorrect sRGB profile
[rtabmap_viz-4] libpng warning: iCCP: known incorrect sRGB profile
[rtabmap_viz-4] libpng warning: iCCP: known incorrect sRGB profile
[rtabmap_viz-4] [WARN] [1727969195.315912192] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[rtabmap_viz-4] [INFO] [1727969195.317528380] [rtabmap.rtabmap_viz]: Reading parameters from the ROS server...
[rgbd_odometry-3] [WARN] [1727969198.343709336] [rtabmap.rgbd_odometry]: rgbd_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
[rgbd_odometry-3] rgbd_odometry subscribed to:
[rgbd_odometry-3]    /rtabmap/rgbd_image
[rtabmap_viz-4] [ERROR] [1727969200.319588169] [rtabmap.rtabmap_viz]: Can't call rtabmap parameters service, is the node running?
[rtabmap_viz-4] [INFO] [1727969200.319688738] [rtabmap.rtabmap_viz]: Parameters read = 0
[rtabmap_viz-4] [INFO] [1727969200.471488170] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_depth = false
[rtabmap_viz-4] [INFO] [1727969200.471505669] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_rgb = false
[rtabmap_viz-4] [INFO] [1727969200.471507774] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_stereo = false
**[rtabmap_viz-4] [INFO] [1727969200.471509487] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_rgbd = true (rgbd_cameras=0)**
[rtabmap_viz-4] [INFO] [1727969200.471512047] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_sensor_data = false
[rtabmap_viz-4] [INFO] [1727969200.471516822] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_odom_info = false
[rtabmap_viz-4] [INFO] [1727969200.471519814] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_user_data = false
[rtabmap_viz-4] [INFO] [1727969200.471521371] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_scan = false
[rtabmap_viz-4] [INFO] [1727969200.471522942] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_scan_cloud = false
[rtabmap_viz-4] [INFO] [1727969200.471524446] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_scan_descriptor = false
[rtabmap_viz-4] [INFO] [1727969200.471526248] [rtabmap.rtabmap_viz]: rtabmap_viz: topic_queue_size = 1
[rtabmap_viz-4] [INFO] [1727969200.471528075] [rtabmap.rtabmap_viz]: rtabmap_viz: sync_queue_size  = 10
[rtabmap_viz-4] [INFO] [1727969200.471529760] [rtabmap.rtabmap_viz]: rtabmap_viz: qos_image       = 1
[rtabmap_viz-4] [INFO] [1727969200.471531459] [rtabmap.rtabmap_viz]: rtabmap_viz: qos_camera_info = 1
[rtabmap_viz-4] [INFO] [1727969200.471533091] [rtabmap.rtabmap_viz]: rtabmap_viz: qos_scan        = 1
[rtabmap_viz-4] [INFO] [1727969200.471534687] [rtabmap.rtabmap_viz]: rtabmap_viz: qos_odom        = 1
[rtabmap_viz-4] [INFO] [1727969200.471536282] [rtabmap.rtabmap_viz]: rtabmap_viz: qos_user_data   = 1
[rtabmap_viz-4] [INFO] [1727969200.471537760] [rtabmap.rtabmap_viz]: rtabmap_viz: approx_sync     = false
[rtabmap_viz-4] [INFO] [1727969200.471542767] [rtabmap.rtabmap_viz]: Setup rgbdX callback
[rtabmap_viz-4] [INFO] [1727969200.472195260] [rtabmap.rtabmap_viz]: 
[rtabmap_viz-4] rtabmap_viz subscribed to (exact sync):
[rtabmap_viz-4]    /rtabmap/odom \
**[rtabmap_viz-4]    /rtabmap/rgbd_images**
[rtabmap_viz-4] [INFO] [1727969200.473272351] [rtabmap.rtabmap_viz]: rtabmap_viz started.
[rtabmap_viz-4] [WARN] [1727969201.464961271] [rtabmap.rtabmap_viz]: Service "rtabmap/resume" not available.
matlabbe commented 2 weeks ago

There was indeed a bug! Looks like that fix got lost in that merge from ros1 branch to ros2 branch. It should be fixed by commit above.

Thanks for reporting!