Beispiel #1
0
def main():
    rospy.init_node('torpedo_task_state_machine')
    sm = smach.StateMachine(outcomes=['torpedo_task_complete'])
    sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_TORPEDO_TASK')
    sis.start()

    board_topic = {
        'x': '/torpedo_board_x',
        'y': '/torpedo_board_y',
        'area': '/torpedo_board_area'
    }

    heart_topic = {
        'x': '/torpedo_heart_x',
        'y': '/torpedo_heart_y',
        'area': '/torpedo_heart_area'
    }

    with sm:
        smach.StateMachine.add('IDLE',
                               state.WaitForTopic('/torpedo_enable'),
                               transitions={
                                   'done': 'YAW_PID_ENABLE',
                                   'notdone': 'IDLE'
                               })
        smach.StateMachine.add('YAW_PID_ENABLE',
                               state.PublishTopic('/yaw_control/pid_enable',
                                                  True),
                               transitions={'done': 'TRACK_BOARD'})
        smach.StateMachine.add('TRACK_BOARD',
                               state.TrackObject(board_topic, 0, 0),
                               transitions={
                                   'done': 'TRACK_HEART',
                                   'notdone': 'TRACK_BOARD',
                                   'reset': 'RESET'
                               })
        smach.StateMachine.add('TRACK_HEART',
                               state.TrackObject(heart_topic, 0,
                                                 TORPEDO_Y_OFFSET),
                               transitions={
                                   'done': 'SHOOT',
                                   'notdone': 'TRACK_HEART',
                                   'reset': 'RESET'
                               })
        smach.StateMachine.add('SHOOT',
                               state.PublishTopic('/torpedo_shoot', True),
                               transitions={'done': 'SHOOT2'})
        smach.StateMachine.add('SHOOT2',
                               state.PublishTopic('/torpedo_shoot', False),
                               transitions={'done': 'COMPLETE'})
        smach.StateMachine.add('COMPLETE',
                               state.PublishTopic('/task_complete', True),
                               transitions={'done': 'IDLE'})
        smach.StateMachine.add('RESET',
                               state.Reset(),
                               transitions={'done': 'IDLE'})

    outcome = sm.execute()
    rospy.spin()
    sis.stop()
Beispiel #2
0
def main():
    # Initialize node with desired node name - ideally task name
    rospy.init_node('gateTask')

    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['task_complete'])

    # Create and start introspection server - fancy way of saying view gui feature
    sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_GATE_TASK')
    sis.start()

    gate_topic = {'x': '/gate_x', 'y': '/gate_y', 'area': '/gate_area'}

    GATE_DEPTH = 18
    GATE_YAW_1 = 1.57
    GATE_YAW_OFFSET = -0.017 * 5
    GATE_YAW_2 = GATE_YAW_1 + GATE_YAW_OFFSET
    GATE_CONVERGE_TICKS = 5000
    GATE_FORWARD_TICKS = 10000
    BUOY_DEPTH = 5 * 12
    BUOY_YAW = 0.017 * 45

    # Open SMACH container
    with sm:
        smach.StateMachine.add('IDLE',
                               state.WaitForTopic('/gate_enable'),
                               transitions={
                                   'done': 'DEPTH_PID_ENABLE',
                                   'notdone': 'IDLE'
                               })
        smach.StateMachine.add('DEPTH_PID_ENABLE',
                               state.PublishTopic('/depth_control/pid_enable',
                                                  True),
                               transitions={'done': 'DIVE_GATE_DEPTH'})
        smach.StateMachine.add('DIVE_GATE_DEPTH',
                               state.ChangeDepthToTarget(GATE_DEPTH),
                               transitions={
                                   'done': 'YAW_PID_ENABLE',
                                   'notdone': 'DIVE_GATE_DEPTH',
                                   'reset': 'RESET'
                               })
        smach.StateMachine.add('YAW_PID_ENABLE',
                               state.PublishTopic('/yaw_control/pid_enable',
                                                  True),
                               transitions={'done': 'ROTATE_TO_GATE'})
        smach.StateMachine.add('ROTATE_TO_GATE',
                               state.RotateYawToAbsoluteTarget(GATE_YAW_1),
                               transitions={
                                   'done': 'TRACK_GATE',
                                   'notdone': 'ROTATE_TO_GATE',
                                   'reset': 'RESET'
                               })
        smach.StateMachine.add('TRACK_GATE',
                               state.TrackObject(gate_topic),
                               transitions={
                                   'done': 'SET_YAW_GATE_OFFSET',
                                   'notdone': 'TRACK_GATE',
                                   'reset': 'RESET'
                               })

        smach.StateMachine.add('SET_YAW_GATE_OFFSET',
                               state.PublishTopicRelative(
                                   '/yaw_control/state',
                                   '/yaw_control/setpoint', Float64,
                                   GATE_YAW_OFFSET),
                               transitions={
                                   'done': 'ROTATE_GATE_LEFT',
                                   'notdone': 'SET_YAW_GATE_OFFSET',
                                   'reset': 'RESET'
                               })

        smach.StateMachine.add('ROTATE_GATE_LEFT',
                               state.WaitForConvergence(
                                   '/yaw_control/state', Float64, GATE_YAW_2,
                                   0.017, GATE_CONVERGE_TICKS),
                               transitions={
                                   'done': 'MOVE_FORWARD_GATE',
                                   'notdone': 'ROTATE_GATE_LEFT',
                                   'reset': 'RESET'
                               })

        smach.StateMachine.add('MOVE_FORWARD_GATE',
                               state.MoveForwardTimed(GATE_FORWARD_TICKS,
                                                      True),
                               transitions={
                                   'done': 'BUOY_DEPTH',
                                   'notdone': 'MOVE_FORWARD_GATE',
                                   'reset': 'RESET'
                               })
        smach.StateMachine.add('BUOY_DEPTH',
                               state.ChangeDepthToTarget(BUOY_DEPTH),
                               transitions={
                                   'done': 'ROTATE_TO_BUOY',
                                   'notdone': 'BUOY_DEPTH',
                                   'reset': 'RESET'
                               })
        smach.StateMachine.add('ROTATE_TO_BUOY',
                               state.RotateYawToAbsoluteTarget(BUOY_YAW),
                               transitions={
                                   'done': 'COMPLETED',
                                   'notdone': 'ROTATE_TO_BUOY',
                                   'reset': 'RESET'
                               })
        smach.StateMachine.add('COMPLETED',
                               state.PublishTopic('/task_complete', True),
                               transitions={'done': 'IDLE'})
        smach.StateMachine.add('RESET',
                               state.Reset(),
                               transitions={'done': 'IDLE'})

    # Execute State Machine
    outcome = sm.execute()

    # Spin node - fancy way of saying run code in a loop
    rospy.spin()
    sis.stop()
Beispiel #3
0
def main():
    rospy.init_node('buoy_task_state_machine')
    sm = smach.StateMachine(outcomes=['buoy_task_complete'])
    sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
    sis.start()

    buoy_flat_topic = {
        'x': '/buoy_flat_x',
        'y': '/buoy_flat_y',
        'area': '/buoy_flat_area'
    }

    buoy_triangle_topic = {
        'x': '/buoy_triangle_x',
        'y': '/buoy_triangle_y',
        'area': '/buoy_triangle_area'
    }

    TOUCH_FLAT_TIMER = 10000  # time required (in ticks) to touch the flat buoy
    MOVE_BACK_1_TIMER = 6000  # time required (in ticks) to move back, away from flat buoy
    MOVE_FORWARD_TIMER = 10000  # time required (in ticks) to move past the flat buoy
    TOUCH_TRIANGLE_TIMER = 10000  # time required (in ticks) to touch the triangle buoy
    MOVE_BACK_2_TIMER = 6000  # time required (in ticks) to move back, away from triangle buoy

    BUOY_ABOVE_DEPTH = 2 * 12  # Adjusts sub to move 2 ft up
    BUOY_BELOW_DEPTH = 3.5 * 12  # Adjusts sub to move 3.5 ft down
    TORPEDO_BOARD_DEPTH = 3 * 12  # Adjusts sub to move 3 ft down
    DEPTH_VARIANCE = 2  # 2 inch

    YAW_BUOY_BACK = 3.14  # the yaw (in radians) to make sub turn 180 degrees to face back buoys
    YAW_TORPEDO_TASK = 0.5  # the yaw (in radians) to face the torpedo task
    YAW_VARIANCE = 0.1  # in radians

    with sm:
        smach.StateMachine.add('IDLE',
                               StartState(),
                               transitions={
                                   'ready': 'TOUCH_FLAT',
                                   'notready': 'IDLE'
                               })
        #transitions={'ready':'TRACK_FLAT', 'notready':'IDLE'})

        #smach.StateMachine.add('TRACK_FLAT', sd.TrackObject(buoy_flat_topic),
        #	transitions={'done':'TOUCH_FLAT', 'notdone':'TRACK_FLAT', 'reset':'RESET'})

        smach.StateMachine.add('TOUCH_FLAT',
                               sd.MoveForwardTimed(TOUCH_FLAT_TIMER, True),
                               transitions={
                                   'done': 'MOVE_BACK_1',
                                   'notdone': 'TOUCH_FLAT',
                                   'reset': 'RESET'
                               })

        smach.StateMachine.add('MOVE_BACK_1',
                               sd.MoveForwardTimed(MOVE_BACK_1_TIMER, False),
                               transitions={
                                   'done': 'MOVE_UP',
                                   'notdone': 'MOVE_BACK_1',
                                   'reset': 'RESET'
                               })

        smach.StateMachine.add('MOVE_UP',
                               sd.ChangeDepthToTarget(BUOY_ABOVE_DEPTH),
                               transitions={
                                   'done': 'MOVE_FORWARD',
                                   'notdone': 'MOVE_UP',
                                   'reset': 'RESET'
                               })

        smach.StateMachine.add('MOVE_FORWARD',
                               sd.MoveForwardTimed(MOVE_FORWARD_TIMER, True),
                               transitions={
                                   'done': 'MOVE_DOWN',
                                   'notdone': 'MOVE_FORWARD',
                                   'reset': 'RESET'
                               })

        smach.StateMachine.add('MOVE_DOWN',
                               sd.ChangeDepthToTarget(BUOY_BELOW_DEPTH),
                               transitions={
                                   'done': 'TURN_AROUND',
                                   'notdone': 'MOVE_DOWN',
                                   'reset': 'RESET'
                               })

        smach.StateMachine.add('TURN_AROUND',
                               sd.Rotate180(YAW_BUOY_BACK),
                               transitions={
                                   'done': 'TOUCH_TRIANGLE',
                                   'notdone': 'TURN_AROUND',
                                   'reset': 'RESET'
                               })
        #	transitions={'done':'TRACK_TRIANGLE', 'notdone':'TURN_AROUND', 'reset':'RESET'})

        #smach.StateMachine.add('TRACK_TRIANGLE', TrackObjectState(buoy_triangle_topic, 0),
        #	transitions={'done':'TOUCH_TRIANGLE', 'notdone':'TRACK_TRIANGLE', 'reset':'RESET'})

        smach.StateMachine.add('TOUCH_TRIANGLE',
                               sd.MoveForwardTimed(TOUCH_TRIANGLE_TIMER, True),
                               transitions={
                                   'done': 'MOVE_BACK_2',
                                   'notdone': 'TOUCH_TRIANGLE',
                                   'reset': 'RESET'
                               })

        smach.StateMachine.add('MOVE_BACK_2',
                               sd.MoveForwardTimed(MOVE_BACK_2_TIMER, False),
                               transitions={
                                   'done': 'FACE_TORPEDO_TASK',
                                   'notdone': 'MOVE_BACK_2',
                                   'reset': 'RESET'
                               })

        smach.StateMachine.add('FACE_TORPEDO_TASK',
                               RotateYawState(YAW_TORPEDO_TASK, YAW_VARIANCE),
                               transitions={
                                   'done': 'MOVE_TORPEDO_DEPTH',
                                   'notdone': 'FACE_TORPEDO_TASK',
                                   'reset': 'RESET'
                               })

        smach.StateMachine.add('MOVE_TORPEDO_DEPTH',
                               sd.ChangeDepthToTarget(TORPEDO_BOARD_DEPTH),
                               transitions={
                                   'done': 'COMPLETED',
                                   'notdone': 'MOVE_TORPEDO_DEPTH',
                                   'reset': 'RESET'
                               })

        smach.StateMachine.add('COMPLETED',
                               sd.PublishTopic('/buoy_task_complete', True),
                               transitions={'done': 'IDLE'})

        smach.StateMachine.add('RESET',
                               sd.Reset(),
                               transitions={'done': 'IDLE'})

    outcome = sm.execute()
    rospy.spin()
    sis.stop()
Beispiel #4
0
def main():
    # Initialize node with desired node name - ideally task name
    rospy.init_node('gateTask')

    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['task_complete'])

    # Create and start introspection server - fancy way of saying view gui feature
    sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_GATE_TASK')
    sis.start()

    gate_topic = {'x': '/gate_x', 'y': '/gate_y', 'area': '/gate_area'}

    GATE_DEPTH = 18  #The depth the sub will be when going through the gate (in inches)
    GATE_YAW_1 = 1.57  #Angle from the Start Positiion in pool to gate (in radians)
    GATE_YAW_OFFSET = -0.017 * 5  #The angle offset from the center of the gate (Not correct, but used for testing)
    GATE_YAW_2 = GATE_YAW_1 + GATE_YAW_OFFSET  #Sum of GATE_YAW_1 and GATE_YAW_OFFSET
    GATE_CONVERGE_TICKS = 5000  #Amount of time the sub will go from PENDING (in secs/ this is an estimate)
    GATE_FORWARD_TICKS = 10000  #Amount of time the sub will go from the dock to gate(in secs/ this is an estimate)
    BUOY_DEPTH = 5 * 12  # (in inches)
    BUOY_YAW = 0.017 * 45  # (in radians)

    # Open SMACH container
    with sm:
        smach.StateMachine.add('IDLE',
                               state.WaitForTopic('/gate_enable'),
                               transitions={
                                   'done': 'DEPTH_PID_ENABLE',
                                   'notdone': 'IDLE'
                               })
        smach.StateMachine.add('DEPTH_PID_ENABLE',
                               state.PublishTopic('/depth_control/pid_enable',
                                                  True),
                               transitions={'done': 'DIVE_GATE_DEPTH'})
        smach.StateMachine.add('DIVE_GATE_DEPTH',
                               state.ChangeDepthToTarget(GATE_DEPTH),
                               transitions={
                                   'done': 'YAW_PID_ENABLE',
                                   'notdone': 'DIVE_GATE_DEPTH',
                                   'reset': 'RESET'
                               })
        smach.StateMachine.add('YAW_PID_ENABLE',
                               state.PublishTopic('/yaw_control/pid_enable',
                                                  True),
                               transitions={'done': 'ROTATE_TO_GATE'})
        smach.StateMachine.add('ROTATE_TO_GATE',
                               state.RotateYawToAbsoluteTarget(GATE_YAW_1),
                               transitions={
                                   'done': 'TRACK_GATE',
                                   'notdone': 'ROTATE_TO_GATE',
                                   'reset': 'RESET'
                               })
        smach.StateMachine.add('TRACK_GATE',
                               state.TrackObject(gate_topic),
                               transitions={
                                   'done': 'SET_YAW_GATE_OFFSET',
                                   'notdone': 'TRACK_GATE',
                                   'reset': 'RESET'
                               })

        smach.StateMachine.add('SET_YAW_GATE_OFFSET',
                               state.PublishTopicRelative(
                                   '/yaw_control/state',
                                   '/yaw_control/setpoint', Float64,
                                   GATE_YAW_OFFSET),
                               transitions={
                                   'done': 'ROTATE_GATE_LEFT',
                                   'notdone': 'SET_YAW_GATE_OFFSET',
                                   'reset': 'RESET'
                               })

        smach.StateMachine.add('ROTATE_GATE_LEFT',
                               state.WaitForConvergence(
                                   '/yaw_control/state', Float64, GATE_YAW_2,
                                   0.017, GATE_CONVERGE_TICKS),
                               transitions={
                                   'done': 'MOVE_FORWARD_GATE',
                                   'notdone': 'ROTATE_GATE_LEFT',
                                   'reset': 'RESET'
                               })

        smach.StateMachine.add('MOVE_FORWARD_GATE',
                               state.MoveForwardTimed(GATE_FORWARD_TICKS,
                                                      True),
                               transitions={
                                   'done': 'BUOY_DEPTH',
                                   'notdone': 'MOVE_FORWARD_GATE',
                                   'reset': 'RESET'
                               })
        smach.StateMachine.add('BUOY_DEPTH',
                               state.ChangeDepthToTarget(BUOY_DEPTH),
                               transitions={
                                   'done': 'ROTATE_TO_BUOY',
                                   'notdone': 'BUOY_DEPTH',
                                   'reset': 'RESET'
                               })
        smach.StateMachine.add('ROTATE_TO_BUOY',
                               state.RotateYawToAbsoluteTarget(BUOY_YAW),
                               transitions={
                                   'done': 'COMPLETED',
                                   'notdone': 'ROTATE_TO_BUOY',
                                   'reset': 'RESET'
                               })
        smach.StateMachine.add('COMPLETED',
                               state.PublishTopic('/task_complete', True),
                               transitions={'done': 'IDLE'})
        smach.StateMachine.add('RESET',
                               state.Reset(),
                               transitions={'done': 'IDLE'})

    # Execute State Machine
    outcome = sm.execute()

    # Spin node - fancy way of saying run code in a loop
    rospy.spin()
    sis.stop()