Added example of using global_pose topic (edge prior, https://github.com/introlab/rtabmap/pull/196)

launch/tests/test_prior.launch

@@ -0,0 +1,82 @@
+
+<launch>
+   
+   <!-- Example with rgbd datasets:
+   $ wget http://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.bag
+   $ rosbag decompress rgbd_dataset_freiburg3_long_office_household.bag
+   $ chmod +x test_prior_rename_kinect_bag_tf.py
+   $ ./test_prior_rename_kinect_bag_tf.py
+
+   We simulate an external "global_pose" by republishing ground truth TF (VICON) with some 
+   covariance, normally you should not have to use test_prior_tf_to_pose.py as the node 
+   publishing the global pose would give it directly as a pose with correct covariance.
+   
+   Rename all child_frame_id "/kinect" to "/kinect_gt" Tf in the bag 
+   using test_prior_rename_kinect_bag_tf.py in this directory!
+   
+   $ roslaunch rtabmap_ros test_prior.launch 
+   $ chmod +x test_prior_tf_to_pose.py
+   $ ./test_prior_tf_to_pose.py
+   $ rosbag play -.-clock rgbd_dataset_freiburg3_long_office_household_tf_renamed.bag
+   -->
+   
+   <param name="use_sim_time" type="bool" value="True"/>
+     
+   <!-- Choose visualization -->
+   <arg name="rviz" default="true" />
+   <arg name="rtabmapviz" default="false" /> 
+   
+   <!-- TF FRAMES -->
+  <node pkg="tf" type="static_transform_publisher" name="world_to_map" 
+    args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 100" />
+    
+  <group ns="rtabmap">
+  
+    <!-- Odometry -->
+    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
+      <remap from="rgb/image"       to="/camera/rgb/image_color"/>
+      <remap from="depth/image"     to="/camera/depth/image"/>
+      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
+      <remap from="odom" to="vis_odom"/>
+	  
+      <param name="odom_frame_id" type="string" value="vis_odom"/>
+      <param name="frame_id" type="string" value="kinect"/>
+    </node>
+
+    <!-- Visual SLAM -->
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
+      <param name="subscribe_depth" type="bool" value="true"/>
+      <param name="frame_id" type="string" value="kinect"/>
+      
+      <param name="RGBD/CreateOccupancyGrid" type="string" value="false"/>
+      <param name="Optimizer/PriorsIgnored" type="string" value="false"/>
+	
+      <remap from="rgb/image" to="/camera/rgb/image_color"/>
+      <remap from="depth/image" to="/camera/depth/image"/>
+      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
+      <remap from="odom" to="vis_odom"/>
+      <remap from="global_pose" to="/global_pose"/>
+      <param name="ground_truth_frame_id" type="string" value="world"/>
+      <param name="ground_truth_base_frame_id" type="string" value="kinect_gt"/>
+    </node>
+    
+    <!-- Visualisation  -->
+    <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_laserScan" type="bool" value="false"/>
+      <param name="subscribe_odom_info" type="bool" value="true"/>
+      <param name="queue_size" type="int" value="30"/>
+      
+      <param name="frame_id" type="string" value="kinect"/>
+    
+      <remap from="rgb/image" to="/camera/rgb/image_color"/>
+      <remap from="depth/image" to="/camera/depth/image"/>
+      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
+      <remap from="odom" to="vis_odom"/>
+    </node>
+        
+  </group>
+  
+  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbdslam_datasets.rviz"/>
+
+</launch>

launch/tests/test_prior_rename_kinect_bag_tf.py

@@ -0,0 +1,19 @@
+#!/usr/bin/env python 
+import rosbag
+from tf.msg import tfMessage
+with rosbag.Bag('rgbd_dataset_freiburg3_long_office_household_tf_renamed.bag', 'w') as outbag:
+	for topic, msg, t in rosbag.Bag('rgbd_dataset_freiburg3_long_office_household.bag').read_messages():
+		if topic == "/tf" and msg.transforms:
+			newList = [];
+			for m in msg.transforms:
+				if m.child_frame_id != "/kinect":
+					newList.append(m)
+				else:
+					m.child_frame_id = "/kinect_gt"
+					newList.append(m)
+					print 'kinect frame renamed!'
+			if len(newList)>0:
+				msg.transforms = newList
+				outbag.write(topic, msg, t)
+		else:
+			outbag.write(topic, msg, t)

launch/tests/test_prior_tf_to_pose.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python  
+import rospy
+import tf
+import numpy
+import tf2_ros
+from geometry_msgs.msg import PoseWithCovarianceStamped
+
+if __name__ == '__main__':
+    rospy.init_node('tf_to_pose', anonymous=True)
+    listener = tf.TransformListener()
+    frame = rospy.get_param('~frame', 'world')
+    childFrame = rospy.get_param('~child_frame', 'kinect_gt')
+    outputFrame = rospy.get_param('~output_frame', 'kinect')
+    cov = rospy.get_param('~cov', 1)
+    rateParam = rospy.get_param('~rate', 30) # 10hz
+    pub = rospy.Publisher('global_pose', PoseWithCovarianceStamped, queue_size=1)
+   
+    print 'start loop!'
+    rate = rospy.Rate(rateParam) 
+    while not rospy.is_shutdown():
+        poseOut = PoseWithCovarianceStamped()
+        try:
+            now = rospy.get_rostime()
+            listener.waitForTransform(frame, childFrame, now, rospy.Duration(0.033))
+            (trans,rot) = listener.lookupTransform(frame, childFrame, now)
+        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf2_ros.TransformException), e:
+            print str(e)
+            rate.sleep()
+            continue
+        
+        poseOut.header.stamp.nsecs = now.nsecs
+        poseOut.header.stamp.secs = now.secs
+        poseOut.header.frame_id = outputFrame
+        poseOut.pose.pose.position.x = trans[0]
+        poseOut.pose.pose.position.y = trans[1]
+        poseOut.pose.pose.position.z = trans[2]
+        poseOut.pose.pose.orientation.x = rot[0]
+        poseOut.pose.pose.orientation.y = rot[1]
+        poseOut.pose.pose.orientation.z = rot[2]
+        poseOut.pose.pose.orientation.w = rot[3]
+        poseOut.pose.covariance = (cov * numpy.eye(6, dtype=numpy.float64)).tolist()
+        poseOut.pose.covariance = [item for sublist in poseOut.pose.covariance for item in sublist]
+
+        print str(poseOut)
+        pub.publish(poseOut)
+        rate.sleep()