introlab / rtabmap_ros

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

3d map using ouster lidar 16 beam and Realsense D435i #952

Open ghost opened 1 year ago

ghost commented 1 year ago

@matlabbe @FrancoisGrondin @doumdi @francoisferland I tried to take a map using both ouster lidar 16 beam and Realseanse D435i using the following launch file, It gives me this warning and a .db file when I tried to view it nothing appear. Is there any launch file for both together to build a map using rtabmap? launch file:

<?xml version="1.0"?>
<launch>

  <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
    <remap from="rgb/image"       to="/camera/color/image_raw"/>
    <remap from="depth/image"     to="/camera/aligned_depth_to_color/image_raw"/>
    <remap from="rgb/camera_info" to="/camera/color/camera_info"/>   
    <remap from="rgbd_image"       to="rgbd_image"/> <!-- output -->

    <!-- Should be true for not synchronized camera topics 
        (e.g., false for kinectv2, zed, realsense, true for xtion, kinect360)-->
    <param name="approx_sync"       value="false"/> 
  </node>

  <!-- Odometry -->
  <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
    <param name="subscribe_rgbd" type="bool"   value="true"/>
    <param name="frame_id"       type="string" value="base_link"/>
    <remap from="rgbd_image" to="rgbd_image"/>
  </node>

  <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
        <param name="frame_id"        type="string" value="base_link"/>
        <param name="subscribe_depth" type="bool"   value="false"/>
        <param name="subscribe_rgbd"  type="bool"   value="true"/>
        <param name="subscribe_scan_cloud"  type="bool"   value="true"/>

        <remap from="odom"       to="odom"/>
        <remap from="rgbd_image" to="rgbd_image"/>
        <remap from="scan_cloud" to="/ouster/points"/>

        <param name="queue_size"  type="int"  value="10"/>
        <param name="approx_sync" type="bool" value="true"/>

        <!-- RTAB-Map's parameters -->
        <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
        <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
        <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
  </node>

</launch>
#############################
  warning:
[ WARN] [1681939077.650004804]: Could not get transform from base_link to camera_color_optical_frame after 0.100000 seconds (for stamp=1681939077.334201)! Error="canTransform: target_frame base_link does not exist.. canTransform returned after 0.100378 timeout was 0.1.".

[ WARN] [1681937643.038526058]: /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 \
   /rgbd_image \
   /ouster/points
########
roslaunch rtabmap_ros test_ouster_gen2_d435i.launch 
... logging to /home/sameh/.ros/log/07d15e80-def7-11ed-a46e-c0389641594b/roslaunch-sameh-29806.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://sameh:33445/

SUMMARY
========

PARAMETERS
 * /rgbd_odometry/frame_id: base_link
 * /rgbd_odometry/subscribe_rgbd: True
 * /rgbd_sync/approx_sync: False
 * /rosdistro: melodic
 * /rosversion: 1.14.13
 * /rtabmap/RGBD/AngularUpdate: 0.01
 * /rtabmap/RGBD/LinearUpdate: 0.01
 * /rtabmap/RGBD/OptimizeFromGraphEnd: false
 * /rtabmap/approx_sync: True
 * /rtabmap/frame_id: base_link
 * /rtabmap/queue_size: 10
 * /rtabmap/subscribe_depth: False
 * /rtabmap/subscribe_rgbd: True
 * /rtabmap/subscribe_scan_cloud: True

NODES
  /
    rgbd_odometry (rtabmap_ros/rgbd_odometry)
    rgbd_sync (nodelet/nodelet)
    rtabmap (rtabmap_ros/rtabmap)

ROS_MASTER_URI=http://localhost:11311

process[rgbd_sync-1]: started with pid [29825]
type is rtabmap_ros/rgbd_sync
process[rgbd_odometry-2]: started with pid [29830]
process[rtabmap-3]: started with pid [29832]
[ INFO] [1681939074.811128305]: Initializing nodelet with 4 worker threads.
[ INFO] [1681939074.832947108]: Starting node...
[ INFO] [1681939075.043080171]: Initializing nodelet with 4 worker threads.
[ INFO] [1681939075.142370672]: Odometry: frame_id               = base_link
[ INFO] [1681939075.142464371]: Odometry: odom_frame_id          = odom
[ INFO] [1681939075.142519953]: Odometry: publish_tf             = true
[ INFO] [1681939075.142552845]: Odometry: wait_for_transform     = true
[ INFO] [1681939075.142592802]: Odometry: wait_for_transform_duration  = 0.100000
[ INFO] [1681939075.142618970]: Odometry: log_to_rosout_level    = 4
[ INFO] [1681939075.142687271]: Odometry: initial_pose           = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
[ INFO] [1681939075.142721466]: Odometry: ground_truth_frame_id  = 
[ INFO] [1681939075.142746340]: Odometry: ground_truth_base_frame_id = base_link
[ INFO] [1681939075.142769499]: Odometry: config_path            = 
[ INFO] [1681939075.142799307]: Odometry: publish_null_when_lost = true
[ INFO] [1681939075.142821684]: Odometry: guess_frame_id         = 
[ INFO] [1681939075.142846210]: Odometry: guess_min_translation  = 0.000000
[ INFO] [1681939075.142869886]: Odometry: guess_min_rotation     = 0.000000
[ INFO] [1681939075.142900902]: Odometry: guess_min_time         = 0.000000
[ INFO] [1681939075.142925360]: Odometry: expected_update_rate   = 0.000000 Hz
[ INFO] [1681939075.142949043]: Odometry: max_update_rate        = 0.000000 Hz
[ INFO] [1681939075.142973067]: Odometry: min_update_rate        = 0.000000 Hz
[ INFO] [1681939075.142998049]: Odometry: wait_imu_to_init       = false
[ INFO] [1681939075.143055201]: Odometry: stereoParams_=0 visParams_=1 icpParams_=0
[ INFO] [1681939075.550301566]: /rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1681939075.550393175]: /rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1681939075.550457271]: /rtabmap(maps): map_cleanup                = true
[ INFO] [1681939075.550488275]: /rtabmap(maps): map_always_update          = false
[ INFO] [1681939075.550507105]: /rtabmap(maps): map_empty_ray_tracing      = true
[ INFO] [1681939075.550524119]: /rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1681939075.550538925]: /rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1681939075.550555400]: /rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1681939075.554186973]: /rtabmap(maps): octomap_tree_depth         = 16
[ INFO] [1681939075.596344982]: /rgbd_sync: approx_sync = false
[ INFO] [1681939075.619547455]: /rgbd_sync: queue_size  = 10
[ INFO] [1681939075.619651220]: /rgbd_sync: depth_scale = 1.000000
[ INFO] [1681939075.619720389]: /rgbd_sync: decimation = 1
[ INFO] [1681939075.619782371]: /rgbd_sync: compressed_rate = 0.000000
[ INFO] [1681939075.686778206]: 
/rgbd_sync subscribed to (exact sync):
   /camera/color/image_raw \
   /camera/aligned_depth_to_color/image_raw \
   /camera/color/camera_info
[ INFO] [1681939075.694852532]: rtabmap: frame_id      = base_link
[ INFO] [1681939075.695014851]: rtabmap: map_frame_id  = map
[ INFO] [1681939075.695100166]: rtabmap: log_to_rosout_level = 4
[ INFO] [1681939075.695168664]: rtabmap: initial_pose  = 
[ INFO] [1681939075.695266696]: rtabmap: use_action_for_goal  = false
[ INFO] [1681939075.695296598]: rtabmap: tf_delay      = 0.050000
[ INFO] [1681939075.695315840]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1681939075.695347086]: rtabmap: odom_sensor_sync   = false
[ INFO] [1681939075.697225500]: rtabmap: gen_scan  = false
[ INFO] [1681939075.697418370]: rtabmap: gen_depth  = false
[ INFO] [1681939075.700587884]: rtabmap: scan_cloud_max_points = 0
[ INFO] [1681939075.700752408]: rtabmap: scan_cloud_is_2d      = false
[ INFO] [1681939077.050055671]: Setting RTAB-Map parameter "RGBD/AngularUpdate"="0.01"
[ INFO] [1681939077.081729909]: Setting RTAB-Map parameter "RGBD/LinearUpdate"="0.01"
[ INFO] [1681939077.146430659]: Setting RTAB-Map parameter "RGBD/OptimizeFromGraphEnd"="false"
[ INFO] [1681939077.350750116]: RGBDOdometry: approx_sync    = true
[ INFO] [1681939077.350809154]: RGBDOdometry: approx_sync_max_interval = 0.000000
[ INFO] [1681939077.350841283]: RGBDOdometry: queue_size     = 5
[ INFO] [1681939077.350865055]: RGBDOdometry: subscribe_rgbd = true
[ INFO] [1681939077.350890797]: RGBDOdometry: rgbd_cameras   = 1
[ INFO] [1681939077.350907367]: RGBDOdometry: keep_color     = false
[ INFO] [1681939077.356602769]: 
/rgbd_odometry subscribed to:
   /rgbd_image
[ WARN] [1681939077.650004804]: Could not get transform from base_link to camera_color_optical_frame after 0.100000 seconds (for stamp=1681939077.334201)! Error="canTransform: target_frame base_link does not exist.. canTransform returned after 0.100378 timeout was 0.1.".
[ WARN] [1681939077.757597366]: Could not get transform from base_link to camera_color_optical_frame after 0.100000 seconds (for stamp=1681939077.400913)! Error="canTransform: target_frame base_link does not exist.. canTransform returned after 0.100211 timeout was 0.1.".
matlabbe commented 1 year ago

[ WARN] [1681939077.650004804]: Could not get transform from base_link to camera_color_optical_frame after 0.100000 seconds (for stamp=1681939077.334201)! Error="canTransform: target_frame base_link does not exist.. canTransform returned after 0.100378 timeout was 0.1.". [ WARN] [1681939077.757597366]: Could not get transform from base_link to camera_color_optical_frame after 0.100000 seconds (for stamp=1681939077.400913)! Error="canTransform: target_frame base_link does not exist.. canTransform returned after 0.100211 timeout was 0.1.".

Looks like you are missing a TF transform between base_link and your camera frame.