Note: This tutorial assumes that you have completed the previous tutorials: RGB-D Hand-Held Mapping With a Kinect. |
Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Remote Mapping
Description: This tutorial shows how to do mapping on a remote computer.Tutorial Level: BEGINNER
Next Tutorial: Setup RTAB-Map on your robot!
Robot
For simplification, I will just show nodes/launch files used for mapping. Your robot may have other stuff launched at the same time. Here I assume that the ROS Master is on the robot, and a Kinect is used.
1. [Optional]: To avoid host name resolution issues, set ROS_IP environment variable for the robot:
$ ifconfig eth0 Link encap:Ethernet HWaddr ##:##:##:##:##:## inet addr:192.168.1.3 Bcast:192.168.1.255 Mask:255.255.255.0 ... $ export ROS_IP=192.168.1.3
2. We will throttle the Kinect messages on the robot at 5 Hz to reduce the bandwidth used on the network (reducing latency). We also want to pre-sync camera topics before sending them over the network. To do both, we can use rtabmap_sync/rgbd_sync nodelet for convenience. Create a file called freenect_throttle.launch and copy/paste the code below.
Note that for the example, I added rgbd_sync argument to switch between rgbd_sync approach and old data_throttle approach. With rtabmap_legacy/data_throttle approach, images can be compressed using image_transport plugins (theora, h264, ...), which could reduce the bandwidth usage. Note that on some computers depending the ROS version, the image transport plugin called compressedDepth for depth images can use a lot of processing power, and thus may decrease the frame rate if the computer cannot keep up. The depth compression approach used in rgbd_sync (using cv::imencode()) seems less computationally expensive, thus affecting less the frame rate in this case.
<launch> <include file="$(find freenect_launch)/launch/freenect.launch"> <arg name="depth_registration" value="True" /> </include> <arg name="rate" default="5"/> <arg name="approx_sync" default="true" /> <!-- true for freenect driver --> <arg name="rgbd_sync" default="true"/> <!-- Use same nodelet used by Freenect/OpenNI --> <group ns="camera"> <node if="$(arg rgbd_sync)" pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_sync/rgbd_sync camera_nodelet_manager" output="screen"> <param name="compressed_rate" type="double" value="$(arg rate)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/> <remap from="rgb/image" to="rgb/image_rect_color"/> <remap from="depth/image" to="depth_registered/image_raw"/> <remap from="rgb/camera_info" to="rgb/camera_info"/> <remap from="rgbd_image" to="rgbd_image"/> </node> <node unless="$(arg rgbd_sync)" pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_legacy/data_throttle camera_nodelet_manager" output="screen"> <param name="rate" type="double" value="$(arg rate)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/> <remap from="rgb/image_in" to="rgb/image_rect_color"/> <remap from="depth/image_in" to="depth_registered/image_raw"/> <remap from="rgb/camera_info_in" to="rgb/camera_info"/> <remap from="rgb/image_out" to="throttled/rgb/image_rect_color"/> <remap from="depth/image_out" to="throttled/depth_registered/image_raw"/> <remap from="rgb/camera_info_out" to="throttled/rgb/camera_info"/> </node> </group> </launch>
Note also that in this example we attach the nodelets to an already started nodelet manager (camera_nodelet_manager) from kinect launch file to avoid serialization/deserialization of image topics. If camera_nodelet_manager doesn't exist (because you are using another camera driver not having a nodelet manager), you can replace "load" by "standalone" to use the nodelets alone, or start your own nodelet manager like this (otherwise rgbd_image topic won't be published!):
<node pkg="nodelet" type="nodelet" name="camera_nodelet_manager" args="manager" output="screen"/>
3. Launch the file:
$ roslaunch freenect_throttle.launch rate:=5
Remote Client Computer
On the client computer, we will launch rtabmap, rgbd_odometry and rtabmap_viz using rtabmap.launch for convenience. For efficiency and to not subscribe to same topics multiple time on the robot, rtabmap.launch will use relays and rgbd_relay to subscribe just once to incoming topics from the robot and relay the messages to multiples nodes on the client side.
1. [Optional]: To avoid host name resolution issues, set ROS_IP environment variable for the client:
$ ifconfig eth0 Link encap:Ethernet HWaddr ##:##:##:##:##:## inet addr:192.168.1.2 Bcast:192.168.1.255 Mask:255.255.255.0 ... $ export ROS_IP=192.168.1.2
2. Set ROS_MASTER_URI with the robot IP/hostname:
$ export ROS_MASTER_URI=http://192.168.1.3:11311
3. Launch rtabmap.launch:
$ roslaunch rtabmap_launch rtabmap.launch subscribe_rgbd:=true rgbd_topic:=/camera/rgbd_image compressed:=true rtabmap_args:="--delete_db_on_start"
If you launched freenect_throttle.launch with rgbd_sync:=false, do this instead:
$ roslaunch rtabmap_launch rtabmap.launch rgb_topic:=/camera/throttled/rgb/image_rect_color depth_topic:=/camera/throttled/depth_registered/image_raw camera_info_topic:=/camera/throttled/rgb/camera_info compressed:=true rtabmap_args:="--delete_db_on_start"
4. You should now see mapping in rtabmap_viz window.
5. If you open the System Monitor of Ubuntu, you should have something like ~950 KiB/s of bandwidth used:
For the data_throttle approach (rgbd_sync:=false), you would get something like ~500 KiB/s instead:
Advanced
1. If you have already odometry on the robot, you can set arguments visual_odometry:=false and odom_topic:="/odom".
2. You can use launch rviz manually or with argument rviz:=true for convenience. If you don't want rtabmap_viz, you can disable it using rtabmap_viz:=false argument.
Extras
Zed RGB-D example
zed_rgbd_throttle.launch:
<launch> <arg name="rate" default="5"/> <!-- Rate we want to publish over network --> <arg name="approx_sync" default="false"/> <!-- false for zed camera --> <arg name="resolution" default="3"/> <!-- Using VGA to use less bandwidth --> <arg name="rgbd_sync" default="true"/> <group ns="camera"> <node pkg="nodelet" type="nodelet" name="camera_nodelet_manager" args="manager" output="screen"/> <include file="$(find zed_wrapper)/launch/zed_camera_nodelet.launch"> <arg name="publish_tf" value="false" /> <arg name="frame_rate" value="15" /> <!-- lowest frame rate we can set with the driver --> <arg name="resolution" value="$(arg resolution)"/> <arg name="nodelet_manager_name" value="camera_nodelet_manager"/> </include> <node pkg="tf" type="static_transform_publisher" name="camera_zed_link" args="0 0 0 0 0 0 camera_link zed_camera_center 100"/> <!-- Use same nodelet used by ZED to avoid image serialization --> <node if="$(arg rgbd_sync)" pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_sync/rgbd_sync camera_nodelet_manager" output="screen"> <param name="compressed_rate" type="double" value="$(arg rate)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/> <remap from="rgb/image" to="rgb/image_rect_color"/> <remap from="depth/image" to="depth/depth_registered"/> <remap from="rgb/camera_info" to="rgb/camera_info"/> <remap from="rgbd_image" to="rgbd_image"/> </node> <node unless="$(arg rgbd_sync)" pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_legacy/data_throttle camera_nodelet_manager" output="screen"> <param name="rate" type="double" value="$(arg rate)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/> <remap from="rgb/image_in" to="rgb/image_rect_color"/> <remap from="depth/image_in" to="depth/depth_registered"/> <remap from="rgb/camera_info_in" to="rgb/camera_info"/> <remap from="rgb/image_out" to="throttled/rgb/image_rect_color"/> <remap from="depth/image_out" to="throttled/depth_registered/image_raw"/> <remap from="rgb/camera_info_out" to="throttled/rgb/camera_info"/> </node> </group> </launch>
Same rtabmap.launch line above with rgbd_image should work on the remote computer. Here we get ~2 MB/s (rgbd_sync:=true) because the depth images are 32 bits instead of 16 bits like in the previous freenect example even if the size of images is relatively the same (Zed in VGA mode has image size of 672x376 vs Kinect with 640x480).
For data_throttle approach (rgbd_sync:=false), the bandwidth usage is quite lower with ~600 KiB/s. Make also sure you launch rtabmap.launch with approx_sync:=false for the zed example.
Zed Stereo example
Example for a stereo camera. zed_stereo_throttle.launch:
<launch> <arg name="rate" default="5"/> <!-- Rate we want to publish over network --> <arg name="approx_sync" default="false"/> <!-- false for zed camera --> <arg name="resolution" default="3"/> <!-- Using VGA to use less bandwidth --> <arg name="stereo_sync" default="true"/> <group ns="camera"> <node pkg="nodelet" type="nodelet" name="camera_nodelet_manager" args="manager" output="screen"/> <include file="$(find zed_wrapper)/launch/zed_camera_nodelet.launch"> <arg name="publish_tf" value="false" /> <arg name="frame_rate" value="15" /> <!-- lowest frame rate we can set with the driver --> <arg name="resolution" value="$(arg resolution)"/> <arg name="nodelet_manager_name" value="camera_nodelet_manager"/> </include> <node pkg="tf" type="static_transform_publisher" name="camera_zed_link" args="0 0 0 0 0 0 camera_link zed_camera_center 100"/> <!-- Use same nodelet used by ZED to avoid image serialization --> <node if="$(arg stereo_sync)" pkg="nodelet" type="nodelet" name="stereo_sync" args="load rtabmap_sync/stereo_sync camera_nodelet_manager" output="screen"> <param name="compressed_rate" type="double" value="$(arg rate)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/> <remap from="left/image_rect" to="left/image_rect_color"/> <remap from="right/image_rect" to="right/image_rect_color"/> <remap from="left/camera_info" to="left/camera_info"/> <remap from="right/camera_info" to="right/camera_info"/> <remap from="rgbd_image" to="rgbd_image"/> </node> <node unless="$(arg stereo_sync)" pkg="nodelet" type="nodelet" name="stereo_throttle" args="load rtabmap_legacy/stereo_throttle camera_nodelet_manager" output="screen"> <param name="rate" type="double" value="$(arg rate)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/> <remap from="left/image" to="left/image_rect_color"/> <remap from="right/image" to="right/image_rect_color"/> <remap from="left/camera_info" to="left/camera_info"/> <remap from="right/camera_info" to="right/camera_info"/> <remap from="left/image_rect_color_throttle" to="throttled/left/image_rect_color"/> <remap from="right/image_rect_color_throttle" to="throttled/right/image_rect"/> <remap from="left/camera_info_throttle" to="throttled/left/camera_info"/> <remap from="right/camera_info_throttle" to="throttled/right/camera_info"/> </node> </group> </launch>
Launch rtabmap.launch on the remote computer for stereo:
1. With roslaunch zed_stereo_throttle.launch stereo_sync:=true
$ roslaunch rtabmap_launch rtabmap.launch subscribe_rgbd:=true rgbd_topic:=/camera/rgbd_image compressed:=true rtabmap_args:="--delete_db_on_start" stereo:=true
2. With roslaunch zed_stereo_throttle.launch stereo_sync:=false
$ roslaunch rtabmap_launch rtabmap.launch stereo_namespace:=/camera/throttled compressed:=true rtabmap_args:="--delete_db_on_start" stereo:=true
With stereo_sync approach, we get 400 KiB/s as with stereo_throttle approach we get 250 KiB/s. In comparison for the previous Zed RGB-D example, we had 2MB/s and 600 KiB/s respectively, so we save more bandwidth when depth images are not used.