Using Realsense r200 camera for mapping

[daniyara]

Hello everyone!

I am very new to ROS and RTAB, so please excuse if this question is too simple.

I am using r200 realsense camera only (no extrenal odometry). My aim is to record a bag file and then be able to play the bag file to recreate the map.

I have tried different approaches; however, I am not able to bring up the rtab gui when replaying the bag file. I have followed this post:
http://official-rtab-map-forum.206.s1.nabble.com/How-to-reocrd-ros-bag-file-for-rtabmap-database-td567.html

Here is the sequence of commands I have used:

1. Launch r200 with: roslaunch realsense_camera r200_nodelet_rgbd.launch

2. Record the Bag file: rosbag record -O trial /camera/rgb/image_rect_color /camera/rgb/camera_info /camera/depth_registered/image_war /tf
  - Please let me know if I am recording sufficient amount of topics (and if they are the right ones)?

3. After shutting down the recording and camera:
 - roscore
 - rosparam set use_sim_time true
 - roslaunch rtabmap_ros data_recorder.launch

4. The terminal displays the message that the rtab was launched, although no window pops up.

5. In a separate terminal window: rosbag play --clock trial.bag

After this sequence I can see that the bag time in incrementing; however, nothing is displayed.

Running "rostopic list" shows:
/camera/depth_registered/image_raw
/camera/rgb/camera_info
/camera/rgb/image_rect_color
/clock
/cloud_map
/global_path
/goal
/goal_node
/goal_out
/goal_reached
/grid_map
/info
/labels
/local_path
/mapData
/mapGraph
/move_base/cancel
/move_base/feedback
/move_base/goal
/move_base/result
/move_base/status
/octomap_binary
/octomap_cloud
/octomap_empty_space
/octomap_full
/octomap_proj
/proj_map
/rosout
/rosout_agg
/scan_map
/tf
/tf_static

rostopic echo /camera/depth_registered/image_raw works as it continuously spits out numbers.

Any help is appreciated.

Cheers

[matlabbe]

Hi,

You can subscribe to rosbag topics directly instead of using data_recorder.launch. For example (not tested) from tutorial on ros wiki:

Record the bag:

$ roslaunch realsense_camera r200_nodelet_rgbd.launch

$ rosbag record -O trial \
   /camera/rgb/image_rect_color \
   /camera/rgb/camera_info \
   /camera/depth_registered/sw_registered/image_rect_raw \
   /tf_static 

EDIT replaced /tf by /tf_static

Replay the bag:

$ roscore

$ rosparam use_sim_time true

$ roslaunch rtabmap_ros rtabmap.launch \
   rtabmap_args:="--delete_db_on_start" \
   frame_id:=camera_link \
   depth_topic:=/camera/depth_registered/sw_registered/image_rect_raw

$ rosbag play --clock trial.bag

The minimal topics are RGB image, registered depth image, rgb camera info and /tf between frame of the camera (e.g., camera_link) and the optical frames.

cheers,
Mathieu

[daniyara]

Hi Mathieu,

Thank you for helping me out! So when I try to play the bag file, I receive the following output in the terminal where I launched the rtabmap.launch file. For some reason I cannot get a transform from camera_link to camera_rgb_optical_frame. What do you think could be the problem?

After running:
$ rosrun tf view_frames
It shows that no transform exists after the camera_link



cear@cear-Kangaroo-Mobile-Desktop:~/bagfiles$ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" frame_id:=camera_link depth_topic:=/camera/depth_registered/sw_registered/image_rect_raw
... logging to /home/cear/.ros/log/1e50fdb6-5a67-11e7-a472-00215cef3f21/roslaunch-cear-Kangaroo-Mobile-Desktop-8580.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://cear-Kangaroo-Mobile-Desktop:33059/

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://localhost:11311

core service [/rosout] found
process[rtabmap/rgbd_odometry-1]: started with pid [8598]
process[rtabmap/rtabmap-2]: started with pid [8599]
process[rtabmap/rtabmapviz-3]: started with pid [8600]
[ INFO] [1498478689.685131258]: Starting node...
[ INFO] [1498478689.721655713]: Initializing nodelet with 4 worker threads.
[ INFO] [1498478689.875068667]: Starting node...
[ INFO] [1498478690.206772816]: rtabmapviz: Using configuration from "/home/cear/.ros/rtabmap_gui.ini"
[ INFO] [1498478691.449311016]: Reading parameters from the ROS server...
[ INFO] [1498478691.795028445]:
/rtabmap/rgbd_odometry subscribed to (approx sync):
   /camera/rgb/image_rect_color,
   /camera/depth_registered/sw_registered/image_rect_raw,
   /camera/rgb/camera_info
[ INFO] [1498478691.975110676]: Parameters read = 0
[ INFO] [1498478692.539826771]:
/rtabmap/rtabmapviz subscribed to:
   /camera/rgb/image_rect_color,
   /camera/depth_registered/sw_registered/image_rect_raw,
   /camera/rgb/camera_info,
   /rtabmap/odom,
   /rtabmap/odom_info
[ INFO] [1498478692.606576772]: rtabmapviz started.
[ INFO] [1498478700.400159167, 1498478370.189552426]: rtabmap: frame_id = camera_link
[ INFO] [1498478700.400320797, 1498478370.189552426]: rtabmap: map_frame_id = map
[ INFO] [1498478700.400373649, 1498478370.189552426]: rtabmap: queue_size = 10
[ INFO] [1498478700.400427051, 1498478370.189552426]: rtabmap: tf_delay = 0.050000
[ INFO] [1498478700.400473440, 1498478370.189552426]: rtabmap: tf_tolerance = 0.100000
[ INFO] [1498478700.400519641, 1498478370.189552426]: rtabmap: depth_cameras = 1
[ INFO] [1498478700.400568031, 1498478370.189552426]: rtabmap: approx_sync = true
[ WARN] [1498478700.773998409, 1498478370.567159581]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478370.307806) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ WARN] [1498478700.986240837, 1498478370.778189319]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478370.541324) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ INFO] [1498478701.009095974, 1498478370.802617783]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true"
[ INFO] [1498478701.012701820, 1498478370.802617783]: Setting RTAB-Map parameter "Mem/InitWMWithAllNodes"="false"
[ WARN] [1498478701.200430880, 1498478370.992665959]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478370.741237) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ WARN] [1498478701.403666770, 1498478371.197024774]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478370.941204) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ WARN] [1498478701.610582046, 1498478371.403818933]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478371.173964) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ WARN] [1498478701.834498100, 1498478371.624309936]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478371.374827) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ WARN] [1498478702.039415284, 1498478371.832435127]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478371.607600) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ WARN] [1498478702.242869444, 1498478372.036025591]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478371.807392) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ WARN] [1498478702.447103380, 1498478372.239976468]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478372.007908) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ WARN] [1498478702.649310486, 1498478372.441382247]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478372.207412) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ WARN] [1498478702.856871508, 1498478372.650019043]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478372.406877) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ WARN] [1498478703.072593140, 1498478372.865104765]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478372.639890) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ WARN] [1498478703.280424083, 1498478373.073957181]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478372.840311) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ INFO] [1498478703.363667409, 1498478373.140633616]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1498478703.364021883, 1498478373.140633616]: rtabmap: Deleted database "/home/cear/.ros/rtabmap.db" (--delete_db_on_start is set).
[ INFO] [1498478703.364126574, 1498478373.140633616]: rtabmap: Using database from "/home/cear/.ros/rtabmap.db".
[ WARN] [1498478703.497307866, 1498478373.290375422]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478373.039886) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ INFO] [1498478703.514196942, 1498478373.300455884]: rtabmap: Database version = "0.11.8".
[ WARN] [1498478703.705277976, 1498478373.498292769]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478373.239953) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ WARN] [1498478703.915433185, 1498478373.707720341]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478373.474020) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ INFO] [1498478703.935389688, 1498478373.723250633]:
/rtabmap/rtabmap subscribed to (approx sync):
   /camera/rgb/image_rect_color,
   /camera/depth_registered/sw_registered/image_rect_raw,
   /camera/rgb/camera_info,
   /rtabmap/odom
[ INFO] [1498478703.935921668, 1498478373.723250633]: rtabmap 0.11.8 started...
[ WARN] [1498478704.132651104, 1498478373.922224956]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478373.673098) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!
[ WARN] [1498478704.330341259, 1498478374.123225648]: odometry: Could not get transform from camera_link to camera_rgb_optical_frame (stamp=1498478373.906647) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)!

[matlabbe]

That TF would be sent by the camera driver. Did you record /TF in the bag?

[daniyara]

I ran the same commands as you mentioned. Moreover, rosbag info shows that /tf messages were recorded, so I am not sure what is the problem.

cear@cear-Kangaroo-Mobile-Desktop:~/bagfiles$ rosbag info trial6.bag
path:        trial6.bag
version:     2.0
duration:    42.9s
start:       Jun 26 2017 16:17:35.27 (1498483055.27)
end:         Jun 26 2017 16:18:18.20 (1498483098.20)
size:        998.0 MB
messages:    3283
compression: none [888/888 chunks]
types:       sensor_msgs/CameraInfo [c9a58c1b0b154e0e6da7578cb991d214]
             sensor_msgs/Image      [060021388200f6f0f447d0fcd9c64743]
             tf2_msgs/TFMessage     [94810edda583a504dfda3829e70d7eec]
topics:      /camera/depth_registered/sw_registered/image_rect_raw   650 msgs    : sensor_msgs/Image    
             /camera/rgb/camera_info                                 996 msgs    : sensor_msgs/CameraInfo
             /camera/rgb/image_rect_color                            701 msgs    : sensor_msgs/Image    
             /tf                                                     936 msgs    : tf2_msgs/TFMessage     (2 connections)

[matlabbe]

What is the TF tree when you replay the bag alone?

[daniyara]

Weirdly it is the same TF tree.


I tried several times with and without rtabmap.launch, the tree is the same.

[matlabbe]

Make sure you launched only "$ roslaunch realsense_camera r200_nodelet_rgbd.launch" before recording the bag.

[daniyara]

Here is what I got:
- Before, when launching the realsense, I also launched rtabmap.launch to see what I was recording in the bag file.  
- Now, I tried recording the bag file only with realsense launch file running and found that no /tf topic was recorded.
- While running the realsense camera, rostopic echo/hz /tf showed no signs that anything was published on this topic.

Why could this be the case?

[matlabbe]

Hi,

I tried with my realsense, the TF missing is in /tf_static, not /tf. I corrected the rosbag record line above:

$ rosbag record -O trial\
    /camera/rgb/image_rect_color\
    /camera/rgb/camera_info\
    /camera/depth_registered/sw_registered/image_rect_raw\
    /tf_static

cheers,
Mathieu

[daniyara]

Thank you Mathieu!

That worked perfectly :)