Updated to 0.10.8: rtabmapviz can receive goals (new topic /rtabmap/goal_node, ne msg rtabmap_ros::Goal) synchronized with global path

CMakeLists.txt

@@ -17,7 +17,7 @@ find_package(octomap_ros)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.10.7 REQUIRED)
+find_package(RTABMap 0.10.8 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 
@@ -52,6 +52,7 @@ add_message_files(
    Link.msg
    OdomInfo.msg
    Point2f.msg
+   Goal.msg
 )
 
 ## Generate services in the 'srv' folder

msg/Goal.msg

@@ -0,0 +1,6 @@
+
+Header header
+
+# Set either node_id or node_label
+int32 node_id
+string node_label

src/CoreWrapper.cpp

@@ -193,6 +193,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 
 	// planning topics
 	goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this);
+	goalNodeSub_ = nh.subscribe("goal_node", 1, &CoreWrapper::goalNodeCallback, this);
 	nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("goal_out", 1);
 	goalReachedPub_ = nh.advertise<std_msgs::Bool>("goal_reached", 1);
 	globalPathPub_ = nh.advertise<nav_msgs::Path>("global_path", 1);
@@ -1322,7 +1323,7 @@ void CoreWrapper::process(
 	}
 }
 
-void CoreWrapper::goalCommonCallback(const std::vector<std::pair<int, Transform> > & poses)
+void CoreWrapper::goalCommonCallback(const std::vector<std::pair<int, Transform> > & poses, const ros::Time & stamp)
 {
 	currentMetricGoal_.setNull();
 	latestNodeWasReached_ = false;
@@ -1343,14 +1344,13 @@ void CoreWrapper::goalCommonCallback(const std::vector<std::pair<int, Transform>
 		else
 		{
 			ROS_INFO("Planning: Path successfully created (size=%d)", (int)poses.size());
-			ros::Time now = ros::Time::now();
 
 			// Global path
 			if(globalPathPub_.getNumSubscribers())
 			{
 				nav_msgs::Path path;
 				path.header.frame_id = mapFrameId_;
-				path.header.stamp = now;
+				path.header.stamp = stamp;
 				path.poses.resize(poses.size());
 				std::stringstream stream;
 				for(unsigned int i=0; i<poses.size(); ++i)
@@ -1381,8 +1381,8 @@ void CoreWrapper::goalCommonCallback(const std::vector<std::pair<int, Transform>
 				}
 			}
 
-			publishCurrentGoal(now);
+			publishCurrentGoal(stamp);
-			publishLocalPath(now);
+			publishLocalPath(stamp);
 		}
 	}
 	else
@@ -1412,7 +1412,37 @@ void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
 	UTimer timer;
 	rtabmap_.computePath(targetPose);
 	ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
-	goalCommonCallback(rtabmap_.getPath());
+	goalCommonCallback(rtabmap_.getPath(), msg->header.stamp);
+}
+
+void CoreWrapper::goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg)
+{
+	int id = msg->node_id;
+	if(id == 0 && !msg->node_label.empty() && rtabmap_.getMemory())
+	{
+		id = rtabmap_.getMemory()->getSignatureIdByLabel(msg->node_label);
+	}
+
+	if(id > 0)
+	{
+		ROS_INFO("Planning: set goal %d", id);
+		UTimer timer;
+		rtabmap_.computePath(id, true);
+		ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
+		goalCommonCallback(rtabmap_.getPath(), msg->header.stamp);
+		if(currentMetricGoal_.isNull())
+		{
+			ROS_ERROR("Planning: Node id %d not found or goal already reached!", id);
+		}
+	}
+	else if(!msg->node_label.empty())
+	{
+		ROS_ERROR("Planning: Node with label \"%s\" not found!", msg->node_label.c_str());
+	}
+	else
+	{
+		ROS_ERROR("Planning: Node id should be > 0 !");
+	}
 }
 
 bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
@@ -1851,31 +1881,18 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 
 bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res)
 {
-	int id = req.node_id;
+	rtabmap_ros::GoalPtr msg(new rtabmap_ros::Goal());
-	if(id == 0 && !req.node_label.empty() && rtabmap_.getMemory())
+	msg->header.stamp = ros::Time::now();
+	msg->node_id = req.node_id;
+	msg->node_label = req.node_label;
+	goalNodeCallback(msg);
+	const std::vector<std::pair<int, Transform> > & path = rtabmap_.getPath();
+	res.path_ids.resize(path.size());
+	res.path_poses.resize(path.size());
+	for(unsigned int i=0; i<path.size(); ++i)
 	{
-		id = rtabmap_.getMemory()->getSignatureIdByLabel(req.node_label);
+		res.path_ids[i] = path[i].first;
-	}
+		rtabmap_ros::transformToPoseMsg(path[i].second, res.path_poses[i]);
-
-	if(id > 0)
-	{
-		ROS_INFO("Planning: set goal %d", id);
-		UTimer timer;
-		rtabmap_.computePath(id, true);
-		ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
-		goalCommonCallback(rtabmap_.getPath());
-		if(currentMetricGoal_.isNull())
-		{
-			ROS_ERROR("Planning: Node id %d not found or goal already reached!", id);
-		}
-	}
-	else if(!req.node_label.empty())
-	{
-		ROS_ERROR("Planning: Node with label \"%s\" not found!", req.node_label.c_str());
-	}
-	else
-	{
-		ROS_ERROR("Planning: Node id should be > 0 !");
 	}
 	return true;
 }

src/CoreWrapper.h

@@ -53,6 +53,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/PublishMap.h"
 #include "rtabmap_ros/SetGoal.h"
 #include "rtabmap_ros/SetLabel.h"
+#include "rtabmap_ros/Goal.h"
 
 #include "MapsManager.h"
 
@@ -172,8 +173,9 @@ private:
 			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
 			const sensor_msgs::LaserScanConstPtr& scanMsg);
 
-	void goalCommonCallback(const std::vector<std::pair<int, rtabmap::Transform> > & poses);
+	void goalCommonCallback(const std::vector<std::pair<int, rtabmap::Transform> > & poses, const ros::Time & stamp);
 	void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
+	void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg);
 	void updateGoal(const ros::Time & stamp);
 
 	void process(
@@ -251,6 +253,7 @@ private:
 
 	//Planning stuff
 	ros::Subscriber goalSub_;
+	ros::Subscriber goalNodeSub_;
 	ros::Publisher nextMetricGoalPub_;
 	ros::Publisher goalReachedPub_;
 	ros::Publisher globalPathPub_;

src/GuiWrapper.cpp

@@ -168,6 +168,14 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 			infoTopic_,
 			mapDataTopic_);
 	infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, _1, _2));
+
+	goalTopic_.subscribe(nh, "goal_node", 1);
+	pathTopic_.subscribe(nh, "global_path", 1);
+	goalPathSync_ = new message_filters::Synchronizer<MyGoalPathSyncPolicy>(
+			MyGoalPathSyncPolicy(queueSize),
+			goalTopic_,
+			pathTopic_);
+	goalPathSync_->registerCallback(boost::bind(&GuiWrapper::goalPathCallback, this, _1, _2));
 }
 
 GuiWrapper::~GuiWrapper()
@@ -243,6 +251,20 @@ void GuiWrapper::infoMapCallback(
 	this->post(new RtabmapEvent(stat));
 }
 
+void GuiWrapper::goalPathCallback(
+		const rtabmap_ros::GoalConstPtr & goalMsg,
+		const nav_msgs::PathConstPtr & pathMsg)
+{
+	// we don't have the node ids, just generate fake ones.
+	std::vector<std::pair<int, Transform> > poses(pathMsg->poses.size());
+	for(unsigned int i=0; i<pathMsg->poses.size(); ++i)
+	{
+		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));
+}
+
 void GuiWrapper::processRequestedMap(const rtabmap_ros::MapData & map)
 {
 	std::map<int, Signature> signatures;
@@ -374,6 +396,17 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
 			{
 				ROS_ERROR("Can't call \"set_goal\" service");
 			}
+			else
+			{
+				UASSERT(setGoalSrv.response.path_ids.size() == setGoalSrv.response.path_poses.size());
+				std::vector<std::pair<int, Transform> > poses(setGoalSrv.response.path_poses.size());
+				for(unsigned int i=0; i<setGoalSrv.response.path_poses.size(); ++i)
+				{
+					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));
+			}
 		}
 		else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal)
 		{

src/GuiWrapper.h

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/Info.h"
 #include "rtabmap_ros/MapData.h"
 #include "rtabmap_ros/OdomInfo.h"
+#include "rtabmap_ros/Goal.h"
 #include "rtabmap/utilite/UEventsHandler.h"
 #include "rtabmap/core/Transform.h"
 
@@ -43,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/LaserScan.h>
 #include <nav_msgs/Odometry.h>
+#include <nav_msgs/Path.h>
 
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
@@ -72,6 +74,7 @@ protected:
 
 private:
 	void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg);
+	void goalPathCallback(const rtabmap_ros::GoalConstPtr & goalMsg, const nav_msgs::PathConstPtr & pathMsg);
 
 	void setupCallbacks(
 			bool subscribeDepth,
@@ -214,7 +217,9 @@ private:
 
 	message_filters::Subscriber<rtabmap_ros::Info> infoTopic_;
 	message_filters::Subscriber<rtabmap_ros::MapData> mapDataTopic_;
-	ros::Subscriber globalPathTopic_;
+
+	message_filters::Subscriber<rtabmap_ros::Goal> goalTopic_;
+	message_filters::Subscriber<nav_msgs::Path> pathTopic_;
 
 	ros::Subscriber defaultSub_; // odometry only
 	std::vector<image_transport::SubscriberFilter*> imageSubs_;
@@ -234,6 +239,11 @@ private:
 			rtabmap_ros::MapData> MyInfoMapSyncPolicy;
 	message_filters::Synchronizer<MyInfoMapSyncPolicy> * infoMapSync_;
 
+	typedef message_filters::sync_policies::ExactTime<
+			rtabmap_ros::Goal,
+			nav_msgs::Path> MyGoalPathSyncPolicy;
+	message_filters::Synchronizer<MyGoalPathSyncPolicy> * goalPathSync_;
+
 	// with odom msg
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::LaserScan,

srv/SetGoal.srv

@@ -3,4 +3,6 @@
 int32 node_id
 string node_label
 ---
-#response 
+#response 
+int32[] path_ids
+geometry_msgs/Pose[] path_poses