Refactored goalCommonCallback(...). Added Patrol.py script.

scripts/patrol.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python
+import rospy
+import sys
+from std_msgs.msg import Bool
+from rtabmap_ros.msg import Goal
+
+pub = rospy.Publisher('/rtabmap/goal_node', Goal, queue_size=1)
+waypoints = []
+currentIndex = 0
+
+def callback(data):
+    global currentIndex
+    if data.data:
+        rospy.loginfo(rospy.get_caller_id() + "Goal %d reached! Publishing next goal in 1 sec...", int(waypoints[currentIndex]))
+        currentIndex = (currentIndex+1) % len(waypoints)
+    else:
+        rospy.loginfo(rospy.get_caller_id() + "Goal %d failed! Retrying in 1 sec...", int(waypoints[currentIndex]))
+
+    rospy.sleep(1.)
+
+    msg = Goal()
+    msg.node_id = int(waypoints[currentIndex])
+    msg.node_label = ""
+    msg.header.stamp = rospy.get_rostime()
+    rospy.loginfo("Publishing goal %d! (%d/%d)", msg.node_id, currentIndex+1, len(waypoints))
+    pub.publish(msg)
+
+def main():
+    rospy.init_node('patrol', anonymous=True)
+    rospy.Subscriber("/rtabmap/goal_reached", Bool, callback)
+    rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal
+
+    # send the first goal
+    msg = Goal()
+    msg.node_id = int(waypoints[currentIndex])
+    msg.node_label = ""
+    while rospy.Time.now().secs == 0:
+         rospy.loginfo("Waiting clock...")
+         rospy.sleep(.1)
+    msg.header.stamp = rospy.Time.now()
+    rospy.loginfo("Publishing goal %d! (%d/%d)", msg.node_id, currentIndex+1, len(waypoints))
+    pub.publish(msg)
+    rospy.spin()    
+
+if __name__ == '__main__':
+    if len(sys.argv) < 3:
+        print("usage: patrol.py waypointA waypointB waypointC ... (at least 2 waypoints)")
+    else:
+        waypoints = sys.argv[1:] 
+        rospy.loginfo("Waypoints: [%s]", str(waypoints).strip('[]'))
+        main()

scripts/patrol.py~

@@ -0,0 +1,50 @@
+#!/usr/bin/env python
+import rospy
+import sys
+from std_msgs.msg import Bool
+from rtabmap_ros.msg import Goal
+
+pub = rospy.Publisher('/rtabmap/goal_node', Goal, queue_size=1)
+waypoints = []
+currentIndex = 0
+
+def callback(data):
+    global currentIndex
+    if data.data:
+        rospy.loginfo(rospy.get_caller_id() + "Goal %d reached! Publishing next goal in 1 sec...", int(waypoints[currentIndex]))
+        currentIndex = (currentIndex+1) % len(waypoints)
+    else:
+        rospy.loginfo(rospy.get_caller_id() + "Goal %d failed! Retrying in 1 sec...", int(waypoints[currentIndex]))
+        rospy.sleep(1.)
+
+    msg = Goal()
+    msg.node_id = int(waypoints[currentIndex])
+    msg.node_label = ""
+    msg.header.stamp = rospy.get_rostime()
+    rospy.loginfo("Publishing goal %d! (%d/%d)", msg.node_id, currentIndex+1, len(waypoints))
+    pub.publish(msg)
+
+def main():
+    rospy.init_node('patrol', anonymous=True)
+    rospy.Subscriber("/rtabmap/goal_reached", Bool, callback)
+    rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal
+
+    # send the first goal
+    msg = Goal()
+    msg.node_id = int(waypoints[currentIndex])
+    msg.node_label = ""
+    while rospy.Time.now().secs == 0:
+         rospy.loginfo("Waiting clock...")
+         rospy.sleep(.1)
+    msg.header.stamp = rospy.Time.now()
+    rospy.loginfo("Publishing goal %d! (%d/%d)", msg.node_id, currentIndex+1, len(waypoints))
+    pub.publish(msg)
+    rospy.spin()    
+
+if __name__ == '__main__':
+    if len(sys.argv) < 3:
+        print("usage: patrol.py waypointA waypointB waypointC ... (at least 2 waypoints)")
+    else:
+        waypoints = sys.argv[1:] 
+        rospy.loginfo("Waypoints: [%s]", str(waypoints).strip('[]'))
+        main()

src/CoreWrapper.cpp

@@ -1326,53 +1326,111 @@ void CoreWrapper::process(
 	}
 }
 
-void CoreWrapper::goalCommonCallback(const std::vector<std::pair<int, Transform> > & poses, const ros::Time & stamp)
+void CoreWrapper::goalCommonCallback(
+		int id,
+		const std::string & label,
+		const Transform & pose,
+		const ros::Time & stamp)
 {
-	currentMetricGoal_.setNull();
+	UTimer timer;
-	latestNodeWasReached_ = false;
+
-	if(poses.size())
+	if(id == 0 && !label.empty() && rtabmap_.getMemory())
 	{
-		currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId());
+		id = rtabmap_.getMemory()->getSignatureIdByLabel(label);
-		if(currentMetricGoal_.isNull())
+	}
+
+	if(id > 0)
+	{
+		ROS_INFO("Planning: set goal %d", id);
+	}
+	else if(!pose.isNull())
+	{
+		ROS_INFO("Planning: set goal %s", pose.prettyPrint().c_str());
+	}
+
+	bool success = false;
+	if((id > 0 && rtabmap_.computePath(id, true)) ||
+	   (!pose.isNull() && rtabmap_.computePath(pose)))
+	{
+		ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
+		const std::vector<std::pair<int, Transform> > & poses = rtabmap_.getPath();
+
+		currentMetricGoal_.setNull();
+		latestNodeWasReached_ = false;
+		if(poses.size() == 0)
 		{
-			ROS_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId());
+			ROS_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm) or too far from the graph (RGBD/GoalMaxDistance=%fm).",
+					rtabmap_.getGoalReachedRadius(),
+					rtabmap_.getLocalRadius());
 			rtabmap_.clearPath();
 			if(goalReachedPub_.getNumSubscribers())
 			{
 				std_msgs::Bool result;
-				result.data = false;
+				result.data = true;
 				goalReachedPub_.publish(result);
 			}
+			success = true;
 		}
 		else
 		{
-			ROS_INFO("Planning: Path successfully created (size=%d)", (int)poses.size());
+			currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId());
-
+			if(!currentMetricGoal_.isNull())
-			// Adjust the target pose relative to last node
-			if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size())
 			{
-				if(rtabmap_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius())
+				ROS_INFO("Planning: Path successfully created (size=%d)", (int)poses.size());
-				{
-					latestNodeWasReached_ = true;
-					currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();
-				}
-			}
 
-			publishCurrentGoal(stamp);
+				// Adjust the target pose relative to last node
-			publishLocalPath(stamp);
+				if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size())
-			publishGlobalPath(stamp);
+				{
+					if(rtabmap_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius())
+					{
+						latestNodeWasReached_ = true;
+						currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();
+					}
+				}
+
+				publishCurrentGoal(stamp);
+				publishLocalPath(stamp);
+				publishGlobalPath(stamp);
+
+				// Just output the path on screen
+				std::stringstream stream;
+				for(std::vector<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					if(iter != poses.begin())
+					{
+						stream << " ";
+					}
+					stream << iter->first;
+				}
+				ROS_INFO("Global path: [%s]", stream.str().c_str());
+				success=true;
+			}
+			else
+			{
+				ROS_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId());
+			}
 		}
 	}
+	else if(!label.empty())
+	{
+		ROS_ERROR("Planning: Node with label \"%s\" not found!", label.c_str());
+	}
+	else if(pose.isNull())
+	{
+		ROS_ERROR("Planning: Node id should be > 0 !");
+	}
 	else
 	{
-		ROS_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm) or too far from the graph (RGBD/GoalMaxDistance=%fm).",
+		ROS_ERROR("Planning: A node near the goal's pose not found!");
-				rtabmap_.getGoalReachedRadius(),
+	}
-				rtabmap_.getLocalRadius());
+
+	if(!success)
+	{
 		rtabmap_.clearPath();
 		if(goalReachedPub_.getNumSubscribers())
 		{
 			std_msgs::Bool result;
-			result.data = true;
+			result.data = false;
 			goalReachedPub_.publish(result);
 		}
 	}
@@ -1386,41 +1444,17 @@ void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
 		ROS_ERROR("Pose received is null!");
 		return;
 	}
-	ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str());
+	goalCommonCallback(0, "", targetPose, msg->header.stamp);
-	UTimer timer;
-	rtabmap_.computePath(targetPose);
-	ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
-	goalCommonCallback(rtabmap_.getPath(), msg->header.stamp);
 }
 
 void CoreWrapper::goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg)
 {
-	int id = msg->node_id;
+	if(msg->node_id <= 0 && msg->node_label.empty())
-	if(id == 0 && !msg->node_label.empty() && rtabmap_.getMemory())
 	{
-		id = rtabmap_.getMemory()->getSignatureIdByLabel(msg->node_label);
+		ROS_ERROR("Node id or label should be set!");
-	}
+		return;
-
-	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 !");
 	}
+	goalCommonCallback(msg->node_id, msg->node_label, Transform(), msg->header.stamp);
 }
 
 bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
@@ -1859,11 +1893,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 
 bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res)
 {
-	rtabmap_ros::GoalPtr msg(new rtabmap_ros::Goal());
+	goalCommonCallback(req.node_id, req.node_label, Transform(), ros::Time::now());
-	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());

src/CoreWrapper.h

@@ -173,7 +173,7 @@ private:
 			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
 			const sensor_msgs::LaserScanConstPtr& scanMsg);
 
-	void goalCommonCallback(const std::vector<std::pair<int, rtabmap::Transform> > & poses, const ros::Time & stamp);
+	void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp);
 	void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
 	void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg);
 	void updateGoal(const ros::Time & stamp);