def waypoints_failed_callback(self, waypoint): if not self.wp_queue: rospy.logerr("Waypoint failed but queue is empty.") return head, is_goal = self.wp_queue[0] if point_equals(waypoint, head): self.wp_queue.popleft() if is_goal: self.goals_failed_pub.publish(head) else: rospy.logerr("Waypoint failed but doesn't match head of queue.")
def waypoints_reached_callback(self, waypoint): """Waypoints reached subscription callback.""" if not self.wp_queue: rospy.logerr("Waypoint reached but queue is empty.") return head, is_goal = self.wp_queue[0] rospy.loginfo("Waypoint reached at (%f, %f), is_goal is %s", head.x, head.y, is_goal) if point_equals(waypoint, head): self.wp_queue.popleft() if is_goal: self.goals_reached_pub.publish(head) else: rospy.logerr("Waypoint reached but doesn't match head of queue.")