rgbd_odometry: fixed delay between input and output when 1 camera is used (queue_size should be 1) causing synchronization problems with depending nodes. rtabmap: handling odomInfo is subscribed (save odom statistics in database).

include/rtabmap_ros/CoreWrapper.h

@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/OdometryInfo.h>
 
 #include "rtabmap_ros/GetMap.h"
 #include "rtabmap_ros/ListLabels.h"
@@ -129,7 +130,8 @@ private:
 			const rtabmap::SensorData & data,
 			const rtabmap::Transform & odom = rtabmap::Transform(),
 			const std::string & odomFrameId = "",
-			const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1));
+			const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
+			const rtabmap::OdometryInfo & odomInfo = rtabmap::OdometryInfo());
 
 	bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);

launch/rtabmap.launch

@@ -123,7 +123,8 @@
       <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
       <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
       <remap from="rgbd_image"      to="$(arg rgbd_topic)"/>
-      <param name="approx_sync"     value="$(arg approx_rgbd_sync)"/>
+      <param name="approx_sync"     type="bool"   value="$(arg approx_rgbd_sync)"/>
+      <param name="queue_size"      type="int"    value="$(arg queue_size)"/>
     </node>
     
     <!-- Visual odometry -->
@@ -203,6 +204,8 @@
       <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
       <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
       <param name="subscribe_user_data"  type="bool"   value="$(arg subscribe_user_data)"/>
+      <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
+      <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="true"/>
       <param name="frame_id"             type="string" value="$(arg frame_id)"/>
       <param name="map_frame_id"         type="string" value="$(arg map_frame_id)"/>
       <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>

src/CoreWrapper.cpp

@@ -1118,11 +1118,18 @@ void CoreWrapper::commonDepthCallbackImpl(
 	}
 	globalPose_.header.stamp = ros::Time(0);
 
+	OdometryInfo odomInfo;
+	if(odomInfoMsg.get())
+	{
+		odomInfo = odomInfoFromROS(*odomInfoMsg);
+	}
+
 	process(lastPoseStamp_,
 			data,
 			lastPose_,
 			odomFrameId,
-			covariance_);
+			covariance_,
+			odomInfo);
 	covariance_ = cv::Mat();
 }
 
@@ -1305,11 +1312,18 @@ void CoreWrapper::commonStereoCallback(
 			userData);
 	data.setGroundTruth(groundTruthPose);
 
+	OdometryInfo odomInfo;
+	if(odomInfoMsg.get())
+	{
+		odomInfo = odomInfoFromROS(*odomInfoMsg);
+	}
+
 	process(lastPoseStamp_,
 			data,
 			lastPose_,
 			odomFrameId,
-			covariance_);
+			covariance_,
+			odomInfo);
 
 	covariance_ = cv::Mat();
 }
@@ -1319,7 +1333,8 @@ void CoreWrapper::process(
 		const SensorData & data,
 		const Transform & odom,
 		const std::string & odomFrameId,
-		const cv::Mat & odomCovariance)
+		const cv::Mat & odomCovariance,
+		const OdometryInfo & odomInfo)
 {
 	UTimer timer;
 	if(rtabmap_.isIDsGenerated() || data.id() > 0)
@@ -1346,7 +1361,42 @@ void CoreWrapper::process(
 			}
 		}
 
-		if(rtabmap_.process(data, odom, covariance))
+		std::map<std::string, float> externalStats;
+		std::vector<float> odomVelocity;
+		if(odomInfo.timeEstimation != 0.0f)
+		{
+			externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
+			externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
+			externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
+			externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
+			externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
+			float speed = 0.0f;
+			if(odomInfo.interval>0.0)
+				speed = odomInfo.transform.x()/odomInfo.interval*3.6;
+			externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
+			externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
+			externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
+			externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
+			externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
+			externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
+			externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
+			externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
+
+			if(odomInfo.interval>0.0)
+			{
+				odomVelocity.resize(6);
+				float x,y,z,roll,pitch,yaw;
+				odomInfo.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				odomVelocity[0] = x/odomInfo.interval;
+				odomVelocity[1] = y/odomInfo.interval;
+				odomVelocity[2] = z/odomInfo.interval;
+				odomVelocity[3] = roll/odomInfo.interval;
+				odomVelocity[4] = pitch/odomInfo.interval;
+				odomVelocity[5] = yaw/odomInfo.interval;
+			}
+		}
+
+		if(rtabmap_.process(data, odom, covariance, odomVelocity, externalStats))
 		{
 			timeRtabmap = timer.ticks();
 			mapToOdomMutex_.lock();

src/nodelets/data_odom_sync.cpp

@@ -82,10 +82,10 @@ private:
 		info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
 		odom_sub_.subscribe(nh, "odom_in", 1);
 
-		imagePub_ = rgb_it.advertise("image_out", 10);
+		imagePub_ = rgb_it.advertise("image_out", 1);
-		imageDepthPub_ = depth_it.advertise("image_out", 10);
+		imageDepthPub_ = depth_it.advertise("image_out", 1);
-		infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 10);
+		infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 1);
-		odomPub_ = nh.advertise<nav_msgs::Odometry>("odom_out", 10);
+		odomPub_ = nh.advertise<nav_msgs::Odometry>("odom_out", 1);
 	};
 
 	void callback(const sensor_msgs::ImageConstPtr& image,

src/nodelets/rgbd_odometry.cpp

@@ -242,7 +242,7 @@ private:
 			}
 			else
 			{
-				rgbdSub_ = nh.subscribe("rgbd_image", queueSize_, &RGBDOdometry::callbackRGBD, this);
+				rgbdSub_ = nh.subscribe("rgbd_image", 1, &RGBDOdometry::callbackRGBD, this);
 
 				subscribedTopicsMsg =
 						uFormat("\n%s subscribed to:\n   %s",

src/nodelets/undistort_depth.cpp

@@ -61,9 +61,7 @@ private:
 		ros::NodeHandle & nh = getNodeHandle();
 		ros::NodeHandle & pnh = getPrivateNodeHandle();
 
-		int queueSize = 10;
 		std::string modelPath;
-		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("model", modelPath, modelPath);
 
 		if(modelPath.empty())
@@ -79,7 +77,7 @@ private:
 		else
 		{
 			image_transport::ImageTransport it(nh);
-			sub_ = it.subscribe("depth", queueSize, &UndistortDepth::callback, this);
+			sub_ = it.subscribe("depth", 1, &UndistortDepth::callback, this);
 			pub_ = it.advertise(uFormat("%s_undistorted", nh.resolveName("depth").c_str()), 1);
 		}
 	}