class StateMachineNode: def __init__(self): rospy.init_node('state_machine') self.markers_subscriber = rospy.Subscriber(AR_TAG_TOPIC, AlvarMarkers, self.markers_callback, queue_size=10) self.starter_subscriber = rospy.Subscriber(STARTER_TOPIC, Bool, self.start_callback, queue_size=10) self.lights_off = False self.state = State.nothing self.drive = Drive() self.image_processor = ImageProcessor() self.cv_bridge = CvBridge() self.drive_speed = 1.2503309 self.left_goal_dist = 0.90 self.right_goal_dist = 0.8 def markers_callback(self, msg): self.update_state(msg.markers) print("State: %s" % self.state) if self.state == State.follow_middle: self.drive.drive_middle(self.drive_speed) elif self.state == State.follow_left: self.drive.drive_left(self.left_goal_dist, self.drive_speed)
class StateMachineNode: def __init__(self): rospy.init_node('state_machine') self.markers_subscriber = rospy.Subscriber(AR_TAG_TOPIC, AlvarMarkers, self.markers_callback, queue_size=10) self.starter_subscriber = rospy.Subscriber(STARTER_TOPIC, Bool, self.start_callback, queue_size=10) self.lights_off = False self.state = State.nothing self.drive = Drive() self.image_processor = ImageProcessor() self.cv_bridge = CvBridge() self.drive_speed = 1.2503309 self.left_goal_dist = 0.90 self.right_goal_dist = 0.8 def markers_callback(self, msg): self.update_state(msg.markers) print("State: %s" % self.state) if self.state == State.follow_middle: self.drive.drive_middle(self.drive_speed) elif self.state == State.follow_left: self.drive.drive_left(self.left_goal_dist, self.drive_speed) elif self.state == State.follow_right: self.drive.drive_right(self.right_goal_dist, self.drive_speed) elif self.state == State.ptf: if self.drive.is_safe(): self.drive.drive_potential(speed=0.8) else: print("I am not safe. Correcting!") self.drive.drive_safety() elif self.state == State.full_throttle: self.drive.drive_right(self.right_goal_dist, 1.50) elif self.state == State.find_bobs_bricks: self.drive.drive_right(self.right_goal_dist, self.drive_speed) if self.image_processor.process_wall_img(): self.state = State.break_bobs_bricks elif self.state == State.break_bobs_bricks: if self.drive.drive_straight(self.drive_speed, 1.0): self.state = State.follow_left def update_state(self, markers): ids = [m.id for m in markers] print("Tags: %s" % ids) if 12 in ids: self.drive.adjustment = 300 self.drive_speed = 1.1 else: self.drive.adjusment = 30 self.drive.drive_speed = 1.25 if 5 in ids: dist = markers[0].pose.pose.position.z if dist < 1.0: self.state = State.follow.middle if 13 in ids: self.drive.adjustment = 30 self.drive_speed = 1.25 print("adjust %s" % self.drive.adjustment) if 13 in ids: dist = markers[0].pose.pose.position.z print("Dist from marker: %s" % dist) if dist < 2.4: self.state = State.follow_middle if any([x in ids for x in FOLLOW_MIDDLE_TAGS]): self.state = State.follow_middle elif any([x in ids for x in FOLLOW_RIGHT_TAGS]): self.state = State.follow_right elif any([x in ids for x in FOLLOW_LEFT_TAGS]): self.state = State.follow_left elif any([x in ids for x in PTF_TAGS ]) and markers[0].pose.pose.position.z < 2.5: self.state = State.ptf elif any([x in ids for x in FULL_THROTTLE_TAGS]): self.state = State.full_throttle elif self.lights_off and self.state == State.nothing: self.state = State.follow_middle def start_callback(self, msg): if msg.data: self.lights_off = True print("The lights are off.")