rtabmap_ros
rtabmap_ros copied to clipboard
introlab
Could not estimate motion of velodyne accordingly to fixed frame velodyne_stabilized
Could not estimate motion of velodyne accordingly to fixed frame velodyne_stabilized between stamps 1703147510.130089 and 1703147510.180440! (Lookup would require extrapolation 0.022364622s into the future. Requested time 1703147510.130089283 but the latest data is at time 1703147510.107724667, when looking up transform from frame [velodyne_stabilized] to frame [velodyne]. canTransform returned after 0.0100917 timeout was 0.01.
Hi! So I am following this code here:
https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_examples/launch/test_velodyne_d435i_deskewing.launch
instead of D camera, I use zed mini since I don't have a D cam, since the zed depth wasn't giving me what I wanted.
Here is my modification:
<arg name="rtabmap_viz" default="true"/>
<arg name="scan_20_hz" default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
<arg name="deskewing" default="true"/>
<arg name="slerp" default="false"/> <!-- If true, a slerp between the first and last time will be used to deskew each point, which is faster than using tf for every point but less accurate -->
<arg name="scan_topic" default="/velodyne_points"/>
<arg name="use_sim_time" default="false"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<arg name="imu_topic" default="/zed/zed_nodelet/imu/data"/>
<arg name="frame_id" default="velodyne"/> <!-- base frame of the robot: for this example, we use velodyne as base frame -->
<arg name="queue_size" default="10"/>
<arg name="queue_size_odom" default="1"/>
<arg name="loop_ratio" default="0.2"/>
<arg name="resolution" default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor -->
<arg name="iterations" default="10"/>
<!-- Grid parameters -->
<arg name="ground_is_obstacle" default="true"/>
<arg name="grid_max_range" default="20"/>
<!-- For F2M Odometry -->
<arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car) -->
<arg name="local_map_size" default="15000"/>
<arg name="key_frame_thr" default="0.4"/>
<!-- For FLOAM Odometry -->
<arg name="floam" default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
<arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings -->
<!-- Static transform between velodyne and D435i: TODO: Adjust with real position/orientation!!! -->
<node unless="$(arg use_sim_time)" pkg="tf" type="static_transform_publisher" name="velodyne_to_base_link_tf" args="0.105, -0.070, -0.504 0, -0.07, 0 velodyne base_link 100"/>
<!-- <node pkg="tf" type="static_transform_publisher" name="zed_to_velodyne"
args="-0.2 0.07 0.12 0 0.07 0 zed_base_link velodyne 100" /> -->
<!-- Velodyne sensor VLP16 -->
<include unless="$(arg use_sim_time)" file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
<arg if="$(arg scan_20_hz)" name="rpm" value="1200"/>
<arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
<arg name="organize_cloud" value="true"/> <!-- should be organized for deskewing -->
</include>
<include file="$(find zed_nodelet_example)/launch/zed_laserscan_nodelet.launch" />
<!-- IMU orientation estimation and publish tf accordingly to os_sensor frame -->
<node pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
<remap from="imu/data_raw" to="$(arg imu_topic)"/>
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
<param name="use_mag" value="false"/>
<param name="world_frame" value="enu"/>
<param name="publish_tf" value="false"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_util/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_util/lidar_deskewing" output="screen">
<param name="wait_for_transform" value="0.01"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="slerp" value="$(arg slerp)"/>
<remap from="input_cloud" to="$(arg scan_topic)"/>
</node>
I didn't include the rtabmap part since the de-skewing was having problems. It keeps giving me this:
[ERROR] [1703147510.041626813]: Could not estimate motion of velodyne accordingly to fixed frame velodyne_stabilized between stamps 1703147509.978895 and 1703147510.029156! (Lookup would require extrapolation 0.006377038s into the future. Requested time 1703147509.978895187 but the latest data is at time 1703147509.972518206, when looking up transform from frame [velodyne_stabilized] to frame [velodyne]. canTransform returned after 0.0100179 timeout was 0.01.)
[ WARN] [1703147510.041659416]: deskewing failed! returning possible skewed cloud!
[ WARN] [1703148007.863547715]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame velodyne (parent velodyne_stabilized) at time 1703148007.741102 according to authority unknown_publisher
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame velodyne (parent velodyne_stabilized) at time 1703148007.808140 according to authority unknown_publisher
at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame velodyne (parent velodyne_stabilized) at time 1703148007.808140 according to authority unknown_publisher
at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp
I tried changing wait_for_transform to bigger but the problem still persist. Another thing I notice is sometimes it would say waiiting for imu/data_raw, but since my imuTopic/filter was still outputting, I didn't think much out it, it might be nothing.
Here's my common.yaml for zed:
# params/common.yaml
# Common parameters to Stereolabs ZED and ZED mini cameras
---
# Dynamic parameters cannot have a namespace
brightness: 4 # Dynamic
contrast: 4 # Dynamic
hue: 0 # Dynamic
saturation: 4 # Dynamic
sharpness: 4 # Dynamic
gamma: 8 # Dynamic - Requires SDK >=v3.1
auto_exposure_gain: true # Dynamic
gain: 100 # Dynamic - works only if `auto_exposure_gain` is false
exposure: 100 # Dynamic - works only if `auto_exposure_gain` is false
auto_whitebalance: true # Dynamic
whitebalance_temperature: 42 # Dynamic - works only if `auto_whitebalance` is false
depth_confidence: 30 # Dynamic
depth_texture_conf: 100 # Dynamic
pub_frame_rate: 15.0 # Dynamic - frequency of publishing of video and depth data
point_cloud_freq: 10.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value)
general:
camera_name: zed # A name for the camera (can be different from camera model and node name and can be overwritten by the launch file)
zed_id: 0
serial_number: 0
gpu_id: -1
base_frame: 'base_link' # must be equal to the frame_id used in the URDF file
verbose: false # Enable info message by the ZED SDK
svo_compression: 2 # `0`: LOSSLESS, `1`: AVCHD, `2`: HEVC
self_calib: true # enable/disable self calibration at starting
camera_flip: false
video:
img_downsample_factor: 0.5 # Resample factor for images [0.01,1.0] The SDK works with native image sizes, but publishes rescaled image.
depth:
quality: 3 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL
depth_stabilization: 1 # [0-100] - 0: Disabled
openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters
depth_downsample_factor: 0.5 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...)
pos_tracking:
pos_tracking_enabled: false # True to enable positional tracking from start
imu_fusion: false # enable/disable IMU fusion. When set to false, only the optical odometry will be used.
publish_tf: false # publish `odom -> base_link` TF
publish_map_tf: false # publish `map -> odom` TF
map_frame: 'map' # main frame
odometry_frame: 'odom' # odometry frame
area_memory_db_path: 'zed_area_memory.area' # file loaded when the node starts to restore the "known visual features" map.
save_area_memory_db_on_exit: false # save the "known visual features" map when the node is correctly closed to the path indicated by `area_memory_db_path`
area_memory: true # Enable to detect loop closure
floor_alignment: false # Enable to automatically calculate camera/floor offset
initial_base_pose: [0.0,0.0,0.0, 0.0,0.0,0.0] # Initial position of the `base_frame` -> [X, Y, Z, R, P, Y]
init_odom_with_first_valid_pose: true # Enable to initialize the odometry with the first valid pose
path_pub_rate: 2.0 # Camera trajectory publishing frequency
path_max_count: -1 # use '-1' for unlimited path size
two_d_mode: false # Force navigation on a plane. If true the Z value will be fixed to "fixed_z_value", roll and pitch to zero
fixed_z_value: 0.00 # Value to be used for Z coordinate if `two_d_mode` is true
mapping:
mapping_enabled: false # True to enable mapping and fused point cloud publication
resolution: 0.05 # maps resolution in meters [0.01f, 0.2f]
max_mapping_range: -1 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0]
fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud
clicked_point_topic: '/clicked_point' # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection
sensors:
sensors_timestamp_sync: true # Synchronize Sensors messages timestamp with latest received frame
max_pub_rate: 400. # max frequency of publishing of sensors data. MAX: 400. - MIN: grab rate
publish_imu_tf: true # publish `IMU -> <cam_name>_left_camera_frame` TF
object_detection:
od_enabled: false # True to enable Object Detection [not available for ZED]
model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE
confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100]
max_range: 15. # Maximum detection range
object_tracking_enabled: true # Enable/disable the tracking of the detected objects
mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models
mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models
mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models
mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX_X' models
mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX_X' models
mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX_X' models
mc_sport: true # Enable/disable the detection of sport-related objects for 'MULTI_CLASS_BOX_X' models
Here's zedm.yaml:
general:
camera_model: 'zedm'
resolution: 1 # '0': HD2K, '1': HD1080, '3': HD720, '5': VGA, '6': AUTO
grab_frame_rate: 15 # Frequency of frame grabbing for internal SDK operations
depth:
min_depth: 0.2 # Min: 0.1, Max: 3.0 - Default 0.3 - Note: reducing this value wil require more computational power and GPU memory
max_depth: 20.0 # Max: 20.0
Here's my TF tree as well:
Any help is appreciated! Thank you!
I think zed IMU data has orientation already computed, you may remove the madgwick filter and connect directly the imu data to imu_to_tf node.
How much did you try to increase wait_for_transform (0.05s, 0.1s, 1s)? How bad was the extrapolation delay? Very large delay could be casued by poor synchronization between the clocks of zed camera and the velodyne. Is the velodyne clock synchronized with host clock, or with a GPS clock?
so basically don't have the velodyne_stabliizer? like:
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_util/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="$(arg imu_topic)"/>
<param name="fixed_frame_id" value="$(arg frame_id)"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_util/lidar_deskewing" output="screen">
<param name="wait_for_transform" value="0.01"/>
<param name="fixed_frame_id" value="$(arg frame_id)"/>
<param name="slerp" value="$(arg slerp)"/>
<remap from="input_cloud" to="$(arg scan_topic)"/>
</node>
Also I have tried 0.05 and 0.01 and 0.5 but still the same issue. It seems to be like 0.001 sec in the future. I am not using GPS, just the velodyne.
Keep the stabilized frame:
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_util/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="$(arg imu_topic)"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_util/lidar_deskewing" output="screen">
<param name="wait_for_transform" value="0.01"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="slerp" value="$(arg slerp)"/>
<remap from="input_cloud" to="$(arg scan_topic)"/>
</node>
What is the warning using wait_for_transform=1 ? It is strange if the extrapolation in the future is below 1 sec, even after waiting 1 sec to get it.
@matlabbe
yes so I tried the code you put and wait_for_transform=1 but still same error:
If you want, I've posted a rosbag: https://drive.google.com/file/d/15fe9FGBOl5JH-XhxSR7iEO7FQW38d2e7/view?usp=drive_link
The error message is slightly different, it seems that TF velodyne_stabilized -> velodyne is just not published.
@matlabbe I'm not sure why, I just did what you told me: remove the
<node pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
<param name="use_mag" value="false"/>
<param name="world_frame" value="enu"/>
<param name="publish_tf" value="false"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_util/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
part and add the
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_util/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="$(arg imu_topic)"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_util/lidar_deskewing" output="screen">
<param name="wait_for_transform" value="0.01"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="slerp" value="$(arg slerp)"/>
<remap from="input_cloud" to="$(arg scan_topic)"/>
</node>
Is the imu_topic still published? otherwise the timestamps may be very off than the ones used by the velodyne.