Did not receive message for 5 seconds, but message are published.
Posted by cjsurjadi on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Did-not-receive-message-for-5-seconds-but-message-are-published-tp9786.html
I'm trying to use Rtabmap with Ouster OS-1 and Realsense D435 with ROS2 Humble. I can run rtabmap with ICP odometry with only the 3D point cloud from Ouster successfully, and I can also run rtabmap with rgbd odometry with only the Realsense successfully. I've attempted combining the lidar with the rgb and depth topic, and syncing them into an rgbd image first then combining it. But in both cases, when I run rtabmap to subscribe to all the topics (3D lidar, rgb and depth image), I receive the error:
My launch file is as the following. I use the same launch file while only changing the odom node and the subscribe_rgb/depth/scan_cloud parameter during testing for only lidar, only rgbd, or both. The standalone testing for lidar and rgbd works.
Rtabmap Log:
Rostopic hz of the subscribed topics:
The rqt graph of my topics are as the following:
The tf frames is as the following:
URL: http://official-rtab-map-forum.206.s1.nabble.com/Did-not-receive-message-for-5-seconds-but-message-are-published-tp9786.html
I'm trying to use Rtabmap with Ouster OS-1 and Realsense D435 with ROS2 Humble. I can run rtabmap with ICP odometry with only the 3D point cloud from Ouster successfully, and I can also run rtabmap with rgbd odometry with only the Realsense successfully. I've attempted combining the lidar with the rgb and depth topic, and syncing them into an rgbd image first then combining it. But in both cases, when I run rtabmap to subscribe to all the topics (3D lidar, rgb and depth image), I receive the error:
[WARN] [1700552704.314934175] [rtabmap]: rtabmap: 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. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10). rtabmap subscribed to (approx sync): /odom \ /camera/color/image_raw \ /camera/aligned_depth_to_color/image_raw \ /camera/color/camera_info \ /ouster/points \ /odom_info
import os 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 def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') qos = LaunchConfiguration('qos') localization = LaunchConfiguration('localization') icp_odom = 'false' vis_odom = 'true' rgbd_sync = 'false' parameters={\ 'frame_id':'base_link', 'use_sim_time':use_sim_time, 'subscribe_depth':True, 'subscribe_rgb':True, 'subscribe_rgbd':False, 'subscribe_scan':False, 'subscribe_scan_cloud':True, 'subscribe_stereo':False, 'subscribe_odom_info':True, 'queue_size':10, 'rgb_image_topic': '/camera/color/image_raw', 'depth_image_topic': '/camera/aligned_depth_to_color/image_raw', 'camera_info_topic': '/camera/color/camera_info', 'left_image_topic':'/camera/infra1/image_rect_raw', 'right_image_topic':'/camera/infra2/image_rect_raw', 'left_camera_info_topic':'/camera/infra1/camera_info', 'right_camera_info_topic':'/camera/infra2/camera_info', 'scan_cloud_topic':'/ouster/points', 'scan_cloud_is_2d':False, 'imu_topic':'/ouster/imu', 'approx_sync':True, 'use_action_for_goal':True, 'qos_scan':qos, 'qos_image':qos, 'qos_imu':qos, 'qod_odom':qos, 'Kp/DetectorStrategy':'2', 'Vis/FeatureType':'2', 'Kp/MaxFeatures':'500', 'Kp/MaxDepth':'4.0', 'Vis/MaxDepth':'4.0', 'Kp/MinDepth':'0.5', 'Vis/MinDepth':'0.5', 'Vis/MinInliers':'15', 'Reg/Strategy':'1', 'Reg/Force3DoF':'True', 'RGBD/NeighborLinkRefining':'True', 'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) } remappings=[ ('rgb/image', '/camera/color/image_raw'), ('depth/image', '/camera/aligned_depth_to_color/image_raw'), ('rgb/camera_info', '/camera/color/camera_info'), ('left/image_rect', '/camera/infra1/image_rect_raw'), ('right/image_rect', '/camera/infra2/image_rect_raw'), ('left/camera_info', '/camera/infra1/camera_info'), ('right/camera_info', '/camera/infra2/camera_info'), ('scan_cloud', '/ouster/points') ] return LaunchDescription([ DeclareLaunchArgument( 'qos', default_value='2', description='QoS used for input sensor topics'), DeclareLaunchArgument( 'localization', default_value='false', description='Launch in localization mode.'), Node( package='imu_filter_madgwick', executable='imu_filter_madgwick_node', name='imu_filter', output='screen', parameters=[{ 'use_mag':False, 'world_frame':'enu', 'publish_tf':False, 'fixed_frame':'os_imu' }], remappings=[ ('imu/data_raw', 'ouster/imu'), ('imu/data', 'ouster/imu_filtered') ]), Node( condition=IfCondition(icp_odom), package='rtabmap_odom', executable='icp_odometry', output='screen', parameters=[{ 'frame_id':'base_link', 'odom_frame_id':'odom', 'wait_for_transform':0.2, 'wait_imu_to_init':False, 'expected_update_rate':15.0, 'subscribe_scan_cloud':True, 'subscribe_scan':False, 'subscribe_rgbd':False, 'publish_tf':True, 'scan_cloud_max_points':32000, 'scan_range_max':20.0, 'scan_range_min':0.2, 'deskewing':False, 'use_sim_time':use_sim_time, 'approx_sync':True, 'queue_size':10 }], remappings=remappings, arguments=[ 'Icp/PointToPlane', 'true', 'Icp/Iterations', '10', 'Icp/VoxelSize', '0.1', 'Icp/Epsilon', '0.001', 'Icp/PointToPlaneK', '20', 'Icp/PointToPlaneRadius', '0', 'Icp/MaxTranslation', '2', 'Icp/MaxCorrespondenceDistance', '1', 'Icp/PM', 'true', 'Icp/PMOutlierRatio', '0.1', 'Icp/CorrespondenceRatio', '0.01', 'Icp/ReciprocalCorrespondences', 'false', 'Odom/ScanKeyFrameThr', '0.8', 'OdomF2M/ScanSubtractRadius', '0.05', 'OdomF2M/ScanMaxSize', '10000', 'OdomF2M/BundleAdjustment', 'false', ]), Node( condition=IfCondition(vis_odom), package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=[{ 'frame_id':'base_link', 'subscribe_depth': True, 'subscribe_odom_info':True, 'approx_sync':False}], remappings=remappings), # SLAM mode: Node( condition=UnlessCondition(localization), package='rtabmap_slam', executable='rtabmap', output='screen', parameters=[parameters], remappings=remappings, arguments=['-d', 'Rtabmap/DetectionRate','1.0', 'Kp/DetectorStrategy', '2', 'Vis/FeatureType','2', 'Kp/MaxFeatures','500', 'Kp/MaxDepth','4.0', 'Vis/MaxDepth','4.0', 'Kp/MinDepth','0.5', 'Vis/MinDepth','0.5', 'Vis/MinInliers','15', 'Reg/Strategy','1', 'Reg/Force3DoF','True', 'RGBD/NeighborLinkRefining','True', 'Grid/RangeMin','0.2', # ignore laser scan points on the robot itself 'Optimizer/GravitySigma','0']), # Disable, # This will delete the previous database (~/.ros/rtabmap.db) # Localization mode: Node( condition=IfCondition(localization), package='rtabmap_slam', executable='rtabmap', output='screen', parameters=[parameters, {'Mem/IncrementalMemory':'False', 'Mem/InitWMWithAllNodes':'True'}], remappings=remappings), Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', parameters=[parameters], remappings=remappings) ])
[INFO] [1700552696.245563741] [rtabmap]: rtabmap(maps): map_filter_radius = 0.000000 [INFO] [1700552696.245777878] [rtabmap]: rtabmap(maps): map_filter_angle = 30.000000 [INFO] [1700552696.245796737] [rtabmap]: rtabmap(maps): map_cleanup = true [INFO] [1700552696.245810056] [rtabmap]: rtabmap(maps): map_always_update = false [INFO] [1700552696.245822661] [rtabmap]: rtabmap(maps): map_empty_ray_tracing = true [INFO] [1700552696.245834977] [rtabmap]: rtabmap(maps): cloud_output_voxelized = true [INFO] [1700552696.245847065] [rtabmap]: rtabmap(maps): cloud_subtract_filtering = false [INFO] [1700552696.245859271] [rtabmap]: rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2 [INFO] [1700552696.245931577] [rtabmap]: rtabmap(maps): octomap_tree_depth = 16 [INFO] [1700552696.273266132] [rtabmap]: rtabmap: frame_id = base_link [INFO] [1700552696.273295856] [rtabmap]: rtabmap: map_frame_id = map [INFO] [1700552696.273308479] [rtabmap]: rtabmap: log_to_rosout_level = 4 [INFO] [1700552696.273319153] [rtabmap]: rtabmap: initial_pose = [INFO] [1700552696.273329045] [rtabmap]: rtabmap: use_action_for_goal = true [INFO] [1700552696.273342196] [rtabmap]: rtabmap: tf_delay = 0.050000 [INFO] [1700552696.273356913] [rtabmap]: rtabmap: tf_tolerance = 0.100000 [INFO] [1700552696.273368042] [rtabmap]: rtabmap: odom_sensor_sync = false [INFO] [1700552696.273377558] [rtabmap]: rtabmap: gen_scan = false [INFO] [1700552696.273386721] [rtabmap]: rtabmap: gen_depth = false [INFO] [1700552696.273396483] [rtabmap]: rtabmap: scan_cloud_max_points = 0 [INFO] [1700552696.273406297] [rtabmap]: rtabmap: scan_cloud_is_2d = false [INFO] [1700552696.315768947] [rtabmap]: Setting RTAB-Map parameter "Grid/RangeMin"="0.2" [INFO] [1700552696.318516713] [rtabmap]: Setting RTAB-Map parameter "Kp/DetectorStrategy"="2" [INFO] [1700552696.318861014] [rtabmap]: Setting RTAB-Map parameter "Kp/MaxDepth"="4.0" [INFO] [1700552696.318996413] [rtabmap]: Setting RTAB-Map parameter "Kp/MinDepth"="0.5" [INFO] [1700552696.322371550] [rtabmap]: Setting RTAB-Map parameter "Optimizer/GravitySigma"="0" [INFO] [1700552696.325822290] [rtabmap]: Setting RTAB-Map parameter "RGBD/NeighborLinkRefining"="True" [INFO] [1700552696.328363786] [rtabmap]: Setting RTAB-Map parameter "Reg/Force3DoF"="True" [INFO] [1700552696.328526880] [rtabmap]: Setting RTAB-Map parameter "Reg/Strategy"="1" [INFO] [1700552696.335974605] [rtabmap]: Setting RTAB-Map parameter "Vis/FeatureType"="2" [INFO] [1700552696.336374452] [rtabmap]: Setting RTAB-Map parameter "Vis/MaxDepth"="4.0" [INFO] [1700552696.336586998] [rtabmap]: Setting RTAB-Map parameter "Vis/MinDepth"="0.5" [INFO] [1700552696.336683655] [rtabmap]: Setting RTAB-Map parameter "Vis/MinInliers"="15" [INFO] [1700552696.337962729] [rtabmap]: Update RTAB-Map parameter "Grid/RangeMin"="0.2" from arguments [INFO] [1700552696.337996901] [rtabmap]: Update RTAB-Map parameter "Kp/DetectorStrategy"="2" from arguments [INFO] [1700552696.338010756] [rtabmap]: Update RTAB-Map parameter "Kp/MaxDepth"="4.0" from arguments [INFO] [1700552696.338022065] [rtabmap]: Update RTAB-Map parameter "Kp/MaxFeatures"="500" from arguments [INFO] [1700552696.338034916] [rtabmap]: Update RTAB-Map parameter "Kp/MinDepth"="0.5" from arguments [INFO] [1700552696.338045646] [rtabmap]: Update RTAB-Map parameter "Optimizer/GravitySigma"="0" from arguments [INFO] [1700552696.338056571] [rtabmap]: Update RTAB-Map parameter "RGBD/NeighborLinkRefining"="True" from arguments [INFO] [1700552696.338068339] [rtabmap]: Update RTAB-Map parameter "Reg/Force3DoF"="True" from arguments [INFO] [1700552696.338078870] [rtabmap]: Update RTAB-Map parameter "Reg/Strategy"="1" from arguments [INFO] [1700552696.338089133] [rtabmap]: Update RTAB-Map parameter "Rtabmap/DetectionRate"="1.0" from arguments [INFO] [1700552696.338099629] [rtabmap]: Update RTAB-Map parameter "Vis/FeatureType"="2" from arguments [INFO] [1700552696.338110085] [rtabmap]: Update RTAB-Map parameter "Vis/MaxDepth"="4.0" from arguments [INFO] [1700552696.338119946] [rtabmap]: Update RTAB-Map parameter "Vis/MinDepth"="0.5" from arguments [INFO] [1700552696.338201596] [rtabmap]: Update RTAB-Map parameter "Vis/MinInliers"="15" from arguments [WARN] [1700552696.338283944] [rtabmap]: Setting "Grid/Sensor" parameter to 0 (default 1) as "subscribe_scan" or "subscribe_scan_cloud" or "gen_scan" is true. The occupancy grid map will be constructed from laser scans. To get occupancy grid map from cloud projection, set "Grid/Sensor" to true. To suppress this warning, add <param name="Grid/Sensor" type="string" value="0"/> [INFO] [1700552696.338315467] [rtabmap]: Setting "Grid/RangeMax" parameter to 0 (default 5.000000) as "subscribe_scan" or "subscribe_scan_cloud" or "gen_scan" is true and Grid/Sensor is 0. [INFO] [1700552696.338333478] [rtabmap]: Setting "Icp/PointToPlaneRadius" parameter to 0 (default 0.000000) as "subscribe_scan_cloud" is true. [WARN] [1700552696.338347429] [rtabmap]: Setting "RGBD/ProximityPathMaxNeighbors" parameter to 1 (default 0) as "subscribe_scan_cloud" is true and "Reg/Strategy" uses ICP. To disable, set "RGBD/ProximityPathMaxNeighbors" to 0. To suppress this warning, add <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/> [INFO] [1700552696.338565934] [rtabmap]: RTAB-Map detection rate = 1.000000 Hz [INFO] [1700552696.338831080] [rtabmap]: rtabmap: Deleted database "/root/.ros/rtabmap.db" (--delete_db_on_start or -d are set). [INFO] [1700552696.338873441] [rtabmap]: rtabmap: Using database from "/root/.ros/rtabmap.db" (0 MB). [INFO] [1700552699.280183795] [rtabmap]: rtabmap: Database version = "0.21.1". [INFO] [1700552699.280257417] [rtabmap]: rtabmap: SLAM mode (Mem/IncrementalMemory=true) [INFO] [1700552699.305008531] [rtabmap]: Setup callbacks [INFO] [1700552699.305191169] [rtabmap]: rtabmap: subscribe_depth = true [INFO] [1700552699.305211576] [rtabmap]: rtabmap: subscribe_rgb = true [INFO] [1700552699.305223125] [rtabmap]: rtabmap: subscribe_stereo = false [INFO] [1700552699.305234229] [rtabmap]: rtabmap: subscribe_rgbd = false (rgbd_cameras=1) [INFO] [1700552699.305246347] [rtabmap]: rtabmap: subscribe_odom_info = true [INFO] [1700552699.305256493] [rtabmap]: rtabmap: subscribe_user_data = false [INFO] [1700552699.305266540] [rtabmap]: rtabmap: subscribe_scan = false [INFO] [1700552699.305276629] [rtabmap]: rtabmap: subscribe_scan_cloud = true [INFO] [1700552699.305286790] [rtabmap]: rtabmap: subscribe_scan_descriptor = false [INFO] [1700552699.305297303] [rtabmap]: rtabmap: queue_size = 10 [INFO] [1700552699.305307958] [rtabmap]: rtabmap: qos_image = 2 [INFO] [1700552699.305318391] [rtabmap]: rtabmap: qos_camera_info = 2 [INFO] [1700552699.305328644] [rtabmap]: rtabmap: qos_scan = 2 [INFO] [1700552699.305338685] [rtabmap]: rtabmap: qos_odom = 0 [INFO] [1700552699.305349331] [rtabmap]: rtabmap: qos_user_data = 0 [INFO] [1700552699.305359722] [rtabmap]: rtabmap: approx_sync = true [INFO] [1700552699.305379741] [rtabmap]: Setup depth callback [INFO] [1700552699.314794013] [rtabmap]: rtabmap subscribed to (approx sync): /odom \ /camera/color/image_raw \ /camera/aligned_depth_to_color/image_raw \ /camera/color/camera_info \ /ouster/points \ /odom_info [WARN] [1700552704.314934175] [rtabmap]: rtabmap: 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. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10). rtabmap subscribed to (approx sync): /odom \ /camera/color/image_raw \ /camera/aligned_depth_to_color/image_raw \ /camera/color/camera_info \ /ouster/points \ /odom_info
ros2 topic hz odom_info average rate: 27.218 min: 0.031s max: 0.044s std dev: 0.00328s window: 29 average rate: 27.200 min: 0.028s max: 0.044s std dev: 0.00362s window: 57 ros2 topic hz odom average rate: 28.528 min: 0.031s max: 0.038s std dev: 0.00179s window: 30 average rate: 28.443 min: 0.031s max: 0.041s std dev: 0.00192s window: 59 average rate: 28.007 min: 0.031s max: 0.066s std dev: 0.00388s window: 87 ros2 topic hz /ouster/points average rate: 6.671 min: 0.099s max: 0.501s std dev: 0.13277s window: 8 average rate: 8.181 min: 0.099s max: 0.501s std dev: 0.09191s window: 18 average rate: 8.748 min: 0.099s max: 0.501s std dev: 0.07446s window: 28 ros2 topic hz /camera/color/image_raw average rate: 28.994 min: 0.029s max: 0.066s std dev: 0.00604s window: 30 average rate: 29.483 min: 0.017s max: 0.066s std dev: 0.00529s window: 60 average rate: 29.665 min: 0.017s max: 0.066s std dev: 0.00433s window: 91 ros2 topic hz /camera/aligned_depth_to_color/image_raw average rate: 29.955 min: 0.032s max: 0.035s std dev: 0.00062s window: 31 average rate: 29.975 min: 0.032s max: 0.035s std dev: 0.00073s window: 62 average rate: 29.976 min: 0.032s max: 0.035s std dev: 0.00071s window: 92 ros2 topic hz /camera/color/camera_info WARNING: topic [/camera/color/camera_info] does not appear to be published yet average rate: 30.023 min: 0.032s max: 0.035s std dev: 0.00057s window: 31 average rate: 29.999 min: 0.027s max: 0.040s std dev: 0.00160s window: 61 average rate: 29.999 min: 0.027s max: 0.040s std dev: 0.00137s window: 91
The tf frames is as the following:
| Free forum by Nabble | Edit this page |