From 68df4b360c7455cbf9b92a73f0a2864fee392597 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sun, 27 Sep 2015 12:49:44 -0400
Subject: [PATCH] Updated patrol.py: either the goal is reached or the plan has
 failed, send next waypoint. Node labels can be also set as waypoints

---
 scripts/patrol.py | 31 ++++++++++++++++++++-----------
 1 file changed, 20 insertions(+), 11 deletions(-)

diff --git a/scripts/patrol.py b/scripts/patrol.py
index 81b462a6..6fe1e02d 100755
--- a/scripts/patrol.py
+++ b/scripts/patrol.py
@@ -11,40 +11,49 @@ 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)
+        rospy.loginfo(rospy.get_caller_id() + "Goal '%s' reached! Publishing next goal in 1 sec...", waypoints[currentIndex])
     else:
-        rospy.loginfo(rospy.get_caller_id() + "Goal %d failed! Retrying in 1 sec...", int(waypoints[currentIndex]))
+        rospy.loginfo(rospy.get_caller_id() + "Goal '%s' failed! Publishing next goal in 1 sec...", waypoints[currentIndex])
+
+    currentIndex = (currentIndex+1) % len(waypoints)
 
     rospy.sleep(1.)
 
     msg = Goal()
-    msg.node_id = int(waypoints[currentIndex])
-    msg.node_label = ""
+    if waypoints[currentIndex].isdigit():
+        msg.node_id = int(waypoints[currentIndex])
+        msg.node_label = ""
+    else:
+        msg.node_id = 0
+        msg.node_label = waypoints[currentIndex]
     msg.header.stamp = rospy.get_rostime()
-    rospy.loginfo("Publishing goal %d! (%d/%d)", msg.node_id, currentIndex+1, len(waypoints))
+    rospy.loginfo("Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
     pub.publish(msg)
 
 def main():
-    rospy.init_node('patrol', anonymous=True)
+    rospy.init_node('patrol', anonymous=False)
     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 = ""
+    if waypoints[currentIndex].isdigit():
+        msg.node_id = int(waypoints[currentIndex])
+        msg.node_label = ""
+    else:
+        msg.node_id = 0
+        msg.node_label = waypoints[currentIndex]
     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))
+    rospy.loginfo("Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], 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)")
+        print("usage: patrol.py waypointA waypointB waypointC ... (at least 2 waypoints, can be node id or label)")
     else:
         waypoints = sys.argv[1:] 
         rospy.loginfo("Waypoints: [%s]", str(waypoints).strip('[]'))