search

introlab / rtabmap_ros

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

Turtlebot3 Demo Subscribing to Both Laser and RGBD? #532

Open uchuuengineer opened 3 years ago

uchuuengineer commented 3 years ago

My project involves localizing to a specific point on a map based on image data, but building the map with LIDAR or a laser scan.

To that end, I modified the model of a turtlebot 3 burger robot by affixing a Kinect depth camera to it. I did this by modifying the turtlebot3_burger.gazebo.xacro and turtlebot3_burger.urdf.xacro files. I then modified the demo_turtlebot3_navigation.launch file to subscribe to rgbd (subscribe_rgbd) data as well as the default subscribe_scan, but RViz developed a lot of errors caused by "Fixed Frame [map] does not exist".

Is it possible to use RTABMap to subscribe to rgbd and laser scan data at the same time? Below is the output I get from terminal (after this, the same part of the final message repeats):

dna@dna-desktop:~$ roslaunch rtabmap_ros demo_turtlebot3_test.launch
... logging to /home/dna/.ros/log/c2313374-5dab-11eb-9267-7085c2dd5b39/roslaunch-dna-desktop-6633.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://localhost:46081/

SUMMARY
========

PARAMETERS
 * /move_base/DWAPlannerROS/acc_lim_theta: 3.2
 * /move_base/DWAPlannerROS/acc_lim_x: 2.5
 * /move_base/DWAPlannerROS/acc_lim_y: 0.0
 * /move_base/DWAPlannerROS/controller_frequency: 10.0
 * /move_base/DWAPlannerROS/forward_point_distance: 0.325
 * /move_base/DWAPlannerROS/goal_distance_bias: 20.0
 * /move_base/DWAPlannerROS/latch_xy_goal_tolerance: False
 * /move_base/DWAPlannerROS/max_scaling_factor: 0.2
 * /move_base/DWAPlannerROS/max_vel_theta: 2.75
 * /move_base/DWAPlannerROS/max_vel_trans: 0.22
 * /move_base/DWAPlannerROS/max_vel_x: 0.22
 * /move_base/DWAPlannerROS/max_vel_y: 0.0
 * /move_base/DWAPlannerROS/min_vel_theta: 1.37
 * /move_base/DWAPlannerROS/min_vel_trans: 0.11
 * /move_base/DWAPlannerROS/min_vel_x: -0.22
 * /move_base/DWAPlannerROS/min_vel_y: 0.0
 * /move_base/DWAPlannerROS/occdist_scale: 0.02
 * /move_base/DWAPlannerROS/oscillation_reset_dist: 0.05
 * /move_base/DWAPlannerROS/path_distance_bias: 32.0
 * /move_base/DWAPlannerROS/publish_cost_grid_pc: True
 * /move_base/DWAPlannerROS/publish_traj_pc: True
 * /move_base/DWAPlannerROS/scaling_speed: 0.25
 * /move_base/DWAPlannerROS/sim_time: 1.5
 * /move_base/DWAPlannerROS/stop_time_buffer: 0.2
 * /move_base/DWAPlannerROS/vth_samples: 40
 * /move_base/DWAPlannerROS/vx_samples: 20
 * /move_base/DWAPlannerROS/vy_samples: 0
 * /move_base/DWAPlannerROS/xy_goal_tolerance: 0.05
 * /move_base/DWAPlannerROS/yaw_goal_tolerance: 0.17
 * /move_base/base_local_planner: dwa_local_planner...
 * /move_base/conservative_reset_dist: 3.0
 * /move_base/controller_frequency: 10.0
 * /move_base/controller_patience: 15.0
 * /move_base/global_costmap/cost_scaling_factor: 3.0
 * /move_base/global_costmap/footprint: [[-0.105, -0.105]...
 * /move_base/global_costmap/global_frame: map
 * /move_base/global_costmap/inflation_radius: 1.0
 * /move_base/global_costmap/map_type: costmap
 * /move_base/global_costmap/observation_sources: scan
 * /move_base/global_costmap/obstacle_range: 3.0
 * /move_base/global_costmap/publish_frequency: 10.0
 * /move_base/global_costmap/raytrace_range: 3.5
 * /move_base/global_costmap/robot_base_frame: base_footprint
 * /move_base/global_costmap/scan/clearing: True
 * /move_base/global_costmap/scan/data_type: LaserScan
 * /move_base/global_costmap/scan/marking: True
 * /move_base/global_costmap/scan/sensor_frame: base_scan
 * /move_base/global_costmap/scan/topic: scan
 * /move_base/global_costmap/static_map: True
 * /move_base/global_costmap/transform_tolerance: 0.5
 * /move_base/global_costmap/update_frequency: 10.0
 * /move_base/local_costmap/cost_scaling_factor: 3.0
 * /move_base/local_costmap/footprint: [[-0.105, -0.105]...
 * /move_base/local_costmap/global_frame: odom
 * /move_base/local_costmap/height: 3
 * /move_base/local_costmap/inflation_radius: 1.0
 * /move_base/local_costmap/map_type: costmap
 * /move_base/local_costmap/observation_sources: scan
 * /move_base/local_costmap/obstacle_range: 3.0
 * /move_base/local_costmap/publish_frequency: 10.0
 * /move_base/local_costmap/raytrace_range: 3.5
 * /move_base/local_costmap/resolution: 0.05
 * /move_base/local_costmap/robot_base_frame: base_footprint
 * /move_base/local_costmap/rolling_window: True
 * /move_base/local_costmap/scan/clearing: True
 * /move_base/local_costmap/scan/data_type: LaserScan
 * /move_base/local_costmap/scan/marking: True
 * /move_base/local_costmap/scan/sensor_frame: base_scan
 * /move_base/local_costmap/scan/topic: scan
 * /move_base/local_costmap/static_map: False
 * /move_base/local_costmap/transform_tolerance: 0.5
 * /move_base/local_costmap/update_frequency: 10.0
 * /move_base/local_costmap/width: 3
 * /move_base/oscillation_distance: 0.2
 * /move_base/oscillation_timeout: 10.0
 * /move_base/planner_frequency: 5.0
 * /move_base/planner_patience: 5.0
 * /move_base/shutdown_costmaps: False
 * /robot_description: <?xml version="1....
 * /robot_state_publisher/publish_frequency: 50.0
 * /robot_state_publisher/tf_prefix: 
 * /rosdistro: melodic
 * /rosversion: 1.14.10
 * /rtabmap/rtabmap/GridGlobal/MinSize: 20
 * /rtabmap/rtabmap/Mem/IncrementalMemory: true
 * /rtabmap/rtabmap/Reg/Force3DoF: true
 * /rtabmap/rtabmap/Reg/Strategy: 1
 * /rtabmap/rtabmap/approx_sync: True
 * /rtabmap/rtabmap/database_path: ~/.ros/rtabmap.db
 * /rtabmap/rtabmap/frame_id: base_footprint
 * /rtabmap/rtabmap/subscribe_depth: False
 * /rtabmap/rtabmap/subscribe_rgb: False
 * /rtabmap/rtabmap/subscribe_rgbd: True
 * /rtabmap/rtabmap/subscribe_scan: True
 * /rtabmap/rtabmap/use_action_for_goal: True
 * /rtabmap/rtabmapviz/approx_sync: True
 * /rtabmap/rtabmapviz/frame_id: base_footprint
 * /rtabmap/rtabmapviz/subscribe_odom: True
 * /rtabmap/rtabmapviz/subscribe_rgbd: True
 * /rtabmap/rtabmapviz/subscribe_scan: True

NODES
  /
    move_base (move_base/move_base)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    rviz (rviz/rviz)
  /rtabmap/
    rgbd_sync (rtabmap_ros/rgbd_sync)
    rtabmap (rtabmap_ros/rtabmap)
    rtabmapviz (rtabmap_ros/rtabmapviz)

ROS_MASTER_URI=http://localhost:11311

process[robot_state_publisher-1]: started with pid [6758]
process[rtabmap/rgbd_sync-2]: started with pid [6759]
process[rtabmap/rtabmap-3]: started with pid [6764]
process[rtabmap/rtabmapviz-4]: started with pid [6766]
process[move_base-5]: started with pid [6768]
process[rviz-6]: started with pid [6773]
[ INFO] [1611427786.067632690]: Initializing nodelet with 24 worker threads.
[ INFO] [1611427786.188196980]: Starting node...
[ INFO] [1611427786.208884477]: Initializing nodelet with 24 worker threads.
[ INFO] [1611427786.260522627]: /rtabmap/rgbd_sync: approx_sync = true
[ INFO] [1611427786.260545697]: /rtabmap/rgbd_sync: queue_size  = 10
[ INFO] [1611427786.260560597]: /rtabmap/rgbd_sync: depth_scale = 1.000000
[ INFO] [1611427786.260570077]: /rtabmap/rgbd_sync: decimation = 1
[ INFO] [1611427786.260580347]: /rtabmap/rgbd_sync: compressed_rate = 0.000000
[ INFO] [1611427786.270843535]: Starting node...
[ INFO] [1611427786.272627625]: /rtabmap/rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1611427786.272655535]: /rtabmap/rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1611427786.272670905]: /rtabmap/rtabmap(maps): map_cleanup                = true
[ INFO] [1611427786.272686016]: /rtabmap/rtabmap(maps): map_always_update          = false
[ INFO] [1611427786.272701046]: /rtabmap/rtabmap(maps): map_empty_ray_tracing      = true
[ INFO] [1611427786.272716436]: /rtabmap/rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1611427786.272733176]: /rtabmap/rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1611427786.272749676]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1611427786.273568296]: 
/rtabmap/rgbd_sync subscribed to (approx sync):
   /camera/rgb/image_raw \
   /camera/depth/image_raw \
   /camera/rgb/camera_info
[ INFO] [1611427786.283022305]: rtabmap: frame_id      = base_footprint
[ INFO] [1611427786.283055076]: rtabmap: map_frame_id  = map
[ INFO] [1611427786.283083566]: rtabmap: use_action_for_goal  = true
[ INFO] [1611427786.283113266]: rtabmap: tf_delay      = 0.050000
[ INFO] [1611427786.283138927]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1611427786.283164217]: rtabmap: odom_sensor_sync   = false
[ INFO] [1611427786.326758955]: Setting RTAB-Map parameter "GridGlobal/MinSize"="20"
[ INFO] [1611427786.363757578]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true"
[ INFO] [1611427786.406891312]: Setting RTAB-Map parameter "Reg/Force3DoF"="true"
[ INFO] [1611427786.407526739]: Setting RTAB-Map parameter "Reg/Strategy"="1"
[ INFO] [1611427786.446832459]: rtabmapviz: Using configuration from "/home/dna/catkin_ws/src/rtabmap_ros/launch/config/rgbd_gui.ini"
[ WARN] [1611427786.544014280]: Setting "Grid/FromDepth" parameter to false (default true) as "subscribe_scan" or "subscribe_scan_cloud" is true. The occupancy grid map will be constructed from laser scans. To get occupancy grid map from cloud projection, set "Grid/FromDepth" to true. To suppress this warning, add <param name="Grid/FromDepth" type="string" value="false"/>
[ INFO] [1611427786.544109201]: Setting "Grid/RangeMax" parameter to 0 (default 5.000000) as "subscribe_scan" or "subscribe_scan_cloud" is true and Grid/FromDepth is false.
[ WARN] [1611427786.544185502]: Setting "RGBD/ProximityPathMaxNeighbors" parameter to 10 (default 0) as "subscribe_scan" is true and "Reg/Strategy" uses ICP. Proximity detection by space will be also done by merging close scans. To disable, set "RGBD/ProximityPathMaxNeighbors" to 0. To suppress this warning, add <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/>
[ INFO] [1611427786.545051131]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1611427786.545217973]: rtabmap: Deleted database "/home/dna/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[ INFO] [1611427786.545246924]: rtabmap: Using database from "/home/dna/.ros/rtabmap.db" (0 MB).
[ INFO] [1611427786.693651842]: rtabmap: Database version = "0.20.6".
[ INFO] [1611427786.712275036]: /rtabmap/rtabmap: subscribe_depth = false
[ INFO] [1611427786.712299666]: /rtabmap/rtabmap: subscribe_rgb = false
[ INFO] [1611427786.712311846]: /rtabmap/rtabmap: subscribe_stereo = false
[ INFO] [1611427786.712327276]: /rtabmap/rtabmap: subscribe_rgbd = true (rgbd_cameras=1)
[ INFO] [1611427786.712339016]: /rtabmap/rtabmap: subscribe_odom_info = false
[ INFO] [1611427786.712351776]: /rtabmap/rtabmap: subscribe_user_data = false
[ INFO] [1611427786.712364437]: /rtabmap/rtabmap: subscribe_scan = true
[ INFO] [1611427786.712379027]: /rtabmap/rtabmap: subscribe_scan_cloud = false
[ INFO] [1611427786.712391817]: /rtabmap/rtabmap: subscribe_scan_descriptor = false
[ INFO] [1611427786.712405837]: /rtabmap/rtabmap: queue_size    = 10
[ INFO] [1611427786.712424487]: /rtabmap/rtabmap: approx_sync   = true
[ INFO] [1611427786.712465038]: Setup rgbd callback
[ INFO] [1611427786.721833855]: 
/rtabmap/rtabmap subscribed to (approx sync):
   /odom \
   /rtabmap/rgbd_image \
   /scan
[ INFO] [1611427786.788148384]: rtabmap 0.20.6 started...
libpng warning: iCCP: known incorrect sRGB profile
libpng warning: iCCP: known incorrect sRGB profile
libpng warning: iCCP: known incorrect sRGB profile
[ INFO] [1611427787.351907963]: Reading parameters from the ROS server...
[ INFO] [1611427787.443227369]: Parameters read = 337
[ INFO] [1611427787.443251189]: Parameters successfully read.
[ INFO] [1611427787.689981193]: /rtabmap/rtabmapviz: subscribe_depth = false
[ INFO] [1611427787.690003903]: /rtabmap/rtabmapviz: subscribe_rgb = false
[ INFO] [1611427787.690015293]: /rtabmap/rtabmapviz: subscribe_stereo = false
[ INFO] [1611427787.690026043]: /rtabmap/rtabmapviz: subscribe_rgbd = true (rgbd_cameras=1)
[ INFO] [1611427787.690041114]: /rtabmap/rtabmapviz: subscribe_odom_info = false
[ INFO] [1611427787.690054354]: /rtabmap/rtabmapviz: subscribe_user_data = false
[ INFO] [1611427787.690065064]: /rtabmap/rtabmapviz: subscribe_scan = true
[ INFO] [1611427787.690079864]: /rtabmap/rtabmapviz: subscribe_scan_cloud = false
[ INFO] [1611427787.690092274]: /rtabmap/rtabmapviz: subscribe_scan_descriptor = false
[ INFO] [1611427787.690104464]: /rtabmap/rtabmapviz: queue_size    = 10
[ INFO] [1611427787.690116774]: /rtabmap/rtabmapviz: approx_sync   = true
[ INFO] [1611427787.690141435]: Setup rgbd callback
[ INFO] [1611427787.737556687]: 
/rtabmap/rtabmapviz subscribed to (approx sync):
   /odom \
   /rtabmap/rgbd_image \
   /scan
[ INFO] [1611427787.737627228]: rtabmapviz started.
[ WARN] [1611427789.873808461, 1591.440000000]: Timed out waiting for transform from base_footprint to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 1591.44 timeout was 0.1.
[ WARN] [1611427794.875464062, 1596.437000000]: /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/rtabmap subscribed to (approx sync):
   /odom \
   /rtabmap/rgbd_image \
   /scan
[ WARN] [1611427794.875824196, 1596.438000000]: /rtabmap/rtabmapviz: 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/rtabmapviz subscribed to (approx sync):
   /odom \
   /rtabmap/rgbd_image \
   /scan
[ WARN] [1611427794.876816717, 1596.439000000]: /rtabmap/rgbd_sync: 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. 
/rtabmap/rgbd_sync subscribed to (approx sync):
   /camera/rgb/image_raw \
   /camera/depth/image_raw \
   /camera/rgb/camera_info
[ WARN] [1611427794.881948725, 1596.444000000]: Timed out waiting for transform from base_footprint to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.1 timeout was 0.1.
[ WARN] [1611427799.878336331, 1601.437000000]: /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/rtabmap subscribed to (approx sync):
   /odom \
   /rtabmap/rgbd_image \
   /scan

Thank you very much for your help.

matlabbe commented 3 years ago

Check which topics are not published:

rostopic hz /odom \
   /rtabmap/rgbd_image \
   /scan \
   /camera/rgb/image_raw \
   /camera/depth/image_raw \
   /camera/rgb/camera_info

/rtabmap/rgbd_image depends on /camera/rgb/image_raw, /camera/depth/image_raw, /camera/rgb/camera_info.

Make sure rgbd_sync topics are remapped correctly to your camera topics.

If you use turtlebot3 "waffle", there is an example launching with the RGB-D camera: http://wiki.ros.org/rtabmap_ros/Tutorials/MappingAndNavigationOnTurtlebot#Turtlebot3_On_Melodic