def make_hedge(shared):
sm_approach = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_approach:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared,
lambda cur: cur.depth(APPROACH_DEPTH)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.2, 0, 0])))
smach.Sequence.add('WAIT_HEDGE',
legacy_vision_states.WaitForObjectsState(
shared,
'find2_forward_camera',
'hedge',
timeout=25),
transitions={'timeout': 'failed'})
smach.Sequence.add('SLEEP',
common_states.SleepState(2)) # Get a bit closer
sm_center = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_center:
smach.Sequence.add(
'CENTER_APPROACH_HEDGE',
legacy_vision_states.CenterApproachObjectState(
shared, 'find2_forward_camera', desired_scale=180))
smach.Sequence.add(
'LEFT',
common_states.WaypointState(shared,
lambda cur: cur.turn_left_deg(90)))
smach.Sequence.add(
'GO', common_states.WaypointState(shared,
lambda cur: cur.right(4)))
smach.Sequence.add(
'RIGHT',
common_states.WaypointState(shared,
lambda cur: cur.turn_right_deg(90)))
sm_pipe = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm_pipe:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(
shared, lambda cur: cur.depth(constants.PIPE_DEPTH)))
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'})
smach.Sequence.add(
'STOP', common_states.WaypointState(shared, lambda cur: cur))
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('APPROACH',
sm_approach,
transitions={
'succeeded': 'CENTER',
'failed': 'PIPE'
})
smach.StateMachine.add('CENTER',
sm_center,
transitions={
'succeeded': 'PIPE',
'failed': 'PIPE'
})
smach.StateMachine.add('PIPE',
sm_pipe,
transitions={'succeeded': 'succeeded'})
return sm
def make_shooter(shared):
# Create a SMACH state machine
sm_approach = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_approach:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared, lambda cur: cur.depth(DEPTH)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.1, 0, 0])))
sm_shoots = []
for color, shooter in zip(COLORS, ['left', 'right']):
sm_shoot = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
sm_shoots.append(sm_shoot)
with sm_shoot:
# smach.Sequence.add('WAIT_SHOOTER',
# legacy_vision_states.WaitForObjectsState(shared, 'find2_forward_camera',
# '/'.join(['shooter', color, 'box'])),
# transitions={'timeout': 'failed'})
# smach.Sequence.add('APPROACH_SHOOTER',
# legacy_vision_states.CenterApproachObjectState(shared, 'find2_forward_camera',
# desired_scale=BOARD_SCALE))
smach.Sequence.add('WAIT_HEXAGON',
legacy_vision_states.WaitForObjectsState(
shared,
'find2_forward_camera',
'/'.join(['shooter_flood', color, SIZE]),
timeout=15),
transitions={'timeout': 'failed'})
smach.Sequence.add(
'APPROACH_HEXAGON',
legacy_vision_states.CenterApproachObjectState(
shared,
'find2_forward_camera',
desired_scale=HEXAGON_SCALE,
gain=.5))
if shooter == 'left':
smach.Sequence.add('OPEN_LOOP_FORWARD',
common_states.WaypointState(shared,
lambda cur: cur.forward(ALIGN_FORWARD)\
.right(ALIGN_STRAFE)\
.up(ALIGN_UP)))
else:
smach.Sequence.add('OPEN_LOOP_FORWARD',
common_states.WaypointState(shared,
lambda cur: cur.forward(ALIGN_FORWARD)\
.left(ALIGN_STRAFE)\
.up(ALIGN_UP)))
smach.Sequence.add('SLEEP', common_states.SleepState(3))
smach.Sequence.add('SHOOT',
subjugator_states.ShootTorpedoState(shooter))
smach.Sequence.add('SLEEP2', common_states.SleepState(3))
sm_retreat = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_retreat:
smach.Sequence.add(
'RETREAT',
common_states.VelocityState(shared, numpy.array([-.2, 0, 0])))
smach.Sequence.add('WAIT', common_states.SleepState(4))
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('APPROACH_SHOOT_1',
sm_approach,
transitions={'succeeded': 'SHOOT_1'})
smach.StateMachine.add('RETREAT_SHOOT_1',
sm_retreat,
transitions={'succeeded': 'SHOOT_1'})
smach.StateMachine.add('SHOOT_1',
sm_shoots[0],
transitions={
'succeeded': 'RETREAT_SHOOT_2',
'failed': 'RETREAT_SHOOT_1'
})
smach.StateMachine.add('RETREAT_SHOOT_2',
sm_retreat,
transitions={'succeeded': 'SHOOT_2'})
smach.StateMachine.add('SHOOT_2',
sm_shoots[1],
transitions={
'succeeded': 'DONE_BACKUP',
'failed': 'RETREAT_SHOOT_2'
})
smach.StateMachine.add('DONE_BACKUP',
common_states.WaypointState(
shared, lambda cur: cur.backward(.5)),
transitions={'succeeded': 'succeeded'})
return sm
def make_bins(shared):
sm_approach = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_approach:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared,
lambda cur: cur.depth(CENTER_DEPTH)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.2, 0, 0])))
smach.Sequence.add('WAIT_ALL',
legacy_vision_states.WaitForObjectsState(
shared, 'find2_down_camera', 'bins/all'),
transitions={'timeout': 'failed'})
smach.Sequence.add('EXTRA_FORWARD', common_states.SleepState(2))
smach.Sequence.add(
'RIGHT_TWEAK',
common_states.WaypointState(shared,
lambda cur: cur.turn_right_deg(25)))
sm_center = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_center:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared,
lambda cur: cur.depth(CENTER_DEPTH)))
smach.Sequence.add('WAIT_ALL',
legacy_vision_states.WaitForObjectsState(
shared,
'find2_down_camera',
'bins/all',
timeout=10),
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('SAVE_POS', common_states.SaveWaypointState('bins'))
sm_drops = []
for bin_string in DROP_ORDER:
sm_drop = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
sm_drops.append(sm_drop)
with sm_drop:
# this could be faster if CenterObjectState let you descend at the same time
smach.Sequence.add('WAIT_SINGLE',
legacy_vision_states.WaitForObjectsState(
shared,
'find2_down_camera',
'bins/single',
timeout=10),
transitions={'timeout': 'failed'})
selector = select_image_text_or_most_central(bin_string)
smach.Sequence.add(
'APPROACH_SINGLE',
legacy_vision_states.CenterApproachObjectState(
shared,
'find2_down_camera',
desired_scale=9000,
selector=selector))
smach.Sequence.add(
'DOWN',
common_states.WaypointState(
shared, lambda cur: cur.depth(3).backward(.15)))
smach.Sequence.add('SLEEP', common_states.SleepState(2))
smach.Sequence.add('DROP', subjugator_states.DropBallState())
smach.Sequence.add('SLEEP2', common_states.SleepState(2))
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('APPROACH',
sm_approach,
transitions={'succeeded': 'CENTER_1'})
smach.StateMachine.add('CENTER_1',
sm_center,
transitions={
'succeeded': 'DROP_1',
'failed': 'APPROACH'
})
smach.StateMachine.add('DROP_1',
sm_drops[0],
transitions={
'succeeded': 'CENTER_2',
'failed': 'RETRY_CENTER_1'
})
smach.StateMachine.add('RETRY_CENTER_1',
common_states.CounterState(1),
transitions={
'succeeded': 'CENTER_1',
'exceeded': 'CENTER_2'
})
smach.StateMachine.add('CENTER_2',
sm_center,
transitions={'succeeded': 'DROP_2'})
smach.StateMachine.add('DROP_2',
sm_drops[1],
transitions={
'succeeded': 'succeeded',
'failed': 'RETRY_CENTER_2'
})
smach.StateMachine.add('RETRY_CENTER_2',
common_states.CounterState(1),
transitions={
'succeeded': 'CENTER_2',
'exceeded': 'succeeded'
})
return sm
def make_manipulation(shared):
sm_wheel = smach.Sequence(['succeeded', 'timeout', 'failed', 'preempted'],
'succeeded')
with sm_wheel:
smach.Sequence.add(
'OPEN_LOOP_FORWARD',
common_states.WaypointState(shared,
lambda cur: cur.forward(1.9),
speed=.25))
smach.Sequence.add(
'WAIT_WHEEL',
legacy_vision_states.WaitForObjectsState(shared,
'find2_forward_camera',
'grapes/grape',
timeout=10))
smach.Sequence.add(
'APPROACH_WHEEL',
legacy_vision_states.CenterObjectState(shared,
'find2_forward_camera',
gain=.2))
smach.Sequence.add('EXTEND',
subjugator_states.GasPoweredStickState(True))
smach.Sequence.add('OPEN_LOOP_FORWARD2',
common_states.WaypointState(shared,
lambda cur: cur.forward(.85)\
.relative([0, -0.02, -0.02])
.relative([0, .075, .075]), speed=.1))
smach.Sequence.add(
'TURN',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.left(.1), lambda cur: cur.down(.3),
lambda cur: cur.right(.45), lambda cur: cur.up(.45),
lambda cur: cur.left(.45), lambda cur: cur.down(.45),
lambda cur: cur.right(.45)
],
speed=.1))
smach.Sequence.add('RETRACT',
subjugator_states.GasPoweredStickState(False))
sm_move_lever = smach.StateMachine(
['succeeded', 'timeout', 'failed', 'preempted'], input_keys=['z'])
with sm_move_lever:
smach.StateMachine.add('COMPARE_Z',
CompareZState(),
transitions=dict(
higher='GO_DOWN',
lower='GO_UP',
))
smach.StateMachine.add(
'GO_UP',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.right(.08),
lambda cur: cur.down(.15),
lambda cur: cur.forward(1),
lambda cur: cur.up(.3),
lambda cur: cur.down(1),
]))
smach.StateMachine.add(
'GO_DOWN',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.right(.08),
lambda cur: cur.up(.15),
lambda cur: cur.forward(1),
lambda cur: cur.down(.3),
lambda cur: cur.up(1),
]))
sm_lever = smach.Sequence(['succeeded', 'timeout', 'failed', 'preempted'],
'succeeded')
with sm_lever:
smach.Sequence.add(
'OPEN_LOOP_FORWARD',
common_states.WaypointState(shared, lambda cur: cur.forward(1.9)))
smach.Sequence.add('SAVE_Z', SaveZState())
smach.Sequence.add(
'WAIT_LEVER',
legacy_vision_states.WaitForObjectsState(shared,
'find2_forward_camera',
'grapes/lever',
timeout=10))
smach.Sequence.add(
'APPROACH_LEVER',
legacy_vision_states.CenterObjectState(shared,
'find2_forward_camera',
gain=.2))
smach.Sequence.add('MOVE_LEVER', sm_move_lever)
# Create a SMACH state machine
sm = smach.Sequence(['succeeded', 'timeout', 'failed', 'preempted'],
'succeeded')
with sm:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared, lambda cur: cur.depth(2.5)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.4, 0, 0])))
smach.Sequence.add(
'WAIT_MANIPULATION',
legacy_vision_states.WaitForObjectsState(shared,
'find2_forward_camera',
'grapes/board',
timeout=15))
smach.Sequence.add(
'APPROACH_MANIPULATION',
legacy_vision_states.CenterApproachObjectState(
shared, 'find2_forward_camera', desired_scale=130e3 / 3**2))
smach.Sequence.add(
'YAW',
common_states.WaypointState(
shared, lambda cur: cur.heading(
math.radians(-179 +
(180 if cur.forward_vector[0] > 0 else 0)))))
smach.Sequence.add(
'APPROACH_MANIPULATION_POST_YAW',
legacy_vision_states.CenterApproachObjectState(
shared, 'find2_forward_camera', desired_scale=130e3 / 3**2))
smach.Sequence.add('WHEEL',
sm_wheel,
transitions=dict(failed='BACKUP'))
smach.Sequence.add(
'BACKUP',
common_states.WaypointState(shared, lambda cur: cur.backward(3)))
smach.Sequence.add(
'REFIND',
common_states.VelocityState(shared, numpy.array([.1, 0, 0])))
smach.Sequence.add(
'WAIT_MANIPULATION2',
legacy_vision_states.WaitForObjectsState(shared,
'find2_forward_camera',
'grapes/board',
timeout=15))
smach.Sequence.add(
'APPROACH_MANIPULATION2',
legacy_vision_states.CenterApproachObjectState(
shared, 'find2_forward_camera', desired_scale=130e3 / 3**2))
smach.Sequence.add('LEVER', sm_lever)
smach.Sequence.add(
'BACKUP2',
common_states.WaypointState(shared, lambda cur: cur.backward(1)))
return sm
def make_grabber(shared):
sm_approach = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_approach:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared,
lambda cur: cur.depth(APPROACH_DEPTH)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.2, 0, 0])))
smach.Sequence.add('WAIT_PIZZA',
legacy_vision_states.WaitForObjectsState(
shared,
'find2_down_camera',
'wreath',
timeout=9999),
transitions={'timeout': 'failed'})
sm_descend_grab = smach.Sequence(
['succeeded', 'failed', 'empty', 'preempted'], 'succeeded')
with sm_descend_grab:
smach.Sequence.add(
'CENTER_APPROACH_PIZZA',
legacy_vision_states.CenterApproachObjectState(shared,
'find2_down_camera',
desired_scale=90,
gain=1))
smach.Sequence.add(
'ALIGN_PIZZA',
legacy_vision_states.AlignObjectState(shared, 'find2_down_camera'))
smach.Sequence.add(
'CENTER_APPROACH_PIZZA2',
legacy_vision_states.CenterApproachObjectState(shared,
'find2_down_camera',
desired_scale=120,
gain=.5))
smach.Sequence.add('OPEN_GRABBER',
subjugator_states.OpenGrabberState())
smach.Sequence.add(
'DOWN',
common_states.WaypointState(shared, lambda cur: cur.down(1.1)))
smach.Sequence.add('WAIT', common_states.SleepState(1))
smach.Sequence.add('CLOSE_GRABBER',
subjugator_states.CloseGrabberState())
sm_extra_grab = smach.Sequence(['succeeded', 'empty', 'preempted'],
'succeeded')
with sm_extra_grab:
smach.Sequence.add('OPEN_GRABBER',
subjugator_states.OpenGrabberState())
smach.Sequence.add(
'DOWN',
common_states.WaypointState(shared, lambda cur: cur.down(.1)))
smach.Sequence.add('WAIT', common_states.SleepState(1))
smach.Sequence.add('CLOSE_GRABBER',
subjugator_states.CloseGrabberState())
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('APPROACH',
sm_approach,
transitions={'succeeded': 'DESCEND_GRAB'})
smach.StateMachine.add('DESCEND_GRAB',
sm_descend_grab,
transitions={
'succeeded': 'UP',
'failed': 'APPROACH',
'empty': 'EXTRA_GRAB'
})
smach.StateMachine.add('EXTRA_GRAB',
sm_extra_grab,
transitions={
'succeeded': 'UP',
'empty': 'RETRY_GRAB_COUNTER'
})
smach.StateMachine.add(
'UP', common_states.WaypointState(shared,
lambda cur: cur.depth(.3)))
smach.StateMachine.add('RETRY_GRAB_COUNTER',
common_states.CounterState(1),
transitions={
'succeeded': 'DESCEND_GRAB',
'exceeded': 'APPROACH'
})
return sm