This post was updated on .
Hi I am trying to run RTabMap with Livox Mid360 and Realsense d435i. While mapping mode worked very smoothly (localization = false), when I turned on the localization mode, after about 1 minutes, I will start seeing the warning message saying that could not get the transform from odom to base_link.
Below is a brief summary of my setup: (1) I am using FastLIO for Livox Mid360 for generating the odometry for it (livox_init -> livox_body through LaserMap). (2) I set up 2 static transforms, the first one is from the /odom to /livox_init, and the second one is from /livox_body to /base_link (3) I turned on "subscribe_scan_cloud" (set to true) with `<remap from="scan_cloud" to="/livox/points3D"/>`, where /livox/points3D is a simple message converter from the Livox CustomMsg to PointCloud2. (4) I don't turn on ICP odometry nor Visual odometry at this moment. (5) The full launch file can be found below. (6) My TF Tree is like /map -> /odom -> /livox_init -> /livox_body ->/base_link (7) Using ROS Noetic > [ WARN] [1685162235.126441643]: Could > not get transform from odom to > base_link after 0.200000 seconds (for > stamp=1685162223.741804)! > Error="Lookup would require > extrapolation 1.495885847s into the > past. Requested time > 1685162223.741804361 but the earliest data is at time 1685162225.237690210, > when looking up transform from frame > [base_link] to frame [odom]. > canTransform returned after 0.200674 > timeout was 0.2.". [ WARN] > [1685162235.328926136]: Could not get > transform from odom to base_link after > 0.200000 seconds (for stamp=1685162223.842756)! > Error="Lookup would require > extrapolation 1.544934257s into the > past. Requested time > 1685162223.842755556 but the earliest data is at time 1685162225.387689829, > when looking up transform from frame > [base_link] to frame [odom]. > canTransform returned after 0.200683 > timeout was 0.2.". [ WARN] > [1685162235.532499467]: Could not get > transform from odom to base_link after > 0.200000 seconds (for stamp=1685162223.941825)! > Error="Lookup would require > extrapolation 1.645868992s into the > past. Requested time > 1685162223.941824913 but the earliest data is at time 1685162225.587693930, > when looking up transform from frame > [base_link] to frame [odom]. > canTransform returned after 0.200672 > timeout was 0.2.". <launch> <arg name="depth_topic" value="/camera/aligned_depth_to_color/image_raw"/> <arg name="frame_id" value="base_link"/> <!-- Note: Need to double check, map is just a guess --> <arg name="visual_odometry" value="false"/> <arg name="odom_topic" value="/Odometry"/> <arg name="rgb_topic" value="/camera/color/image_raw"/> <arg name="camera_info_topic" value="/camera/color/camera_info"/> <arg name="queue_size" value="200"/> <arg name="odom_frame_id" value="odom"/> <arg name="map_frame_id" value="map"/> <arg name="subscribe_scan_cloud" value="true"/> <!-- Localization-only mode --> <arg name="localization" default="false"/> <!-- Corresponding config files --> <arg name="cfg" default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app --> <arg name="ground_truth_frame_id" default=""/> <!-- e.g., "world" --> <arg name="ground_truth_base_frame_id" default=""/> <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) --> <arg name="namespace" default="rtabmap"/> <arg name="database_path" default="~/.ros/rtabmap.db"/> <arg name="queue_size" default="10"/> <arg name="wait_for_transform" default="0.2"/> <arg name="args" default=""/> <!-- delete_db_on_start, udebug --> <arg name="rtabmap_args" default="$(arg args)"/> <!-- deprecated, use "args" argument --> <arg name="gdb" default="false"/> <!-- Launch nodes in gdb for debugging (apt install xterm gdb) --> <arg if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/> <arg unless="$(arg gdb)" name="launch_prefix" default=""/> <arg name="output" default="screen"/> <!-- Control node output (screen or log) --> <arg name="publish_tf_map" default="true"/> <!-- if timestamps of the input topics are synchronized using approximate or exact time policy--> <arg if="$(arg stereo)" name="approx_sync" default="false"/> <arg unless="$(arg stereo)" name="approx_sync" default="$(arg depth)"/> <!-- RGB-D related topics --> <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" /> <!-- stereo related topics --> <arg name="stereo_namespace" default="/stereo_camera"/> <arg name="left_image_topic" default="$(arg stereo_namespace)/left/image_rect_color" /> <arg name="right_image_topic" default="$(arg stereo_namespace)/right/image_rect" /> <!-- using grayscale image for efficiency --> <arg name="left_camera_info_topic" default="$(arg stereo_namespace)/left/camera_info" /> <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" /> <!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet --> <arg name="rgbd_sync" default="false"/> <!-- pre-sync rgb_topic, depth_topic, camera_info_topic --> <arg name="approx_rgbd_sync" default="true"/> <!-- false=exact synchronization --> <arg name="subscribe_rgbd" default="$(arg rgbd_sync)"/> <arg name="rgbd_topic" default="rgbd_image" /> <arg name="depth_scale" default="1.0" /> <!-- Deprecated, use rgbd_depth_scale instead --> <arg name="rgbd_depth_scale" default="$(arg depth_scale)" /> <arg name="rgbd_decimation" default="1" /> <arg name="compressed" default="false"/> <!-- If you want to subscribe to compressed image topics --> <arg name="rgb_image_transport" default="compressed"/> <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") --> <arg name="depth_image_transport" default="compressedDepth"/> <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") --> <arg name="subscribe_scan" default="false"/> <arg name="scan_topic" default="/scan"/> <arg name="scan_cloud_topic" default="/scan_cloud"/> <arg name="subscribe_scan_descriptor" default="false"/> <arg name="scan_descriptor_topic" default="/scan_descriptor"/> <arg name="scan_cloud_max_points" default="0"/> <arg name="scan_cloud_filtered" default="false"/> <!-- use filtered cloud from icp_odometry for mapping --> <arg name="gen_scan" default="false"/> <!-- only works with depth image and if not subscribing to scan topic--> <arg name="icp_odometry" default="false"/> <!-- Launch rtabmap icp odometry node --> <arg name="odom_topic" default="odom"/> <!-- Odometry topic name --> <arg name="vo_frame_id" default="$(arg odom_topic)"/> <!-- Visual/Icp odometry frame ID for TF --> <arg name="publish_tf_odom" default="true"/> <arg name="odom_tf_angular_variance" default="1"/> <!-- If TF is used to get odometry, this is the default angular variance --> <arg name="odom_tf_linear_variance" default="1"/> <!-- If TF is used to get odometry, this is the default linear variance --> <arg name="odom_args" default=""/> <!-- More arguments for odometry (overwrite same parameters in rtabmap_args) --> <arg name="odom_sensor_sync" default="false"/> <arg name="odom_guess_frame_id" default=""/> <arg name="odom_guess_min_translation" default="0"/> <arg name="odom_guess_min_rotation" default="0"/> <arg name="odom_max_rate" default="0"/> <arg name="odom_expected_rate" default="0"/> <arg name="imu_topic" default="/rtabmap/imu"/> <!-- only used with VIO approaches --> <arg name="wait_imu_to_init" default="true"/> <arg name="use_odom_features" default="false"/> <arg name="scan_cloud_assembling" default="false"/> <arg name="scan_cloud_assembling_time" default="1"/> <arg name="scan_cloud_assembling_fixed_frame" default=""/> <arg name="scan_cloud_assembling_voxel_size" default="0.05"/> <arg name="scan_cloud_assembling_noise_radius" default="0.0"/> <!-- 0=disabled --> <arg name="scan_cloud_assembling_noise_min_neighbors" default="5"/> <arg name="subscribe_user_data" default="false"/> <!-- user data synchronized subscription --> <arg name="user_data_topic" default="/user_data"/> <arg name="user_data_async_topic" default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) --> <arg name="gps_topic" default="/gps/fix" /> <!-- gps async subscription --> <arg name="tag_topic" default="/tag_detections" /> <!-- apriltags async subscription --> <arg name="tag_linear_variance" default="0.0001" /> <arg name="tag_angular_variance" default="9999" /> <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable --> <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above --> <arg if="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)_relay"/> <arg unless="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)"/> <arg if="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)_relay"/> <arg unless="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)"/> <arg if="$(arg compressed)" name="left_image_topic_relay" default="$(arg left_image_topic)_relay"/> <arg unless="$(arg compressed)" name="left_image_topic_relay" default="$(arg left_image_topic)"/> <arg if="$(arg compressed)" name="right_image_topic_relay" default="$(arg right_image_topic)_relay"/> <arg unless="$(arg compressed)" name="right_image_topic_relay" default="$(arg right_image_topic)"/> <arg if="$(arg rgbd_sync)" name="rgbd_topic_relay" default="$(arg rgbd_topic)"/> <arg unless="$(arg rgbd_sync)" name="rgbd_topic_relay" default="$(arg rgbd_topic)_relay"/> <!-- Nodes --> <group ns="$(arg namespace)"> <!-- Visual odometry --> <group unless="$(arg icp_odometry)"> <group if="$(arg visual_odometry)"> <!-- RGB-D Odometry --> <node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)"> <remap from="rgb/image" to="$(arg rgb_topic_relay)"/> <remap from="depth/image" to="$(arg depth_topic_relay)"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> <remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/> <remap from="odom" to="$(arg odom_topic)"/> <remap from="imu" to="$(arg imu_topic)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/> <param name="publish_tf" type="bool" value="$(arg publish_tf_odom)"/> <param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/> <param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/> <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/> <param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/> <param name="config_path" type="string" value="$(arg cfg)"/> <param name="queue_size" type="int" value="$(arg queue_size)"/> <param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/> <param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/> <param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/> <param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/> <param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/> <param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/> <param name="keep_color" type="bool" value="$(arg use_odom_features)"/> </node> <!-- Stereo Odometry --> <node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)"> <remap from="left/image_rect" to="$(arg left_image_topic_relay)"/> <remap from="right/image_rect" to="$(arg right_image_topic_relay)"/> <remap from="left/camera_info" to="$(arg left_camera_info_topic)"/> <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/> <remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/> <remap from="odom" to="$(arg odom_topic)"/> <remap from="imu" to="$(arg imu_topic)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/> <param name="publish_tf" type="bool" value="$(arg publish_tf_odom)"/> <param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/> <param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/> <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/> <param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/> <param name="config_path" type="string" value="$(arg cfg)"/> <param name="queue_size" type="int" value="$(arg queue_size)"/> <param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/> <param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/> <param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/> <param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/> <param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/> <param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/> <param name="keep_color" type="bool" value="$(arg use_odom_features)"/> </node> </group> </group> <!-- ICP Odometry --> <node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)"> <remap from="scan" to="$(arg scan_topic)"/> <remap from="scan_cloud" to="$(arg scan_cloud_topic)"/> <remap from="odom" to="$(arg odom_topic)"/> <remap from="imu" to="$(arg imu_topic)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/> <param name="publish_tf" type="bool" value="$(arg publish_tf_odom)"/> <param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/> <param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/> <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/> <param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/> <param name="config_path" type="string" value="$(arg cfg)"/> <param name="queue_size" type="int" value="$(arg queue_size)"/> <param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/> <param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/> <param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/> <param name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/> <param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/> <param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/> </node> <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen"> <remap if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/> <remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/> <remap from="odom" to="$(arg odom_topic)"/> <param name="assembling_time" type="double" value="$(arg scan_cloud_assembling_time)"/> <param name="fixed_frame_id" type="string" value="$(arg scan_cloud_assembling_fixed_frame)"/> <param name="voxel_size" type="double" value="$(arg scan_cloud_assembling_voxel_size)"/> <param name="noise_radius" type="double" value="$(arg scan_cloud_assembling_noise_radius)"/> <param name="noise_min_neighbors" type="int" value="$(arg scan_cloud_assembling_noise_min_neighbors)"/> </node> <!-- Visual SLAM (robot side) --> <!-- args: "delete_db_on_start" and "udebug" --> <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)"> <param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/> <param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="$(arg depth)"/> <param name="subscribe_rgb" type="bool" value="$(arg depth)"/> <param name="subscribe_rgbd" type="bool" value="$(eval subscribe_rgbd or use_odom_features)"/> <param name="subscribe_stereo" type="bool" value="$(arg stereo)"/> <param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/> <param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/> <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/> <param name="subscribe_user_data" type="bool" value="$(arg subscribe_user_data)"/> <!-- <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/> <param if="$(arg icp_odometry)" name="subscribe_odom_info" type="bool" value="true"/> --> <param name="subscribe_odom_info" type="bool" value="false"/> <param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="map_frame_id" type="string" value="$(arg map_frame_id)"/> <param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/> <param name="publish_tf" type="bool" value="$(arg publish_tf_map)"/> <param name="gen_scan" type="bool" value="$(arg gen_scan)"/> <param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/> <param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/> <param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/> <param name="odom_tf_linear_variance" type="double" value="$(arg odom_tf_linear_variance)"/> <param name="odom_sensor_sync" type="bool" value="$(arg odom_sensor_sync)"/> <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/> <param name="database_path" type="string" value="$(arg database_path)"/> <param name="approx_sync" type="bool" value="$(eval approx_sync and not use_odom_features)"/> <param name="config_path" type="string" value="$(arg cfg)"/> <param name="queue_size" type="int" value="$(arg queue_size)"/> <param name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/> <param name="landmark_linear_variance" type="double" value="$(arg tag_linear_variance)"/> <param name="landmark_angular_variance" type="double" value="$(arg tag_angular_variance)"/> <remap from="rgb/image" to="$(arg rgb_topic_relay)"/> <remap from="depth/image" to="$(arg depth_topic_relay)"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> <remap if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/> <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/> <remap from="left/image_rect" to="$(arg left_image_topic_relay)"/> <remap from="right/image_rect" to="$(arg right_image_topic_relay)"/> <remap from="left/camera_info" to="$(arg left_camera_info_topic)"/> <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/> <remap from="scan" to="$(arg scan_topic)"/> <remap if="$(eval scan_cloud_assembling)" from="scan_cloud" to="assembled_cloud"/> <remap if="$(eval scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="odom_filtered_input_scan"/> <remap if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="$(arg scan_cloud_topic)"/> <remap from="scan_descriptor" to="$(arg scan_descriptor_topic)"/> <remap from="user_data" to="$(arg user_data_topic)"/> <remap from="user_data_async" to="$(arg user_data_async_topic)"/> <remap from="gps/fix" to="$(arg gps_topic)"/> <remap from="tag_detections" to="$(arg tag_topic)"/> <remap from="odom" to="$(arg odom_topic)"/> <remap from="imu" to="$(arg imu_topic)"/> <remap from="initialpose" to="/initialpose"/> <remap from="scan_cloud" to="/livox/points3D"/> <param name="use_action_for_goal" type="bool" value="true"/> <remap from="move_base" to="/move_base"/> <param name="Rtabmap/DetectionRate" type="string" value="5"/> <remap from="grid_map" to="/map"/> <param name="Grid/NoiseFilteringMinNeighbors" type="string" value="10"/> <param name="Grid/NoiseFilteringRadius" type="string" value="0.1"/> <param name="Grid/RangeMax" type="string" value="3.0"/> <param name="Grid/MaxObstacleHeight" type="string" value="1.2"/> <param name="Mem/UseOdomGravity" type="bool" value="false"/> <!-- localization mode --> <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/> <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/> <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/> </node> </group> </launch> |
Administrator
|
It looks like the timstamp of the realsense and the Livox are drifting apart. Is Livox publish data using host computer stamp? (like a PTP ethernet time sync mode) For the realsense, make sure yo use host timestamp mode, so that lidar and camera are sync on same clock. |
Free forum by Nabble | Edit this page |