def make_starting_gate(shared):
sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm:
smach.Sequence.add('GO',
common_states.WaypointSeriesState(shared, [lambda cur: cur.depth(constants.PIPE_DEPTH),
lambda cur: cur.forward(10)]))
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'})
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
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([.4, 0, 0])))
smach.Sequence.add('WAIT_SHOOTER',
object_finder_states.WaitForObjectsState(
shared, 'find_forward', shooter_desc_cb, .95),
transitions={'timeout': 'failed'})
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(
'APPROACH_SHOOTER',
object_finder_states.ApproachObjectState(shared,
'find_forward',
'forward_camera',
BOARD_DIST,
marker=color))
smach.Sequence.add(
'OPEN_LOOP_FORWARD',
common_states.WaypointState(
shared,
lambda cur: cur.forward(BOARD_DIST - HEXAGON_DIST)))
smach.Sequence.add('WAIT_HEXAGON',
object_finder_states.WaitForObjectsState(
shared, 'find_forward',
lambda: hexagon_desc_cb(SIZE, color), .99),
transitions={'timeout': 'failed'})
smach.Sequence.add(
'APPROACH_HEXAGON',
object_finder_states.ApproachObjectState(
shared, 'find_forward', 'forward_camera', HEXAGON_DIST))
smach.Sequence.add('OPEN_LOOP_FORWARD2',
common_states.WaypointState(shared,
lambda cur: cur.forward(HEXAGON_DIST-SHOOT_DIST)\
.relative([0, .12, .18]
if shooter == 'left' else
[0, -.12, .18])))
smach.Sequence.add('SHOOT',
subjugator_states.ShootTorpedoState(shooter))
sm_retreat = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_retreat:
smach.Sequence.add(
'RETREAT',
common_states.WaypointState(
shared, lambda cur: cur.backward(BOARD_DIST + 1).depth(DEPTH)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.2, 0, 0])))
smach.Sequence.add('WAIT_SHOOTER',
object_finder_states.WaitForObjectsState(
shared, 'find_forward', shooter_desc_cb, .9),
transitions={'timeout': 'failed'})
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_manipulation(shared):
traj = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
manipulation_desc = TargetDesc()
manipulation_desc.type = TargetDesc.TYPE_OBJECT
manipulation_desc.object_filename = roslib.packages.resource_file(
'auvsi_robosub', 'models', '2013/manipulation.obj')
manipulation_desc.prior_distribution.pose.orientation = Quaternion(
*traj.turn_left_deg(180).orientation)
manipulation_desc.disallow_yawing = True
manipulation_desc.min_dist = BOARD_DIST - 1
manipulation_desc.max_dist = BOARD_DIST + 3
wheel = TargetDesc()
wheel.type = TargetDesc.TYPE_OBJECT
wheel.object_filename = roslib.packages.resource_file(
'auvsi_robosub', 'models', '2013/manipulation_wheel.obj')
wheel.prior_distribution.pose.orientation = Quaternion(
*traj.turn_left_deg(180).orientation)
cov = numpy.zeros((6, 6))
a = numpy.array([traj.forward_vector]).T * 100
cov[3:, 3:] += a.dot(a.T)
wheel.prior_distribution.covariance = cov.flatten()
wheel.min_dist = WHEEL_DIST * .9
wheel.max_dist = WHEEL_DIST / .9
wheel.allow_rolling = True
wheel.disallow_yawing = True
# Create a SMACH state machine
sm = smach.Sequence(['succeeded', 'timeout', 'failed', 'preempted'],
'succeeded')
# Open the container
with sm:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared, lambda cur: cur.depth(1.5)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.2, 0, 0])))
smach.Sequence.add(
'WAIT_MANIPULATION',
object_finder_states.WaitForObjectsState(
shared, 'find_forward', lambda: [manipulation_desc], .95))
smach.Sequence.add(
'STOP', common_states.VelocityState(shared, numpy.array([0, 0,
0])))
smach.Sequence.add('SLEEP', common_states.SleepState(5))
smach.Sequence.add(
'APPROACH_MANIPULATION',
object_finder_states.ApproachObjectState(shared, 'find_forward',
'forward_camera',
BOARD_DIST))
smach.Sequence.add(
'OPEN_LOOP_FORWARD',
common_states.WaypointState(
shared,
lambda cur: cur.forward(BOARD_DIST - WHEEL_DIST).left(.5)))
smach.Sequence.add(
'WAIT_WHEEL',
object_finder_states.WaitForObjectsState(shared, 'find_forward',
lambda: [wheel], .99))
smach.Sequence.add('WAIT_WHEEL_MORE', common_states.SleepState(5))
smach.Sequence.add(
'APPROACH_WHEEL',
object_finder_states.ApproachObjectState(shared, 'find_forward',
'forward_camera',
WHEEL_DIST))
smach.Sequence.add('EXTEND',
subjugator_states.GasPoweredStickState(True))
smach.Sequence.add('OPEN_LOOP_FORWARD2',
common_states.WaypointState(shared,
lambda cur: cur.forward(WHEEL_DIST-TURN_DIST)\
.relative([0, .06, .06])))
smach.Sequence.add(
'TURN',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.down(.2), lambda cur: cur.right(.2),
lambda cur: cur.up(.2), lambda cur: cur.left(.2),
lambda cur: cur.down(.2), lambda cur: cur.right(.2)
]))
smach.Sequence.add('RETRACT',
subjugator_states.GasPoweredStickState(False))
smach.Sequence.add(
'BACKUP',
common_states.WaypointState(shared, lambda cur: cur.backward(1)))
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_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_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_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
def make_buoy(shared):
buoy_sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with buoy_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([.2, 0, 0])))
smach.Sequence.add('WAIT_BUOYS',
object_finder_states.WaitForObjectsState(
shared, 'find_forward', lambda: [buoy_desc],
.90),
transitions={'timeout': 'failed'})
smach.Sequence.add(
'APPROACH_BUOY',
object_finder_states.ApproachObjectState(shared, 'find_forward',
'forward_camera', 2))
smach.Sequence.add(
'BUMP',
common_states.WaypointSeriesState(
shared,
[lambda cur: cur.forward(2.5), lambda cur: cur.backward(.5)]))
smach.Sequence.add(
'OVER',
common_states.WaypointSeriesState(
shared,
[lambda cur: cur.depth(.5), lambda cur: cur.forward(2.5)]))
search_pattern_sm = smach.StateMachine(['preempted'])
with search_pattern_sm:
smach.StateMachine.add(
'START',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.depth(constants.PIPE_DEPTH),
lambda cur: cur.right(SEARCH_WIDTH / 2)
],
speed=constants.PIPE_SPEED),
transitions={'succeeded': 'SEARCH'})
smach.StateMachine.add('SEARCH',
common_states.WaypointSeriesState(
shared, [
lambda cur: cur.left(SEARCH_WIDTH),
lambda cur: cur.forward(SEARCH_ADVANCE),
lambda cur: cur.right(SEARCH_WIDTH),
lambda cur: cur.forward(SEARCH_ADVANCE)
],
speed=constants.PIPE_SPEED),
transitions={'succeeded': 'SEARCH'})
search_sm = smach.Concurrence(
['succeeded', 'failed', 'preempted'],
default_outcome='failed',
outcome_map={'succeeded': {
'WAIT': 'succeeded'
}},
child_termination_cb=lambda so: True)
with search_sm:
smach.Concurrence.add('PATTERN', search_pattern_sm)
smach.Concurrence.add(
'WAIT',
legacy_vision_states.WaitForObjectsState(shared,
'find2_down_camera',
'pipe'))
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('BUOY',
buoy_sm,
transitions={
'succeeded': 'SEARCH',
'failed': 'SEARCH'
})
smach.StateMachine.add('SEARCH', search_sm)
return sm
