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