Added /gps/fix input topic, data_player can also publish GPS and global poses if they are set in database

include/rtabmap_ros/CoreWrapper.h

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <std_msgs/Empty.h>
 #include <std_msgs/Int32.h>
+#include <sensor_msgs/NavSatFix.h>
 #include <nav_msgs/GetMap.h>
 #include <nav_msgs/GetPlan.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
@@ -121,6 +122,7 @@ private:
 
 	void userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & dataMsg);
 	void globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & globalPoseMsg);
+	void gpsFixAsyncCallback(const sensor_msgs::NavSatFixConstPtr & gpsFixMsg);
 
 	void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg);
 
@@ -282,6 +284,8 @@ private:
 
 	ros::Subscriber globalPoseAsyncSub_;
 	geometry_msgs::PoseWithCovarianceStamped globalPose_;
+	ros::Subscriber gpsFixAsyncSub_;
+	rtabmap::GPS gps_;
 
 	bool stereoToDepth_;
 	bool odomSensorSync_;

launch/rtabmap.launch

@@ -98,6 +98,8 @@
   <arg name="user_data_topic"          default="/user_data"/>
   <arg name="user_data_async_topic"    default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
   
+  <arg name="gps_topic"                default="/gps/fix" />         <!-- gps async subscription -->
+  
   <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
   <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
   <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
@@ -270,6 +272,7 @@
       <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
       <remap from="user_data"              to="$(arg user_data_topic)"/>
       <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
+      <remap from="gps/fix"                to="$(arg gps_topic)"/>
       <remap from="odom"                   to="$(arg odom_topic)"/>
       
       <!-- localization mode -->

src/CoreWrapper.cpp

@@ -669,6 +669,7 @@ void CoreWrapper::onInit()
 
 	userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this);
 	globalPoseAsyncSub_ = nh.subscribe("global_pose", 1, &CoreWrapper::globalPoseAsyncCallback, this);
+	gpsFixAsyncSub_ = nh.subscribe("gps/fix", 1, &CoreWrapper::gpsFixAsyncCallback, this);
 }
 
 CoreWrapper::~CoreWrapper()
@@ -1268,6 +1269,12 @@ void CoreWrapper::commonDepthCallbackImpl(
 	}
 	globalPose_.header.stamp = ros::Time(0);
 
+	if(gps_.stamp() > 0.0)
+	{
+		data.setGPS(gps_);
+	}
+	gps_ = rtabmap::GPS();
+
 	OdometryInfo odomInfo;
 	if(odomInfoMsg.get())
 	{
@@ -1496,6 +1503,51 @@ void CoreWrapper::commonStereoCallback(
 			userData);
 	data.setGroundTruth(groundTruthPose);
 
+	//global pose
+	if(!globalPose_.header.stamp.isZero())
+	{
+		// assume sensor is fixed
+		Transform sensorToBase = rtabmap_ros::getTransform(
+				globalPose_.header.frame_id,
+				frameId_,
+				lastPoseStamp_,
+				tfListener_,
+				waitForTransform_?waitForTransformDuration_:0.0);
+		if(!sensorToBase.isNull())
+		{
+			Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
+			globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
+
+			// Correction of the global pose accounting the odometry movement since we received it
+			Transform correction = rtabmap_ros::getTransform(
+					frameId_,
+					odomFrameId,
+					globalPose_.header.stamp,
+					lastPoseStamp_,
+					tfListener_,
+					waitForTransform_?waitForTransformDuration_:0.0);
+			if(!correction.isNull())
+			{
+				globalPose *= correction;
+			}
+			else
+			{
+				NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. "
+						"If odometry is small since it received the global pose and "
+						"covariance is large, this should not be a problem.");
+			}
+			cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
+			data.setGlobalPose(globalPose, globalPoseCovariance);
+		}
+	}
+	globalPose_.header.stamp = ros::Time(0);
+
+	if(gps_.stamp() > 0.0)
+	{
+		data.setGPS(gps_);
+	}
+	gps_ = rtabmap::GPS();
+
 	OdometryInfo odomInfo;
 	if(odomInfoMsg.get())
 	{
@@ -1804,6 +1856,30 @@ void CoreWrapper::globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianc
 	}
 }
 
+void CoreWrapper::gpsFixAsyncCallback(const sensor_msgs::NavSatFixConstPtr & gpsFixMsg)
+{
+	if(!paused_)
+	{
+		double error = 10.0;
+		if(gpsFixMsg->position_covariance_type != sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN)
+		{
+			double variance = uMax3(gpsFixMsg->position_covariance.at(0), gpsFixMsg->position_covariance.at(4), gpsFixMsg->position_covariance.at(8));
+			if(variance>0.0)
+			{
+				error = sqrt(variance);
+			}
+		}
+		gps_ = rtabmap::GPS(
+				gpsFixMsg->header.stamp.toSec(),
+				gpsFixMsg->longitude,
+				gpsFixMsg->latitude,
+				gpsFixMsg->altitude,
+				error,
+				0);
+	}
+}
+
+
 void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
 {
 	Transform intialPose = rtabmap_ros::transformFromPoseMsg(msg->pose.pose);
@@ -2032,6 +2108,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
 	mapsManager_.clear();
 	previousStamp_ = ros::Time(0);
 	globalPose_.header.stamp = ros::Time(0);
+	gps_ = rtabmap::GPS();
 	userDataMutex_.lock();
 	userData_ = cv::Mat();
 	userDataMutex_.unlock();
@@ -2092,6 +2169,7 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
 	userData_ = cv::Mat();
 	userDataMutex_.unlock();
 	globalPose_.header.stamp = ros::Time(0);
+	gps_ = rtabmap::GPS();
 
 	NODELET_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str());
 	UFile::copy(databasePath_, databasePath_+".back");

src/DbPlayerNode.cpp

@@ -31,6 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <rosgraph_msgs/Clock.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <nav_msgs/Odometry.h>
@@ -170,6 +172,8 @@ int main(int argc, char** argv)
 	ros::Publisher odometryPub;
 	ros::Publisher scanPub;
 	ros::Publisher scanCloudPub;
+	ros::Publisher globalPosePub;
+	ros::Publisher gpsFixPub;
 	ros::Publisher clockPub;
 	tf2_ros::TransformBroadcaster tfBroadcaster;
 
@@ -311,6 +315,26 @@ int main(int argc, char** argv)
 			}
 		}
 
+		if(!odom.data().globalPose().isNull() &&
+			odom.data().globalPoseCovariance().cols==6 &&
+			odom.data().globalPoseCovariance().rows==6)
+		{
+			if(globalPosePub.getTopic().empty())
+			{
+				globalPosePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("global_pose", 1);
+				ROS_INFO("Global pose will be published.");
+			}
+		}
+
+		if(odom.data().gps().stamp() > 0.0)
+		{
+			if(gpsFixPub.getTopic().empty())
+			{
+				gpsFixPub = nh.advertise<sensor_msgs::NavSatFix>("gps/fix", 1);
+				ROS_INFO("GPS will be published.");
+			}
+		}
+
 		// publish transforms first
 		if(publishTf)
 		{
@@ -372,6 +396,33 @@ int main(int argc, char** argv)
 			}
 		}
 
+		// Publish async topics first (so that they can catched by rtabmap before the image topics)
+		if(globalPosePub.getNumSubscribers() > 0 &&
+			!odom.data().globalPose().isNull() &&
+			odom.data().globalPoseCovariance().cols==6 &&
+			odom.data().globalPoseCovariance().rows==6)
+		{
+			geometry_msgs::PoseWithCovarianceStamped msg;
+			rtabmap_ros::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose);
+			memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double));
+			msg.header.frame_id = frameId;
+			msg.header.stamp = time;
+			globalPosePub.publish(msg);
+		}
+
+		if(odom.data().gps().stamp() > 0.0)
+		{
+			sensor_msgs::NavSatFix msg;
+			msg.longitude = odom.data().gps().longitude();
+			msg.latitude = odom.data().gps().latitude();
+			msg.altitude = odom.data().gps().altitude();
+			msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
+			msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error();
+			msg.header.frame_id = frameId;
+			msg.header.stamp.fromSec(odom.data().gps().stamp());
+			gpsFixPub.publish(msg);
+		}
+
 		if(type >= 0)
 		{
 			if(rgbCamInfoPub.getNumSubscribers() && type == 0)