From 64085e146e2d371a2fab0740c139846993b44886 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@f169173b-cf89-36c8-b27e-44dbe73f0c83>
Date: Tue, 21 Oct 2014 20:51:24 +0000
Subject: [PATCH] ros-pkg: added az3_mapping_robot_stereo_nav.launch

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1912 f169173b-cf89-36c8-b27e-44dbe73f0c83
---
 .../az3_mapping_robot_stereo_nav.launch       | 157 ++++++++++++++++++
 1 file changed, 157 insertions(+)
 create mode 100644 launch/azimut3/az3_mapping_robot_stereo_nav.launch

diff --git a/launch/azimut3/az3_mapping_robot_stereo_nav.launch b/launch/azimut3/az3_mapping_robot_stereo_nav.launch
new file mode 100644
index 00000000..2b8ac52e
--- /dev/null
+++ b/launch/azimut3/az3_mapping_robot_stereo_nav.launch
@@ -0,0 +1,157 @@
+
+<launch>
+    <!-- "Disable" wheel odometry from azimut3 -->
+    <group ns="base_controller">
+      <param name="odom_frame_id" type="string" value="az3_odom"/>
+      <param name="base_frame_id" type="string" value="az3_base_link"/>
+    </group>
+    <remap from="/base_controller/odom" to="/base_controller/az3_odom"/>
+
+	<!-- AZIMUT 3 bringup: launch motors and TF -->
+	<include file="$(find az3_bringup)/az3_standalone.launch"/>
+	
+	<node name="joy" pkg="joy" type="joy_node"/>
+
+    <group ns="teleop">
+        <remap from="joy" to="/joy"/>
+        <node name="teleop" pkg="nodelet" type="nodelet" 
+            args="standalone azimut_tools/Teleop"/>
+        <param name="cmd_eta/abtr_priority" value="50"/>
+    </group>
+
+    <group ns="go_back">
+        <remap from="joy"  to="/joy"/>
+        <remap from="goal" to="/planner_goal"/>
+        <node name="go_back" pkg="azimut_tools" type="go_back"/>
+        <rosparam>
+            btn_save: 1
+            btn_goto: 3
+        </rosparam>
+    </group>
+
+    <group ns="planner">
+        <remap from="openni_points" to="/planner_cloud"/>
+        <remap from="base_scan" to="/base_scan"/>
+        <remap from="map" to="/map"/>
+        <remap from="move_base_simple/goal" to="/planner_goal"/>
+        <include file="$(find az3_navigation)/launch/az3_move_base_slam.launch"/>
+        <param name="cmd_vel/abtr_priority" value="10"/>
+    </group>
+
+    <node name="az3_abtr" pkg="azimut_tools" type="azimut_abtr_priority_node">
+        <remap from="abtr_cmd_eta" to="/base_controller/cmd_eta"/>
+    </node>
+
+    <node name="register_cmd_eta" pkg="abtr_priority" type="register"
+        args="/cmd_eta /teleop/cmd_eta"/>
+    <node name="register_cmd_vel" pkg="abtr_priority" type="register"
+        args="/cmd_vel /planner/cmd_vel"/>
+  
+	<!-- Stereo camera -->
+	<node pkg="camera1394stereo" type="camera1394stereo_node" name="camera1394stereo_node" output="screen" >
+	    <param name="video_mode" value="format7_mode3" />
+	    <param name="format7_color_coding" value="raw16" />
+	    <param name="bayer_pattern" value="bggr" />
+	    <param name="bayer_method" value="" />
+	    <param name="stereo_method" value="Interlaced" />
+	    <param name="camera_info_url_left" value="" />
+	    <param name="camera_info_url_right" value="" />
+	</node>
+  
+  	<!-- TF transforms for the stereo camera -->
+  	<arg name="pi/2" value="1.5707963267948966" />
+	<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
+	<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
+	    args="$(arg optical_rotate) stereo_camera_base stereo_camera 100" />  
+	<node pkg="tf" type="static_transform_publisher" name="base_to_camera_base_link"
+	    args="0.01 0.06 0.90 0 0.2 0 base_link stereo_camera_base 100" />  
+	    
+	<!-- Run the ROS package stereo_image_proc for image rectification-->
+	<group ns="/stereo_camera" >
+		<!-- HACK: the fps parameter on camera1394stereo_node doesn't work for my camera!?!?
+			 Throttle camera images to 10 Hz () -->
+		<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap/stereo_throttle">
+			<remap from="left/image"       to="left/image_raw"/>
+		    <remap from="right/image"      to="right/image_raw"/>
+		    <remap from="left/camera_info" to="left/camera_info"/>
+		    <remap from="right/camera_info" to="right/camera_info"/>
+		    
+		    <param name="queue_size" type="int" value="10"/>
+		    <param name="rate" type="double" value="10"/>
+		</node>
+	
+		<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
+			<remap from="left/image_raw" to="left/image_raw_throttle"/>
+			<remap from="left/camera_info" to="left/camera_info_throttle"/>
+			<remap from="right/image_raw" to="right/image_raw_throttle"/>
+			<remap from="right/camera_info" to="right/camera_info_throttle"/>
+		</node>
+	</group>
+	
+	<!-- Visual Odometry -->
+	<node pkg="viso2_ros" type="stereo_odometer" name="stereo_odometer" output="screen">
+	  <remap from="stereo" to="/stereo_camera"/>
+	  <remap from="image" to="image_rect"/>
+	  <remap from="left/camera_info" to="left/camera_info_throttle" />
+	  <remap from="right/camera_info" to="right/camera_info_throttle" />
+	    
+	  <param name="base_link_frame_id" value="base_footprint"/>
+	  <param name="odom_frame_id" value="odom"/>
+	  <param name="ref_frame_change_method" value="1"/>
+	</node>
+
+  <!-- SLAM (robot 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_stereo" type="bool" value="true"/>
+	  <param name="subscribe_depth" type="bool" value="false"/>
+
+	  <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
+ 	  <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
+    	  <remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
+   	  <remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
+      	
+	  <remap from="odom" to="/stereo_odometer/odometry"/>
+	  <remap from="mapData" to="mapData_notOptimized"/>
+	
+	  <param name="queue_size" type="int" value="30"/>
+	
+	  <!-- RTAB-Map's parameters -->
+	  <param name="Rtabmap/TimeThr" type="string" value="700"/>
+   	  <param name="Rtabmap/DetectionRate" type="string" value="1"/>
+ 	  <param name="RGBD/ToroIterations" type="string" value="0"/> <!-- Disable graph optimization for map_optimizer node below -->
+	 
+	  <param name="Kp/TfIdfLikelihoodUsed" type="string" value="true"/>
+	  <param name="Kp/DetectorStrategy" type="string" value="0"/>
+    	  <param name="Kp/WordsPerImage" type="string" value="200"/>
+    	  <param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
+    	  <param name="Kp/NNStrategy" type="string" value="1"/>
+	  
+	  <param name="Bayes/FullPredictionUpdate" type="string" value="false"/>
+	  
+	  <param name="SURF/HessianThreshold" type="string" value="1000"/>
+	  
+    	  <param name="LccBow/MaxDepth" type="string" value="3"/>
+    	  <param name="LccBow/MinInliers" type="string" value="10"/>
+    	  <param name="LccBow/InlierDistance" type="string" value="0.02"/>
+      
+    	  <param name="LccReextract/Activated" type="string" value="true"/>
+    	  <param name="LccReextract/FeatureType" type="string" value="4"/>
+	  <param name="LccReextract/MaxWords" type="string" value="400"/>
+	  <param name="LccReextract/NNDR" type="string" value="0.8"/>
+	  <param name="LccReextract/NNType" type="string" value="3"/>   
+    </node>
+    
+    <node pkg="rtabmap" type="map_optimizer" name="map_optimizer">
+   	  <remap from="mapData" to="mapData_notOptimized"/>
+	  <remap from="mapData_notOptimized_optimized" to="mapData"/> 
+    </node>
+    <node pkg="rtabmap" type="map_assembler" name="map_assembler">
+    	  <param name="occupancy_grid" type="bool" value="true"/>
+    </node>
+ 	
+  </group>
+  
+</launch>