def _test_smach_execute2():
    rospy.init_node('smach')

    rospy.on_shutdown(myhook)

    sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
                  connector_outcome='succeeded')

    with sq:
        Sequence.add('CHECK',
                     CheckSmachEnabledState(),
                     transitions={'aborted': 'SLEEP'})
        Sequence.add('SLEEP', SleepState(10))

    rospy.loginfo("Executing test...")
    TransformListenerSingleton.get()
    #outcome = sq.execute()

    #rospy.loginfo("outcome: %s" % outcome)

    # Create and start the introspection server
    sis = smach_ros.IntrospectionServer('server_name', sq, '/SM_ROOT')
    sis.start()

    # Execute the state machine
    print "EXECUTING.."
    outcome = sq.execute()

    # Wait for ctrl-c to stop the application
    print "SPINNING.ms."
    rospy.spin()
    #rospy.signal_shutdown("shutting down now")
    sis.stop()
Esempio n. 2
0
def navigate_behind_cube(target, cube, squares):
    sm = Sequence(outcomes=['ok', 'err'], connector_outcome='ok')
    with sm:
        Sequence.add('choose', ChooseNewNavGoalState(target, cube, squares,
                                                     0.5))
        Sequence.add('goto', NavigateToGoalState())
    return sm
Esempio n. 3
0
def main():
    rospy.init_node('smach_example_state_machine')

    init_robot_pose()
    move_base_override.init()

    reset_robot_actuators()

    odom_sub = rospy.Subscriber('/odom', Odometry, odometry_cb)

    sq = Sequence(outcomes=[Transitions.SUCCESS, Transitions.FAILURE],
                  connector_outcome=Transitions.SUCCESS)
    with sq:
        Sequence.add('waiting', WaitStartState())

        for i in range(3):
            Sequence.add('fishing {}'.format(i), create_fish_sequence())
#        Sequence.add('inner_door', create_door_state_machine(0.3))
#        Sequence.add('outer_door', create_door_state_machine(0.6))

    # Create and start the introspection server
    sis = IntrospectionServer('strat', sq, '/strat')
    sis.start()

    # Execute the state machine
    outcome = sq.execute()

    # Wait for ctrl-c to stop the application
    rospy.spin()
    sis.stop()
Esempio n. 4
0
def create_sequence(list_of_states, connector_outcome="succeeded", state_names=[]):
    """Creates a Sequence (container type) that connects the provided states.
    The first state in the list will also be the first state in the sequence, and so on.
    All states in list_of_states must have the outcomes ["preempted", "succeeded", "aborted"].

    Args:
        list_of_states (list[State]): states that should be connected in a sequence
        connector_outcome (str, optional):
                The outcome that causes a transition to the next state. Defaults to "succeeded".
        state_names (list, optional):
                names that the states in the sequence are given. Defaults to [].

    Returns:
        Sequence: a Sequence of connected states
    """
    container = Sequence(
        outcomes=["preempted", "succeeded", "aborted"],
        connector_outcome=connector_outcome,
    )
    with container:
        if len(list_of_states) == len(state_names):
            for name, state in zip(state_names, list_of_states):
                Sequence.add(name, state)
        else:  # no state names provided
            counter = 0
            for state in list_of_states:
                Sequence.add("State-%d" % counter, state)
                counter += 1
    return container
Esempio n. 5
0
def move_to_obstacle():  # type: () -> StateMachine
    sq = Sequence(outcomes=['ok'], connector_outcome='ok')
    with sq:
        Sequence.add('FOLLOW', LineFollowState(forward_speed, PIDController(kp=kp, ki=ki, kd=kd), white_filter,
                                               TransitionAt(proximity_detector)))
        Sequence.add('STOP', StopState())
    return sq
Esempio n. 6
0
def main():
    rospy.init_node('smach_example_state_machine')

    init_robot_pose()
    move_base_override.init()

    reset_robot_actuators()

    odom_sub = rospy.Subscriber('/odom', Odometry, odometry_cb)

    sq = Sequence(outcomes=[Transitions.SUCCESS, Transitions.FAILURE],
                  connector_outcome=Transitions.SUCCESS)
    with sq:
        Sequence.add('waiting', WaitStartState())

        for i in range(3):
            Sequence.add('fishing {}'.format(i), create_fish_sequence())


#        Sequence.add('inner_door', create_door_state_machine(0.3))
#        Sequence.add('outer_door', create_door_state_machine(0.6))

# Create and start the introspection server
    sis = IntrospectionServer('strat', sq, '/strat')
    sis.start()

    # Execute the state machine
    outcome = sq.execute()

    # Wait for ctrl-c to stop the application
    rospy.spin()
    sis.stop()
def _test_smach_execute2():
    rospy.init_node('smach')
    
    rospy.on_shutdown(myhook)
    
    sq = Sequence(
        outcomes = ['succeeded','aborted','preempted'],
        connector_outcome = 'succeeded')
    
    with sq:
        Sequence.add('CHECK', CheckSmachEnabledState(),
                     transitions={'aborted':'SLEEP'})
        Sequence.add('SLEEP', SleepState(10))
    
    rospy.loginfo("Executing test...")
    TransformListenerSingleton.get()
    #outcome = sq.execute()
    
    #rospy.loginfo("outcome: %s" % outcome)
       
    # Create and start the introspection server
    sis = smach_ros.IntrospectionServer('server_name', sq, '/SM_ROOT')
    sis.start()
    
    
    # Execute the state machine
    print "EXECUTING.."
    outcome = sq.execute()

    # Wait for ctrl-c to stop the application
    print "SPINNING.ms."
    rospy.spin()
    #rospy.signal_shutdown("shutting down now")
    sis.stop()
def main():
    rospy.init_node('tinker_mission_follow')
    rospy.loginfo(colored('starting follow and guide task ...', 'green'))

    # Main StateMachine
    state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
    with state:
        StateMachine.add('Start_Button', MonitorStartButtonState(), 
                transitions={'valid': 'Start_Button', 'invalid': '1_Start'})
        StateMachine.add('1_Start', MonitorKinectBodyState(), 
                transitions={'valid':'1_Start', 'invalid':'Sequence'})
        sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded')
        with sequence:
            follow_concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
                default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=[])

            with follow_concurrence:
                Concurrence.add('FollowMe', FollowMeState())
                Concurrence.add('KeyWordsRecognition', KeywordsRecognizeState('stop'))

            Sequence.add('Follow_concurrence', follow_concurrence)
	   
        StateMachine.add('Sequence', sequence, {'succeeded': 'succeeded', 'aborted': 'aborted'})

    # Run state machine introspection server for smach viewer
    intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation')
    intro_server.start()

    outcome = state.execute()
    rospy.spin()
    intro_server.stop()
Esempio n. 9
0
def on_ramp():  # type: () -> StateMachine
    sq = Sequence(outcomes=['ok'], connector_outcome='ok')
    with sq:
        Sequence.add('GOAL_ONRAMP',
                     NavigateToNamedPoseState('on_ramp'),
                     transitions={'err': 'ABSORB'})
        Sequence.add('ABSORB', AbsorbResultState())
    return sq
Esempio n. 10
0
def move_to_stop_line(lower=False):  # type: (bool) -> StateMachine
    detector = RedDetector() if not lower else RedLowerDetector()
    sq = Sequence(outcomes=['ok'], connector_outcome='ok')
    with sq:
        # Forward until stop line
        Sequence.add('FOLLOW', LineFollowState(forward_speed, PIDController(kp=kp, ki=ki, kd=kd), white_filter,
                                               TransitionAfter(detector)))
        Sequence.add('STOP', StopState())
    return sq
Esempio n. 11
0
 def execute(self, ud):
     sq = Sequence(outcomes=['ok', 'err'], connector_outcome='ok')
     with sq:
         Sequence.add('SELECT_NUMBER', SelectNumberState(min=1, max=8))
         Sequence.add('GOAL', NavigateToNumberState())
     outcome = sq.execute(ud)
     if outcome == 'ok':
         notify_unmarked()
     return 'ok'
Esempio n. 12
0
def main():
    rospy.init_node('mission_planner')
    sq = Sequence(outcomes=['ok', 'err'],
                  connector_outcome='ok',
                  input_keys=['coord'])
    with sq:
        Sequence.add('move-to-coord', MoveRoverToCoord())

    sq.execute({'coord': (1.0, 1.0)})
Esempio n. 13
0
File: paint.py Progetto: GUTeamU/TP3
def ext_plan():
	sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded')
	sq.userdata.position = EXT_POSITION;

	plan = gensm_plan_vis_exec(PlanExtAxisState(), confirm=CONFIRM, visualize=VISUALIZE, execute=EXECUTE)
	with sq:
		Sequence.add("EXTA", plan)

	return sq
Esempio n. 14
0
def add_waypoints(waypoints):
    for key, (x, y), angle in waypoints:
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = 'odom'
        goal.target_pose.pose.position.x = x
        goal.target_pose.pose.position.y = y
        goal.target_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, radians(angle)))

        Sequence.add(key, SimpleActionState('move_base', MoveBaseAction, goal=goal),
                transitions={'preempted': Transitions.FAILURE, 'aborted': Transitions.FAILURE})
Esempio n. 15
0
 def execute(self, ud):
     sq = Sequence(outcomes=['ok', 'err'],
                   connector_outcome='ok',
                   output_keys=['green_shape'])
     with sq:
         Sequence.add('LOCATION2',
                      Location2State(),
                      transitions={'err': 'TURN'})
         Sequence.add('TURN', RotateState(np.pi))
     return sq.execute(ud)
Esempio n. 16
0
 def execute(self, ud):
     sq = Sequence(outcomes=['ok', 'err'], connector_outcome='ok')
     with sq:
         Sequence.add('GOAL_OFFRAMP_START',
                      NavigateToNamedPoseState('off_ramp_start'),
                      transitions={'err': 'GOAL_OFFRAMP_END'})
         Sequence.add('GOAL_OFFRAMP_END',
                      NavigateToNamedPoseState('off_ramp_end'))
     sq.execute(ud)
     return 'ok'
Esempio n. 17
0
def main():
    # Make a placeholder state machine for now. Can replace with something
    # interesting later.
    rospy.init_node('state_machine')
    sm = Sequence(outcomes=['preempted'], connector_outcome='ok')
    with sm:
        Sequence.add('forward', forward())
        Sequence.add('turn', turn(), {'ok': 'forward'})

    sm.execute()
Esempio n. 18
0
def location2():  # type: () -> StateMachine
    sq = Sequence(outcomes=['ok'],
                  connector_outcome='ok',
                  output_keys=['green_shape'])
    with sq:
        Sequence.add('LOCATION2',
                     Location2State(),
                     transitions={'err': 'TURN'})
        Sequence.add('TURN', RotateState(np.pi))
    return sq
Esempio n. 19
0
def GoMoveit(robot, arms):

    sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
                  input_keys=['side', 'trayectory_name'],
                  connector_outcome='succeeded')

    with sq:
        Sequence.add('TRAYECTORY',
                     basic.SetPositionNamed(robot, arms, blind=False))
    return sq
Esempio n. 20
0
def GoHomeSafe(robot, arms):

    sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
                  input_keys=['side', 'joint_goal'],
                  connector_outcome='succeeded')

    sq.userdata.home_safe = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

    with sq:
        Sequence.add(
            'PRE2_PRE1',
            basic.SetPositionNamed(robot,
                                   arms,
                                   blind=True,
                                   init='pre_2',
                                   goal='pre_1'))
        Sequence.add('SLEEP', basic.Sleep(robot, 1))
        Sequence.add('PRE1_HOME',
                     basic.JointGoal(robot),
                     remapping={
                         'joint_goal': 'joint_goal',
                         'side': 'side'
                     })
        Sequence.add('HOME',
                     basic.JointGoal(robot),
                     remapping={
                         'joint_goal': 'home_safe',
                         'side': 'side'
                     })
    return sq
Esempio n. 21
0
 def execute(self, ud):
     sq = Sequence(outcomes=['ok'], connector_outcome='ok')
     with sq:
         Sequence.add(
             'FOLLOW',
             LineFollowState(forward_speed,
                             PIDController(kp=kp, ki=ki,
                                           kd=kd), white_filter,
                             TransitionAt(proximity_detector)))
         Sequence.add('STOP', StopState())
     return sq.execute(ud)
Esempio n. 22
0
def off_ramp():  # type: () -> StateMachine
    sq = Sequence(outcomes=['ok'], connector_outcome='ok')
    with sq:
        Sequence.add('INIT_AMCL', InitAMCLState(initial_pose=lambda: get_named_pose('initial_pose'), clear_costmaps=True))
        Sequence.add('GOAL_OFFRAMP_START', NavigateToNamedPoseState('off_ramp_start'), transitions={'err': 'GOAL_OFFRAMP_END'})
        Sequence.add('GOAL_OFFRAMP_END', NavigateToNamedPoseState('off_ramp_end'), transitions={'err': 'ABSORB'})
        Sequence.add('ABSORB', AbsorbResultState())
    return sq
Esempio n. 23
0
def get_go_and_return_smach():
    sq = Sequence(outcomes=['aborted', 'preempted'],
                  connector_outcome='succeeded')

    sq.userdata.goal_position_x = 1
    sq.userdata.goal_position_y = 0
    sq.userdata.goal_position_yaw = 1
    sq.userdata.saved_position_x = 1
    sq.userdata.saved_position_y = 0
    sq.userdata.saved_position_yaw = 1

    wfg = move_base.WaitForGoalState() # We don't want multiple subscribers so we need one WaitForGoal state

    with sq:
        ### Add states to the container

        # save position
        Sequence.add('SAVE_ROBOT_POSITION', move_base.ReadRobotPositionState(),
                     remapping={'x':'saved_position_x',
                                'y':'saved_position_y',
                                'yaw':'saved_position_yaw'},
                     )

        # wait for new goal
        Sequence.add('WAIT_FOR_GOAL', wfg,
                     remapping={'x':'goal_position_x',
                                'y':'goal_position_y',
                                'yaw':'goal_position_yaw'}
                     )

        # nav to goal
        Sequence.add('MOVE_BASE_GO', move_base.MoveBaseState('/map'),
                     remapping={'x':'goal_position_x',
                                'y':'goal_position_y',
                                'yaw':'goal_position_yaw'
                                }
                     )

        # wait
        Sequence.add('PAUSE_AT_GOAL', util.SleepState(2))

        # nav back
        Sequence.add('MOVE_BASE_RETURN', move_base.MoveBaseState('/map'),
                     remapping={'x':'saved_position_x',
                                'y':'saved_position_y',
                                'yaw':'saved_position_yaw'
                                },
                     transitions={'succeeded':'SAVE_ROBOT_POSITION'}
                     )

    return sq
Esempio n. 24
0
def enter_split():  # type: () -> StateMachine
    sq = Sequence(outcomes=['ok'], connector_outcome='ok')
    with sq:
        Sequence.add('FOLLOW_W', LineFollowState(forward_speed, PIDController(kp=kp, ki=ki, kd=kd), white_filter,
                                                 TransitionAt(RedDetector())))
        Sequence.add('FOLLOW_R', LineFollowState(forward_speed, PIDController(kp=kp, ki=ki, kd=kd), red_filter,
                                                 TransitionAfter(RedDetector())))
        Sequence.add('STOP', StopState())
        Sequence.add('TURN', RotateState(np.pi / 2 * 2 / 3))
    return sq
Esempio n. 25
0
def add_waypoints(waypoints):
    for key, (x, y), angle in waypoints:
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = 'odom'
        goal.target_pose.pose.position.x = x
        goal.target_pose.pose.position.y = y
        goal.target_pose.pose.orientation = Quaternion(
            *quaternion_from_euler(0, 0, radians(angle)))

        Sequence.add(key,
                     SimpleActionState('move_base', MoveBaseAction, goal=goal),
                     transitions={
                         'preempted': Transitions.FAILURE,
                         'aborted': Transitions.FAILURE
                     })
def main():
    rospy.init_node('tinker_mission_follow')
    rospy.loginfo(colored('starting follow and guide task ...', 'green'))

    # Main StateMachine
    state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
    with state:
        StateMachine.add('Start_Button',
                         MonitorStartButtonState(),
                         transitions={
                             'valid': 'Start_Button',
                             'invalid': '1_Start'
                         })
        StateMachine.add('1_Start',
                         MonitorKinectBodyState(),
                         transitions={
                             'valid': '1_Start',
                             'invalid': 'Sequence'
                         })
        sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
                            connector_outcome='succeeded')
        with sequence:
            follow_concurrence = Concurrence(
                outcomes=['succeeded', 'aborted', 'preempted'],
                default_outcome='succeeded',
                child_termination_cb=lambda x: True,
                input_keys=[])

            with follow_concurrence:
                Concurrence.add('FollowMe', FollowMeState())
                Concurrence.add('KeyWordsRecognition',
                                KeywordsRecognizeState('stop'))

            Sequence.add('Follow_concurrence', follow_concurrence)

        StateMachine.add('Sequence', sequence, {
            'succeeded': 'succeeded',
            'aborted': 'aborted'
        })

    # Run state machine introspection server for smach viewer
    intro_server = IntrospectionServer('tinker_mission_navigation', state,
                                       '/tinker_mission_navigation')
    intro_server.start()

    outcome = state.execute()
    rospy.spin()
    intro_server.stop()
Esempio n. 27
0
 def execute(self, ud):
     seq = Sequence(outcomes=['ok', 'err'], connector_outcome='ok')
     with seq:
         for index, waypoint in enumerate(ud.waypoints):  # type: int
             seq.add('NavigateTo{}'.format(index),
                     NavigateToWaypointState(waypoint))
     return seq.execute()
Esempio n. 28
0
    def test_action_client_timeout(self):
        """Test simple action state server timeout"""
        sq = Sequence(['succeeded', 'aborted', 'preempted'], 'succeeded')

        sq.userdata['g1'] = g1

        with sq:
            # Test single goal policy
            Sequence.add(
                'GOAL_STATIC',
                SimpleActionState("reference_action_not_available",
                                  TestAction,
                                  goal=g1,
                                  server_wait_timeout=rospy.Duration(1.0)))

        sq_outcome = sq.execute()
Esempio n. 29
0
 def run(self):
     sq = Sequence(outcomes=['Completed_Successfully', 'Aborted'],
                   connector_outcome='Completed_Successfully')
     with sq:
         # Iterate through each dist in front, going that dist
         # and then drawing a dot.
         for i, dist in enumerate(self.dists_in_front):
             go_fwd_lbl = 'Go Forward %d' % i
             sleep_lbl = 'Sleep %d' % i
             Sequence.add(go_fwd_lbl,
                          GoForward(dist),
                          transitions={'Aborted': 'Aborted'})
             Sequence.add(sleep_lbl,
                          Draw(),
                          transitions={'Aborted': 'Aborted'})
     # start the state machine
     return sq.execute()
Esempio n. 30
0
def get_random_goal_smach(frame="/base_link"):
    """Return a SMACH Sequence for navigation to a newly randomly calulated goal.
    Combines CalcRandomGoalState with MoveBaseState

    frame: defaults to /base_link
    """
    sq = Sequence(outcomes=["succeeded", "aborted", "preempted"], connector_outcome="succeeded")

    sq.userdata.x = 0
    sq.userdata.y = 0
    sq.userdata.yaw = 0

    with sq:
        # implicit usage of above userdata
        Sequence.add("CALC_RANDOM_GOAL", CalcRandomGoalState())
        Sequence.add("MOVE_RANDOM_GOAL", MoveBaseState(frame))
    return sq
Esempio n. 31
0
def get_random_goal_smach(frame='/base_link'):
    """Return a SMACH Sequence for navigation to a newly randomly calulated goal.
    Combines CalcRandomGoalState with MoveBaseState

    frame: defaults to /base_link
    """
    sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded')

    sq.userdata.x = 0
    sq.userdata.y = 0
    sq.userdata.yaw = 0

    with sq:
        # implicit usage of above userdata
        Sequence.add("CALC_RANDOM_GOAL", CalcRandomGoalState())
        Sequence.add("MOVE_RANDOM_GOAL", MoveBaseState(frame))
    return sq
Esempio n. 32
0
def location1(cam_pixel_to_point):  # type: (CamPixelToPointServer) -> StateMachine
    sq = Sequence(outcomes=['ok'], connector_outcome='ok')
    with sq:
        Sequence.add('TURN_L', RotateState(np.pi/2))
        Sequence.add('LOCATION1', Location1State(cam_pixel_to_point), transitions={'err': 'TURN_R'})
        Sequence.add('TURN_R', RotateState(-np.pi/2))
    return sq
Esempio n. 33
0
    def draw_points(self, waypoints):
        """ Draws the given waypoints.
            waypoints : a list of point objects with x, y, and z fields.
        """
        rows = self.extract_rows(waypoints)
        sq = Sequence(outcomes=['Completed_Successfully', 'Aborted'],
                      connector_outcome='Completed_Successfully')

        list_of_rows = []
        # How far did we travel?
        dists_back = []
        for row in rows:
            abs_dists_in_front = [point.y for point in row]
            # Keep track of the farthest distance traveled
            dists_back.append(abs_dists_in_front[-1])
            dists_in_front = self.abs_to_rel(abs_dists_in_front)
            list_of_rows.append(dists_in_front)

        # assume all lines are spaced equally, and that lines are
        # seperated perfectly on the x axis
        line_spacing = abs(rows[0][0].x - rows[1][0].x)
        print 'Line Spacing: %f' % line_spacing

        with sq:
            # Turn to the left initially to start things off.
            Sequence.add('Initial Turn',
                         Turn(-1 * math.pi / 2),
                         transitions={'Aborted': 'Aborted'})

            for i, row in enumerate(rows):
                print 'Added row ' + str(i) + ' to State Machine'
                dists_in_front = list_of_rows[i]

                # Draw a Row
                Sequence.add('Draw Row %d' % i,
                             DrawRow(dists_in_front),
                             transitions={'Aborted': 'Aborted'})
                # If we're on the last line, no need to return
                if i < len(rows) - 1:
                    # Return back to the beginning of row
                    Sequence.add('Go Back %d' % i,
                                 GoForward(-1 * dists_back[i]),
                                 transitions={'Aborted': 'Aborted'})
                    # Go to the next line
                    Sequence.add('Carriage Return %d' % i,
                                 CarriageReturn(line_spacing),
                                 transitions={'Aborted': 'Aborted'})

        return sq.execute()
Esempio n. 34
0
File: paint.py Progetto: GUTeamU/TP3
def away_plan():
	sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded')

	pose = PoseStamped()
	pose.header.frame_id = 'base_link'
	pose.pose.position.x = AWAY_X
	pose.pose.position.y = AWAY_Y
	pose.pose.position.z = AWAY_Z
	pose.pose.orientation = DRAW_ORIENTATION

	sq.userdata.goal = pose
	sq.userdata.ik_link = AWAY_HAND_LINK
	sq.userdata.frame_id = FRAME_ID

	plan = gensm_plan_vis_exec(Plan1ToXtionPoseState(), confirm=CONFIRM, visualize=VISUALIZE, execute=EXECUTE)
	with sq:
		Sequence.add("EXTA", plan)

	return sq
Esempio n. 35
0
def GoHome(robot, arms):

    sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
                  input_keys=['side', 'trayectory_name'],
                  connector_outcome='succeeded')

    with sq:
        Sequence.add(
            'PRE2_PRE1',
            basic.SetPositionNamed(robot,
                                   arms,
                                   blind=True,
                                   init='pre_2',
                                   goal='pre_1'))
        Sequence.add(
            'PRE1_HOME',
            basic.JO(robot, arms, blind=False, init='pre_1', goal='home'))

    return sq
Esempio n. 36
0
 def execute(self, ud):
     sq = Sequence(outcomes=['ok', 'err'], connector_outcome='ok')
     with sq:
         Sequence.add('TURN_L', RotateState(np.pi / 2))
         Sequence.add('LOCATION1',
                      Location1State(),
                      transitions={'err': 'TURN_R'})
         Sequence.add('TURN_R', RotateState(-np.pi / 2))
     return sq.execute(ud)
Esempio n. 37
0
    def test_action_preemption(self):
        """Test action preemption"""
        sq = Sequence(['succeeded', 'aborted', 'preempted'], 'succeeded')

        class SlowRunningState(State):
            def __init__(self):
                State.__init__(self,
                               outcomes=['succeeded', 'aborted', 'preempted'])

            def execute(self, ud):
                start_time = rospy.Time.now()
                while rospy.Time.now() - start_time < rospy.Duration(10):
                    rospy.sleep(0.05)
                    if self.preempt_requested():
                        self.service_preempt()
                        return 'preempted'
                return 'succeeded'

        with sq:
            Sequence.add('PREEMPT_ME', SlowRunningState())

        asw = ActionServerWrapper('preempt_action_sm',
                                  TestAction,
                                  sq,
                                  succeeded_outcomes=['succeeded'],
                                  aborted_outcomes=['aborted'],
                                  preempted_outcomes=['preempted'])
        asw.run_server()

        ac = SimpleActionClient('preempt_action_sm', TestAction)
        ac.wait_for_server(rospy.Duration(30))

        ac.send_goal(g1)
        rospy.sleep(5.0)
        ac.cancel_goal()

        start_time = rospy.Time.now()
        while ac.get_state() == GoalStatus.ACTIVE and rospy.Time.now(
        ) - start_time < rospy.Duration(30):
            rospy.sleep(0.5)
        assert ac.get_state() == GoalStatus.PREEMPTED
Esempio n. 38
0
def park_into_pose(goal, offset=0.5):  # type: (Callable[[], PoseStamped]) -> StateMachine
    v = 0.2
    end_offset = 0.09

    dt = offset / v

    sq = Sequence(outcomes=['ok', 'err'], connector_outcome='ok')
    with sq:
        sq.add('MOVE_TO_OFFSET', NavigateToGoalState(offset_goal(goal, offset)))
        # sq.add('MOVE_FORWARD', ForwardState(v, dt))
        sq.add('MOVE_FORWARD', MoveToState(offset_goal(goal, -end_offset), v))
        sq.add('STOP', StopState())
    return sq
Esempio n. 39
0
def create_fish_sequence():
    seq = Sequence(outcomes=[Transitions.SUCCESS, Transitions.FAILURE],
                   connector_outcome=Transitions.SUCCESS)

    approach = (
        ('approach', mirror_point(0.60, 0.3), -90),
        ('approach2', mirror_point(0.60, 0.15), -90),
    )

    drop = (
        ('get_out', mirror_point(0.85, 0.25), -180),
        ('drop', mirror_point(1.22, 0.12), -180),
    )

    with seq:
        add_waypoints(approach)
        Sequence.add('calage', FishApproachState())
        Sequence.add('grab_fish', FishAndHoldState())
        add_waypoints(drop)
        Sequence.add('drop_fish', FishDropState())
        Sequence.add('end_fishing', FishCloseState())

    return seq
Esempio n. 40
0
def get_per_goal_subsmach():
    sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
                  connector_outcome='succeeded',
                  input_keys=['x', 'y', 'yaw'])

    with sq:
        # check for permission
        Sequence.add('SLEEP_UNTIL_ENABLED', util.get_sleep_until_smach_enabled_smach())
        # nav to goal
        Sequence.add('MOVE_BASE_GOAL', move_base.MoveBaseState(),
                     remapping={'x':'x',
                                'y':'y',
                                'yaw':'yaw'}
                     )
        # wait
        Sequence.add('PAUSE_AT_GOAL', util.SleepState(5))
    return sq
def test_runner():
    rospy.init_node('runner_test')

    runner = Runner()

    sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
                  connector_outcome='succeeded')

    wfg = WaitForGoalState() # We don't want multiple subscribers so we need one WaitForGoal state

    with sq:

        Sequence.add('SLEEP', SleepState(5))

        Sequence.add('WAIT_FOR_GOAL', wfg,
                     transitions={'aborted':'SLEEP'})

        Sequence.add('MOVE_BASE_RGOAP', MoveBaseRGOAPState(runner),
                     transitions={'succeeded':'SLEEP'})

    execute_smach_container(sq, enable_introspection=True)
def main():
    rospy.init_node('tinker_mission_person_recognition')
    rospy.loginfo(colored('starting person recognition task ...', 'green'))

    # Main StateMachine
    state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
    with state:
        StateMachine.add('0_wait_for_start', MonitorStartButtonState(), transitions={'valid': '0_wait_for_start','invalid': '0_arm_mode'})
        StateMachine.add('0_arm_mode', ArmModeState(ArmModeState.Arm_Mode_Kinect), transitions={'succeeded':'0_wait_for_human'})
        StateMachine.add('0_wait_for_human', SpeakState('I am tinker , I am waiting for operator'), 
                transitions={'succeeded': '1_Start'})
        StateMachine.add('1_Start', MonitorKinectBodyState(), transitions={'valid':'1_Start', 'invalid':'Sequence'})
        sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
                            connector_outcome='succeeded')
        with sequence:
            Sequence.add('2_1_train_speak', SpeakState('please look at the camera'))
            Sequence.add('2_2_train', TrainPersonState())
            Sequence.add('2_3_train_finish', SpeakState('I have memorized you, thanks'))
            Sequence.add('3_wait', DelayState(10))
            Sequence.add('4_turn_back', ChassisSimpleMoveState(theta=3.23)) 
            Sequence.add('4_1_turn_back', ChassisSimpleMoveState(x=-1.0)) 
            Sequence.add('5_1_find_operator', FindPersonState()) 
            Sequence.add('5_2_point_operator', MoveArmState(offset=Point(0, 0, 0)), remapping={'target':'person_pose'}) 
            Sequence.add('5_3_say', SpeakState('I have found you'))
            Sequence.add('5_4_wait', DelayState(5))

        StateMachine.add('Sequence', sequence, {'succeeded': 'Sequence_reset', 'aborted': 'Sequence_reset'})

        sequence_reset = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
                            connector_outcome='succeeded')
        with sequence_reset:
            Sequence.add('6_1_arm_init', ArmModeState(ArmModeState.Arm_Mode_Init))
            Sequence.add('6_2_report', GenerateReportState(image='human_result.png', text='human_result.txt'))
            Sequence.add('6_3_finish', SpeakState('task finished'))

        StateMachine.add('Sequence_reset', sequence_reset, {'succeeded': 'succeeded', 'aborted': 'aborted'})

    # Run state machine introspection server for smach viewer
    intro_server = IntrospectionServer('tinker_mission_person_recognition', state, '/tinker_mission_person_recognition')
    intro_server.start()

    outcome = state.execute()
    rospy.spin()
    intro_server.stop()
Esempio n. 43
0
def get_patrol_smach():

    sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
                  connector_outcome='succeeded')

    sq.userdata.saved_position_x = 1
    sq.userdata.saved_position_y = 0
    sq.userdata.saved_position_yaw = 1

    sq.userdata.goal_position_1_x = 1
    sq.userdata.goal_position_1_y = 0
    sq.userdata.goal_position_1_yaw = 1

    sq.userdata.goal_position_2_x = 1
    sq.userdata.goal_position_2_y = 0
    sq.userdata.goal_position_2_yaw = 1


    wfg = move_base.WaitForGoalState() # We don't want multiple subscribers so we need one WaitForGoal state

    with sq:
        ## Add states to the container

        # save position
        Sequence.add('SAVE_ROBOT_POSITION', move_base.ReadRobotPositionState(),
                     remapping={'x':'saved_position_x',
                                'y':'saved_position_y',
                                'yaw':'saved_position_yaw'},
                     )

        ## command details

        # wait for goal 1
        Sequence.add('WAIT_FOR_GOAL_1', wfg,
                     remapping={'x':'goal_position_1_x',
                                'y':'goal_position_1_y',
                                'yaw':'goal_position_1_yaw'}
                         )

        # wait for goal 2
        Sequence.add('WAIT_FOR_GOAL_2', wfg,
                     remapping={'x':'goal_position_2_x',
                                'y':'goal_position_2_y',
                                'yaw':'goal_position_2_yaw'}
                         )

        ## action

        # goal 1
        Sequence.add('MOVE_GOAL_1', get_per_goal_subsmach(),
                     remapping={'x':'goal_position_1_x',
                                'y':'goal_position_1_y',
                                'yaw':'goal_position_1_yaw'
                                },
                     transitions={'aborted':'MOVE_GOAL_2'}
                     )

        # goal 2
        Sequence.add('MOVE_GOAL_2', get_per_goal_subsmach(),
                     remapping={'x':'goal_position_2_x',
                                'y':'goal_position_2_y',
                                'yaw':'goal_position_2_yaw'},
                     transitions={'aborted':'MOVE_GOAL_1',
                                  'succeeded':'MOVE_GOAL_1'}    # continue with goal 1
                     )

        ## ending

        # nav back
        Sequence.add('MOVE_BASE_RETURN', move_base.MoveBaseState(),
                     remapping={'x':'saved_position_x',
                                'y':'saved_position_y',
                                'yaw':'saved_position_yaw'}
                     )

    return sq
Esempio n. 44
0
def main():
    rospy.init_node('tinker_RIPS')
    rospy.loginfo(colored('starting RIPS task ...', 'green'))

    # load waypoints from xml
    pose_list = TKpos.PoseList.parse(open(rospy.get_param('~waypoint_xml'), 'r'))
    for pose in pose_list.pose:
        WayPointGoalState.waypoint_dict[pose.name] = TKpos.Pose.X(pose)

    # Main StateMachine
    state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
    with state:
        StateMachine.add('Start_Button', MonitorStartButtonState(), 
                transitions={'valid': 'Start_Button', 'invalid': 'Sequence'})

        sequence_1 = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
                connector_outcome='succeeded')
        with sequence_1:
            Sequence.add('GoToWaypoin1', WayPointGoalState('waypoint1'), transitions={'aborted': 'GoToWaypoin1'})
            Sequence.add('ArriveWaypoint1', SpeakState('Wait for continue'))

        StateMachine.add('Sequence', sequence_1, {'succeeded': 'Continue', 'aborted': 'aborted'})
        StateMachine.add('Continue', MonitorStartButtonState(), 
                transitions={'valid': 'Continue', 'invalid': 'Sequence_2'})
        sequence_2 = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
                connector_outcome='succeeded')
        with sequence_2:
            Sequence.add('GoToWaypoin2', WayPointGoalState('waypoint2'), transitions={'aborted': 'GoToWaypoin2'}) 
            Sequence.add('ArriveWaypoint2', SpeakState('I have arrived at way point two'))
            Sequence.add('GoToWaypoin3', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3'})
            Sequence.add('ArriveWaypoint3', SpeakState('I have arrived at way point three'))
        StateMachine.add('Sequence_2', sequence_2, {'succeeded': 'succeeded', 'aborted': 'aborted'})

    # Run state machine introspection server for smach viewer
    intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation')
    intro_server.start()

    outcome = state.execute()
    rospy.spin()
    intro_server.stop()
Esempio n. 45
0
def main():
    rospy.init_node('tinker_mission_navigation')
    rospy.loginfo(colored('starting navigation task ...', 'green'))

    # load waypoints from xml
    pose_list = TKpos.PoseList.parse(open(rospy.get_param('~waypoint_xml'), 'r'))
    for pose in pose_list.pose:
        WayPointGoalState.waypoint_dict[pose.name] = TKpos.Pose.X(pose)

    # Main StateMachine
    state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
    with state:
        StateMachine.add('Start_Button', MonitorStartButtonState(), 
                transitions={'valid': 'Start_Button', 'invalid': 'Sequence'})

        sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
                connector_outcome='succeeded')
        with sequence:
#            Sequence.add('GoToWaypoin1', WayPointGoalState('waypoint1'), transitions={'aborted': 'GoToWaypoin1'})
#            Sequence.add('ArriveWaypoint1', SpeakState('I have arrived at way point one'))
#            Sequence.add('GoToWaypoin2', WayPointGoalState('waypoint2'), 
#                    transitions={'succeeded': 'ArriveWaypoint2', 'aborted': 'Obstacle'}) 
#            Sequence.add('Obstacle', SpeakState('Obstacle in front of me'))
#            Sequence.add('ObstacleDelay', DelayState(10),
#                    transitions={'succeeded': 'GoToWaypoin2'}) 
#            Sequence.add('ArriveWaypoint2', SpeakState('I have arrived at way point two'))
            Sequence.add('GoToWaypoin3', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3'})
            Sequence.add('ArriveWaypoint3', SpeakState('I have arrived at way point three'))
            Sequence.add('StopCommandAndGo', SpeakState('Please GO. If you want to stop, say stop tinker'))
            Sequence.add('Train_human', FollowTrainState())

            follow_concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
                default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=[])
            with follow_concurrence:
                Concurrence.add('FollowMe', FollowMeState())
#                Concurrence.add('KeyWordsRecognition', KeywordsRecognizeState('stop'))

            Sequence.add('Follow_concurrence', follow_concurrence)
            Sequence.add('GoToWaypoin3Again', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3Again'})
            Sequence.add('ArriveWaypoint3Again', SpeakState('I am back at way point three'))
            Sequence.add('GoOut', WayPointGoalState('out'), transitions={'aborted': 'GoOut'})
	   
        StateMachine.add('Sequence', sequence, {'succeeded': 'succeeded', 'aborted': 'aborted'})

    # Run state machine introspection server for smach viewer
    intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation')
    intro_server.start()

    outcome = state.execute()
    rospy.spin()
    intro_server.stop()
Esempio n. 46
0
def tasker():

    rospy.init_node('tasker')

    wfg = WaitForGoalState() # We don't want multiple subscribers so we need one WaitForGoal state

    runner = SMACHRunner(rgoap_subclasses)


    ## sub machines
    sq_move_to_new_goal = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
                                   connector_outcome='succeeded')
    with sq_move_to_new_goal:
        Sequence.add('WAIT_FOR_GOAL', wfg)
        Sequence.add('MOVE_BASE_RGOAP', MoveBaseRGOAPState(runner))


    sq_autonomous_rgoap = Sequence(outcomes=['preempted'],
                                  connector_outcome='succeeded')
    with sq_autonomous_rgoap:
        Sequence.add('SLEEP_UNTIL_ENABLED', get_sleep_until_smach_enabled_smach())
        Sequence.add('AUTONOMOUS_RGOAP', AutonomousRGOAPState(runner),
                     transitions={'succeeded':'SLEEP_UNTIL_ENABLED',
                                  'aborted':'SLEEP'})
        Sequence.add('SLEEP', SleepState(5),
                     transitions={'succeeded':'SLEEP_UNTIL_ENABLED'})


    ## tasker machine
    sm_tasker = StateMachine(outcomes=['succeeded', 'aborted', 'preempted',
                                       'field_error', 'undefined_task'],
                             input_keys=['task_goal'])
    with sm_tasker:
        ## add all tasks to be available
        # states using rgoap
        StateMachine.add('MOVE_TO_NEW_GOAL_RGOAP', sq_move_to_new_goal)
        StateMachine.add('INCREASE_AWARENESS_RGOAP', IncreaseAwarenessRGOAPState(runner))
        StateMachine.add('AUTONOMOUS_RGOAP_CYCLE', sq_autonomous_rgoap)
        StateMachine.add('AUTONOMOUS_RGOAP_SINGLE_GOAL', AutonomousRGOAPState(runner))

        # states from uashh_smach
        StateMachine.add('LOOK_AROUND', get_lookaround_smach())
        StateMachine.add('GLIMPSE_AROUND', get_lookaround_smach(glimpse=True))
        StateMachine.add('MOVE_ARM_CRAZY', get_lookaround_smach(crazy=True))

        StateMachine.add('MOVE_TO_RANDOM_GOAL', get_random_goal_smach())
        StateMachine.add('MOVE_TO_NEW_GOAL_AND_RETURN', task_go_and_return.get_go_and_return_smach())
        StateMachine.add('PATROL_TO_NEW_GOAL', task_patrol.get_patrol_smach())
        StateMachine.add('MOVE_AROUND', task_move_around.get_move_around_smach())

        StateMachine.add('SLEEP_FIVE_SEC', SleepState(5))


        ## now the task receiver is created and automatically links to
        ##   all task states added above
        task_states_labels = sm_tasker.get_children().keys()
        task_states_labels.sort()  # sort alphabetically and then by _RGOAP
        task_states_labels.sort(key=lambda label: '_RGOAP' in label, reverse=True)

        task_receiver_transitions = {'undefined_outcome':'undefined_task'}
        task_receiver_transitions.update({l:l for l in task_states_labels})

        StateMachine.add('TASK_RECEIVER',
                         UserDataToOutcomeState(task_states_labels,
                                                'task_goal',
                                                lambda ud: ud.task_id),
                         task_receiver_transitions)

    sm_tasker.set_initial_state(['TASK_RECEIVER'])

    rospy.loginfo('tasker starting, available tasks: %s', ', '.join(task_states_labels))
    pub = rospy.Publisher('/task/available_tasks', String, latch=True)
    thread.start_new_thread(rostopic.publish_message, (pub, String, [', '.join(task_states_labels)], 1))

    asw = ActionServerWrapper('activate_task', TaskActivationAction,
                              wrapped_container=sm_tasker,
                              succeeded_outcomes=['succeeded'],
                              aborted_outcomes=['aborted', 'undefined_task'],
                              preempted_outcomes=['preempted'],
                              goal_key='task_goal'
                              )

    # Create and start the introspection server
    sis = IntrospectionServer('smach_tasker_action', sm_tasker, '/SM_ROOT')
    sis.start()

    asw.run_server()

    rospy.spin()
    sis.stop()
Esempio n. 47
0
def _get_vertical_sequence(task, x, y, z, phi, grab_width, parent_frame):
    sq = Sequence(
    outcomes = ['succeeded','aborted','preempted'],
    connector_outcome = 'succeeded')

    sq.userdata.x = x
    sq.userdata.y = y
    sq.userdata.z = z
    sq.userdata.z_pre = z + PRE_GRAB_OFFSET # cm pre grab position offset
    sq.userdata.phi = phi
    sq.userdata.parent_frame = parent_frame
    
    with sq:
        Sequence.add('MOVE_ARM_GRAB_PRE',
                     MoveArmVerticalGrabState(),
                     remapping={'x':'x',
                                'y':'y',
                                'z':'z_pre',
                                'phi':'phi',
                                'parent_frame':'parent_frame'}
                     )
        
        if(task == 'grab'):
            Sequence.add('MOVE_GRIPPER_OPEN', 
                         move_joints.get_move_gripper_state(GRIPPER_MAX_WIDTH))
            
        Sequence.add('MOVE_ARM_GRAB',
                     MoveArmVerticalGrabState(),
                     remapping={'x':'x',
                                'y':'y',
                                'z':'z',
                                'phi':'phi',
                                'parent_frame':'parent_frame'}
                     )
        
        if(task == 'grab'):
            Sequence.add('MOVE_GRIPPER_CLOSE',
                         move_joints.get_move_gripper_state(grab_width))
        else:
            Sequence.add('MOVE_GRIPPER_OPEN',
                         move_joints.get_move_gripper_state(GRIPPER_MAX_WIDTH))

        Sequence.add('MOVE_ARM_GRAB_POST',
                     MoveArmVerticalGrabState(),
                     remapping={'x':'x',
                                'y':'y',
                                'z':'z_pre',
                                'phi':'phi',
                                'parent_frame':'parent_frame'}
                     )

    return sq
def main():
    rospy.init_node('tinker_mission_manipulation')
    trans = tf.TransformListener()
    rospy.loginfo("Waiting for tf ...")
    rospy.sleep(3)
    assert (len(trans.getFrameStrings()) > 0)

    state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
    with state:
        def kinect_callback(userdata, result):
            userdata.objects = []
            objects = []
            sum_x = 0
            for obj in result.objects.objects:
                position = obj.pose.pose.pose.position
                if position.y > 0.5 or position.y < -0.5:
                    continue
                _levels = [0.10, 0.44, 0.78, 1.12, 1.46, 1.80] 
                for l in _levels:
                    if fabs(l - position.z) < 0.05:
                        position.z = l + 0.05
                if position.z > 1.1:
                    position.z += 0.05
                obj.header.stamp = rospy.Time(0)
                kinect_point = PointStamped(header=obj.header, point=position)
                odom_point = trans.transformPoint('odom', kinect_point)
                sum_x += odom_point.point.x
                rospy.loginfo(colored('[Kinect Object(odom)] from:(%f %f %f)', 'yellow'), odom_point.point.x,
                              odom_point.point.y, odom_point.point.z)
                objects.append(odom_point)

            avg_x = sum_x / len(objects)
            
            for from_point in objects:
                to_point = copy.deepcopy(from_point)
                to_point.point.x = avg_x 
                to_point.point.z = find_div(from_point.point.z)
                userdata.objects.append({'from': from_point, 'to': to_point})
                rospy.loginfo(colored('[Kinect Object(odom)] to:(%f %f %f)', 'yellow'), to_point.point.x,
                              to_point.point.y, to_point.point.z)
            rospy.loginfo(colored('Total Object: %d','green'), len(objects))
            return 'succeeded'

        StateMachine.add('Arm_Mode_Kinect', ArmModeState(ArmModeState.Arm_Mode_Kinect), 
                transitions={'succeeded': 'Start_Button'})
        StateMachine.add('Start_Button', MonitorStartButtonState(), 
                transitions={'valid': 'Start_Button', 'invalid': 'S_Kinect_Recognition'})
        StateMachine.add('S_Kinect_Recognition',
                ServiceState(service_name='/kinect_find_objects',
                    service_spec=FindObjects,
                    input_keys=['objects'],
                    output_keys=['objects'],
                    response_cb=kinect_callback),
                transitions={'succeeded': 'Generate_Report'})
        StateMachine.add('Generate_Report', GenerateReportState(image='result.png', text='object_names.txt'), 
                transitions={'succeeded': 'IT_Objects_Iterator'} )

        objects_iterator = Iterator(outcomes=['succeeded', 'preempted', 'aborted'],
                input_keys=['objects'], output_keys=[],
                it=lambda: state.userdata.objects, it_label='target',
                exhausted_outcome='succeeded')

        with objects_iterator:
            fetch_object_sequence = Sequence(outcomes=['succeeded', 'aborted', 'continue', 'preempted'],
                                             input_keys=['target'],
                                             connector_outcome='succeeded')
            with fetch_object_sequence:
                Sequence.add('Speak', SpeakState('New Object recognized'))
                Sequence.add('Gripper_Photo', GripperState(GripperState.GRIPPER_OPEN))
                Sequence.add('Move_For_Photo', MoveArmState(Point(-0.7, 0, 0), target_key='from'), 
                        transitions={'aborted':'continue'})
                concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
                                          default_outcome='succeeded',
                                          child_termination_cb=lambda x: True,
                                          input_keys=['target'])
                with concurrence:
                    Concurrence.add('Move_Fetch', MoveArmState(Point(0.06, 0, 0), target_key='from'))
                    Concurrence.add('Gripper_Laser_sensor', MonitorState('/gripper_laser_sensor', Bool, cond_cb=lambda x,y: False))

                Sequence.add('Move_Fetch_Concurrence', concurrence)
                Sequence.add('Gripper_Fetch', GripperState(GripperState.GRIPPER_CLOSE))
                Sequence.add('Move_Fetch_Back', MoveArmState(Point(-0.7, 0, 0), target_key='from'))
                Sequence.add('Move_Down', MoveArmState(Point(-0.6, 0, 0), target_key='to'))
                Sequence.add('Move_Put', MoveArmState(Point(0, 0, 0), target_key='to'))
                Sequence.add('Gripper_Put', GripperState(GripperState.GRIPPER_OPEN))
                Sequence.add('Move_Put_Back', MoveArmState(Point(-0.6, 0, 0), target_key='to'), transitions={'succeeded': 'continue'})

            Iterator.set_contained_state('Seq_Fetch_Object', fetch_object_sequence, loop_outcomes=['continue'])

        # end of objects_iterator
        StateMachine.add('IT_Objects_Iterator', objects_iterator,
                transitions= {'succeeded': 'A_Move_Reset', 'aborted': 'A_Move_Reset'})

        StateMachine.add('A_Move_Reset', ArmModeState(ArmModeState.Arm_Mode_Init),
                         transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})

    # Run state machine introspection server for smach viewer
    intro_server = IntrospectionServer('tinker_mission_manipulation', state, '/tinker_mission_manipulation')
    intro_server.start()

    outcome = state.execute()
    rospy.spin()
    intro_server.stop()
def _get_transport_box_smach():
    sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
                  connector_outcome='succeeded')

    sq.userdata.gripper_x = 0.33
    sq.userdata.gripper_y = 0
    sq.userdata.gripper_z = 0.42
    sq.userdata.gripper_phi = math.radians(10)
#    sq.userdata.x = 0.33
#    sq.userdata.y = 0
#    sq.userdata.z = 0.42
#    sq.userdata.phi = 0

    with sq:
        ## Add states to the container

        Sequence.add('MOVE_BASE_Forward', move_base.get_move_base_in_odom_state(1, 0))

        Sequence.add('MOVE_ARM_GRAB_0',
                     grab_vertical.get_vertical_grab_sequence(-0.23, -0.5, 0.85 + 0.1,
                                                              math.radians(90),
                                                              BOX_THICKNESS,
                                                              "/base_link")
                     )


        Sequence.add('MOVE_ARM_ZERO', move_arm.get_move_arm_to_zero_state())


        Sequence.add('MOVE_BASE_Backward', move_base.get_move_base_in_odom_state(0, 0))

        Sequence.add('MOVE_ARM_DROP_0',
                     grab_vertical.get_vertical_drop_sequence(-0.23, -0.5, 0.85 + 0.1,
                                                              math.radians(90),
                                                              BOX_THICKNESS,
                                                              "/base_link")
                     )

        Sequence.add('MOVE_ARM_ZERO_2', move_arm.get_move_arm_to_zero_state())




#        Sequence.add('MOVE_ARM_GRAB_0',
#                     grab_vertical.get_vertical_grab_sequence(-0.23, -0.5, 0.85+0.1,
#                                                              math.radians(90),
#                                                              BOX_THICKNESS,
#                                                              "/base_link")
#                     )
#
#
#        Sequence.add('MOVE_ARM_ZERO', move_arm.get_move_arm_to_zero_state())
#
#        Sequence.add('MOVE_BASE_Forward', move_base.get_move_base_in_odom_state(1, 0));
#
#        Sequence.add('MOVE_ARM_DROP_0',
#                     grab_vertical.get_vertical_drop_sequence(-0.23, -0.5, 0.85+0.1,
#                                                              math.radians(90),
#                                                              BOX_THICKNESS,
#                                                              "/base_link")
#                     )
#
#        Sequence.add('MOVE_ARM_ZERO_2', move_arm.get_move_arm_to_zero_state())
#
#
#        Sequence.add('MOVE_BASE_Backward', move_base.get_move_base_in_odom_state(0, 0));

    return sq
Esempio n. 50
0
File: paint.py Progetto: GUTeamU/TP3
def main():
	
	rospy.init_node('paint')

	sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded')

	with sq:
		Sequence.add('OPEN_GRIPPER', GripperState(2, True), {'succeeded':'TURN', 'aborted':'HOME'})

		Sequence.add("TURN", ext_plan(), transitions={'aborted':'HOME', 'succeeded':'AWAY'})

		Sequence.add("AWAY", away_plan(), transitions={'aborted':'HOME', 'succeeded':'GRAB'})

		Sequence.add("GRAB", grab_plan(sq), transitions={'aborted':'HOME', 'succeeded':'DRAW'})

		Sequence.add("DRAW", draw_plan(), transitions={'aborted':'HOME', 'succeeded':'RELEASE'})

		Sequence.add("RELEASE", GripperState(2, True), transitions={'aborted':'HOME', 'succeeded':'HOME'})

		# TODO
		# Open hand to release -> HOME
		Sequence.add("HOME", home_plan(), transitions={'aborted':'POWER_OFF'})

		Sequence.add("POWER_OFF", SetServoPowerOffState())



	sis = smach_ros.IntrospectionServer('paint', sq, '/SM_ROOT')
	sis.start()
	os.system('clear')
	outcome = sq.execute()
	rospy.loginfo("State machine exited with outcome: " + outcome)
	sis.stop()