patrol.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import rospy
3 import sys
4 from std_msgs.msg import Bool
5 from rtabmap_ros.msg import Goal
6 
7 pub = rospy.Publisher('rtabmap/goal_node', Goal, queue_size=1)
8 waypoints = []
9 currentIndex = 0
10 waitingTime = 1.0
11 frameId = ""
12 
13 def callback(data):
14  global currentIndex
15  global waitingTime
16  global frameId
17  if data.data:
18  rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' reached! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime)
19  else:
20  rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' failed! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime)
21 
22  currentIndex = (currentIndex+1) % len(waypoints)
23 
24  # Waiting time before sending next goal
25  rospy.sleep(waitingTime)
26 
27  msg = Goal()
28  msg.frame_id = frameId
29  try:
30  int(waypoints[currentIndex])
31  is_dig = True
32  except ValueError:
33  is_dig = False
34  if is_dig:
35  msg.node_id = int(waypoints[currentIndex])
36  msg.node_label = ""
37  else:
38  msg.node_id = 0
39  msg.node_label = waypoints[currentIndex]
40 
41  rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
42  msg.header.stamp = rospy.get_rostime()
43  pub.publish(msg)
44 
45 def main():
46  rospy.init_node('patrol', anonymous=False)
47  sub = rospy.Subscriber("rtabmap/goal_reached", Bool, callback)
48  global waitingTime
49  global frameId
50  waitingTime = rospy.get_param('~time', waitingTime)
51  frameId = rospy.get_param('~frame_id', frameId)
52  rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal
53 
54  rospy.loginfo(rospy.get_caller_id() + ": Waypoints: [%s]", str(waypoints).strip('[]'))
55  rospy.loginfo(rospy.get_caller_id() + ": time: %f", waitingTime)
56  rospy.loginfo(rospy.get_caller_id() + ": publish goal on %s", pub.resolved_name)
57  rospy.loginfo(rospy.get_caller_id() + ": receive goal status on %s", sub.resolved_name)
58 
59  # send the first goal
60  msg = Goal()
61  msg.frame_id = frameId
62  try:
63  int(waypoints[currentIndex])
64  is_dig = True
65  except ValueError:
66  is_dig = False
67  if is_dig:
68  msg.node_id = int(waypoints[currentIndex])
69  msg.node_label = ""
70  else:
71  msg.node_id = 0
72  msg.node_label = waypoints[currentIndex]
73  while rospy.Time.now().secs == 0:
74  rospy.loginfo(rospy.get_caller_id() + ": Waiting clock...")
75  rospy.sleep(.1)
76  msg.header.stamp = rospy.Time.now()
77  rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
78  pub.publish(msg)
79  rospy.spin()
80 
81 if __name__ == '__main__':
82  if len(sys.argv) < 3:
83  print("usage: patrol.py waypointA waypointB waypointC ... [_time:=1 frame_id:=base_footprint] [topic remaps] (at least 2 waypoints, can be node id, landmark or label)")
84  else:
85  waypoints = sys.argv[1:]
86  waypoints = [x for x in waypoints if not x.startswith('/') and not x.startswith('_')]
87  main()
def main()
Definition: patrol.py:45
def callback(data)
Definition: patrol.py:13


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40