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