rtabmap_ros: patrol.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import rospy
00003 import sys
00004 from std_msgs.msg import Bool
00005 from rtabmap_ros.msg import Goal
00006 
00007 pub = rospy.Publisher('/rtabmap/goal_node', Goal, queue_size=1)
00008 waypoints = []
00009 currentIndex = 0
00010 
00011 def callback(data):
00012     global currentIndex
00013     if data.data:
00014         rospy.loginfo(rospy.get_caller_id() + "Goal '%s' reached! Publishing next goal in 1 sec...", waypoints[currentIndex])
00015     else:
00016         rospy.loginfo(rospy.get_caller_id() + "Goal '%s' failed! Publishing next goal in 1 sec...", waypoints[currentIndex])
00017 
00018     currentIndex = (currentIndex+1) % len(waypoints)
00019 
00020     rospy.sleep(1.)
00021 
00022     msg = Goal()
00023     if waypoints[currentIndex].isdigit():
00024         msg.node_id = int(waypoints[currentIndex])
00025         msg.node_label = ""
00026     else:
00027         msg.node_id = 0
00028         msg.node_label = waypoints[currentIndex]
00029     msg.header.stamp = rospy.get_rostime()
00030     rospy.loginfo("Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
00031     pub.publish(msg)
00032 
00033 def main():
00034     rospy.init_node('patrol', anonymous=False)
00035     rospy.Subscriber("/rtabmap/goal_reached", Bool, callback)
00036     rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal
00037 
00038     # send the first goal
00039     msg = Goal()
00040     if waypoints[currentIndex].isdigit():
00041         msg.node_id = int(waypoints[currentIndex])
00042         msg.node_label = ""
00043     else:
00044         msg.node_id = 0
00045         msg.node_label = waypoints[currentIndex]
00046     while rospy.Time.now().secs == 0:
00047          rospy.loginfo("Waiting clock...")
00048          rospy.sleep(.1)
00049     msg.header.stamp = rospy.Time.now()
00050     rospy.loginfo("Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
00051     pub.publish(msg)
00052     rospy.spin()    
00053 
00054 if __name__ == '__main__':
00055     if len(sys.argv) < 3:
00056         print("usage: patrol.py waypointA waypointB waypointC ... (at least 2 waypoints, can be node id or label)")
00057     else:
00058         waypoints = sys.argv[1:] 
00059         rospy.loginfo("Waypoints: [%s]", str(waypoints).strip('[]'))
00060         main()