Added test_two_kinects_one_map.launch. Removed tf_prefix parameter (frame_id, odom_frame_id and map_frame_id should be set directly)

launch/rtabmap.launch

@@ -30,11 +30,14 @@
   <arg name="rviz_cfg"                default="$(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="odom_frame_id"           default=""/>                <!-- If set, TF is used to get odometry instead of the topic -->
+  <arg name="map_frame_id"            default="map"/>
   <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.2"/>
-  <arg name="rtabmap_args"            default=""/>              <!-- delete_db_on_start, udebug -->
+  <arg name="args"                    default=""/>              <!-- delete_db_on_start, udebug -->
+  <arg name="rtabmap_args"            default="$(arg args)"/>   <!-- deprecated, use "args" argument -->
   <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
   
   <!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
@@ -65,7 +68,7 @@
    
   <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 -->
-  <arg name="odom_frame_id"            default=""/>              <!-- If set, TF is used to get odometry instead of the topic -->
+  <arg name="vo_frame_id"              default="odom"/>
   <arg name="odom_tf_angular_variance" default="1"/>             <!-- If TF is used to get odometry, this is the default angular variance -->
   <arg name="odom_tf_linear_variance"  default="1"/>             <!-- If TF is used to get odometry, this is the default linear variance -->
   <arg name="odom_args"                default="$(arg rtabmap_args)"/>
@@ -98,6 +101,7 @@
         <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
 	  
         <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
+        <param name="odom_frame_id"               type="string" value="$(arg vo_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)"/>
@@ -117,6 +121,7 @@
         <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
 	  
         <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
+        <param name="odom_frame_id"               type="string" value="$(arg vo_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)"/>
@@ -134,6 +139,7 @@
       <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
       <param name="subscribe_user_data"  type="bool"   value="$(arg subscribe_user_data)"/>
       <param name="frame_id"             type="string" value="$(arg frame_id)"/>
+      <param name="map_frame_id"         type="string" value="$(arg map_frame_id)"/>
       <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
       <param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
       <param name="odom_tf_linear_variance"  type="double" value="$(arg odom_tf_linear_variance)"/>

launch/tests/test_two_kinects_one_map.launch

@@ -0,0 +1,62 @@
+
+<launch>
+   
+  <!-- Testing 2 Kinects localizing in the same map at the same time -->
+  <!-- Prerequisities: the default ~/.ros/rtabmap.db should be 
+                       already created with one of the Kinect using mapping tutorial:
+                       http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping 
+              $ roslaunch freenect_launch freenect.launch depth_registration:=true
+              $ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="-.-delete_db_on_start"
+  -->
+  
+   
+  <!-- Cameras -->
+  <include file="$(find freenect_launch)/launch/freenect.launch">
+    <arg name="depth_registration" value="True" />
+    <arg name="camera" value="camera1" />
+    <arg name="device_id" value="#1" />
+  </include>
+
+  <include file="$(find freenect_launch)/launch/freenect.launch">
+    <arg name="depth_registration" value="True" />
+    <arg name="camera" value="camera2" />
+    <arg name="device_id" value="#2" />
+  </include>
+
+  <node pkg="tf" type="static_transform_publisher" name="world_to_map1"
+        args="0 0 0 0 0 0 world map1 100" />  
+  <node pkg="tf" type="static_transform_publisher" name="world_to_map2"
+        args="0 0 0 0 0 0 world map2 100" />  
+  
+  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+     <arg name="namespace"         value="rtabmap1"/>
+     <arg name="localization"      value="true" />
+     <arg name="frame_id"          value="camera1_link" />
+     <arg name="map_frame_id"      value="map1" />
+     <arg name="vo_frame_id"       value="odom1" />
+
+     <arg name="rgb_topic"         value="/camera1/rgb/image_rect_color" />
+     <arg name="depth_topic"       value="/camera1/depth_registered/image_raw" />
+     <arg name="camera_info_topic" value="/camera1/rgb/camera_info" />
+
+     <arg name="rtabmapviz" value="false"/>
+  </include>
+
+  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+     <arg name="namespace"         value="rtabmap2"/>
+     <arg name="localization"      value="true" />
+     <arg name="frame_id"          value="camera2_link" />
+     <arg name="map_frame_id"      value="map2" />
+     <arg name="vo_frame_id"       value="odom2" />
+
+     <arg name="rgb_topic"         value="/camera2/rgb/image_rect_color" />
+     <arg name="depth_topic"       value="/camera2/depth_registered/image_raw" />
+     <arg name="camera_info_topic" value="/camera2/rgb/camera_info" />
+
+     <arg name="rtabmapviz" value="false"/>
+  </include>
+
+  <!-- Visualization RVIZ -->
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
+
+</launch>

src/CoreWrapper.cpp

@@ -126,7 +126,6 @@ void CoreWrapper::onInit()
 	bool publishTf = true;
 	double tfDelay = 0.05; // 20 Hz
 	double tfTolerance = 0.1; // 100 ms
-	std::string tfPrefix = "";
 
 	pnh.param("config_path",         configPath_, configPath_);
 	pnh.param("database_path",       databasePath_, databasePath_);
@@ -145,7 +144,10 @@ void CoreWrapper::onInit()
 
 	pnh.param("publish_tf",          publishTf, publishTf);
 	pnh.param("tf_delay",            tfDelay, tfDelay);
-	pnh.param("tf_prefix",           tfPrefix, tfPrefix);
+	if(pnh.hasParam("tf_prefix"))
+	{
+		ROS_ERROR("tf_prefix parameter has been removed, use directly map_frame_id, odom_frame_id and frame_id parameters.");
+	}
 	pnh.param("tf_tolerance",        tfTolerance, tfTolerance);
 	pnh.param("odom_tf_angular_variance", odomDefaultAngVariance_, odomDefaultAngVariance_);
 	pnh.param("odom_tf_linear_variance", odomDefaultLinVariance_, odomDefaultLinVariance_);
@@ -166,31 +168,6 @@ void CoreWrapper::onInit()
 				"switches scan values.");
 	}
 
-	if(!tfPrefix.empty())
-	{
-		if(!frameId_.empty())
-		{
-			frameId_ = tfPrefix+"/"+frameId_;
-		}
-		if(!mapFrameId_.empty())
-		{
-			mapFrameId_ = tfPrefix+"/"+mapFrameId_;
-		}
-		if(!odomFrameId_.empty())
-		{
-			odomFrameId_ = tfPrefix+"/"+odomFrameId_;
-		}
-		if(!groundTruthFrameId_.empty())
-		{
-			groundTruthFrameId_ = tfPrefix+"/"+groundTruthFrameId_;
-		}
-		if(!groundTruthBaseFrameId_.empty())
-		{
-			groundTruthBaseFrameId_ = tfPrefix+"/"+groundTruthBaseFrameId_;
-		}
-		// keep worldFrameId_ without prefix as it should be global
-	}
-
 	NODELET_INFO("rtabmap: frame_id      = %s", frameId_.c_str());
 	if(!odomFrameId_.empty())
 	{

src/OdometryROS.cpp

@@ -112,12 +112,14 @@ void OdometryROS::onInit()
 
 	Transform initialPose = Transform::getIdentity();
 	std::string initialPoseStr;
-	std::string tfPrefix;
 	std::string configPath;
 	pnh.param("frame_id", frameId_, frameId_);
 	pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
 	pnh.param("publish_tf", publishTf_, publishTf_);
-	pnh.param("tf_prefix", tfPrefix, tfPrefix);
+	if(pnh.hasParam("tf_prefix"))
+	{
+		ROS_ERROR("tf_prefix parameter has been removed, use directly odom_frame_id and frame_id parameters.");
+	}
 	pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
 	pnh.param("wait_for_transform_duration",  waitForTransformDuration_, waitForTransformDuration_);
 	pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw"
@@ -141,22 +143,6 @@ void OdometryROS::onInit()
 		configPath = UDirectory::currentDir(true) + configPath;
 	}
 
-	if(!tfPrefix.empty())
-	{
-		if(!frameId_.empty())
-		{
-			frameId_ = tfPrefix + "/" + frameId_;
-		}
-		if(!odomFrameId_.empty())
-		{
-			odomFrameId_ = tfPrefix + "/" + odomFrameId_;
-		}
-		if(!groundTruthFrameId_.empty())
-		{
-			groundTruthFrameId_ = tfPrefix + "/" + groundTruthFrameId_;
-		}
-	}
-
 	if(initialPoseStr.size())
 	{
 		std::vector<std::string> values = uListToVector(uSplit(initialPoseStr, ' '));