Fixed issue #40: RtabmapGlobalPathEvent build error

src/CoreWrapper.cpp

@@ -1344,7 +1344,8 @@ void CoreWrapper::goalCommonCallback(
 		int id,
 		const std::string & label,
 		const Transform & pose,
-		const ros::Time & stamp)
+		const ros::Time & stamp,
+		double * planningTime)
 {
 	UTimer timer;
 
@@ -1362,10 +1363,19 @@ void CoreWrapper::goalCommonCallback(
 		ROS_INFO("Planning: set goal %s", pose.prettyPrint().c_str());
 	}
 
+	if(planningTime)
+	{
+		*planningTime = 0.0;
+	}
+
 	bool success = false;
 	if((id > 0 && rtabmap_.computePath(id, true)) ||
 	   (!pose.isNull() && rtabmap_.computePath(pose)))
 	{
+		if(planningTime)
+		{
+			*planningTime = timer.elapsed();
+		}
 		ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
 		const std::vector<std::pair<int, Transform> > & poses = rtabmap_.getPath();
 
@@ -1905,10 +1915,12 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 
 bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res)
 {
-	goalCommonCallback(req.node_id, req.node_label, Transform(), ros::Time::now());
+	double planningTime = 0.0;
+	goalCommonCallback(req.node_id, req.node_label, Transform(), ros::Time::now(), &planningTime);
 	const std::vector<std::pair<int, Transform> > & path = rtabmap_.getPath();
 	res.path_ids.resize(path.size());
 	res.path_poses.resize(path.size());
+	res.planning_time = planningTime;
 	for(unsigned int i=0; i<path.size(); ++i)
 	{
 		res.path_ids[i] = path[i].first;

src/CoreWrapper.h

@@ -173,7 +173,7 @@ private:
 			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
 			const sensor_msgs::LaserScanConstPtr& scanMsg);
 
-	void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp);
+	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);
 	void updateGoal(const ros::Time & stamp);

src/GuiWrapper.cpp

@@ -292,7 +292,7 @@ void GuiWrapper::goalPathCallback(
 		poses[i].first = -int(i)-1;
 		poses[i].second = rtabmap_ros::transformFromPoseMsg(pathMsg->poses[i].pose);
 	}
-	this->post(new RtabmapGlobalPathEvent(goalMsg->node_id, goalMsg->node_label, poses));
+	this->post(new RtabmapGlobalPathEvent(goalMsg->node_id, goalMsg->node_label, poses, 0.0));
 }
 
 void GuiWrapper::goalReachedCallback(
@@ -441,7 +441,7 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
 					poses[i].first = setGoalSrv.response.path_ids[i];
 					poses[i].second = rtabmap_ros::transformFromPoseMsg(setGoalSrv.response.path_poses[i]);
 				}
-				this->post(new RtabmapGlobalPathEvent(setGoalSrv.request.node_id, setGoalSrv.request.node_label, poses));
+				this->post(new RtabmapGlobalPathEvent(setGoalSrv.request.node_id, setGoalSrv.request.node_label, poses, setGoalSrv.response.planning_time));
 			}
 		}
 		else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal)