Updated to RTAB-Map 0.8.5: Added node labeling service, and set goal by label

CMakeLists.txt

@@ -54,6 +54,7 @@ add_message_files(
    PublishMap.srv
    ResetPose.srv
    SetGoal.srv
+   SetLabel.srv
  )
 
 ## Generate added messages and services with any dependencies listed here

include/rtabmap_ros/MsgConversion.h

@@ -96,6 +96,8 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg);
 void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg);
 
+inline double timestampFromROS(const ros::Time & stamp) {return double(stamp.sec) + double(stamp.nsec)/1000000000.0;}
+
 }
 
 #endif /* MSGCONVERSION_H_ */

msg/NodeData.msg

@@ -1,6 +1,9 @@
 
 int32 id
 int32 mapId
+int32 weight
+float64 stamp
+string label
 
 # Pose from odometry not corrected
 geometry_msgs/Pose pose

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.8.4</version>
+  <version>0.8.5</version>
   <description>RTAB-Map's ros-pkg. RTAB-Map is an RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

src/CoreWrapper.cpp

@@ -353,6 +353,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	getProjMapSrv_ = nh.advertiseService("get_proj_map", &CoreWrapper::getProjMapCallback, this);
 	publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
 	setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this);
+	setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this);
 	octomapBinarySrv_ = nh.advertiseService("octomap_binary", &CoreWrapper::octomapBinaryCallback, this);
 	octomapFullSrv_ = nh.advertiseService("octomap_full", &CoreWrapper::octomapFullCallback, this);
 
@@ -717,6 +718,7 @@ void CoreWrapper::commonDepthCallback(
 	float cy = model.cy();
 
 	process(ptrImage->header.seq,
+			ptrImage->header.stamp,
 			ptrImage->image,
 			lastPose_,
 			odomFrameId,
@@ -798,6 +800,7 @@ void CoreWrapper::commonStereoCallback(
 	float baseline = model.baseline();
 
 	process(leftImageMsg->header.seq,
+			leftImageMsg->header.stamp,
 			ptrLeftImage->image,
 			lastPose_,
 			odomFrameId,
@@ -926,6 +929,7 @@ void CoreWrapper::stereoScanTFCallback(
 
 void CoreWrapper::process(
 		int id,
+		const ros::Time & stamp,
 		const cv::Mat & image,
 		const Transform & odom,
 		const std::string & odomFrameId,
@@ -990,7 +994,8 @@ void CoreWrapper::process(
 				odom,
 				odomRotationalVariance,
 				odomTransitionalVariance,
-				id);
+				id,
+				rtabmap_ros::timestampFromROS(stamp));
 
 		if(rtabmap_.process(data))
 		{
@@ -1001,15 +1006,14 @@ void CoreWrapper::process(
 			mapToOdomMutex_.unlock();
 
 			// Publish local graph, info
-			ros::Time timeNow = ros::Time::now();
+			this->publishStats(stamp);
-			this->publishStats(timeNow);
 			std::map<int, rtabmap::Transform> filteredPoses;
 
 			filteredPoses = this->updateMapCaches(rtabmap_.getLocalOptimizedPoses(),
 					cloudMapPub_.getNumSubscribers() != 0,
 					projMapPub_.getNumSubscribers() != 0,
 					gridMapPub_.getNumSubscribers() != 0);
-			this->publishMaps(filteredPoses, timeNow);
+			this->publishMaps(filteredPoses, stamp);
 
 			// clear memory if no one subscribed
 			if(mapCacheCleanup_)
@@ -1062,11 +1066,11 @@ void CoreWrapper::process(
 						{
 							currentMetricGoal_ = updatedGoalPose;
 
-							publishCurrentGoal(timeNow);
+							publishCurrentGoal(stamp);
 						}
 
 						// publish local path
-						publishLocalPath(timeNow);
+						publishLocalPath(stamp);
 					}
 					else
 					{
@@ -1509,7 +1513,6 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 		if(poses.size() && poses.size() != mapIds.size())
 		{
 			ROS_ERROR("poses and map ids are not the same size!? %d vs %d", (int)poses.size(), (int)mapIds.size());
-			return false;
 		}
 
 		ros::Time now = ros::Time::now();
@@ -1595,7 +1598,6 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 	else
 	{
 		UWARN("No subscribers, don't need to publish!");
-		return false;
 	}
 
 	return true;
@@ -1603,13 +1605,60 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 
 bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res)
 {
-	int id = req.target_node_id;
+	int id = req.node_id;
-	ROS_INFO("Planning: set goal %d", id);
+	if(id == 0 && !req.node_label.empty() && rtabmap_.getMemory())
-	UTimer timer;
+	{
-	rtabmap_.computePath(id, req.in_global_graph);
+		id = rtabmap_.getMemory()->getSignatureIdByLabel(req.node_label);
-	ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
+	}
-	goalCommonCallback(rtabmap_.getPath());
+
-	return !currentMetricGoal_.isNull();
+	if(id > 0)
+	{
+		ROS_INFO("Planning: set goal %d", id);
+		UTimer timer;
+		rtabmap_.computePath(id, true);
+		ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
+		goalCommonCallback(rtabmap_.getPath());
+		if(currentMetricGoal_.isNull())
+		{
+			ROS_ERROR("Planning: Node id %d not found or goal already reached!", id);
+		}
+	}
+	else if(!req.node_label.empty())
+	{
+		ROS_ERROR("Planning: Node with label \"%s\" not found!", req.node_label.c_str());
+	}
+	else
+	{
+		ROS_ERROR("Planning: Node id should be > 0 !");
+	}
+	return true;
+}
+
+bool CoreWrapper::setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res)
+{
+	if(rtabmap_.labelLocation(req.node_id, req.node_label))
+	{
+		if(req.node_id > 0)
+		{
+			ROS_INFO("Set label \"%s\" to node %d", req.node_label.c_str(), req.node_id);
+		}
+		else
+		{
+			ROS_INFO("Set label \"%s\" to last node", req.node_label.c_str());
+		}
+	}
+	else
+	{
+		if(req.node_id > 0)
+		{
+			ROS_ERROR("Could not set label \"%s\" to node %d", req.node_label.c_str(), req.node_id);
+		}
+		else
+		{
+			ROS_ERROR("Could not set label \"%s\" to last node", req.node_label.c_str());
+		}
+	}
+	return true;
 }
 
 void CoreWrapper::publishStats(const ros::Time & stamp)

src/CoreWrapper.h

@@ -56,6 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/GetMap.h"
 #include "rtabmap_ros/PublishMap.h"
 #include "rtabmap_ros/SetGoal.h"
+#include "rtabmap_ros/SetLabel.h"
 
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
@@ -163,6 +164,7 @@ private:
 
 	void process(
 			int id,
+			const ros::Time & stamp,
 			const cv::Mat & image,
 			const rtabmap::Transform & odom = rtabmap::Transform(),
 			const std::string & odomFrameId = "",
@@ -188,6 +190,7 @@ private:
 	bool getGridMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
 	bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
 	bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
+	bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res);
 	bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
 	bool octomapFullCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
 
@@ -372,6 +375,7 @@ private:
 	ros::ServiceServer getGridMapSrv_;
 	ros::ServiceServer publishMapDataSrv_;
 	ros::ServiceServer setGoalSrv_;
+	ros::ServiceServer setLabelSrv_;
 	ros::ServiceServer octomapBinarySrv_;
 	ros::ServiceServer octomapFullSrv_;
 

src/DataRecorderNode.cpp

@@ -290,7 +290,8 @@ private:
 			Transform(),
 			1.0f,
 			1.0f,
-			0);
+			0,
+			rtabmap_ros::timestampFromROS(imageMsg->header.stamp));
 		recorder_.addData(data);
 	}
 
@@ -373,7 +374,8 @@ private:
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
 			rotVariance>0?rotVariance:1.0f,
 			transVariance>0?transVariance:1.0f,
-			0);
+			0,
+			rtabmap_ros::timestampFromROS(imageMsg->header.stamp));
 		recorder_.addData(data);
 	}
 
@@ -472,7 +474,8 @@ private:
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
 			rotVariance>0?rotVariance:1.0f,
 			transVariance>0?transVariance:1.0f,
-			0);
+			0,
+			rtabmap_ros::timestampFromROS(imageMsg->header.stamp));
 		recorder_.addData(data);
 	}
 
@@ -529,7 +532,8 @@ private:
 				rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
 				rotVariance>0?rotVariance:1.0f,
 				transVariance>0?transVariance:1.0f,
-				0);
+				0,
+				rtabmap_ros::timestampFromROS(leftImageMsg->header.stamp));
 		recorder_.addData(data);
 	}
 
@@ -583,7 +587,8 @@ private:
 				Transform(),
 				1.0f,
 				1.0f,
-				0);
+				0,
+				rtabmap_ros::timestampFromROS(leftImageMsg->header.stamp));
 		recorder_.addData(data);
 	}
 

src/GuiWrapper.cpp

@@ -429,7 +429,8 @@ void GuiWrapper::depthCallback(
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
 			rotVariance,
 			transVariance,
-			odomMsg->header.seq);
+			odomMsg->header.seq,
+			rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
 	this->post(new OdometryEvent(image));
 }
 
@@ -487,7 +488,8 @@ void GuiWrapper::depthOdomInfoCallback(
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
 			rotVariance,
 			transVariance,
-			odomMsg->header.seq);
+			odomMsg->header.seq,
+			rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
 	OdometryInfo info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
 	this->post(new OdometryEvent(image, info));
 }
@@ -556,7 +558,8 @@ void GuiWrapper::depthScanCallback(
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
 			rotVariance,
 			transVariance,
-			odomMsg->header.seq);
+			odomMsg->header.seq,
+			rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
 	this->post(new OdometryEvent(image));
 }
 
@@ -648,7 +651,8 @@ void GuiWrapper::stereoScanCallback(
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
 			rotVariance,
 			transVariance,
-			odomMsg->header.seq);
+			odomMsg->header.seq,
+			rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
 	this->post(new OdometryEvent(image));
 }
 
@@ -730,7 +734,8 @@ void GuiWrapper::stereoOdomInfoCallback(
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
 			rotVariance,
 			transVariance,
-			odomMsg->header.seq);
+			odomMsg->header.seq,
+			rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
 	OdometryInfo info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
 	this->post(new OdometryEvent(image, info));
 }
@@ -812,7 +817,8 @@ void GuiWrapper::stereoCallback(
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
 			rotVariance,
 			transVariance,
-			odomMsg->header.seq);
+			odomMsg->header.seq,
+			rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
 	this->post(new OdometryEvent(image));
 }
 

src/MsgConversion.cpp

@@ -337,8 +337,12 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 		ROS_ERROR("Words 2D and 3D should be the same size (%d, %d)!", (int)words.size(), (int)words3D.size());
 	}
 
-	return rtabmap::Signature(msg.id,
+	return rtabmap::Signature(
+			msg.id,
 			msg.mapId,
+			msg.weight,
+			msg.stamp,
+			msg.label,
 			words,
 			words3D,
 			transformFromPoseMsg(msg.pose),
@@ -356,6 +360,9 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 	// add data
 	msg.id = signature.id();
 	msg.mapId = signature.mapId();
+	msg.weight = signature.getWeight();
+	msg.stamp = signature.getStamp();
+	msg.label = signature.getLabel();
 	transformToPoseMsg(signature.getPose(), msg.pose);
 	compressedMatToBytes(signature.getImageCompressed(), msg.image);
 	compressedMatToBytes(signature.getDepthCompressed(), msg.depth);

src/RGBDOdometryNode.cpp

@@ -149,7 +149,8 @@ public:
 						rtabmap::Transform(),
 						1.0f,
 						1.0f,
-						0);
+						0,
+						rtabmap_ros::timestampFromROS(image->header.stamp));
 
 				this->processData(data, image->header);
 			}

src/StereoOdometryNode.cpp

@@ -173,7 +173,8 @@ public:
 						rtabmap::Transform(),
 						1.0f,
 						1.0f,
-						0);
+						0,
+						rtabmap_ros::timestampFromROS(imageRectLeft->header.stamp));
 
 				this->processData(data, imageRectLeft->header);
 			}

srv/SetGoal.srv

@@ -1,5 +1,6 @@
 #request
-int32 target_node_id
+# Set either node_id or node_label
-bool in_global_graph
+int32 node_id
+string node_label
 ---
 #response 

srv/SetLabel.srv

@@ -0,0 +1,6 @@
+#request
+# Set node_id = 0 to set label to last node
+int32 node_id
+string node_label
+---
+#response