def __init__(self):
rospy.init_node('xm_winter_test')
rospy.on_shutdown(self.shutdown)
rospy.loginfo('now we will start a long battle, game start!')
self.smach_bool = False
self.xm_EnterRoom = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
#这部分移植学长代码
with self.xm_EnterRoom:
# 进门的状态机:门开关检测->等待->冲进房间->导航到指定地点->succeeded
# if use arm, use this state
# self.xm_EnterRoom.userdata.arm_mode_1 =0
# self.xm_EnterRoom.userdata.arm_ps = PointStamped()
# StateMachine.add('NAV_POSE',
# ArmCmd(),
# transitions={'succeeded':'DOOR_DETECT','aborted':'NAV_POSE','error':'error'},
# remapping ={'arm_ps':'arm_ps','mode':'arm_mode_1'})
# 下列是用到的userdata
# 等待时间
self.xm_EnterRoom.userdata.rec = 3.0
# xm冲进房间的坐标
self.xm_EnterRoom.userdata.go_point = Point(1.5, 0.0, 0.0)
# 与发令者对话坐标
self.xm_EnterRoom.userdata.start_waypoint = gpsr_target['speaker'][
'pos']
# 自我介绍的话
self.xm_EnterRoom.userdata.sentences = 'I am sil meng, you can command me'
# 检测门的状态是开门还是关门
StateMachine.add('DOOR_DETECT',
DoorDetect().door_detect_,
transitions={
'invalid': 'WAIT',
'valid': 'DOOR_DETECT',
'preempted': 'aborted'
})
# 在刷新建图后等待一段时间
# 使用userdata: rec
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'SIMPLE_MOVE',
'error': 'error'
},
remapping={'rec': 'rec'})
# 在开启导航之前首先让xm冲进房间
# 使用userdata: go_point
StateMachine.add('SIMPLE_MOVE',
SimpleMove(),
remapping={'point': 'go_point'},
transitions={
'succeeded': 'NAV_1',
'aborted': 'NAV_1',
'error': 'error'
})
# 到房间里的指定地点接受gpsr命令
# 使用userdata: start_waypoint
StateMachine.add('NAV_1',
NavStack(),
transitions={
'succeeded': 'SPEAK1',
'aborted': 'NAV_1',
'error': 'error'
},
remapping={'pos_xm': 'start_waypoint'})
# 介绍自己
# 使用userdata: sectences
StateMachine.add('SPEAK1',
Speak(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={'sentences': 'sentences'})
self.xm_Nav = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
with self.xm_Nav:
# userdata 列表
# xm的位置信息,由GetTarget()得到
self.xm_Nav.userdata.pos_xm = Pose()
# 这个target_mode == 1返回Pose()，如果== 0 返回名字
self.xm_Nav.userdata.target_mode = 1
#这里的不可能产生aborted
StateMachine.add('GETTAGET_POSE',
GetTarget(),
transitions={
'succeeded': 'NAV_GO',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'target': 'target',
'current_task': 'current_task',
'current_target': 'pos_xm',
'mode': 'target_mode'
})
#如果找不到路径继续执行NavStack的execute(),知道找到为止
StateMachine.add('NAV_GO',
NavStack(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'NAV_GO',
'error': 'error'
},
remapping={'pos_xm': 'pos_xm'})
#当运行命令发生错误时出现aborted情况
self.xm_Nav.userdata.sentences = 'I have arrived here now, you can ask me some question'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={'sentences': 'sentences'})
self.xm_Find = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
##########################################################################################################
################### 这里比较复杂,需要考虑一下好好写 ###################################
##########################################################################################################
################### 检测到一个人,导航到这个人的面前 ###################################
################## 在房间里转圈->直到找到人->得到人的坐标->导航到人->succeeded ##################################
#########################################################################################################
###################难点:在房间里转圈 ###################################
################### 转圈和寻找人要在一个容器里 ###################################
################### 通过FindPeople().find_people_计算得到的xm位姿有可能无法导航到###########################
#########################################################################################################
###################注意:运行FindPeople()必须先RunNode(),上述部分问题可能在 ###################################
################### people_tracking有解决,注意和图像组沟通 ###################################
#########################################################################################################
with self.xm_Find:
# self.xm_Find.userdata.degree = 45.0
self.xm_Find.userdata.rec = 2.0
# run the people_tracking node
StateMachine.add('RUNNODE',
RunNode(),
transitions={
'succeeded': 'WAIT',
'aborted': 'succeeded'
})
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'GET_PEOPLE_POS',
'error': 'error'
},
remapping={'rec': 'rec'})
#the data should be PointStamped()
# 这里必须先运行节点，也就是用RunNode()状态
self.xm_Find.userdata.pos_xm = Pose()
StateMachine.add('GET_PEOPLE_POS',
FindPeople().find_people_,
transitions={
'invalid': 'NAV_PEOPLE',
'valid': 'SPEAK',
'preempted': 'aborted'
},
remapping={'pos_xm': 'pos_xm'})
# this state will use the userdata remapping in the last state
StateMachine.add('NAV_PEOPLE',
NavStack(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'NAV_PEOPLE',
'error': 'error'
},
remapping={'pos_xm': 'pos_xm'})
self.xm_Find.userdata.sentences = 'you can ask me some question'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'CLOSEKINECT',
'error': 'error'
},
remapping={'sentences': 'sentences'})
# close the KinectV2
StateMachine.add('CLOSEKINECT',
CloseKinect(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# StateMachine.add('TURN',
# TurnDegree(),
# transitions={'succeeded':'TURN','aborted':'aborted','error':'error'}
# remapping={'degree':'degree'})
# self.xm_Find_people = Concurrence(outcomes = ['succeeded', 'people', 'error'],
# default_outcome='succeeded',
# child_termination_cb=self.turn_termination_cb,
# outcome_cb = self.turn_outcome_cb)
# with xm_Find_people:
# Concurrence.add('TURN',
# TurnDegree(),
# remapping={'degree':'degree'})
#########################################################################################################
self.test = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.test:
# 测试总状态机
# userdata 列表
# 目标列表,用于获取坐标等操作
self.test.userdata.target = list()
# 动作列表,用于NextDo检测下一步的状态
self.test.userdata.action = list()
# 所有的测试数(动作数),由GetTask得到
self.test.userdata.task_num = 0
# 目前进行到的测试(动作)
self.test.userdata.current_task = -1
# 进门到指定地点
StateMachine.add('ENTER_ROOM',
self.xm_EnterRoom,
transitions={
'succeeded': 'RECEIVE_TASK',
'aborted': 'RECEIVE_TASK',
'error': 'error'
})
# 得到命令,如果没有得到指令则重新运行这个状态
StateMachine.add('RECEIVE_TASK',
GetTask(),
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'RECEIVE_TASK',
'error': 'error'
},
remapping={
'target': 'target',
'action': 'action',
'task_num': 'task_num'
})
# 循环运行这个状态,得到下一步要做的事情
StateMachine.add('GET_NEXT_TASK',
NextDo(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error',
'find': 'FIND',
'go': 'GO',
'answer': 'ANSWER'
},
remapping={
'action': 'action',
'current_task': 'current_task',
'task_num': 'task_num'
})
# 找到人并向他靠近,目前处于未完成状态
StateMachine.add('FIND',
self.xm_Find,
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'GET_NEXT_TASK',
'error': 'error'
},
remapping={
'target': 'target',
'current_task': 'current_task'
})
# 导航到某个房间
StateMachine.add('GO',
self.xm_Nav,
transitions={
'succeeded': 'ANSWER',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'target': 'target',
'current_task': 'current_task'
})
# 回答问题
StateMachine.add('ANSWER',
Anwser(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# 用于将状态机可视化
self.intro_server = IntrospectionServer('test', self.test, '/SM_ROOT')
self.intro_server.start()
# 运行状态机
out = self.test.execute()
# 状态机运行结束后结束可视化
self.intro_server.stop()
if out == 'succeeded':
self.smach_bool = True
def __init__(self):
rospy.init_node('whoiswho_nopick')
rospy.logwarn('start who is who test')
self.smach_bool = False
rospy.on_shutdown(self.shutdown)
# subprocess.call("xterm -e rosrun xm_speech xm_speech_client.py &", shell = True)
# add waypoints into the list
self.waypoints = []
location = (
Point(1.003, -0.217, 0.000), #找人数的点
Point(5.284, -1.246, 0.000), #出门抓东西的点
Point(0.000, 0.000, 0.000)) #结束出门的点
quaternions = (Quaternion(0.000, 0.000, 0.723, 0.691),
Quaternion(0.000, 0.000, 0.050,
0.999), Quaternion(0.000, 0.000, 0.000, 1))
# euler_angles=[0.000,3.1415,0.000]
# for angle in euler_angles:
# q_angle = quaternion_from_euler(0, 0, angle, axes="sxyz")
# q = Quaternion(*q_angle)
# quaternions.append(q)
for i in range(3):
self.waypoints.append(Pose(location[i], quaternions[i]))
# the locate can specified by ourselves
self.sm_EnterRoom = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_EnterRoom:
# rospy.logerr('sm_EnterRoom add State')
# arm go to the nav_pose in the srdf
# self.sm_EnterRoom.userdata.arm_mode =0
# self.sm_EnterRoom.userdata.arm_ps_1 = PointStamped()
# StateMachine.add('NAV_POSE',
# ArmCmd(),
# transitions={'succeeded':'DOOR_DETECT','aborted':'NAV_POSE','error':'error'},
# remapping ={'mode':'arm_mode','arm_ps':'arm_ps_1'})
# wait the door to open
# StateMachine.add('DOOR_DETECT',
# DoorDetect().door_detect_,
# transitions={'invalid':'WAIT','valid':'DOOR_DETECT','preempted':'error'})
# simple delay 5s
# self.sm_EnterRoom.userdata.rec = 5.0
# StateMachine.add('WAIT',
# Wait(),
# transitions={'succeeded':'NAV_1','error':'error'},
# remapping ={'rec':'rec'})
# self.sm_EnterRoom.userdata.point = Point(1.5,0.0,0.0)
# StateMachine.add('SIMPLE_MOVE',
# SimpleMove_move(),
# transitions={'succeeded':'NAV_1','aborted':'NAV_1','error':'error'},
# remapping={'point':'point'})
# navstack to room of crowds
# waypoints are the list of Pose fit the data type need
self.sm_EnterRoom.userdata.start_waypoint = self.waypoints[0]
# self.sm_EnterRoom.userdata.nav_mode =0
StateMachine.add('NAV_1',
NavStack(),
transitions={
'succeeded': 'SELF_INTRO',
'aborted': 'NAV_1',
'error': 'error'
},
remapping={'pos_xm': 'start_waypoint'})
# self-introduce
self.sm_EnterRoom.userdata.sentences_2 = 'I am robot xiaomeng'
StateMachine.add('SELF_INTRO',
Speak(),
remapping={'sentences': 'sentences_2'},
transitions={
'succeeded': 'succeeded',
'aborted': 'SELF_INTRO',
'error': 'error'
})
# we have already identity the people from 0-4 when the first recongization
self.sm_FaceDetect = StateMachine(
outcomes=['succeeded', 'aborted', 'error'],
output_keys=['people_position', 'num_list'])
with self.sm_FaceDetect:
self.sm_FaceDetect.userdata.people_position = list()
self.sm_FaceDetect.userdata.sentences = 'please look at me'
StateMachine.add('SPEAK',
Speak(),
remapping={'sentences': "sentences"},
transitions={
'succeeded': 'GET_POSITION',
'aborted': 'aborted',
'error': 'error'
})
# call face_reco service for get all the people position which is a list
self.sm_FaceDetect.userdata.name_id = -1
self.sm_FaceDetect.userdata.num_list = list()
StateMachine.add('GET_POSITION',
FaceReco(),
remapping={
'name_id': 'name_id',
'position': 'people_position',
'num_list': 'num_list'
},
transitions={
'succeeded': 'succeeded',
'again': 'GET_POSITION',
'aborted': 'GET_POSITION',
'error': 'error',
'turn_l': 'TURN_L',
'turn_r': 'TURN_R',
'train_error': 'aborted'
})
# if the face-recognize failed, we should make some remedy to make the state continue
self.sm_FaceDetect.userdata.turn_point_1 = Point(0.0, 0.0, pi / 8)
StateMachine.add('TURN_L',
SimpleMove_move(),
transitions={
'succeeded': 'SPEAK_2',
'error': 'error'
},
remapping={'point': 'turn_point_1'})
self.sm_FaceDetect.userdata.turn_point_2 = Point(0.0, 0.0, -pi / 8)
StateMachine.add('TURN_R',
SimpleMove_move(),
remapping={'point': 'turn_point_2'},
transitions={
'succeeded': 'SPEAK_2',
'error': 'error'
})
StateMachine.add('SPEAK_2',
Speak(),
remapping={'sentences': 'sentences'},
transitions={
'succeeded': 'GET_POSITION',
'aborted': 'aborted',
'error': 'error'
})
self.sm_GetTarget = StateMachine(
outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target']) #the target is a string.....
with self.sm_GetTarget:
# because xm is nav to pose in the nav_pose
self.sm_GetTarget.userdata.nav_ps = self.waypoints[1]
# this smach code donnot to grasp ,so this part is useless
StateMachine.add('NAV_TARGET',
NavStack(),
remapping={'pos_xm': 'nav_ps'},
transitions={
'succeeded': 'FIND_OBJECT',
'aborted': 'NAV_TARGET',
'error': 'error'
})
self.sm_GetTarget.userdata.object_pos = PointStamped()
StateMachine.add('FIND_OBJECT',
new_vision.FindObject(),
remapping={
'name': 'target',
'object_pos': 'object_pos'
},
transitions={
'succeeded': 'TALK',
'aborted': 'FIND_OBJECT',
'error': 'error'
})
self.sm_GetTarget.userdata.sentences = 'I find the target'
StateMachine.add('TALK',
Speak(),
remapping={'sentences': 'sentences'},
transitions={
'succeeded': 'PICK',
'aborted': 'TALK',
'error': 'error'
})
self.sm_GetTarget.userdata.arm_mode_1 = 1
StateMachine.add('PICK',
ArmCmd(),
remapping={
'mode': 'arm_mode_1',
'arm_ps': 'object_pos'
},
transitions={
'succeeded': 'succeeded',
'aborted': 'succeeded',
'error': 'error'
})
self.sm_GetTarget.userdata.arm_mode_2 = 0
self.sm_GetTarget.userdata.arm_ps_2 = PointStamped()
StateMachine.add('PUT',
PlaceBag(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# concurrence for the speech_node
# we want to make the speech-meaning a timeout, so we use the concurrence function
# but in fact we can also use multithread to slove the problem
self.meta_Remember = Concurrence(
outcomes=['succeeded', 'aborted', 'error'],
output_keys=['name', 'target'],
default_outcome='succeeded',
outcome_map={'succeeded': {
'NAME_AND_THING': 'succeeded'
}},
child_termination_cb=self.child_cb)
with self.meta_Remember:
Concurrence.add('GET_EMPTY_NAME', Hehe())
Concurrence.add('NAME_AND_THING',
NameAndThing(),
remapping={
'name': 'name',
'target': 'target'
})
# input one position is a list
# io_keys can wirte and read
# userdata can update real-time
# no need to call the cv service
self.sm_Remember = StateMachine(
outcomes=['succeeded', 'aborted', 'error'],
input_keys=['person_position'],
output_keys=['name', 'target'])
with self.sm_Remember:
# rospy.logerr("sm_Remember add state")
# self.sm_Remember.userdata.nav_mode =1
# rospy.logerr("Is here1?")
StateMachine.add('NAV_GO',
NavStack(),
remapping={'pos_xm': 'person_position'},
transitions={
'succeeded': 'TALK',
'aborted': 'NAV_GO',
'error': 'error'
})
self.sm_Remember.userdata.sentences = "what is your name and what do you want"
StateMachine.add('TALK',
Speak(),
remapping={'sentences': 'sentences'},
transitions={
'succeeded': 'GET_BOTH',
'aborted': 'TALK',
'error': 'error'
})
# StateMachine.add('RUNNODE',
# RunNode(),
# transitions ={'succeeded':'GET_BOTH','aborted':'aborted'})
StateMachine.add('GET_BOTH',
self.meta_Remember,
remapping={
'name': 'name',
'target': 'target'
},
transitions={
'succeeded': 'WAIT',
'aborted': 'GET_BOTH',
'error': 'error'
})
self.sm_Remember.userdata.rec = 2.0
StateMachine.add('WAIT',
Wait(),
remapping={'rec': 'rec'},
transitions={
'succeeded': 'succeeded',
'error': "error"
})
self.sm_GiveBack = StateMachine(
outcomes=['succeeded', 'aborted', 'error'],
input_keys=['name_id', 'name_list',
'target_list']) # the name is a string
with self.sm_GiveBack:
# rospy.logerr('sm_GiveBack add State')
# self.sm_GiveBack.userdata.nav_ps = self.waypoints[0]
# self.sm_GiveBack.userdata.nav_mode_1 =0
# StateMachine.add('NAV_ROOM',
# NavStack(),
# remapping ={'pos_xm':'nav_ps','mode':'nav_mode_1'},
# transitions ={'succeeded':'FACE_RECO','aborted':'NAV_ROOM','error':'error'})
# self.sm_GiveBack.userdata.name_id =0
# self.sm_GiveBack.userdata.name_list =list()
self.sm_GiveBack.userdata.sentences = "please look at me"
StateMachine.add('SPEAK',
Speak(),
remapping={'sentences': 'sentences'},
transitions={
'succeeded': 'WAIT',
'aborted': 'aborted',
'error': 'error'
})
self.sm_GiveBack.userdata.rec = 5.0
StateMachine.add('WAIT',
Wait(),
remapping={'rec': 'rec'},
transitions={
'succeeded': 'FACE_RECO',
'error': 'error'
})
self.sm_GiveBack.userdata.person_position = PointStamped()
StateMachine.add('FACE_RECO',
FaceReco(),
remapping={
'position': 'person_position',
'name_id': 'name_id'
},
transitions={
'succeeded': 'NAV_GO',
'again': 'FACE_RECO',
'aborted': 'FACE_RECO',
'error': 'error',
'turn_l': 'TURN_L',
'turn_r': 'TURN_R',
'train_error': 'aborted'
})
self.sm_GiveBack.userdata.turn_point_1 = Point(0.0, 0.0, pi / 8)
StateMachine.add('TURN_L',
SimpleMove_move(),
transitions={
'succeeded': 'SPEAK_2',
'error': 'error'
},
remapping={'point': 'turn_point_1'})
self.sm_GiveBack.userdata.turn_point_2 = Point(0.0, 0.0, -pi / 8)
StateMachine.add('TURN_R',
SimpleMove_move(),
remapping={'point': 'turn_point_2'},
transitions={
'succeeded': 'SPEAK_2',
'error': 'error'
})
StateMachine.add('SPEAK_2',
Speak(),
remapping={'sentences': 'sentences'},
transitions={
'succeeded': 'FACE_RECO',
'aborted': 'aborted',
'error': 'error'
})
#please pay attention!
# self.sm_GiveBack.userdata.nav_mode_2 =1
StateMachine.add('NAV_GO',
NavStack(),
remapping={'pos_xm': 'person_position'},
transitions={
'succeeded': 'GET_NAME',
'aborted': 'NAV_GO',
'error': 'error'
})
StateMachine.add("GET_NAME",
NameHehe(),
remapping={
'name_list': 'name_list',
'name_id': 'name_id',
'target_list': 'target_list',
'sentences': 'sentences_1'
},
transitions={
'succeeded': 'TALK_1',
'aborted': 'aborted',
'error': 'error'
})
self.sm_GiveBack.userdata.sentences_1 = ''
StateMachine.add('TALK_1',
Speak(),
remapping={'sentences': "sentences_1"},
transitions={
'succeeded': 'PUT',
'aborted': "PUT",
'error': 'error'
})
StateMachine.add('PUT',
PlaceBag(),
transitions={
'succeeded': 'succeeded',
'aborted': 'succeeded'
})
# this pose should be specified to make the people get it
# self.sm_GiveBack.userdata.arm_mode =2
# self.sm_GiveBack.userdata.place_ps = PointStamped()
# self.sm_GiveBack.userdata.place_ps.header.frame_id ='base_footprint'
# self.sm_GiveBack.userdata.place_ps.point.x =0.7
# self.sm_GiveBack.userdata.place_ps.point.y =0.0
# self.sm_GiveBack.userdata.place_ps.point.z =0.3
# StateMachine.add('PLACE',
# ArmCmd(),
# remapping ={'mode':'arm_mode','arm_ps':'place_ps'},
# transitions ={'succeeded':'TALK_2','aborted':'PLACE','error':'error'})
# self.sm_GiveBack.userdata.sentences_2 = 'I get the thing you want, am I?'
# StateMachine.add('TALK_2',
# Speak(),
# remapping ={'sentences':'sentences_2'},
# transitions ={'succeeded':'succeeded','aborted':"aborted",'error':'error'})
self.sm_EndTask = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_EndTask:
# rospy.logerr('sm_EndTask add State')
self.sm_EndTask.userdata.nav_ps = self.waypoints[2]
# self.sm_EndTask.userdata.nav_mode = 0
StateMachine.add('NAV_BYEBYE',
NavStack(),
remapping={'pos_xm': 'nav_ps'},
transitions={
'succeeded': 'succeeded',
'aborted': 'NAV_BYEBYE',
'error': 'error'
})
self.sm_EndTask.userdata.point = Point(1.8, 0.0, 0.0)
# StateMachine.add('SIMPLE_MOVE',
# SimpleMove_move(),
# remapping={'point':'point'},
# transitions={'succeeded':'succeeded','aborted':'aborted','error':'error'})
self.WHOISWHO = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.WHOISWHO:
# we can generate some state which contant the list_value
StateMachine.add('ENTERROOM',
self.sm_EnterRoom,
transitions={
'succeeded': 'FACEDETECT',
'aborted': 'aborted',
'error': 'error'
})
self.WHOISWHO.userdata.people_position = list()
self.WHOISWHO.userdata.num_list = list()
StateMachine.add('FACEDETECT',
self.sm_FaceDetect,
remapping={
'people_position': 'people_position',
'num_list': 'num_list'
},
transitions={
'succeeded': 'GETPERSON',
'aborted': 'aborted',
'error': 'error'
})
self.WHOISWHO.userdata.person_position = PointStamped()
# if someone is recongized failed, we should make the total list in the same order
# get the last element of the list
# FGM: This state is to get different people pos
StateMachine.add('GETPERSON',
GetValue(),
remapping={
'element_list': 'people_position',
'element': 'person_position'
},
transitions={
'succeeded': 'REMEMBER',
'aborted': "GETTARGET",
'error': 'error'
})
# the cv will remember the face with the increasing ID
self.WHOISWHO.userdata.name = ''
self.WHOISWHO.userdata.target = ''
self.WHOISWHO.userdata.name_list = list()
self.WHOISWHO.userdata.target_list = list()
# if donnot need to remember, the order of ids should be the same with the positions return
# the first of the position should be the 0
# last be the 4
StateMachine.add('REMEMBER',
self.sm_Remember,
remapping={
'person_position': 'person_position',
'name': 'name',
'target': 'target'
},
transitions={
'succeeded': 'NAMEINLIST',
'aborted': 'aborted',
'error': 'error'
})
#this state use for joining the name and the target into the name_list and target_list
# insert in the head of the lists
# if the name and target are empty, we should also append them to the lists
StateMachine.add('NAMEINLIST',
NameInList(),
remapping={
'name': 'name',
'target': 'target',
'name_list': 'name_list',
'target_list': 'target_list'
},
transitions={
'succeeded': 'GETPERSON',
'aborted': 'aborted',
'error': 'error'
})
# ############################################################################
# so far here ,the tasks of remember is completed , the rest is the tasks to return the target to the people
# we should take out the name and the matched target for xm to grasp and give back
# in fact, we will pop the name and target out the list,so the name_id is gradually reduced
# ############################################################################
StateMachine.add('GETTARGET',
GetValue(),
remapping={
'element_list': 'target_list',
'element': 'target'
},
transitions={
'succeeded': 'CATCHTARGET',
'aborted': 'ENDTASK',
'error': 'error'
})
StateMachine.add('CATCHTARGET',
self.sm_GetTarget,
transitions={
'succeeded': 'NAV_ROOM',
'aborted': 'NAV_ROOM',
'error': 'error'
},
remapping={'target': 'target'})
# ############################################################################
self.WHOISWHO.userdata.nav_ps = self.waypoints[0]
# self.WHOISWHO.userdata.nav_mode_1 =0
StateMachine.add('NAV_ROOM',
NavStack(),
remapping={'pos_xm': 'nav_ps'},
transitions={
'succeeded': 'GETID',
'aborted': 'GETID',
'error': 'error'
})
StateMachine.add('CHECKFINISH',
CBState(self.checkfinish,
outcomes=['finish', 'continue'],
input_keys=['num_list']),
transitions={
'finish': 'ENDTASK',
'continue': 'GETTARGET'
},
remapping={'num_list': 'num_list'})
# StateMachine.add('CLEAR_MAP',
# ClearMap(),
# transitions ={'succeeded':'GETID','aborted':'GETID'})
# get the last num of the list, ID
# if the name and target list is empty, will skipped this item
self.WHOISWHO.userdata.sentences_1 = 'please change the order'
StateMachine.add('SPEAK_1',
Speak(),
remapping={'sentences': 'sentences_1'},
transitions={
'succeeded': 'WAIT_HEHE',
'aborted': 'WAIT_HEHE',
'error': 'error'
})
self.WHOISWHO.userdata.rec_hehe = 10.0
StateMachine.add('WAIT_HEHE',
Wait(),
remapping={'rec': 'rec_hehe'},
transitions={
'succeeded': 'SPEAK_2',
'error': 'error'
})
self.WHOISWHO.userdata.sentences_2 = 'I will make the recongization task'
StateMachine.add('SPEAK_2',
Speak(),
transitions={
'succeeded': 'GETID',
'aborted': 'GETID',
'error': 'error'
},
remapping={'sentences': 'sentences_2'})
self.WHOISWHO.userdata.name_id = 0
StateMachine.add('GETID',
GetId(),
remapping={
'output_id': 'name_id',
'input_list': 'name_list',
'num_list': 'num_list'
},
transitions={
'succeeded': 'GIVEBACK',
'aborted': 'ENDTASK',
'error': 'error'
})
StateMachine.add('GIVEBACK',
self.sm_GiveBack,
remapping={
'name_id': 'name_id',
'name_list': 'name_list',
'target_list': 'target_list'
},
transitions={
'succeeded': 'CHECKFINISH',
'aborted': 'CHECKFINISH',
'error': 'error'
})
StateMachine.add('ENDTASK',
self.sm_EndTask,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
})
# rospy.logerr('sm_Top execute begin')
intro_server = IntrospectionServer('whoiswho', self.WHOISWHO,
'SM_ROOT')
intro_server.start()
out = self.WHOISWHO.execute()
intro_server.stop()
if out == 'succeeded':
self.smach_bool = True
remapping={
'tagid': 'tagid', # input
'explore_radius': 'explore_radius', # input
'explore_rfid_reads': 'explore_rfid_reads'
}, # output
# transitions={'succeeded':'succeeded'})
transitions={'succeeded': 'BEST_VANTAGE'})
sm_vantage = sm_best_vantage()
smach.StateMachine.add(
'BEST_VANTAGE', # outcomes: succeeded, aborted, preempted
sm_vantage,
remapping={
'tagid': 'tagid', # input
'rfid_reads': 'explore_rfid_reads'
}, # input
transitions={'succeeded': 'succeeded'})
sis = IntrospectionServer('sm_rfid_explore', sm, '/SM_ROOT_RFID_EXPLORE')
sis.start()
rospy.sleep(3.0)
sm.userdata.tagid = 'person '
sm.userdata.explore_radius = 2.7
# sm.userdata.explore_rfid_reads = []
outcome = sm.execute()
# print sm.userdata.explore_rfid_reads
rospy.spin()
sis.stop()
def __init__(self):
rospy.init_node('carry_smach_move', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# First define the corner orientations as Euler angles
euler_angles = (0.0, 3 * pi / 2, pi / 2, 3 * pi / 2, pi / 2,
3 * pi / 2, 3 * pi / 2, 0.0, pi, 0.0, pi, 3 * pi / 2)
# Then convert the angles to quaternions
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
# Create a list to hold the waypoint poses
self.waypoints = list()
# Append each of the four waypoints to the list. Each waypoint
# is a pose consisting of a position and orientation in the map frame.
self.waypoints.append(Pose(Point(0.0, -3.0, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(2.0, -2.0, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(2.9, 1.0, 0.0), quaternions[2]))
self.waypoints.append(Pose(Point(3.0, 1.2, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(4.23, 1.1, 0.0), quaternions[4]))
self.waypoints.append(Pose(Point(4.23, 1.1, 0.0), quaternions[5]))
self.waypoints.append(Pose(Point(4.4, -0.6, 0.0), quaternions[6]))
self.waypoints.append(Pose(Point(2.3, 1.7, 0.0), quaternions[7]))
self.waypoints.append(Pose(Point(2.3, 1.7, 0.0), quaternions[8]))
self.waypoints.append(Pose(Point(-1.3, 0.7, 0.0), quaternions[9]))
self.waypoints.append(Pose(Point(-1.3, 0.7, 0.0), quaternions[10]))
self.waypoints.append(Pose(Point(-1.6, 0.1, 0.0), quaternions[11]))
# State machine
#self.sm_out_main = StateMachine(outcomes=['succeeded','aborted','preempted'])
#self.sm_out_main.userdata.goal = self.waypoints[2];
#with self.sm_out_main:
# StateMachine.add('GO_OUT_MAIN_ROOM', Nav2Waypoint(),
# transitions={'succeeded':'succeeded',
# 'aborted':'aborted'})
# Concurrent State machine
self.sm_in_main_room = Concurrence(
outcomes=[
'succeeded', 'aborted', 'preempted', 'go_kitchen',
'go_bedroom', 'go_sleep'
],
default_outcome='succeeded',
child_termination_cb=self.in_main_room_child_termination_cb,
outcome_cb=self.in_main_room_outcome_cb)
self.sm_in_main_room.userdata.goal = self.waypoints[1]
with self.sm_in_main_room:
Concurrence.add(
'GO_TO_KITCHEN',
MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_BEDROOM',
MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_SLEEP',
MonitorState("/CARRY/go_sleep", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[1]))
# Concurrent State machine
self.sm_sleep = Concurrence(
outcomes=[
'succeeded', 'aborted', 'preempted', 'go_kitchen',
'go_bedroom', 'go_main_room'
],
default_outcome='succeeded',
child_termination_cb=self.in_sleep_child_termination_cb,
outcome_cb=self.in_sleep_outcome_cb)
self.sm_sleep.userdata.goal = self.waypoints[0]
with self.sm_sleep:
Concurrence.add(
'GO_TO_KITCHEN',
MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_BEDROOM',
MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_MAIN_ROOM',
MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[0]))
# Concurrent State machine
self.sm_in_kitchen = Concurrence(
outcomes=[
'succeeded', 'aborted', 'preempted', 'go_main_room',
'go_bedroom', 'go_sleep'
],
default_outcome='succeeded',
child_termination_cb=self.in_kitchen_child_termination_cb,
outcome_cb=self.in_kitchen_outcome_cb)
self.sm_in_kitchen.userdata.goal = self.waypoints[6]
with self.sm_in_kitchen:
Concurrence.add(
'GO_TO_MAIN_ROOM',
MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_BEDROOM',
MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_SLEEP',
MonitorState("/CARRY/go_sleep", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[6]))
# Concurrent State machine
self.sm_in_bedroom = Concurrence(
outcomes=[
'succeeded', 'aborted', 'preempted', 'go_main_room',
'go_kitchen', 'go_sleep'
],
default_outcome='succeeded',
child_termination_cb=self.in_bedroom_child_termination_cb,
outcome_cb=self.in_bedroom_outcome_cb)
self.sm_in_bedroom.userdata.goal = self.waypoints[11]
with self.sm_in_bedroom:
Concurrence.add(
'GO_TO_MAIN_ROOM',
MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_KITCHEN',
MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_SLEEP',
MonitorState("/CARRY/go_sleep", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[11]))
# Create the top level state machine
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
StateMachine.add('IN_MAIN_ROOM',
self.sm_in_main_room,
transitions={
'succeeded': 'IN_MAIN_ROOM',
'go_kitchen': 'NAV_M2K_M',
'go_sleep': 'IN_SLEEP',
'go_bedroom': 'NAV_M2B_M'
})
StateMachine.add('IN_KITCHEN',
self.sm_in_kitchen,
transitions={
'succeeded': 'succeeded',
'go_main_room': 'NAV_K2M_K',
'go_sleep': 'NAV_K2S_K',
'go_bedroom': 'NAV_K2B_K'
})
StateMachine.add('IN_BEDROOM',
self.sm_in_bedroom,
transitions={
'succeeded': 'succeeded',
'go_kitchen': 'NAV_B2K_B',
'go_sleep': 'NAV_B2S_B',
'go_main_room': 'NAV_B2M_B'
})
StateMachine.add('IN_SLEEP',
self.sm_sleep,
transitions={
'succeeded': 'succeeded',
'go_kitchen': 'NAV_S2K_M',
'go_main_room': 'IN_MAIN_ROOM',
'go_bedroom': 'NAV_S2B_M'
})
StateMachine.add('NAV_M2K_M',
Nav2Waypoint(self.waypoints[2]),
transitions={
'succeeded': 'NAV_M2K_K',
'aborted': 'aborted'
})
StateMachine.add('NAV_M2K_K',
Nav2Waypoint(self.waypoints[5]),
transitions={
'succeeded': 'NAV_M2K_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_M2K_END',
Nav2Waypoint(self.waypoints[6]),
transitions={
'succeeded': 'IN_KITCHEN',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2M_K',
Nav2Waypoint(self.waypoints[4]),
transitions={
'succeeded': 'NAV_K2M_M',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2M_M',
Nav2Waypoint(self.waypoints[3]),
transitions={
'succeeded': 'NAV_K2M_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2M_END',
Nav2Waypoint(self.waypoints[1]),
transitions={
'succeeded': 'IN_MAIN_ROOM',
'aborted': 'aborted'
})
StateMachine.add('NAV_M2B_M',
Nav2Waypoint(self.waypoints[2]),
transitions={
'succeeded': 'NAV_M2B_C',
'aborted': 'aborted'
})
StateMachine.add('NAV_M2B_C',
Nav2Waypoint(self.waypoints[8]),
transitions={
'succeeded': 'NAV_M2B_B',
'aborted': 'aborted'
})
StateMachine.add('NAV_M2B_B',
Nav2Waypoint(self.waypoints[10]),
transitions={
'succeeded': 'NAV_M2B_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_M2B_END',
Nav2Waypoint(self.waypoints[11]),
transitions={
'succeeded': 'IN_BEDROOM',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2S_K',
Nav2Waypoint(self.waypoints[4]),
transitions={
'succeeded': 'NAV_K2S_M',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2S_M',
Nav2Waypoint(self.waypoints[3]),
transitions={
'succeeded': 'NAV_K2S_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2S_END',
Nav2Waypoint(self.waypoints[0]),
transitions={
'succeeded': 'IN_SLEEP',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2B_K',
Nav2Waypoint(self.waypoints[4]),
transitions={
'succeeded': 'NAV_K2B_C',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2B_C',
Nav2Waypoint(self.waypoints[8]),
transitions={
'succeeded': 'NAV_K2B_B',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2B_B',
Nav2Waypoint(self.waypoints[10]),
transitions={
'succeeded': 'NAV_K2B_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2B_END',
Nav2Waypoint(self.waypoints[11]),
transitions={
'succeeded': 'IN_BEDROOM',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2K_B',
Nav2Waypoint(self.waypoints[9]),
transitions={
'succeeded': 'NAV_B2K_C',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2K_C',
Nav2Waypoint(self.waypoints[7]),
transitions={
'succeeded': 'NAV_B2K_K',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2K_K',
Nav2Waypoint(self.waypoints[5]),
transitions={
'succeeded': 'NAV_B2K_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2K_END',
Nav2Waypoint(self.waypoints[6]),
transitions={
'succeeded': 'IN_KITCHEN',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2S_B',
Nav2Waypoint(self.waypoints[9]),
transitions={
'succeeded': 'NAV_B2S_C',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2S_C',
Nav2Waypoint(self.waypoints[7]),
transitions={
'succeeded': 'NAV_B2S_M',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2S_M',
Nav2Waypoint(self.waypoints[3]),
transitions={
'succeeded': 'NAV_B2S_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2S_END',
Nav2Waypoint(self.waypoints[0]),
transitions={
'succeeded': 'IN_SLEEP',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2M_B',
Nav2Waypoint(self.waypoints[9]),
transitions={
'succeeded': 'NAV_B2M_C',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2M_C',
Nav2Waypoint(self.waypoints[7]),
transitions={
'succeeded': 'NAV_B2M_M',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2M_M',
Nav2Waypoint(self.waypoints[3]),
transitions={
'succeeded': 'NAV_B2M_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2M_END',
Nav2Waypoint(self.waypoints[1]),
transitions={
'succeeded': 'IN_MAIN_ROOM',
'aborted': 'aborted'
})
StateMachine.add('NAV_S2B_M',
Nav2Waypoint(self.waypoints[2]),
transitions={
'succeeded': 'NAV_S2B_C',
'aborted': 'aborted'
})
StateMachine.add('NAV_S2B_C',
Nav2Waypoint(self.waypoints[8]),
transitions={
'succeeded': 'NAV_S2B_B',
'aborted': 'aborted'
})
StateMachine.add('NAV_S2B_B',
Nav2Waypoint(self.waypoints[10]),
transitions={
'succeeded': 'NAV_S2B_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_S2B_END',
Nav2Waypoint(self.waypoints[11]),
transitions={
'succeeded': 'IN_BEDROOM',
'aborted': 'aborted'
})
StateMachine.add('NAV_S2K_M',
Nav2Waypoint(self.waypoints[2]),
transitions={
'succeeded': 'NAV_S2K_K',
'aborted': 'aborted'
})
StateMachine.add('NAV_S2K_K',
Nav2Waypoint(self.waypoints[5]),
transitions={
'succeeded': 'NAV_S2K_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_S2K_END',
Nav2Waypoint(self.waypoints[6]),
transitions={
'succeeded': 'IN_KITCHEN',
'aborted': 'aborted'
})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('carry_sm', self.sm_top,
'/SM_CARRY_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
})
StateMachine.add(
"GOTO1",
Goto1,
transitions={
"ok": "ok",
"preempted": "preempted",
"fail": "fail" # fixme: add goto2
},
remapping={"goal_position": "tennis_position"})
StateMachine.add("SEARCH",
SearchState(),
transitions={
"go_back": "GOTO1",
"tennis": "GOTO1",
"preempted": "preempted"
},
remapping={"tennis_position": "goal_position"})
isw = IntrospectionServer("nav_server", sm, "/NAV")
asw = ActionServerWrapper("/nav",
RunNavAction,
sm,
succeeded_outcomes=["ok"],
aborted_outcomes=["fail"],
goal_key="goal")
isw.start()
asw.run_server()
rospy.spin()
def main():
# Open the container
rospy.init_node("pick_object_wbc_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
# read large object list
sm.userdata.large_objects = rospy.get_param("~large_objects", ["S40_40_B", "S40_40_G", "M30", "BEARING_BOX", "MOTOR"])
sm.userdata.reperceive = rospy.get_param("~reperceive", True)
with sm:
smach.StateMachine.add(
"SELECT_OBJECT",
SelectObject("/mcr_perception/object_selector/input/object_name"),
transitions={"succeeded": "GENERATE_OBJECT_POSE"},
)
# generates a pose of object
smach.StateMachine.add(
"GENERATE_OBJECT_POSE",
gbs.send_and_wait_events_combined(
event_in_list=[
("/mcr_perception/object_selector/event_in", "e_trigger")
],
event_out_list=[
("/mcr_perception/object_selector/event_out", "e_selected", True,)
],
timeout_duration=10,
),
transitions={
"success": "SET_DBC_PARAMS",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"SET_DBC_PARAMS",
gbs.set_named_config("dbc_pick_object"),
transitions={
"success": "MOVE_ROBOT_AND_TRY_PICKING",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
# whole body control command. It moves direct base controller and
# checks if an IK soln exists for the arm.
smach.StateMachine.add(
"MOVE_ROBOT_AND_TRY_PICKING",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_try")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=50,
),
transitions={
"success": "CHECK_IF_REPERCEIVE",
"timeout": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
"failure": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"CHECK_IF_REPERCEIVE",
ShouldReperceive(),
transitions={
"yes": "OPEN_GRIPPER_FOR_REPERCEIVE",
"no": "GENERATE_OBJECT_POSE_AGAIN",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER_FOR_REPERCEIVE",
gms.control_gripper("open"),
transitions={"succeeded": "MOVE_ARM"},
)
# move arm to appropriate position
smach.StateMachine.add(
"MOVE_ARM",
gms.move_arm("look_at_workspace_from_near"),
transitions={
"succeeded": "START_OBJECT_LIST_MERGER",
"failed": "MOVE_ARM",
},
)
smach.StateMachine.add(
"START_OBJECT_LIST_MERGER",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/object_list_merger/event_in", "e_start")],
event_out_list=[("/mcr_perception/object_list_merger/event_out", "e_started", True)],
timeout_duration=5,
),
transitions={
"success": "WAIT_FOR_ARM_TO_STABILIZE",
"timeout": "TRY_PICKING",
"failure": "TRY_PICKING",
},
)
smach.StateMachine.add(
"WAIT_FOR_ARM_TO_STABILIZE",
mir_gbs.wait_for(0.5),
transitions={
"succeeded": "START_OBJECT_RECOGNITION",
},
)
# New perception pipeline state machine
smach.StateMachine.add(
"START_OBJECT_RECOGNITION",
gbs.send_and_wait_events_combined(
event_in_list=[("/mir_perception/multimodal_object_recognition/event_in", "e_start")],
event_out_list=[("/mir_perception/multimodal_object_recognition/event_out", "e_done", True)],
timeout_duration=10,
),
transitions={
"success": "STOP_RECOGNITION",
"timeout": "TRY_PICKING",
"failure": "TRY_PICKING",
},
)
smach.StateMachine.add(
"STOP_RECOGNITION",
gbs.send_and_wait_events_combined(
event_in_list=[("/mir_perception/multimodal_object_recognition/event_in", "e_stop")],
event_out_list=[("/mir_perception/multimodal_object_recognition/event_out", "e_stopped", True)],
timeout_duration=5,
),
transitions={
"success": "STOP_OBJECT_LIST_MERGER",
"timeout": "TRY_PICKING",
"failure": "TRY_PICKING",
},
)
smach.StateMachine.add(
"STOP_OBJECT_LIST_MERGER",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/object_list_merger/event_in", "e_stop")],
event_out_list=[("/mcr_perception/object_list_merger/event_out", "e_stopped", True)],
timeout_duration=5,
),
transitions={
"success": "PUBLISH_MERGED_OBJECT_LIST",
"timeout": "TRY_PICKING",
"failure": "TRY_PICKING",
},
)
smach.StateMachine.add(
"PUBLISH_MERGED_OBJECT_LIST",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/object_list_merger/event_in", "e_trigger_local")],
event_out_list=[("/mcr_perception/object_list_merger/event_out", "e_done", True)],
timeout_duration=5,
),
transitions={
"success": "SELECT_OBJECT_AGAIN",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"SELECT_OBJECT_AGAIN",
SelectObject("/mcr_perception/local_object_selector/input/object_name"),
transitions={"succeeded": "GENERATE_UPDATED_OBJECT_POSE"},
)
# generates a pose of object
smach.StateMachine.add(
"GENERATE_UPDATED_OBJECT_POSE",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/local_object_selector/event_in", "e_trigger")],
event_out_list=[("/mcr_perception/local_object_selector/event_out", "e_selected", True)],
timeout_duration=10,
),
transitions={
"success": "CHECK_IF_OBJECT_LARGE_LOCAL",
"timeout": "GENERATE_OBJECT_POSE_AGAIN",
"failure": "GENERATE_OBJECT_POSE_AGAIN",
},
)
# generates a pose of object
smach.StateMachine.add(
"GENERATE_OBJECT_POSE_AGAIN",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/object_selector/event_in", "e_re_trigger")],
event_out_list=[("/mcr_perception/object_selector/event_out", "e_selected", True)],
timeout_duration=10,
),
transitions={
"success": "CHECK_IF_OBJECT_LARGE",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"CHECK_IF_OBJECT_LARGE_LOCAL",
IsObjectLarge(),
transitions={
"large": "OPEN_GRIPPER_WIDE_LOCAL",
"small": "OPEN_GRIPPER_NARROW_LOCAL",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER_WIDE_LOCAL",
gms.control_gripper("open"),
transitions={"succeeded": "TRY_PICKING"},
)
smach.StateMachine.add(
"OPEN_GRIPPER_NARROW_LOCAL",
gms.control_gripper("open_narrow"),
transitions={"succeeded": "TRY_PICKING"},
)
# move only arm for wbc
smach.StateMachine.add(
"TRY_PICKING",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_start_arm_only")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=20,
),
transitions={
"success": "CLOSE_GRIPPER",
"timeout": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
"failure": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"CHECK_IF_OBJECT_LARGE",
IsObjectLarge(),
transitions={
"large": "OPEN_GRIPPER_WIDE",
"small": "OPEN_GRIPPER_NARROW",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER_WIDE",
gms.control_gripper("open"),
transitions={"succeeded": "MOVE_ROBOT_AND_PICK"},
)
smach.StateMachine.add(
"OPEN_GRIPPER_NARROW",
gms.control_gripper("open_narrow"),
transitions={"succeeded": "MOVE_ROBOT_AND_PICK"},
)
# whole body control command. It moves direct base controller and
# calls pre-grasp planner, and (optionally) moves arm to object pose
smach.StateMachine.add(
"MOVE_ROBOT_AND_PICK",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_start")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=50,
),
transitions={
"success": "CLOSE_GRIPPER",
"timeout": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
"failure": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
gbs.send_event(
[
("/waypoint_trajectory_generation/event_in", "e_stop"),
("/wbc/event_in", "e_stop"),
]
),
transitions={"success": "OVERALL_FAILED"},
)
smach.StateMachine.add(
"CLOSE_GRIPPER",
gms.control_gripper("close"),
transitions={"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE"},
)
# move arm to stage_intemediate position
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE",
gms.move_arm("stage_intermediate"),
transitions={
"succeeded": "VERIFY_OBJECT_GRASPED",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE",
},
)
smach.StateMachine.add(
"VERIFY_OBJECT_GRASPED",
gbs.send_and_wait_events_combined(
event_in_list=[
("/gripper_controller/grasp_monitor/event_in", "e_trigger")
],
event_out_list=[
(
"/gripper_controller/grasp_monitor/event_out",
"e_object_grasped",
True,
)
],
timeout_duration=5,
),
transitions={
"success": "OVERALL_SUCCESS",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
sm.register_transition_cb(transition_cb)
sm.register_start_cb(start_cb)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer(
"pick_object_smach_viewer", sm, "/PICK_OBJECT_SMACH_VIEWER"
)
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="wbc_pick_object_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
return res.labels
if __name__ == '__main__':
rospy.init_node('smach_example')
# Create and start the introspection server (to be able to display the state machine using
# rosrun smach_viewer smach_viewer.py)
infomax_sm = StateMachine(outcomes=['succeeded','aborted','preempted'],
input_keys=['do_infomax_goal'],
output_keys=['do_infomax_result','do_infomax_feedback'])
infomax_sm.userdata.sm_action_index = 0
infomax_sm.userdata.do_infomax_feedback = DoInfomaxFeedback()
infomax_sm.userdata.do_infomax_result = DoInfomaxResult()
sis = IntrospectionServer('infomax_machine', infomax_sm, '/INFOMAX_MACHINE')
t = Thread(target=sis.start)
t.start()
with infomax_sm:
find_clusters_sm = create_find_clusters_sm()
StateMachine.add('FIND_CLUSTERS',
find_clusters_sm,
transitions={'succeeded':'SELECT_CLUSTER',
'aborted':'FAIL_RESET_ROBOT'},
remapping={'segmentation_result':'segmentation_result',
'cluster_information':'cluster_information',
'action_index':'sm_action_index'})
@smach.cb_interface(input_keys=['segmentation_result', 'cluster_information'],
output_keys=['closest_index'],
def __init__(self, ordenARealizar):
"""Constructor of the Main class
Args:
ordenARealizar (number): Go from the table to the one you go to
"""
self.ordenARealizar = ordenARealizar
rospy.loginfo("Init con ")
rospy.loginfo(ordenARealizar)
maquinaEstadosNavegacion = StateMachine(
outcomes=['succeeded', 'aborted'])
# maquinaEstadosNavegacion.userdata.chargeInput = 1
maquinaEstadosNavegacion.userdata.ordenARealizar = self.ordenARealizar
# global ordenARealizar
with maquinaEstadosNavegacion:
# Estado power_on
StateMachine.add('POWER_ON',
PowerOnRobot(),
transitions={
'succeeded': 'WAITING_ORDER',
'aborted': 'aborted'
})
# Estado esperar orden
StateMachine.add('WAITING_ORDER',
WaitingOrder(),
transitions={
'mesa1': waypoints[0][0],
'mesa2': waypoints[1][0],
'mesa3': waypoints[2][0],
'mesa4': waypoints[3][0],
'mesa5': waypoints[4][0],
'cocina': waypoints[5][0],
'aborted': 'WAITING_ORDER'
},
remapping={
'input': 'ordenARealizar',
'output': ''
})
# Estado mesas
# MESA 1
StateMachine.add(waypoints[0][0],
Navigate(waypoints[0][1], waypoints[0][2],
waypoints[0][0]),
transitions={
'succeeded': 'succeeded',
'aborted': waypoints[5][0]
})
# MESA 2
StateMachine.add(waypoints[1][0],
Navigate(waypoints[1][1], waypoints[1][2],
waypoints[1][0]),
transitions={
'succeeded': 'succeeded',
'aborted': waypoints[5][0]
})
# MESA 3
StateMachine.add(waypoints[2][0],
Navigate(waypoints[2][1], waypoints[2][2],
waypoints[2][0]),
transitions={
'succeeded': 'succeeded',
'aborted': waypoints[5][0]
})
# MESA 4
StateMachine.add(waypoints[3][0],
Navigate(waypoints[3][1], waypoints[3][2],
waypoints[3][0]),
transitions={
'succeeded': 'succeeded',
'aborted': waypoints[5][0]
})
# MESA 5
StateMachine.add(waypoints[4][0],
Navigate(waypoints[4][1], waypoints[4][2],
waypoints[4][0]),
transitions={
'succeeded': 'succeeded',
'aborted': waypoints[5][0]
})
# Cocina
StateMachine.add(waypoints[5][0],
Navigate(waypoints[5][1], waypoints[5][2],
waypoints[5][0]),
transitions={
'succeeded': 'succeeded',
'aborted': 'WAITING_ORDER'
})
rospy.loginfo("Statemachine.add done")
intro_server = IntrospectionServer('Coockbot',
maquinaEstadosNavegacion,
'/SM_ROOT')
intro_server.start() # iniciamos el servidor
maquinaEstados_ejecucion = maquinaEstadosNavegacion.execute(
) # ejecutamos la maquina de estados
intro_server.stop()
def main():
rospy.init_node('tinker_mission_manipulation')
trans = tf.TransformListener()
rospy.loginfo("Waiting for tf ...")
rospy.sleep(3)
assert (len(trans.getFrameStrings()) > 0)
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
def kinect_callback(userdata, result):
userdata.objects = []
objects = []
sum_x = 0
for obj in result.objects.objects:
position = obj.pose.pose.pose.position
if position.y > 0.5 or position.y < -0.5:
continue
_levels = [0.10, 0.44, 0.78, 1.12, 1.46, 1.80]
for l in _levels:
if fabs(l - position.z) < 0.05:
position.z = l + 0.05
if position.z > 1.1:
position.z += 0.05
obj.header.stamp = rospy.Time(0)
kinect_point = PointStamped(header=obj.header, point=position)
odom_point = trans.transformPoint('odom', kinect_point)
sum_x += odom_point.point.x
rospy.loginfo(
colored('[Kinect Object(odom)] from:(%f %f %f)', 'yellow'),
odom_point.point.x, odom_point.point.y, odom_point.point.z)
objects.append(odom_point)
avg_x = sum_x / len(objects)
for from_point in objects:
to_point = copy.deepcopy(from_point)
to_point.point.x = avg_x
to_point.point.z = find_div(from_point.point.z)
userdata.objects.append({'from': from_point, 'to': to_point})
rospy.loginfo(
colored('[Kinect Object(odom)] to:(%f %f %f)', 'yellow'),
to_point.point.x, to_point.point.y, to_point.point.z)
rospy.loginfo(colored('Total Object: %d', 'green'), len(objects))
return 'succeeded'
StateMachine.add('Arm_Mode_Kinect',
ArmModeState(ArmModeState.Arm_Mode_Kinect),
transitions={'succeeded': 'Start_Button'})
StateMachine.add('Start_Button',
MonitorStartButtonState(),
transitions={
'valid': 'Start_Button',
'invalid': 'S_Kinect_Recognition'
})
StateMachine.add('S_Kinect_Recognition',
ServiceState(service_name='/kinect_find_objects',
service_spec=FindObjects,
input_keys=['objects'],
output_keys=['objects'],
response_cb=kinect_callback),
transitions={'succeeded': 'Generate_Report'})
StateMachine.add('Generate_Report',
GenerateReportState(image='result.png',
text='object_names.txt'),
transitions={'succeeded': 'IT_Objects_Iterator'})
objects_iterator = Iterator(
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['objects'],
output_keys=[],
it=lambda: state.userdata.objects,
it_label='target',
exhausted_outcome='succeeded')
with objects_iterator:
fetch_object_sequence = Sequence(
outcomes=['succeeded', 'aborted', 'continue', 'preempted'],
input_keys=['target'],
connector_outcome='succeeded')
with fetch_object_sequence:
Sequence.add('Speak', SpeakState('New Object recognized'))
Sequence.add('Gripper_Photo',
GripperState(GripperState.GRIPPER_OPEN))
Sequence.add('Move_For_Photo',
MoveArmState(Point(-0.7, 0, 0),
target_key='from'),
transitions={'aborted': 'continue'})
concurrence = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
child_termination_cb=lambda x: True,
input_keys=['target'])
with concurrence:
Concurrence.add(
'Move_Fetch',
MoveArmState(Point(0.06, 0, 0), target_key='from'))
Concurrence.add(
'Gripper_Laser_sensor',
MonitorState('/gripper_laser_sensor',
Bool,
cond_cb=lambda x, y: False))
Sequence.add('Move_Fetch_Concurrence', concurrence)
Sequence.add('Gripper_Fetch',
GripperState(GripperState.GRIPPER_CLOSE))
Sequence.add(
'Move_Fetch_Back',
MoveArmState(Point(-0.7, 0, 0), target_key='from'))
Sequence.add('Move_Down',
MoveArmState(Point(-0.6, 0, 0), target_key='to'))
Sequence.add('Move_Put',
MoveArmState(Point(0, 0, 0), target_key='to'))
Sequence.add('Gripper_Put',
GripperState(GripperState.GRIPPER_OPEN))
Sequence.add('Move_Put_Back',
MoveArmState(Point(-0.6, 0, 0), target_key='to'),
transitions={'succeeded': 'continue'})
Iterator.set_contained_state('Seq_Fetch_Object',
fetch_object_sequence,
loop_outcomes=['continue'])
# end of objects_iterator
StateMachine.add('IT_Objects_Iterator',
objects_iterator,
transitions={
'succeeded': 'A_Move_Reset',
'aborted': 'A_Move_Reset'
})
StateMachine.add('A_Move_Reset',
ArmModeState(ArmModeState.Arm_Mode_Init),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_manipulation', state,
'/tinker_mission_manipulation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def __init__(self):
rospy.init_node('clean_house', anonymous=False)
rospy.on_shutdown(self.shutdown)
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
self.move_base.wait_for_server(rospy.Duration(15))
rospy.loginfo("Connected to move_base action server")
quaternions = list()
euler_angles = (pi / 2, pi, 3 * pi / 2, 0)
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
# Create a list to hold the waypoint poses
self.waypoints = list()
self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(-1.0, -1.5, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(1.5, 1.0, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(2.0, -2.0, 0.0), quaternions[1]))
room_locations = (('table', self.waypoints[0]), ('delivery_area',
self.waypoints[1]),
('kitchen', self.waypoints[2]), ('home',
self.waypoints[3]))
# Store the mapping as an ordered dictionary so we can visit the rooms in sequence
self.room_locations = OrderedDict(room_locations)
nav_states = {}
for room in self.room_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.room_locations[room]
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(5.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states[room] = move_base_state
sm_order_confirmation = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with sm_order_confirmation:
StateMachine.add('ORDER_CONFIRMATION_PROCESS',
OrderConfirmation(1),
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
sm_restaurant = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with sm_restaurant:
StateMachine.add('POWER_ON',
PowerOnRobot(),
transitions={'succeeded': 'BUTTON_STATE'})
StateMachine.add('BUTTON_STATE',
ButtonState(1),
transitions={
'succeeded': 'GO_TO_CUSTOMER_TABLE',
'aborted': '',
'preempted': ''
})
StateMachine.add('GO_TO_CUSTOMER_TABLE',
nav_states['table'],
transitions={
'succeeded': 'ORDER_CONFIRMATION',
'aborted': 'ORDER_CONFIRMATION',
'preempted': 'ORDER_CONFIRMATION'
})
StateMachine.add('ORDER_CONFIRMATION',
sm_order_confirmation,
transitions={
'succeeded': 'GO_TO_DELIVERY_AREA',
'aborted': 'GO_TO_KITCHEN',
'preempted': 'GO_TO_KITCHEN'
})
#StateMachine.add('ORDER_CONFIRMATION', OrderConfirmation(1), transitions={'succeeded':'GO_TO_DELIVERY_AREA','aborted':'GO_TO_KITCHEN','preempted':'GO_TO_KITCHEN'})
StateMachine.add('GO_TO_DELIVERY_AREA',
nav_states['delivery_area'],
transitions={
'succeeded': 'DELIVER_FOOD',
'aborted': 'DELIVER_FOOD',
'preempted': 'DELIVER_FOOD'
})
StateMachine.add('GO_TO_KITCHEN',
nav_states['kitchen'],
transitions={
'succeeded': 'SPEAK_OUT',
'aborted': 'GO_TO_KITCHEN',
'preempted': 'GO_TO_KITCHEN'
})
StateMachine.add('SPEAK_OUT',
SpeakOut(1),
transitions={
'succeeded': 'GO_TO_DELIVERY_AREA',
'aborted': 'SPEAK_OUT',
'preempted': 'SPEAK_OUT'
})
StateMachine.add('DELIVER_FOOD',
nav_states['table'],
transitions={
'succeeded': 'GO_TO_HOME',
'aborted': 'GO_TO_HOME',
'preempted': 'GO_TO_HOME'
})
StateMachine.add('GO_TO_HOME',
nav_states['home'],
transitions={
'succeeded': 'BUTTON_STATE',
'aborted': 'GO_TO_HOME',
'preempted': 'GO_TO_HOME'
})
intro_server = IntrospectionServer('restaurant', sm_restaurant,
'/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = sm_restaurant.execute()
intro_server.stop()
def __init__(self):
rospy.init_node('clean_house', anonymous=False)
# 设置机器人关闭函数(stop the robot)
rospy.on_shutdown(self.shutdown)
# 初始化一些参数和变量
setup_task_environment(self)
# 将room locations转换成SMACH move_base action states
nav_states = {}
for room in self.room_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.room_locations[room]
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(15.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states[room] = move_base_state
''' Create individual state machines for assigning tasks to each room '''
# 为living room子任务创建一个状态机
sm_living_room = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 然后添加子任务
with sm_living_room:
StateMachine.add('VACUUM_FLOOR',
VacuumFloor('living_room', 5),
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
# 为kitchen子任务创建一个状态机
sm_kitchen = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 然后添加子任务
with sm_kitchen:
StateMachine.add('MOP_FLOOR',
MopFloor('kitchen', 5),
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
# 为bathroom子任务创建一个状态机
sm_bathroom = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 然后添加子任务
with sm_bathroom:
StateMachine.add('SCRUB_TUB',
ScrubTub('bathroom', 7),
transitions={'succeeded': 'MOP_FLOOR'})
StateMachine.add('MOP_FLOOR',
MopFloor('bathroom', 5),
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
# 为hallway子任务创建一个状态机
sm_hallway = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 然后添加子任务
with sm_hallway:
StateMachine.add('VACUUM_FLOOR',
VacuumFloor('hallway', 5),
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
# 初始化整个状态机
sm_clean_house = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 从nav状态机和room cleaning状态机构建clean house状态机
with sm_clean_house:
StateMachine.add('START',
nav_states['hallway'],
transitions={
'succeeded': 'LIVING_ROOM',
'aborted': 'LIVING_ROOM',
'preempted': 'LIVING_ROOM'
})
''' Add the living room subtask(s) '''
StateMachine.add('LIVING_ROOM',
nav_states['living_room'],
transitions={
'succeeded': 'LIVING_ROOM_TASKS',
'aborted': 'KITCHEN',
'preempted': 'KITCHEN'
})
# 当任务完成时, 继续进行kitchen任务
StateMachine.add('LIVING_ROOM_TASKS',
sm_living_room,
transitions={
'succeeded': 'KITCHEN',
'aborted': 'KITCHEN',
'preempted': 'KITCHEN'
})
''' Add the kitchen subtask(s) '''
StateMachine.add('KITCHEN',
nav_states['kitchen'],
transitions={
'succeeded': 'KITCHEN_TASKS',
'aborted': 'BATHROOM',
'preempted': 'BATHROOM'
})
# 当任务完成时, 继续进行bathroom任务
StateMachine.add('KITCHEN_TASKS',
sm_kitchen,
transitions={
'succeeded': 'BATHROOM',
'aborted': 'BATHROOM',
'preempted': 'BATHROOM'
})
''' Add the bathroom subtask(s) '''
StateMachine.add('BATHROOM',
nav_states['bathroom'],
transitions={
'succeeded': 'BATHROOM_TASKS',
'aborted': 'HALLWAY',
'preempted': 'HALLWAY'
})
# 当任务完成时, 继续进行hallway任务
StateMachine.add('BATHROOM_TASKS',
sm_bathroom,
transitions={
'succeeded': 'HALLWAY',
'aborted': 'HALLWAY',
'preempted': 'HALLWAY'
})
''' Add the hallway subtask(s) '''
StateMachine.add('HALLWAY',
nav_states['hallway'],
transitions={
'succeeded': 'HALLWAY_TASKS',
'aborted': '',
'preempted': ''
})
# 当任务完成时, stop
StateMachine.add('HALLWAY_TASKS',
sm_hallway,
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
# 创建并开始SMACH introspection server
intro_server = IntrospectionServer('clean_house', sm_clean_house,
'/SM_ROOT')
intro_server.start()
# 运行状态机
sm_outcome = sm_clean_house.execute()
if len(task_list) > 0:
message = "Ooops! Not all chores were completed."
message += "The following rooms need to be revisited: "
message += str(task_list)
else:
message = "All chores complete!"
rospy.loginfo(message)
easygui.msgbox(message, title="Finished Cleaning")
intro_server.stop()
def main():
rospy.init_node("place_object_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal", "feedback", "result"],
output_keys=["feedback", "result"],
)
with sm:
# add states to the container
smach.StateMachine.add(
"CHECK_IF_SHELF_INITIAL",
CheckIfLocationIsShelf(),
transitions={
"shelf": "MOVE_ARM_TO_SHELF_INTERMEDIATE",
"not_shelf": "MOVE_ARM_TO_PRE_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_SHELF_INTERMEDIATE",
gms.move_arm("shelf_intermediate"),
transitions={
"succeeded": "MOVE_ARM_TO_SHELF_INTERMEDIATE_2",
"failed": "MOVE_ARM_TO_SHELF_INTERMEDIATE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_SHELF_INTERMEDIATE_2",
gms.move_arm("shelf_intermediate_2"),
transitions={
"succeeded": "MOVE_ARM_TO_PRE_GRASP_LOWER",
"failed": "MOVE_ARM_TO_SHELF_INTERMEDIATE_2",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_PRE_GRASP_LOWER",
gms.move_arm("shelf_pre_grasp_lower"),
transitions={
"succeeded": "MOVE_ARM_TO_SHELF_PLACE_FINAL",
"failed": "MOVE_ARM_TO_PRE_GRASP_LOWER",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_SHELF_PLACE_FINAL",
gms.move_arm("shelf_place_final"),
transitions={
"succeeded": "OPEN_GRIPPER_SHELF",
"failed": "MOVE_ARM_TO_SHELF_PLACE_FINAL",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER_SHELF",
gms.control_gripper("open"),
transitions={"succeeded": "MOVE_ARM_TO_SHELF_PLACE_FINAL_RETRACT"},
)
smach.StateMachine.add(
"MOVE_ARM_TO_SHELF_PLACE_FINAL_RETRACT",
gms.move_arm("shelf_place_final"),
transitions={
"succeeded": "MOVE_ARM_TO_PRE_GRASP_LOWER_RETRACT",
"failed": "MOVE_ARM_TO_SHELF_PLACE_FINAL_RETRACT",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_PRE_GRASP_LOWER_RETRACT",
gms.move_arm("shelf_pre_grasp_lower"),
transitions={
"succeeded": "MOVE_ARM_TO_SHELF_INTERMEDIATE_2_RETRACT",
"failed": "MOVE_ARM_TO_PRE_GRASP_LOWER_RETRACT",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_SHELF_INTERMEDIATE_2_RETRACT",
gms.move_arm("shelf_intermediate_2"),
transitions={
"succeeded": "MOVE_ARM_TO_SHELF_INTERMEDIATE_RETRACT",
"failed": "MOVE_ARM_TO_SHELF_INTERMEDIATE_2_RETRACT",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_SHELF_INTERMEDIATE_RETRACT",
gms.move_arm("shelf_intermediate"),
transitions={
"succeeded": "MOVE_ARM_TO_NEUTRAL",
"failed": "MOVE_ARM_TO_SHELF_INTERMEDIATE_RETRACT",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_PRE_PLACE",
gms.move_arm("look_at_turntable"),
transitions={
"succeeded": "START_PLACE_POSE_SELECTOR",
"failed": "MOVE_ARM_TO_PRE_PLACE",
},
)
smach.StateMachine.add(
"START_PLACE_POSE_SELECTOR",
gbs.send_event([("/mcr_perception/place_pose_selector/event_in",
"e_start")]),
transitions={"success": "GET_POSE_TO_PLACE_OBJECT"},
)
smach.StateMachine.add(
"GET_POSE_TO_PLACE_OBJECT",
GetPoseToPlaceOject(
"/mcr_perception/place_pose_selector/platform_name",
"/mcr_perception/place_pose_selector/place_pose",
"/mcr_perception/place_pose_selector/event_out",
15.0,
),
transitions={
"succeeded": "MOVE_ARM_TO_PLACE_OBJECT",
"failed": "MOVE_ARM_TO_DEFAULT_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_DEFAULT_PLACE",
gms.move_arm("15cm/pose4"),
transitions={
"succeeded": "OPEN_GRIPPER",
"failed": "MOVE_ARM_TO_DEFAULT_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_PLACE_OBJECT",
gms.move_arm(),
transitions={
"succeeded": "OPEN_GRIPPER",
"failed": "OPEN_GRIPPER",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open"),
transitions={"succeeded": "STOP_PLACE_POSE_SELECTOR"},
)
smach.StateMachine.add(
"STOP_PLACE_POSE_SELECTOR",
gbs.send_event([("/mcr_perception/place_pose_selector/event_in",
"e_stop")]),
transitions={"success": "MOVE_ARM_TO_NEUTRAL"},
)
smach.StateMachine.add(
"MOVE_ARM_TO_NEUTRAL",
gms.move_arm("barrier_tape"),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "MOVE_ARM_TO_NEUTRAL",
},
)
sm.register_transition_cb(transition_cb)
sm.register_start_cb(start_cb)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer("place_object_smach_viewer", sm,
"/STAGE_OBJECT_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="place_object_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('smach_usecase_step_05')
# Construct static goals
polygon_big = turtle_actionlib.msg.ShapeGoal(edges=4, radius=1)
polygon_small = turtle_actionlib.msg.ShapeGoal(edges=3, 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(
0, 0, 0.0)), {'succeeded': 'TELEPORT2'})
# Teleport turtle 2
StateMachine.add(
'TELEPORT2',
ServiceState('turtle2/teleport_absolute',
turtlesim.srv.TeleportAbsolute,
request=turtlesim.srv.TeleportAbsoluteRequest(
0, 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
Concurrence.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):
rospy.init_node('patrol_smach_concurrence', anonymous=False)
# 设置关闭机器人函数(stop the robot)
rospy.on_shutdown(self.shutdown)
# 初始化一些参数和变量
setup_task_environment(self)
# 跟踪到达目标位置的成功率
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
# 保存上一个或者当前的导航目标点的变量
self.last_nav_state = None
# 指示是否正在充电的标志
self.recharging = False
# 保存导航目标点的列表
nav_states = list()
# 把waypoints变成状态机的状态
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
# 为扩展底座（docking station）创建一个MoveBaseAction state
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.docking_station_pose
nav_docking_station = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(20.0),
server_wait_timeout=rospy.Duration(10.0))
# 初始化导航的状态机
self.sm_nav = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 使用transitions将导航的状态添加到状态机
with self.sm_nav:
StateMachine.add('NAV_STATE_0',
nav_states[0],
transitions={
'succeeded': 'NAV_STATE_1',
'aborted': 'NAV_STATE_1'
})
StateMachine.add('NAV_STATE_1',
nav_states[1],
transitions={
'succeeded': 'NAV_STATE_2',
'aborted': 'NAV_STATE_2'
})
StateMachine.add('NAV_STATE_2',
nav_states[2],
transitions={
'succeeded': 'NAV_STATE_3',
'aborted': 'NAV_STATE_3'
})
StateMachine.add('NAV_STATE_3',
nav_states[3],
transitions={
'succeeded': 'NAV_STATE_4',
'aborted': 'NAV_STATE_4'
})
StateMachine.add('NAV_STATE_4',
nav_states[0],
transitions={
'succeeded': '',
'aborted': ''
})
# 在sm_nav状态机中注册一个回调函数以启动状态转换（state transitions）
self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[])
# 初始化充电的状态机
self.sm_recharge = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_recharge:
StateMachine.add('NAV_DOCKING_STATION',
nav_docking_station,
transitions={'succeeded': 'RECHARGE_BATTERY'})
StateMachine.add('RECHARGE_BATTERY',
ServiceState(
'battery_simulator/set_battery_level',
SetBatteryLevel,
100,
response_cb=self.recharge_cb),
transitions={'succeeded': ''})
# 使用并发容器（Concurrence container）创建nav_patrol状态机
self.nav_patrol = Concurrence(
outcomes=['succeeded', 'recharge', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# 将sm_nav machine和battery MonitorState添加到nav_patrol状态机里面
with self.nav_patrol:
Concurrence.add('SM_NAV', self.sm_nav)
Concurrence.add(
'MONITOR_BATTERY',
MonitorState("battery_level", Float32, self.battery_cb))
# 创建顶层状态机
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 将nav_patrol,sm_recharge和Stop添加到sm_top状态机
with self.sm_top:
StateMachine.add('PATROL',
self.nav_patrol,
transitions={
'succeeded': 'PATROL',
'recharge': 'RECHARGE',
'stop': 'STOP'
})
StateMachine.add('RECHARGE',
self.sm_recharge,
transitions={'succeeded': 'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})
# 创建并开始SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# 运行状态机
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
'TORSO_SETUP',
SimpleActionState( 'torso_controller/position_joint_action',
SingleJointPositionAction,
goal = tgoal),
transitions = { 'succeeded': 'succeeded' })
sm.execute()
if __name__ == '__main__':
# if False:
rospy.init_node('smach_aware_home')
sm = cousins_demo()
sis = IntrospectionServer('sm_aware_home', sm, '/SM_AWARE_HOME')
sis.start()
rospy.logout( 'READY TO RUN AWARE_HOME DEMO' )
sm.userdata.person_id = 'person '
# sm.userdata.explore_radius = 1.5
# sm.userdata.explore_rfid_reads = []
outcome = sm.execute()
# 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(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':'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
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('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))
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()
##################################
# Construct action server wrapper
asw = ActionServerWrapper(
'my_action_server_name', MachineAction,
wrapped_container = sm0,
succeeded_outcomes = ['did_something','did_something_else'],
aborted_outcomes = ['aborted'],
preempted_outcomes = ['preempted'] )
# Run the server in a background thread
asw.run_server()
# Define transit state
transit = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
# path_client(self, x_0, y_0, x_1, y_1, u_d, R):
_goal1 = los.path_client(-2, -2, 15, 3, 0.2, -0.5, 0.5)
_goal2 = los.path_client(15, 3, 20, -2, 0.2, -0.5, 0.5)
# State machine
with transit:
StateMachine.add('OPEN_LOOP', ControlMode(OPEN_LOOP), transitions={'success':'transit'})
StateMachine.add('transit', SimpleActionState('los_path',LosPathFollowingAction, goal=_goal1), transitions={'succeeded':'goal2'})
StateMachine.add('goal2', SimpleActionState('los_path',LosPathFollowingAction, goal=_goal2), transitions={'succeeded':'succeeded'})
# Define outcomes
shapes = StateMachine(outcomes=['succeeded', 'preempted', 'aborted', 'completed'])
# Creating a hierarchical state machine nesting several sm's
with shapes:
StateMachine.add('terminal', patrol, transitions={'succeeded':'square'})
StateMachine.add('square', square, transitions={'succeeded':'transit'})
StateMachine.add('transit', transit, transitions={'succeeded':'aborted'})
sis = IntrospectionServer(str(rospy.get_name()), shapes, '/SM_ROOT' + str(rospy.get_name()))
sis.start()
shapes.execute()
rospy.spin()
sis.stop()
smach.StateMachine.add(
'DELIVERY', # outcomes: succeeded, aborted, preempted
sm_delivery,
remapping = { 'tagid' : 'person_id'}, #input
transitions = { 'succeeded': 'succeeded' })
return sm
if __name__ == '__main__':
rospy.init_node('smach_rfid_explore')
sm = cousins_demo()
sis = IntrospectionServer('sm_cousins_demo', sm, '/SM_COUSINS_DEMO')
sis.start()
rospy.logout( 'READY TO RUN COUSINS DEMO' )
sm.userdata.explore_id = ''
sm.userdata.obj_id = 'SpectrMedBot'
sm.userdata.person_id = 'person '
sm.userdata.explore_radius = 1.8
# sm.userdata.explore_rfid_reads = []
outcome = sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('perceive_cavity_server')
# Construct state machine
sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['goal'],
output_keys=['feedback', 'result'])
sm.userdata.next_arm_pose_index = 0
# Open the container
with sm:
# approach to platform
smach.StateMachine.add(
'SETUP',
Setup(),
transitions={'succeeded': 'PUBLISH_REFERENCE_FRAME_FOR_WBC'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
'PUBLISH_REFERENCE_FRAME_FOR_WBC',
gbs.send_event([('/static_transform_publisher_node/event_in',
'e_start')]),
transitions={'success': 'START_POSE_SELECTOR'})
# start the cavity pose selector to accumulate the poses
smach.StateMachine.add(
'START_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/cavity_pose_selector/event_in',
'e_start')]),
transitions={'success': 'LOOK_AROUND'})
# move arm to selected pose
smach.StateMachine.add('LOOK_AROUND',
gms.move_arm('look_at_workspace_from_near',
blocking=True),
transitions={
'succeeded': 'RECOGNIZE_CAVITIES',
'failed': 'LOOK_AROUND'
})
# trigger perception pipeline
smach.StateMachine.add('RECOGNIZE_CAVITIES',
gps.find_cavities(retries=1),
transitions={
'cavities_found':
'POPULATE_RESULT_WITH_CAVITIES',
'no_cavities_found': 'OVERALL_FAILED'
})
# populate action server result with perceived objects
smach.StateMachine.add('POPULATE_RESULT_WITH_CAVITIES',
PopulateResultWithCavities(),
transitions={'succeeded': 'OVERALL_SUCCESS'})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = IntrospectionServer('perceive_cavity_smach_viewer', sm,
'/PERCEIVE_CAVITY_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='perceive_cavity_server',
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='goal',
feedback_key='feedback',
result_key='result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
transitions={'succeeded': 'succeeded'})
return sm
if __name__ == '__main__':
import optparse
p = optparse.OptionParser()
p.add_option('--tag',
action='store',
type='string',
dest='tagid',
help='Tagid to approach',
default='person ')
opt, args = p.parse_args()
print 'SERVO APPROACH to ID: \'%s\'' % (opt.tagid)
rospy.init_node('smach_example_state_machine')
sm = sm_rfid_servo_approach()
sis = IntrospectionServer('RFID_servo_approach', sm,
'/SM_RFID_SERVO_APPROACH')
sis.start()
sm.userdata.tagid = opt.tagid
outcome = sm.execute()
sis.stop()
def start_tasks(
list_tasks
): #En list_tasks vienen los nombres [carniceria, pescaderia, cajero]
sm_rito = StateMachine(outcomes=['succeeded', 'aborted'])
sm_rito.userdata.sm_input = 1
cont = 0
with sm_rito:
StateMachine.add('POWER_ON',
PowerOnRobot(),
transitions={
'succeeded': 'WAITING_ORDER',
'aborted': 'aborted'
})
if len(list_tasks) > 0:
StateMachine.add('WAITING_ORDER',
WaitingOrder(1),
transitions={
'succeeded':
waypoints_dict[list_tasks[0]]['tag'],
'aborted': 'WAITING_ORDER'
})
for i in list_tasks:
cont = cont + 1
if cont + 1 > len(list_tasks):
StateMachine.add(waypoints_dict[i]['tag'],
Navigate(waypoints_dict[i]['coord'],
waypoints_dict[i]['dir'],
waypoints_dict[i]['tag']),
transitions={
'succeeded': 'SCAN-' + str(cont),
'aborted': 'WAITING_ORDER'
})
StateMachine.add(waypoints_dict[i]['function'],
waypoints_dict[i]['class'],
transitions={
'succeeded': 'SCAN-' + str(cont),
'aborted': 'aborted'
})
StateMachine.add('SCAN-' + str(cont),
Scan(),
transitions={
'succeeded': 'GO_TO_CAJERO',
'aborted': 'WAITING_ORDER'
})
StateMachine.add('GO_TO_CAJERO',
Navigate(
waypoints_dict['cajero']['coord'],
waypoints_dict['cajero']['dir'],
waypoints_dict['cajero']['tag']),
transitions={
'succeeded':
waypoints_dict['centro']['tag'],
'aborted':
'WAITING_ORDER'
})
StateMachine.add('IR_CAJERO',
IrCajero(),
transitions={
'succeeded':
waypoints_dict['centro']['tag'],
'aborted':
'aborted'
})
StateMachine.add(waypoints_dict['centro']['tag'],
Navigate(
waypoints_dict['centro']['coord'],
waypoints_dict['centro']['dir'],
waypoints_dict['centro']['tag']),
transitions={
'succeeded': 'CHARGE',
'aborted': 'WAITING_ORDER'
})
StateMachine.add('CHARGE',
Charge(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
},
remapping={
'input': 'sm_input',
'output': ''
})
else:
StateMachine.add(waypoints_dict[i]['tag'],
Navigate(waypoints_dict[i]['coord'],
waypoints_dict[i]['dir'],
waypoints_dict[i]['tag']),
transitions={
'succeeded': 'SCAN-' + str(cont),
'aborted': 'WAITING_ORDER'
})
StateMachine.add(waypoints_dict[i]['function'],
waypoints_dict[i]['class'],
transitions={
'succeeded': 'SCAN-' + str(cont),
'aborted': 'WAITING_ORDER'
})
StateMachine.add(
'SCAN-' + str(cont),
Scan(),
transitions={
'succeeded':
waypoints_dict[list_tasks[cont]]['tag'],
'aborted': 'WAITING_ORDER'
})
# En vez de Keep Clothes, es la funcion en cada caso
intro_server = IntrospectionServer('rito_machine', sm_rito, '/SM_ROOT')
intro_server.start()
#Ejecutamos la maquina de estados
sm_outcome = sm_rito.execute()
intro_server.stop()
def __init__(self):
rospy.init_node('Shopping')
self.smach_bool = False
rospy.on_shutdown(self.shutdown)
self.get_command = StateMachine(outcomes=['succeeded', 'aborted'])
self.sm_EnterRoom = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_EnterRoom:
# rospy.logerr('sm_EnterRoom add State')342
# arm go to the nav_pose in the srdf
# self.sm_EnterRoom.userdata.arm_mode =0
# self.sm_EnterRoom.userdata.arm_ps_1 = PointStamped()
# StateMachine.add('NAV_POSE',
# ArmCmd(),
# transitions={'succeeded':'DOOR_DETECT','aborted':'NAV_POSE','error':'error'},
# remapping ={'mode':'arm_mode','arm_ps':'arm_ps_1'})
# wait the door to open
# StateMachine.add('DOOR_DETECT',
# DoorDetect().door_detect_,
# transitions={'invalid':'WAIT','valid':'DOOR_DETECT','preempted':'error'})
# simple delay 5s
self.sm_EnterRoom.userdata.rec = 5.0
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'SIMPLE_MOVE',
'error': 'error'
},
remapping={'rec': 'rec'})
self.sm_EnterRoom.userdata.point = Point(1.5, 0.0, 0.0)
StateMachine.add('SIMPLE_MOVE',
SimpleMove_move(),
transitions={
'succeeded': 'NAV_1',
'aborted': 'NAV_1',
'error': 'error'
},
remapping={'point': 'point'})
# navstack to room of crowds
# waypoints are the list of Pose fit the data type need
self.sm_EnterRoom.userdata.start_waypoint = gpsr_target['speaker'][
'pos']
# self.sm_EnterRoom.userdata.nav_mode =0
StateMachine.add('NAV_1',
NavStack(),
transitions={
'succeeded': 'succeeded',
'aborted': 'NAV_1',
'error': 'error'
},
remapping={'pos_xm': 'start_waypoint'})
# self-introduce
# self.sm_EnterRoom.userdata.sentences_2 = 'I am robot xiaomeng, I come from the nwpu'
# StateMachine.add('SELF_INTRO',
# Speak(),
# remapping ={'sentences':'sentences_2'},
# transitions ={'succeeded':'succeeded','aborted':'SELF_INTRO','error':'error'})
with self.get_command:
StateMachine.add('GET_FOLLOW',
GetSignal(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
self.trace = Concurrence(outcomes=['remeber', 'stop', 'aborted'],
default_outcome='stop',
outcome_map={
'remeber': {
'STOP': 'remeber'
},
'stop': {
'STOP': 'stop'
},
'aborted': {
'FOLLOW': 'aborted'
}
},
child_termination_cb=self.trace_child_cb,
input_keys=['PT_LIST', 'mission'],
output_keys=['PT_LIST', 'mission'])
with self.trace:
self.meta_follow = StateMachine(
['succeeded', 'aborted', 'preempted'])
with self.meta_follow:
StateMachine.add('FIND',
FindPeople().find_people_,
transitions={
'invalid': 'META_NAV',
'valid': 'FIND',
'preempted': 'preempted'
},
remapping={'pos_xm': 'pos_xm'})
self.meta_nav = Concurrence(
outcomes=['time_over', 'get_pos', 'aborted'],
default_outcome='aborted',
outcome_map={
'time_over': {
'WAIT': 'succeeded'
},
'get_pos': {
'NAV': 'succeeded'
},
'aborted': {
'NAV': 'aborted'
}
},
child_termination_cb=self.nav_child_cb,
input_keys=['pos_xm'])
with self.meta_nav:
Concurrence.add('NAV',
NavStack(),
remapping={'pos_xm': 'pos_xm'})
Concurrence.add('WAIT', Wait_trace())
StateMachine.add('META_NAV',
self.meta_nav,
transitions={
'get_pos': 'FIND',
'time_over': 'FIND',
'aborted': 'FIND'
})
Concurrence.add('FOLLOW', self.meta_follow)
Concurrence.add('STOP',
CheckStop(),
remapping={
'PT_LIST': 'PT_LIST',
'mission': 'mission'
})
# self.get_mission = StateMachine(outcomes=['succeeded','aborted'],
# output_keys=['things_list'])
# with self.get_mission:
# self.get_mission.userdata.things_list = list()
# StateMachine.add('LISTEN',
# GetMission(),
# remapping = ['things_list'],
# transitions={'succeeded':'succeeded','aborted':'aborted'})
self.nav_mission = StateMachine(
outcomes=['succeeded', 'aborted', 'error'], input_keys=['mission'])
with self.nav_mission:
self.nav_mission.userdata.pos_xm = Pose()
StateMachine.add('GET_TARGET',
GetTarget(),
remapping={
'pos_xm': 'pos_xm',
'mission': 'mission',
'mission_name': 'mission_name'
},
transitions={
'succeeded': 'NAV',
'aborted': 'aborted',
'finish': 'NAV_FINAL'
})
StateMachine.add('NAV',
NavStack(),
remapping={'pos_xm': 'pos_xm'},
transitions={
'succeeded': 'GET_TARGET',
'aborted': 'aborted',
'error': 'error'
})
self.sm_Pick = StateMachine(
outcomes=['succeeded', 'aborted', 'error'],
input_keys=['name'])
with self.sm_Pick:
self.sm_Pick.userdata.name = ''
self.sm_Pick.userdata.target_mode = 0
self.sm_Pick.userdata.objmode = -1
StateMachine.add('RUNNODE_IMG',
RunNode_img(),
transitions={
'succeeded': 'GETNAME',
'aborted': 'RUNNODE_IMG'
})
self.sm_Pick.userdata.object_pos = PointStamped()
StateMachine.add('FIND_OBJECT',
FindObject(),
transitions={
'succeeded': 'POS_JUSTFY',
'aborted': 'FIND_OBJECT',
'error': 'SPEAK'
},
remapping={
'name': 'name',
'object_pos': 'object_pos',
'objmode': 'objmode'
})
#making the xm foreward the object may make the grasping task easier
self.sm_Pick.userdata.pose = Pose()
StateMachine.add('POS_JUSTFY',
PosJustfy(),
remapping={
'object_pos': 'object_pos',
'pose': 'pose'
},
transitions={
'succeeded': 'NAV_TO',
'aborted': 'aborted',
'error': 'error'
})
StateMachine.add('NAV_TO',
NavStack(),
transitions={
'succeeded': 'RUNNODE_IMG2',
'aborted': 'NAV_TO',
'error': 'error'
},
remapping={"pos_xm": 'pose'})
StateMachine.add('RUNNODE_IMG2',
RunNode_img(),
transitions={
'succeeded': 'FIND_AGAIN',
'aborted': 'RUNNODE_IMG2'
})
StateMachine.add('FIND_AGAIN',
FindObject(),
transitions={
'succeeded': 'PICK',
'aborted': 'FIND_AGAIN',
'error': 'SPEAK'
},
remapping={
'name': 'name',
'object_pos': 'object_pos',
'objmode': 'objmode'
})
self.sm_Pick.userdata.arm_mode_1 = 1
StateMachine.add('PICK',
ArmCmd(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'arm_ps': 'object_pos',
'mode': 'arm_mode_1'
})
self.sm_Pick.userdata.sentences = 'xiao meng can not find things'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
})
StateMachine.add('PICK_THING',
self.sm_Pick,
transitions={
'succeeded': 'GET_TARGET',
'aborted': 'aborted',
'error': 'error'
},
remapping={'name': 'mission_name'})
StateMachine.add('NAV_FINAL',
NavStack(),
remapping={'pos_xm': 'pos_xm'},
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
})
self.GetTask = StateMachine(outcomes=['succeeded', 'error', 'aborted'],
output_keys=['task'])
with self.GetTask:
self.FindPeo = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.FindPeo:
self.FindPeo.userdata.rec = 2.0
StateMachine.add(
'RUNNODE',
RunNode(),
transitions={
'succeeded': 'WAIT',
'aborted': 'RUNNODE'
}
) # On simulation , We need to skip RunNode ;So aborted -> Wait Or aborted->RunNode
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'GET_PEOPLE_POS',
'error': 'error'
},
remapping={'rec': 'rec'})
self.FindPeo.userdata.pos_xm = Pose()
StateMachine.add('GET_PEOPLE_POS',
FindPeople().find_people_,
transitions={
'invalid': 'NAV_PEOPLE',
'valid': 'GET_PEOPLE_POS',
'preempted': 'aborted'
},
remapping={'pos_xm': 'pos_xm'})
StateMachine.add('NAV_PEOPLE',
NavStack(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'NAV_PEOPLE',
'error': 'error',
'preempted': 'NAV_PEOPLE'
},
remapping={'pos_xm': 'pos_xm'})
self.FindPeo.userdata.sentences = 'I find you'
StateMachine.add('SPEAK',
SpeakSentence(),
transitions={
'succeeded': 'CLOSEKINECT',
'error': 'error'
},
remapping={'sentences': 'sentences'})
StateMachine.add('CLOSEKINECT',
CloseKinect(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
StateMachine.add('NAV_PEO',
self.FindPeo,
transitions={
'succeeded': 'ASK_TASK',
'aborted': 'ASK_TASK',
'error': 'error'
})
self.GetTask.userdata.sentence1 = 'what do you want?'
StateMachine.add('ASK_TASK',
Speak(),
transitions={
'succeeded': 'ANS1',
'aborted': 'aborted',
'error': 'error'
},
remapping={'sentence': 'sentence1'})
StateMachine.add('ANS1',
ShoppingGetTask(),
transitions={
'succeeded': 'ASK_TASK2',
'aborted': 'ASK_TAKS2'
},
remapping={'task': 'task'})
StateMachine.add('ASK_TASK2',
Speak(),
transitions={
'succeeded': 'ANS2',
'aborted': 'aborted',
'error': 'error'
},
remapping={'sentence': 'sentence1'})
StateMachine.add('ANS2',
ShoppingGetTask(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
},
remapping={'task': 'task'})
self.DealTask = StateMachine(outcomes=['succeeded', 'error', 'error'],
input_keys=['task', 'mission'])
with self.DealTask:
self.DealTask.userdata.nav_pos = Pose()
self.DealTask.userdata.indice = 0
self.DealTask.userdata.indice2 == 3
self.DealTask.userdata.name = ''
StateMachine.add('NXT_TASK',
ShoppingNextTask(),
transitions={
'go': 'NAV',
'back': 'NAV_CASH',
'finish': 'succeeded',
'aborted': "aborted"
},
remapping={
'pose': 'nav_pos',
'name': 'name',
'indice': 'indice'
})
StateMachine.add('NAV',
NavStack(),
transitions={
'succeeded': 'PICK',
'aborted': 'NAV',
'error': 'error'
},
remapping={'pos_xm', 'nav_pos'})
StateMachine.add('PICK',
self.sm_Pick,
transitions={
'succeeded': 'NXT_TASK',
'aborted': 'NXT_TASK',
'error': 'error'
},
remapping={'name': 'name'})
StateMachine.add('NAV_CASH',
NavStack(),
transitions={
'succeeded': 'PLACE',
'aborted': 'NAV_CASH',
'error': 'error'
},
remapping={'pos_xm', 'nav_pos'})
StateMachine.add('PLACE',
Place2(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
})
self.shopping = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.shopping:
# 字典名字与位置对应
self.shopping.userdata.PT_LIST = {}
self.shopping.userdata.mission = {}
self.shopping.userdata.task = list()
self.shopping.userdata.rec = 5.0
# StateMachine.add('ENTERROOM',
# self.sm_EnterRoom,
# transitions={'succeeded':'START','aborted':'aborted'})
StateMachine.add('START',
self.get_command,
transitions={
'succeeded': 'RUNNODE',
'aborted': 'aborted'
})
StateMachine.add('RUNNODE',
RunNode(),
transitions={
'succeeded': 'WAIT',
'aborted': 'aborted'
})
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'TRACE',
'error': 'error'
},
remapping={'rec': 'rec'})
StateMachine.add('TRACE',
self.trace,
transitions={
'remeber': 'TRACE',
'stop': 'GET_TASK',
'aborted': 'aborted'
},
remapping={
'PT_LIST': 'PT_LIST',
'mission': 'mission'
})
StateMachine.add('GET_TASK',
self.GetTask,
transitions={
'succeeded': 'DEAL_TASK',
'aborted': 'aborted',
'error': 'error'
},
remapping={'task': 'task'})
StateMachine.add('DEAL_TASK',
self.DealTask,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={'task': 'task'})
# StateMachine.add('NAV_MISSION',
# self.nav_mission,
# transitions = {'succeeded':'succeeded','aborted':'aborted','error':'error'},
# remapping={'mission':'mission'})
intro_server = IntrospectionServer('shoping', self.shopping, 'SM_ROOT')
intro_server.start()
out = self.shopping.execute()
intro_server.stop()
if out == 'succeeded':
self.smach_bool = True
def __init__(self):
rospy.init_node('GpsrSmach')
rospy.on_shutdown(self.shutdown)
rospy.logerr('you are all sillyb')
self.smach_bool = False
self.sm_EnterRoom = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_EnterRoom:
# if use arm, use this state
# self.sm_EnterRoom.userdata.arm_mode_1 =0
# self.sm_EnterRoom.userdata.arm_ps = PointStamped()
# StateMachine.add('NAV_POSE',
# ArmCmd(),
# transitions={'succeeded':'DOOR_DETECT','aborted':'NAV_POSE','error':'error'},
# remapping ={'arm_ps':'arm_ps','mode':'arm_mode_1'})
#first detect the door is open or not
StateMachine.add('DOOR_DETECT',
DoorDetect().door_detect_,
transitions={
'invalid': 'WAIT',
'valid': 'DOOR_DETECT',
'preempted': 'aborted'
})
# after clear the costmap, wait for some seconds
self.sm_EnterRoom.userdata.rec = 3.0
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'SIMPLE_MOVE',
'error': 'error'
},
remapping={'rec': 'rec'})
# before executing the nav-stack, move directly through the door may make the nav-stack easier
self.sm_EnterRoom.userdata.go_point = Point(1.5, 0.0, 0.0)
StateMachine.add('SIMPLE_MOVE',
SimpleMove(),
remapping={'point': 'go_point'},
transitions={
'succeeded': 'NAV_1',
'aborted': 'NAV_1',
'error': 'error'
})
# go to the known Pose in the map to get the gpsr-command
self.sm_EnterRoom.userdata.start_waypoint = gpsr_target['speaker'][
'pos']
StateMachine.add('NAV_1',
NavStack(),
transitions={
'succeeded': 'SPEAK1',
'aborted': 'NAV_1',
'error': 'error'
},
remapping={'pos_xm': 'start_waypoint'})
self.sm_EnterRoom.userdata.sentences = 'I arrived,please ask me a question'
StateMachine.add('SPEAK1',
Speak(),
transitions={
'succeeded': 'succeeded',
'error': 'error'
},
remapping={'sentences': 'sentences'})
self.sm_Find = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
# 寻找人
with self.sm_Find:
# for the people-finding task, pay attention that at the end of the task,
# run a simple executable file to make the cv-node release the KinectV2
# otherwise, the find-object task may throw error and all the cv-node may
# break down
self.sm_Person = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_Person:
self.sm_Person.userdata.rec = 2.0
# run the people_tracking node
StateMachine.add('RUNNODE',
RunNode(),
transitions={
'succeeded': 'WAIT',
'aborted': 'succeeded'
})
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'GET_PEOPLE_POS',
'error': 'error'
},
remapping={'rec': 'rec'})
#the data should be PointStamped()
# 这里必须先运行节点，也就是用RunNode()状态
self.sm_Person.userdata.pos_xm = Pose()
StateMachine.add('GET_PEOPLE_POS',
FindPeople().find_people_,
transitions={
'invalid': 'NAV_PEOPLE',
'valid': 'SPEAK',
'preempted': 'aborted'
},
remapping={'pos_xm': 'pos_xm'})
# this state will use the userdata remapping in the last state
StateMachine.add('NAV_PEOPLE',
NavStack(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'NAV_PEOPLE',
'error': 'error'
},
remapping={'pos_xm': 'pos_xm'})
self.sm_Person.userdata.sentences = 'I find you'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'CLOSEKINECT',
'error': 'error'
},
remapping={'sentences': 'sentences'})
# close the KinectV2
StateMachine.add('CLOSEKINECT',
CloseKinect(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
self.sm_Pos = StateMachine(
outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
with self.sm_Pos:
# this command may be foolish, but it may be used in the next competition
# get the name of the target
# in the target_gpsr.py we define some Pose() , they are all named as ${room_name}+'_table_1(2)'
# because each room we have some best detecting positions, we should make xm go to these places
# for object-find tasks
self.sm_Pos.userdata.pose = Pose()
self.sm_Pos.userdata.mode_1 = 1
StateMachine.add('GET_POS',
GetPos(),
remapping={
'target': 'target',
'current_task': 'current_task',
'pose': 'pose',
'mode': 'mode_1'
},
transitions={
'succeeded': 'NAV_HEHE',
'aborted': 'aborted',
'error': 'error'
})
StateMachine.add('NAV_HEHE',
NavStack(),
remapping={'pos_xm': 'pose'},
transitions={
'succeeded': 'GET_TARGET',
'aborted': 'NAV_HEHE',
'error': 'error'
})
self.sm_Pos.userdata.target_mode = 0
self.sm_Pos.userdata.name = ''
StateMachine.add('GET_TARGET',
GetTarget(),
remapping={
'target': 'target',
'current_task': 'current_task',
'mode': 'target_mode',
'current_target': 'name'
},
transitions={
'succeeded': 'FIND_OBJECT',
'aborted': 'aborted',
'error': 'error'
})
self.sm_Pos.userdata.object_pos = PointStamped()
StateMachine.add('FIND_OBJECT',
FindObject(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'GET_POS_1',
'error': 'error'
},
remapping={
'name': 'name',
'object_pos': 'object_pos'
})
# if xm not finds the object in the first detecting-place, she should go to the second
# specified place for detect-task, but as U can see in the target_gpsr.py, some room are
# only one best detecting-place
self.sm_Pos.userdata.pose_1 = Pose()
self.sm_Pos.userdata.mode_2 = 2
StateMachine.add('GET_POS_1',
GetPos(),
remapping={
'target': 'target',
'current_task': 'current_task',
'pose': 'pose_1',
'mode': 'mode_2'
},
transitions={
'succeeded': 'NAV_HAHA',
'aborted': 'aborted',
'error': 'error'
})
StateMachine.add('NAV_HAHA',
NavStack(),
remapping={'pos_xm': 'pose_1'},
transitions={
'succeeded': 'FIND_OBJECT_1',
'aborted': 'NAV_HAHA',
'error': 'error'
})
self.sm_Pos.userdata.object_pos = PointStamped()
StateMachine.add('FIND_OBJECT_1',
FindObject(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'SPEAK',
'error': 'error'
},
remapping={
'name': 'name',
'object_pos': 'object_pos'
})
self.sm_Pos.userdata.sentences = 'I find it'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'succeeded',
'error': 'error'
},
remapping={'sentences': 'sentences'})
# this state is used for swithing the mode, finding people or finding object?
StateMachine.add('PERSON_OR_POS',
PersonOrPosition(),
transitions={
'person': 'PERSON',
'position': 'POS',
'error': 'error'
},
remapping={
'target': 'target',
'current_task': 'current_task'
})
StateMachine.add('PERSON',
self.sm_Person,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
})
StateMachine.add('POS',
self.sm_Pos,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'target': 'target',
'current_task': 'current_task'
})
#nav tasks
self.sm_Nav = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
with self.sm_Nav:
self.sm_Nav.userdata.pos_xm = Pose()
self.sm_Nav.userdata.target_mode = 1
StateMachine.add('GETTARGET',
GetTarget(),
transitions={
'succeeded': "NAV_GO",
'aborted': 'aborted',
'error': 'error'
},
remapping={
'target': 'target',
'current_task': "current_task",
'current_target': 'pos_xm',
'mode': 'target_mode'
})
StateMachine.add('NAV_GO',
NavStack(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'NAV_GO',
'error': 'error'
},
remapping={'pos_xm': 'pos_xm'})
self.sm_Nav.userdata.sentences = 'I arrive here'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'succeeded',
'error': 'error'
},
remapping={'sentences': 'sentences'})
#follow task
# we will use the concurrence fun
# the tutorials in the wiki too easy, it is no help for you to understand the concurrence
# please see the test-snippet of the source of executive-smach
# https://github.com/ros/executive_smach/blob/indigo-devel/smach_ros/test/concurrence.py
self.sm_Follow = Concurrence(
outcomes=['succeeded', 'aborted', 'error'],
default_outcome='succeeded',
outcome_map={'succeeded': {
'STOP': 'succeeded'
}},
child_termination_cb=self.child_cb)
with self.sm_Follow:
self.meta_follow = StateMachine(outcomes=['succeeded', 'aborted'])
with self.meta_follow:
StateMachine.add('FOLLOW',
SimpleFollow(),
transitions={
'succeeded': 'FOLLOW',
'aborted': 'aborted',
'preempt': 'succeeded'
})
Concurrence.add('FOLLOW', self.meta_follow)
Concurrence.add('STOP', CheckStop())
Concurrence.add('RUNNODE', RunNode())
# self.meta_Follow = StateMachine(outcomes =['succeeded','aborted','error'])
# with self.meta_Follow:
# StateMachine.add('RUNNODE',
# RunNode(),
# transitions={'succeeded':'FIND_PEOPLE','aborted':'succeeded'})
# self.meta_Follow.userdata.pos_xm = Pose()
# StateMachine.add('FIND_PEOPLE',
# FindPeople().find_people_,
# remapping ={'pos_xm':'pos_xm'},
# transitions ={'invalid':'MOVE','valid':'FIND_PEOPLE','preempted':'aborted'})
# StateMachine.add('MOVE',
# NavStack(),
# remapping ={'pos_xm':'pos_xm'},
# transitions={'succeeded':'FIND_PEOPLE','aborted':'MOVE','error':'error'})
# self.meta_Stop = StateMachine(outcomes =['succeeded','aborted'])
# with self.meta_Stop:
# StateMachine.add('STOP',
# StopFollow(),
# transitions ={'succeeded':'succeeded','aborted':'STOP'})
# Concurrence.add('FOLLOW',
# self.meta_Follow)
# Concurrence.add('STOP',
# self.meta_Stop)
# speak task
self.sm_Talk = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.sm_Talk:
StateMachine.add('SPEAK',
Anwser(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
self.sm_Pick = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
with self.sm_Pick:
self.sm_Pick.userdata.name = ''
self.sm_Pick.userdata.target_mode = 0
StateMachine.add('GETNAME',
GetTarget(),
remapping={
'target': 'target',
'current_task': 'current_task',
'mode': 'target_mode',
'current_target': 'name'
},
transitions={
'succeeded': 'FIND_OBJECT',
'aborted': 'aborted',
'error': 'error'
})
self.sm_Pick.userdata.object_pos = PointStamped()
StateMachine.add('FIND_OBJECT',
FindObject(),
transitions={
'succeeded': 'POS_JUSTFY',
'aborted': 'FIND_OBJECT',
'error': 'error'
},
remapping={
'name': 'name',
'object_pos': 'object_pos'
})
#making the xm foreward the object may make the grasping task easier
self.sm_Pick.userdata.pose = Pose()
StateMachine.add('POS_JUSTFY',
PosJustfy(),
remapping={
'object_pos': 'object_pos',
'pose': 'pose'
},
transitions={
'succeeded': 'NAV_TO',
'aborted': 'aborted',
'error': 'error'
})
StateMachine.add('NAV_TO',
NavStack(),
transitions={
'succeeded': 'FIND_AGAIN',
'aborted': 'NAV_TO',
'error': 'error'
},
remapping={"pos_xm": 'pose'})
StateMachine.add('FIND_AGAIN',
FindObject(),
transitions={
'succeeded': 'PICK',
'aborted': 'FIND_AGAIN',
'error': 'error'
},
remapping={
'name': 'name',
'object_pos': 'object_pos'
})
self.sm_Pick.userdata.arm_mode_1 = 1
StateMachine.add('PICK',
ArmCmd(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'arm_ps': 'object_pos',
'mode': 'arm_mode_1'
})
self.sm_Place = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_Place:
# place_ps please specified due to the scene
self.sm_Place.userdata.place_ps = PointStamped()
self.sm_Place.userdata.place_ps.header.frame_id = 'base_link'
self.sm_Place.userdata.place_ps.point.x = 0.8
self.sm_Place.userdata.place_ps.point.y = 0.0
self.sm_Place.userdata.place_ps.point.z = 0.6
# without moveit, if is just place it in a open space
self.sm_Place.userdata.arm_mode_1 = 2
StateMachine.add('PLACE',
ArmCmd(),
transitions={
'succeeded': 'succeeded',
'aborted': 'PLACE',
'error': 'error'
},
remapping={
'arm_ps': 'arm_ps',
'mode': 'arm_mode_1'
})
self.sm_GPSR = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.sm_GPSR:
self.sm_GPSR.userdata.target = list()
self.sm_GPSR.userdata.action = list()
self.sm_GPSR.userdata.task_num = 0
self.sm_GPSR.userdata.current_task = -1
# get the task due to the speech-node
StateMachine.add('RECEIVE_TASKS',
GetTask(),
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'RECEIVE_TASKS',
'error': 'error'
},
remapping={
'target': 'target',
'action': 'action',
'task_num': 'task_num'
})
#get the task in order and execute the task
# if want to go to the door at the end of the task, please modify:
# succeeded->succeeded --> succeeded->BYEBYE
StateMachine.add('GET_NEXT_TASK',
NextDo(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error',
'find': 'FIND',
'go': 'GO',
'follow': 'FOLLOW',
'pick': 'GET_NEXT_TASK',
'place': 'GET_NEXT_TASK',
'talk': 'TALK',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'action': 'action',
'current_task': 'current_task',
'task_num': 'task_num'
})
# all the task-group
StateMachine.add('GO',
self.sm_Nav,
remapping={
'target': 'target',
'current_task': 'current_task'
},
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'GO'
})
StateMachine.add('FIND',
self.sm_Find,
remapping={
'target': 'target',
'current_task': 'current_task'
},
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'FIND'
})
StateMachine.add('FOLLOW',
self.sm_Follow,
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'FOLLOW'
})
StateMachine.add('PICK',
self.sm_Pick,
remapping={
'target': 'target',
'current_task': 'current_task'
},
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'PICK'
})
StateMachine.add('TALK',
self.sm_Talk,
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'TALK'
})
StateMachine.add('PLACE',
self.sm_Place,
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'PLACE'
})
self.sm_GPSR.userdata.string = 'I finish all the three tasks, I will go out the stage'
StateMachine.add('BYEBYE',
Speak(),
transitions={
'succeeded': 'GOOUT',
'error': 'error'
},
remapping={'sentences': 'string'})
#no need
self.sm_GPSR.userdata.waypoint = gpsr_target['end']['pos']
StateMachine.add('GOOUT',
NavStack(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={"pos_xm": 'waypoint'})
intro_server = IntrospectionServer('enter_room', self.sm_EnterRoom,
'/SM_ROOT')
intro_server.start()
out_1 = self.sm_EnterRoom.execute()
intro_server.stop()
#只运行一次
intro_server = IntrospectionServer('sm_gpsr', self.sm_GPSR, '/SM_ROOT')
intro_server.start()
out_2 = self.sm_GPSR.execute()
intro_server.stop()
self.smach_bool = True
def __init__(self):
rospy.init_node('ware_house', anonymous=False)
rospy.on_shutdown(self.shutdown)
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
self.move_base.wait_for_server(rospy.Duration(15))
rospy.loginfo("Connected to move_base action server")
#create a of angles
quaternions = list()
euler_angles = (90.2, 90.3, -89.7, -90, 180)
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
print(q)
# self.quaternions.append(0,0, 0.7862, 0.6179)
# self.quaternions.append(0,0, 0.7775, 0.6288)
# self.quaternions.append(0,0, -0.441, 0.8974)
# self.quaternions.append(0,0, -0.6639, 0.7478)
# Create a list to hold the waypoint poses
self.waypoints = list()
self.waypoints.append(Pose(Point(-6.61, -6.23, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(1.24, 12, -0.003), quaternions[1]))
self.waypoints.append(Pose(Point(7.52, 16.9, 0.0211), quaternions[2]))
self.waypoints.append(Pose(Point(7.52, -9.67, 0.0), quaternions[3]))
self.waypoints.append(
Pose(Point(-17.3676, -20.9895, 0.0), quaternions[4]))
room_locations = (('loading_point1', self.waypoints[0]),
('loading_point2', self.waypoints[1]),
('unloading_point1', self.waypoints[2]),
('unloading_point2', self.waypoints[3]),
('halting_point', self.waypoints[4]))
# Store the mapping as an ordered dictionary so we can visit the rooms in sequence
self.room_locations = OrderedDict(room_locations)
print(self.room_locations)
nav_states = {}
for room in self.room_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.room_locations[room]
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(23.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states[room] = move_base_state
sm_warehouse = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with sm_warehouse:
StateMachine.add('POWER_ON',
PowerOnRobot(),
transitions={'succeeded': 'LOADING_POINT1'})
StateMachine.add('LOADING_POINT1',
nav_states['loading_point1'],
transitions={
'succeeded': 'LOADING_POINT2',
'aborted': 'LOADING_POINT2',
'preempted': 'LOADING_POINT2'
})
StateMachine.add('LOADING_POINT2',
nav_states['loading_point2'],
transitions={
'succeeded': 'UNLOADING_POINT1',
'aborted': 'UNLOADING_POINT1',
'preempted': 'UNLOADING_POINT1'
})
StateMachine.add('UNLOADING_POINT1',
nav_states['unloading_point1'],
transitions={
'succeeded': 'UNLOADING_POINT2',
'aborted': 'UNLOADING_POINT2',
'preempted': 'UNLOADING_POINT2'
})
# StateMachine.add('WAITING_STATE1', WaitingState(1), transitions={'succeeded':''})
StateMachine.add('UNLOADING_POINT2',
nav_states['unloading_point2'],
transitions={
'succeeded': 'HALTING_POINT',
'aborted': 'HALTING_POINT',
'preempted': 'HALTING_POINT'
})
# StateMachine.add('WAITING_STATE2', WaitingState(1), transitions={'succeeded':''})
StateMachine.add('HALTING_POINT',
nav_states['halting_point'],
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
intro_server = IntrospectionServer('warehouse', sm_warehouse,
'/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = sm_warehouse.execute()
intro_server.stop()
def __init__(self):
rospy.init_node('patrol_smach_concurrence', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
# A variable to hold the last/current navigation goal
self.last_nav_state = None
# A flag to indicate whether or not we are rechargin
self.recharging = False
# A list to hold then navigation goa
nav_states = list()
# Turn the waypoints into SMACH states
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(40.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
# Create a MoveBaseAction state for the docking station
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.docking_station_pose
nav_docking_station = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(40.0),
server_wait_timeout=rospy.Duration(10.0))
# Initialize the navigation state machine
self.sm_nav = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add the nav states to the state machine with the appropriate transitions
with self.sm_nav:
StateMachine.add('NAV_STATE_0',
nav_states[0],
transitions={
'succeeded': 'NAV_STATE_1',
'aborted': 'NAV_STATE_1'
})
StateMachine.add('NAV_STATE_1',
nav_states[1],
transitions={
'succeeded': 'NAV_STATE_2',
'aborted': 'NAV_STATE_2'
})
StateMachine.add('NAV_STATE_2',
nav_states[2],
transitions={
'succeeded': 'NAV_STATE_3',
'aborted': 'NAV_STATE_3'
})
StateMachine.add('NAV_STATE_3',
nav_states[3],
transitions={
'succeeded': 'NAV_STATE_4',
'aborted': 'NAV_STATE_4'
})
StateMachine.add('NAV_STATE_4',
nav_states[0],
transitions={
'succeeded': '',
'aborted': ''
})
# Register a callback function to fire on state transitions within the sm_nav state machine
self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[])
# Initialize the recharge state machine
self.sm_recharge = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_recharge:
StateMachine.add('NAV_DOCKING_STATION',
nav_docking_station,
transitions={'succeeded': ''})
#StateMachine.add('RECHARGE_BATTERY', ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb),
#transitions={'succeeded':''})
# Create the nav_patrol state machine using a Concurrence container
self.nav_patrol = 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_nav machine and a battery MonitorState to the nav_patrol machine
with self.nav_patrol:
Concurrence.add('SM_NAV', self.sm_nav)
Concurrence.add(
'MONITOR_BATTERY',
MonitorState("battery_level", Float32, self.battery_cb))
# Create the top level state machine
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
StateMachine.add('PATROL',
self.nav_patrol,
transitions={
'succeeded': 'PATROL',
'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('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
executePlanAction,
goal_slots=['trajectory']),
transitions={
'succeeded': 'DROP_ARM_PLAN',
'aborted': 'DROP_ARM_PLAN',
'preempted': 'DROP_ARM_PLAN'
},
remapping={'trajectory': 'arm_plan'})
smach.StateMachine.add('OPEN_EEF',
OpenEef(),
transitions={
'open_eef_ok': 'RESET_POSITION',
'open_eef_error': 'OPEN_EEF'
})
smach.StateMachine.add('TEST_FAILED',
FailTest(),
transitions={'exit': 'test_failed'})
sis = IntrospectionServer('smach_introspection_server', sm,
'manipulation_smach')
sis.start()
outcome = sm.execute()
while not rospy.is_shutdown():
rospy.spin()
sis.stop()
def __init__(self):
rospy.init_node('patrol_smach', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
self.init()
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait up to 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move_base action server")
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
self.n_patrols = 2
# Turn the waypoints into SMACH states
nav_states = list()
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0))
# Initialize the top level state machine
self.sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm:
# Initialize the iterator
self.sm_patrol_iterator = Iterator(outcomes = ['succeeded','preempted','aborted'],
input_keys = [],
it = lambda: range(0, self.n_patrols),
output_keys = [],
it_label = 'index',
exhausted_outcome = 'succeeded')
with self.sm_patrol_iterator:
# Initialize the patrol state machine
self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted','continue'])
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1','preempted':'NAV_STATE_1'})
StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2','preempted':'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3','preempted':'NAV_STATE_3'})
StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4','preempted':'NAV_STATE_4'})
StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'continue','aborted':'continue','preempted':'continue'})
# Close the sm_patrol machine and add it to the iterator
Iterator.set_contained_state('PATROL_STATE', self.sm_patrol, loop_outcomes=['continue'])
# Close the top level state machine
StateMachine.add('PATROL_ITERATOR', self.sm_patrol_iterator, {'succeeded':'succeeded', 'aborted':'aborted'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
"FALTA POR IMPLEMENTAR MAQUINA DE ESTADOS DEL DETECTOR DE CARATULAS"
)
return "succeeded"
def getInstance(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['kid_name'])
with sm:
smach.StateMachine.add('SETUP',
Setup(robot),
transitions={'succeeded': 'DETECTOR'})
smach.StateMachine.add('DETECTOR',
CoverDetector(robot),
transitions={'succeeded': 'succeeded'})
return sm
if __name__ == '__main__':
rospy.init_node('COVERDETECTOR')
robot = robot_factory.build(["audition", "tts"], core=True)
sm = getInstance(robot)
sis = IntrospectionServer('cover_detector', sm,
'/EA_COVER_DETECTOR') #Smach Viewer
sis.start()
outcome = sm.execute()
sis.stop()
from state.stop import StoppedState
def make_realcourse_follow_state():
forward_speed = 0.4
line_filter = lambda hsv: cv2.inRange(hsv, np.array([0, 0, 200]),
np.array([255, 50, 255]))
stop_filter = (lambda hsv: cv2.inRange(hsv, np.asarray(
[0, 70, 50]), np.asarray([10, 255, 250])) | cv2.inRange(
hsv, np.asarray([170, 70, 50]), np.asarray([180, 255, 250])))
return LineFollowState(
forward_speed=forward_speed,
line_filter=line_filter,
stop_detector=FilterStopBehavior(stop_filter=stop_filter))
rospy.init_node('follower')
sm = StateMachine(outcomes=['ok', 'stop'])
with sm:
StateMachine.add('FOLLOW',
make_realcourse_follow_state(),
transitions={'stop': 'STOP'})
StateMachine.add('STOP', StoppedState(), transitions={'ok': 'FOLLOW'})
sis = IntrospectionServer('smach_server', sm, '/SM_ROOT')
sis.start()
sm.execute()
def __init__(self):
rospy.init_node('Follow')
rospy.on_shutdown(self.shutdown)
rospy.logerr('Let\'s go')
self.trace = Concurrence(outcomes=['succeeded', 'aborted'],
default_outcome='aborted',
outcome_map={
'succeeded': {
'STOP': 'stop'
},
'aborted': {
'FOLLOW': 'aborted'
},
'aborted': {
'FOLLOW': 'preempted'
}
},
child_termination_cb=self.child_cb,
input_keys=['people_id'])
with self.trace:
self.meta_follow = StateMachine(
['succeeded', 'aborted', 'preempted'],
input_keys=['people_id'])
with self.meta_follow:
self.meta_follow.userdata.pos_xm = Pose()
self.meta_follow.userdata.rec = 0.2
StateMachine.add('FIND',
LegTracker().tracker,
transitions={
'invalid': 'WAIT',
'valid': 'FIND',
'preempted': 'preempted'
},
remapping={
'pos_xm': 'pos_xm',
'people_id': 'people_id'
})
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'META_NAV',
'error': 'META_NAV'
})
self.meta_nav = Concurrence(
outcomes=['time_over', 'get_pos', 'aborted'],
default_outcome='aborted',
outcome_map={
'time_over': {
'WAIT': 'succeeded'
},
'get_pos': {
'NAV': 'succeeded'
},
'aborted': {
'NAV': 'aborted'
}
},
child_termination_cb=self.nav_child_cb,
input_keys=['pos_xm'])
with self.meta_nav:
Concurrence.add('NAV',
NavStack(),
remapping={'pos_xm': 'pos_xm'})
Concurrence.add('WAIT', Wait_trace())
StateMachine.add('META_NAV',
self.meta_nav,
transitions={
'get_pos': 'FIND',
'time_over': 'FIND',
'aborted': 'FIND'
})
Concurrence.add('FOLLOW', self.meta_follow)
Concurrence.add('STOP', CheckStop())
self.sm_GPSR = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.sm_GPSR:
self.sm_GPSR.userdata.target = list()
self.sm_GPSR.userdata.action = list()
self.sm_GPSR.userdata.task_num = 0
self.sm_GPSR.userdata.current_task = -1
self.sm_GPSR.userdata.people_id = -1
# StateMachine.add('RUNNODE',RunNode(),
# transitions={'succeeded':'XM','aborted':'aborted'})
StateMachine.add(
'XM',
self.trace,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
#'preempted':'error'
})
intro_server = IntrospectionServer('sm_gpsr', self.sm_GPSR, '/SM_ROOT')
intro_server.start()
out = self.sm_GPSR.execute()
print out
intro_server.stop()
self.smach_bool = True
