def main(): rospy.init_node('patrol', anonymous=False) sub = rospy.Subscriber("rtabmap/goal_reached", Bool, callback) global waitingTime global frameId waitingTime = rospy.get_param('~time', waitingTime) frameId = rospy.get_param('~frame_id', frameId) rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal rospy.loginfo(rospy.get_caller_id() + ": Waypoints: [%s]", str(waypoints).strip('[]')) rospy.loginfo(rospy.get_caller_id() + ": time: %f", waitingTime) rospy.loginfo(rospy.get_caller_id() + ": publish goal on %s", pub.resolved_name) rospy.loginfo(rospy.get_caller_id() + ": receive goal status on %s", sub.resolved_name) # send the first goal msg = Goal() msg.frame_id = frameId try: int(waypoints[currentIndex]) is_dig = True except ValueError: is_dig = False if is_dig: 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(rospy.get_caller_id() + ": Waiting clock...") rospy.sleep(.1) msg.header.stamp = rospy.Time.now() rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints)) pub.publish(msg) rospy.spin()
def callback(data): global currentIndex global waitingTime global frameId if data.data: rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' reached! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime) else: rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' failed! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime) currentIndex = (currentIndex+1) % len(waypoints) # Waiting time before sending next goal rospy.sleep(waitingTime) msg = Goal() msg.frame_id = frameId try: int(waypoints[currentIndex]) is_dig = True except ValueError: is_dig = False if is_dig: msg.node_id = int(waypoints[currentIndex]) msg.node_label = "" else: msg.node_id = 0 msg.node_label = waypoints[currentIndex] rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints)) msg.header.stamp = rospy.get_rostime() pub.publish(msg)
def main(): 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() 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 '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints)) pub.publish(msg) rospy.spin()
def callback(data): global currentIndex if data.data: 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 '%s' failed! Publishing next goal in 1 sec...", waypoints[currentIndex]) currentIndex = (currentIndex+1) % len(waypoints) rospy.sleep(1.) msg = Goal() 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 '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints)) pub.publish(msg)