From e5c0c01acc087c9e5606d91158561899b281cfa9 Mon Sep 17 00:00:00 2001
From: Mathieu Labbe <matlabbe@gmail.com>
Date: Tue, 16 Jun 2015 17:42:56 -0400
Subject: [PATCH] updated with upstream library changes

---
 msg/OdomInfo.msg      |  9 ++++++++-
 src/MsgConversion.cpp | 19 +++++++++++++++++--
 src/OdometryROS.cpp   |  7 +++++++
 3 files changed, 32 insertions(+), 3 deletions(-)

diff --git a/msg/OdomInfo.msg b/msg/OdomInfo.msg
index c23cc699..e5e3e09f 100644
--- a/msg/OdomInfo.msg
+++ b/msg/OdomInfo.msg
@@ -30,7 +30,11 @@ int32 inliers
 float32 variance
 int32 features
 int32 localMapSize
-float32 time
+float32 timeEstimation
+float32 timeParticleFiltering
+float32 stamp
+float32 interval
+float32 distanceTravelled
 
 int32 type
 
@@ -43,3 +47,6 @@ Point2f[] refCorners
 Point2f[] newCorners
 int32[] cornerInliers
 
+geometry_msgs/Transform transform
+geometry_msgs/Transform transformFiltered
+
diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp
index 60348efb..69a48d9e 100644
--- a/src/MsgConversion.cpp
+++ b/src/MsgConversion.cpp
@@ -551,8 +551,12 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 	info.features = msg.features;
 	info.inliers = msg.inliers;
 	info.localMapSize = msg.localMapSize;
-	info.time = msg.time;
+	info.timeEstimation = msg.timeEstimation;
 	info.variance = msg.variance;
+	info.timeParticleFiltering =  msg.timeParticleFiltering;
+	info.stamp = msg.stamp;
+	info.interval = msg.interval;
+	info.distanceTravelled = msg.distanceTravelled;
 
 	info.type = msg.type;
 
@@ -569,6 +573,9 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 	info.newCorners = points2fFromROS(msg.newCorners);
 	info.cornerInliers = msg.cornerInliers;
 
+	info.transform = transformFromGeometryMsg(msg.transform);
+	info.transformFiltered = transformFromGeometryMsg(msg.transformFiltered);
+
 	return info;
 }
 
@@ -579,8 +586,13 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	msg.features = info.features;
 	msg.inliers = info.inliers;
 	msg.localMapSize = info.localMapSize;
-	msg.time = info.time;
+	msg.timeEstimation = info.timeEstimation;
 	msg.variance = info.variance;
+	msg.timeParticleFiltering =  info.timeParticleFiltering;
+	msg.stamp = info.stamp;
+	msg.interval = info.interval;
+	msg.distanceTravelled = info.distanceTravelled;
+
 
 	msg.type = info.type;
 
@@ -594,6 +606,9 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	points2fToROS(info.newCorners, msg.newCorners);
 	msg.cornerInliers = info.cornerInliers;
 
+	transformToGeometryMsg(info.transform, msg.transform);
+	transformToGeometryMsg(info.transformFiltered, msg.transformFiltered);
+
 }
 
 }
diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp
index d64bae51..c9e2d5ef 100644
--- a/src/OdometryROS.cpp
+++ b/src/OdometryROS.cpp
@@ -157,6 +157,7 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
 	oldParameterNames.push_back("Odom/LocalHistory");
 	oldParameterNames.push_back("Odom/NearestNeighbor");
 	oldParameterNames.push_back("Odom/NNDR");
+	oldParameterNames.push_back("GFTT/MaxCorners");
 	for(std::list<std::string>::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter)
 	{
 		std::string vStr;
@@ -192,6 +193,12 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
 						Parameters::kOdomBowNNDR().c_str());
 				parameters_.at(Parameters::kOdomBowNNDR())= vStr;
 			}
+			else if(iter->compare("GFTT/MaxCorners") == 0)
+			{
+				ROS_WARN("Parameter GFTT/MaxCorners doesn't exist anymore, use %s. Please update your launch file accordingly.",
+						Parameters::kOdomMaxFeatures().c_str());
+				parameters_.at(Parameters::kOdomMaxFeatures())= vStr;
+			}
 		}
 	}