search

introlab / rtabmap_ros

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

rtabmap with R200 camera on Intel Aero drone #163

Open mindThomas opened 7 years ago

mindThomas commented 7 years ago

Hi. I have been trying to get rtabmap working with the R200 camera connected to our Intel Aero drone. For now I have not been trying to get it working on the Intel Aero board but rather using the distributed/networked structure of ROS and trying to run rtabmap on my computer running Ubuntu 14.04.

I have built rtabmap and rtabmap-ros from source, both trying the indigo-devel branch and latestly the master branch, but none of them with any success. I am afraid that the problem has something to do with the TF's not being published, as I can only see parts related to camera_link etc. being published in the /tf_static topic.

Everytime I try to run rtabmap it loads fine without any errors but the visualization screen stays blank.

This is the output of the latest run built from the master branch:

thomas@Thomas-Lenovo:~/catkin_ws$ roslaunch rtabmap_ros rgbd_mapping.launch rtabmap_args:="--delete_db_on_start"
... logging to /home/thomas/.ros/log/f25cd3c6-1dec-11e7-877e-ce9ddddaaf59/roslaunch-Thomas-Lenovo-24754.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://192.168.8.3:46079/

SUMMARY
========

PARAMETERS
 * /rosdistro: indigo
 * /rosversion: 1.11.21
 * /rtabmap/rgbd_odometry/approx_sync: True
 * /rtabmap/rgbd_odometry/config_path: 
 * /rtabmap/rgbd_odometry/frame_id: camera_link
 * /rtabmap/rgbd_odometry/odom_frame_id: odom
 * /rtabmap/rgbd_odometry/queue_size: 10
 * /rtabmap/rgbd_odometry/wait_for_transform_duration: 0.2
 * /rtabmap/rtabmap/Mem/IncrementalMemory: true
 * /rtabmap/rtabmap/Mem/InitWMWithAllNodes: false
 * /rtabmap/rtabmap/approx_sync: True
 * /rtabmap/rtabmap/config_path: 
 * /rtabmap/rtabmap/database_path: ~/.ros/rtabmap.db
 * /rtabmap/rtabmap/frame_id: camera_link
 * /rtabmap/rtabmap/map_frame_id: map
 * /rtabmap/rtabmap/odom_frame_id: 
 * /rtabmap/rtabmap/odom_tf_angular_variance: 1.0
 * /rtabmap/rtabmap/odom_tf_linear_variance: 1.0
 * /rtabmap/rtabmap/queue_size: 10
 * /rtabmap/rtabmap/subscribe_depth: True
 * /rtabmap/rtabmap/subscribe_scan: False
 * /rtabmap/rtabmap/subscribe_scan_cloud: False
 * /rtabmap/rtabmap/subscribe_stereo: False
 * /rtabmap/rtabmap/subscribe_user_data: False
 * /rtabmap/rtabmap/wait_for_transform_duration: 0.2
 * /rtabmap/rtabmapviz/approx_sync: True
 * /rtabmap/rtabmapviz/frame_id: camera_link
 * /rtabmap/rtabmapviz/odom_frame_id: 
 * /rtabmap/rtabmapviz/queue_size: 10
 * /rtabmap/rtabmapviz/subscribe_depth: True
 * /rtabmap/rtabmapviz/subscribe_odom_info: True
 * /rtabmap/rtabmapviz/subscribe_scan: False
 * /rtabmap/rtabmapviz/subscribe_scan_cloud: False
 * /rtabmap/rtabmapviz/subscribe_stereo: False
 * /rtabmap/rtabmapviz/wait_for_transform_duration: 0.2

NODES
  /rtabmap/
    rgbd_odometry (rtabmap_ros/rgbd_odometry)
    rtabmap (rtabmap_ros/rtabmap)
    rtabmapviz (rtabmap_ros/rtabmapviz)

ROS_MASTER_URI=http://192.168.8.1:11311/

core service [/rosout] found
process[rtabmap/rgbd_odometry-1]: started with pid [24763]
process[rtabmap/rtabmap-2]: started with pid [24764]
process[rtabmap/rtabmapviz-3]: started with pid [24765]
[ INFO] [1491847411.864648672]: Starting node...
[ INFO] [1491847411.993931874]: Starting node...
[ INFO] [1491847412.172359296]: Initializing nodelet with 4 worker threads.
[ INFO] [1491847412.177012975]: Initializing nodelet with 4 worker threads.
[ INFO] [1491847412.327886299]: rtabmapviz: Using configuration from "/home/thomas/.ros/rtabmap_gui.ini"
[ INFO] [1491847413.013893192]: /rtabmap/rtabmap(maps): grid_cell_size             = 0.050000
[ INFO] [1491847413.013933998]: /rtabmap/rtabmap(maps): grid_incremental           = false
[ INFO] [1491847413.013951039]: /rtabmap/rtabmap(maps): grid_size                  = 0.000000
[ INFO] [1491847413.013965147]: /rtabmap/rtabmap(maps): grid_eroded                = false
[ INFO] [1491847413.013977745]: /rtabmap/rtabmap(maps): grid_footprint_radius      = 0.000000
[ INFO] [1491847413.013991120]: /rtabmap/rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1491847413.014005390]: /rtabmap/rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1491847413.014018848]: /rtabmap/rtabmap(maps): map_cleanup                = true
[ INFO] [1491847413.014034769]: /rtabmap/rtabmap(maps): map_negative_poses_ignored = false
[ INFO] [1491847413.014048175]: /rtabmap/rtabmap(maps): map_negative_scan_ray_tracing = true
[ INFO] [1491847413.014063488]: /rtabmap/rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1491847413.014078410]: /rtabmap/rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1491847413.014092686]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1491847413.057928002]: /rtabmap/rtabmap(maps): octomap_tree_depth         = 16
[ INFO] [1491847413.058617495]: /rtabmap/rtabmap(maps): octomap_occupancy_thr      = 0.500000
[ INFO] [1491847413.265974088]: Reading parameters from the ROS server...
[ INFO] [1491847413.747649264]: rtabmap: frame_id      = camera_link
[ INFO] [1491847413.747831313]: rtabmap: map_frame_id  = map
[ INFO] [1491847413.747954120]: rtabmap: tf_delay      = 0.050000
[ INFO] [1491847413.748055237]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1491847413.748156883]: rtabmap: odom_sensor_sync   = false
[ INFO] [1491847420.848274485]: Parameters read = 0
[ INFO] [1491847423.112973948]: /rtabmap/rtabmapviz: queue_size    = 10
[ INFO] [1491847423.113106793]: /rtabmap/rtabmapviz: rgbd_cameras = 1
[ INFO] [1491847423.113170584]: /rtabmap/rtabmapviz: approx_sync   = true
[ INFO] [1491847423.113365009]: Setup depth callback
[ INFO] [1491847424.663148634]: 
/rtabmap/rtabmapviz subscribed to (approx sync):
   /rtabmap/odom,
   /camera/rgb/image_rect_color,
   /camera/depth_registered/image_raw,
   /camera/rgb/camera_info,
   /rtabmap/odom_info
[ INFO] [1491847424.663547823]: rtabmapviz started.
[ INFO] [1491847426.428821243]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true"
[ INFO] [1491847426.461953762]: Setting RTAB-Map parameter "Mem/InitWMWithAllNodes"="false"
[ INFO] [1491847429.311715170]: 
/rtabmap/rgbd_odometry subscribed to (approx sync):
   /camera/rgb/image_rect_color,
   /camera/depth_registered/image_raw,
   /camera/rgb/camera_info
[ INFO] [1491847496.320660498]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1491847496.321001481]: rtabmap: Deleted database "/home/thomas/.ros/rtabmap.db" (--delete_db_on_start is set).
[ INFO] [1491847496.321082443]: rtabmap: Using database from "/home/thomas/.ros/rtabmap.db".
[ INFO] [1491847496.988443638]: rtabmap: Database version = "0.12.4".
[ INFO] [1491847500.728787387]: /rtabmap/rtabmap: queue_size    = 10
[ INFO] [1491847500.728877693]: /rtabmap/rtabmap: rgbd_cameras = 1
[ INFO] [1491847500.729025922]: /rtabmap/rtabmap: approx_sync   = true
[ INFO] [1491847500.729386080]: Setup depth callback
[ INFO] [1491847502.967542813]: 
/rtabmap/rtabmap subscribed to (approx sync):
   /rtabmap/odom,
   /camera/rgb/image_rect_color,
   /camera/depth_registered/image_raw,
   /camera/rgb/camera_info
[ INFO] [1491847503.518693168]: rtabmap 0.12.4 started...

And this is the output when I ran the build from the indigo-devel branch (doesn't seem like a huge difference though):

thomas@Thomas-Lenovo:~/catkin_ws$ roslaunch rtabmap_ros rgbd_mapping.launch rtabmap_args:="--delete_db_on_start"
... logging to /home/thomas/.ros/log/f25cd3c6-1dec-11e7-877e-ce9ddddaaf59/roslaunch-Thomas-Lenovo-25315.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://192.168.8.3:38314/

SUMMARY
========

PARAMETERS
 * /rosdistro: indigo
 * /rosversion: 1.11.21
 * /rtabmap/rgbd_odometry/approx_sync: True
 * /rtabmap/rgbd_odometry/config_path: 
 * /rtabmap/rgbd_odometry/frame_id: camera_link
 * /rtabmap/rgbd_odometry/queue_size: 10
 * /rtabmap/rgbd_odometry/wait_for_transform_duration: 0.2
 * /rtabmap/rtabmap/Mem/IncrementalMemory: true
 * /rtabmap/rtabmap/Mem/InitWMWithAllNodes: false
 * /rtabmap/rtabmap/approx_sync: True
 * /rtabmap/rtabmap/config_path: 
 * /rtabmap/rtabmap/database_path: ~/.ros/rtabmap.db
 * /rtabmap/rtabmap/frame_id: camera_link
 * /rtabmap/rtabmap/queue_size: 10
 * /rtabmap/rtabmap/subscribe_depth: True
 * /rtabmap/rtabmap/subscribe_scan: False
 * /rtabmap/rtabmap/subscribe_scan_cloud: False
 * /rtabmap/rtabmap/subscribe_stereo: False
 * /rtabmap/rtabmap/wait_for_transform_duration: 0.2
 * /rtabmap/rtabmapviz/frame_id: camera_link
 * /rtabmap/rtabmapviz/queue_size: 10
 * /rtabmap/rtabmapviz/subscribe_depth: True
 * /rtabmap/rtabmapviz/subscribe_odom_info: True
 * /rtabmap/rtabmapviz/subscribe_scan: False
 * /rtabmap/rtabmapviz/subscribe_scan_cloud: False
 * /rtabmap/rtabmapviz/subscribe_stereo: False
 * /rtabmap/rtabmapviz/wait_for_transform_duration: 0.2

NODES
  /rtabmap/
    rgbd_odometry (rtabmap_ros/rgbd_odometry)
    rtabmap (rtabmap_ros/rtabmap)
    rtabmapviz (rtabmap_ros/rtabmapviz)

ROS_MASTER_URI=http://192.168.8.1:11311/

core service [/rosout] found
process[rtabmap/rgbd_odometry-1]: started with pid [25324]
process[rtabmap/rtabmap-2]: started with pid [25325]
process[rtabmap/rtabmapviz-3]: started with pid [25326]
[ INFO] [1491842804.516829885]: Starting node...
[ INFO] [1491842804.608644961]: Starting node...
[ INFO] [1491842805.242406961]: Initializing nodelet with 4 worker threads.
[ INFO] [1491842806.243257490]: rtabmapviz: Using configuration from "-d"
[ INFO] [1491842807.094530783]: Reading parameters from the ROS server...
[ INFO] [1491842814.060577750]: rtabmap: frame_id = camera_link
[ INFO] [1491842814.060630449]: rtabmap: map_frame_id = map
[ INFO] [1491842814.060656038]: rtabmap: queue_size = 10
[ INFO] [1491842814.060721523]: rtabmap: tf_delay = 0.050000
[ INFO] [1491842814.060744056]: rtabmap: tf_tolerance = 0.100000
[ INFO] [1491842814.060761154]: rtabmap: depth_cameras = 1
[ INFO] [1491842814.060778866]: rtabmap: approx_sync = true
[ INFO] [1491842827.107727877]: Parameters read = 223
[ INFO] [1491842827.107771787]: Parameters successfully read.
[ INFO] [1491842832.345274641]: 
/rtabmap/rtabmapviz subscribed to:
   /camera/rgb/image_rect_color,
   /camera/depth_registered/image_raw,
   /camera/rgb/camera_info,
   /rtabmap/odom,
   /rtabmap/odom_info
[ INFO] [1491842836.215480740]: rtabmapviz started.
[ INFO] [1491842838.591993960]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true"
[ INFO] [1491842838.705769558]: Setting RTAB-Map parameter "Mem/InitWMWithAllNodes"="false"
[ INFO] [1491842854.798987088]: 
/rtabmap/rgbd_odometry subscribed to (approx sync):
   /camera/rgb/image_rect_color,
   /camera/depth_registered/image_raw,
   /camera/rgb/camera_info

Any suggestions or hints?

matlabbe commented 7 years ago

Hi,

Are you streaming /camera/rgb/image_rect_color, /camera/depth_registered/image_raw and /camera/rgb/camera_info on WiFi? Make sure they are received on your computer:

$ rostopic hz /camera/rgb/image_rect_color
$ rostopic hz /camera/depth_registered/image_raw
$ rostopic hz /camera/rgb/camera_info

You may want to stream them as compressed too. You can use argument compressed for convenience. This will add relays to limit the bandwidth used, see tutorial Remote Mapping.

$ roslaunch rtabmap_ros rgbd_mapping.launch rtabmap_args:="--delete_db_on_start" compressed:=true

PS: I suggest to update the code from source if you cloned master branch after April 6 (after this commit https://github.com/introlab/rtabmap/commit/1d8952867d8b98ee3b0e5ad5390e7400fe5a7858 that caused this issue https://github.com/introlab/rtabmap_ros/issues/162).

cheers, Mathieu

mindThomas commented 7 years ago

Source code has been pulled from master today with which a clean build has been performed. Yes, the image topics are streamed over WiFi which can be seen/visualized either using Rviz or using the image viewer of rqt. The data refresh rate (FPS) is very slow though (<5 Hz).

OBS. The depth image is contained within "/camera/depth/image_raw" Can't remember if it was also contained within "/camera/depth_registered/image_raw", but I have tried adding the argument "depth_registered_topic:=/camera/depth/image_raw" without luck.

matlabbe commented 7 years ago

Do you have warnings like that after 5 seconds you started the launch?

[ INFO] [1491863470.494017006]: rtabmap 0.12.5 started...
[ WARN] [1491863474.292900716]: /rtabmap/rgbd_odometry: 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_odometry subscribed to (approx sync):
   /camera/rgb/image_rect_color,
   /camera/depth_registered/image_raw,
   /camera/rgb/camera_info
[ WARN] [1491863475.005535189]: /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):
   /rtabmap/odom,
   /camera/rgb/image_rect_color,
   /camera/depth_registered/image_raw,
   /camera/rgb/camera_info,
   /rtabmap/odom_info
[ WARN] [1491863475.486579636]: /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):
   /rtabmap/odom,
   /camera/rgb/image_rect_color,
   /camera/depth_registered/image_raw,
   /camera/rgb/camera_info

Or any other errors/warnings?

cheers

mindThomas commented 7 years ago

If I do not set /use_sim_time to true I get the above mentioned warnings after 5 seconds. In both cases though I get nothing in the RTAB-MAP visualizer. Keep in mind that I want Visual Odometry only so I have no external odometry sensors publishing.

Here an output of the rates (with /use_sim_time set to false) after waiting/averaging for about 10 seconds:

/camera/rgb/image_rect_color
average rate: 10.101
    min: 0.071s max: 0.621s std dev: 0.03016s window: 585

/camera/depth/image_rect_raw
average rate: 30.035
    min: 0.024s max: 0.077s std dev: 0.00533s window: 328

/camera/rgb/camera_info
average rate: 30.006
    min: 0.000s max: 0.082s std dev: 0.00276s window: 990

With /use_sim_time set to true I don't get any rate.

matlabbe commented 7 years ago

Hi,

use_sim_time=true should be used only with a rosbag (and maybe with a simulation environment too).

The warnings don't appear when use_sim_time=true because the simulated clock is not published, so never reaching 5 seconds of clock time. We should then fix the warnings when use_sim_time=false (don't need to explicitly set it to false).

Can you try the Hand-held tutorial for RealSense R200? If realsense_camera package is used, the registered depth image is called /camera/depth_registered/sw_registered/image_rect_raw. The launch would be:

$ roslaunch rtabmap_ros rgbd_mapping.launch rtabmap_args:="--delete_db_on_start" depth_registered_topic:=/camera/depth_registered/sw_registered/image_rect_raw

cheers, Mathieu