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_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.")
