From 144a7f1e94100130d28fe28f6bda5624561fb993 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Wed, 28 Oct 2015 20:05:04 -0400
Subject: [PATCH] Added rtabmap.launch

---
 launch/rtabmap.launch | 166 ++++++++++++++++++++++++++++++++++++++++++
 src/CoreWrapper.cpp   |   2 +-
 src/GuiWrapper.cpp    |   4 +-
 3 files changed, 170 insertions(+), 2 deletions(-)
 create mode 100644 launch/rtabmap.launch

diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch
new file mode 100644
index 00000000..3b224d57
--- /dev/null
+++ b/launch/rtabmap.launch
@@ -0,0 +1,166 @@
+
+<launch>
+  <!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->
+
+  <!-- For rgbd:=true
+        Your RGB-D sensor should be already started with "depth_registration:=true".
+        Examples:
+           $ roslaunch freenect_launch freenect.launch depth_registration:=true 
+           $ roslaunch openni2_launch openni2.launch depth_registration:=true -->
+           
+  <!-- For stereo:=true
+        Your camera should be calibrated and publishing rectified left and right 
+        images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
+        Example:
+           $ roslaunch rtabmap_ros bumblebee.launch -->
+     
+  <!-- Choose between RGB-D and stereo -->      
+  <arg name="rgbd"            default="true"/>
+  <arg name="stereo"          default="false"/>
+ 
+  <!-- Choose visualization -->
+  <arg name="rtabmapviz"              default="true" /> 
+  <arg name="rviz"                    default="false" />
+  
+  <!-- Corresponding config files -->
+  <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
+  <arg name="rviz_cfg"                default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
+  
+  <arg name="frame_id"                default="camera_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
+  <arg name="namespace"               default="rtabmap"/>
+  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
+  <arg name="queue_size"              default="10"/>
+  <arg name="wait_for_transform"      default="0.1"/>
+  <arg name="rtabmap_args"            default=""/>              <!-- delete_db_on_start, udebug -->
+  <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
+  
+  <!-- RGB-D related topics -->
+  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
+  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
+  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
+  
+  <!-- stereo related topics -->
+  <arg name="stereo_namespace"        default="/stereo_camera"/>
+  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
+  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />      <!-- using grayscale image for efficiency -->
+  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
+  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
+  <arg name="approx_sync"             default="false"/>         <!-- if timestamps of the stereo images are not synchronized -->
+  
+  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
+   
+  <arg name="subscribe_scan"          default="false"/>
+  <arg name="scan_topic"              default="/scan"/>
+   
+  <arg name="visual_odometry"         default="true"/>          <!-- Launch rtabmap visual odometry node -->
+  <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
+  
+  <!-- Nodes -->
+  <group ns="$(arg namespace)">
+  
+    <!-- RGB-D Odometry -->
+    <group if="$(arg rgbd)">
+      <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="compressed in:=$(arg rgb_topic) raw out:=$(arg rgb_topic)" />
+      <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=$(arg depth_registered_topic) raw out:=$(arg depth_registered_topic)" />
+  
+      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" launch-prefix="$(arg launch_prefix)">
+        <remap from="rgb/image"       to="$(arg rgb_topic)"/>
+        <remap from="depth/image"     to="$(arg depth_topic)"/>
+        <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
+	  
+        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
+        <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
+        <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
+        <param name="config_path"                 type="string" value="$(arg cfg)"/>
+        <param name="queue_size"                  type="int" value="$(arg queue_size)"/>
+      </node>
+    </group>
+    
+    <!-- Stereo Odometry -->
+    <group if="$(arg stereo)">
+      <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="compressed in:=$(arg left_image_topic) raw out:=$(arg left_image_topic)" />
+      <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic)" />
+  
+      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
+        <remap from="left/image_rect"        to="$(arg left_image_topic)"/>
+        <remap from="right/image_rect"       to="$(arg right_image_topic)"/>
+        <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
+        <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
+	  
+        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
+        <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
+        <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
+        <param name="config_path"                 type="string" value="$(arg cfg)"/>
+        <param name="queue_size"                  type="int" value="$(arg queue_size)"/>
+      </node>
+    </group>
+  
+    <!-- Visual SLAM (robot side) -->
+    <!-- args: "delete_db_on_start" and "udebug" -->
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
+      <param name="subscribe_depth"     type="bool"   value="$(arg rgbd)"/>
+      <param name="subscribe_stereo"    type="bool"   value="$(arg stereo)"/>
+      <param name="subscribe_laserScan" type="bool"   value="$(arg subscribe_scan)"/>
+      <param name="frame_id"            type="string" value="$(arg frame_id)"/>
+      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
+      <param name="database_path"       type="string" value="$(arg database_path)"/>
+      <param name="stereo_approx_sync"  type="bool"   value="$(arg approx_sync)"/>
+      <param name="config_path"          type="string" value="$(arg cfg)"/>
+      <param name="queue_size"                  type="int" value="$(arg queue_size)"/>
+      
+      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
+      <remap from="depth/image"     to="$(arg depth_topic)"/>
+      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
+	
+      <remap from="left/image_rect"        to="$(arg left_image_topic)"/>
+      <remap from="right/image_rect"       to="$(arg right_image_topic)"/>
+      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
+      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
+      
+      <remap from="scan"                   to="$(arg scan_topic)"/>
+      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
+    </node>
+  
+    <!-- Visualisation RTAB-Map -->
+    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg cfg)" output="screen">
+      <param name="subscribe_depth"      type="bool"   value="$(arg rgbd)"/>
+      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
+      <param name="subscribe_laserScan"  type="bool"   value="$(arg subscribe_scan)"/>
+      <param name="subscribe_odom_info"  type="bool"   value="$(arg visual_odometry)"/>
+      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
+      <param name="wait_for_transform_duration"   type="double"   value="$(arg wait_for_transform)"/>
+      <param name="queue_size"                  type="int" value="$(arg queue_size)"/>
+    
+      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
+      <remap from="depth/image"     to="$(arg depth_topic)"/>
+      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
+      
+      <remap from="left/image_rect"        to="$(arg left_image_topic)"/>
+      <remap from="right/image_rect"       to="$(arg right_image_topic)"/>
+      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
+      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
+      
+      <remap from="scan"                   to="$(arg scan_topic)"/>
+      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
+    </node>
+  
+  </group>
+  
+  <!-- Visualization RVIZ -->
+  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
+  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
+    <remap from="left/image"        to="$(arg left_image_topic)"/>
+    <remap from="right/image"       to="$(arg right_image_topic)"/>
+    <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
+    <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
+    <remap from="rgb/image"         to="$(arg rgb_topic)"/>
+    <remap from="depth/image"       to="$(arg depth_topic)"/>
+    <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
+    <remap from="cloud"             to="voxel_cloud" />
+
+    <param name="decimation"  type="double" value="2"/>
+    <param name="voxel_size"  type="double" value="0.02"/>
+    <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
+  </node>
+
+</launch>
diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index 37f29ce2..9d764a3e 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -124,7 +124,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	pnh.param("subscribe_stereo",    subscribeStereo, subscribeStereo);
 	if(subscribeDepth && subscribeStereo)
 	{
-		UWARN("Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
+		ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
 		subscribeDepth = false;
 	}
 	if(subscribeLaserScan)
diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp
index 216b7c7d..040e9f03 100644
--- a/src/GuiWrapper.cpp
+++ b/src/GuiWrapper.cpp
@@ -1212,7 +1212,9 @@ void GuiWrapper::setupCallbacks(
 
 	if(subscribeDepth && subscribeStereo)
 	{
-		ROS_WARN("\"subscribe_depth\" already true, ignoring \"subscribe_stereo\".");
+		ROS_WARN("rtabmapviz: Parameters subscribe_depth and subscribe_stereo cannot be true at the "
+				 "same time. Parameter subscribe_depth is set to false.");
+		subscribeDepth = false;
 	}
 	if(!subscribeDepth && !subscribeStereo && subscribeLaserScan)
 	{