def __init__(self):
# init node
rospy.init_node('pool_patrol', anonymous=False)
# Set the shutdown fuction (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initilalize the mission parameters and variables
setup_task_environment(self)
# get vehicle pose
navigation = Navigation()
# Turn the target locations into SMACH MoveBase and LosPathFollowing action states
nav_terminal_states = {}
nav_transit_states = {}
# DP controller
for target in self.pool_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'odom'
nav_goal.target_pose.pose = self.pool_locations[target]
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.nav_result_cb,
exec_timeout=self.nav_timeout,
server_wait_timeout=rospy.Duration(10.0))
nav_terminal_states[target] = move_base_state
# Path following
for target in self.pool_locations.iterkeys():
nav_goal = LosPathFollowingGoal()
#nav_goal.prev_waypoint = navigation.vehicle_pose.position
nav_goal.next_waypoint = self.pool_locations[target].position
nav_goal.forward_speed.linear.x = 0.2
nav_goal.desired_depth.z = -0.5
nav_goal.sphereOfAcceptance = 0.2
los_path_state = SimpleActionState(
'los_path',
LosPathFollowingAction,
goal=nav_goal,
result_cb=self.nav_result_cb,
exec_timeout=self.nav_timeout,
server_wait_timeout=rospy.Duration(10.0))
nav_transit_states[target] = los_path_state
""" Create individual state machines for assigning tasks to each target zone """
# Create a state machine for the orienting towards the gate subtask(s)
sm_gate_task = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Then add the subtask(s)
with sm_gate_task:
StateMachine.add('ORIENT_TO_GATE',
OrientToTarget('gate', 5),
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
""" Assemble a Hierarchical State Machine """
# Initialize the HSM
hsm_pool_patrol = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Build the HSM from nav states and target states
with hsm_pool_patrol:
""" Navigate to GATE in TERMINAL mode """
StateMachine.add('DOCKING',
ControlMode(OPEN_LOOP),
transitions={
'succeeded': 'p0',
'aborted': '',
'preempted': ''
})
StateMachine.add('p0',
nav_transit_states['p0'],
transitions={
'succeeded': 'p1',
'aborted': '',
'preempted': ''
})
StateMachine.add('p1',
nav_transit_states['p1'],
transitions={
'succeeded': 'p2',
'aborted': '',
'preempted': ''
})
StateMachine.add('p2',
nav_transit_states['p2'],
transitions={
'succeeded': 'p3',
'aborted': '',
'preempted': ''
})
StateMachine.add('p3',
nav_transit_states['p3'],
transitions={
'succeeded': 'p4',
'aborted': '',
'preempted': ''
})
StateMachine.add('p4',
nav_transit_states['p4'],
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
# Create and start the SMACH Introspection server
intro_server = IntrospectionServer(str(rospy.get_name()),
hsm_pool_patrol, '/SM_ROOT')
intro_server.start()
# Execute the state machine
hsm_outcome = hsm_pool_patrol.execute()
intro_server.stop()
def __init__(self):
# init node
rospy.init_node('pool_patrol', anonymous=False)
# Set the shutdown fuction (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initilalize the mission parameters and variables
setup_task_environment(self)
# Turn the target locations into SMACH MoveBase and LosPathFollowing action states
nav_terminal_states = {}
nav_transit_states = {}
# DP controller
for target in self.pool_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'odom'
nav_goal.target_pose.pose = self.pool_locations[target]
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.nav_result_cb,
exec_timeout=self.nav_timeout,
server_wait_timeout=rospy.Duration(10.0))
nav_terminal_states[target] = move_base_state
for target in self.pool_locations.iterkeys():
print(target)
nav_goal = LosPathFollowingGoal()
#nav_goal.prev_waypoint = navigation.vehicle_pose.position
nav_goal.next_waypoint = self.pool_locations[target].position
nav_goal.forward_speed.linear.x = self.transit_speed
nav_goal.desired_depth.z = self.pool_locations[target].position.z
nav_goal.sphereOfAcceptance = self.los_sphere_of_acceptance
los_path_state = SimpleActionState(
'los_path',
LosPathFollowingAction,
goal=nav_goal,
result_cb=self.nav_result_cb,
exec_timeout=self.nav_timeout,
server_wait_timeout=rospy.Duration(10.0))
nav_transit_states[target] = los_path_state
# Initialize the HSM
hsm_pool_patrol = StateMachine(outcomes=[
'succeeded', 'aborted', 'preempted', 'passed', 'missed', 'unseen',
'found'
])
# Build the HSM from nav states and target states
with hsm_pool_patrol:
# Qualification Run
StateMachine.add('OPEN_LOOP',
ControlMode(OPEN_LOOP),
transitions={
'succeeded': 'GO_TO_CORNER1',
'aborted': 'OPEN_LOOP',
'preempted': 'OPEN_LOOP'
})
StateMachine.add('GO_TO_CORNER1',
nav_transit_states['corner_1'],
transitions={
'succeeded': 'POSE_HEADING_1',
'aborted': 'GO_TO_CORNER1',
'preempted': 'GO_TO_CORNER1'
})
StateMachine.add('POSE_HEADING_1',
ControlMode(POSE_HEADING_HOLD),
transitions={
'succeeded': 'DIVE',
'aborted': 'POSE_HEADING_1',
'preempted': 'POSE_HEADING_1'
})
StateMachine.add('DIVE',
nav_terminal_states['corner_1'],
transitions={
'succeeded': 'OPEN_LOOP_1',
'aborted': 'DIVE',
'preempted': 'DIVE'
})
StateMachine.add('OPEN_LOOP_1',
ControlMode(OPEN_LOOP),
transitions={
'succeeded': 'GO_TO_GATE',
'aborted': 'OPEN_LOOP_1',
'preempted': 'OPEN_LOOP_1'
})
StateMachine.add('GO_TO_GATE',
nav_transit_states['corner_2'],
transitions={
'succeeded': 'POSE_HEADING_HOLD_1',
'aborted': 'GO_TO_GATE',
'preempted': 'GO_TO_GATE'
})
StateMachine.add('POSE_HEADING_HOLD_1',
ControlMode(POSE_HEADING_HOLD),
transitions={
'succeeded': 'HOLD_GATE',
'aborted': 'POSE_HEADING_HOLD_1',
'preempted': 'POSE_HEADING_HOLD_1'
})
StateMachine.add('HOLD_GATE',
nav_terminal_states['corner_2'],
transitions={
'succeeded': 'OPEN_LOOP_2',
'aborted': 'DIVE',
'preempted': 'DIVE'
})
StateMachine.add('OPEN_LOOP_2',
ControlMode(OPEN_LOOP),
transitions={
'succeeded': 'GO_TO_BOUY',
'aborted': 'OPEN_LOOP_2',
'preempted': 'OPEN_LOOP_2'
})
StateMachine.add('GO_TO_BOUY',
nav_transit_states['corner_3'],
transitions={
'succeeded': 'POSE_HEADING_HOLD_2',
'aborted': 'GO_TO_BOUY',
'preempted': 'GO_TO_BOUY'
})
StateMachine.add('POSE_HEADING_HOLD_2',
ControlMode(POSE_HEADING_HOLD),
transitions={
'succeeded': 'HOLD_BOUY',
'aborted': 'POSE_HEADING_HOLD_2',
'preempted': 'POSE_HEADING_HOLD_2'
})
StateMachine.add('HOLD_BOUY',
nav_terminal_states['corner_3'],
transitions={
'succeeded': 'OPEN_LOOP_3',
'aborted': 'DIVE',
'preempted': 'DIVE'
})
StateMachine.add('OPEN_LOOP_3',
ControlMode(OPEN_LOOP),
transitions={
'succeeded': 'GO_TO_START',
'aborted': 'OPEN_LOOP_3',
'preempted': 'OPEN_LOOP_3'
})
StateMachine.add('GO_TO_START',
nav_transit_states['corner_4'],
transitions={
'succeeded': 'POSE_HEADING_HOLD_3',
'aborted': 'GO_TO_START',
'preempted': 'GO_TO_START'
})
StateMachine.add('POSE_HEADING_HOLD_3',
ControlMode(POSE_HEADING_HOLD),
transitions={
'succeeded': 'HOLD_START',
'aborted': 'POSE_HEADING_HOLD_3',
'preempted': 'POSE_HEADING_HOLD_3'
})
StateMachine.add('HOLD_START',
nav_terminal_states['corner_4'],
transitions={
'succeeded': 'OPEN_LOOP',
'aborted': 'HOLD_START',
'preempted': 'HOLD_START'
})
# Create and start the SMACH Introspection server
intro_server = IntrospectionServer(str(rospy.get_name()),
hsm_pool_patrol, '/SM_ROOT')
intro_server.start()
# Execute the state machine
hsm_outcome = hsm_pool_patrol.execute()
intro_server.stop()
def __init__(self):
# init node
rospy.init_node('pool_patrol', anonymous=False)
# Set the shutdown fuction (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initilalize the mission parameters and variables
setup_task_environment(self)
# A variable to hold the last/current mission goal/state
self.last_mission_state = None
# A flag to indicate whether or not we are recharging
self.recharging = False
# Turn the target locations into SMACH MoveBase and LosPathFollowing action states
nav_terminal_states = {}
nav_transit_states = {}
# DP controller
for target in self.pool_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'odom'
nav_goal.target_pose.pose = self.pool_locations[target]
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.nav_result_cb,
exec_timeout=self.nav_timeout,
server_wait_timeout=rospy.Duration(10.0))
nav_terminal_states[target] = move_base_state
# Path following
for target in self.pool_locations.iterkeys():
nav_goal = LosPathFollowingGoal()
#nav_goal.prev_waypoint = navigation.vehicle_pose.position
nav_goal.next_waypoint = self.pool_locations[target].position
nav_goal.forward_speed.linear.x = self.transit_speed
nav_goal.desired_depth.z = self.search_depth
nav_goal.sphereOfAcceptance = self.search_area_size
los_path_state = SimpleActionState(
'los_path',
LosPathFollowingAction,
goal=nav_goal,
result_cb=self.nav_result_cb,
exec_timeout=self.nav_timeout,
server_wait_timeout=rospy.Duration(10.0))
nav_transit_states[target] = los_path_state
""" Create individual state machines for assigning tasks to each target zone """
# Create a state machine container for the orienting towards the gate subtask(s)
self.sm_gate_tasks = StateMachine(outcomes=[
'found', 'unseen', 'missed', 'passed', 'aborted', 'preempted'
])
# Then add the subtask(s)
with self.sm_gate_tasks:
# if gate is found, pass pixel info onto TrackTarget. If gate is not found, look again
StateMachine.add('SCANNING_OBJECTS',
SearchForTarget('gate'),
transitions={
'found': 'CAMERA_CENTERING',
'unseen': 'BROADEN_SEARCH',
'passed': '',
'missed': ''
},
remapping={
'px_output': 'object_px',
'fx_output': 'object_fx',
'search_output': 'object_search',
'search_confidence_output':
'object_confidence'
})
StateMachine.add('CAMERA_CENTERING',
TrackTarget('gate',
self.pool_locations['gate'].position),
transitions={'succeeded': 'SCANNING_OBJECTS'},
remapping={
'px_input': 'object_px',
'fx_input': 'object_fx',
'search_input': 'object_search',
'search_confidence_input': 'object_confidence'
})
StateMachine.add('BROADEN_SEARCH',
TrackTarget('gate',
self.pool_locations['gate'].position),
transitions={'succeeded': 'SCANNING_OBJECTS'},
remapping={
'px_input': 'object_px',
'fx_input': 'object_fx',
'search_input': 'object_search',
'search_confidence_input': 'object_confidence'
})
# Create a state machine container for returning to dock
self.sm_docking = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add states to container
with self.sm_docking:
StateMachine.add('RETURN_TO_DOCK',
nav_transit_states['docking'],
transitions={
'succeeded': 'DOCKING_SECTOR',
'aborted': '',
'preempted': 'RETURN_TO_DOCK'
})
StateMachine.add('DOCKING_SECTOR',
ControlMode(POSE_HEADING_HOLD),
transitions={
'succeeded': 'DOCKING_PROCEEDURE',
'aborted': '',
'preempted': ''
})
StateMachine.add('DOCKING_PROCEEDURE',
nav_terminal_states['docking'],
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
""" Assemble a Hierarchical State Machine """
# Initialize the HSM
self.sm_mission = StateMachine(outcomes=[
'succeeded', 'aborted', 'preempted', 'passed', 'missed', 'unseen',
'found'
])
# Build the HSM from nav states and target states
with self.sm_mission:
""" Navigate to GATE in TERMINAL mode """
StateMachine.add('TRANSIT_TO_GATE',
nav_transit_states['gate'],
transitions={
'succeeded': 'GATE_SEARCH',
'aborted': 'DOCKING',
'preempted': 'TRANSIT_TO_GATE'
})
""" When in GATE sector"""
StateMachine.add('GATE_SEARCH',
self.sm_gate_tasks,
transitions={
'passed': 'GATE_PASSED',
'missed': 'TRANSIT_TO_GATE',
'aborted': 'DOCKING'
})
""" Transiting to gate """
StateMachine.add('GATE_PASSED',
ControlMode(OPEN_LOOP),
transitions={
'succeeded': 'TRANSIT_TO_POLE',
'aborted': 'DOCKING',
'preempted': 'TRANSIT_TO_GATE'
})
StateMachine.add('TRANSIT_TO_POLE',
nav_transit_states['pole'],
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
""" When in POLE sector"""
#StateMachine.add('POLE_PASSING_TASK', sm_pole_tasks, transitions={'passed':'POLE_PASSING_TASK','missed':'RETURN_TO_DOCK','aborted':'RETURN_TO_DOCK'})
""" When aborted, return to docking """
StateMachine.add('DOCKING',
self.sm_docking,
transitions={'succeeded': ''})
# Register a callback function to fire on state transitions within the sm_mission state machine
self.sm_mission.register_transition_cb(self.mission_transition_cb,
cb_args=[])
# Initialize the recharge state machine
self.sm_recharge = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add states to the container
with self.sm_recharge:
""" To be able to dock we have to be in the recharging area """
StateMachine.add('DOCKING',
self.sm_docking,
transitions={'succeeded': 'RECHARGE_BATTERY'})
""" Batteru is recharged by using the set_battery_level service to 100 percent battery level """
StateMachine.add('RECHARGE_BATTERY',
ServiceState(
'battery_simulator/set_battery_level',
SetBatteryLevel,
100,
response_cb=self.recharge_cb),
transitions={'succeeded': 'RECHARGE_FINISHED'})
""" when recharge is finished, we have to set the DP controller in open loop so we can use the transit controller """
StateMachine.add('RECHARGE_FINISHED',
ControlMode(OPEN_LOOP),
transitions={'succeeded': ''})
# Create the sm_concurrence state machine using a concurrence container
self.sm_concurrence = Concurrence(
outcomes=['succeeded', 'recharge', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_mission and a battery MonitorState to the sm_concurrence container
with self.sm_concurrence:
Concurrence.add('SM_MISSION', self.sm_mission)
Concurrence.add(
'MONITOR_BATTERY',
MonitorState("/manta/battery_level", Float32, self.battery_cb))
# Create the top level state machine
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add sm_concurrence, sm_recharge and a Stop() objective to sm_top
with self.sm_top:
StateMachine.add('PATROL',
self.sm_concurrence,
transitions={
'succeeded': '',
'recharge': 'RECHARGE',
'stop': 'STOP'
})
StateMachine.add('RECHARGE',
self.sm_recharge,
transitions={'succeeded': 'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})
# Create and start the SMACH Introspection server
intro_server = IntrospectionServer(str(rospy.get_name()), self.sm_top,
'/SM_ROOT')
intro_server.start()
# Execute the state machine
hsm_outcome = self.sm_top.execute()
intro_server.stop()
def __init__(self):
# init node
rospy.init_node('pool_patrol', anonymous=False)
# Set the shutdown fuction (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initilalize the mission parameters and variables
setup_task_environment(self)
# Turn the target locations into SMACH MoveBase and LosPathFollowing action states
nav_terminal_states = {}
nav_transit_states = {}
# DP controller
for target in self.pool_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'odom'
nav_goal.target_pose.pose = self.pool_locations[target]
move_base_state = SimpleActionState('move_base', MoveBaseAction,
goal=nav_goal,
result_cb=self.nav_result_cb,
exec_timeout=self.nav_timeout,
server_wait_timeout=rospy.Duration(10.0))
nav_terminal_states[target] = move_base_state
# Path following
for target in self.pool_locations.iterkeys():
nav_goal = LosPathFollowingGoal()
#nav_goal.prev_waypoint = navigation.vehicle_pose.position
nav_goal.next_waypoint = self.pool_locations[target].position
nav_goal.forward_speed.linear.x = self.transit_speed
nav_goal.desired_depth.z = self.search_depth
nav_goal.sphereOfAcceptance = self.search_area_size
los_path_state = SimpleActionState('los_path', LosPathFollowingAction,
goal=nav_goal,
result_cb=self.nav_result_cb,
exec_timeout=self.nav_timeout,
server_wait_timeout=rospy.Duration(10.0))
nav_transit_states[target] = los_path_state
""" Create individual state machines for assigning tasks to each target zone """
# Create a state machine container for the orienting towards the gate subtask(s)
sm_gate_tasks = StateMachine(outcomes=['found','unseen','missed','passed','aborted','preempted'])
# Then add the subtask(s)
with sm_gate_tasks:
# if gate is found, pass pixel info onto TrackTarget. If gate is not found, look again
StateMachine.add('SCANNING_OBJECTS', SearchForTarget('gate'), transitions={'found':'CAMERA_CENTERING','unseen':'BROADEN_SEARCH','passed':'','missed':''},
remapping={'px_output':'object_px','fx_output':'object_fx','search_output':'object_search','search_confidence_output':'object_confidence'})
StateMachine.add('CAMERA_CENTERING', TrackTarget('gate', self.pool_locations['gate'].position), transitions={'succeeded':'SCANNING_OBJECTS'},
remapping={'px_input':'object_px','fx_input':'object_fx','search_input':'object_search','search_confidence_input':'object_confidence'})
StateMachine.add('BROADEN_SEARCH', TrackTarget('gate', self.pool_locations['gate'].position), transitions={'succeeded':'SCANNING_OBJECTS'},
remapping={'px_input':'object_px','fx_input':'object_fx','search_input':'object_search','search_confidence_input':'object_confidence'})
# Create a state machine container for returning to dock
sm_docking = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Add states to container
with sm_docking:
StateMachine.add('RETURN_TO_DOCK', nav_transit_states['docking'], transitions={'succeeded':'DOCKING_SECTOR','aborted':'','preempted':'RETURN_TO_DOCK'})
StateMachine.add('DOCKING_SECTOR', ControlMode(POSE_HEADING_HOLD), transitions={'succeeded':'DOCKING_PROCEEDURE','aborted':'','preempted':''})
StateMachine.add('DOCKING_PROCEEDURE', nav_terminal_states['docking'], transitions={'succeeded':'','aborted':'','preempted':''})
""" Assemble a Hierarchical State Machine """
# Initialize the HSM
hsm_pool_patrol = StateMachine(outcomes=['succeeded','aborted','preempted','passed','missed','unseen','found'])
# Build the HSM from nav states and target states
with hsm_pool_patrol:
""" Navigate to GATE in TERMINAL mode """
StateMachine.add('TRANSIT_TO_GATE', nav_transit_states['gate'], transitions={'succeeded':'GATE_SEARCH','aborted':'DOCKING','preempted':'DOCKING'})
""" When in GATE sector"""
StateMachine.add('GATE_SEARCH', sm_gate_tasks, transitions={'passed':'GATE_PASSED','missed':'DOCKING','aborted':'DOCKING'})
""" Transiting to gate """
StateMachine.add('GATE_PASSED', ControlMode(OPEN_LOOP), transitions={'succeeded':'TRANSIT_TO_POLE','aborted':'DOCKING','preempted':'DOCKING'})
StateMachine.add('TRANSIT_TO_POLE', nav_transit_states['pole'], transitions={'succeeded':'DOCKING','aborted':'DOCKING','preempted':'DOCKING'})
""" When in POLE sector"""
#StateMachine.add('POLE_PASSING_TASK', sm_pole_tasks, transitions={'passed':'POLE_PASSING_TASK','missed':'RETURN_TO_DOCK','aborted':'RETURN_TO_DOCK'})
""" When aborted, return to docking """
StateMachine.add('DOCKING', sm_docking, transitions={'succeeded':'','aborted':'','preempted':''})
# Create and start the SMACH Introspection server
intro_server = IntrospectionServer(str(rospy.get_name()),hsm_pool_patrol,'/SM_ROOT')
intro_server.start()
# Execute the state machine
hsm_outcome = hsm_pool_patrol.execute()
intro_server.stop()
