From d0c5951b33e9f287423d12214661fc4d3223392a Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Tue, 2 Oct 2018 18:06:46 -0400
Subject: [PATCH] Fixed rtabmap node not initializing when use_sim_time is true
 and no clock is yet published

---
 include/rtabmap_ros/CoreWrapper.h         |  2 +-
 launch/demo/demo_turtlebot_mapping.launch | 12 ++++++++---
 src/CoreWrapper.cpp                       | 26 ++++++++++++++---------
 3 files changed, 26 insertions(+), 14 deletions(-)

diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h
index 2697416f..52c753fb 100644
--- a/include/rtabmap_ros/CoreWrapper.h
+++ b/include/rtabmap_ros/CoreWrapper.h
@@ -261,7 +261,7 @@ private:
 	ros::ServiceServer octomapFullSrv_;
 #endif
 
-	MoveBaseClient mbClient_;
+	MoveBaseClient * mbClient_;
 
 	boost::thread* transformThread_;
 	bool tfThreadRunning_;
diff --git a/launch/demo/demo_turtlebot_mapping.launch b/launch/demo/demo_turtlebot_mapping.launch
index 727b6afc..c7db7a12 100644
--- a/launch/demo/demo_turtlebot_mapping.launch
+++ b/launch/demo/demo_turtlebot_mapping.launch
@@ -64,13 +64,21 @@
 	  <param name="subscribe_depth"     type="bool"   value="true"/>
 	  <param name="subscribe_scan"      type="bool"   value="true"/>
 	  <param name="map_negative_poses_ignored" type="bool" value="true"/>
+
+          <!-- When sending goals on /rtabmap/goal topic, use actionlib to communicate with move_base --> 
+	  <param name="use_action_for_goal" type="bool" value="true"/>
+          <remap from="move_base"            to="/move_base"/>
 	
 	  <!-- inputs -->
 	  <remap from="scan"            to="/scan"/>
 	  <remap from="rgb/image"       to="$(arg rgb_topic)"/>
   	  <remap from="depth/image"     to="$(arg depth_topic)"/>
   	  <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
-	  <remap unless="$(arg rgbd_odometry)" from="odom" to="/odom"/>
+
+          <!-- Fix odom covariance as in simulation the covariance in /odom topic is high (0.1 for linear and 0.05 for angular) -->
+          <param unless="$(arg rgbd_odometry)" name="odom_frame_id" value="odom"/>
+          <param unless="$(arg rgbd_odometry)" name="odom_tf_linear_variance" value="0.001"/>
+          <param unless="$(arg rgbd_odometry)" name="odom_tf_angular_variance" value="0.001"/>
   	  
   	  <!-- output -->
   	  <remap from="grid_map" to="/map"/>
@@ -88,10 +96,8 @@
 	  <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/> 
 	  <param name="Rtabmap/TimeThr"              type="string" value="700"/>
 	  <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
-	  <param name="Optimizer/Slam2D"             type="string" value="true"/>
 	  <param name="Reg/Force3DoF"                type="string" value="true"/>
 	  <param name="GridGlobal/MinSize"           type="string" value="20"/>
-          <param name="RGBD/OptimizeMaxError"        type="string" value="0.1"/>
 
 	  
 	  <!-- localization mode -->
diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index ac4107f5..3e27598c 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -115,7 +115,7 @@ CoreWrapper::CoreWrapper() :
 		createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
 		maxMappingNodes_(Parameters::defaultGridGlobalMaxNodes()),
 		previousStamp_(0),
-		mbClient_("move_base", true)
+		mbClient_(0)
 {
 	globalPose_.header.stamp = ros::Time(0);
 }
@@ -680,6 +680,8 @@ 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 mbClient_;
 }
 
 void CoreWrapper::loadParameters(const std::string & configFile, ParametersMap & parameters)
@@ -1645,9 +1647,9 @@ void CoreWrapper::process(
 						else
 						{
 							NODELET_WARN("Planning: Plan failed!");
-							if(mbClient_.isServerConnected())
+							if(mbClient_ && mbClient_->isServerConnected())
 							{
-								mbClient_.cancelGoal();
+								mbClient_->cancelGoal();
 							}
 						}
 						if(goalReachedPub_.getNumSubscribers())
@@ -2542,9 +2544,9 @@ bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Em
 			goalReachedPub_.publish(result);
 		}
 	}
-	if(mbClient_.isServerConnected())
+	if(mbClient_ && mbClient_->isServerConnected())
 	{
-		mbClient_.cancelGoal();
+		mbClient_->cancelGoal();
 	}
 
 	return true;
@@ -2753,17 +2755,21 @@ void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
 
 		if(useActionForGoal_)
 		{
-			if(!mbClient_.isServerConnected())
+			if(mbClient_ == 0 || !mbClient_->isServerConnected())
 			{
 				NODELET_INFO("Connecting to move_base action server...");
-				mbClient_.waitForServer(ros::Duration(5.0));
+				if(mbClient_ == 0)
+				{
+					mbClient_ = new MoveBaseClient("move_base", true);
+				}
+				mbClient_->waitForServer(ros::Duration(5.0));
 			}
-			if(mbClient_.isServerConnected())
+			if(mbClient_ && mbClient_->isServerConnected())
 			{
 				move_base_msgs::MoveBaseGoal goal;
 				goal.target_pose = poseMsg;
 
-				mbClient_.sendGoal(goal,
+				mbClient_->sendGoal(goal,
 						boost::bind(&CoreWrapper::goalDoneCb, this, _1, _2),
 						boost::bind(&CoreWrapper::goalActiveCb, this),
 						boost::bind(&CoreWrapper::goalFeedbackCb, this, _1));
@@ -2771,7 +2777,7 @@ void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
 			}
 			else
 			{
-				NODELET_ERROR("Cannot connect to move_base action server!");
+				NODELET_ERROR("Cannot connect to move_base action server (called \"%s\")!", this->getNodeHandle().resolveName("move_base").c_str());
 			}
 		}
 		if(nextMetricGoalPub_.getNumSubscribers())