From d1763bd52ecae8c81cd57ae7a817571af0d36f77 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Mon, 30 Apr 2018 14:52:31 -0400
Subject: [PATCH] Added /rtabmap/initialpose input topic, added use_saved_map
 parameters (default true).

---
 include/rtabmap_ros/CoreWrapper.h |  4 ++++
 src/CoreWrapper.cpp               | 17 ++++++++++++++++-
 src/MapsManager.cpp               |  3 ++-
 3 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h
index 90fa81a3..dc02e88f 100644
--- a/include/rtabmap_ros/CoreWrapper.h
+++ b/include/rtabmap_ros/CoreWrapper.h
@@ -121,6 +121,8 @@ private:
 	void userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & dataMsg);
 	void globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & globalPoseMsg);
 
+	void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg);
+
 	void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0);
 	void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
 	void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg);
@@ -198,6 +200,7 @@ private:
 	bool waitForTransform_;
 	double waitForTransformDuration_;
 	bool useActionForGoal_;
+	bool useSavedMap_;
 	bool genScan_;
 	double genScanMaxDepth_;
 	double genScanMinDepth_;
@@ -213,6 +216,7 @@ private:
 	ros::Publisher mapGraphPub_;
 	ros::Publisher labelsPub_;
 	ros::Publisher mapPathPub_;
+	ros::Subscriber initialPoseSub_;
 
 	//Planning stuff
 	ros::Subscriber goalSub_;
diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index 8fbb8582..6203428d 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -99,6 +99,7 @@ CoreWrapper::CoreWrapper() :
 		waitForTransform_(true),
 		waitForTransformDuration_(0.2), // 200 ms
 		useActionForGoal_(false),
+		useSavedMap_(true),
 		genScan_(false),
 		genScanMaxDepth_(4.0),
 		genScanMinDepth_(0.0),
@@ -158,6 +159,7 @@ void CoreWrapper::onInit()
 	pnh.param("wait_for_transform",  waitForTransform_, waitForTransform_);
 	pnh.param("wait_for_transform_duration",  waitForTransformDuration_, waitForTransformDuration_);
 	pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
+	pnh.param("use_saved_map", useSavedMap_, useSavedMap_);
 	pnh.param("gen_scan",            genScan_, genScan_);
 	pnh.param("gen_scan_max_depth",  genScanMaxDepth_, genScanMaxDepth_);
 	pnh.param("gen_scan_min_depth",  genScanMinDepth_, genScanMinDepth_);
@@ -208,6 +210,7 @@ void CoreWrapper::onInit()
 	mapGraphPub_ = nh.advertise<rtabmap_ros::MapGraph>("mapGraph", 1);
 	labelsPub_ = nh.advertise<visualization_msgs::MarkerArray>("labels", 1);
 	mapPathPub_ = nh.advertise<nav_msgs::Path>("mapPath", 1);
+	initialPoseSub_ = nh.subscribe("initialpose", 1, &CoreWrapper::initialPoseCallback, this);
 
 	// planning topics
 	goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this);
@@ -472,7 +475,7 @@ void CoreWrapper::onInit()
 	// Init RTAB-Map
 	rtabmap_.init(parameters_, databasePath_);
 
-	if(rtabmap_.getMemory())
+	if(rtabmap_.getMemory() && useSavedMap_)
 	{
 		float xMin, yMin, gridCellSize;
 		cv::Mat map = rtabmap_.getMemory()->load2DMap(xMin, yMin, gridCellSize);
@@ -1662,6 +1665,18 @@ void CoreWrapper::globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianc
 	}
 }
 
+void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
+{
+	Transform intialPose = rtabmap_ros::transformFromPoseMsg(msg->pose.pose);
+	if(intialPose.isNull())
+	{
+		NODELET_ERROR("Pose received is null!");
+		return;
+	}
+
+	rtabmap_.setInitialPose(intialPose);
+}
+
 void CoreWrapper::goalCommonCallback(
 		int id,
 		const std::string & label,
diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp
index 37319731..85cde958 100644
--- a/src/MapsManager.cpp
+++ b/src/MapsManager.cpp
@@ -433,6 +433,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 		}
 
 		bool longUpdate = false;
+		UTimer longUpdateTimer;
 		if(filteredPoses.size() > 20)
 		{
 			if(updateGridCache && gridMaps_.size() < 5)
@@ -685,7 +686,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 
 		if(longUpdate)
 		{
-			ROS_WARN("Map(s) updated!");
+			ROS_WARN("Map(s) updated! (%f s)", longUpdateTimer.ticks());
 		}
 	}