def main():
    rospy.init_node('smach_usecase_step_02')

    # Create a SMACH state machine
    sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])

    # Open the container
    with sm0:
        # Reset turtlesim
        StateMachine.add('RESET',
                ServiceState('reset', std_srvs.srv.Empty),
                {'succeeded':'SPAWN'})

        # Create a second turtle
        StateMachine.add('SPAWN',
                ServiceState('spawn', turtlesim.srv.Spawn,
                    request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')))

    # Attach a SMACH introspection server
    sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
    sis.start()

    # Execute SMACH tree
    outcome = sm0.execute()

    # Signal ROS shutdown (kill threads in background)
    rospy.spin()

    sis.stop()
def main():
    rospy.init_node('smach_usecase_step_04')

    # Construct static goals
    polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0)
    polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5) 

    # Create a SMACH state machine
    sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])

    # Open the container
    with sm0:
        # Reset turtlesim
        StateMachine.add('RESET',
                ServiceState('reset', std_srvs.srv.Empty),
                {'succeeded':'SPAWN'})

        # Create a second turtle
        StateMachine.add('SPAWN',
                ServiceState('spawn', turtlesim.srv.Spawn,
                    request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')),
                {'succeeded':'TELEPORT1'})

        # Teleport turtle 1
        StateMachine.add('TELEPORT1',
                ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute,
                    request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)),
                {'succeeded':'TELEPORT2'})

        # Teleport turtle 2
        StateMachine.add('TELEPORT2',
                ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
                    request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
                {'succeeded':'BIG'})

        # Draw a large polygon with the first turtle
        StateMachine.add('BIG',
                SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
                    goal = polygon_big),
                {'succeeded':'SMALL'})

        # Draw a small polygon with the second turtle
        StateMachine.add('SMALL',
                SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
                    goal = polygon_small))

    # Attach a SMACH introspection server
    sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
    sis.start()

    # Set preempt handler
    smach.set_preempt_handler(sm0)

    # Execute SMACH tree in a separate thread so that we can ctrl-c the script
    smach_thread = threading.Thread(target = sm0.execute)
    smach_thread.start()

    # Signal handler
    rospy.spin()
Ejemplo n.º 3
0
def main():
    rospy.init_node('smach_usecase_step_06')

    # Construct static goals
    polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0)
    polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5) 

    # Create a SMACH state machine
    sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])

    # Open the container
    with sm0:
        # Reset turtlesim
        StateMachine.add('RESET',
                ServiceState('reset', std_srvs.srv.Empty),
                {'succeeded':'SPAWN'})

        # Create a second turtle
        StateMachine.add('SPAWN',
                ServiceState('spawn', turtlesim.srv.Spawn,
                    request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')),
                {'succeeded':'TELEPORT1'})

        # Teleport turtle 1
        StateMachine.add('TELEPORT1',
                ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute,
                    request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)),
                {'succeeded':'TELEPORT2'})

        # Teleport turtle 2
        StateMachine.add('TELEPORT2',
                ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
                    request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
                {'succeeded':'DRAW_SHAPES'})

        # Draw some polygons
        shapes_cc = Concurrence(
                outcomes=['succeeded','aborted','preempted'],
                default_outcome='aborted',
                outcome_map = {'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}})
        StateMachine.add('DRAW_SHAPES',shapes_cc)
        with shapes_cc:
            # Draw a large polygon with the first turtle
            Concurrence.add('BIG',
                    SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
                        goal = polygon_big))
            # Draw a small polygon with the second turtle
            draw_monitor_cc = Concurrence(
                    ['succeeded','aborted','preempted'],
                    'aborted',
                    child_termination_cb = lambda so: True,
                    outcome_map = {
                        'succeeded':{'DRAW':'succeeded'},
                        'preempted':{'DRAW':'preempted','MONITOR':'preempted'},
                        'aborted':{'MONITOR':'invalid'}})
            Concurrence.add('SMALL',draw_monitor_cc)
            with draw_monitor_cc:
                Concurrence.add('DRAW',
                        SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
                            goal = polygon_small))

                def turtle_far_away(ud, msg):
                    """Returns True while turtle pose in msg is at least 1 unit away from (9,5)"""
                    if sqrt(pow(msg.x-9.0,2) + pow(msg.y-5.0,2)) > 2.0:
                        return True
                    return False
                Concurrence.add('MONITOR',
                        MonitorState('/turtle1/pose',turtlesim.msg.Pose,
                            cond_cb = turtle_far_away))

    # Attach a SMACH introspection server
    sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
    sis.start()

    # Set preempt handler
    smach.set_preempt_handler(sm0)

    # Execute SMACH tree in a separate thread so that we can ctrl-c the script
    smach_thread = threading.Thread(target = sm0.execute)
    smach_thread.start()

    # Signal handler
    rospy.spin()
Ejemplo n.º 4
0
def main():
    rospy.init_node('smach_usecase_step_06')

    # Construct static goals
    polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0)
    polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5) 

    # Create a SMACH state machine
    sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])

    # Open the container
    with sm0:
        # Reset turtlesim
        StateMachine.add('RESET',
                ServiceState('reset', std_srvs.srv.Empty),
                {'succeeded':'SPAWN'})

        # Create a second turtle
        StateMachine.add('SPAWN',
                ServiceState('spawn', turtlesim.srv.Spawn,
                    request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')),
                {'succeeded':'TELEPORT1'})

        # Teleport turtle 1
        StateMachine.add('TELEPORT1',
                ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute,
                    request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)),
                {'succeeded':'DRAW_SHAPES'})

        # Draw some polygons
        shapes_cc = Concurrence(
                outcomes=['succeeded','aborted','preempted'],
                default_outcome='aborted',
                outcome_map = {'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}})
        StateMachine.add('DRAW_SHAPES',shapes_cc)
        with shapes_cc:
            # Draw a large polygon with the first turtle
            Concurrence.add('BIG',
                    SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
                        goal = polygon_big))

            # Draw a small polygon with the second turtle
            small_shape_sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
            Concurrence.add('SMALL',small_shape_sm)
            with small_shape_sm:
                # Teleport turtle 2
                StateMachine.add('TELEPORT2',
                        ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
                            request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
                        {'succeeded':'DRAW_WITH_MONITOR'})

                # Construct a concurrence for the shape action and the monitor
                draw_monitor_cc = Concurrence(
                        ['succeeded','aborted','preempted','interrupted'],
                        'aborted',
                        child_termination_cb = lambda so: True,
                        outcome_map = {
                            'succeeded':{'DRAW':'succeeded'},
                            'preempted':{'DRAW':'preempted','MONITOR':'preempted'},
                            'interrupted':{'MONITOR':'invalid'}})

                StateMachine.add('DRAW_WITH_MONITOR',
                        draw_monitor_cc,
                        {'interrupted':'WAIT_FOR_CLEAR'})

                with draw_monitor_cc:
                    Concurrence.add('DRAW',
                            SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
                                goal = polygon_small))
                    def turtle_far_away(ud, msg):
                        """Returns True while turtle pose in msg is at least 1 unit away from (9,5)"""
                        if sqrt(pow(msg.x-9.0,2) + pow(msg.y-5.0,2)) > 2.0:
                            return True
                        return False
                    Concurrence.add('MONITOR',
                            MonitorState('/turtle1/pose',turtlesim.msg.Pose,
                                cond_cb = turtle_far_away))

                StateMachine.add('WAIT_FOR_CLEAR',
                        MonitorState('/turtle1/pose',turtlesim.msg.Pose,
                            cond_cb = lambda ud,msg: not turtle_far_away(ud,msg)),
                        {'valid':'WAIT_FOR_CLEAR','invalid':'TELEPORT2'})


    # Attach a SMACH introspection server
    sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
    sis.start()

    # Set preempt handler
    smach.set_preempt_handler(sm0)

    # Execute SMACH tree in a separate thread so that we can ctrl-c the script
    smach_thread = threading.Thread(target = sm0.execute)
    smach_thread.start()

    # Signal handler
    rospy.spin()