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()
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()
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()
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()