def make_surface(shared):
sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm:
smach.Sequence.add(
'SURFACE',
common_states.WaypointState(shared, lambda cur: cur.depth(-.1)))
smach.Sequence.add('SLEEP', common_states.SleepState(5))
smach.Sequence.add(
'DOWN',
common_states.WaypointState(shared, lambda cur: cur.depth(.3)))
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_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_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