Пример #1
0
from auvsi_robosub import subjugator_states
from uf_smach import common_states, legacy_vision_states, missions

import numpy
import smach


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


missions.register_factory('surface', make_surface)
Пример #2
0
            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
                           
missions.register_factory('shooter', make_shooter)
Пример #3
0
        )
        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


missions.register_factory("bins", make_bins)
missions.register_factory("bins_return", make_bins_return)
Пример #4
0
    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_hydrophones_close(shared):
    sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')

    with sm:
        smach.Sequence.add('DEPTH',
                           common_states.WaypointState(shared,
                                                       lambda cur: cur.depth(1.5)))
        smach.Sequence.add('HYDROPHONES_TRAVEL',
                           hydrophone_states.HydrophoneTravelState(shared, FREQ, FREQ_RANGE),
                           transitions={'failed': 'HYDROPHONES_TRAVEL'})
        smach.Sequence.add('HYDROPHONES_APPROACH',
                           hydrophone_states.HydrophoneApproachState(shared, FREQ, FREQ_RANGE),
                           transitions={'failed': 'succeeded'}) # Ensure we drop
        smach.Sequence.add('BACKUP',
                           common_states.WaypointState(shared,
                                                       lambda cur: cur.backward(.3)))
    return sm

missions.register_factory('hydrophone_grab', make_hydrophones)
missions.register_factory('hydrophone_drop', make_hydrophones_close)
Пример #5
0
                                                       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

missions.register_factory('manipulation', make_manipulation)
Пример #6
0
        ['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


missions.register_factory('return_path', make_return_path)
Пример #7
0
from auvsi_robosub import subjugator_states
from uf_smach import common_states, legacy_vision_states, missions

import numpy
import smach

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

missions.register_factory('surface', make_surface)
Пример #8
0
    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(0.3)))
        smach.StateMachine.add(
            "RETRY_GRAB_COUNTER",
            common_states.CounterState(1),
            transitions={"succeeded": "DESCEND_GRAB", "exceeded": "APPROACH"},
        )
    return sm


def make_grabber_drop(shared):
    sm = smach.Sequence(["succeeded", "failed", "preempted"], "succeeded")
    with sm:
        smach.Sequence.add("DEPTH", common_states.WaypointState(shared, lambda cur: cur.depth(2)))
        smach.Sequence.add("OPEN_GRABBER", subjugator_states.OpenGrabberState())
        smach.Sequence.add("UP_DEPTH", common_states.WaypointState(shared, lambda cur: cur.depth(0.5)))
    return sm


missions.register_factory("grabber", make_grabber)
missions.register_factory("grabber_drop", make_grabber_drop)
Пример #9
0
                                                                 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

missions.register_factory('buoy', make_buoy)
Пример #10
0
    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


missions.register_factory("hedge", make_hedge)
Пример #11
0
                                   '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_grabber_drop(shared):
    sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
    with sm:
        smach.Sequence.add(
            'DEPTH',
            common_states.WaypointState(shared, lambda cur: cur.depth(2)))
        smach.Sequence.add('OPEN_GRABBER',
                           subjugator_states.OpenGrabberState())
        smach.Sequence.add(
            'UP_DEPTH',
            common_states.WaypointState(shared, lambda cur: cur.depth(.5)))
    return sm


missions.register_factory('grabber', make_grabber)
missions.register_factory('grabber_drop', make_grabber_drop)
Пример #12
0
            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([0.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


missions.register_factory("manipulation_thresh", make_manipulation)
Пример #13
0
from auvsi_robosub import subjugator_states, constants
from uf_smach import common_states, legacy_vision_states, missions

import numpy
import smach


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


missions.register_factory("starting_gate", make_starting_gate)
Пример #14
0
                               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


missions.register_factory('buoy', make_buoy)
Пример #15
0
                                   '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_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


missions.register_factory('shooter', make_shooter)
missions.register_factory('shooter_approach', make_shooter_approach)
Пример #16
0
                                                   '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


missions.register_factory('bins', make_bins)
missions.register_factory('bins_return', make_bins_return)
Пример #17
0
            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


missions.register_factory('manipulation', make_manipulation)
Пример #18
0
                           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_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

missions.register_factory('shooter', make_shooter)
missions.register_factory('shooter_approach', make_shooter_approach)
Пример #19
0
    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


missions.register_factory('shooter_pf', make_shooter)
Пример #20
0
from auvsi_robosub import subjugator_states, constants
from uf_smach import common_states, legacy_vision_states, missions

import numpy
import smach

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

missions.register_factory('starting_gate', make_starting_gate)
    
Пример #21
0
            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


missions.register_factory('hedge', make_hedge)
Пример #22
0
        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=0.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


missions.register_factory("return_path", make_return_path)