i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated patrol.py: either the goal is reached or the plan has failed, send next waypoint. Node labels can be also set as waypoints

Browse Source
This commit is contained in:
matlabbe
2015-09-27 12:49:44 -04:00
parent 4931866baf
commit 68df4b360c
1 changed files with 20 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
scripts/patrol.py
+20 -11
View File
@@ -11,40 +11,49 @@ currentIndex = 0
def callback(data): def callback(data):
global currentIndex global currentIndex
if data.data: if data.data:
rospy.loginfo(rospy.get_caller_id() + "Goal %d reached! Publishing next goal in 1 sec...", int(waypoints[currentIndex])) rospy.loginfo(rospy.get_caller_id() + "Goal '%s' reached! Publishing next goal in 1 sec...", waypoints[currentIndex])
currentIndex = (currentIndex+1) % len(waypoints)
else: 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.) rospy.sleep(1.)
msg = Goal() msg = Goal()
msg.node_id = int(waypoints[currentIndex]) if waypoints[currentIndex].isdigit():
msg.node_label = "" 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() 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) pub.publish(msg)
def main(): def main():
rospy.init_node('patrol', anonymous=True) rospy.init_node('patrol', anonymous=False)
rospy.Subscriber("/rtabmap/goal_reached", Bool, callback) rospy.Subscriber("/rtabmap/goal_reached", Bool, callback)
rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal
# send the first goal # send the first goal
msg = Goal() msg = Goal()
msg.node_id = int(waypoints[currentIndex]) if waypoints[currentIndex].isdigit():
msg.node_label = "" 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: while rospy.Time.now().secs == 0:
rospy.loginfo("Waiting clock...") rospy.loginfo("Waiting clock...")
rospy.sleep(.1) rospy.sleep(.1)
msg.header.stamp = rospy.Time.now() 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) pub.publish(msg)
rospy.spin() rospy.spin()
if __name__ == '__main__': if __name__ == '__main__':
if len(sys.argv) < 3: 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: else:
waypoints = sys.argv[1:] waypoints = sys.argv[1:]
rospy.loginfo("Waypoints: [%s]", str(waypoints).strip('[]')) rospy.loginfo("Waypoints: [%s]", str(waypoints).strip('[]'))