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
