Added odom_info_lite topic (same as odom_info but without data). Optimized intermediate nodes processing time by avoiding converting all data in odom_info message.

include/rtabmap_ros/MsgConversion.h

@@ -168,7 +168,7 @@ rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg);
 void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg);
 
 std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info);
-rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg);
+rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg, bool ignoreData = false);
 void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg);
 
 cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg);

include/rtabmap_ros/OdometryROS.h

@@ -113,6 +113,7 @@ private:
 
 	ros::Publisher odomPub_;
 	ros::Publisher odomInfoPub_;
+	ros::Publisher odomInfoLitePub_;
 	ros::Publisher odomLocalMap_;
 	ros::Publisher odomLocalScanMap_;
 	ros::Publisher odomLastFrame_;

src/CoreWrapper.cpp

@@ -1909,7 +1909,7 @@ void CoreWrapper::process(
 					std::vector<float> odomVelocity;
 					if(iter->second.timeEstimation != 0.0f)
 					{
-						OdometryInfo info = odomInfoFromROS(iter->second);
+						OdometryInfo info = odomInfoFromROS(iter->second, true);
 						externalStats = rtabmap_ros::odomInfoToStatistics(info);
 
 						if(info.interval>0.0)
@@ -2098,6 +2098,7 @@ void CoreWrapper::process(
 			rtabmapROSStats_.clear();
 		}
 
+		timeMsgConversion += timer.ticks();
 		if(rtabmap_.process(data, odom, covariance, odomVelocity, externalStats))
 		{
 			timeRtabmap = timer.ticks();

src/MsgConversion.cpp

@@ -1381,7 +1381,7 @@ std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo &
 	return stats;
 }
 
-rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
+rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg, bool ignoreData)
 {
 	rtabmap::OdometryInfo info;
 	info.lost = msg.lost;
@@ -1413,31 +1413,34 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 
 	info.type = msg.type;
 
-	UASSERT(msg.wordsKeys.size() == msg.wordsValues.size());
-	for(unsigned int i=0; i<msg.wordsKeys.size(); ++i)
-	{
-		info.words.insert(std::make_pair(msg.wordsKeys[i], keypointFromROS(msg.wordsValues[i])));
-	}
-
 	info.reg.matchesIDs = msg.wordMatches;
 	info.reg.inliersIDs = msg.wordInliers;
 
-	info.refCorners = points2fFromROS(msg.refCorners);
+	if(!ignoreData)
-	info.newCorners = points2fFromROS(msg.newCorners);
-	info.cornerInliers = msg.cornerInliers;
-
-	info.transform = transformFromGeometryMsg(msg.transform);
-	info.transformFiltered = transformFromGeometryMsg(msg.transformFiltered);
-	info.transformGroundTruth = transformFromGeometryMsg(msg.transformGroundTruth);
-	info.guess = transformFromGeometryMsg(msg.guess);
-
-	UASSERT(msg.localMapKeys.size() == msg.localMapValues.size());
-	for(unsigned int i=0; i<msg.localMapKeys.size(); ++i)
 	{
-		info.localMap.insert(std::make_pair(msg.localMapKeys[i], point3fFromROS(msg.localMapValues[i])));
+		UASSERT(msg.wordsKeys.size() == msg.wordsValues.size());
-	}
+		for(unsigned int i=0; i<msg.wordsKeys.size(); ++i)
+		{
+			info.words.insert(std::make_pair(msg.wordsKeys[i], keypointFromROS(msg.wordsValues[i])));
+		}
 
-	info.localScanMap = rtabmap::LaserScan(rtabmap::uncompressData(msg.localScanMap), 0, 0, (rtabmap::LaserScan::Format)msg.localScanMapFormat);
+		info.refCorners = points2fFromROS(msg.refCorners);
+		info.newCorners = points2fFromROS(msg.newCorners);
+		info.cornerInliers = msg.cornerInliers;
+
+		info.transform = transformFromGeometryMsg(msg.transform);
+		info.transformFiltered = transformFromGeometryMsg(msg.transformFiltered);
+		info.transformGroundTruth = transformFromGeometryMsg(msg.transformGroundTruth);
+		info.guess = transformFromGeometryMsg(msg.guess);
+
+		UASSERT(msg.localMapKeys.size() == msg.localMapValues.size());
+		for(unsigned int i=0; i<msg.localMapKeys.size(); ++i)
+		{
+			info.localMap.insert(std::make_pair(msg.localMapKeys[i], point3fFromROS(msg.localMapValues[i])));
+		}
+
+		info.localScanMap = rtabmap::LaserScan(rtabmap::uncompressData(msg.localScanMap), 0, 0, (rtabmap::LaserScan::Format)msg.localScanMapFormat);
+	}
 	return info;
 }
 

src/OdometryROS.cpp

@@ -115,6 +115,7 @@ void OdometryROS::onInit()
 
 	odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
 	odomInfoPub_ = nh.advertise<rtabmap_ros::OdomInfo>("odom_info", 1);
+	odomInfoLitePub_ = nh.advertise<rtabmap_ros::OdomInfo>("odom_info_lite", 1);
 	odomLocalMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
 	odomLocalScanMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_scan_map", 1);
 	odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
@@ -856,13 +857,25 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
 		}
 	}
 
-	if(odomInfoPub_.getNumSubscribers())
+	if(odomInfoPub_.getNumSubscribers() || odomInfoLitePub_.getNumSubscribers())
 	{
 		rtabmap_ros::OdomInfo infoMsg;
 		odomInfoToROS(info, infoMsg);
 		infoMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 		infoMsg.header.frame_id = odomFrameId_;
 		odomInfoPub_.publish(infoMsg);
+
+		infoMsg.wordInliers.clear();
+		infoMsg.wordMatches.clear();
+		infoMsg.wordsKeys.clear();
+		infoMsg.wordsValues.clear();
+		infoMsg.refCorners.clear();
+		infoMsg.newCorners.clear();
+		infoMsg.cornerInliers.clear();
+		infoMsg.localMapKeys.clear();
+		infoMsg.localMapValues.clear();
+		infoMsg.localScanMap.clear();
+		odomInfoLitePub_.publish(infoMsg);
 	}
 
 	if(!data.imageRaw().empty() && odomRgbdImagePub_.getNumSubscribers())