Added rgbdslam_datasets.launch example to run RGBD SLAM datasets with RTAB-Map. Added "wait_to_transform" parameter (bool) to all nodes calling lookUpTransform()

CMakeLists.txt

@@ -176,6 +176,9 @@ target_link_libraries(data_recorder rtabmap_ros ${QT_LIBRARIES} ${Libraries})
 add_executable(data_player src/DbPlayerNode.cpp)
 target_link_libraries(data_player rtabmap_ros ${QT_LIBRARIES} ${Libraries})
 
+add_executable(odom_msg_to_tf src/OdomMsgToTFNode.cpp)
+target_link_libraries(odom_msg_to_tf rtabmap_ros ${Libraries})
+
 #############
 ## Install ##
 #############

launch/azimut3/az3_mapping_robot_stereo_nav.launch

@@ -103,6 +103,7 @@
 
 			<param name="frame_id" type="string" value="base_footprint"/>		
 			<param name="map_frame_id" type="string" value="map"/>
+			<param name="wait_for_transform" type="bool" value="true"/>
 			<param name="min_cluster_size" type="int" value="20"/>
 			<param name="max_obstacles_height" type="double" value="0.0"/>
   		</node>

launch/config/rgbdslam_datasets.rviz

@@ -0,0 +1,291 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Info1
+        - /PointCloud23
+        - /PointCloud23/Status1
+      Splitter Ratio: 0.529595
+    Tree Height: 422
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: map
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 2.02684
+        Min Value: -1.12646
+        Value: true
+      Axis: Z
+      Channel Name: rgb
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 2.34177e-38
+      Min Color: 0; 0; 0
+      Min Intensity: 9.21942e-41
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /voxel_cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        kinect:
+          Value: true
+        kinect_est:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
+        openni_camera:
+          Value: true
+        openni_depth_frame:
+          Value: true
+        openni_depth_optical_frame:
+          Value: true
+        openni_rgb_frame:
+          Value: true
+        openni_rgb_optical_frame:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        world:
+          kinect:
+            openni_camera:
+              openni_depth_frame:
+                openni_depth_optical_frame:
+                  {}
+              openni_rgb_frame:
+                openni_rgb_optical_frame:
+                  {}
+          map:
+            odom:
+              kinect_est:
+                {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /camera/rgb/image_rect_color
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rtabmap/MapCloud
+      Cloud decimation: 4
+      Cloud max depth (m): 4
+      Cloud voxel size (m): 0.01
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Download graph: false
+      Download map: false
+      Enabled: true
+      Filter floor (m): 0
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: MapCloud
+      Node filtering angle (degrees): 15
+      Node filtering radius (m): 0.1
+      Position Transformer: XYZ
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /rtabmap/mapData
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rtabmap/Info
+      Enabled: true
+      Name: Info
+      Topic: /rtabmap/info
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /rtabmap/odom_last_frame
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 1.17984
+        Min Value: 0.0147058
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 85; 255; 127
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /rtabmap/odom_local_map
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 3.32013
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.216951
+        Y: 0.819032
+        Z: 1.30659
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.554801
+      Target Frame: base_link
+      Value: Orbit (rviz)
+      Yaw: 4.9892
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 824
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001530000031afc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000000000000235000000dd00fffffffb0000000a0049006d006100670065010000023b000000df0000001600fffffffb0000000a0049006d00610067006501000001fd0000011d0000000000000000000000010000010f00000312fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000312000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003a00000031a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1273
+  X: 316
+  Y: 95

launch/tests/rgbdslam_datasets.launch

@@ -0,0 +1,100 @@
+
+<launch>
+   
+   <!-- RGB-D MAPPING VERSION -->
+   <!-- WARNING : Database is automatically deleted on each startup -->
+   <!--           See "delete_db_on_start" option below... -->
+   
+   <param name="use_sim_time" type="bool" value="True"/>
+   
+   <!-- ODOMETRY ARGUMENTS: "strategy", "feature", "nn" and "local_map":
+        -Strategy: 0=BOW (bag-of-words) 1=Optical Flow
+        -Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
+        -Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE 
+           Set to 1 for float descriptor like SIFT/SURF                  
+           Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK      
+        -Local map size: number of unique features to keep track 
+    -->
+   <arg name="strategy" default="0" />
+   <arg name="feature" default="6" />
+   <arg name="nn" default="3" />
+   <arg name="local_map" default="1000" />
+ 
+  <!-- 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"/>
+	  
+	  <param name="Odom/Strategy" type="string" value="$(arg strategy)"/> 
+	  <param name="Odom/FeatureType" type="string" value="$(arg feature)"/>            
+      <param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
+      <param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
+      
+      <param name="frame_id" type="string" value="kinect"/>
+      <param name="publish_tf" type="bool" value="false"/>
+      <param name="queue_size" type="int" value="30"/>
+      <param name="wait_for_transform" type="bool" value="true"/>
+      <param name="ground_truth_frame_id" type="string" value="world"/>
+    </node>
+    <!-- rename the child frame of odometry -->
+    <node name="odom_msg_to_tf" pkg="rtabmap_ros" type="odom_msg_to_tf">
+      <param name="frame_id" type="string" value="kinect_est"/>
+    </node>
+    
+    <!-- Visual SLAM -->
+    <!-- args: "delete_db_on_start" and "udebug" -->
+    <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="subscribe_laserScan" type="bool" value="false"/>
+      
+      <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="odom"/>
+      
+      <param name="LccBow/MinInliers" type="string" value="10"/>
+      <param name="LccBow/InlierDistance" type="string" value="0.05"/>
+
+      <param name="queue_size" type="int" value="30"/>
+    </node>
+    
+    <!-- Visualisation  -->
+    <!--
+    <node 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="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="odom"/>
+    </node>
+    -->
+        
+  </group>
+  
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbdslam_datasets.rviz"/>
+  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap/point_cloud_xyzrgb">
+    <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="cloud"           to="voxel_cloud" />
+    
+    <param name="queue_size" type="int" value="10"/>
+    <param name="decimation" type="double" value="4"/>
+  </node>
+
+</launch>

src/CoreWrapper.cpp

@@ -67,6 +67,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 		odomFrameId_(""),
 		configPath_(""),
 		databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
+		waitForTransform_(false),
 		mapToOdom_(tf::Transform::getIdentity()),
 		depthSync_(0),
 		depthScanSync_(0),
@@ -116,6 +117,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 
 	pnh.param("publish_tf", publishTf, publishTf);
 	pnh.param("tf_delay", tfDelay, tfDelay);
+	pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
 
 	ROS_INFO("rtabmap: frame_id = %s", frameId_.c_str());
 	ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
@@ -471,6 +473,15 @@ void CoreWrapper::depthCallback(
 		Transform localTransform;
 		try
 		{
+			if(waitForTransform_)
+			{
+				if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
+				{
+					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
+					return;
+				}
+			}
+
 			tf::StampedTransform tmp;
 			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
 			localTransform = transformFromTF(tmp);
@@ -549,6 +560,20 @@ void CoreWrapper::depthScanCallback(
 		Transform localTransform;
 		try
 		{
+			if(waitForTransform_)
+			{
+				if(!tfListener_.waitForTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, ros::Duration(1)))
+				{
+					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), scanMsg->header.frame_id.c_str());
+					return;
+				}
+				if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
+				{
+					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
+					return;
+				}
+			}
+
 			tf::StampedTransform tmp;
 			tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
 			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
@@ -638,6 +663,15 @@ void CoreWrapper::stereoCallback(
 		Transform localTransform;
 		try
 		{
+			if(waitForTransform_)
+			{
+				if(!tfListener_.waitForTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, ros::Duration(1)))
+				{
+					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), leftImageMsg->header.frame_id.c_str());
+					return;
+				}
+			}
+
 			tf::StampedTransform tmp;
 			tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
 			localTransform = transformFromTF(tmp);
@@ -720,6 +754,20 @@ void CoreWrapper::stereoScanCallback(
 		Transform localTransform;
 		try
 		{
+			if(waitForTransform_)
+			{
+				if(!tfListener_.waitForTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, ros::Duration(1)))
+				{
+					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), scanMsg->header.frame_id.c_str());
+					return;
+				}
+				if(!tfListener_.waitForTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, ros::Duration(1)))
+				{
+					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), leftImageMsg->header.frame_id.c_str());
+					return;
+				}
+			}
+
 			tf::StampedTransform tmp;
 			tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
 			tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);

src/CoreWrapper.h

@@ -130,6 +130,7 @@ private:
 	std::string odomFrameId_;
 	std::string configPath_;
 	std::string databasePath_;
+	bool waitForTransform_;
 
 	tf::Transform mapToOdom_;
 	boost::mutex mapToOdomMutex_;

src/DataRecorderNode.cpp

@@ -70,6 +70,7 @@ public:
 	DataRecorderWrapper() :
 		fileName_("output.db"),
 		frameId_("base_link"),
+		waitForTransform_(false),
 		depthScanSync_(0),
 		depthSync_(0),
 		scanSync_(0),
@@ -90,6 +91,7 @@ public:
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("output_file_name", fileName_, fileName_);
 		pnh.param("frame_id", frameId_, frameId_);
+		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
 
 		setupCallbacks(subscribeOdometry, subscribeDepth, subscribeStereo, subscribeLaserScan, queueSize);
 	}
@@ -234,6 +236,15 @@ private:
 		Transform localTransform;
 		try
 		{
+			if(waitForTransform_)
+			{
+				if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
+				{
+					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
+					return;
+				}
+			}
+
 			tf::StampedTransform tmp;
 			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
 			localTransform = transformFromTF(tmp);
@@ -303,6 +314,15 @@ private:
 		Transform localTransform;
 		try
 		{
+			if(waitForTransform_)
+			{
+				if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
+				{
+					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
+					return;
+				}
+			}
+
 			tf::StampedTransform tmp;
 			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
 			localTransform = transformFromTF(tmp);
@@ -372,6 +392,16 @@ private:
 		// TF ready?
 		try
 		{
+
+			if(waitForTransform_)
+			{
+				if(!tfListener_.waitForTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, ros::Duration(1)))
+				{
+					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), scanMsg->header.frame_id.c_str());
+					return;
+				}
+			}
+
 			tf::StampedTransform tmp;
 			tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
 		}
@@ -418,6 +448,20 @@ private:
 		Transform localTransform;
 		try
 		{
+			if(waitForTransform_)
+			{
+				if(!tfListener_.waitForTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, ros::Duration(1)))
+				{
+					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), scanMsg->header.frame_id.c_str());
+					return;
+				}
+				if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
+				{
+					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
+					return;
+				}
+			}
+
 			tf::StampedTransform tmp;
 			tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
 			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
@@ -499,6 +543,15 @@ private:
 		Transform localTransform;
 		try
 		{
+			if(waitForTransform_)
+			{
+				if(!tfListener_.waitForTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, ros::Duration(1)))
+				{
+					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), leftImageMsg->header.frame_id.c_str());
+					return;
+				}
+			}
+
 			tf::StampedTransform tmp;
 			tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
 			localTransform = transformFromTF(tmp);
@@ -544,6 +597,15 @@ private:
 		Transform localTransform;
 		try
 		{
+			if(waitForTransform_)
+			{
+				if(!tfListener_.waitForTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, ros::Duration(1)))
+				{
+					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), leftImageMsg->header.frame_id.c_str());
+					return;
+				}
+			}
+
 			tf::StampedTransform tmp;
 			tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
 			localTransform = transformFromTF(tmp);
@@ -581,6 +643,7 @@ private:
 	DataRecorder recorder_;
 	std::string fileName_;
 	std::string frameId_;
+	bool waitForTransform_;
 
 	image_transport::Subscriber defaultSub_;
 	image_transport::SubscriberFilter imageSub_;

src/GuiWrapper.cpp

@@ -62,6 +62,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 		app_(0),
 		mainWindow_(0),
 		frameId_("base_link"),
+		waitForTransform_(false),
 		cameraNodeName_("")
 {
 	ros::NodeHandle nh;
@@ -103,6 +104,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 	pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
 	pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
 	pnh.param("queue_size", queueSize, queueSize);
+	pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
 	pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_); // used to pause the rtabmap/camera when pausing the process
 	this->setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize);
 
@@ -493,6 +495,15 @@ void GuiWrapper::depthCallback(
 	Transform localTransform;
 	try
 	{
+		if(waitForTransform_)
+		{
+			if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
+			{
+				ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
+				return;
+			}
+		}
+
 		tf::StampedTransform tmp;
 		tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
 		localTransform = transformFromTF(tmp);
@@ -535,6 +546,15 @@ void GuiWrapper::scanCallback(
 	// TF ready?
 	try
 	{
+		if(waitForTransform_)
+		{
+			if(!tfListener_.waitForTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, ros::Duration(1)))
+			{
+				ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), scanMsg->header.frame_id.c_str());
+				return;
+			}
+		}
+
 		tf::StampedTransform tmp;
 		tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
 	}
@@ -585,6 +605,15 @@ void GuiWrapper::depthScanCallback(
 		laser_geometry::LaserProjection projection;
 		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
 
+		if(waitForTransform_)
+		{
+			if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
+			{
+				ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
+				return;
+			}
+		}
+
 		tf::StampedTransform tmp;
 		tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
 		localTransform = transformFromTF(tmp);

src/GuiWrapper.h

@@ -95,6 +95,7 @@ private:
 
 	// odometry subscription stuffs
 	std::string frameId_;
+	bool waitForTransform_;
 	tf::TransformListener tfListener_;
 
 	message_filters::Subscriber<rtabmap_ros::InfoEx> infoExTopic_;

src/OdomMsgToTFNode.cpp

@@ -0,0 +1,90 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <rtabmap_ros/MsgConversion.h>
+
+class OdomMsgToTF
+{
+
+public:
+	OdomMsgToTF() :
+		frameId_(""),
+		odomFrameId_("")
+	{
+		ros::NodeHandle pnh("~");
+		pnh.param("frame_id", frameId_, frameId_);
+		pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
+
+		ros::NodeHandle nh;
+		odomTopic_ = nh.subscribe("odom", 1, &OdomMsgToTF::odomReceivedCallback, this);
+	}
+
+	virtual ~OdomMsgToTF(){}
+
+	void odomReceivedCallback(const nav_msgs::OdometryConstPtr & msg)
+	{
+		if(frameId_.empty())
+		{
+			frameId_ = msg->child_frame_id;
+		}
+		if(odomFrameId_.empty())
+		{
+			odomFrameId_ = msg->header.frame_id;
+		}
+		tf::StampedTransform t;
+		rtabmap::Transform pose = rtabmap::transformFromPoseMsg(msg->pose.pose);
+		if(pose.isNull())
+		{
+			ROS_WARN("Odometry received is null! Cannot send tf...");
+		}
+		else
+		{
+			rtabmap::transformToTF(pose, t);
+			tfBroadcaster_.sendTransform(tf::StampedTransform (t, msg->header.stamp, odomFrameId_, frameId_));
+		}
+	}
+
+private:
+	std::string frameId_;
+	std::string odomFrameId_;
+
+	ros::Subscriber odomTopic_;
+	tf::TransformBroadcaster tfBroadcaster_;
+};
+
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "odom_msg_to_tf");
+	OdomMsgToTF odomToTf;
+	ros::spin();
+	return 0;
+}

src/OdometryROS.cpp

@@ -51,7 +51,9 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
 	odometry_(0),
 	frameId_("base_link"),
 	odomFrameId_("odom"),
+	groundTruthFrameId_(""),
 	publishTf_(true),
+	waitForTransform_(false),
 	paused_(false)
 {
 	this->processArguments(argc, argv);
@@ -69,7 +71,9 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
 	pnh.param("frame_id", frameId_, frameId_);
 	pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
 	pnh.param("publish_tf", publishTf_, publishTf_);
+	pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
 	pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw"
+	pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
 	if(initialPoseStr.size())
 	{
 		std::vector<std::string> values = uListToVector(uSplit(initialPoseStr, ' '));
@@ -276,6 +280,36 @@ void OdometryROS::processArguments(int argc, char * argv[])
 
 Transform OdometryROS::processData(SensorData & data, const std_msgs::Header & header, int & quality)
 {
+	if(odometry_->getPose().isNull() &&
+	   !groundTruthFrameId_.empty())
+	{
+		tf::StampedTransform initialPose; // sync with the first value of the ground truth
+		try
+		{
+			if(this->waitForTransform())
+			{
+				if(!this->tfListener().waitForTransform(groundTruthFrameId_, frameId_, header.stamp, ros::Duration(1)))
+				{
+					ROS_WARN("Could not get transform from %s to %s after 1 second!", groundTruthFrameId_.c_str(), frameId_.c_str());
+					return rtabmap::Transform(); // return null
+				}
+			}
+			this->tfListener().lookupTransform(groundTruthFrameId_, frameId_, header.stamp, initialPose);
+		}
+		catch(tf::TransformException & ex)
+		{
+			ROS_WARN("%s",ex.what());
+			return rtabmap::Transform(); // return null
+		}
+		Transform pose = rtabmap::transformFromTF(initialPose);
+		ROS_INFO("Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
+				pose.prettyPrint().c_str(),
+				groundTruthFrameId_.c_str(),
+				frameId_.c_str());
+		odometry_->reset(pose);
+	}
+
+	// process data
 	rtabmap::Transform pose = odometry_->process(data, &quality);
 	if(!pose.isNull())
 	{

src/OdometryROS.h

@@ -64,6 +64,7 @@ public:
 	const tf::TransformListener & tfListener() const {return tfListener_;}
 	bool isPaused() const {return paused_;}
 	bool isOdometryBOW() const;
+	bool waitForTransform() const {return waitForTransform_;}
 
 private:
 	rtabmap::Odometry * odometry_;
@@ -71,7 +72,9 @@ private:
 	// parameters
 	std::string frameId_;
 	std::string odomFrameId_;
+	std::string groundTruthFrameId_;
 	bool publishTf_;
+	bool waitForTransform_;
 	ParametersMap parameters_;
 
 	ros::Publisher odomPub_;

src/RGBDOdometryNode.cpp

@@ -111,6 +111,14 @@ public:
 			tf::StampedTransform localTransform;
 			try
 			{
+				if(this->waitForTransform())
+				{
+					if(!this->tfListener().waitForTransform(this->frameId(), image->header.frame_id, image->header.stamp, ros::Duration(1)))
+					{
+						ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), image->header.frame_id.c_str());
+						return;
+					}
+				}
 				this->tfListener().lookupTransform(this->frameId(), image->header.frame_id, image->header.stamp, localTransform);
 			}
 			catch(tf::TransformException & ex)

src/StereoOdometryNode.cpp

@@ -128,7 +128,15 @@ public:
 			tf::StampedTransform localTransform;
 			try
 			{
-				UDEBUG("");
+				if(this->waitForTransform())
+				{
+					if(!this->tfListener().waitForTransform(this->frameId(), imageRectLeft->header.frame_id, imageRectLeft->header.stamp, ros::Duration(1)))
+					{
+						ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), imageRectLeft->header.frame_id.c_str());
+						return;
+					}
+				}
+
 				this->tfListener().lookupTransform(this->frameId(), imageRectLeft->header.frame_id, imageRectLeft->header.stamp, localTransform);
 			}
 			catch(tf::TransformException & ex)

src/nodelets/obstacles_detection.cpp

@@ -68,7 +68,8 @@ public:
 		normalEstimationRadius_(0.05),
 		groundNormalAngle_(M_PI_4),
 		minClusterSize_(20),
-		maxObstaclesHeight_(0)
+		maxObstaclesHeight_(0),
+		waitForTransform_(false)
 	{}
 
 	virtual ~ObstaclesDetection()
@@ -87,6 +88,7 @@ private:
 		pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
 		pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
 		pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
+		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
 
 		cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this);
 
@@ -103,12 +105,15 @@ private:
 			Transform localTransform;
 			try
 			{
-				tf::StampedTransform tmp;
-				if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
+				if(waitForTransform_)
 				{
-					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
-					return;
+					if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
+					{
+						ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
+						return;
+					}
 				}
+				tf::StampedTransform tmp;
 				tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
 				localTransform = transformFromTF(tmp);
 			}
@@ -174,6 +179,7 @@ private:
 	double groundNormalAngle_;
 	int minClusterSize_;
 	double maxObstaclesHeight_;
+	bool waitForTransform_;
 
 	tf::TransformListener tfListener_;