def main():
rospy.init_node('exploration_fsm')
sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with sm:
StateMachine.add('INITIAL_TURN',
InitialTurnState(),
transitions={
'succeeded': 'EXPLORATION',
'aborted': 'EXPLORATION',
'preempted': 'preempted'
})
StateMachine.add('EXPLORATION',
simpleExplorationContainer(),
transitions={'preempted': 'preempted'})
sis = smach_ros.IntrospectionServer('fsm_introspection', sm,
'/EXPLORATION_FSM')
sis.start()
smach_ros.set_preempt_handler(sm)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target=sm.execute)
smach_thread.start()
rospy.spin()
sis.stop()
def main():
rospy.init_node("fsm_dummy")
sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with sm:
StateMachine.add('INITIAL_TURN',
SimpleActionState('/initial_turn', InitialTurnAction),
transitions={
'succeeded': 'EXPLORATION',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('EXPLORATION',
exploration.ExplorationContainer(),
transitions={
'victim_thermal': 'IDENTIFICATION_SIMPLE',
'victim_camera': 'IDENTIFICATION_TRACKING',
'aborted': 'aborted',
'preempted': 'preempted',
'time_out': 'AGGRESSIVE_EXPLORATION'
})
StateMachine.add('IDENTIFICATION_SIMPLE',
identification.IdentificationSimpleContainer(),
transitions={
'parked': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('IDENTIFICATION_TRACKING',
identification.IdentificationTrackingContainer(),
transitions={
'identification_finished': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('AGGRESSIVE_EXPLORATION',
SimpleActionState('/navigation/initial_turn',
InitialTurnAction),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/DUMMY_FSM')
sis.start()
smach_ros.set_preempt_handler(sm)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target=sm.execute)
smach_thread.start()
rospy.spin()
sis.stop()
def main():
rospy.init_node("fsm_dummy")
sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm:
StateMachine.add('INITIAL_TURN', SimpleActionState('/navigation/initial_turn', InitialTurnAction),
transitions={'succeeded':'EXPLORATION','aborted':'aborted','preempted':'preempted'})
StateMachine.add('EXPLORATION', exploration.ExplorationContainer(),
transitions={'victim_thermal':'IDENTIFICATION_SIMPLE',
'victim_camera':'IDENTIFICATION_TRACKING','aborted':'aborted','preempted':'preempted','time_out':'AGGRESSIVE_EXPLORATION'})
StateMachine.add('IDENTIFICATION_SIMPLE', identification.IdentificationSimpleContainer(),
transitions={'parked':'succeeded','aborted':'aborted','preempted':'preempted'})
StateMachine.add('IDENTIFICATION_TRACKING', identification.IdentificationTrackingContainer(),
transitions={'identification_finished':'succeeded','aborted':'aborted','preempted':'preempted'})
StateMachine.add('AGGRESSIVE_EXPLORATION', SimpleActionState('/navigation/initial_turn', InitialTurnAction),
transitions={'succeeded':'succeeded','aborted':'aborted','preempted':'preempted'})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/DUMMY_FSM')
sis.start()
smach_ros.set_preempt_handler(sm)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target = sm.execute)
smach_thread.start()
rospy.spin()
sis.stop()
def main():
rospy.init_node('smach_usecase_step_04')
# Construct static goals
polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0)
polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5)
# Create a SMACH state machine
sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Open the container
with sm0:
# Reset turtlesim
StateMachine.add('RESET',
ServiceState('reset', std_srvs.srv.Empty),
{'succeeded':'SPAWN'})
# Create a second turtle
StateMachine.add('SPAWN',
ServiceState('spawn', turtlesim.srv.Spawn,
request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')),
{'succeeded':'TELEPORT1'})
# Teleport turtle 1
StateMachine.add('TELEPORT1',
ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)),
{'succeeded':'TELEPORT2'})
# Teleport turtle 2
StateMachine.add('TELEPORT2',
ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
{'succeeded':'BIG'})
# Draw a large polygon with the first turtle
StateMachine.add('BIG',
SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
goal = polygon_big),
{'succeeded':'SMALL'})
# Draw a small polygon with the second turtle
StateMachine.add('SMALL',
SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
goal = polygon_small))
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Set preempt handler
smach_ros.set_preempt_handler(sm0)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target = sm0.execute)
smach_thread.start()
# Signal handler
rospy.spin()
def __init__(self, input_keys=[], output_keys=[]):
outcomes = ['succeeded', 'aborted', 'preempted']
smach.Container.__init__(self, outcomes, input_keys, output_keys)
self._root = None
self._initial_task = None
self._current_exec_root = None
self._lock = threading.Lock()
self._condition = threading.Condition(self._lock)
self._result = None
self._task_for_label = {}
self._label_for_task = {}
# Intentionally public
self.userdata = smach.UserData()
# Ensures that we halt when task is shut down
smach_ros.set_preempt_handler(self)
def __init__(self, input_keys=[], output_keys=[]):
outcomes = ["succeeded", "aborted", "preempted"]
smach.Container.__init__(self, outcomes, input_keys, output_keys)
self._root = None
self._initial_task = None
self._current_exec_root = None
self._lock = threading.Lock()
self._condition = threading.Condition(self._lock)
self._result = None
self._task_for_label = {}
self._label_for_task = {}
# Intentionally public
self.userdata = smach.UserData()
# Ensures that we halt when task is shut down
smach_ros.set_preempt_handler(self)
def main():
rospy.init_node('smach_usecase_step_06')
# Construct static goals
polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0)
polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5)
# Create a SMACH state machine
sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Open the container
with sm0:
# Reset turtlesim
StateMachine.add('RESET',
ServiceState('reset', std_srvs.srv.Empty),
{'succeeded':'SPAWN'})
# Create a second turtle
StateMachine.add('SPAWN',
ServiceState('spawn', turtlesim.srv.Spawn,
request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')),
{'succeeded':'TELEPORT1'})
# Teleport turtle 1
StateMachine.add('TELEPORT1',
ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)),
{'succeeded':'TELEPORT2'})
# Teleport turtle 2
StateMachine.add('TELEPORT2',
ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
{'succeeded':'DRAW_SHAPES'})
# Draw some polygons
shapes_cc = Concurrence(
outcomes=['succeeded','aborted','preempted'],
default_outcome='aborted',
outcome_map = {'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}})
StateMachine.add('DRAW_SHAPES',shapes_cc)
with shapes_cc:
# Draw a large polygon with the first turtle
Concurrence.add('BIG',
SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
goal = polygon_big))
# Draw a small polygon with the second turtle
draw_monitor_cc = Concurrence(
['succeeded','aborted','preempted'],
'aborted',
child_termination_cb = lambda so: True,
outcome_map = {
'succeeded':{'DRAW':'succeeded'},
'preempted':{'DRAW':'preempted','MONITOR':'preempted'},
'aborted':{'MONITOR':'invalid'}})
Concurrence.add('SMALL',draw_monitor_cc)
with draw_monitor_cc:
Concurrence.add('DRAW',
SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
goal = polygon_small))
def turtle_far_away(ud, msg):
"""Returns True while turtle pose in msg is at least 1 unit away from (9,5)"""
if sqrt(pow(msg.x-9.0,2) + pow(msg.y-5.0,2)) > 2.0:
return True
return False
Concurrence.add('MONITOR',
MonitorState('/turtle1/pose',turtlesim.msg.Pose,
cond_cb = turtle_far_away))
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Set preempt handler
smach_ros.set_preempt_handler(sm0)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target = sm0.execute)
smach_thread.start()
# Signal handler
rospy.spin()
def main():
rospy.init_node('fsm')
sm = StateMachine(outcomes=['preempted'])
with sm:
StateMachine.add('MONITOR_START',
MonitorModeState(robotModeMsg.MODE_START_AUTONOMOUS),
transitions={
'invalid': 'MONITOR_START',
'valid': 'ALL',
'preempted': 'preempted'
})
sm_everything = StateMachine(outcomes=['preempted'])
with sm_everything:
StateMachine.add('WAIT_FOR_SLAM',
Timer(10),
transitions={
'time_out': 'INITIAL_TURN',
'preempted': 'preempted'
})
StateMachine.add('INITIAL_TURN',
InitialTurnState(),
transitions={
'succeeded': 'ROBOT_MODE_EXPLORATION',
'aborted': 'ROBOT_MODE_EXPLORATION',
'preempted': 'preempted'
})
StateMachine.add('ROBOT_MODE_EXPLORATION',
ChangeRobotModeState(
robotModeMsg.MODE_EXPLORATION),
transitions={
'succeeded': 'EXPLORATION',
'preempted': 'preempted'
})
StateMachine.add('EXPLORATION',
explorationWithVictims(),
transitions={
'camera_alert': 'MONITOR_VICTIM_AND_DO_WORK',
'thermal_alert': 'MONITOR_VICTIM_AND_DO_WORK',
'preempted': 'preempted'
})
#~ sm.userdata.victim_info = None
sm_victim = StateMachine(
outcomes=['verified', 'not_verified', 'preempted', 'aborted'],
output_keys=['victim_info'])
with sm_victim:
sm_victim.userdata.victim_info = None
StateMachine.add('VICTIM_APPROACH',
cameraIdentificationSimple(),
transitions={
'victim_approached': 'ROBOT_MODE_DF_HOLD',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('ROBOT_MODE_DF_HOLD',
ChangeRobotModeState(
robotModeMsg.MODE_DF_HOLD),
transitions={
'succeeded': 'DATA_FUSION_HOLD',
'preempted': 'preempted'
})
StateMachine.add('DATA_FUSION_HOLD',
dataFusionHold(),
transitions={
'verified': 'verified',
'not_verified': 'not_verified',
'preempted': 'preempted'
},
remapping={'victim_info': 'victim_info'})
cc = Concurrence(outcomes=[
'verified', 'not_verified', 'update_victim', 'preempted',
'aborted'
],
default_outcome='not_verified',
output_keys=['victim_info'],
outcome_map={
'verified': {
'VICTIM_IDENTIFICATION': 'verified',
'VICTIM_UPDATE': 'preempted'
},
'not_verified': {
'VICTIM_IDENTIFICATION': 'not_verified',
'VICTIM_UPDATE': 'preempted'
},
'preempted': {
'VICTIM_IDENTIFICATION': 'preempted',
'VICTIM_UPDATE': 'preempted'
},
'aborted': {
'VICTIM_IDENTIFICATION': 'aborted',
'VICTIM_UPDATE': 'preempted'
},
'update_victim': {
'VICTIM_UPDATE': 'update_victim',
'VICTIM_IDENTIFICATION': 'preempted'
}
},
child_termination_cb=_termination_cb)
with cc:
Concurrence.add('VICTIM_IDENTIFICATION',
sm_victim,
remapping={'victim_info': 'victim_info'})
Concurrence.add('VICTIM_UPDATE', MonitorVictimUpdateState())
StateMachine.add('MONITOR_VICTIM_AND_DO_WORK',
cc,
transitions={
'verified': 'VICTIM_VALIDATION',
'not_verified': 'MONITOR_VICTIM_AND_DO_WORK',
'update_victim': 'MONITOR_VICTIM_AND_DO_WORK',
'preempted': 'preempted',
'aborted': 'ROBOT_MODE_EXPLORATION'
},
remapping={'victim_info': 'victim_info'})
StateMachine.add('VICTIM_VALIDATION',
validateVictim(),
transitions={
'valid': 'MONITOR_VICTIM_AND_DO_WORK',
'not_valid': 'MONITOR_VICTIM_AND_DO_WORK',
'preempted': 'preempted'
},
remapping={'victim_info': 'victim_info'})
cc = Concurrence(outcomes=['preempted', 'shutdown'],
default_outcome='preempted',
outcome_map={
'preempted': {
'AUTONOMOUS': 'preempted',
'MONITOR_SHUTDOWN': 'preempted'
},
'shutdown': {
'MONITOR_SHUTDOWN': 'valid',
'AUTONOMOUS': 'preempted'
}
},
child_termination_cb=_termination_cb_all)
with cc:
Concurrence.add('AUTONOMOUS', sm_everything)
Concurrence.add(
'MONITOR_SHUTDOWN',
MonitorModeState(robotModeMsg.MODE_TELEOPERATED_LOCOMOTION))
StateMachine.add('ALL',
cc,
transitions={
'shutdown': 'MONITOR_START',
'preempted': 'preempted'
})
sis = smach_ros.IntrospectionServer('fsm_introspection', sm,
'/PANDORA_FSM')
sis.start()
smach_ros.set_preempt_handler(sm)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target=sm.execute)
smach_thread.start()
rospy.spin()
sis.stop()
def polygonial():
ids = [2, 5, 1]
#define the differents points
test_point = Point()
test_point.x = 0
test_point.y = 0
test_point.z = 0.5
home_points_fo8 = [Point(), Point(), Point()]
home_points_fo8[0].x = 0.0
home_points_fo8[0].y = 0.0
home_points_fo8[0].z = 0.3
home_points_fo8[1].x = 0.3
home_points_fo8[1].y = 0.0
home_points_fo8[1].z = 0.3
home_points_fo8[2].x = -0.3
home_points_fo8[2].y = 0.0
home_points_fo8[2].z = 0.3
home_points_heli = [Point(), Point(), Point()]
home_points_heli[0].x = 0.0
home_points_heli[0].y = 0.0
home_points_heli[0].z = 0.3
home_points_heli[1].x = 0.0
home_points_heli[1].y = 0.9 - 0.4
home_points_heli[1].z = 0.9
home_points_heli[2].x = -0.3
home_points_heli[2].y = 0.0
home_points_heli[2].z = 0.3
my_points = [
Point(),
Point(),
Point(),
Point(),
Point(),
Point(),
Point(),
Point()
]
my_points[0].x = 0.5
my_points[0].y = -0.5
my_points[0].z = 0.5
my_points[1].x = 0.7
my_points[1].y = 0.0
my_points[1].z = 0.5
my_points[2].x = 0.5
my_points[2].y = 0.5
my_points[2].z = 0.5
my_points[3].x = 0
my_points[3].y = 0.7
my_points[3].z = 0.5
my_points[4].x = -0.5
my_points[4].y = 0.5
my_points[4].z = 0.5
my_points[5].x = -0.7
my_points[5].y = 0
my_points[5].z = 0.5
my_points[6].x = -0.5
my_points[6].y = -0.5
my_points[6].z = 0.5
my_points[7].x = 0
my_points[7].y = -0.7
my_points[7].z = 0.5
# Create a SMACH state machine
sm0 = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
#progressively add drones
with sm0:
"""
StateMachine.add('HELI_START',
SimpleActionState('drone1detect_perimeter',
my_newAction, goal = my_newGoal(point = home_points_heli[0], id = ids[0] )),
transitions={'succeeded' : 'HELI_EXECUTE', 'aborted' : 'land_all', 'preempted' : 'land_all'})
StateMachine.add('HELI_EXECUTE',
SimpleActionState('trajectory_action',
doTrajAction, goal = doTrajGoal(shape = 1, id = ids[0] )),
transitions={'succeeded' : 'HELI_END', 'aborted' : 'land_all', 'preempted' : 'land_all'})
StateMachine.add('HELI_END',
SimpleActionState('land_drone1',
my_newAction, goal = my_newGoal(point = my_points[3], id = ids[0]) ),
transitions={'succeeded' : 'drone2-FIG8', 'aborted' : 'land_all', 'preempted' : 'land_all'})
"""
StateMachine.add('drone2-FIG8',
SimpleActionState('drone2detect_perimeter',
my_newAction,
goal=my_newGoal(
point=home_points_fo8[2],
id=ids[1])),
transitions={
'succeeded': 'drone3-FIG8',
'aborted': 'land_all',
'preempted': 'land_all'
})
StateMachine.add('drone3-FIG8',
SimpleActionState('drone3detect_perimeter',
my_newAction,
goal=my_newGoal(
point=home_points_fo8[1],
id=ids[2])),
transitions={
'succeeded': 'FIG8_EXECUTE',
'aborted': 'land_all',
'preempted': 'land_all'
})
# fig8_sm = Concurrence(['drone2-2', 'land_all'], 'land_all',
# outcome_map={'drone2-2' : {'FIG8_EXECUTE_drone3' : 'succeeded', 'FIG8_EXECUTE_drone2' : 'succeeded'}})
fig8_sm = Concurrence(['succeeded', 'aborted', 'preempted'],
'succeeded') #,
#outcome_map={'succeeded' : 'drone2-2', 'preempted' : 'land_all', 'aborted' : 'land_all' })
StateMachine.add('FIG8_EXECUTE',
fig8_sm,
transitions={
'succeeded': 'drone2-2',
'aborted': 'land_all',
'preempted': 'land_all'
})
## Bug BBL: DO NOT try to decide on outcomes in a concurrence, do it in the SM.add!!
with fig8_sm:
Concurrence.add(
'FIG8_EXECUTE_drone2',
SimpleActionState('fig8_drone2',
doTrajAction,
goal=doTrajGoal(shape=8, id=ids[1])))
Concurrence.add(
'FIG8_EXECUTE_drone3',
SimpleActionState('fig8_drone3',
doTrajAction,
goal=doTrajGoal(shape=8, id=ids[2])))
heli_sm = Concurrence(['succeeded', 'aborted', 'preempted'],
'succeeded') #,
#outcome_map={'succeeded' : 'drone2-2', 'preempted' : 'land_all', 'aborted' : 'land_all' })
StateMachine.add('HELI_EXECUTE',
heli_sm,
transitions={
'succeeded': 'drone2-2',
'aborted': 'land_all',
'preempted': 'land_all'
})
## Bug BBL: DO NOT try to decide on outcomes in a concurrence, do it in the SM.add!!
with heli_sm:
Concurrence.add(
'HELI_EXECUTE_drone2',
SimpleActionState('heli_drone2',
doTrajAction,
goal=doTrajGoal(shape=0, id=ids[1])))
Concurrence.add(
'HELI_EXECUTE_drone3',
SimpleActionState('heli_drone3',
doTrajAction,
goal=doTrajGoal(shape=0, id=ids[2])))
fig8_end = Concurrence(['succeeded', 'aborted', 'preempted'],
'succeeded') #,
#outcome_map={'succeeded' : 'drone2-2', 'preempted' : 'land_all', 'aborted' : 'land_all' })
StateMachine.add('FIG8_END',
fig8_end,
transitions={
'succeeded': 'drone2-2',
'aborted': 'land_all',
'preempted': 'land_all'
})
## Bug BBL: DO NOT try to decide on outcomes in a concurrence, do it in the SM.add!!
with fig8_end:
Concurrence.add(
'FIG8_END_drone2',
SimpleActionState('land_drone2',
my_newAction,
goal=my_newGoal(point=my_points[3],
id=ids[1])))
Concurrence.add(
'FIG8_END_drone3',
SimpleActionState('land_drone3',
my_newAction,
goal=my_newGoal(point=my_points[3],
id=ids[2])))
StateMachine.add('drone2-2',
SimpleActionState('drone2detect_perimeter',
my_newAction,
goal=my_newGoal(point=my_points[2],
id=ids[1])),
transitions={
'succeeded': 'drone3-1',
'aborted': 'land_all',
'preempted': 'land_all'
})
StateMachine.add('drone3-1',
SimpleActionState('drone3detect_perimeter',
my_newAction,
goal=my_newGoal(point=my_points[0],
id=ids[2])),
transitions={
'succeeded': 'drone1-3',
'aborted': 'land_all',
'preempted': 'land_all'
})
StateMachine.add('drone1-3',
SimpleActionState('drone1detect_perimeter',
my_newAction,
goal=my_newGoal(point=my_points[4],
id=ids[0])),
transitions={
'succeeded': 'infinit_loop',
'aborted': 'land_all',
'preempted': 'land_all'
})
land_sm = Concurrence(['succeeded', 'aborted', 'preempted'],
'succeeded')
StateMachine.add('land_all', land_sm)
with land_sm:
# DUPLICATA FOR HIGH LEVEL
# #Land Drone If Aborted
Concurrence.add(
'LAND_DRONE1',
SimpleActionState('land_drone1',
my_newAction,
goal=my_newGoal(point=my_points[3],
id=ids[0])))
# #Land Drone If Aborted
Concurrence.add(
'LAND_DRONE2',
SimpleActionState('land_drone2',
my_newAction,
goal=my_newGoal(point=my_points[3],
id=ids[1])))
# #Land Drone If Aborted
Concurrence.add(
'LAND_DRONE3',
SimpleActionState('land_drone3',
my_newAction,
goal=my_newGoal(point=my_points[3],
id=ids[2])))
############################################
sm1 = Concurrence(
['succeeded', 'aborted', 'preempted'],
'succeeded',
#child_termination_cb = lambda so: True,
#outcome_map = {
#'succeeded':{'WAIT_FOR_CLEAR':'valid'},
#'aborted':{'DRONE1':'aborted'}}
)
StateMachine.add('infinit_loop', sm1)
with sm1:
drone1 = StateMachine(
outcomes=['succeeded', 'aborted',
'preempted']) # ['succeeded','aborted','preempted']
test_drone = StateMachine(
outcomes=['succeeded', 'aborted',
'preempted']) # ['succeeded','aborted','preempted']
#Concurrence.add('DRONE1', test_drone)
with test_drone:
order = (5, 6, 7, 0, 1, 2, 3, 4)
for i in range(7):
point_for_state = Point()
point_for_state.x = test_point.x
point_for_state.y = test_point.y
if i <= 4:
point_for_state.z = test_point.z + (i * 0.1)
else:
point_for_state.z = test_point.z - (i * 0.1) + 0.8
StateMachine.add('DRONE1-' + str(order[i]),
SimpleActionState(
'drone1detect_perimeter',
my_newAction,
goal=my_newGoal(point=point_for_state,
id=ids[0])),
transitions={
'succeeded':
'DRONE1-' + str(order[i + 1]),
'aborted': 'LAND_DRONE1',
'preempted': 'LAND_DRONE1'
})
#make it infinit
smach.StateMachine.add(
'DRONE1-' + str(order[-1]),
SimpleActionState('drone1detect_perimeter',
my_newAction,
goal=my_newGoal(point=test_point,
id=ids[0])),
transitions={
'succeeded': 'DRONE1-' + str(order[0]),
'aborted': 'LAND_DRONE1',
'preempted': 'LAND_DRONE1'
})
Concurrence.add('DRONE1', drone1)
# Open the container
with drone1:
#add each state
order = (5, 6, 7, 0, 1, 2, 3, 4)
for i in range(7):
point_for_state = Point()
point_for_state.x = my_points[order[i]].x
point_for_state.y = my_points[order[i]].y
if i <= 4:
point_for_state.z = my_points[order[i]].z + (i * 0.1)
else:
point_for_state.z = my_points[order[i]].z - (i *
0.1) + 0.8
StateMachine.add('DRONE1-' + str(order[i]),
SimpleActionState(
'drone1detect_perimeter',
my_newAction,
goal=my_newGoal(point=point_for_state,
id=ids[0])),
transitions={
'succeeded':
'DRONE1-' + str(order[i + 1]),
'aborted': 'LAND_DRONE1',
'preempted': 'LAND_DRONE1'
})
#make it infinit
smach.StateMachine.add(
'DRONE1-' + str(order[-1]),
SimpleActionState('drone1detect_perimeter',
my_newAction,
goal=my_newGoal(
point=my_points[order[-1]],
id=ids[0])),
transitions={
'succeeded': 'DRONE1-' + str(order[0]),
'aborted': 'LAND_DRONE1',
'preempted': 'LAND_DRONE1'
})
# #Land Drone If Aborted
smach.StateMachine.add(
'LAND_DRONE1',
SimpleActionState('land_drone1',
my_newAction,
goal=my_newGoal(point=my_points[3],
id=ids[0])),
transitions={'succeeded': 'LAND_DRONE1'})
drone2 = StateMachine(
outcomes=['succeeded', 'aborted',
'preempted']) # ['succeeded','aborted','preempted']
Concurrence.add('DRONE2', drone2)
# Open the container
with drone2:
#add each state
order = (3, 4, 5, 6, 7, 0, 1, 2)
for i in range(7):
point_for_state = Point()
point_for_state.x = my_points[order[i]].x
point_for_state.y = my_points[order[i]].y
if i <= 4:
point_for_state.z = my_points[order[i]].z + (i * 0.1)
else:
point_for_state.z = my_points[order[i]].z - (i *
0.1) + 0.8
StateMachine.add('DRONE2-' + str(order[i]),
SimpleActionState(
'drone2detect_perimeter',
my_newAction,
goal=my_newGoal(point=point_for_state,
id=ids[1])),
transitions={
'succeeded':
'DRONE2-' + str(order[i + 1]),
'aborted': 'LAND_DRONE2',
'preempted': 'LAND_DRONE2'
})
#make it infinit
smach.StateMachine.add(
'DRONE2-' + str(order[-1]),
SimpleActionState('drone2detect_perimeter',
my_newAction,
goal=my_newGoal(
point=my_points[order[-1]],
id=ids[1])),
transitions={
'succeeded': 'DRONE2-' + str(order[0]),
'aborted': 'LAND_DRONE2',
'preempted': 'LAND_DRONE2'
})
# #Land Drone If Aborted
smach.StateMachine.add(
'LAND_DRONE2',
SimpleActionState('land_drone2',
my_newAction,
goal=my_newGoal(point=my_points[3],
id=ids[1])),
transitions={'succeeded': 'LAND_DRONE2'})
drone3 = StateMachine(
outcomes=['succeeded', 'aborted',
'preempted']) # ['succeeded','aborted','preempted']
Concurrence.add('DRONE3-', drone3)
# Open the container
with drone3:
#add each state
order = (1, 2, 3, 4, 5, 6, 7, 0)
for i in range(7):
point_for_state = Point()
point_for_state.x = my_points[order[i]].x
point_for_state.y = my_points[order[i]].y
if i <= 4:
point_for_state.z = my_points[order[i]].z + (i * 0.1)
else:
point_for_state.z = my_points[order[i]].z - (i *
0.1) + 0.8
StateMachine.add('DRONE3-' + str(order[i]),
SimpleActionState(
'drone3detect_perimeter',
my_newAction,
goal=my_newGoal(point=point_for_state,
id=ids[2])),
transitions={
'succeeded':
'DRONE3-' + str(order[i + 1]),
'aborted': 'LAND_DRONE3',
'preempted': 'LAND_DRONE3'
})
#make it infinit
smach.StateMachine.add(
'DRONE3-' + str(order[-1]),
SimpleActionState('drone3detect_perimeter',
my_newAction,
goal=my_newGoal(
point=my_points[order[-1]],
id=ids[2])),
transitions={
'succeeded': 'DRONE3-' + str(order[0]),
'aborted': 'LAND_DRONE3',
'preempted': 'LAND_DRONE3'
})
# #Land Drone If Aborted
smach.StateMachine.add(
'LAND_DRONE3',
SimpleActionState('land_drone3',
my_newAction,
goal=my_newGoal(point=my_points[3],
id=ids[2])),
transitions={'succeeded': 'LAND_DRONE3'})
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Set preempt handler
smach_ros.set_preempt_handler(sm0)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target=sm0.execute)
smach_thread.start()
def main():
rospy.init_node('hcr_state_node')
sm_hcr = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
"""
Initialize userdata
"""
# target and current positions
pose_init = geometry_msgs.msg.PoseStamped()
pose_init.pose.position.x = 0
pose_init.pose.position.y = 0
pose_init.pose.position.z = 0
pose_init.pose.orientation.x = 0
pose_init.pose.orientation.y = 0
pose_init.pose.orientation.z = 0
pose_init.pose.orientation.w = 1
sm_hcr.userdata.pose_base_cur = geometry_msgs.msg.PoseStamped()
sm_hcr.userdata.pose_base_tar = geometry_msgs.msg.PoseStamped()
sm_hcr.userdata.pose_object_cur = geometry_msgs.msg.PoseStamped()
sm_hcr.userdata.pose_object_tar = geometry_msgs.msg.PoseStamped()
sm_hcr.userdata.pose_base_cur = pose_init
sm_hcr.userdata.pose_base_cur.header.frame_id = "map"
sm_hcr.userdata.pose_base_tar = pose_init
sm_hcr.userdata.pose_base_tar.header.frame_id = "map"
sm_hcr.userdata.pose_object_cur = pose_init
sm_hcr.userdata.pose_object_tar = pose_init
# Flags (flg)
sm_hcr.userdata.flg_move_detect_done = False
sm_hcr.userdata.flg_move_search_done = False
sm_hcr.userdata.flg_object_grasp_done = False
sm_hcr.userdata.flg_object_place_done = False
sm_hcr.userdata.flg_move_new_goal = False
# List of detected frames (type class Frame) for each run
sm_hcr.userdata.list_detected_frames_tmp = []
sm_hcr.userdata.list_detected_frames_1 = []
sm_hcr.userdata.list_detected_frames_2 = []
sm_hcr.userdata.list_detected_frames_3 = []
"""
Initialize subscribers
"""
def _move_base_goal_cb(pose):
sm_hcr.userdata.pose_base_tar = pose
sm_hcr.userdata.flg_move_new_goal = True
rospy.Subscriber('/move_base_simple/goal', geometry_msgs.msg.PoseStamped,
_move_base_goal_cb)
"""
Initialize thread
"""
def thread_tf_target_object_pose():
while 1:
br = tf2_ros.TransformBroadcaster()
t = geometry_msgs.msg.TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = sm_hcr.userdata.pose_object_tar.header.frame_id
t.child_frame_id = "target_object"
t.transform.translation = sm_hcr.userdata.pose_object_tar.pose.position
t.transform.rotation = sm_hcr.userdata.pose_object_tar.pose.orientation
try:
br.sendTransform(t)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo(
"ERROR: thread_tf_target_object_pose: tf-exeption")
with sm_hcr:
### LOGIC ###
smach.StateMachine.add('LOGIC',
logic.Logic(),
transitions={
'aborted': 'aborted',
'reset': 'RESET',
'move_detect': 'MOVE_DETECT',
'move_search': 'MOVE_SEARCH',
'object_grasp': 'OBJECT_GRASP',
'object_place': 'OBJECT_PLACE'
},
remapping={
'flg_reset': 'flg_reset',
'flg_score': 'flg_score',
'flg_move_detect_done':
'flg_move_detect_done',
'flg_move_search_done':
'flg_move_search_done',
'flg_object_grasp_done':
'flg_object_grasp_done',
'flg_object_place_done':
'flg_object_place_done'
})
### RESET ###
smach.StateMachine.add('RESET',
logic.Reset(),
transitions={'succeeded': 'LOGIC'},
remapping={
'flg_move_detect_done':
'flg_move_detect_done',
'flg_move_search_done':
'flg_move_search_done',
'flg_move_new_goal':
'flg_move_new_goal',
'flg_object_grasp_done':
'flg_object_grasp_done',
'flg_object_place_done':
'flg_object_place_done',
'pose_base_tar':
'pose_base_tar',
'list_detected_frames_tmp':
'list_detected_frames_tmp',
'list_detected_frames_1':
'list_detected_frames_1',
'list_detected_frames_2':
'list_detected_frames_2',
'list_detected_frames_3':
'list_detected_frames_3'
})
### SCORE ###
smach.StateMachine.add('SCORE',
logic.Score(),
transitions={
'succeeded': 'LOGIC',
'aborted': 'RESET'
},
remapping={
'list_detected_frames_1':
'list_detected_frames_1',
'list_detected_frames_2':
'list_detected_frames_2',
'list_detected_frames_3':
'list_detected_frames_3'
})
### MOVE_DETECT ###
sm_MoveDetect = smach.StateMachine(
outcomes=['succeeded', 'aborted'],
input_keys=['pose_base_cur', 'list_detected_frames_tmp'],
output_keys=[
'pose_base_cur', 'list_detected_frames_tmp',
'list_detected_frames_1', 'flg_move_detect_done'
])
# sub Detect
smach.StateMachine.add('MOVE_DETECT', sm_MoveDetect, {
'succeeded': 'SCORE',
'aborted': 'RESET'
})
with sm_MoveDetect:
smach.StateMachine.add('DETECT',
measure.Detect(),
transitions={'succeeded': 'STOP'})
smach.StateMachine.add(
'STOP',
move.Stop(),
transitions={'succeeded': 'MEASURE'},
remapping={'pose_base_cur': 'pose_base_cur'})
smach.StateMachine.add(
'MEASURE',
measure.Measure(),
transitions={'succeeded': 'MOVE_DETECT_EXIT'},
remapping={
'pose_base_cur': 'pose_base_cur',
'list_detected_frames_tmp': 'list_detected_frames_tmp'
})
smach.StateMachine.add('MOVE_DETECT_EXIT',
logic.MoveDetectExit(),
transitions={'succeeded': 'succeeded'},
remapping={
'list_detected_frames_tmp':
'list_detected_frames_tmp',
'list_detected_frames_1':
'list_detected_frames_1',
'flg_move_detect_done':
'flg_move_detect_done'
})
### MOVE_SEARCH ###
sm_MoveSearch = smach.StateMachine(
outcomes=['succeeded', 'aborted'],
input_keys=[
'pose_base_tar', 'pose_base_cur', 'pose_object_tar',
'list_detected_frames_tmp'
],
output_keys=[
'pose_base_tar', 'pose_base_cur', 'list_detected_frames_tmp',
'list_detected_frames_2', 'flg_move_search_done'
])
smach.StateMachine.add('MOVE_SEARCH', sm_MoveSearch, {
'succeeded': 'SCORE',
'aborted': 'RESET'
})
with sm_MoveSearch:
smach.StateMachine.add(
'BASEPOSE_FINDER',
logic.BasePoseFinder(),
transitions={'succeeded': 'MOVE'},
# transitions={'succeeded':'MEASURE'},
remapping={
'pose_object_tar': 'pose_object_tar',
'pose_base_tar': 'pose_base_tar'
})
smach.StateMachine.add('MOVE',
move.Move(),
transitions={'succeeded': 'MEASURE'},
remapping={
'pose_base_tar': 'pose_base_tar',
'pose_base_cur': 'pose_base_cur'
})
smach.StateMachine.add(
'MEASURE',
measure.Measure(),
transitions={'succeeded': 'MOVE_SEARCH_EXIT'},
remapping={
'pose_base_cur': 'pose_base_cur',
'list_detected_frames_tmp': 'list_detected_frames_tmp'
})
smach.StateMachine.add('MOVE_SEARCH_EXIT',
logic.MoveSearchExit(),
transitions={'succeeded': 'succeeded'},
remapping={
'list_detected_frames_tmp':
'list_detected_frames_tmp',
'list_detected_frames_2':
'list_detected_frames_2',
'flg_move_search_done':
'flg_move_search_done'
})
### OBJECT_GRASP ###
sm_ObjectGrasp = smach.StateMachine(
outcomes=['succeeded', 'aborted'],
input_keys=['flg_object_grasp_done', 'pose_object_tar'],
output_keys=['flg_object_grasp_done'])
smach.StateMachine.add('OBJECT_GRASP', sm_ObjectGrasp, {
'succeeded': 'LOGIC',
'aborted': 'RESET'
})
with sm_ObjectGrasp:
smach.StateMachine.add(
'PICK',
grasp.Pick(),
transitions={'succeeded': 'OBJECT_GRASP_EXIT'},
remapping={'pose_object_tar': 'pose_object_tar'})
smach.StateMachine.add(
'OBJECT_GRASP_EXIT',
logic.ObjectGraspExit(),
transitions={'succeeded': 'succeeded'},
remapping={'flg_object_grasp_done': 'flg_object_grasp_done'})
### OBJECT_PLACE ###
sm_ObjectPlace = smach.StateMachine(
outcomes=['succeeded', 'aborted'],
input_keys=[
'pose_base_cur', 'pose_base_tar', 'flg_object_place_done'
],
output_keys=['pose_base_cur', 'flg_object_place_done'])
smach.StateMachine.add('OBJECT_PLACE', sm_ObjectPlace, {
'succeeded': 'LOGIC',
'aborted': 'RESET'
})
with sm_ObjectPlace:
smach.StateMachine.add('MOVE',
move.Move(),
transitions={'succeeded': 'PLACE'},
remapping={
'pose_base_tar': 'pose_base_tar',
'pose_base_cur': 'pose_base_cur'
})
smach.StateMachine.add(
'PLACE',
grasp.Place(),
transitions={'succeeded': 'OBJECT_PLACE_EXIT'})
smach.StateMachine.add(
'OBJECT_PLACE_EXIT',
logic.ObjectPlaceExit(),
transitions={'succeeded': 'succeeded'},
remapping={'flg_object_place_done': 'flg_object_place_done'})
# # attach a SMACH introspection server
sis = smach_ros.IntrospectionServer('hcr_state_machine', sm_hcr,
'/HCR_State_Machine')
sis.start()
# set preempt handler
smach_ros.set_preempt_handler(sm_hcr)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
thread_tf_tarBasePose = threading.Thread(
target=thread_tf_target_object_pose)
thread_tf_tarBasePose.start()
thread_smach = threading.Thread(target=sm_hcr.execute)
thread_smach.start()
# signal handler
rospy.spin()
def polygonial():
ids = [4, 5, 6]
#define the differents points
my_points = [Point(), Point(), Point(), Point()]
my_points[0].x = 0.9 - 0.4
my_points[0].y = 0.0 - 0.4
my_points[0].z = 0.9
my_points[1].x = 0.9 - 0.4
my_points[1].y = 0.9 - 0.4
my_points[1].z = 0.9
my_points[2].x = 0.0 - 0.4
my_points[2].y = 0.9 - 0.4
my_points[2].z = 0.9
my_points[3].x = 0.0 - 0.4
my_points[3].y = 0.0 - 0.4
my_points[3].z = 0.9
# Create a SMACH state machine
sm0 = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
#progressively add drones
with sm0:
# StateMachine.add('drone1-1',
# SimpleActionState('drone1detect_perimeter',
# my_newAction, goal = my_newGoal(point = my_points[0], id = ids[0] )),
# transitions={'succeeded' : 'drone1-2'})
# StateMachine.add('drone1-2',
# SimpleActionState('drone1detect_perimeter',
# my_newAction, goal = my_newGoal(point = my_points[1], id = ids[0] )),
# transitions={'succeeded' : 'drone2-1'})
# StateMachine.add('drone2-1',
# SimpleActionState('drone2detect_perimeter',
# my_newAction, goal = my_newGoal(point = my_points[0], id = ids[1] )),
# transitions={'succeeded' : 'drone1-3'})
StateMachine.add('drone1-3',
SimpleActionState('drone1detect_perimeter',
my_newAction,
goal=my_newGoal(point=my_points[2],
id=ids[0])),
transitions={
'succeeded': 'drone2-2',
'aborted': 'land_all'
})
StateMachine.add('drone2-2',
SimpleActionState('drone2detect_perimeter',
my_newAction,
goal=my_newGoal(point=my_points[1],
id=ids[1])),
transitions={
'succeeded': 'drone3-1',
'aborted': 'land_all'
})
StateMachine.add('drone3-1',
SimpleActionState('drone3detect_perimeter',
my_newAction,
goal=my_newGoal(point=my_points[0],
id=ids[2])),
transitions={
'succeeded': 'infinit_loop',
'aborted': 'land_all'
})
land_sm = Concurrence(['succeeded', 'aborted', 'preempted'],
'succeeded')
StateMachine.add('land_all', land_sm)
with land_sm:
# DUPLICATA FOR HIGH LEVEL
# #Land Drone If Aborted
Concurrence.add(
'LAND_DRONE1',
SimpleActionState('land_drone1',
my_newAction,
goal=my_newGoal(point=my_points[3],
id=ids[0])))
# #Land Drone If Aborted
Concurrence.add(
'LAND_DRONE2',
SimpleActionState('land_drone2',
my_newAction,
goal=my_newGoal(point=my_points[3],
id=ids[1])))
# #Land Drone If Aborted
Concurrence.add(
'LAND_DRONE3',
SimpleActionState('land_drone3',
my_newAction,
goal=my_newGoal(point=my_points[3],
id=ids[2])))
############################################
sm1 = Concurrence(
['succeeded', 'aborted', 'preempted'],
'succeeded',
#child_termination_cb = lambda so: True,
#outcome_map = {
#'succeeded':{'WAIT_FOR_CLEAR':'valid'},
#'aborted':{'DRONE1':'aborted'}}
)
StateMachine.add('infinit_loop', sm1)
with sm1:
drone1 = StateMachine(
outcomes=['succeeded', 'aborted',
'preempted']) # ['succeeded','aborted','preempted']
Concurrence.add('DRONE1', drone1)
# Open the container
with drone1:
#add each state
order = (3, 0, 1, 2)
for i in range(3):
StateMachine.add(
'DRONE1-' + str(order[i]),
SimpleActionState('drone1detect_perimeter',
my_newAction,
goal=my_newGoal(
point=my_points[order[i]],
id=ids[0])),
transitions={
'succeeded': 'DRONE1-' + str(order[i + 1]),
'aborted': 'LAND_DRONE1'
})
#make it infinit
smach.StateMachine.add(
'DRONE1-' + str(2),
SimpleActionState(
'drone1detect_perimeter',
my_newAction,
goal=my_newGoal(point=my_points[order[3]], id=ids[0])),
transitions={
'succeeded': 'DRONE1-' + str(order[0]),
'aborted': 'LAND_DRONE1'
})
# #Land Drone If Aborted
smach.StateMachine.add(
'LAND_DRONE1',
SimpleActionState('land_drone1',
my_newAction,
goal=my_newGoal(point=my_points[3],
id=ids[0])),
transitions={'succeeded': 'LAND_DRONE1'})
drone2 = StateMachine(
outcomes=['succeeded', 'aborted',
'preempted']) # ['succeeded','aborted','preempted']
Concurrence.add('DRONE2', drone2)
# Open the container
with drone2:
#add each state
order = (2, 3, 0, 1)
for i in range(3):
StateMachine.add(
'DRONE2-' + str(order[i]),
SimpleActionState('drone2detect_perimeter',
my_newAction,
goal=my_newGoal(
point=my_points[order[i]],
id=ids[1])),
transitions={
'succeeded': 'DRONE2-' + str(order[i + 1]),
'aborted': 'LAND_DRONE2'
})
#make it infinit
smach.StateMachine.add(
'DRONE2-' + str(1),
SimpleActionState(
'drone2detect_perimeter',
my_newAction,
goal=my_newGoal(point=my_points[order[3]], id=ids[1])),
transitions={
'succeeded': 'DRONE2-' + str(order[0]),
'aborted': 'LAND_DRONE2'
})
# #Land Drone If Aborted
smach.StateMachine.add(
'LAND_DRONE2',
SimpleActionState('land_drone2',
my_newAction,
goal=my_newGoal(point=my_points[3],
id=ids[1])),
transitions={'succeeded': 'LAND_DRONE2'})
drone3 = StateMachine(
outcomes=['succeeded', 'aborted',
'preempted']) # ['succeeded','aborted','preempted']
Concurrence.add('DRONE3-', drone3)
# Open the container
with drone3:
#add each state
order = (1, 2, 3, 0)
for i in range(3):
StateMachine.add(
'DRONE3-' + str(order[i]),
SimpleActionState('drone3detect_perimeter',
my_newAction,
goal=my_newGoal(
point=my_points[order[i]],
id=ids[2])),
transitions={
'succeeded': 'DRONE3-' + str(order[i + 1]),
'aborted': 'LAND_DRONE3'
})
#make it infinit
smach.StateMachine.add(
'DRONE3-' + str(0),
SimpleActionState(
'drone3detect_perimeter',
my_newAction,
goal=my_newGoal(point=my_points[order[3]], id=ids[2])),
transitions={
'succeeded': 'DRONE3-' + str(order[0]),
'aborted': 'LAND_DRONE3'
})
# #Land Drone If Aborted
smach.StateMachine.add(
'LAND_DRONE3',
SimpleActionState('land_drone3',
my_newAction,
goal=my_newGoal(point=my_points[3],
id=ids[2])),
transitions={'succeeded': 'LAND_DRONE3'})
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Set preempt handler
smach_ros.set_preempt_handler(sm0)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target=sm0.execute)
smach_thread.start()
def main():
"""
This is a State Machine for a Robot Dog name wheely.
The main function initializes the ros node, subscribes to the topic: robot's odometry topic ("/odom"), laser scan data (/scan), and the robot's camera image topic ("camera1/image_raw/compressed"), and creates a publisher for the robot's velocity commands on the topic ("/cmd_vel"), a publisher for cancelling the goal prvided to the move_base server ("/move_base/cancel").
This function also intializes the "detectBallFlag", "expLiteFlag", "playflag", "nearhuman", "room", "ballToBeFound" parameters in the ROS Parameter Server.
This function also defines the state machines and adds four states/behaviors to the container.
"""
global pub_cmd_vel, pub_cancel
rospy.set_param("expLiteFlag", 0)
rospy.set_param("playflag", 0)
rospy.set_param("nearHuman", 0)
rospy.set_param("room", "")
rospy.set_param("ballToBeFound", "")
sub_camera = rospy.Subscriber("/camera1/image_raw/compressed",
CompressedImage,
imageCallback,
queue_size=1)
sub_odom = rospy.Subscriber("/odom", Odometry, robotPos)
sub_laser = rospy.Subscriber('/scan', LaserScan, clbk_laser)
pub_cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
pub_cancel = rospy.Publisher("/move_base/cancel", GoalID, queue_size=1)
rospy.init_node('Assignment_3_State_Machine')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['container_interface'])
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('NORMAL',
Normal(),
transitions={
'after finishing normal behavior': 'SLEEP',
'speech command:play': 'PLAY'
})
smach.StateMachine.add(
'SLEEP',
Sleep(),
transitions={'after finishing sleep time': 'NORMAL'})
smach.StateMachine.add('PLAY',
Play(),
transitions={
'after finishing play time': 'NORMAL',
'ball not already detected': 'FIND'
})
smach.StateMachine.add('FIND',
Find(),
transitions={
'required ball found': 'PLAY',
'required ball not found': 'PLAY'
})
# Create and start the introspection server for visualization. We
# can visualize this is smach viewer
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# Execute the state machine
smach_ros.set_preempt_handler(sm)
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def polygonial():
# Construct static goals
polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0)
polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5)
# Create a SMACH state machine
sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Open the container
with sm0:
# Reset turtlesim
StateMachine.add('RESET',
ServiceState('reset', std_srvs.srv.Empty),
{'succeeded':'SPAWN'})
# Create a second turtle
StateMachine.add('SPAWN',
ServiceState('spawn', turtlesim.srv.Spawn,
request = turtlesim.srv.SpawnRequest(5.0,2.0,0.0,'turtle2')),
{'succeeded':'PRECISE'})
# fib_goal = FibonacciGoal(order=20)
# fib_fullaction = SimpleActionState('fibonacci', FibonacciAction,
# goal=fib_goal)
#
# StateMachine.add('FIBONACCI',
# fib_fullaction,
# transitions={'succeeded':'PRECISE'})
goto_goal = FibonacciGoal(order=20)
goto_fullaction = SimpleActionState('detect_perimeter', FibonacciAction,
goal=goto_goal)
StateMachine.add('PRECISE',
goto_fullaction,
transitions={'succeeded':'TELEPORT1'})
# Teleport turtle 1
StateMachine.add('TELEPORT1',
ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)),
{'succeeded':'DRAW_SHAPES'})
print("hi")
# Draw some polygons
shapes_cc = Concurrence(
outcomes=['succeeded','aborted','preempted'],
default_outcome='aborted',
outcome_map = {'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}})
StateMachine.add('DRAW_SHAPES',shapes_cc)
with shapes_cc:
# Draw a large polygon with the first turtle
Concurrence.add('BIG',
SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
goal = polygon_big))
# Draw a small polygon with the second turtle
small_shape_sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
Concurrence.add('SMALL',small_shape_sm)
with small_shape_sm:
# Teleport turtle 2
StateMachine.add('TELEPORT2',
ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
{'succeeded':'DRAW_WITH_MONITOR'})
# Construct a concurrence for the shape action and the monitor
draw_monitor_cc = Concurrence(
['succeeded','aborted','preempted','interrupted'],
'aborted',
child_termination_cb = lambda so: True,
outcome_map = {
'succeeded':{'DRAW':'succeeded'},
'preempted':{'DRAW':'preempted','MONITOR':'preempted'},
'interrupted':{'MONITOR':'invalid'}})
StateMachine.add('DRAW_WITH_MONITOR',
draw_monitor_cc,
{'interrupted':'WAIT_FOR_CLEAR'})
with draw_monitor_cc:
Concurrence.add('DRAW',
SimpleActionState('togoal', FibonacciAction,
goal=goto_goal))
def turtle_far_away(ud, msg):
"""Returns True while turtle pose in msg is at least 1 unit away from (2,5)"""
if sqrt(pow(msg.x-9.0,2) + pow(msg.y-5.0,2)) > 2.0:
return True
return False
Concurrence.add('MONITOR',
MonitorState('/turtle1/pose',turtlesim.msg.Pose,
cond_cb = turtle_far_away))
StateMachine.add('WAIT_FOR_CLEAR',
MonitorState('/turtle1/pose',turtlesim.msg.Pose,
cond_cb = lambda ud,msg: not turtle_far_away(ud,msg)),
{'valid':'WAIT_FOR_CLEAR','invalid':'TELEPORT2'})
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Set preempt handler
smach_ros.set_preempt_handler(sm0)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target = sm0.execute)
smach_thread.start()
Concurrence.add('INSPECTION', INSPECTION())
StateMachine.add('INSPECTION_ACTION',
sm_inspection,
transitions={
'success': 'CHOOSE_ACTION',
'fail': 'stop'
})
# Attach a SMACH introspection server
sis = IntrospectionServer('baxter_SMACH_introspection', sm,
'/BAXTER_DEMO')
sis.start()
# Set preempt handler
smach_ros.set_preempt_handler(sm)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target=sm.execute)
smach_thread.start()
# Signal handler (wait for CTRL+C)
rospy.spin()
rospy.on_shutdown(clean_shutdown)
except rospy.ROSInterruptException:
# Baxter screen output
image_pub.publish(msg_sleeping)
