RGB-D Handheld Mapping with RealSense R200
I was following the RGB-D Handheld Mapping tutorial . First I ran the following command
$ roslaunch realsense_camera r200_nodelet_default.launch
While the realsense_camera node is running, in a new tab I ran
$ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" depth_topic:=/camera/depth_registered/sw_registered/image_rect_raw
The RTABMAP GUI is launched as expected but there is no image.
In the terminal that I launched the RTABMAP GUI, I am receiving the following warning.
[ WARN] [1532462055.394145038]: /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/image_raw,
/camera/rgb/camera_info
Same thing happens when I run
$ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" depth_topic:=/camera/depth/points/image_raw
Can you help me solve this issue? Thank you.
I am running Ros Kinetic on Ubuntu 16.04 64-bit machine.
Edit
The published topics are:
$ rostopic list
/camera/color/camera_info
/camera/color/image_raw
/camera/color/image_raw/compressed
/camera/color/image_raw/compressed/parameter_descriptions
/camera/color/image_raw/compressed/parameter_updates
/camera/color/image_raw/compressedDepth
/camera/color/image_raw/compressedDepth/parameter_descriptions
/camera/color/image_raw/compressedDepth/parameter_updates
/camera/color/image_raw/theora
/camera/color/image_raw/theora/parameter_descriptions
/camera/color/image_raw/theora/parameter_updates
/camera/depth/camera_info
/camera/depth/image_raw
/camera/depth/image_raw/compressed
/camera/depth/image_raw/compressed/parameter_descriptions
/camera/depth/image_raw/compressed/parameter_updates
/camera/depth/image_raw/compressedDepth
/camera/depth/image_raw/compressedDepth/parameter_descriptions
/camera/depth/image_raw/compressedDepth/parameter_updates
/camera/depth/image_raw/theora
/camera/depth/image_raw/theora/parameter_descriptions
/camera/depth/image_raw/theora/parameter_updates
/camera/depth/points
/camera/driver/parameter_descriptions
/camera/driver/parameter_updates
/camera/ir/camera_info
/camera/ir/image_raw
/camera/ir/image_raw/compressed
/camera/ir/image_raw/compressed/parameter_descriptions
/camera/ir/image_raw/compressed/parameter_updates
/camera/ir/image_raw/compressedDepth
/camera/ir/image_raw/compressedDepth/parameter_descriptions
/camera/ir/image_raw/compressedDepth/parameter_updates
/camera/ir/image_raw/theora
/camera/ir/image_raw/theora/parameter_descriptions
/camera/ir/image_raw/theora/parameter_updates
/camera/ir2/camera_info
/camera/ir2/image_raw
/camera/ir2/image_raw/compressed
/camera/ir2/image_raw/compressed/parameter_descriptions
/camera/ir2/image_raw/compressed/parameter_updates
/camera/ir2/image_raw/compressedDepth
/camera/ir2/image_raw/compressedDepth/parameter_descriptions
/camera/ir2/image_raw/compressedDepth/parameter_updates
/camera/ir2/image_raw/theora
/camera/ir2/image_raw/theora/parameter_descriptions
/camera/ir2/image_raw/theora/parameter_updates
/camera/nodelet_manager/bond
/rosout
/rosout_agg
/tf
/tf_static
I think the RTABMAP GUI is not looking for the right topic; there is nothing matching "/camera/depth_registered/sw_registered/image_rect_raw". What should I change the
$ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" depth_topic:=/camera/depth_registered/sw_registered/image_rect_raw
command with?
Edit 2
As Martin suggested, I tried running
$ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" depth_topic:=/camera/depth/image_raw
Again, there's no image in the GUI. I also ran "rostopic hz /camera/depth/image_raw" and got:
$ rostopic hz /camera/depth/image_raw
subscribed to [/camera/depth/image_raw]
average rate: 60.027
min: 0.016s max: 0.017s std dev: 0.00024s window: 57
average rate: 60.022
min: 0.016s max: 0.017s std dev: 0.00025s window: 117
average rate: 60.018
min: 0.016s max: 0.017s std dev: 0.00025s window: 177
average rate: 60 ...
Looks like first we need to check what the topic is. "rostopic list" should display it, afterwards try "rostopic echo --noarr <topic>" to see if you get some data.
Hello @Humpelstilzchen can you check my edit? Thank you.
Which
realsense_camera
package version do you have? Just tried with latest version of their indigo-devel branch and/camera/depth_registered/sw_registered/image_rect_raw
should be published byrealsense_camera
node.Just found the difference, try
roslaunch realsense_camera r200_nodelet_rgbd.launch
instead like in the tutorial.@matlabbe This one worked. Thank you very much!
Can you please attach your images directly to the question?