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