def make_hydrophones(shared):
sm_travel = smach.StateMachine(['succeeded', 'preempted'])
with sm_travel:
smach.StateMachine.add('HYDROPHONES',
hydrophone_states.HydrophoneTravelState(shared, FREQ, FREQ_RANGE),
transitions={'failed': 'GO_AWAY'})
if constants.MODE == 'competition':
smach.StateMachine.add('GO_AWAY',
common_states.WaypointSeriesState(shared,
[lambda cur: cur.set_orientation(orientation_helpers.NORTH).turn_right_deg(22.5),
lambda cur: cur.forward(5)]),
transitions={'succeeded': 'HYDROPHONES'})
else:
smach.StateMachine.add('GO_AWAY',
common_states.WaypointSeriesState(shared,
[lambda cur: cur.set_orientation(orientation_helpers.NORTH).turn_left_deg(22.5),
lambda cur: cur.forward(5)]),
transitions={'succeeded': 'HYDROPHONES'})
sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm:
smach.Sequence.add('RETRACT',
subjugator_states.GasPoweredStickState(False))
smach.Sequence.add('DEPTH',
common_states.WaypointState(shared,
lambda cur: cur.depth(APPROACH_DEPTH)))
smach.Sequence.add('TRAVEL', sm_travel)
return sm
def make_return_path(shared):
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',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.left(2), lambda cur: cur.forward(2),
lambda cur: cur.right(4), lambda cur: cur.backward(2)
],
speed=.3))
smach.Concurrence.add(
'WAIT',
legacy_vision_states.WaitForObjectsState(shared,
'find2_down_camera',
'pipe'))
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('RETURN_WAYPOINT',
common_states.ReturnToWaypointState(
shared, 'last_path'),
transitions={'succeeded': 'SEARCH'})
smach.StateMachine.add('SEARCH', search_sm)
return sm
def make_shooter_approach(shared):
sm = common_states.WaypointSeriesState(shared, [
lambda cur: cur.turn_left_deg(65)
if constants.MODE == 'competition' else cur.turn_left_deg(30),
lambda cur: cur.right(.5)
])
return sm
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_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_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_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