rtabmap: added subscribe_inter_odom_info option (sync odometry and info msgs for intermediate nodes)

include/rtabmap_ros/CoreWrapper.h

@@ -57,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/Goal.h"
 #include "rtabmap_ros/GetPlan.h"
 #include "rtabmap_ros/CommonDataSubscriber.h"
+#include "rtabmap_ros/OdomInfo.h"
 
 #include "MapsManager.h"
 
@@ -144,6 +145,7 @@ private:
 #endif
 	void imuAsyncCallback(const sensor_msgs::ImuConstPtr & tagDetections);
 	void interOdomCallback(const nav_msgs::OdometryConstPtr & msg);
+	void interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, const rtabmap_ros::OdomInfoConstPtr & msg2);
 
 	void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg);
 
@@ -320,8 +322,13 @@ private:
 	std::map<int, geometry_msgs::PoseWithCovarianceStamped> tags_;
 	ros::Subscriber imuSub_;
 	std::map<double, rtabmap::Transform> imus_;
+
 	ros::Subscriber interOdomSub_;
-	std::list<nav_msgs::Odometry> interOdoms_;
+	std::list<std::pair<nav_msgs::Odometry, rtabmap_ros::OdomInfo>> interOdoms_;
+	message_filters::Subscriber<nav_msgs::Odometry> interOdomSyncSub_;
+	message_filters::Subscriber<rtabmap_ros::OdomInfo> interOdomInfoSyncSub_;
+	typedef message_filters::sync_policies::ExactTime<nav_msgs::Odometry, rtabmap_ros::OdomInfo> MyExactInterOdomSyncPolicy;
+	message_filters::Synchronizer<MyExactInterOdomSyncPolicy> * interOdomSync_;
 
 	bool stereoToDepth_;
 	bool odomSensorSync_;

src/CoreWrapper.cpp

@@ -112,6 +112,7 @@ CoreWrapper::CoreWrapper() :
 		transformThread_(0),
 		tfThreadRunning_(false),
 		stereoToDepth_(false),
+		interOdomSync_(0),
 		odomSensorSync_(false),
 		rate_(Parameters::defaultRtabmapDetectionRate()),
 		createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
@@ -518,8 +519,22 @@ void CoreWrapper::onInit()
 			NODELET_INFO("Create intermediate nodes");
 			if(rate_ == 0.0f)
 			{
-				NODELET_INFO("Subscribe to inter odom messges");
+				bool interOdomInfo = false;
-				interOdomSub_ = nh.subscribe("inter_odom", 1, &CoreWrapper::interOdomCallback, this);
+				pnh.getParam("subscribe_inter_odom_info", interOdomInfo);
+				if(interOdomInfo)
+				{
+					NODELET_INFO("Subscribe to inter odom + info messages");
+					interOdomSync_ = new message_filters::Synchronizer<MyExactInterOdomSyncPolicy>(MyExactInterOdomSyncPolicy(queueSize_), interOdomSyncSub_, interOdomInfoSyncSub_);
+					interOdomSync_->registerCallback(boost::bind(&CoreWrapper::interOdomInfoCallback, this, _1, _2));
+					interOdomSyncSub_.subscribe(nh, "inter_odom", 1);
+					interOdomInfoSyncSub_.subscribe(nh, "inter_odom_info", 1);
+				}
+				else
+				{
+					NODELET_INFO("Subscribe to inter odom messages");
+					interOdomSub_ = nh.subscribe("inter_odom", 1, &CoreWrapper::interOdomCallback, this);
+				}
+
 			}
 		}
 	}
@@ -742,6 +757,7 @@ CoreWrapper::~CoreWrapper()
 	rtabmap_.close();
 	printf("rtabmap: Saving database/long-term memory...done! (located at %s, %ld MB)\n", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024));
 
+	delete interOdomSync_;
 	delete mbClient_;
 }
 
@@ -1638,24 +1654,24 @@ void CoreWrapper::process(
 	if(rtabmap_.isIDsGenerated() || data.id() > 0)
 	{
 		// Add intermediate nodes?
-		for(std::list<nav_msgs::Odometry>::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();)
+		for(std::list<std::pair<nav_msgs::Odometry, rtabmap_ros::OdomInfo> >::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();)
 		{
-			if(iter->header.stamp < lastPoseStamp_)
+			if(iter->first.header.stamp < lastPoseStamp_)
 			{
-				Transform interOdom = rtabmap_ros::transformFromPoseMsg(iter->pose.pose);
+				Transform interOdom = rtabmap_ros::transformFromPoseMsg(iter->first.pose.pose);
 				if(!interOdom.isNull())
 				{
 					cv::Mat covariance;
-					double variance = iter->twist.covariance[0];
+					double variance = iter->first.twist.covariance[0];
 					if(variance == BAD_COVARIANCE || variance <= 0.0f)
 					{
 						//use the one of the pose
-						covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->pose.covariance.data()).clone();
+						covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->first.pose.covariance.data()).clone();
 						covariance /= 2.0;
 					}
 					else
 					{
-						covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->twist.covariance.data()).clone();
+						covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->first.twist.covariance.data()).clone();
 					}
 					if(!uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
 					{
@@ -1684,18 +1700,56 @@ void CoreWrapper::process(
 							Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 							0,
 							cv::Size(1,2));
-					SensorData interData(rgb, depth, model, -1, rtabmap_ros::timestampFromROS(iter->header.stamp));
+					SensorData interData(rgb, depth, model, -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
 					Transform gt;
 					if(!groundTruthFrameId_.empty())
 					{
-						gt = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, iter->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
+						gt = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, iter->first.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
 					}
 					interData.setGroundTruth(gt);
-					rtabmap_.process(interData, interOdom, covariance);
+
+					std::map<std::string, float> externalStats;
+					std::vector<float> odomVelocity;
+					if(iter->second.timeEstimation != 0.0f)
+					{
+						OdometryInfo info = odomInfoFromROS(iter->second);
+						externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", info.localBundleTime*1000.0f));
+						externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", info.localBundleConstraints));
+						externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", info.localBundleOutliers));
+						externalStats.insert(std::make_pair("Odometry/TotalTime/ms", info.timeEstimation*1000.0f));
+						externalStats.insert(std::make_pair("Odometry/Registration/ms", info.reg.totalTime*1000.0f));
+						float speed = 0.0f;
+						if(info.interval>0.0)
+							speed = info.transform.x()/info.interval*3.6;
+						externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
+						externalStats.insert(std::make_pair("Odometry/Inliers/", info.reg.inliers));
+						externalStats.insert(std::make_pair("Odometry/Features/", info.features));
+						externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", info.distanceTravelled));
+						externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", info.keyFrameAdded));
+						externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", info.localKeyFrames));
+						externalStats.insert(std::make_pair("Odometry/LocalMapSize/", info.localMapSize));
+						externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", info.localScanMapSize));
+						externalStats.insert(std::make_pair("Odometry/RAM_usage/MB", info.memoryUsage));
+
+						if(info.interval>0.0)
+						{
+							odomVelocity.resize(6);
+							float x,y,z,roll,pitch,yaw;
+							info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+							odomVelocity[0] = x/info.interval;
+							odomVelocity[1] = y/info.interval;
+							odomVelocity[2] = z/info.interval;
+							odomVelocity[3] = roll/info.interval;
+							odomVelocity[4] = pitch/info.interval;
+							odomVelocity[5] = yaw/info.interval;
+						}
+					}
+
+					rtabmap_.process(interData, interOdom, covariance, odomVelocity, externalStats);
 				}
 				interOdoms_.erase(iter++);
 			}
-			else if(iter->header.stamp == lastPoseStamp_)
+			else if(iter->first.header.stamp == lastPoseStamp_)
 			{
 				interOdoms_.erase(iter++);
 				break;
@@ -2181,7 +2235,15 @@ void CoreWrapper::interOdomCallback(const nav_msgs::OdometryConstPtr & msg)
 {
 	if(!paused_)
 	{
-		interOdoms_.push_back(*msg);
+		interOdoms_.push_back(std::make_pair(*msg, rtabmap_ros::OdomInfo()));
+	}
+}
+
+void CoreWrapper::interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, const rtabmap_ros::OdomInfoConstPtr & msg2)
+{
+	if(!paused_)
+	{
+		interOdoms_.push_back(std::make_pair(*msg1, *msg2));
 	}
 }