From b60fc7689888473dc9c512284a81eec8931458af Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Tue, 18 Sep 2018 18:46:26 -0400
Subject: [PATCH] Updated hector demo to fix assert on wrong covariance

---
 launch/demo/demo_hector_mapping.launch | 48 +++++++++++++++-----------
 1 file changed, 28 insertions(+), 20 deletions(-)

diff --git a/launch/demo/demo_hector_mapping.launch b/launch/demo/demo_hector_mapping.launch
index 741c55bc..122f7668 100644
--- a/launch/demo/demo_hector_mapping.launch
+++ b/launch/demo/demo_hector_mapping.launch
@@ -48,8 +48,8 @@
   <!-- If argument "hector" is false, we use rtabmap's icp odometry to generate odometry for us -->
   <node unless="$(arg hector)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" >
      <remap from="scan"      to="/jn0/base_scan"/>
-	 <remap from="odom"      to="/scanmatch_odom"/>
-	 <remap from="odom_info"      to="/rtabmap/odom_info"/>
+     <remap from="odom"      to="/scanmatch_odom"/>
+     <remap from="odom_info"      to="/rtabmap/odom_info"/>
 	  
      <param name="frame_id"        type="string" value="base_footprint"/>   
      
@@ -71,44 +71,52 @@
     <!-- SLAM (robot side) -->
     <!-- args: "delete_db_on_start" and "udebug" -->
     <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
-	  <param name="frame_id" type="string" value="base_footprint"/>
+      <param name="frame_id" type="string" value="base_footprint"/>
 	
-	  <param name="subscribe_depth" type="bool" value="true"/>
-	  <param name="subscribe_scan"  type="bool" value="true"/>
+      <param name="subscribe_depth" type="bool" value="true"/>
+      <param name="subscribe_scan"  type="bool" value="true"/>
 	
-	  <remap from="odom" to="/scanmatch_odom"/>
-	  <remap from="scan" to="/jn0/base_scan"/>
+      <remap from="scan" to="/jn0/base_scan"/>
 
-	  <remap from="rgb/image"       to="/data_throttled_image"/>
-  	  <remap from="depth/image"     to="/data_throttled_image_depth"/>
-  	  <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
+      <remap from="rgb/image"       to="/data_throttled_image"/>
+      <remap from="depth/image"     to="/data_throttled_image_depth"/>
+      <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
   	
       <param name="rgb/image_transport" type="string" value="compressed"/>
       <param name="depth/image_transport" type="string" value="compressedDepth"/>
+
+      <!-- As hector doesn't provide compatible covariance in the odometry topic, don't use the topic and fix the covariance -->
+      <param if="$(arg hector)" name="odom_frame_id"            type="string" value="hector_map"/>
+      <param if="$(arg hector)" name="odom_tf_linear_variance"  type="double" value="0.0005"/>
+      <param if="$(arg hector)" name="odom_tf_angular_variance" type="double" value="0.0005"/>
+
+      <remap unless="$(arg hector)" from="odom" to="/scanmatch_odom"/>
+      <param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>
 	
-	  <!-- RTAB-Map's parameters -->
-	  <param name="Reg/Strategy"       type="string" value="1"/>    <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
-	  <param name="Vis/MaxDepth"       type="string" value="10.0"/> <!-- 3D visual words maximum depth 0=infinity -->
-	  <param name="Vis/InlierDistance" type="string" value="0.1"/>  <!-- 3D visual words correspondence distance -->
-	  <param name="Reg/Force3DoF"      type="string" value="true"/>
-	  <param name="RGBD/ProximityBySpace"      type="string" value="false"/>
+      <!-- RTAB-Map's parameters -->
+      <param name="Reg/Strategy"       type="string" value="1"/>    <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
+      <param name="Reg/Force3DoF"      type="string" value="true"/>
+      <param name="RGBD/ProximityBySpace"      type="string" value="false"/>
     </node>
     
     <!-- Visualisation RTAB-Map -->
     <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
-  	  <param name="subscribe_depth"     type="bool" value="true"/>
+      <param name="subscribe_depth"     type="bool" value="true"/>
       <param name="subscribe_laserScan" type="bool" value="true"/>
       <param name="frame_id"            type="string" value="base_footprint"/>
-      <param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>
     
       <remap from="rgb/image"       to="/data_throttled_image"/>
       <remap from="depth/image"     to="/data_throttled_image_depth"/>
       <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
       <remap from="scan"            to="/jn0/base_scan"/>
-      <remap from="odom"            to="/scanmatch_odom"/>
-      
       <param name="rgb/image_transport"   type="string" value="compressed"/>
       <param name="depth/image_transport" type="string" value="compressedDepth"/>
+
+      <!-- As hector doesn't provide compatible covariance in the odometry topic, don't use the topic and fix the covariance -->
+      <param if="$(arg hector)" name="odom_frame_id"            type="string" value="hector_map"/>
+
+      <remap unless="$(arg hector)" from="odom" to="/scanmatch_odom"/>
+      <param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>
     </node>
   
   </group>