Added "odom_sensor_sync" ros parameter (rtabmap and rtabmapviz nodes)

include/rtabmap_ros/CoreWrapper.h

@@ -100,7 +100,6 @@ private:
 
 	bool commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg);
 	bool commonOdomTFUpdate(const ros::Time & stamp); // TF odom
-	rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
 
 	void commonDepthCallback(
 				const std::string & odomFrameId,
@@ -511,6 +510,7 @@ private:
 	boost::thread* transformThread_;
 
 	bool stereoToDepth_;
+	bool odomSensorSync_;
 	float rate_;
 	bool createIntermediateNodes_;
 	ros::Time time_;

include/rtabmap_ros/GuiWrapper.h

@@ -269,6 +269,7 @@ private:
 	std::string odomFrameId_;
 	bool waitForTransform_;
 	double waitForTransformDuration_;
+	bool odomSensorSync_;
 	tf::TransformListener tfListener_;
 
 	message_filters::Subscriber<rtabmap_ros::Info> infoTopic_;

launch/demo/demo_robot_mapping.launch

@@ -40,7 +40,6 @@
 	  <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
 	  <param name="RGBD/ProximityBySpace"     type="string" value="true"/>  <!-- Local loop closure detection (using estimated position) with locations in WM -->
 	  <param name="RGBD/ProximityByTime"      type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
-	  <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
 	  <param name="Reg/Strategy"              type="string" value="1"/>     <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
 	  <param name="Vis/InlierDistance"        type="string" value="0.1"/>   <!-- 3D visual words correspondence distance -->
 	  <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->

launch/demo/demo_stereo_outdoor.launch

@@ -77,6 +77,8 @@
 
          <!-- RTAB-Map's parameters -->
          <param name="Rtabmap/TimeThr"                   type="string" value="700"/>
+         <param name="Grid/DepthDecimation"              type="string" value="4"/>
+         <param name="Grid/FlatObstacleDetected"         type="string" value="true"/>
          <param name="Kp/MaxFeatures"                    type="string" value="200"/>
          <param name="Kp/MaxDepth"                       type="string" value="10"/>
          <param name="Kp/DetectorStrategy"               type="string" value="0"/>   <!-- use SURF -->

launch/tests/rgbdslam_datasets.launch

@@ -62,6 +62,8 @@
       <param name="RGBD/LoopClosureReextractFeatures" type="string" value="false"/>
       <param name="Mem/RawDescriptorsKept" type="string" value="true"/>
       <param name="Kp/DetectorStrategy" type="string" value="0"/>
+      <param name="RGBD/CreateOccupancyGrid" type="string" value="false"/>
+      <param name="Rtabmap/CreateIntermediateNodes" type="string" value="true"/>
 
       <param name="frame_id" type="string" value="kinect"/>
       <param name="ground_truth_frame_id" type="string" value="world"/> 

src/CoreWrapper.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <std_msgs/Bool.h>
 #include <sensor_msgs/image_encodings.h>
 #include <cv_bridge/cv_bridge.h>
+#include <pcl/io/pcd_io.h>
 
 #include <visualization_msgs/MarkerArray.h>
 
@@ -120,6 +121,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 		stereoExactTFSync_(0),
 		transformThread_(0),
 		stereoToDepth_(false),
+		odomSensorSync_(false),
 		rate_(Parameters::defaultRtabmapDetectionRate()),
 		createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
 		time_(ros::Time::now()),
@@ -168,6 +170,10 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 			subscribeDepth = true;
 		}
 	}
+	if(subscribeStereo)
+	{
+		approxSync = false; // default for stereo: exact sync
+	}
 
 	pnh.param("config_path",         configPath_, configPath_);
 	pnh.param("database_path",       databasePath_, databasePath_);
@@ -205,6 +211,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 	pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
 	pnh.param("flip_scan", flipScan_, flipScan_);
 	pnh.param("stereo_to_depth", stereoToDepth_, stereoToDepth_);
+	pnh.param("odom_sensor_sync", odomSensorSync_, odomSensorSync_);
 
 	if(!tfPrefix.empty())
 	{
@@ -253,6 +260,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 	ROS_INFO("rtabmap: tf_tolerance  = %f", tfTolerance);
 	ROS_INFO("rtabmap: depth_cameras = %d", depthCameras);
 	ROS_INFO("rtabmap: approx_sync   = %s", approxSync?"true":"false");
+	ROS_INFO("rtabmap: odom_sensor_sync   = %s", odomSensorSync_?"true":"false");
 
 	infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
 	mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
@@ -762,7 +770,7 @@ bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp)
 	if(!paused_)
 	{
 		// Odom TF ready?
-		Transform odom = getTransform(odomFrameId_, frameId_, stamp);
+		Transform odom = rtabmap_ros::getTransform(odomFrameId_, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
 		if(odom.isNull())
 		{
 			return false;
@@ -811,35 +819,6 @@ bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp)
 	return false;
 }
 
-Transform CoreWrapper::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
-{
-	// TF ready?
-	Transform transform;
-	try
-	{
-		if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_>0.0)
-		{
-			//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
-			std::string errorMsg;
-			if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_), ros::Duration(0.01), &errorMsg))
-			{
-				ROS_WARN("rtabmap: Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\"",
-						fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_, stamp.toSec(), errorMsg.c_str());
-				return transform;
-			}
-		}
-
-		tf::StampedTransform tmp;
-		tfListener_.lookupTransform(fromFrameId, toFrameId, stamp, tmp);
-		transform = rtabmap_ros::transformFromTF(tmp);
-	}
-	catch(tf::TransformException & ex)
-	{
-		ROS_WARN("%s",ex.what());
-	}
-	return transform;
-}
-
 void CoreWrapper::commonDepthCallback(
 		const std::string & odomFrameId,
 		const sensor_msgs::ImageConstPtr& imageMsg,
@@ -872,7 +851,7 @@ void CoreWrapper::commonDepthCallback(
 			depthMsgs,
 			cameraInfoMsgs,
 			frameId_,
-			odomFrameId,
+			odomSensorSync_?odomFrameId:"",
 			lastPoseStamp_,
 			rgb,
 			depth,
@@ -899,26 +878,26 @@ void CoreWrapper::commonDepthCallback(
 		}
 	}
 
+	cv::Mat scan;
+	Transform scanLocalTransform = Transform::getIdentity();
 	pcl::PointCloud<pcl::PointXYZ> scanCloud2d;
 	bool genMaxScanPts = 0;
 	if(scan2dMsg.get() == 0 && scan3dMsg.get() == 0 && genScan_)
 	{
-		scanCloud2d += util3d::laserScanFromDepthImages(
+		scanCloud2d = util3d::laserScanFromDepthImages(
 				depth,
 				cameraModels,
 				genScanMaxDepth_,
 				genScanMinDepth_);
 		genMaxScanPts += depth.cols;
+		scan = util3d::laserScan2dFromPointCloud(scanCloud2d);
 	}
-
+	else if(scan2dMsg.get() != 0)
-	cv::Mat scan;
-	Transform scanLocalTransform = Transform::getIdentity();
-	if(scan2dMsg.get() != 0)
 	{
 		if(!rtabmap_ros::convertScanMsg(
 				scan2dMsg,
 				frameId_,
-				odomFrameId,
+				odomSensorSync_?odomFrameId:"",
 				lastPoseStamp_,
 				scan,
 				scanLocalTransform,
@@ -940,7 +919,7 @@ void CoreWrapper::commonDepthCallback(
 		if(!rtabmap_ros::convertScan3dMsg(
 				scan3dMsg,
 				frameId_,
-				odomFrameId,
+				odomSensorSync_?odomFrameId:"",
 				lastPoseStamp_,
 				scanCloudNormalK_,
 				scan,
@@ -952,15 +931,11 @@ void CoreWrapper::commonDepthCallback(
 			return;
 		}
 	}
-	else if(scanCloud2d.size())
-	{
-		scan = util3d::laserScan2dFromPointCloud(scanCloud2d);
-	}
 
 	Transform groundTruthPose;
 	if(!groundTruthFrameId_.empty())
 	{
-		groundTruthPose = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_);
+		groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
 	}
 
 	SensorData data(scan,
@@ -1004,7 +979,7 @@ void CoreWrapper::commonStereoCallback(
 			leftCamInfoMsg,
 			rightCamInfoMsg,
 			frameId_,
-			odomFrameId,
+			odomSensorSync_?odomFrameId:"",
 			lastPoseStamp_,
 			left,
 			right,
@@ -1064,7 +1039,7 @@ void CoreWrapper::commonStereoCallback(
 		if(!rtabmap_ros::convertScanMsg(
 				scan2dMsg,
 				frameId_,
-				odomFrameId,
+				odomSensorSync_?odomFrameId:"",
 				lastPoseStamp_,
 				scan,
 				scanLocalTransform,
@@ -1086,7 +1061,7 @@ void CoreWrapper::commonStereoCallback(
 		if(!rtabmap_ros::convertScan3dMsg(
 				scan3dMsg,
 				frameId_,
-				odomFrameId,
+				odomSensorSync_?odomFrameId:"",
 				lastPoseStamp_,
 				scanCloudNormalK_,
 				scan,
@@ -1102,7 +1077,7 @@ void CoreWrapper::commonStereoCallback(
 	Transform groundTruthPose;
 	if(!groundTruthFrameId_.empty())
 	{
-		groundTruthPose = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_);
+		groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
 	}
 
 	SensorData data(scan,
@@ -1459,7 +1434,8 @@ void CoreWrapper::process(
 		{
 			timeRtabmap = timer.ticks();
 		}
-		ROS_INFO("rtabmap: Rate=%.2fs, Limit=%.3fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs (local map=%d, WM=%d)",
+		ROS_INFO("rtabmap (%d): Rate=%.2fs, Limit=%.3fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs (local map=%d, WM=%d)",
+				rtabmap_.getLastLocationId(),
 				rate_>0?1.0f/rate_:0,
 				rtabmap_.getTimeThreshold()/1000.0f,
 				timeRtabmap,
@@ -1609,7 +1585,7 @@ void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
 	// transform goal in /map frame
 	if(mapFrameId_.compare(msg->header.frame_id) != 0)
 	{
-		Transform t = this->getTransform(mapFrameId_, msg->header.frame_id, msg->header.stamp);
+		Transform t = rtabmap_ros::getTransform(mapFrameId_, msg->header.frame_id, msg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
 		if(t.isNull())
 		{
 			ROS_ERROR("Cannot transform goal pose from \"%s\" frame to \"%s\" frame!",

src/GuiWrapper.cpp

@@ -65,6 +65,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 		frameId_("base_link"),
 		waitForTransform_(true),
 		waitForTransformDuration_(0.2), // 200 ms
+		odomSensorSync_(false),
 		cameraNodeName_(""),
 		lastOdomInfoUpdateTime_(0),
 		depthScanSync_(0),
@@ -131,6 +132,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 	pnh.param("tf_prefix", tfPrefix, tfPrefix);
 	pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
 	pnh.param("wait_for_transform_duration",  waitForTransformDuration_, waitForTransformDuration_);
+	pnh.param("odom_sensor_sync", odomSensorSync_, odomSensorSync_);
 	pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_); // used to pause the rtabmap_ros/camera when pausing the process
 	pnh.param("init_cache_path", initCachePath, initCachePath);
 	if(initCachePath.size())
@@ -570,7 +572,7 @@ void GuiWrapper::commonDepthCallback(
 					depthMsgs,
 					cameraInfoMsgs,
 					frameId_,
-					odomHeader.frame_id,
+					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					rgb,
 					depth,
@@ -588,7 +590,7 @@ void GuiWrapper::commonDepthCallback(
 			if(!rtabmap_ros::convertScanMsg(
 					scan2dMsg,
 					frameId_,
-					odomHeader.frame_id,
+					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					scan,
 					scanLocalTransform,
@@ -604,7 +606,7 @@ void GuiWrapper::commonDepthCallback(
 			if(!rtabmap_ros::convertScan3dMsg(
 					scan3dMsg,
 					frameId_,
-					odomHeader.frame_id,
+					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					0,
 					scan,
@@ -727,7 +729,7 @@ void GuiWrapper::commonStereoCallback(
 				leftCamInfoMsg,
 				rightCamInfoMsg,
 				frameId_,
-				odomHeader.frame_id,
+				odomSensorSync_?odomHeader.frame_id:"",
 				odomHeader.stamp,
 				left,
 				right,
@@ -744,7 +746,7 @@ void GuiWrapper::commonStereoCallback(
 			if(!rtabmap_ros::convertScanMsg(
 					scan2dMsg,
 					frameId_,
-					odomHeader.frame_id,
+					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					scan,
 					scanLocalTransform,
@@ -760,7 +762,7 @@ void GuiWrapper::commonStereoCallback(
 			if(!rtabmap_ros::convertScan3dMsg(
 					scan3dMsg,
 					frameId_,
-					odomHeader.frame_id,
+					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					0,
 					scan,

src/MsgConversion.cpp

@@ -994,7 +994,7 @@ bool convertRGBDMsgs(
 			return false;
 		}
 		// sync with odometry stamp
-		if(odomStamp != depthMsgs[i]->header.stamp)
+		if(!odomFrameId.empty() && odomStamp != depthMsgs[i]->header.stamp)
 		{
 			rtabmap::Transform sensorT = getTransform(
 					frameId,
@@ -1010,6 +1010,7 @@ bool convertRGBDMsgs(
 			}
 			else
 			{
+				//ROS_WARN("RGBD correction = %s (time diff=%fs)", sensorT.prettyPrint().c_str(), fabs(depthMsgs[i]->header.stamp.toSec()-odomStamp.toSec()));
 				localTransform = sensorT * localTransform;
 			}
 		}
@@ -1113,7 +1114,7 @@ bool convertStereoMsg(
 		return false;
 	}
 	// sync with odometry stamp
-	if(odomStamp != leftImageMsg->header.stamp)
+	if(!odomFrameId.empty() && odomStamp != leftImageMsg->header.stamp)
 	{
 		rtabmap::Transform sensorT = getTransform(
 				frameId,
@@ -1163,7 +1164,7 @@ bool convertScanMsg(
 {
 	// make sure the frame of the laser is updated too
 	rtabmap::Transform tmpT = getTransform(
-			odomFrameId,
+			odomFrameId.empty()?frameId:odomFrameId,
 			scan2dMsg->header.frame_id,
 			scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment),
 			listener,
@@ -1187,14 +1188,14 @@ bool convertScanMsg(
 	//transform in frameId_ frame
 	sensor_msgs::PointCloud2 scanOut;
 	laser_geometry::LaserProjection projection;
-	projection.transformLaserScanToPointCloud(odomFrameId, *scan2dMsg, scanOut, listener);
+	projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, *scan2dMsg, scanOut, listener);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::fromROSMsg(scanOut, *pclScan);
 
 	//transform back in laser frame
 	rtabmap::Transform laserToOdom = getTransform(
 			scan2dMsg->header.frame_id,
-			odomFrameId,
+			odomFrameId.empty()?frameId:odomFrameId,
 			scan2dMsg->header.stamp,
 			listener,
 			waitForTransform);
@@ -1204,7 +1205,7 @@ bool convertScanMsg(
 	}
 
 	// sync with odometry stamp
-	if(odomStamp != scan2dMsg->header.stamp)
+	if(!odomFrameId.empty() && odomStamp != scan2dMsg->header.stamp)
 	{
 		rtabmap::Transform sensorT = getTransform(
 				frameId,
@@ -1220,6 +1221,7 @@ bool convertScanMsg(
 		}
 		else
 		{
+			//ROS_WARN("scan correction = %s (time diff=%fs)", sensorT.prettyPrint().c_str(), fabs(scan2dMsg->header.stamp.toSec()-odomStamp.toSec()));
 			scanLocalTransform = sensorT * scanLocalTransform;
 		}
 	}
@@ -1256,7 +1258,7 @@ bool convertScan3dMsg(
 	}
 
 	// sync with odometry stamp
-	if(odomStamp != scan3dMsg->header.stamp)
+	if(!odomFrameId.empty() && odomStamp != scan3dMsg->header.stamp)
 	{
 		rtabmap::Transform sensorT = getTransform(
 				frameId,

src/nodelets/stereo_odometry.cpp

@@ -86,7 +86,6 @@ private:
 		bool approxSync = false;
 		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("queue_size", queueSize_, queueSize_);
-		NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
 
 		ros::NodeHandle left_nh(nh, "left");
 		ros::NodeHandle right_nh(nh, "right");

src/rviz/MapCloudDisplay.cpp

@@ -485,7 +485,7 @@ void MapCloudDisplay::downloadMap()
 		ros::NodeHandle nh;
 		QMessageBox * messageBox = new QMessageBox(
 				QMessageBox::NoIcon,
-				tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map").c_str()),
+				tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map_data").c_str()),
 				tr("Downloading the map... please wait (rviz could become gray!)"),
 				QMessageBox::NoButton);
 		messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
@@ -493,18 +493,18 @@ void MapCloudDisplay::downloadMap()
 		QApplication::processEvents();
 		uSleep(100); // hack make sure the text in the QMessageBox is shown...
 		QApplication::processEvents();
-		if(!ros::service::call("rtabmap/get_map", getMapSrv))
+		if(!ros::service::call("rtabmap/get_map_data", getMapSrv))
 		{
 			ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. "
 					  "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
 					  "to \"get_map\" in the launch "
-					  "file like: <remap from=\"rtabmap/get_map\" to=\"get_map\"/>.",
+					  "file like: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.",
-					  nh.resolveName("rtabmap/get_map").c_str());
+					  nh.resolveName("rtabmap/get_map_data").c_str());
 			messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. "
 					  "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
 					  "to \"get_map\" in the launch "
-					  "file like: <remap from=\"rtabmap/get_map\" to=\"get_map\"/>.").
+					  "file like: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.").
-					  arg(nh.resolveName("rtabmap/get_map").c_str()));
+					  arg(nh.resolveName("rtabmap/get_map_data").c_str()));
 		}
 		else
 		{