def make_bins_return(shared): sm = smach.Sequence(["succeeded", "failed", "preempted"], "succeeded") with sm: smach.Sequence.add("RETURN", common_states.ReturnToWaypointState(shared, "bins")) smach.Sequence.add( "WAIT_BINS", legacy_vision_states.WaitForObjectsState(shared, "find2_down_camera", "bins/all", timeout=5), transitions={"timeout": "failed"}, ) smach.Sequence.add("CENTER_ALL", legacy_vision_states.CenterObjectState(shared, "find2_down_camera")) smach.Sequence.add( "ALIGN_ALL", legacy_vision_states.AlignObjectState(shared, "find2_down_camera", body_vec_align=[0, 1, 0]) ) smach.Sequence.add("CENTER_ALL_2", legacy_vision_states.CenterObjectState(shared, "find2_down_camera")) smach.Sequence.add( "TURN_DEPTH", common_states.WaypointState(shared, lambda cur: cur.depth(constants.PIPE_DEPTH).turn_left_deg(90)), ) smach.Sequence.add("APPROACH", common_states.VelocityState(shared, numpy.array([constants.PIPE_SPEED, 0, 0]))) smach.Sequence.add( "WAIT_PIPE", legacy_vision_states.WaitForObjectsState(shared, "find2_down_camera", "pipe"), transitions={"timeout": "failed"}, ) selector = legacy_vision_states.select_by_body_direction([0, 1 if constants.MODE == "competition" else -1, 0]) smach.Sequence.add("CENTER_PIPE", legacy_vision_states.CenterObjectState(shared, "find2_down_camera", selector)) smach.Sequence.add( "TURN_HELP_PIPE", common_states.WaypointState( shared, lambda cur: cur.turn_left_deg(45) if constants.MODE == "competition" else cur.turn_right_deg(45) ), ) return sm
def _make_path_sm(self, shared, path, dist): if path in ('left', 'right'): selector = legacy_vision_states.select_by_body_direction( [0, 1 if path == 'left' else -1, 0]) else: selector = legacy_vision_states.select_first sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm: smach.Sequence.add('WAIT_PIPE_'+path.upper(), legacy_vision_states.WaitForObjectsState(shared, 'find2_down_camera', 'pipe', timeout=5), transitions={'timeout': 'failed'}) smach.Sequence.add('CENTER_PIPE_'+path.upper(), legacy_vision_states.CenterObjectState(shared, 'find2_down_camera', selector)) smach.Sequence.add('ALIGN_PIPE_'+path.upper(), legacy_vision_states.AlignObjectState(shared, 'find2_down_camera', selector)) smach.Sequence.add('SAVE_POS_'+path.upper(), common_states.SaveWaypointState('last_path')) if dist > 0: smach.Sequence.add('OPEN_LOOP', common_states.WaypointState(shared, lambda cur: cur.forward(dist))) return sm
def make_bins_return(shared): sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm: smach.Sequence.add('RETURN', common_states.ReturnToWaypointState(shared, 'bins')) smach.Sequence.add('WAIT_BINS', legacy_vision_states.WaitForObjectsState( shared, 'find2_down_camera', 'bins/all', timeout=5), transitions={'timeout': 'failed'}) smach.Sequence.add( 'CENTER_ALL', legacy_vision_states.CenterObjectState(shared, 'find2_down_camera')) smach.Sequence.add( 'ALIGN_ALL', legacy_vision_states.AlignObjectState(shared, 'find2_down_camera', body_vec_align=[0, 1, 0])) smach.Sequence.add( 'CENTER_ALL_2', legacy_vision_states.CenterObjectState(shared, 'find2_down_camera')) smach.Sequence.add('TURN_DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(constants.PIPE_DEPTH)\ .turn_left_deg(90))) smach.Sequence.add( 'APPROACH', common_states.VelocityState( shared, numpy.array([constants.PIPE_SPEED, 0, 0]))) smach.Sequence.add('WAIT_PIPE', legacy_vision_states.WaitForObjectsState( shared, 'find2_down_camera', 'pipe'), transitions={'timeout': 'failed'}) selector = legacy_vision_states.select_by_body_direction( [0, 1 if constants.MODE == 'competition' else -1, 0]) smach.Sequence.add( 'CENTER_PIPE', legacy_vision_states.CenterObjectState(shared, 'find2_down_camera', selector)) smach.Sequence.add( 'TURN_HELP_PIPE', common_states.WaypointState( shared, lambda cur: cur.turn_left_deg(45) if constants.MODE == 'competition' else cur.turn_right_deg(45))) return sm