From 6fe89ac67bab67eaa2bf2808b4a0a794fc3cf9b2 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@f169173b-cf89-36c8-b27e-44dbe73f0c83>
Date: Tue, 15 Jul 2014 13:30:46 +0000
Subject: [PATCH] added az3_remote_mapping launch files

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1563 f169173b-cf89-36c8-b27e-44dbe73f0c83
---
 .../azimut3/az3_remote_mapping_robot.launch   | 38 ++++++++
 launch/azimut3/az3_remote_mapping_rviz.launch | 87 +++++++++++++++++++
 2 files changed, 125 insertions(+)
 create mode 100644 launch/azimut3/az3_remote_mapping_robot.launch
 create mode 100644 launch/azimut3/az3_remote_mapping_rviz.launch

diff --git a/launch/azimut3/az3_remote_mapping_robot.launch b/launch/azimut3/az3_remote_mapping_robot.launch
new file mode 100644
index 00000000..52bb6b79
--- /dev/null
+++ b/launch/azimut3/az3_remote_mapping_robot.launch
@@ -0,0 +1,38 @@
+
+<launch>
+
+  <!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
+  <include file="$(find az3_bringup)/az3_standalone.launch"/>
+  <!-- <include file="$(find az3_bringup)/joystick.launch"/> -->
+  
+  <!-- OpenNI -->
+  <!-- Xtion -->
+  <include file="$(find openni2_launch)/launch/openni2.launch">
+    <arg name="depth_registration" value="True" />
+    <arg name="rgb_camera_info_url"
+        value="file://$(find rtabmap)/launch/calibration/rgb_PS1080_PrimeSense.yaml" />
+   <arg name="depth_camera_info_url"
+        value="file://$(find rtabmap)/launch/calibration/depth_PS1080_PrimeSense.yaml" />
+  </include>
+
+  <!-- Xtion frame -->
+  <node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
+      args="0.077 0.067 0.185 0.0 0.0 0.0 /base_link /camera_link 100" />
+
+  <!-- Throttling messages -->
+  <group ns="camera">
+    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap/data_throttle camera_nodelet_manager" output="screen">
+      <param name="max_rate" type="double" value="5.0"/>
+   
+      <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="depth_registered/camera_info"/>
+    
+      <remap from="rgb/image_out"       to="data_throttled_image"/>
+      <remap from="depth/image_out"     to="data_throttled_image_depth"/>
+      <remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
+    </node>  
+  </group>  
+
+  <!-- SLAM is done on client side...-->
+</launch>
diff --git a/launch/azimut3/az3_remote_mapping_rviz.launch b/launch/azimut3/az3_remote_mapping_rviz.launch
new file mode 100644
index 00000000..ea458654
--- /dev/null
+++ b/launch/azimut3/az3_remote_mapping_rviz.launch
@@ -0,0 +1,87 @@
+
+<launch>
+
+  <!-- Remote teleop -->
+  <include file="$(find az3_bringup)/joystick.launch"/> 
+  
+  <!-- Visualization and SLAM nodes use same data, so just subscribe once and relay messages -->
+  <node name="odom_relay" type="relay" pkg="topic_tools" args="/base_controller/odom /base_controller/odom_relay" >
+    <param name="lazy" type="bool" value="false"/>
+  </node>
+  <node name="scan_relay" type="relay" pkg="topic_tools" args="/base_scan /base_scan_relay" >
+    <param name="lazy" type="bool" value="false"/>
+  </node>
+  <node name="camera_info_relay" type="relay" pkg="topic_tools" args="/camera/data_throttled_camera_info /camera/data_throttled_camera_info_relay" >
+    <param name="lazy" type="bool" value="false"/>
+  </node>
+  <node name="republish_rgb" type="republish" pkg="image_transport" args="theora in:=/camera/data_throttled_image raw out:=/camera/data_throttled_image_relay" />
+  <node name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=/camera/data_throttled_image_depth raw out:=/camera/data_throttled_image_depth_relay" />
+  
+  <!-- SLAM client side -->
+  <!-- args: "delete_db_on_start" and "udebug" -->
+  <group ns="rtabmap">
+    <node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
+	  <param name="frame_id" type="string" value="base_footprint"/>
+	
+	  <param name="subscribe_depth" type="bool" value="true"/>
+	  <param name="subscribe_laserScan" type="bool" value="true"/>
+	
+	  <remap from="odom" to="/base_controller/odom_relay"/>
+	  <remap from="scan" to="/base_scan_relay"/>
+
+	  <remap from="rgb/image" to="/camera/data_throttled_image_relay"/>
+  	  <remap from="depth/image" to="/camera/data_throttled_image_depth_relay"/>
+  	  <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
+	
+	  <param name="queue_size" type="int" value="10"/>
+	
+	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
+	  <param name="RGBD/ScanMatchingSize" type="string" value="1"/>             <!-- Do odometry correction with consecutive laser scans -->
+	  <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>   <!-- Local loop closure detection (using estimated position) with locations in WM -->
+	  <param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>    <!-- Local loop closure detection with locations in STM -->
+	  <param name="Mem/BadSignaturesIgnored" type="string" value="false"/>      <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
+	  <param name="LccIcp/Type" type="string" value="2"/>                       <!-- Loop closure transformation refining with ICP: 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
+	  <param name="LccIcp2/Iterations" type="string" value="100"/>
+	  <param name="LccIcp2/VoxelSize" type="string" value="0"/>
+	  <param name="LccBow/MinInliers" type="string" value="5"/>                 <!-- 3D visual words minimum inliers to accept loop closure -->
+	  <param name="LccBow/MaxDepth" type="string" value="4.0"/>                 <!-- 3D visual words maximum depth 0=infinity -->
+	  <param name="LccBow/InlierDistance" type="string" value="0.1"/>           <!-- 3D visual words correspondence distance -->
+	  <param name="RGBD/AngularUpdate" type="string" value="0.01"/>             <!-- Update map only if the robot is moving -->
+	  <param name="RGBD/LinearUpdate" type="string" value="0.01"/>              <!-- Update map only if the robot is moving -->
+	  <param name="Rtabmap/TimeThr" type="string" value="700"/>
+	  <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
+	  <param name="Mem/RehearsedNodesKept" type="string" value="false"/>   
+    </node>
+    
+    <!-- Grid map assembler for rviz -->
+    <node pkg="rtabmap" type="grid_map_assembler" name="grid_map_assembler" output="screen"/>
+  </group>
+   
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap)/launch/config/azimut3_remote.rviz"/>
+  
+  <!-- Below, construct point cloud of the latest throttled data -->
+  <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
+  <node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap/data_odom_sync standalone_nodelet">
+    <remap from="rgb/image_in"       to="/camera/data_throttled_image_relay"/>
+    <remap from="depth/image_in"     to="/camera/data_throttled_image_depth_relay"/>
+    <remap from="rgb/camera_info_in" to="/camera/data_throttled_camera_info_relay"/>
+    <remap from="odom_in"             to="/base_controller/odom_relay"/>
+    
+    <remap from="rgb/image_out"       to="data_odom_sync/image"/>
+    <remap from="depth/image_out"     to="data_odom_sync/depth"/>
+    <remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
+    <remap from="odom_out"            to="odom_sync"/>
+    
+    <param name="queue_size" type="int" value="30"/>
+  </node>
+  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/point_cloud_xyzrgb standalone_nodelet">
+    <remap from="rgb/image"       to="data_odom_sync/image"/>
+    <remap from="depth/image"     to="data_odom_sync/depth"/>
+    <remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
+    <remap from="cloud"           to="voxel_cloud" />
+    
+    <param name="queue_size" type="int" value="10"/>
+    <param name="voxel_size" type="double" value="0.01"/>
+  </node>
+
+</launch>