def main():
#Create a rospy node for the state machine
rospy.init_node('IRA_statemachine')
#Creating a state machine for banking scenario
sm_bank = StateMachine(outcomes=['Stop'])
#Create Smach containers for each state
with sm_bank:
StateMachine.add('Face Recognition',
frn.Face_recognition(),
transitions={'Detected':'Speech_Gesture','Not_Detected':'Face Recognition'},remapping={'Face_recognition_out':'speech_in'})
sm_con = smach.Concurrence(outcomes=['Waiting','Done'],default_outcome='Waiting',outcome_map={'Done':{'Speech1':'Completed','Gesture1':'Completed'}})
with sm_con:
smach.Concurrence.add('Speech1',ss1.speech())
smach.Concurrence.add('Gesture1',g1.gesture())
smach.StateMachine.add('Speech_Gesture', sm_con,
transitions={'Waiting':'Stop',
'Done':'Face Recognition'})
StateMachine.add('Hotword_detection',
hwdc.Hotword_detection(),
transitions={'Detected':'Speech_Recognition','Not_Detected':'Face Recognition'})
StateMachine.add('Speech_Recognition',
src.Speech_recognition(),
transitions={'Completed':'Speech_database','Not_Completed':'Face Recognition'},remapping={'speech_recognition_out':'Database_in'})
StateMachine.add('Speech_database',
sdc.Speech_database(),
transitions={'Completed':'CON','Not_Completed':'Face Recognition'},remapping={'speech_database_out':'Text_in'})
sm_con = smach.Concurrence(outcomes=['Waiting','Done'],default_outcome='Waiting',outcome_map={'Done':{'Text_speech':'Completed'}},input_keys=['Text_in'])
with sm_con:
smach.Concurrence.add('Text_speech',ts.Text_speech())
#smach.Concurrence.add('ui1',ui1.Text_reply())
StateMachine.add('CON',
sm_con,
transitions={'Waiting':'Stop','Done':'Hotword_detection'})
#Code for initiating the smach_viewer
Monitor = smach_ros.IntrospectionServer('IRA', sm_bank, '/IRA_FLOW')
Monitor.start()
#Execute the state machine
outcome = sm_bank.execute()
#Stop the smach_viewer if state machine stops
Monitor.stop
def pass_first():
pass_ball_first = smach.StateMachine(outcomes=['successed', 'failed'])
rospy.logwarn("pass_ball_first STARTING!!!")
with pass_ball_first:
start = smach.Concurrence(outcomes=['successed','failed'],
default_outcome='successed',
outcome_map={'successed':{'SHOVEL_DOWN':'successed',
'MOVE_POINT_TO_SHOOT':'successed'}})
with start:
smach.Concurrence.add('SHOVEL_DOWN',Shovel_Down())
smach.Concurrence.add('MOVE_POINT_TO_SHOOT',Move_Point_To_Shoot())
smach.StateMachine.add('Start',start,transitions={'successed':'SHOOT1',
'failed':'failed' })
smach.StateMachine.add('SHOOT1',Shoot(),
transitions={'successed':'Shovel_Control_Up1',
'failed':'failed'})
smach.StateMachine.add('Shovel_Control_Up1',Shovel_UP(),
transitions={'successed':'ADJUST1',
'failed':'failed'})
smach.StateMachine.add('ADJUST1',Move_To_Find_Ball(),
transitions={'successed':'FindBall',
'failed':'failed'})
smach.StateMachine.add('FindBall', Search_Ball(),
transitions={'successed': 'ADJUST2',
'failed': 'failed'})
smach.StateMachine.add('ADJUST2',Shoot_Adjust(),
transitions={'successed':'SHOOT2',
'failed':'failed'})
smach.StateMachine.add('SHOOT2',Shoot(),
transitions={'successed':'Shovel_Control_Up2',
'failed':'failed'})
smach.StateMachine.add('Shovel_Control_Up2',Shovel_UP(),
transitions={'successed':'My Life For Auir',
'failed':'failed'})
Auir = smach.Concurrence(outcomes=['successed','failed'],
default_outcome='failed',
outcome_map={'successed':{'RETURN':'successed',
'Return_Adjust':'successed'}})
with Auir:
smach.Concurrence.add('RETURN',Return())
smach.Concurrence.add('Return_Adjust',Return_Adjust())
smach.StateMachine.add('My Life For Auir',Auir,transitions={'successed':'successed',
'failed':'failed'})
pass_ball_first.execute()
rospy.logerr('Pass_Ball_First has finished!!!!')
def main():
rospy.init_node('arm_test_sm')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'failed'])
# Open the container
with sm:
# Add states to the container
# smach.StateMachine.add('INIT', FakeState(),
# transitions={'succeeded':'STOW_ARM',
# 'aborted':'failed',
# 'preempted':'failed'})
# smach.StateMachine.add('STOW_ARM', ArmStowState(),
# transitions={'succeeded':'BEFORE_DETECT',
# 'aborted':'failed',
# 'preempted':'failed'})
smach.StateMachine.add('BEFORE_DETECT',
FakeState(),
transitions={
'succeeded': 'WAIT_DETECT',
'aborted': 'failed',
'preempted': 'failed'
})
wait_detect_concurrence = smach.Concurrence(
outcomes=['succeeded', 'failed'],
default_outcome='failed',
output_keys=['detection_msg'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with wait_detect_concurrence:
smach.Concurrence.add('WAIT_FOR_DETECTION', create_detect_state())
#smach.Concurrence.add('WAIT_DETECT_FAKE', FakeState())
smach.StateMachine.add('WAIT_DETECT',
wait_detect_concurrence,
transitions={
'succeeded': 'DETECTION_PICKUP',
'failed': 'failed'
})
smach.StateMachine.add('DETECTION_PICKUP',
DetectionPickupState(
rospy.Duration.from_sec(40.0)),
transitions={
'succeeded': 'succeeded',
'aborted': 'failed',
'preempted': 'failed'
})
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
#rospy.spin()
sis.stop()
def main():
rospy.init_node('smach_example_state_machine')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('FOO',
Foo(),
transitions={
'succeeded': 'SUB',
'aborted': 'aborted',
'preempted': 'preempted'
})
sm_con = smach.Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='aborted',
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm_con:
smach.Concurrence.add('BAR', Bar())
smach.Concurrence.add('BAS', Bas())
smach.StateMachine.add('SUB', sm_con)
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
rospy.spin()
sis.stop()
def execute(self, ud):
rospy.loginfo("Starting next task....")
cm_movingToPoint = smach.Concurrence(
outcomes=['marker_detected', 'marker_undetected', 'time_out'],
default_outcome='marker_undetected',
outcome_map={
'marker_detected': {
'MARKER_DETECT': 'succeeded'
},
'time_out': {
'CONTROLLER': 'succeeded'
}
})
moveGoal = advancedControllerGoal()
moveGoal.x = ud.x2
moveGoal.y = ud.y2
moveGoal.depth = ud.depth
markGoal = markerGoal()
markGoal.order = header.DETECT_MARKER
with cm_movingToPoint:
smach.Concurrence.add(
'CONTROLLER',
SimpleActionState("advancedController",
advancedControllerAction,
goal=moveGoal))
smach.Concurrence.add(
'MARKER_DETECT',
SimpleActionState("marker_detect", goal=markGoal))
return 'done'
def main():
rospy.init_node('s_m', anonymous=True)
base = smach.Concurrence(outcomes=[
'looking1', 'looking2', 'looking3', 'looking4', 'moving_yes',
'moving_no', 'base_reset'
],
default_outcome='base_reset',
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with base:
smach.Concurrence.add('WAITING', Waiting())
smach.Concurrence.add(
'SENSOR1', smach_ros.MonitorState("chat", String, monitor_cb1))
smach.Concurrence.add(
'SENSOR2', smach_ros.MonitorState("chat", String, monitor_cb2))
smach.Concurrence.add(
'SENSOR3', smach_ros.MonitorState("chat", String, monitor_cb3))
smach.Concurrence.add(
'SENSOR4', smach_ros.MonitorState("chat", String, monitor_cb4))
smach.Concurrence.add(
'SENSOR_YES', smach_ros.MonitorState("chat", String,
monitor_cb_yes))
smach.Concurrence.add(
'SENSOR_NO', smach_ros.MonitorState("chat", String, monitor_cb_no))
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['done'])
# Open the container
with sm:
smach.StateMachine.add('SETUP',
Setup(),
transitions={'setup_done': 'BASE'})
smach.StateMachine.add('BASE',
base,
transitions={
'looking1': 'GOAL_1',
'looking2': 'GOAL_2',
'looking3': 'GOAL_3',
'looking4': 'GOAL_4',
'moving_yes': 'GOAL_YES',
'moving_no': 'GOAL_NO',
'base_reset': 'BASE'
})
smach.StateMachine.add('GOAL_1', gaze1(), {'succeeded': 'BASE'})
smach.StateMachine.add('GOAL_2', gaze2(), {'succeeded': 'BASE'})
smach.StateMachine.add('GOAL_3', gaze3(), {'succeeded': 'BASE'})
smach.StateMachine.add('GOAL_4', gaze4(), {'succeeded': 'BASE'})
smach.StateMachine.add('GOAL_YES', yes(), {'succeeded': 'BASE'})
smach.StateMachine.add('GOAL_NO', no(), {'succeeded': 'BASE'})
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
def __allocMoveConcurrency__(self, slave_namespaces):
map_abort = dict()
for name in slave_namespaces:
map_abort[name + "/Move"] = 'aborted'
map_abort[name + "/Move"] = 'preempted'
map_succed = dict()
for name in slave_namespaces:
map_succed[name + "/Move"] = 'succeeded'
sm_con = smach.Concurrence(outcomes=['success', 'error_occured'],
input_keys=['slaves'],
output_keys=['last_name'],
default_outcome='error_occured',
outcome_map={
'success': map_succed,
'error_occured': map_abort
})
with sm_con:
for name in slave_namespaces:
smach.Concurrence.add(
name + "/Move",
smach_ros.SimpleActionState(name + '/move_base',
MoveBaseAction,
goal_cb=partial(
self.__calcGoal__, name),
input_keys=['slaves'],
output_keys=['last_name']))
return sm_con
def __init__(self, in_ttl=20.0):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'time_out'],
input_keys=['object_to_grasp', 'time_out_grasp'],
output_keys=['standard_error'])
with self:
self.userdata.time_grasp = 0.0
self.userdata.time_first = True
sm_conc = smach.Concurrence(outcomes=['succeeded', 'time_out'],
default_outcome='succeeded',
input_keys=[
'object_to_grasp', 'time_grasp',
'time_first', 'time_out_grasp'
],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm_conc:
#Needs to be replaced by the actual Find_and_grasp_object SM
sm_conc.add(
'Find_and_grasp_object',
#Find_and_grab_object(),
DummyStateMachine())
sm_conc.add('Time_State', Time_State(ttl=in_ttl))
smach.StateMachine.add('GRASP_CONCURRENCE',
sm_conc,
transitions={
'succeeded': 'succeeded',
'time_out': 'time_out'
})
def make_return_path(shared):
search_sm = smach.Concurrence(
['succeeded', 'failed', 'preempted'],
default_outcome='failed',
outcome_map={'succeeded': {
'WAIT': 'succeeded'
}},
child_termination_cb=lambda so: True)
with search_sm:
smach.Concurrence.add(
'PATTERN',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.left(2), lambda cur: cur.forward(2),
lambda cur: cur.right(4), lambda cur: cur.backward(2)
],
speed=.3))
smach.Concurrence.add(
'WAIT',
legacy_vision_states.WaitForObjectsState(shared,
'find2_down_camera',
'pipe'))
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('RETURN_WAYPOINT',
common_states.ReturnToWaypointState(
shared, 'last_path'),
transitions={'succeeded': 'SEARCH'})
smach.StateMachine.add('SEARCH', search_sm)
return sm
def GetMoveRobotSateMachine():
sm_move = smach.Concurrence(
outcomes=['succeeded', 'aborted'],
default_outcome='aborted',
outcome_map={
'succeeded': {
'MOVE_BASE': 'succeeded',
'MOVE_PTU': 'succeeded'
}
},
input_keys=['goal_robot_pose', 'goal_ptu_position'])
with sm_move:
smach.Concurrence.add(
'MOVE_PTU',
MovePTU(),
remapping={'goal_ptu_position': 'goal_ptu_position'})
if rospy.get_param("/scene_exploration_sm/UseFakeMoveBase") is True:
smach.Concurrence.add(
'MOVE_BASE',
FakeMoveBase(),
remapping={'goal_robot_pose': 'goal_robot_pose'})
else:
smach.Concurrence.add(
'MOVE_BASE',
MoveBase(),
remapping={'goal_robot_pose': 'goal_robot_pose'})
return sm_move
def main():
rospy.init_node('smach_example_state_machine')
sm_top = smach.StateMachine(outcomes=['outcome6'])
with sm_top:
smach.StateMachine.add('BAS', Bas(), transitions={'outcome3': 'CON'})
sm_con = smach.Concurrence(
outcomes=['outcome4', 'outcome5'],
default_outcome='outcome4',
outcome_map={'outcome5': {
'BAR': 'outcome1',
'FOO': 'outcome2'
}})
with sm_con:
smach.Concurrence.add('FOO', Foo())
smach.Concurrence.add('BAR', Bar())
smach.StateMachine.add('CON',
sm_con,
transitions={
'outcome4': 'CON',
'outcome5': 'outcome6'
})
outcome = sm_top.execute()
def wrap_state_force_detection(state_name,
state,
force_thresh,
outcomes,
input_keys=[]):
collision_outcomes = ['collision']
total_outcomes = outcomes + collision_outcomes
def child_term_cb(outcome_map):
# stop execution under all circumstances
return True
def out_cb(outcome_map):
if outcome_map['FORCE_MONITOR_' + state_name] == 'collision':
return 'collision'
return outcome_map[state_name]
sm_coll_detect_state = smach.Concurrence(
outcomes=total_outcomes,
input_keys=input_keys,
default_outcome='aborted',
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm_coll_detect_state:
smach.Concurrence.add('FORCE_MONITOR_' + state_name,
ForceCollisionMonitor(force_thresh))
smach.Concurrence.add(state_name, state)
return sm_coll_detect_state
def main():
def subscriber_cb(data_raw, smdata):
rospy.loginfo('SUBSCRIBE VECTOR3 %s' % data_raw)
smdata.x = data_raw.x
smdata.y = data_raw.y
smdata.z = data_raw.z
# smdata.vector3 = data_raw
rospy.init_node('smach_example_state_machine')
sm_top = smach.StateMachine(outcomes=['out1'])
with sm_top:
sm = smach.Concurrence(outcomes=['succeed', 'failed'],
# input_keys=['x'],
# output_keys=['x', 'y', 'z'],
default_outcome='failed',
outcome_map={'succeed': {'LISTENER1': 'right', 'LISTENER2': 'right'}})
with sm:
# Add states to the container
smach.Concurrence.add('LISTENER1', Listen1(),)
# remapping={'x': 'x'})
smach.Concurrence.add('LISTENER2', Listen2(),)
# remapping={'x': 'x'})
smach.StateMachine.add('SM', sm, transitions={'succeed': 'RESULT', 'failed': 'SM'})
smach.StateMachine.add('RESULT', Result(), transitions={'result': 'SM'})
# sis = smach_ros.IntrospectionServer('introspection_server', sm_mother, 'INTROSPECTION SERVER')
# sis.start()
# Execute SMACH plan
outcome = sm_top.execute()
rospy.spin()
def get_nav_approach(self):
def child_term_cb(outcome_map):
return True
def out_cb(outcome_map):
if outcome_map['DETECT_FORWARD_DISTANCE'] == 'reached':
return 'succeeded'
return 'shutdown'
sm_nav_approach_state = smach.Concurrence(
outcomes=['succeeded', 'shutdown'],
default_outcome='shutdown',
child_termination_cb=child_term_cb,
outcome_cb=out_cb,
output_keys=['nav_dist'])
with sm_nav_approach_state:
approach_goal = ApproachGoal()
approach_goal.forward_vel = 0.05
approach_goal.forward_mult = 0.50
smach.Concurrence.add(
'MOVE_FORWARD',
SimpleActionState('/approach_table/move_forward_act',
ApproachAction,
goal=approach_goal))
smach.Concurrence.add(
'DETECT_FORWARD_DISTANCE',
DetectForwardDistance(self.get_transform,
self.nav_approach_dist))
return sm_nav_approach_state
def main():
sm = smach.StateMachine(outcomes=['final_outcome_a', 'final_outcome_b', 'final_outcome_c'])
with sm:
sm_con = smach.Concurrence(outcomes=['con_outcome_1', 'con_outcome_2', 'con_default_outcome'],
default_outcome='con_default_outcome',
outcome_map={'con_outcome_1': { 'FOO_0': 'outcome_b', 'FOO_1': 'outcome_b'},
'con_outcome_2': { 'FOO_0': 'outcome_a', 'FOO_1': 'outcome_a'}})
with sm_con:
smach.Concurrence.add('FOO_0', Foo('FOO_0', 'outcome_a'))
smach.Concurrence.add('FOO_1', Foo('FOO_1', 'outcome_a'))
smach.StateMachine.add('CON', sm_con,
transitions={'con_default_outcome':'CON',
'con_outcome_1':'final_outcome_b',
'con_outcome_2':'FOO_2'})
smach.StateMachine.add('FOO_2', Foo('FOO_2', 'outcome_a'),
transitions={'outcome_a':'final_outcome_a',
'outcome_b':'final_outcome_c'})
outcome = sm.execute()
def main():
rospy.init_node('smach_example_state_machine')
# Create the top level SMACH state machine
sm_top = smach.StateMachine(outcomes=['outcome6'])
# Open the container
with sm_top:
smach.StateMachine.add('BAS', Bas(), transitions={'outcome3': 'CON'})
# Create the sub SMACH state machine
sm_con = smach.Concurrence(
outcomes=['outcome4', 'outcome5'],
default_outcome='outcome4',
outcome_map={'outcome5': {
'FOO': 'outcome2',
'BAR': 'outcome1'
}})
# Open the container
with sm_con:
# Add states to the container
smach.Concurrence.add('FOO', Foo())
smach.Concurrence.add('BAR', Bar())
smach.StateMachine.add('CON',
sm_con,
transitions={
'outcome4': 'CON',
'outcome5': 'outcome6'
})
# Execute SMACH plan
outcome = sm_top.execute()
def build(self):
# Build the autonomous state as concurrence between wiimote and measuring behaviour to allow user preemption
autonomous = smach.Concurrence(outcomes=['exitAutomode'],
default_outcome='exitAutomode',
outcome_map={
'exitAutomode': {
'HMI': 'preempted',
'MEASURE': 'preempted'
}
},
child_termination_cb=onPreempt)
with autonomous:
smach.Concurrence.add('HMI', wii_states.interfaceState(self.hmi))
smach.Concurrence.add('MEASURE', measure_point.build())
# Build the top level mission control from the remote control state and the autonomous state
mission_control = smach.StateMachine(outcomes=['preempted'])
with mission_control:
smach.StateMachine.add('REMOTE_CONTROL',
wii_states.remoteControlState(self.hmi),
transitions={
'enterAutomode': 'AUTO_MODE',
'preempted': 'preempted'
})
smach.StateMachine.add(
'AUTO_MODE',
autonomous,
transitions={'exitAutomode': 'REMOTE_CONTROL'})
return mission_control
def main():
rospy.init_node('smach_example_state_machine')
# Create the top level SMACH state machine
sm_top = smach.StateMachine(outcomes=['outcome6'])
# Open the container
with sm_top:
smach.StateMachine.add('BAS', Bas(),
transitions={'outcome3': 'CON'})
# Create the sub SMACH state machine
# Once the outcome meet the outcome_map, sm_con publish 'outcome5' and leave the con state machine.
sm_con = smach.Concurrence(outcomes=['outcome4', 'outcome5'],
default_outcome='outcome4',
outcome_map={'outcome5':
{'FOO': 'outcome2',
'BAR': 'outcome1'}})
# Open the container
with sm_con:
# Add states to the container
smach.Concurrence.add('FOO', Foo())
smach.Concurrence.add('BAR', Bar())
smach.StateMachine.add('CON', sm_con,
transitions={'outcome4': 'CON',
'outcome5': 'outcome6'})
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('server_name', sm_top, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm_top.execute()
sis.stop()
def main():
rospy.init_node("smach_example_state_machine")
# Create the top level SMACH state machine
sm_top = smach.StateMachine(outcomes=["outcome6"])
# Open the container
with sm_top:
smach.StateMachine.add("BAS", Bas(), transitions={"outcome3": "CON"})
# Create the sub SMACH state machine
sm_con = smach.Concurrence(
outcomes=["outcome4", "outcome5"],
default_outcome="outcome4",
outcome_map={"outcome5": {"FOO": "outcome2", "BAR": "outcome1"}},
)
# Open the container
with sm_con:
# Add states to the container
smach.Concurrence.add("FOO", Foo())
smach.Concurrence.add("BAR", Bar())
smach.StateMachine.add(
"CON", sm_con, transitions={"outcome4": "CON", "outcome5": "outcome6"}
)
# Execute SMACH plan
outcome = sm_top.execute()
def main():
#初始化节点
rospy.init_node('smach_example_state_machine_con')
# Create a SMACH state machine
# 使用StateMachine创建一个状态机
# 最外层状态机 最终输出的结果 'outcome6'
sm_top = smach.StateMachine(outcomes=['outcome6'])
# 打开容器
with sm_top:
# 添加一个状态
smach.StateMachine.add(
'BAS',
Bas(), #状态
transitions={'outcome3': 'CON_SM'}) #跳转到 CON_SM子状态机
# 添加一个 子 状态机 sm_con
# 创建 可并行的子状态机 同步状态机
sm_con = smach.Concurrence(
outcomes=['outcome4', 'outcome5'], #子状态机两个输出
default_outcome='outcome4', #默认输出'outcome4' 循环执行该状态机
# 设置了状态机同步运行的状态跳转
outcome_map={
'outcome5': # 另一个'outcome5'
{
'FOO': 'outcome2', # 当'FOO'输出'outcome2'且'BAR'输出'outcome1'
'BAR': 'outcome1'
}
}) # 时 才输出 'outcome5' 跳转到 最外层状态机
with sm_con: #使用同步状态机 向子状态机 容器内 添加状态
smach.Concurrence.add('FOO', Foo())
smach.Concurrence.add('BAR', Bar())
# 添加一个并行的子状态机
smach.StateMachine.add(
'CON_SM',
sm_con,
transitions={
'outcome4': 'CON_SM', # 子状态机的一个输出 默认转到自己 循环
'outcome5': 'outcome6'
})
# Create and start the introspection server
# 可视化状态机服务器 可是使用 rosrun smach_viewer smach_viewer.py 查看
# 第一个参数是服务器的名字，可以根据需要自由给定；
# 第二个参数是所要监测的状态机；
# 第三个参数代表状态机的层级，因为SMACH状态机支持嵌套，状态内部还可以有自己的状态机。
sis = smach_ros.IntrospectionServer('my_smach_introspection_server',
sm_top, '/SM_ROOT')
sis.start()
# Execute SMACH plan
# 执行状态机
outcome = sm_top.execute()
# Wait for ctrl-c to stop the application
# 等待状态机结束
rospy.spin()
sis.stop() #停止 可视化状态机服务器
def createConcurrence(self,fg_state,outcome_cb=None,child_termination_cb=lambda x: True):
# Create the sub SMACH state machine
if not outcome_cb:
outcome_cb = self.concurrent_outcome_cb(self,fg_state)
return smach.Concurrence(outcomes=['TASK_COMPLETED','TASK_INITIALISATION_FAILED','TASK_INTERRUPTED',
'TASK_FAILED','TASK_TIMEOUT','MISSION_COMPLETED'], default_outcome='TASK_FAILED',
outcome_cb = outcome_cb,
child_termination_cb=child_termination_cb)
def main():
rospy.init_node('smach_example_state_machine')
# Create the top level SMACH state machine
sm_top = smach.StateMachine(outcomes=['outcome6'])
# Open the container
with sm_top:
smach.StateMachine.add('INIT',
smach_ros.MonitorState("/sm_reset", Empty,
monitor_cb),
transitions={
'invalid': 'CON',
'valid': 'INIT',
'preempted': 'INIT'
})
# Create the sub SMACH state machine
sm_con = smach.Concurrence(
outcomes=['outcome4', 'outcome5'],
default_outcome='outcome4',
outcome_map={'outcome5': {
'FOO': 'outcome2',
'BAR': 'outcome1'
}})
# Open the container
with sm_con:
# Add states to the container
smach.Concurrence.add('FOO',
smach_ros.MonitorState(
"/sm_reset2", Empty, monitor_cb),
transitions={
'invalid': 'outcome2',
'valid': 'FOO',
'preempted': 'FOO'
})
smach.Concurrence.add('BAR', Bar())
smach.StateMachine.add('CON',
sm_con,
transitions={
'outcome4': 'CON',
'outcome5': 'outcome6'
})
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('my_smach_introspection_server',
sm_top, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm_top.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def __init__(self):
#rospy.init_node('state_machine')
# Create a SMACH state machine
self.sm_top = smach.StateMachine(outcomes=['succeeded', 'error'])
# get test config
package_name = rospy.get_param("/atf/package_name")
print("package_name:", package_name)
test_generation_config_file = rospy.get_param("/atf/test_generation_config_file", "atf/test_generation_config.yaml")
print("test_generation_config_file:", test_generation_config_file)
test_name = rospy.get_param("/atf/test_name")
print("test_name:", test_name)
atf_configuration_parser = ATFConfigurationParser(package_name, test_generation_config_file)
tests = atf_configuration_parser.get_tests()
for test in tests:
#print "test.name:", test.name
if test_name == test.name:
break
#print "current test:", test.name
outcome_map_succeeded = {}
outcome_map_error = {}
for testblock in list(test.testblockset_config.keys()):
outcome_map_succeeded[testblock] = 'succeeded'
outcome_map_error[testblock] = 'error'
outcome_map = {'succeeded':outcome_map_succeeded,
'error':outcome_map_error}
recorder_handle = ATFRecorder(test)
# Open the container
with self.sm_top:
# Create the sub SMACH state machine
sm_con = smach.Concurrence(outcomes=['succeeded','error'],
default_outcome='error',
outcome_map=outcome_map)
sm_con.userdata = self.sm_top.userdata
# Open the container
with sm_con:
# Add states to the container
for testblock in list(test.testblockset_config.keys()):
#print "adding testblock:", testblock
smach.Concurrence.add(testblock, SmAtfTestblock(testblock, recorder_handle))
# TODO preempt all other concurrent States as soon as one state returns 'error'
smach.StateMachine.add('CON', sm_con,
transitions={'succeeded':'succeeded',
'error':'error'})
self.sis = smach_ros.IntrospectionServer('/state_machine/machine', self.sm_top, 'SM_ATF')
self.sis.start()
def main():
rospy.init_node('smach_example_state_machine')
# Create the top level SMACH state machine
sm_top = smach.StateMachine(outcomes=['done', 'exit'])
# Open the container
with sm_top:
goal1 = MoveBaseGoal()
goal1.target_pose.header.frame_id = "dtw_robot1/map"
goal1.target_pose.pose.position.x = 0.5
goal1.target_pose.pose.orientation.w = 1.0
smach.StateMachine.add('MOVE1',
smach_ros.SimpleActionState(
'/dtw_robot1/move_base',
MoveBaseAction,
goal=goal1),
transitions={
'succeeded': 'TASK2',
'preempted': 'done',
'aborted': 'done'
})
task2_concurrence = smach.Concurrence(
outcomes=['done', 'exit'],
default_outcome='done',
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with task2_concurrence:
goal2 = MoveBaseGoal()
goal2.target_pose.header.frame_id = "dtw_robot1/map"
goal2.target_pose.pose.position.x = -0.5
goal2.target_pose.pose.orientation.w = 1.0
smach.Concurrence.add(
'MOVE2',
smach_ros.SimpleActionState('/dtw_robot1/move_base',
MoveBaseAction,
goal=goal2))
smach.Concurrence.add(
'MONITOR2',
smach_ros.MonitorState("/sm_stop", Empty, monitor_cb))
smach.StateMachine.add('TASK2',
task2_concurrence,
transitions={
'done': 'done',
'exit': 'exit'
})
# Execute SMACH plan
sis = smach_ros.IntrospectionServer('smach_server', sm_top, '/SM_ROOT')
sis.start()
outcome = sm_top.execute()
rospy.spin()
sis.stop()
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error'])
with self:
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = None
self.userdata.tts_lang = None
self.userdata.standar_error = "ok"
self.userdata.word_to_listen = None
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={
'succeeded': 'CONCURRENCE',
'aborted': 'aborted',
'preempted': 'preempted'
})
sm = smach.Concurrence(
outcomes=['DOOR_OPEN', 'OPERATOR', 'aborted'],
default_outcome='DOOR_OPEN',
input_keys=['word_to_listen'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm:
# it will finisheed with succeeded if it check a door
sm.add('CHECK_DOOR', look_for_elevator_door())
# here i have to listen if they say me to get out of the lift
sm.add('LISTEN_OPERATOR_FOR_EXIT',
ListenWordSM_Concurrent("go out"))
smach.StateMachine.add('CONCURRENCE',
sm,
transitions={
'OPERATOR': 'SAY_OUT',
'DOOR_OPEN': 'SAY_OUT',
'aborted': 'CONCURRENCE'
})
# it says i'm going out
smach.StateMachine.add('SAY_OUT',
text_to_say(SAY_OUT_FRASE),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def main():
rospy.init_node('st_mach')
rospy.loginfo('Inited node st_mach')
global pub
pub = rospy.Publisher('/drone_state', String, queue_size=10)
rospy.Subscriber('/mavros/battery', BatteryState, battery_update)
rospy.Subscriber('/detected_objects', PoseArray, HumFound)
objects_history = []
global battery
battery = None
# creating the concurrence state machine
flying_concurrence = smach.Concurrence(
outcomes=['patrol_done', 'low_bat', 'hovering'],
default_outcome='patrol_done',
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with flying_concurrence:
smach.Concurrence.add(
'START_HOVERING',
smach_ros.MonitorState("/sm_hover", Empty, monitor_cb))
smach.Concurrence.add(
'RETURN_TO_BASE',
smach_ros.MonitorState("/sm_charge", Empty, monitor_cb))
smach.Concurrence.add('PATROL_MODE', Flying())
smach.Concurrence.add('BATTERY_MONITOR', Bat_Monitor())
smach.Concurrence.add('OBJECT_FOUND', Camera_Search())
sm = smach.StateMachine(outcomes=['charging'])
with sm:
smach.StateMachine.add('BASE_CHARGING',
Base(),
transitions={'setup_done': 'FLYING'})
smach.StateMachine.add('FLYING',
Flying(),
transitions={
'patrol_done': 'RETURN_TO_BASE',
'low_bat': 'RETURN_TO_BASE',
'hovering': 'HOVER'
})
smach.StateMachine.add('RETURN_TO_BASE',
Landing(),
transitions={'landing_succeded': 'charging'})
smach.StateMachine.add('HOVER',
Hover(),
transitions={'hovering_timeout': 'FLYING'})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('psr_state_machine', anonymous=True)
# Create a SMACH state machine 'sm'
sm = smach.StateMachine(outcomes=['Exit'])
# Local variables for sm
#sm.userdata.sm_psr_data = [1]
# Open the container sm
with sm:
# Add states to the container 'sm'
smach.StateMachine.add('IDLE', Idle(),
transitions={'Go':'INITIAL',
'ShutDown':'Exit'})
smach.StateMachine.add('INITIAL', Initial(),
transitions={'Go':'CON',
'Cancel':'IDLE'})
# Create a sub SMACH state machine 'sm_con'
sm_con = smach.Concurrence(outcomes=['Completed','Aborted'],
default_outcome='Aborted',
outcome_map={'Completed':
{ 'PSR_CLIENT':'Completed',
'DATA_COLLECT':'Completed'}})
#sm_con.userdata.sm_con_psr_data = [2]
# Add states to the container 'sm_con'
with sm_con:
smach.Concurrence.add('PSR_CLIENT', PSR_client())
smach.Concurrence.add('DATA_COLLECT', Data_collect())
smach.StateMachine.add('CON', sm_con,
transitions={'Completed':'SAVE',
'Aborted':'SAVE'})
# Add states to the container 'sm'
smach.StateMachine.add('SAVE', Save_Data(),
transitions={'Done':'IDLE'})
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('server_psr', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def create_sm():
sm = smach.StateMachine(outcomes=['DONE'])
sm.userdata = create_userdata()
with sm:
smach.StateMachine.add('LAUNCH',
launch_sm.create(),
transitions={
'succeeded': 'HEALTHY',
'preempted': 'SHUTDOWN',
'failed': 'SHUTDOWN'
})
safety_concurrence = smach.Concurrence(
outcomes=['succeeded', 'failed', 'preempted'],
default_outcome='failed',
outcome_map={
'failed': {
'SAFETY': 'invalid',
'STATE_MACHINE': 'preempted'
},
'preempted': {
'SAFETY': 'preempted',
'STATE_MACHINE': 'preempted'
},
'succeeded': {
'SAFETY': 'valid',
'STATE_MACHINE': 'succeeded'
}
},
child_termination_cb=lambda _: True,
input_keys=['sounds'],
output_keys=['sounds'])
with safety_concurrence:
smach.Concurrence.add('SAFETY', SafetyState())
smach.Concurrence.add('STATE_MACHINE', HealthyStateMachine())
smach.StateMachine.add('HEALTHY',
safety_concurrence,
transitions={
'succeeded': 'SHUTDOWN',
'failed': 'ERROR',
'preempted': 'SHUTDOWN'
})
smach.StateMachine.add('ERROR',
ErrorState(),
transitions={'succeeded': 'SHUTDOWN'})
smach.StateMachine.add('SHUTDOWN',
ShutdownState(),
transitions={'succeeded': 'DONE'})
return sm
def __init__(self, type_movement="yes", tts_text=''):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['tts_text', 'type_movment'],
output_keys=['standard_error'])
self.type_movement = type_movement
self.tts_text = tts_text
with self:
self.userdata.tts_text = None
self.userdata.type_movment = None
self.userdata.tts_lang = None
self.userdata.tts_wait_before_speaking = 0
self.userdata.standard_error = 'OK'
self.userdata.manip_time_to_play = 30
self.userdata.skip_planning = False
smach.StateMachine.add('INIT_VAR',
init_var(self.type_movement, self.tts_text),
transitions={
'succeeded': 'PUT_MOVMENT',
'aborted': 'aborted',
'preempted': 'preempted'
})
sm = smach.Concurrence(
outcomes=['succeeded', 'preempted', 'aborted'],
default_outcome='succeeded',
input_keys=[
'tts_text', 'manip_motion_to_play', 'manip_time_to_play',
'tts_wait_before_speaking', 'tts_lang', 'skip_planning'
])
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = None
self.userdata.tts_lang = None
with sm:
sm.add('SAY', text_to_say())
sm.add('MOVE', play_motion_sm(skip_planning=False))
smach.StateMachine.add('PUT_MOVMENT',
sm,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def SynchroConcurrence():
Synchro_cc = smach.Concurrence(
outcomes=["succeeded", "aborted"],
default_outcome='aborted',
outcome_map={"succeeded": {
'Ping': "ping",
'Pong': "pong"
}})
with Synchro_cc:
Synchro_cc.add('Ping', Ping())
Synchro_cc.add('Pong', Pong())
return Synchro_cc
