def __init__(self):
rospy.init_node('move_to_and_inspect_point_sm', anonymous=False)
hsm = StateMachine(outcomes=['finished statemachine'])
with hsm:
StateMachine.add( 'GO_TO_POINT',
SimpleActionState( 'pid_global',
MoveAction,
makeMoveGoal("pid_global_plan", -3.0, 0, -0.5, radius_of_acceptance = 2.0)),
transitions = { "succeeded": 'INSPECT_POINT',
"preempted": 'INSPECT_POINT',
"aborted": 'INSPECT_POINT' })
StateMachine.add( 'INSPECT_POINT',
SimpleActionState( 'inspect_point',
MoveAction,
makeMoveGoal("inspect_point", -3.0, 0.0, -0.5, radius_of_acceptance=2.0)),
transitions = { 'succeeded': 'INSPECT_POINT',
"preempted": 'INSPECT_POINT',
"aborted": 'INSPECT_POINT' })
intro_server = IntrospectionServer(str(rospy.get_name()), hsm,'/SM_ROOT')
intro_server.start()
hsm.execute()
#patrol.execute()
print("State machine execute finished")
intro_server.stop()
def main():
rospy.init_node('update_userdata_from_old_state')
state_a = UpdateName('name_topic')
state_b = PrintName(duration=4.0)
state_c = PrintName(duration=2.0)
top = StateMachine(outcomes=['success'])
top.userdata.my_name = "Joseph"
with top:
StateMachine.add('A',
state_a,
transitions={'success': 'B'},
remapping={'my_name': 'my_name'})
StateMachine.add('B',
state_b,
transitions={'success': 'C'},
remapping={'my_name': 'my_name'})
StateMachine.add('C',
state_c,
transitions={'success': 'A'},
remapping={'my_name': 'my_name'})
top.execute()
class test():
def __init__(self):
rospy.init_node('go_and_follow')
self.Test = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
self.Test.userdata.PT_LIST = {}
self.Test.userdata.mission = {}
with self.Test:
self.Test.userdata.targets = list()
self.Test.userdata.actions = list()
self.Test.userdata.people_condition = {}
self.Test.userdata.command = 2
# StateMachine.add('GET_TARGET',
# CheckStop2(),
# transitions={'stop': 'succeeded', 'aborted': 'GET_TARGET', 'remeber':'GET_TARGET'})
StateMachine.add(
'GET_TASK',
GeneralAnswer(),
transitions={
'succeeded': 'GET_TASK',
'aborted': 'aborted'
},
remapping={
'targets': 'targets',
'actions': 'actions',
#'task_num':'tasksNum'
})
self.Test.execute()
def main():
global rate
global is_tape
global center_tape
global pub
rospy.init_node('tape_master')
rate = rospy.Rate(20)
is_tape_sub = rospy.Subscriber('tape_detect_msg', Bool, is_tape_callback)
center_tape_sub = rospy.Subscriber('tape_center_percent_msg', Float32,
center_tape_callback)
pub = rospy.Publisher('vesc/ackermann_cmd_mux/input/navigation',
AckermannDriveStamped,
queue_size=1)
tape = StateMachine(outcomes=['decide_tape', 'no_tape', 'follow_tape'])
with tape:
StateMachine.add('DECIDE',
decision(),
transitions={
'decide_tape': 'DECIDE',
'no_tape': 'STOP',
'follow_tape': 'FOLLOW'
})
StateMachine.add('STOP', stop(), transitions={'decide_tape': 'DECIDE'})
StateMachine.add('FOLLOW',
follow(),
transitions={'decide_tape': 'DECIDE'})
tape.execute()
def PatrolStateMachine(listOfWaypoints):
tempWaypoint = ()
waypoints = []
#convert waypoints to ros topic format
#waypoints.append([str(0), (listOfWaypoints[-1][1],listOfWaypoints[-1][0]), (0.0, 0.0, 0.0, 1.0)])
for i in range(0, len(listOfWaypoints), 1):
tempWaypoint = (listOfWaypoints[i][1], listOfWaypoints[i][0])
waypoints.append([str(i), tempWaypoint, (0.0, 0.0, 0.0, 1.0)])
#start state machine
patrol = StateMachine(['succeeded', 'aborted', 'preempted'])
with patrol:
for i, w in enumerate(waypoints):
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.frame_id = 'map'
goal_pose.target_pose.pose.position.x = w[1][0]
goal_pose.target_pose.pose.position.y = w[1][1]
goal_pose.target_pose.pose.position.z = 0.0
goal_pose.target_pose.pose.orientation.x = w[2][0]
goal_pose.target_pose.pose.orientation.y = w[2][1]
goal_pose.target_pose.pose.orientation.z = w[2][2]
goal_pose.target_pose.pose.orientation.w = w[2][3]
StateMachine.add(w[0],
SimpleActionState('move_base',
MoveBaseAction,
goal=goal_pose),
transitions={'succeeded': waypoints[(i + 1) % \
len(waypoints)][0]})
patrol.execute()
def main():
rospy.init_node('move_gripper_client')
parser = argparse.ArgumentParser(
description='Client for Korus` move gripper action server')
parser.add_argument('gripper_angle', type=float, help='gripper angle')
args = parser.parse_args()
sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
sm.userdata.gripper_angle = args.gripper_angle
with sm:
def MoveGripperResultCb(userdata, status, result):
rospy.loginfo('action finished in state ' + str(status))
rospy.loginfo('action result: ')
rospy.loginfo(str(result))
smach.StateMachine.add(
'MoveGripper',
SimpleActionState('/korus/move_gripper',
MoveGripperAction,
goal_slots=['gripper_angle'],
result_cb=MoveGripperResultCb))
sm.execute()
class turn():
def __init__(self):
rospy.init_node('turn')
rospy.logwarn('ssssss')
self.Turn = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.Turn:
self.Turn.userdata.degree = pi
self.Turn.userdata.sentences = 'I want play a riddle game'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'turn',
'aborted': 'aborted',
'error': 'error'
},
remapping={'sentences': 'sentences'})
StateMachine.add('turn',
TurnDegree(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={'degree': 'degree'})
self.Turn.execute()
class test():
def __init__(self):
rospy.init_node('test')
rospy.on_shutdown(self.shutdown)
self.top = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.top:
self.top.userdata.go_point = Point(1, 0, 0)
self.top.userdata.degree = 3.14
StateMachine.add('sm1',
LinearDisplacement(),
transitions={
'succeeded': 'sm2',
'preempted': 'sm2',
'error': 'error'
},
remapping={'displacementVec': 'go_point'})
StateMachine.add('sm2',
TurnDegree(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
},
remapping={'degree': 'degree'})
self.top.execute()
def shutdown(self):
rospy.logerr('done')
def main():
smach_states = rospy.get_param("smach_states")
# The autonomy_sm handles the full autonomy sequence for the competition run
autonomy_sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
with autonomy_sm:
# TODO: create localization state. might just be a MonitorState
# Sequentially add all the states from the config file to the state machine,
# where state i transitions to state i+1
names = []
for i in range(len(smach_states)):
state = smach_states[i]
name, action_state = create_action_state(state)
# If the state name is already in the state machine, add the new
# one with increasing numbers
if name in names:
name = name+"2"
while name in names:
name = name[:-1]+str(int(name[-1])+1) # increase num
names.append(name)
StateMachine.add_auto(name, action_state, connector_outcomes=['succeeded'])
# StateMachine.add()
# Create the concurrence contained for the fully autonomy sequence. This
# runs the state machine for the competition run. It also concurrently runs
# a state with a timer counting down from 10 minutes and a state that listens
# to the /click/start_button topic. If either of these are triggered, it will
# end autonomy and place us into the teleop state.
# TODO: add 10 minute competition timer state
autonomy_concurrence = Concurrence(outcomes=['enter_teleop', 'stay', 'aborted'],
default_outcome='enter_teleop',
child_termination_cb=autonomy_child_term_cb,
outcome_cb=autonomy_out_cb)
with autonomy_concurrence:
# state that runs full autonomy state machine
Concurrence.add('AUTONOMY', autonomy_sm)
# state that listens for toggle message
Concurrence.add('TOGGLE_LISTEN', MonitorState('/click/start_button', Empty, monitor_cb))
# Top level state machine, containing the autonomy and teleop machines.
top_sm = StateMachine(outcomes=['DONE'])
with top_sm:
StateMachine.add('TELEOP_MODE', MonitorState('/click/start_button', Empty, monitor_cb), transitions={'invalid':'AUTONOMY_MODE', 'valid':'TELEOP_MODE', 'preempted':'AUTONOMY_MODE'})
StateMachine.add('AUTONOMY_MODE', autonomy_concurrence,
transitions={'enter_teleop':'TELEOP_MODE', 'stay':'AUTONOMY_MODE', 'aborted':'DONE'})
#StateMachine.add('TELEOP_MODE', MonitorState('/click/start_button', Empty, monitor_cb),
# transitions={'invalid':'DONE', 'valid':'TELEOP_MODE', 'preempted':'DONE'})
sis = IntrospectionServer('smach_introspection_server', top_sm, '/COMPETITION_SMACH')
sis.start()
top_sm.execute()
rospy.spin()
sis.stop()
def main(msg):
if (msg.data == True):
print('Creating SMACH')
sm = StateMachine(['complete', 'failed'])
sis = smach_ros.IntrospectionServer('smach_introspect', sm, '/SM_ROOT')
with sm:
StateMachine.add('MANUAL_OVERRIDE',
MANUAL_OVERRIDE(),
transitions={
'success': 'GET_HOME',
'failed': 'failed'
})
StateMachine.add('GET_HOME',
GET_HOME(),
transitions={
'success': 'START_GMAPPING',
'failed': 'failed'
})
StateMachine.add('START_GMAPPING',
START_GMAPPING(),
transitions={
'success': 'GET_POINTS',
'failed': 'failed'
})
StateMachine.add('GET_POINTS',
GET_POINTS(),
transitions={
'success': 'MAKE_MAP',
'failed': 'failed'
})
StateMachine.add('MAKE_MAP',
MAKE_MAP(),
transitions={
'success': 'DISPLAY_MAP',
'failed': 'failed'
})
StateMachine.add('DISPLAY_MAP',
DISPLAY_MAP(),
transitions={
'success': 'USER_CONFIRMATION',
'failed': 'failed'
})
StateMachine.add('USER_CONFIRMATION',
USER_CONFIRMATION(),
transitions={
'confirm': 'complete',
'update': 'UPDATE_POINTS',
'failed': 'failed'
})
StateMachine.add('UPDATE_POINTS',
UPDATE_POINTS(),
transitions={
'success': 'MAKE_MAP',
'failed': 'failed'
})
sis.start()
sm.execute()
def turnOnStateMachine(request):
sm = StateMachine(outcomes=['success'])
sm.userdata.direction = request.direction
with sm:
StateMachine.add('A',
A(),
transitions={
'1': 'B',
'0': 'D'
},
remapping={
'input': 'direction',
'output': ''
})
StateMachine.add('B',
B(),
transitions={
'1': 'C',
'0': 'success'
},
remapping={
'input': 'direction',
'output': ''
})
StateMachine.add('C',
C(),
transitions={
'1': 'D',
'0': 'B'
},
remapping={
'input': 'direction',
'output': ''
})
StateMachine.add('D',
D(),
transitions={
'1': 'success',
'0': 'C'
},
remapping={
'input': 'direction',
'output': ''
})
## sis = smach_os.IntrospectionServer('server_name', sm, '/SM_ROOT')
## sis.start()
sm.execute()
## sis.stop()
response = jinko_service_msgResponse()
response.success = True
return response
class ArGraspTask():
def __init__(self):
rospy.init_node('xm_arm_ar')
rospy.logwarn('Come on! Let us go now!')
self.arm_stack_service = rospy.Service('arm_stack',xm_PickOrPlace,self.callback)
self.sm_ArTags =StateMachine(outcomes =['succeeded','aborted','error'])
with self.sm_ArTags:
self.sm_ArTags.userdata.arm_ps = PointStamped()
StateMachine.add('GETPOISITION',
MonitorState('ar_pose_marker',AlvarMarkers,self.position_cb,max_checks=1,output_keys =['obj_pos']),
transitions={'valid':'PICK','invalid':'aborted','preempted':'aborted'},
remapping={'obj_pos':'arm_ps'})
self.sm_ArTags.userdata.arm_mode_1 =1
StateMachine.add('PICK',
ArmGo(),
transitions={'succeeded':'PLACE','aborted':'GETPOISITION','error':'error'},
remapping={'arm_ps':'arm_ps','arm_mode':'arm_mode_1'})
self.sm_ArTags.userdata.arm_mode_2 = 2
StateMachine.add('PLACE',ArmGo(),
transitions={"succeeded":'succeeded','aborted':'aborted','error':'error'},
remapping={'arm_ps':'arm_ps','arm_mode':'arm_mode_2'})
outcome = self.sm_ArTags.execute()
def position_cb(self,userdata,msg):
if msg.markers is not None:
obj_pos = PointStamped()
obj_pos.point = msg.markers[0].pose.pose.position
obj_pos.header.frame_id ='camera_Link'
userdata.obj_pos = obj_pos
return True
else:
return False
def callback(self,req):
self.arm_point = req.goal_position
self.arm_mode = req.action
sm_result = self.sm_ArTags.execute(parent_ud ={'arm_point':self.arm_point,'arm_mode':self.arm_mode})
res = xm_PickOrPlaceResponse()
if sm_result == 'succeeded':
rospy.logwarn('arm_stack succeeded')
res.result =True
else:
rospy.logwarn('moveit failed, please justfy the position of robot')
res.result =False
return res
def callback(data):
#difine state when normal which is state one
class one(state):
def _init_(self):
state._init_(self, outcomes=['emergency'])
def execute(self, userdata):
rospy.loginfo('Executing state normal')
if battery_state > 10:
m = data.data.split()
print(m)
if (int(m[0]) > 100):
bot.drive_direct(-35, 35)
elif (int(m[1]) > 100):
bot.drive_direct(-35, 35)
elif (int(m[2]) > 100):
bot.drive_direct(-35, 35)
elif (int(m[3]) > 100):
bot.drive_direct(35, -35)
elif (int(m[4]) > 100):
bot.drive_direct(35, -35)
elif (int(m[5]) > 100):
bot.drive_direct(35, -35)
else:
bot.drive_straight(35)
else:
return 'emergency'
class two(state):
#difine state when emergancy which is state two
def __init__(self):
State.__init__(self, outcomes=['normal'])
def execute(self, userdata):
rospy.loginfo('Executing state emergency')
bot.drive_straight(0)
if battery_state > 10:
return 'normal'
if __name__ == '__main__':
# Create a SMACH state machine
sm = StateMachine(outcomes=['normal', 'emergency'])
# Open the container
with sm:
# Add states to the container
StateMachine.add('ONE', one(), transitions={'emergency': 'TWO'})
StateMachine.add('TWO', two(), transitions={'normal': 'ONE'})
# Execute SMACH plan
sm.execute()
class EnterRoom():
def __init__(self):
rospy.init_node('enter_room')
rospy.on_shutdown(self.shutdown)
self.sm_enter = StateMachine(outcomes= ['succeeded','aborted','preempted'])
self.nav_states_ = WayPoint().nav_states_
with self.sm_enter :
# navpose is the pose of xm_arm
self.sm_enter.userdata.mode = 0
StateMachine.add('NAV_POSE',
ArmCmd(),
transitions={'succeeded':'WAIT1','aborted':'NAV_POSE'},
remapping = {'mode':'mode'})
# this time (rec =1) may need to justfy to satisfy the scene
self.sm_enter.userdata.delay_time =1.0
StateMachine.add('WAIT1',
Wait(),
transitions={'succeeded':'DOOR'},
remapping = {'rec':'delay_time'})
StateMachine.add('DOOR',
DoorDetect().door_detect_,
transitions={'invalid':'NAV1','valid':'DOOR'})
StateMachine.add('NAV1',
self.nav_states_[0],
transitions={'succeeded':'TURN_POSE','aborted':'NAV1'})
pt = Point()
pt.x =0
pt.y=0
pt.z= pi/8
self.sm_enter.userdata.turn_pose = pt
StateMachine.add('TURN_POSE',
SimpleMove(),
transitions={'succeeded':'SPEAK','aborted':'SPEAK'},
remapping = {'turn_pose':'turn_pose'})
self.sm_enter.userdata.sentences = 'please look at me'
StateMachine.add('SPEAK',
Speak(),
transitions={'succeeded':'succeeded'},
remapping ={'sentences':'sentences'})
self.smach_bool = False
self.sm_enter.execute()
self.smach_bool =True
def shutdown(self):
if self.smach_bool ==True:
rospy.loginfo('xm completes the whole tasks , >3<')
else:
rospy.loginfo('Stopping..., you may write the wrong code , but this is not the fault of xm #_ #')
def test_userdata(self):
"""Test serial manipulation of userdata."""
pinger_thread = threading.Thread(target=pinger)
pinger_thread.start()
init_ud = UserData()
init_ud.b = 'A'
sm = StateMachine(['valid', 'invalid', 'preempted'])
sm.set_initial_state(['MON'], userdata=init_ud)
assert 'b' in sm.userdata
assert sm.userdata.b == 'A'
with sm:
StateMachine.add(
'MON',
MonitorState('/ping',
Empty,
cond_cb,
input_keys=['b'],
output_keys=['a']))
outcome = sm.execute()
assert outcome == 'invalid'
assert 'b' in sm.userdata
assert sm.userdata.b == 'A'
assert 'a' in sm.userdata
assert sm.userdata.a == 'A'
def main():
rospy.init_node('follow_waypoints')
sm = StateMachine(outcomes=['success'])
with sm:
StateMachine.add('GET_PATH', GetPath(),
transitions={'success': 'FOLLOW_PATH', 'killed': 'success'},
remapping={'waypoints': 'waypoints'})
StateMachine.add('FOLLOW_PATH', FollowPath(),
transitions={'success': 'PATH_COMPLETE'},
remapping={'waypoints': 'waypoints'})
StateMachine.add('PATH_COMPLETE', PathComplete(),
transitions={'success': 'GET_PATH'})
sm.execute()
def main():
rospy.init_node('smach_usecase_executive')
sm_root = StateMachine(outcomes=['preempted', 'aborted', 'succeeded'])
with sm_root:
StateMachine.add('RESET',
ServiceState('/reset', Empty, request=EmptyRequest()),
transitions={'succeeded': 'SPAWN'})
StateMachine.add(
'SPAWN',
ServiceState('/spawn', Empty, request=EmptyRequest()),
#request=SpawnRequest(1.0, 1.0, 2.0, 'kkk')),
transitions={'aborted': 'aborted'})
# transitions={
# 'preemtped': 'preemtped',
# 'aborted': 'aborted',
# 'succeeded': 'succeeded'})
sis = IntrospectionServer('exmaple', sm_root, '/USE_CASE')
sis.start()
outcome = sm_root.execute()
rospy.spin()
def main():
rospy.init_node('smach_usecase_step_02')
# 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')))
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Execute SMACH tree
outcome = sm0.execute()
# Signal ROS shutdown (kill threads in background)
rospy.spin()
sis.stop()
def main():
rospy.init_node('smach_example_hierarchical_state_machine')
# Create the top level SMACH state machine
sm_top = StateMachine(outcomes=['outcome5'])
# Open the container
with sm_top:
StateMachine.add('BAS', Bas(), transitions={'outcome3': 'SUB'})
# Create the sub SMACH state machine
sm_sub = StateMachine(outcomes=['outcome4'])
# Open the container
with sm_sub:
# Add state to the container
StateMachine.add('FOO',
Foo(),
transitions={
'outcome1': 'BAR',
'outcome2': 'outcome4'
})
StateMachine.add('BAR', Bar(), transitions={'outcome1': 'FOO'})
StateMachine.add('SUB', sm_sub, transitions={'outcome4': 'outcome5'})
sis = IntrospectionServer('example', sm_top, '/SM_STATEMACHINE')
sis.start()
# Execute SMACH plan
outcome = sm_top.execute()
rospy.spin()
sis.stop()
def test_service_cb(self):
"""Test calling a service with a callback."""
srv = rospy.Service('/empty', std_srvs.Empty, empty_server)
sm = StateMachine(['succeeded', 'aborted', 'preempted', 'done'])
with sm:
def foo_response_cb(userdata, response):
userdata.foo_var_out = 'foo!'
return 'succeeded'
StateMachine.add('FOO',
ServiceState('/empty',
std_srvs.Empty,
response_cb=foo_response_cb,
output_keys=['foo_var_out']),
remapping={'foo_var_out': 'sm_var'},
transitions={'succeeded': 'done'})
outcome = sm.execute()
rospy.logwarn("OUTCOME: " + outcome)
assert outcome == 'done'
def main():
#Create a rospy node for the state machine
rospy.init_node('IRA_statemachine')
#Creating a state machine for banking scenario
sm_bank = StateMachine(outcomes=['Stop'])
#Create Smach containers for each state
with sm_bank:
StateMachine.add('Face Recognition',
frn.Face_recognition(),
transitions={'Detected':'Welcome_speech','Not_Detected':'Face Recognition'},remapping={'Face_recognition_out':'speech_in'})
StateMachine.add('Welcome_speech',
ss1.welcome_speech(),
transitions={'completed':'Hotword_detection'})
StateMachine.add('Hotword_detection',
hwd.Hotword_detection(),
transitions={'Detected':'Speech_Recognition','Not_Detected':'Stop'})
StateMachine.add('Speech_Recognition',
sr.Speech_Recognition(),
transitions={'Completed':'Hotword_detection'})
#Code for initiating the smach_viewer
Monitor = smach_ros.IntrospectionServer('IRA', sm_bank, '/IRA_FLOW')
Monitor.start()
#Execute the state machine
outcome = sm_bank.execute()
#Stop the smach_viewer if state machine stops
Monitor.stop
def main():
#Create a rospy node for the state machine
rospy.init_node('IRA_statemachine')
#Creating a state machine for banking scenario
sm_bank = StateMachine(outcomes=['Stop'])
#Create Smach containers for each state
with sm_bank:
StateMachine.add('Face Recognition',
frn.Face_recognition(),
transitions={'Detected':'Speech_Gesture','Not_Detected':'Face Recognition'},remapping={'Face_recognition_out':'speech_in'})
sm_con = smach.Concurrence(outcomes=['Waiting','Done'],default_outcome='Waiting',outcome_map={'Done':{'Speech1':'Completed','Gesture1':'Completed'}})
with sm_con:
smach.Concurrence.add('Speech1',ss1.speech())
smach.Concurrence.add('Gesture1',g1.gesture())
smach.StateMachine.add('Speech_Gesture', sm_con,
transitions={'Waiting':'Stop',
'Done':'Face Recognition'})
StateMachine.add('Hotword_detection',
hwdc.Hotword_detection(),
transitions={'Detected':'Speech_Recognition','Not_Detected':'Face Recognition'})
StateMachine.add('Speech_Recognition',
src.Speech_recognition(),
transitions={'Completed':'Speech_database','Not_Completed':'Face Recognition'},remapping={'speech_recognition_out':'Database_in'})
StateMachine.add('Speech_database',
sdc.Speech_database(),
transitions={'Completed':'CON','Not_Completed':'Face Recognition'},remapping={'speech_database_out':'Text_in'})
sm_con = smach.Concurrence(outcomes=['Waiting','Done'],default_outcome='Waiting',outcome_map={'Done':{'Text_speech':'Completed'}},input_keys=['Text_in'])
with sm_con:
smach.Concurrence.add('Text_speech',ts.Text_speech())
#smach.Concurrence.add('ui1',ui1.Text_reply())
StateMachine.add('CON',
sm_con,
transitions={'Waiting':'Stop','Done':'Hotword_detection'})
#Code for initiating the smach_viewer
Monitor = smach_ros.IntrospectionServer('IRA', sm_bank, '/IRA_FLOW')
Monitor.start()
#Execute the state machine
outcome = sm_bank.execute()
#Stop the smach_viewer if state machine stops
Monitor.stop
def __init__(self):
rospy.init_node('monitor_fake_battery', anonymous=False)
rospy.on_shutdown(self.shutdown)
# Set the low battery threshold (between 0 and 100)
self.low_battery_threshold = rospy.get_param('~low_battery_threshold', 50)
# Initialize the state machine
sm_battery_monitor = StateMachine(outcomes=[])
with sm_battery_monitor:
# Add a MonitorState to subscribe to the battery level topic
StateMachine.add('MONITOR_BATTERY',
MonitorState('battery_level', Float32, self.battery_cb),
transitions={'invalid':'RECHARGE_BATTERY',
'valid':'MONITOR_BATTERY',
'preempted':'MONITOR_BATTERY'},)
# Add a ServiceState to simulate a recharge using the set_battery_level service
StateMachine.add('RECHARGE_BATTERY',
ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, request=100),
transitions={'succeeded':'MONITOR_BATTERY',
'aborted':'MONITOR_BATTERY',
'preempted':'MONITOR_BATTERY'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('monitor_battery', sm_battery_monitor, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = sm_battery_monitor.execute()
intro_server.stop()
class TestGripperSmach:
def __init__(self):
rospy.init_node('test_gripper_smach', anonymous=False)
gripper_goal = xm_GripperCommandGoal()
gripper_goal.command.position = 0.10
gripper_goal.command.torque = 0.5
gripper_state = SimpleActionState(
'xm_move_gripper',
xm_GripperCommandAction,
goal=gripper_goal,
result_cb=self.gripper_state_cb,
exec_timeout=rospy.Duration(10),
server_wait_timeout=rospy.Duration(10))
self.gripper_smach = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.gripper_smach:
StateMachine.add('GRIPPER_OPEN',
gripper_state,
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
gripper_outcome = self.gripper_smach.execute()
rospy.loginfo('State Machine Outcome: ' + str(gripper_outcome))
def gripper_state_cb(self, userdata, status, result):
if result:
rospy.loginfo("Complete!")
def main():
rospy.init_node('smach_example_state_machine')
sm = StateMachine(['exit'])
with sm:
for key, (x, y, next_point) in WAYPOINTS.items():
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.orientation.w = 1.
StateMachine.add(key, SimpleActionState('move_base',
MoveBaseAction,
goal=goal),
transitions={'succeeded': next_point, 'aborted': 'exit', 'preempted': 'exit'})
# Create and start the introspection server
sis = IntrospectionServer('strat', sm, '/SM_ROOT')
sis.start()
# Execute the state machine
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
class WaypointStateMachine(object):
def __init__(self):
self.sm = StateMachine(outcomes=["success", "failure", "preempted"])
self.outcome = None
self.action_server = None
with self.sm:
StateMachine.add("GOTO_WAYPOINT",
GoToWaypointState(),
transitions={
"success": "GOTO_WAYPOINT",
"finished": "success",
"failure": "failure",
"preempted": "preempted"
},
remapping={
"waypoints": "sm_waypoints",
"waypoint_index_in": "sm_waypoint_index",
"waypoint_index_out": "sm_waypoint_index",
"action_server": "sm_action_server",
})
def execute(self, waypoints, action_server):
rospy.loginfo(
"To cancel the waypoint follower, run: 'rostopic pub -1 /dodobot/follow_path/cancel actionlib_msgs/GoalID -- {}'"
)
rospy.loginfo(
"To cancel the current goal, run: 'rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID -- {}'"
)
self.sm.userdata.sm_waypoint_index = 0
self.sm.userdata.sm_waypoints = waypoints
self.sm.userdata.sm_action_server = action_server
self.outcome = self.sm.execute()
return self.outcome
class TestGripperSmach:
def __init__(self):
rospy.init_node("test_gripper_smach", anonymous=False)
gripper_goal = xm_GripperCommandGoal()
gripper_goal.command.position = 0.10
gripper_goal.command.torque = 0.5
gripper_state = SimpleActionState(
"xm_move_gripper",
xm_GripperCommandAction,
goal=gripper_goal,
result_cb=self.gripper_state_cb,
exec_timeout=rospy.Duration(10),
server_wait_timeout=rospy.Duration(10),
)
self.gripper_smach = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with self.gripper_smach:
StateMachine.add(
"GRIPPER_OPEN", gripper_state, transitions={"succeeded": "", "aborted": "", "preempted": ""}
)
gripper_outcome = self.gripper_smach.execute()
rospy.loginfo("State Machine Outcome: " + str(gripper_outcome))
def gripper_state_cb(self, userdata, status, result):
if result:
rospy.loginfo("Complete!")
class TestFollow():
def __init__(self):
rospy.init_node('TestFollow')
rospy.on_shutdown(self.shutdown)
rospy.logerr('Follow start')
self.smach_bool = False
self.newFollow = StateMachine(outcomes = ['succeeded' , 'aborted','error'])
with self.newFollow:
StateMachine.add('RUNNODE' , RunNode() , transitions = {'succeeded':'FOLLOW' ,
'aborted':'RUNNODE',
})
StateMachine.add('FOLLOW' ,FollowTest() , transitions = {'succeeded':'succeeded' ,
'error':'error',
'aborted':'aborted'} )
intro_server = IntrospectionServer('sm_gpsr',self.newFollow,'/SM_ROOT')
intro_server.start()
out = self.newFollow.execute()
rospy.logerr(out)
intro_server.stop()
self.smach_bool = True
def shutdown(self):
if self.smach_bool ==True:
rospy.loginfo('smach succeeded')
else:
rospy.loginfo('smach error')
def main():
rospy.init_node('tinker_mission_follow')
rospy.loginfo(colored('starting follow and guide task ...', 'green'))
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('Start_Button', MonitorStartButtonState(),
transitions={'valid': 'Start_Button', 'invalid': '1_Start'})
StateMachine.add('1_Start', MonitorKinectBodyState(),
transitions={'valid':'1_Start', 'invalid':'Sequence'})
sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded')
with sequence:
follow_concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=[])
with follow_concurrence:
Concurrence.add('FollowMe', FollowMeState())
Concurrence.add('KeyWordsRecognition', KeywordsRecognizeState('stop'))
Sequence.add('Follow_concurrence', follow_concurrence)
StateMachine.add('Sequence', sequence, {'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def main():
rospy.init_node('smach_example_state_machine')
# Create a SMACH state machine
sm = StateMachine(outcomes=['outcome4'])
sm.userdata.sm_counter = 0
sis = IntrospectionServer('example', sm, '/SM_ROOT')
sis.start()
# Open the container
with sm:
# Add states to the container
StateMachine.add('FOO',
Foo(),
transitions={
'outcome1': 'BAR',
'outcome2': 'outcome4'
},
remapping={
'foo_counter_in': 'sm_counter',
'foo_counter_out': 'sm_counter'
})
StateMachine.add('BAR',
Bar(),
transitions={'outcome1': 'FOO'},
remapping={'bar_counter_in': 'sm_counter'})
# Execute SMACH plan
outcome = sm.execute()
rospy.spin()
def test_group(self):
"""Test adding a bunch of states with group args."""
class DoneState(State):
def __init__(self):
State.__init__(self, outcomes=['done'])
def execute(self, ud=None):
return 'done'
sm = StateMachine(['succeeded', 'done'])
with sm:
StateMachine.add('FAILSAUCE', DoneState())
transitions = {'aborted': 'FAILSAUCE', 'preempted': 'FAILSAUCE'}
with sm:
StateMachine.add(
'FIRST',
SimpleActionState('reference_action', TestAction, goal=g1),
transitions)
StateMachine.add(
'SECOND',
SimpleActionState('reference_action', TestAction, goal=g2),
transitions)
StateMachine.add(
'THIRD',
SimpleActionState('reference_action', TestAction, goal=g1),
transitions)
outcome = sm.execute()
assert outcome == 'done'
def test_userdata_nesting(self):
"""Test serial manipulation of userdata."""
sm = StateMachine(['done', 'preempted', 'aborted'])
with sm:
StateMachine.add('SETTER', Setter(), {'done': 'GETTER'})
StateMachine.add('GETTER', Getter(), {'done': 'NEST'})
sm2 = StateMachine(['done', 'aborted', 'preempted'])
sm2.register_input_keys(['a'])
with sm2:
StateMachine.add(
'ASSERTER',
ConditionState(lambda ud: 'a' in ud, input_keys=['a']), {
'true': 'done',
'false': 'aborted'
})
StateMachine.add('NEST', sm2)
outcome = sm.execute()
assert outcome == 'done'
assert 'a' in sm.userdata
assert 'b' in sm.userdata
assert sm.userdata.a == 'A'
assert sm.userdata.b == 'A'
class TestTurn():
def __init__(self):
rospy.init_node('turn_test', anonymous=False)
rospy.on_shutdown(self.shutdown)
rospy.loginfo('started test the turn')
self.test_bool = False
self.test_turn = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.test_turn:
self.test_turn.userdata.degree = -3.1415926 / 8
StateMachine.add('TURN',
TurnDegree(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={'degree': 'degree'})
out = self.test_turn.execute()
rospy.loginfo(out)
if out == 'succeeded':
self.test_bool = True
def shutdown(self):
if self.test_bool == True:
rospy.logerr('test succeeded')
else:
rospy.logerr('test failed')
def __init__(self):
global OBJECT_ID
rospy.init_node("test")
sm=StateMachine(outcomes=["succeeded","aborted","preempted","valid","invalid"])
self.lift=actionlib.SimpleActionClient('xm_move_arm_height', xm_ArmHeightAction)
arm_goal = xm_ArmHeightGoal()
arm_goal.distance_x = 1
arm_goal.distance_z = -0.2
self.arm_state=SimpleActionState('xm_move_arm_height',xm_ArmHeightAction,goal=arm_goal,
result_cb=self.arm_state_cb, exec_timeout=rospy.Duration(60),
server_wait_timeout=rospy.Duration(60))
gripper_goal = xm_GripperCommandGoal()
gripper_goal.command.position = 0.0
# gripper_goal.command.torque = 0.5
# gripper_goal.header.frame
gripper_state = SimpleActionState('xm_move_gripper', xm_GripperCommandAction, goal=gripper_goal,
result_cb=self.wrist_cb, exec_timeout=rospy.Duration(10),
server_wait_timeout=rospy.Duration(10))
wrist_goal=xm_WristPositionGoal()
wrist_goal.angle=0
wrist_state=SimpleActionState("xm_move_wrist", xm_WristPositionAction,goal=wrist_goal,
result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10),
server_wait_timeout=rospy.Duration(10))
self.monitor_times=1
sm.userdata.ob=0
with sm:
# StateMachine.add("FIND_OBJECT1", Find_Object(),
# transitions={"succeeded": "", 'aborted': ''}, )
# StateMachine.add("angle", ChangeAngular(angle=0.2), transitions={"succeeded":""})
# StateMachine.add("INIT",ServiceState('xm_move_arm_position/move_position', xm_ArmPosition,"prepare",response_cb=self.responce_cb),transitions={'succeeded':''})
# StateMachine.add("HEIGHT",self.arm_state,transitions={"succeeded":''})
# StateMachine.add("GRIP",gripper_state,transitions={"succeeded":''})
# StateMachine.add("AR_TARGET2", MonitorState("ar_pose_marker", AlvarMarkers,self.get_mark_pos_cb), transitions={"valid":"AR_TARGET2","invalid":""})
# StateMachine.add("WRIST", wrist_state, transitions={"succeeded":""})
StateMachine.add("CHANGE_HEIGHT1", Lift_HEIGHT(height=0.0), transitions={"succeeded":""})
# StateMachine.add("changemode", ChangeMode("arm"), transitions={"succeeded":""},)
# for i in range(4):
sm.execute()
# OBJECT_ID+=1
rospy.sleep(1)
def main():
rospy.init_node('tinker_mission_navigation')
rospy.loginfo(colored('starting navigation task ...', 'green'))
# load waypoints from xml
pose_list = TKpos.PoseList.parse(open(rospy.get_param('~waypoint_xml'), 'r'))
for pose in pose_list.pose:
WayPointGoalState.waypoint_dict[pose.name] = TKpos.Pose.X(pose)
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('Start_Button', MonitorStartButtonState(),
transitions={'valid': 'Start_Button', 'invalid': 'Sequence'})
sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence:
# Sequence.add('GoToWaypoin1', WayPointGoalState('waypoint1'), transitions={'aborted': 'GoToWaypoin1'})
# Sequence.add('ArriveWaypoint1', SpeakState('I have arrived at way point one'))
# Sequence.add('GoToWaypoin2', WayPointGoalState('waypoint2'),
# transitions={'succeeded': 'ArriveWaypoint2', 'aborted': 'Obstacle'})
# Sequence.add('Obstacle', SpeakState('Obstacle in front of me'))
# Sequence.add('ObstacleDelay', DelayState(10),
# transitions={'succeeded': 'GoToWaypoin2'})
# Sequence.add('ArriveWaypoint2', SpeakState('I have arrived at way point two'))
Sequence.add('GoToWaypoin3', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3'})
Sequence.add('ArriveWaypoint3', SpeakState('I have arrived at way point three'))
Sequence.add('StopCommandAndGo', SpeakState('Please GO. If you want to stop, say stop tinker'))
Sequence.add('Train_human', FollowTrainState())
follow_concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=[])
with follow_concurrence:
Concurrence.add('FollowMe', FollowMeState())
# Concurrence.add('KeyWordsRecognition', KeywordsRecognizeState('stop'))
Sequence.add('Follow_concurrence', follow_concurrence)
Sequence.add('GoToWaypoin3Again', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3Again'})
Sequence.add('ArriveWaypoint3Again', SpeakState('I am back at way point three'))
Sequence.add('GoOut', WayPointGoalState('out'), transitions={'aborted': 'GoOut'})
StateMachine.add('Sequence', sequence, {'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
class RandomPatrol():
def __init__(self):
rospy.init_node('random_patrol', 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)
# Initialize the patrol state machine
self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Set the userdata.waypoints variable to the pre-defined waypoints
self.sm_patrol.userdata.waypoints = self.waypoints
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
transitions={'succeeded':'NAV_WAYPOINT'},
remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'PICK_WAYPOINT',
'aborted':'PICK_WAYPOINT',
'preempted':'PICK_WAYPOINT'},
remapping={'waypoint_in':'patrol_waypoint'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_patrol.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.sm_patrol.request_preempt()
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
def main():
rospy.init_node('tinker_mission_person_recognition')
rospy.loginfo(colored('starting person recognition task ...', 'green'))
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('0_wait_for_start', MonitorStartButtonState(), transitions={'valid': '0_wait_for_start','invalid': '0_arm_mode'})
StateMachine.add('0_arm_mode', ArmModeState(ArmModeState.Arm_Mode_Kinect), transitions={'succeeded':'0_wait_for_human'})
StateMachine.add('0_wait_for_human', SpeakState('I am tinker , I am waiting for operator'),
transitions={'succeeded': '1_Start'})
StateMachine.add('1_Start', MonitorKinectBodyState(), transitions={'valid':'1_Start', 'invalid':'Sequence'})
sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence:
Sequence.add('2_1_train_speak', SpeakState('please look at the camera'))
Sequence.add('2_2_train', TrainPersonState())
Sequence.add('2_3_train_finish', SpeakState('I have memorized you, thanks'))
Sequence.add('3_wait', DelayState(10))
Sequence.add('4_turn_back', ChassisSimpleMoveState(theta=3.23))
Sequence.add('4_1_turn_back', ChassisSimpleMoveState(x=-1.0))
Sequence.add('5_1_find_operator', FindPersonState())
Sequence.add('5_2_point_operator', MoveArmState(offset=Point(0, 0, 0)), remapping={'target':'person_pose'})
Sequence.add('5_3_say', SpeakState('I have found you'))
Sequence.add('5_4_wait', DelayState(5))
StateMachine.add('Sequence', sequence, {'succeeded': 'Sequence_reset', 'aborted': 'Sequence_reset'})
sequence_reset = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence_reset:
Sequence.add('6_1_arm_init', ArmModeState(ArmModeState.Arm_Mode_Init))
Sequence.add('6_2_report', GenerateReportState(image='human_result.png', text='human_result.txt'))
Sequence.add('6_3_finish', SpeakState('task finished'))
StateMachine.add('Sequence_reset', sequence_reset, {'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_person_recognition', state, '/tinker_mission_person_recognition')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def main():
rospy.init_node('tinker_RIPS')
rospy.loginfo(colored('starting RIPS task ...', 'green'))
# load waypoints from xml
pose_list = TKpos.PoseList.parse(open(rospy.get_param('~waypoint_xml'), 'r'))
for pose in pose_list.pose:
WayPointGoalState.waypoint_dict[pose.name] = TKpos.Pose.X(pose)
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('Start_Button', MonitorStartButtonState(),
transitions={'valid': 'Start_Button', 'invalid': 'Sequence'})
sequence_1 = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence_1:
Sequence.add('GoToWaypoin1', WayPointGoalState('waypoint1'), transitions={'aborted': 'GoToWaypoin1'})
Sequence.add('ArriveWaypoint1', SpeakState('Wait for continue'))
StateMachine.add('Sequence', sequence_1, {'succeeded': 'Continue', 'aborted': 'aborted'})
StateMachine.add('Continue', MonitorStartButtonState(),
transitions={'valid': 'Continue', 'invalid': 'Sequence_2'})
sequence_2 = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence_2:
Sequence.add('GoToWaypoin2', WayPointGoalState('waypoint2'), transitions={'aborted': 'GoToWaypoin2'})
Sequence.add('ArriveWaypoint2', SpeakState('I have arrived at way point two'))
Sequence.add('GoToWaypoin3', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3'})
Sequence.add('ArriveWaypoint3', SpeakState('I have arrived at way point three'))
StateMachine.add('Sequence_2', sequence_2, {'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def __init__(self):
global target
rospy.init_node('object', anonymous=False)
rospy.on_shutdown(self.shutdown)
grasp_sm=StateMachine(outcomes=['succeeded','aborted','preempted'])
self.target=Point
gripper_goal = xm_GripperCommandGoal()
gripper_goal.command.position = 0.10
gripper_goal.command.torque = 0.5
# gripper_goal.header.frame
gripper_state = SimpleActionState('xm_move_gripper', xm_GripperCommandAction, goal=gripper_goal,
result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10),
server_wait_timeout=rospy.Duration(10))
#
# self.lift=actionlib.SimpleActionClient('xm_move_arm_height', xm_ArmHeightAction)
# arm_goal = xm_ArmHeightGoal()
# arm_goal.distance_x = 0.5
# arm_goal.distance_z = 0.1
# self.arm_state=SimpleActionState('xm_move_gripper',xm_ArmHeightAction,goal=arm_goal,
# result_cb=self.arm_state_cb, exec_timeout=rospy.Duration(10),
# server_wait_timeout=rospy.Duration(10))
change2arm=ChangeMode("arm")
change2base=ChangeMode("base")
rospy.loginfo("ready")
with grasp_sm:
# StateMachine.add("INIT",ServiceState('xm_move_arm_position/move_position', xm_ArmPosition,"init",response_cb=self.responce_cb),transitions={'succeeded':'FIND_OBJECT'})
StateMachine.add("FIND_OBJECT", ServiceState("Find_Object", xm_ObjectDetect,1, response_cb=self.find_object_cb), transitions={"succeeded":"BACK",'aborted':'FIND_OBJECT'})
StateMachine.add("BACK", Forword_BacK(dis= -0.5), transitions={'succeeded':'CHANGE_2_ARM1'})
StateMachine.add("CHANGE_2_ARM1",change2arm,transitions={'succeeded':"READY"})
StateMachine.add("READY",ServiceState('xm_move_arm_position/move_position', xm_ArmPosition,"prepare",response_cb=self.responce_cb),transitions={'succeeded':'CHANGE_2_BASE1'})
StateMachine.add("CHANGE_2_BASE1",change2base,transitions={"succeeded":"ALIGN_Y"})
StateMachine.add("ALIGN_Y",Left_Right(dis=target.y-0),transitions= {"succeeded" :"CHANGE_2_ARM2"})
StateMachine.add("CHANGE_2_ARM2",change2arm,transitions={'succeeded':"OPEN_GRIPPER"})
StateMachine.add("OPEN_GRIPPER",gripper_state,transitions={'succeeded':'CHANGE_2_BASE2'})
StateMachine.add("CHANGE_2_BASE2",change2base,transitions={'succeeded':""})
outcome=grasp_sm.execute("FIND_OBJECT")
self.goal.target_pose.pose.position.x = position[0]
self.goal.target_pose.pose.position.y = position[1]
self.goal.target_pose.pose.position.z = 0.0
self.goal.target_pose.pose.orientation.x = orientation[0]
self.goal.target_pose.pose.orientation.y = orientation[1]
self.goal.target_pose.pose.orientation.z = orientation[2]
self.goal.target_pose.pose.orientation.w = orientation[3]
def execute(self, userdata):
# Send goal and wait
self.client.send_goal(self.goal)
self.client.wait_for_result()
return 'success'
if __name__ == '__main__':
rospy.init_node('patrol')
# Create an SM to lookup the waypoints, going through them sequentially and continuously
patrol = StateMachine(outcomes=['success'])
with patrol:
for i,w in enumerate(waypoints):
StateMachine.add(w[0],
Waypoint(w[1], w[2]),
transitions={'success':waypoints[(i + 1) % len(waypoints)][0]})
patrol.execute()
class Patrol():
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.setup_task_environment()
#self.setup_initial_pose()
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
# A list to hold then navigation waypoints
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(30.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
# Initialize the patrol state machine
self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])
# 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[4], transitions={'succeeded':'NAV_STATE_5','aborted':'NAV_STATE_5','preempted':'NAV_STATE_4'})
StateMachine.add('NAV_STATE_5', nav_states[5], transitions={'succeeded':'NAV_STATE_6','aborted':'NAV_STATE_6','preempted':'NAV_STATE_4'})
StateMachine.add('NAV_STATE_6', nav_states[6], transitions={'succeeded':'','aborted':'','preempted':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT')
intro_server.start()
sm_outcome = self.sm_patrol.execute()
rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count))
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def setup_initial_pose(self):
# rostopic pub /initialpose geometry_msgs/PoseWithCovarianceSmped '{ header: { frame_id: "map" }, pose: { pose: { position: { x: 2.6731004715, y: 2.44490814209, z: 0 }, orientation: { x: 0 , y: 0, z: 0, w: 1 } } } }'
self.initial_pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, latch=True)
mypose = PoseWithCovarianceStamped()
mypose.header.frame_id = 'map'
mypose.pose.pose = self.waypoints[1]
self.initial_pose_pub.publish(mypose)
def move_base_result_cb(self, userdata, status, result):
if status == actionlib.GoalStatus.SUCCEEDED:
self.n_succeeded += 1
elif status == actionlib.GoalStatus.ABORTED:
self.n_aborted += 1
elif status == actionlib.GoalStatus.PREEMPTED:
self.n_preempted += 1
try:
rospy.loginfo("n_succeeded "+ str(self.n_succeeded))
rospy.loginfo("n_aborted "+ str(self.n_aborted))
rospy.loginfo("n_preempted "+ str(self.n_preempted))
rospy.loginfo("Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted)))
except:
pass
def setup_task_environment(self):
# How big is the square we want the robot to patrol?
self.square_size = rospy.get_param("~square_size", 1.0) # meters
# Set the low battery threshold (between 0 and 100)
self.low_battery_threshold = rospy.get_param('~low_battery_threshold', 50)
# How many times should we execute the patrol loop
self.n_patrols = rospy.get_param("~n_patrols", 2) # meters
# How long do we have to get to each waypoint?
self.move_base_timeout = rospy.get_param("~move_base_timeout", 10) #seconds
# Initialize the patrol counter
self.patrol_count = 0
# 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")
# Orientation list
quaternions = list()
quaternions.append(Quaternion(0.0, 0.0, 0.0, 1.0))
quaternions.append(Quaternion(0.0, 0.0, 1.0, 6.12303176911))
quaternions.append(Quaternion(0.0, 0.0, 0.707106781187, 0.707106781187))
quaternions.append(Quaternion(0.0, 0.0, 1.0, -0.0034963161226))
quaternions.append(Quaternion(0.0, 0.0, -0.707106781187, 0.707106781187))
quaternions.append(Quaternion(0.0, 0.0, 0.707106781187, 0.707106781187))
quaternions.append(Quaternion(0.0, 0.0, 0.707106781187, 0.707106781187))
# Position list
points = list()
points.append(Point(2.6731004715, 2.44490814209, 0))
points.append(Point(2.6731004715, 0.81360769272, 0))
points.append(Point(0.4786823391, 0.81360769272, 0))
points.append(Point(1.6281396152, 2.44490814209, 0))
points.append(Point(-0.6823855638, 2.44490814209, 0))
points.append(Point(1.7326359748, -0.06299890577, 0))
points.append(Point(-0.7520495653, 0.01827591657, 0))
# 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(points[0], quaternions[0]) )
self.waypoints.append( Pose(points[1], quaternions[1]) )
self.waypoints.append( Pose(points[2], quaternions[2]) )
self.waypoints.append( Pose(points[3], quaternions[3]) )
self.waypoints.append( Pose(points[4], quaternions[4]) )
self.waypoints.append( Pose(points[5], quaternions[5]) )
self.waypoints.append( Pose(points[6], quaternions[6]) )
# Where is the docking station?
# let's consider docking station is our kitchen
self.docking_station_pose = (Pose(points[0], quaternions[0]))
# Initialize markers for the waypoints for RViz
self.init_waypoint_markers()
# Set a visualization marker at each waypoint
for waypoint in self.waypoints[1:]:
p = Point()
p = waypoint.position
self.waypoint_markers.points.append(p)
# Initialize a marker for the docking station for RViz
self.init_docking_station_marker()
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
rospy.loginfo("Starting Tasks")
# Publish the waypoint markers
self.marker_pub.publish(self.waypoint_markers)
rospy.sleep(1)
self.marker_pub.publish(self.waypoint_markers) # <- this is weird
# Publish the docking station marker
self.docking_station_marker_pub.publish(self.docking_station_marker)
rospy.sleep(1)
def init_waypoint_markers(self):
# Set up our waypoint markers
marker_scale = 0.2
marker_lifetime = 0 # 0 is forever
marker_ns = 'waypoints'
marker_id = 0
marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0}
# Define a marker publisher.
self.marker_pub = rospy.Publisher('waypoint_markers', Marker)
# Initialize the marker points list.
self.waypoint_markers = Marker()
self.waypoint_markers.ns = marker_ns
self.waypoint_markers.id = marker_id
self.waypoint_markers.type = Marker.CUBE_LIST
self.waypoint_markers.action = Marker.ADD
self.waypoint_markers.lifetime = rospy.Duration(marker_lifetime)
self.waypoint_markers.scale.x = marker_scale
self.waypoint_markers.scale.y = marker_scale
self.waypoint_markers.color.r = marker_color['r']
self.waypoint_markers.color.g = marker_color['g']
self.waypoint_markers.color.b = marker_color['b']
self.waypoint_markers.color.a = marker_color['a']
self.waypoint_markers.header.frame_id = 'odom'
self.waypoint_markers.header.stamp = rospy.Time.now()
self.waypoint_markers.points = list()
def init_docking_station_marker(self):
# Define a marker for the charging station
marker_scale = 0.3
marker_lifetime = 0 # 0 is forever
marker_ns = 'waypoints'
marker_id = 0
marker_color = {'r': 0.7, 'g': 0.7, 'b': 0.0, 'a': 1.0}
self.docking_station_marker_pub = rospy.Publisher('docking_station_marker', Marker)
self.docking_station_marker = Marker()
self.docking_station_marker.ns = marker_ns
self.docking_station_marker.id = marker_id
self.docking_station_marker.type = Marker.CYLINDER
self.docking_station_marker.action = Marker.ADD
self.docking_station_marker.lifetime = rospy.Duration(marker_lifetime)
self.docking_station_marker.scale.x = marker_scale
self.docking_station_marker.scale.y = marker_scale
self.docking_station_marker.scale.z = 0.02
self.docking_station_marker.color.r = marker_color['r']
self.docking_station_marker.color.g = marker_color['g']
self.docking_station_marker.color.b = marker_color['b']
self.docking_station_marker.color.a = marker_color['a']
self.docking_station_marker.header.frame_id = 'odom'
self.docking_station_marker.header.stamp = rospy.Time.now()
self.docking_station_marker.pose = self.docking_station_pose
def shutdown(self):
rospy.loginfo("Stopping the robot...")
#self.sm_patrol.request_preempt()
#self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
def __init__(self):
rospy.init_node("navgation_test")
rospy.on_shutdown(self.shutdown)
self.waypoints=[];
Location= (Point(3.099,-1.796,0),
Point(6.732,-3.260,0),
Point(7.058,-0.119,0),
Point(-0.595,0.069,0),)
quaternions=[];
euler_angles=[0.039,0.056,-3.093,-3.133];
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(4):
self.waypoints.append(Pose(Location[i], quaternions[i]))
point_locations = (('Point1', self.waypoints[0]),
('Point2', self.waypoints[1]),
('Point3', self.waypoints[2]),
('Point4', self.waypoints[3]))
self.room_locations = OrderedDict(point_locations)
# 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))
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
rospy.loginfo("Connected to move_base action server")
nav_states={}
self.people_in_sight=0
# Create a navigation task for each room
for room in self.room_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.header.stamp = rospy.Time.now()
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(100.0),
server_wait_timeout=rospy.Duration(100.0))
nav_states[room] = move_base_state
sm_nav1= StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"])
with sm_nav1:
StateMachine.add('START',MonitorState("task_comming", xm_Task, self.start_cb),
transitions={'invalid':'WAIT4DOOR_OPEN',
'valid':'START',
'preempted':'WAIT4DOOR_OPEN'})
StateMachine.add('WAIT4DOOR_OPEN',MonitorState("doormsg",door_msg,self.door_is_open_cb),
transitions={'invalid':'INIT',
'valid':'WAIT4DOOR_OPEN',
'preempted':'INIT'})
StateMachine.add("INIT", ServiceState("pan_srv", xm_Pan,True, response_cb=self.init_cb),transitions={"succeeded":"NAV_1","aborted":"NAV_1"})
StateMachine.add("NAV_1",nav_states['Point1'],transitions={'succeeded':'','aborted':'','preempted':''})
sm_nav2=StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_nav2:
StateMachine.add("NAV_2",nav_states['Point2'],transitions={'succeeded':'','aborted':'Check_People','preempted':''})
StateMachine.add("Check_People",MonitorState("tracker/tracks_smoothed",TrackArray,self.check_people_cb),
transitions={'valid':'WAIT',
'invalid':'WAIT',
'preempted':'WAIT'})
StateMachine.add('WAIT',wait(time=5,people_pass=self.people_in_sight),
transitions={'succeeded':'NAV_2_END'})
StateMachine.add("NAV_2_END",nav_states['Point2'],transitions={'succeeded':'','aborted':'','preempted':''})
sm_nav3=StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_nav3:
# StateMachine.add("NAV_3",nav_states['Point3'],transitions={'succeeded':'WAIT_4_CMD','aborted':'NAV_3','preempted':'NAV_3'})
sm_follow=Concurrence(outcomes=['succeeded', 'aborted'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
with sm_follow:
Concurrence.add('START_FOLLOW',MonitorState("task_comming", xm_Task, self.follow_stop_cb))
co_follow=Concurrence(outcomes=['succeeded','aborted'],
default_outcome='succeeded',
child_termination_cb=self.co_follow_cb,
outcome_cb=self.co_follow_outcome_cb)
with co_follow:
Concurrence.add('mo_L',MonitorState('people_tf_position',xm_Tracker,
self.pos_M_cb))
Concurrence.add('nav', Nav2Waypoint())
Concurrence.add('FOLLOW',co_follow)
StateMachine.add('WAIT_4_CMD',MonitorState("task_comming", xm_Task, self.follow_start_cb),
transitions={'invalid':'FOLLOW_MODE',
'valid':'FOLLOW_MODE',
'preempted':'FOLLOW_MODE'})
StateMachine.add("FOLLOW_MODE",sm_follow,transitions={'succeeded':'','aborted':''})
sm_nav4=StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"])
with sm_nav4:
StateMachine.add("WAIT_4_BACK",MonitorState("task_comming",xm_Task,self.cmd_back_cb),transitions={"invalid":"NAV_4","valid":"WAIT_4_BACK","preempted":""})
StateMachine.add("NAV_4",nav_states['Point4'],transitions={'succeeded':'','aborted':'','preempted':''})
sm_nav_test=StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"])
with sm_nav_test:
# StateMachine.add("NAV_1",sm_nav1,transitions={'succeeded':'NAV_2','aborted':'NAV_2','preempted':'NAV_2'})
# StateMachine.add("NAV_2",sm_nav2,transitions={'succeeded':'NAV_3','aborted':'NAV_3','preempted':'NAV_3'})
StateMachine.add("NAV_3",sm_nav3,transitions={'succeeded':'NAV_4','aborted':'NAV_4','preempted':'NAV_4'})
StateMachine.add("NAV_4",sm_nav4,transitions={'succeeded':'','aborted':'','preempted':''})
sm_outcome = sm_nav_test.execute()
class main():
def __init__(self):
rospy.init_node('find_treasure', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# How long do we have to get to each waypoint?
self.move_base_timeout = rospy.get_param("~move_base_timeout", 10) #seconds
# Initialize the patrol counter
self.patrol_count = 0
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# 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")
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# First define the corner orientations as Euler angles
euler_angles = (pi/2, pi, 3*pi/2, 0)
# 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.
# -0.163200, 0.044660, 0.193186
self.waypoints.append(Pose(Point(0.9579, 1.8710, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(0.7555, 0.0692, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(-0.72511, 0.4952, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(0.167730, 2.18168, 0.0), quaternions[2]))
position_locations = list()
position_locations.append(('position1', self.waypoints[0]))
position_locations.append(('position2', self.waypoints[1]))
position_locations.append(('position3', self.waypoints[2]))
position_locations.append(('position4', self.waypoints[3]))
self.position_locations = OrderedDict(position_locations)
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
rospy.loginfo("Starting Tasks")
# Initialize a number of parameters and variables
# setup_task_environment(self)
RVizUtils.get_instance().init_primary_waypoint_markers(self.waypoints)
''' Create individual state machines for assigning tasks to each position '''
# Initialize the overall state machine
self.sm_find_treasure = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Build the find treasure state machine from the nav states and treasure finding states
with self.sm_find_treasure:
# First State Machine
sm_nav = StateMachine(outcomes=['succeeded','aborted','preempted'])
sm_nav.userdata.waypoints = self.waypoints
sm_nav.userdata.waypoints_primary_count = len(self.waypoints)
rospy.loginfo(sm_nav.userdata.waypoints)
with sm_nav:
# StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
# transitions={'succeeded':'NAV_WAYPOINT','aborted':'','preempted':''},
# remapping={'waypoint_out':'patrol_waypoint',
# 'waypoints_primary_count':'waypoints_primary_count'})
StateMachine.add('NAV_WAYPOINT', NavState(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'',
'preempted':''},
remapping={'waypoint_in':'patrol_waypoint',
'waypoints_primary_count':'waypoints_primary_count'})
# Second State Machine
sm_read_tags = StateMachine(outcomes=['valid','invalid','preempted'])
sm_read_tags.userdata.waypoints = self.waypoints
with sm_read_tags:
StateMachine.add('read_tag', ReadTagState('ar_pose_marker', AlvarMarkers),
transitions={'invalid':'read_tag', 'valid':'read_tag', 'preempted':''})
# Third State Machine
sm_detect_faces = StateMachine(outcomes=['valid','invalid','preempted'])
with sm_detect_faces:
StateMachine.add('detect_face', FaceDetectState("/camera/rgb/image_color", Image),
transitions={'invalid':'detect_face', 'valid':'detect_face', 'preempted':''})
# Forth State Machine
sm_recognize_faces = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_recognize_faces:
StateMachine.add('recognize_face', FaceRecognitionState(), transitions={'succeeded':'', 'aborted':'', 'preempted':''})
# goal = FaceRecognitionGoal()
# goal.order_id = 1
# goal.order_argument = ''
# StateMachine.add('recognize_face', SimpleActionState('face_recognition',
# FaceRecognitionAction, goal=goal), transitions={'succeeded':'', 'aborted':'', 'preempted':''})
sm_con = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],default_outcome='succeeded',
child_termination_cb=self.child_term_cb, outcome_cb=self.out_cb)
with sm_con:
Concurrence.add('SM_NAV', sm_nav)
Concurrence.add('SM_READ_TAGS', sm_read_tags)
# Concurrence.add('SM_DETECT_FACES', sm_detect_faces)
Concurrence.add('SM_RECOGNIZE_FACES', sm_recognize_faces)
sm_estimate_position = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_estimate_position:
StateMachine.add('estimate_pose', PoseEstimation('ar_pose_marker', AlvarMarkers),
transitions={'succeeded':'', 'aborted':'estimate_pose', 'preempted':''})
StateMachine.add('POSE_ESTIMATE', sm_estimate_position, transitions={'succeeded':'CON','aborted':'CON','preempted':'CON'})
StateMachine.add('CON',sm_con, transitions={'succeeded':'','aborted':'','preempted':''})
# Create and start the SMACH introspection server sm_find_treasure
intro_server = IntrospectionServer('find_treasure', self.sm_find_treasure, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_find_treasure.execute()
rospy.loginfo('the length now is')
rospy.loginfo(self.waypoints)
intro_server.stop()
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.sm_find_treasure.request_preempt()
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
def out_cb(self,outcome_map):
return 'succeeded'
def child_term_cb(self,outcome_map):
rospy.loginfo(outcome_map)
if 'SM_NAV' in outcome_map and (outcome_map['SM_NAV'] == 'succeeded' or outcome_map['SM_NAV'] == 'aborted'):
rospy.loginfo('It visited all points')
return True
return False
Drive(1),
transitions={'success':'TURN_{0}'.format(i + 1)})
# Add all the turns
for i in xrange(sides - 1):
StateMachine.add('TURN_{0}'.format(i + 1),
Turn(360.0 / sides),
transitions={'success':'SIDE_{0}'.format(i + 2)})
# Add the final side
StateMachine.add('SIDE_{0}'.format(sides),
Drive(1),
transitions={'success':'success'})
return polygon
# END PART_1
if __name__ == '__main__':
# BEGIN PART_2
triangle = polygon(3)
square = polygon(4)
# END PART_2
shapes = StateMachine(outcomes=['success'])
with shapes:
StateMachine.add('TRIANGLE', triangle, transitions={'success':'SQUARE'})
StateMachine.add('SQUARE', square, transitions={'success':'success'})
shapes.execute()
def __init__(self):
rospy.init_node('clean_house', 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)
# Turn the room locations into 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 '''
# Create a state machine for the living room subtask(s)
sm_living_room = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Then add the subtask(s)
with sm_living_room:
StateMachine.add('VACUUM_FLOOR', VacuumFloor('living_room', 5), transitions={'succeeded':'','aborted':'','preempted':''})
# Create a state machine for the kitchen subtask(s)
sm_kitchen = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Then add the subtask(s)
with sm_kitchen:
StateMachine.add('MOP_FLOOR', MopFloor('kitchen', 5), transitions={'succeeded':'','aborted':'','preempted':''})
# Create a state machine for the bathroom subtask(s)
sm_bathroom = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Then add the subtasks
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':''})
# Create a state machine for the hallway subtask(s)
sm_hallway = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Then add the subtasks
with sm_hallway:
StateMachine.add('VACUUM_FLOOR', VacuumFloor('hallway', 5), transitions={'succeeded':'','aborted':'','preempted':''})
# Initialize the overall state machine
sm_clean_house = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Build the clean house state machine from the nav states and room cleaning states
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'})
# When the tasks are done, continue on to the 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'})
# When the tasks are done, continue on to the 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'})
# When the tasks are done, return to the 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':''})
# When the tasks are done, stop
StateMachine.add('HALLWAY_TASKS', sm_hallway, transitions={'succeeded':'','aborted':'','preempted':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('clean_house', sm_clean_house, '/SM_ROOT')
intro_server.start()
# Execute the state machine
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 __init__(self):
rospy.init_node("deliver_food", anonymous=False)
self.initialize_destination()
self.lalala = 100
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
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(60.0),
server_wait_timeout=rospy.Duration(10.0),
)
nav_states[room] = move_base_state
rospy.loginfo(
room
+ " -> ["
+ str(round(self.room_locations[room].position.x, 2))
+ ", "
+ str(round(self.room_locations[room].position.y, 2))
+ "]"
)
sm_rotate_search = Concurrence(
outcomes=["find", "not_find"],
default_outcome="not_find",
child_termination_cb=self.concurrence_child_termination_callback,
outcome_cb=self.concurrence_outcome_callback,
)
with sm_rotate_search:
Concurrence.add("ROTATE", Rotate360(0.4, 2 * pi))
Concurrence.add("SEARCH", SearchTable())
sm_table1 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table1:
StateMachine.add(
"GOTO_TABLE1",
nav_states["table1"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "", "preempted": ""}
)
sm_table2 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table2:
StateMachine.add(
"GOTO_TABLE2",
nav_states["table2"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "", "preempted": ""}
)
sm_table3 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table3:
StateMachine.add(
"GOTO_TABLE3",
nav_states["table3"],
transitions={
"succeeded": "ROTATE_SEARCH",
"aborted": "GOTO_CHECKPOINT_2",
"preempted": "GOTO_CHECKPOINT_2",
},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_CHECKPOINT_1a"}
)
# if something wrong when we tried to go to the table
StateMachine.add(
"GOTO_CHECKPOINT_2",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_TABLE3_2", "aborted": "GOTO_CHECKPOINT_1"},
)
StateMachine.add(
"GOTO_CHECKPOINT_1",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_CHECKPOINT_2z", "aborted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_CHECKPOINT_2z",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_TABLE3_2", "aborted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE3_2",
nav_states["table3"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "CLEARING_NOISE_GO"},
)
StateMachine.add(
"CLEARING_NOISE_GO",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_TABLE3_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE3_3",
nav_states["table3"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN"},
)
# if something wrong when we tried to go back to kitchen
StateMachine.add(
"GOTO_CHECKPOINT_1a",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_KITCHEN_2", "aborted": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_CHECKPOINT_2a",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_CHECKPOINT_1a", "aborted": "CLEARING_NOISE_BACK_a"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_a",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_CHECKPOINT_2a", "not_full_rotate": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_KITCHEN_2",
nav_states["kitchen"],
transitions={"succeeded": "", "aborted": "CLEARING_NOISE_BACK_b"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_b",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_KITCHEN_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN_3", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_KITCHEN_2"}
)
sm_table4 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table4:
StateMachine.add(
"GOTO_TABLE4",
nav_states["table4"],
transitions={
"succeeded": "ROTATE_SEARCH",
"aborted": "GOTO_CHECKPOINT_2",
"preempted": "GOTO_CHECKPOINT_2",
},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN",
nav_states["kitchen"],
transitions={"succeeded": "", "aborted": "GOTO_CHECKPOINT_1a", "preempted": "GOTO_CHECKPOINT_1a"},
)
# if something wrong when we tried to go to the table
StateMachine.add(
"GOTO_CHECKPOINT_2",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_TABLE4_2", "aborted": "GOTO_CHECKPOINT_1"},
)
StateMachine.add(
"GOTO_CHECKPOINT_1",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_CHECKPOINT_2", "aborted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE4_2",
nav_states["table4"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "CLEARING_NOISE_GO"},
)
StateMachine.add(
"CLEARING_NOISE_GO",
Rotate360(0.4, 2 * pi),
transitions={"full_rotate": "GOTO_TABLE4_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE4_3",
nav_states["table4"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN"},
)
# if something wrong when we tried to go back to kitchen
StateMachine.add(
"GOTO_CHECKPOINT_1a",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_KITCHEN_2", "aborted": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_CHECKPOINT_2a",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_CHECKPOINT_1a", "aborted": "CLEARING_NOISE_BACK_a"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_a",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_CHECKPOINT_2a", "not_full_rotate": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_KITCHEN_2",
nav_states["kitchen"],
transitions={"succeeded": "", "aborted": "CLEARING_NOISE_BACK_b"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_b",
Rotate360(0.4, 2 * pi),
transitions={"full_rotate": "GOTO_KITCHEN_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN_3", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_KITCHEN_2"}
)
sm_table5 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table5:
StateMachine.add(
"GOTO_TABLE5",
nav_states["table5"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "", "preempted": ""}
)
sm_table6 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table6:
StateMachine.add(
"GOTO_TABLE6",
nav_states["table6"],
transitions={
"succeeded": "ROTATE_SEARCH",
"aborted": "GOTO_CHECKPOINT_2",
"preempted": "GOTO_CHECKPOINT_2",
},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN",
nav_states["kitchen"],
transitions={"succeeded": "", "aborted": "GOTO_CHECKPOINT_1a", "preempted": "GOTO_CHECKPOINT_1a"},
)
# if something wrong when we tried to go to the table
StateMachine.add(
"GOTO_CHECKPOINT_2",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_TABLE6_2", "aborted": "GOTO_CHECKPOINT_1"},
)
StateMachine.add(
"GOTO_CHECKPOINT_1",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_CHECKPOINT_2", "aborted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE6_2",
nav_states["table6"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "CLEARING_NOISE_GO"},
)
StateMachine.add(
"CLEARING_NOISE_GO",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_TABLE6_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE6_3",
nav_states["table6"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN"},
)
# if something wrong when we tried to go back to kitchen
StateMachine.add(
"GOTO_CHECKPOINT_1a",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_KITCHEN_2", "aborted": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_CHECKPOINT_2a",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_CHECKPOINT_1a", "aborted": "CLEARING_NOISE_BACK_a"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_a",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_CHECKPOINT_2a", "not_full_rotate": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_KITCHEN_2",
nav_states["kitchen"],
transitions={"succeeded": "", "aborted": "CLEARING_NOISE_BACK_b"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_b",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_KITCHEN_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN_3", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_KITCHEN_2"}
)
# let's initialize the overall state machine
sm_deliverfood = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_deliverfood:
StateMachine.add("STARTING_TASK", Welcome(), transitions={"succeeded": "COMPUTER_VISION_TASK"})
StateMachine.add(
"COMPUTER_VISION_TASK",
ComputerVision(),
transitions={
"detect1": "TABLE_ONE_TASK",
"detect2": "TABLE_TWO_TASK",
"detect3": "TABLE_THREE_TASK",
"detect4": "TABLE_FOUR_TASK",
"detect5": "TABLE_FIVE_TASK",
"detect6": "TABLE_SIX_TASK",
"preempted": "",
},
)
StateMachine.add(
"TABLE_ONE_TASK",
sm_table1,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"TABLE_TWO_TASK",
sm_table2,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"TABLE_THREE_TASK",
sm_table3,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"TABLE_FOUR_TASK",
sm_table4,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"TABLE_FIVE_TASK",
sm_table5,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"TABLE_SIX_TASK",
sm_table6,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"GOTO_KITCHEN",
nav_states["kitchen"],
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
# Create and start the SMACH introspection server
intro_server = IntrospectionServer("deliver_food", sm_deliverfood, "/SM_ROOT")
intro_server.start()
# Execute the state machine
sm_outcome = sm_deliverfood.execute()
rospy.on_shutdown(self.shutdown)
class main:
def __init__(self):
rospy.init_node("follow")
rospy.on_shutdown(self.shutdown)
self.state = None
self.sm_follow = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
# 总状态机follow
with self.sm_follow:
self.co_follow = Concurrence(
outcomes=["succeeded", "preempted", "aborted"],
default_outcome="succeeded",
# outcome_cb = self.co_follow_outcome_cb ,
# child_termination_cb=self.co_follow_child_cb
)
with self.co_follow:
Concurrence.add("mo_L", MonitorState("people_position_estimation", PositionMeasurement, self.pos_M_cb))
Concurrence.add("nav", Nav2Waypoint())
# self.sm_nav=StateMachine(outcomes=('succeeded','aborted','preempted'))
# with self.sm_nav:
# # StateMachine.add('wait',MonitorState('sr_data',Int16,self.wait_cb),transitions={'valid':'wait','invalid':'nav_speech','preempted':'nav_speech'})
#
# self.speech_nav=Concurrence(outcomes=['get_in','succeeded'],
# default_outcome='succeeded',
# outcome_cb=self.speech_nav_outcome_cb,
# child_termination_cb=self.speech_nav_child_termination_cb
# )
# with self.speech_nav:
# Concurrence.add('speech',MonitorState('sr_data',Int16,self.nav_speech_cb))
# Concurrence.add('nav', Nav2Waypoint())
# StateMachine.add('nav_speech',self.speech_nav,transitions={'get_in':'Get_in',})
# self.get_in_speech=Concurrence(outcomes=['get_out','succeeded'],
# default_outcome='succeeded',
# outcome_cb=self.speech_get_in_outcome_cb,
# child_termination_cb=self.speech_get_in_child_termination_cb
#
# )
# with self.get_in_speech:
# Concurrence.add('speech',MonitorState('sr_data',Int16,self.get_in_speech_cb))
# Concurrence.add('get_in',Get_in())
# StateMachine.add('Get_in',self.get_in_speech,transitions={'get_out':'Get_out'})
# StateMachine.add('Get_out',Get_out(), transitions={'succeeded':'nav_speech','preempted':'','aborted':'Get_out' })
# Concurrence.add('Nav',self.sm_nav)
StateMachine.add("Nav_top", self.co_follow)
a = self.sm_follow.execute()
def pos_M_cb(self, userdata, msg):
global current_people_pose, updata_flag
# lock= threading.Lock()
# with lock:
if msg.pos is not None:
current_people_pose = msg.pos
updata_flag += 1
return True
def wait_cb(self, userdata, msg):
global FOLLOW
if msg.data == FOLLOW:
return False
else:
return True
def nav_speech_cb(self, userdata, msg):
global GET_IN, GET_OUT
if msg.data == GET_IN:
self.state = "get_in"
return False
else:
return True
def get_in_speech_cb(self, userdata, msg):
global GET_IN, GET_OUT
if msg.data == GET_OUT:
self.state = "get_out"
return False
else:
return True
def speech_nav_outcome_cb(self, outcome_map):
if outcome_map["speech"] == "invalid":
if self.state == "get_in":
return "get_in"
def speech_nav_child_termination_cb(self, outcome_map):
if outcome_map["speech"] == "invalid":
return True
def speech_get_in_outcome_cb(self, outcome_map):
if outcome_map["speech"] == "invalid":
if self.state == "get_out":
return "get_out"
def speech_get_in_child_termination_cb(self, outcome_map):
if outcome_map["speech"] == "invalid":
return True
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.sm_follow.request_preempt()
rospy.sleep(1)
pass
def execute(self, userdata):
print 'one'
sleep(1)
return 'success' # One of the outcomes should be returned
class Two(State):
def __init__(self):
# Call the parent class constructor, passing all the possible outcomes of the state
State.__init__(self, outcomes=['success'])
# execute method - where the actions are descripted
def execute(self, userdata):
print 'two'
sleep(1)
return 'success' # One of the outcomes should be returned
if __name__ == '__main__':
# Create a StateMachine, passing a list of its possible overall outcomes
sm = StateMachine(outcomes=['success'])
# Populate the state machine with our states
with sm:
# (state_name, state_instance, transitions={dict with the transitions})
StateMachine.add('ONE', One(), transitions={'success':'TWO'})
StateMachine.add('TWO', Two(), transitions={'success':'ONE'})
# Execute the state machine
sm.execute()
class main():
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()
def move_base_result_cb(self, userdata, status, result):
if status == actionlib.GoalStatus.SUCCEEDED:
self.n_succeeded += 1
elif status == actionlib.GoalStatus.ABORTED:
self.n_aborted += 1
elif status == actionlib.GoalStatus.PREEMPTED:
self.n_preempted += 1
try:
rospy.loginfo("Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted)))
except:
pass
def init(self):
# How big is the square we want the robot to patrol?
self.square_size = rospy.get_param("~square_size", 1.0) # meters
# How many times should we execute the patrol loop
self.n_patrols = rospy.get_param("~n_patrols", 3) # meters
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# First define the corner orientations as Euler angles
euler_angles = (pi/2, pi, 3*pi/2, 0)
# 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, 0.0, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(self.square_size, 0.0, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(0.0, self.square_size, 0.0), quaternions[2]))
# Initialize the waypoint visualization markers for RViz
self.init_waypoint_markers()
# Set a visualization marker at each waypoint
for waypoint in self.waypoints:
p = Point()
p = waypoint.position
self.waypoint_markers.points.append(p)
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
rospy.loginfo("Starting SMACH test")
# Publish the waypoint markers
self.marker_pub.publish(self.waypoint_markers)
rospy.sleep(1)
self.marker_pub.publish(self.waypoint_markers)
def init_waypoint_markers(self):
# Set up our waypoint markers
marker_scale = 0.2
marker_lifetime = 0 # 0 is forever
marker_ns = 'waypoints'
marker_id = 0
marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0}
# Define a marker publisher.
self.marker_pub = rospy.Publisher('waypoint_markers', Marker)
# Initialize the marker points list.
self.waypoint_markers = Marker()
self.waypoint_markers.ns = marker_ns
self.waypoint_markers.id = marker_id
self.waypoint_markers.type = Marker.CUBE_LIST
self.waypoint_markers.action = Marker.ADD
self.waypoint_markers.lifetime = rospy.Duration(marker_lifetime)
self.waypoint_markers.scale.x = marker_scale
self.waypoint_markers.scale.y = marker_scale
self.waypoint_markers.color.r = marker_color['r']
self.waypoint_markers.color.g = marker_color['g']
self.waypoint_markers.color.b = marker_color['b']
self.waypoint_markers.color.a = marker_color['a']
self.waypoint_markers.header.frame_id = 'odom'
self.waypoint_markers.header.stamp = rospy.Time.now()
self.waypoint_markers.points = list()
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.sm_patrol.request_preempt()
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
class SMACHAI():
def __init__(self):
rospy.init_node('petit_smach_ai', 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 = (pi/2, pi, 3*pi/2, 0)
# 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()
self.square_size = 1.0
# 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, 0.0, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(self.square_size, 0.0, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(0.0, self.square_size, 0.0), quaternions[2]))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist)
self.stopping = False
self.recharging = False
self.robot_side = 1
# State machine for Action1
self.sm_action1 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action1.userdata.mandibles_sleep = 0.1
with self.sm_action1:
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'UPDATE_DROPCUBE_OBJ',
'aborted':'aborted'})
# StateMachine.add('UPDATE_DROPCUBE_OBJ', UpdateObjectiveDropCubes(),
# transitions={'succeeded':'CLOSE_MANDIBLES',
# 'aborted':'aborted'})
StateMachine.add('UPDATE_DROPCUBE_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioCube_cb, response_cb=self.updatePrioCube_cb,
output_keys=['waypoint_out'],
input_keys=['robot_side']),
transitions={'succeeded':'HCLOSE_MANDIBLES',
'aborted':'HCLOSE_MANDIBLES'},
remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action2
self.sm_action2 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action2.userdata.speed = -0.1;
self.sm_action2.userdata.distance = 0.3;
self.sm_action2.userdata.mandibles_sleep = 0.1
with self.sm_action2:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'OPEN_MANDIBLES',
'aborted':'aborted'})
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'MOVE_BACKWARD',
'aborted':'aborted'})
StateMachine.add('MOVE_BACKWARD', MoveForward(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action3
self.sm_action3 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action3.userdata.mandibles_sleep = 0.1
self.sm_action3.userdata.speed = -0.1;
self.sm_action3.userdata.distance = 0.2;
with self.sm_action3:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'MOVE_PUSH',
'aborted':'aborted'})
StateMachine.add('MOVE_PUSH', MovePush(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action4
self.sm_action4 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action4.userdata.mandibles_sleep = 0.1
self.sm_action4.userdata.speed_x = 0.1
self.sm_action4.userdata.speed_y = 0.1
self.sm_action4.userdata.distance = 0.5;
with self.sm_action4:
StateMachine.add('CLOSE_MANDIBLES', CloseMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'SLIDE',
'aborted':'aborted'})
StateMachine.add('SLIDE', Slide(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action5
self.sm_action5 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action5.userdata.mandibles_sleep = 0.1
with self.sm_action5:
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'UPDATE_DROPSHELL_OBJ',
'aborted':'aborted'})
# StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(),
# transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES',
# 'aborted':'aborted'})
StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb,
output_keys=['waypoint_out'],
input_keys=['robot_side']),
transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES',
'aborted':'ALMOSTCLOSE_MANDIBLES'})
StateMachine.add('ALMOSTCLOSE_MANDIBLES', AlmostCloseMandibles(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action6
self.sm_action6 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action6.userdata.mandibles_sleep = 0.1
self.sm_action6.userdata.speed = 0.1;
self.sm_action6.userdata.distance = 0.2;
with self.sm_action6:
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'FORWARD',
'aborted':'aborted'})
StateMachine.add('FORWARD', MoveForward(),
transitions={'succeeded':'UPDATE_DROPSHELL_OBJ',
'aborted':'aborted'})
# StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(),
# transitions={'succeeded':'HCLOSE_MANDIBLES',
# 'aborted':'aborted'})
StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb,
output_keys=['waypoint_out'],
input_keys=['robot_side']),
transitions={'succeeded':'HCLOSE_MANDIBLES',
'aborted':'HCLOSE_MANDIBLES'})
StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action7
self.sm_action7 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action7.userdata.mandibles_sleep = 0.1
with self.sm_action7:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Actions
self.sm_actions = StateMachine(outcomes=['succeeded','aborted','preempted'])
self.sm_actions.userdata.waypoints = self.waypoints
with self.sm_actions:
StateMachine.add('PICK_WAYPOINT', ServiceState('/PETIT/get_objective', GetObjective, response_cb=self.objective_cb,
output_keys=['waypoint_out'],
outcomes=['action1','action2','action3','action4','action5','action6','action7','aborted','succeeded','preempted']),
transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','action7':'SM_ACTION7','aborted':'SM_ACTION1'},
remapping={'waypoint_out':'patrol_waypoint'})
#StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
# transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','aborted':'SM_ACTION1'},
# remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('SM_ACTION1', self.sm_action1, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION2', self.sm_action2, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION3', self.sm_action3, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION4', self.sm_action4, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION5', self.sm_action5, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION6', self.sm_action6, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION7', self.sm_action7, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('REMOVE_OBJECTIVE', RemoveObjective(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'},
remapping={'waypoint_in':'remove_waypoint'})
# State machine with concurrence
self.sm_concurrent = Concurrence(outcomes=['succeeded', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_actions machine and a battery MonitorState to the nav_patrol machine
with self.sm_concurrent:
Concurrence.add('SM_ACTIONS', self.sm_actions)
Concurrence.add('MONITOR_TIME', MonitorState("/GENERAL/remain", Int32, self.time_cb))
Concurrence.add('MONITOR_BATTERY', MonitorState("/PETIT/adc", Int32, 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:
# @smach.cb_interface()
# def requestPrioCube_cb(userdata, request):
# update_request = UpdatePriority().Request
# update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300)
# update_request.goal.pose.position.y = 0.800
# update_request.prio = 100
# return update_request
StateMachine.add('WAIT_COLOR', MonitorState("/GENERAL/color", Int32, self.color_cb), transitions={'valid':'WAIT_START', 'preempted':'WAIT_START', 'invalid':'WAIT_START'})
StateMachine.add('WAIT_START', MonitorState("/GENERAL/start", Empty, self.start_cb), transitions={'valid':'CONCURRENT', 'preempted':'CONCURRENT', 'invalid':'CONCURRENT'})
StateMachine.add('CONCURRENT', self.sm_concurrent, transitions={'succeeded':'CONCURRENT', '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()
def time_cb(self, userdata, msg):
if msg.data < 2:
self.stopping = True
return False
else:
self.stopping = False
return True
def start_cb(self, userdata, msg):
rospy.loginfo("Start !")
return False
def color_cb(self, userdata, msg):
rospy.loginfo("Color " + str(msg.data))
self.robot_side = msg.data
self.sm_action1.userdata.robot_side = self.robot_side
self.sm_action2.userdata.robot_side = self.robot_side
self.sm_action3.userdata.robot_side = self.robot_side
self.sm_action4.userdata.robot_side = self.robot_side
self.sm_action5.userdata.robot_side = self.robot_side
self.sm_action6.userdata.robot_side = self.robot_side
self.sm_action7.userdata.robot_side = self.robot_side
self.sm_top.userdata.robot_side = self.robot_side # TODO REMOVE
return False
def battery_cb(self, userdata, msg):
if msg.data < 320:
self.recharging = True
return False
else:
self.recharging = False
return True
def objective_cb(self, userdata, response):
#objective_response = GetObjective().Response
userdata.waypoint_out = response.goal
waypoint_type = response.type.data
rospy.loginfo("goal: " + str(response.goal))
if(waypoint_type == 1):
return 'action1'
if(waypoint_type == 2):
return 'action2'
if(waypoint_type == 3):
return 'action3'
if(waypoint_type == 4):
return 'action4'
if(waypoint_type == 5):
return 'action5'
if(waypoint_type == 6):
return 'action6'
if(waypoint_type == 7):
return 'action7'
return 'aborted'
def requestPrioCube_cb(self, userdata, request):
rospy.loginfo("Side : " + str(userdata.robot_side))
update_request = UpdatePriorityRequest()
update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 1.300)
update_request.goal.pose.position.y = 1.000
update_request.prio.data = 100
rospy.loginfo("Request Priority Drop cubes update")
return update_request
#request.goal.pose.position.x = userdata.robot_side*(1.500 - 1.300)
#request.goal.pose.position.y = 1.000
#request.prio = 100
#return request
def updatePrioCube_cb(self, userdata, response):
rospy.loginfo("Priority Drop cubes updated")
return 'aborted'
def requestPrioShell_cb(self, userdata, request):
rospy.loginfo("Side : " + str(userdata.robot_side))
update_request = UpdatePriorityRequest()
update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300)
update_request.goal.pose.position.y = 0.800
update_request.prio.data = 100
rospy.loginfo("Request Priority Drop shell update")
return update_request
#request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300)
#request.goal.pose.position.y = 0.800
#request.prio = 100
#return request
def updatePrioShell_cb(self, userdata, response):
rospy.loginfo("Priority Drop shell updated")
return 'aborted'
# Gets called when ANY child state terminates
def concurrence_child_termination_cb(self, outcome_map):
# If the current navigation task has succeeded, return True
if outcome_map['SM_ACTIONS'] == 'succeeded':
return True
# If the MonitorState state returns False (invalid), store the current nav goal and recharge
if outcome_map['MONITOR_TIME'] == 'invalid':
rospy.loginfo("LOW TIME! NEED TO STOP...")
return True
if outcome_map['MONITOR_BATTERY'] == 'invalid':
rospy.loginfo("LOW BATTERY! NEED TO STOP...")
return True
else:
return False
# Gets called when ALL child states are terminated
def concurrence_outcome_cb(self, outcome_map):
# If the battery is below threshold, return the 'recharge' outcome
if outcome_map['MONITOR_TIME'] == 'invalid':
rospy.loginfo("TIME FINISHED !! GOING TO STOP ! ")
return 'stop'
if outcome_map['MONITOR_BATTERY'] == 'invalid':
return 'stop'
# Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop'
elif outcome_map['SM_ACTIONS'] == 'succeeded':
#self.patrol_count += 1
#rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count))
# If we have not completed all our patrols, start again at the beginning
#if self.n_patrols == -1 or self.patrol_count < self.n_patrols:
#self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData())
return 'succeeded'
# Otherwise, we are finished patrolling so return 'stop'
#else:
#self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData())
#return 'stop'
# Recharge if all else fails
else:
return 'recharge'
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.sm_actions.request_preempt()
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
class SMACHAI():
def __init__(self):
rospy.init_node('petit_smach_ai', 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, pi, pi/2, -pi/2, pi/4, 0)
# 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()
self.square_size = 1.0
# 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.4, 0.65, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(-1.0, 0.50, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(-0.4, 0.35, 0.0), quaternions[2]))
self.waypoints.append(Pose(Point(-0.8, 0.65, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(-1.2, 0.30, 0.0), quaternions[4]))
self.waypoints.append(Pose(Point(-0.4, 0.657, 0.0), quaternions[5]))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist)
self.stopping = False
self.recharging = False
self.robot_side = 1
# State machine for Actions
self.sm_actions = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_actions:
# Calib X
StateMachine.add('CALIBRATE_X', CalibX(),
transitions={'succeeded':'CALIBRATE_Y',
'aborted':'aborted'})
# Calib Y
StateMachine.add('CALIBRATE_Y', CalibY(),
transitions={'succeeded':'MAX_SPEED_1',
'aborted':'aborted'})
# PICK-UP
# Change speed
StateMachine.add('MAX_SPEED_1', MaxLinearSpeed(70000),
transitions={'succeeded':'GOTO_1_1',
'aborted':'aborted'})
# NavPoint 1
StateMachine.add('GOTO_1_1', Nav2Waypoint(self.waypoints[0]),
transitions={'succeeded':'MAX_SPEED_2',
'aborted':'aborted'})
# Change speed
StateMachine.add('MAX_SPEED_2', MaxLinearSpeed(40000),
transitions={'succeeded':'PAUSE_1',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_1', Pause(1.0),
transitions={'succeeded':'FORWARD_1',
'aborted':'aborted'})
# Avancer
StateMachine.add('FORWARD_1', MoveForward(0.15),
transitions={'succeeded':'FORKS_10',
'aborted':'aborted'})
# Monter fourches
StateMachine.add('FORKS_10', Forks(90),
transitions={'succeeded':'FORKS_11',
'aborted':'aborted'})
StateMachine.add('FORKS_11', Forks(87),
transitions={'succeeded':'FORKS_12',
'aborted':'aborted'})
StateMachine.add('FORKS_12', Forks(83),
transitions={'succeeded':'BACKWARD_1',
'aborted':'aborted'})
# Reculer
StateMachine.add('BACKWARD_1', MoveForward(-0.15),
transitions={'succeeded':'ROTATE_1',
'aborted':'aborted'})
# Rotation
StateMachine.add('ROTATE_1', MoveRotate(pi),
transitions={'succeeded':'MAX_SPEED_3',
'aborted':'aborted'})
# DROP-OFF
# Change speed
StateMachine.add('MAX_SPEED_3', MaxLinearSpeed(70000),
transitions={'succeeded':'GOTO_2_1',
'aborted':'aborted'})
# NavPoint 2
StateMachine.add('GOTO_2_1', Nav2Waypoint(self.waypoints[1]),
transitions={'succeeded':'MAX_SPEED_4',
'aborted':'aborted'})
# Change speed
StateMachine.add('MAX_SPEED_4', MaxLinearSpeed(40000),
transitions={'succeeded':'PAUSE_2',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_2', Pause(1.0),
transitions={'succeeded':'FORWARD_2',
'aborted':'aborted'})
# Avancer
StateMachine.add('FORWARD_2', MoveForward(0.13),
transitions={'succeeded':'FORKS_20',
'aborted':'aborted'})
# Baisser fourches
StateMachine.add('FORKS_20', Forks(87),
transitions={'succeeded':'FORKS_21',
'aborted':'aborted'})
StateMachine.add('FORKS_21', Forks(91),
transitions={'succeeded':'FORKS_22',
'aborted':'aborted'})
StateMachine.add('FORKS_22', Forks(95),
transitions={'succeeded':'BACKWARD_2',
'aborted':'aborted'})
# Reculer
StateMachine.add('BACKWARD_2', MoveForward(-0.15),
transitions={'succeeded':'ROTATE_2',
'aborted':'aborted'})
# Rotation
StateMachine.add('ROTATE_2', MoveRotate(0),
transitions={'succeeded':'GOTO_3_1',
'aborted':'aborted'})
# NavPoint 3
StateMachine.add('GOTO_3_1', Nav2Waypoint(self.waypoints[2]),
transitions={'succeeded':'PAUSE_3',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_3', Pause(2.0),
transitions={'succeeded':'GOTO_4_1',
'aborted':'aborted'})
# NavPoint 4
StateMachine.add('GOTO_4_1', Nav2Waypoint(self.waypoints[3]),
transitions={'succeeded':'PAUSE_4',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_4', Pause(2.0),
transitions={'succeeded':'GOTO_2_2',
'aborted':'aborted'})
# PICK-UP
# NavPoint 2
StateMachine.add('GOTO_2_2', Nav2Waypoint(self.waypoints[1]),
transitions={'succeeded':'PAUSE_5',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_5', Pause(1.0),
transitions={'succeeded':'FORWARD_3',
'aborted':'aborted'})
# Avancer
StateMachine.add('FORWARD_3', MoveForward(0.15),
transitions={'succeeded':'FORKS_30',
'aborted':'aborted'})
# Monter fourches
StateMachine.add('FORKS_30', Forks(90),
transitions={'succeeded':'FORKS_31',
'aborted':'aborted'})
StateMachine.add('FORKS_31', Forks(87),
transitions={'succeeded':'FORKS_32',
'aborted':'aborted'})
StateMachine.add('FORKS_32', Forks(83),
transitions={'succeeded':'BACKWARD_3',
'aborted':'aborted'})
# Reculer
StateMachine.add('BACKWARD_3', MoveForward(-0.15),
transitions={'succeeded':'ROTATE_3',
'aborted':'aborted'})
# Rotation
StateMachine.add('ROTATE_3', MoveRotate(0),
transitions={'succeeded':'GOTO_1_2',
'aborted':'aborted'})
# DROP-OFF
# NavPoint 1
StateMachine.add('GOTO_1_2', Nav2Waypoint(self.waypoints[5]),
transitions={'succeeded':'PAUSE_6',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_6', Pause(1.0),
transitions={'succeeded':'FORWARD_4',
'aborted':'aborted'})
# Avancer
StateMachine.add('FORWARD_4', MoveForward(0.13),
transitions={'succeeded':'FORKS_40',
'aborted':'aborted'})
# Baisser fourches
StateMachine.add('FORKS_40', Forks(87),
transitions={'succeeded':'FORKS_41',
'aborted':'aborted'})
StateMachine.add('FORKS_41', Forks(91),
transitions={'succeeded':'FORKS_42',
'aborted':'aborted'})
StateMachine.add('FORKS_42', Forks(95),
transitions={'succeeded':'BACKWARD_4',
'aborted':'aborted'})
# Reculer
StateMachine.add('BACKWARD_4', MoveForward(-0.15),
transitions={'succeeded':'ROTATE_4',
'aborted':'aborted'})
# Rotation
StateMachine.add('ROTATE_4', MoveRotate(pi),
transitions={'succeeded':'GOTO_5_1',
'aborted':'aborted'})
# NavPoint 5
StateMachine.add('GOTO_5_1', Nav2Waypoint(self.waypoints[4]),
transitions={'succeeded':'CALIBRATE_X',
'aborted':'aborted'})
# 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('ACTIONS', self.sm_actions, transitions={'succeeded':'ACTIONS', 'aborted':'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()
def time_cb(self, userdata, msg):
if msg.data < 2:
self.stopping = True
return False
else:
self.stopping = False
return True
def start_cb(self, userdata, msg):
rospy.loginfo("Start !")
return False
def color_cb(self, userdata, msg):
rospy.loginfo("Color " + str(msg.data))
self.robot_side = msg.data
self.sm_action1.userdata.robot_side = self.robot_side
self.sm_action2.userdata.robot_side = self.robot_side
self.sm_action3.userdata.robot_side = self.robot_side
self.sm_action4.userdata.robot_side = self.robot_side
self.sm_action5.userdata.robot_side = self.robot_side
self.sm_action6.userdata.robot_side = self.robot_side
self.sm_action7.userdata.robot_side = self.robot_side
self.sm_top.userdata.robot_side = self.robot_side # TODO REMOVE
return False
def battery_cb(self, userdata, msg):
if msg.data < 320:
self.recharging = True
return False
else:
self.recharging = False
return True
def objective_cb(self, userdata, response):
#objective_response = GetObjective().Response
userdata.waypoint_out = response.goal
waypoint_type = response.type.data
rospy.loginfo("goal: " + str(response.goal))
if(waypoint_type == 1):
return 'action1'
if(waypoint_type == 2):
return 'action2'
if(waypoint_type == 3):
return 'action3'
if(waypoint_type == 4):
return 'action4'
if(waypoint_type == 5):
return 'action5'
if(waypoint_type == 6):
return 'action6'
if(waypoint_type == 7):
return 'action7'
return 'aborted'
def requestPrioCube_cb(self, userdata, request):
rospy.loginfo("Side : " + str(userdata.robot_side))
update_request = UpdatePriorityRequest()
update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 1.300)
update_request.goal.pose.position.y = 1.000
update_request.prio.data = 100
rospy.loginfo("Request Priority Drop cubes update")
return update_request
#request.goal.pose.position.x = userdata.robot_side*(1.500 - 1.300)
#request.goal.pose.position.y = 1.000
#request.prio = 100
#return request
def updatePrioCube_cb(self, userdata, response):
rospy.loginfo("Priority Drop cubes updated")
return 'aborted'
def requestPrioShell_cb(self, userdata, request):
rospy.loginfo("Side : " + str(userdata.robot_side))
update_request = UpdatePriorityRequest()
update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300)
update_request.goal.pose.position.y = 0.800
update_request.prio.data = 100
rospy.loginfo("Request Priority Drop shell update")
return update_request
#request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300)
#request.goal.pose.position.y = 0.800
#request.prio = 100
#return request
def updatePrioShell_cb(self, userdata, response):
rospy.loginfo("Priority Drop shell updated")
return 'aborted'
# Gets called when ANY child state terminates
def concurrence_child_termination_cb(self, outcome_map):
# If the current navigation task has succeeded, return True
if outcome_map['SM_ACTIONS'] == 'succeeded':
return True
# If the MonitorState state returns False (invalid), store the current nav goal and recharge
if outcome_map['MONITOR_TIME'] == 'invalid':
rospy.loginfo("LOW TIME! NEED TO STOP...")
return True
if outcome_map['MONITOR_BATTERY'] == 'invalid':
rospy.loginfo("LOW BATTERY! NEED TO STOP...")
return True
else:
return False
# Gets called when ALL child states are terminated
def concurrence_outcome_cb(self, outcome_map):
# If the battery is below threshold, return the 'recharge' outcome
if outcome_map['MONITOR_TIME'] == 'invalid':
rospy.loginfo("TIME FINISHED !! GOING TO STOP ! ")
return 'stop'
if outcome_map['MONITOR_BATTERY'] == 'invalid':
return 'stop'
# Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop'
elif outcome_map['SM_ACTIONS'] == 'succeeded':
#self.patrol_count += 1
#rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count))
# If we have not completed all our patrols, start again at the beginning
#if self.n_patrols == -1 or self.patrol_count < self.n_patrols:
#self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData())
return 'succeeded'
# Otherwise, we are finished patrolling so return 'stop'
#else:
#self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData())
#return 'stop'
# Recharge if all else fails
else:
return 'recharge'
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.sm_actions.request_preempt()
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
class Patrol:
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
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 list to hold then navigation waypoints
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(10.0),
server_wait_timeout=rospy.Duration(10.0),
)
nav_states.append(move_base_state)
# Initialize the patrol state machine
self.sm_patrol = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
# 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": "", "aborted": "", "preempted": ""}
)
# Create and start the SMACH introspection server
intro_server = IntrospectionServer("patrol", self.sm_patrol, "/SM_ROOT")
intro_server.start()
# Execute the state machine for the specified number of patrols
while (self.n_patrols == -1 or self.patrol_count < self.n_patrols) and not rospy.is_shutdown():
sm_outcome = self.sm_patrol.execute()
self.patrol_count += 1
rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count))
rospy.loginfo("State Machine Outcome: " + str(sm_outcome))
intro_server.stop()
def move_base_result_cb(self, userdata, status, result):
if status == actionlib.GoalStatus.SUCCEEDED:
self.n_succeeded += 1
elif status == actionlib.GoalStatus.ABORTED:
self.n_aborted += 1
elif status == actionlib.GoalStatus.PREEMPTED:
self.n_preempted += 1
try:
rospy.loginfo(
"Success rate: "
+ str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted))
)
except:
pass
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.sm_patrol.request_preempt()
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
def __init__(self):
global DIS_CHANGED, ANGLE_CHANGED, OBJECT_DIS, ARM_LENGTH, OBJECT_POS, AR_POS,OBJECT_NAME,OBJECT_ID
rospy.init_node("object", anonymous=False)
rospy.on_shutdown(self.shutdown)
id_list=[3,1,4]
self.monitor_times = 0
change2base = ChangeMode("base")
change2arm = ChangeMode("arm")
wrist_goal = xm_WristPositionGoal()
wrist_goal.angle = 0.785
wrist_up_state = SimpleActionState("xm_move_wrist", xm_WristPositionAction, goal=wrist_goal,
result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(20),
server_wait_timeout=rospy.Duration(20))
wrist_goal = xm_WristPositionGoal()
wrist_goal.angle = 0
wrist_down_state = SimpleActionState("xm_move_wrist", xm_WristPositionAction, goal=wrist_goal,
result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(20),
server_wait_timeout=rospy.Duration(20))
gripper_goal = xm_GripperCommandGoal()
gripper_goal.command.position = 0.15
gripper_open_state = SimpleActionState('xm_move_gripper', xm_GripperCommandAction, goal=gripper_goal,
result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10),
server_wait_timeout=rospy.Duration(10))
#
gripper_goal = xm_GripperCommandGoal()
gripper_goal.command.position = 0.05
gripper_close_state = SimpleActionState('xm_move_gripper', xm_GripperCommandAction, goal=gripper_goal,
result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10),
server_wait_timeout=rospy.Duration(10))
init_sm = StateMachine(outcomes=["succeeded", "aborted", "preempted","valid","invalid"])
with init_sm:
StateMachine.add("CHANGE_2_ARM0", change2arm, transitions={'succeeded': "INIT","aborted":"CHANGE_2_ARM0"})
StateMachine.add("INIT", ServiceState('xm_move_arm_position/move_position', xm_ArmPosition, "init",
response_cb=self.responce_cb),
transitions={'succeeded': 'WRIST_DOWN0'})
StateMachine.add("WRIST_DOWN0", wrist_down_state, transitions={"succeeded": "CLOSE_GRIPPER0"})
StateMachine.add("CLOSE_GRIPPER0", gripper_close_state, transitions={'succeeded': 'CHANGE_2_BASE0'})
StateMachine.add("CHANGE_2_BASE0", change2base, transitions={"succeeded": "SPEAK_READY","aborted":"CHANGE_2_BASE0"})
# StateMachine.add("WAIT_4_START", MonitorState("task_comming", xm_Task, self.start_cb),
# transitions={'invalid': 'SPEAK_READY',
# 'valid': 'WAIT_4_START',
# 'preempted': ''})
StateMachine.add("SPEAK_READY",Speak(str="I am ready,,test start"),transitions={'succeeded': ''})
init_sm_again=StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with init_sm_again:
StateMachine.add("CHANGE_2_ARM0", change2arm, transitions={'succeeded': "INIT","aborted":"CHANGE_2_ARM0"})
StateMachine.add("INIT", ServiceState('xm_move_arm_position/move_position', xm_ArmPosition, "init",
response_cb=self.responce_cb),
transitions={'succeeded': 'WRIST_DOWN0'})
StateMachine.add("WRIST_DOWN0", wrist_down_state, transitions={"succeeded": "CLOSE_GRIPPER0"})
StateMachine.add("CLOSE_GRIPPER0", gripper_close_state, transitions={'succeeded': 'CHANGE_2_BASE0'})
StateMachine.add("CHANGE_2_BASE0", change2base, transitions={"succeeded": "SPEAK_READY","aborted":"CHANGE_2_BASE0"})
StateMachine.add("SPEAK_READY",Speak(str=" test start again"),transitions={'succeeded': ''})
find_object_sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
find_object_sm.userdata.object = 1
with find_object_sm:
StateMachine.add("NAV_2_START_POS", Forword_BacK(dis=0.5), transitions={"succeeded": "FIND_OBJECT1"})
StateMachine.add("FIND_OBJECT1", Find_Object(),
transitions={"succeeded": "CALCULATE_ANGLE1", 'aborted': 'FIND_OBJECT1'}, )
StateMachine.add("CALCULATE_ANGLE1", Calculate_angle(), transitions={"succeeded": "CHANGE_ANGLE1"},
remapping={'angle': 'angle_out'})
StateMachine.add("CHANGE_ANGLE1", ChangeAngular(), transitions={"succeeded": "FIND_OBJECT2"},
remapping={"angle_in": 'angle_out'})
StateMachine.add("FIND_OBJECT2", Find_Object(),
transitions={"succeeded": "SPEAK_OBJECT", 'aborted': 'FIND_OBJECT2'}, )
StateMachine.add("SPEAK_OBJECT",Speak(mode=1),transitions={'succeeded': 'CALCULATE_ANGLE2'})
StateMachine.add("CALCULATE_ANGLE2", Calculate_angle(), transitions={"succeeded": "CHANGE_ANGLE2"},
remapping={'angle': 'angle_out'})
StateMachine.add("CHANGE_ANGLE2", ChangeAngular(), transitions={"succeeded": ""},
remapping={"angle_in": 'angle_out'})
grasp_ready_sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with grasp_ready_sm:
StateMachine.add("SPEAK_GRASP",Speak(mode=2),transitions={'succeeded': 'BACK'})
StateMachine.add("BACK", Forword_BacK(dis=-0.5), transitions={'succeeded': 'CHANGE_2_ARM1'})
StateMachine.add("CHANGE_2_ARM1", change2arm, transitions={'succeeded': "READY","aborted":"CHANGE_2_ARM1"})
StateMachine.add("READY", ServiceState('xm_move_arm_position/move_position', xm_ArmPosition, "prepare",
response_cb=self.responce_cb),
transitions={'succeeded': 'OPEN_GRIPER'})
StateMachine.add("OPEN_GRIPER", gripper_open_state, transitions={"succeeded": "CHANGE_HEIGHT1"})
StateMachine.add("CHANGE_HEIGHT1", Lift_HEIGHT(height=-0.06), transitions={"succeeded": "AR_TARGET"})
StateMachine.add("AR_TARGET", MonitorState("ar_pose_marker", AlvarMarkers, self.get_mark_pos_cb),
transitions={"valid": "AR_TARGET", "invalid": "CHANGE_2_BASE1"})
StateMachine.add("CHANGE_2_BASE1", change2base, transitions={"succeeded": "CALCULATE_DIS","aborted":"CHANGE_2_BASE1"})
StateMachine.add("CALCULATE_DIS", Calculate_length(), transitions={'succeeded': ''})
grasp_sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with grasp_sm:
StateMachine.add("FORWORD", Forword_BacK_Global(), transitions={"succeeded": "CLOSE_GRIPPER"})
StateMachine.add("CLOSE_GRIPPER", CLOSE_GRIPPER(), transitions={'succeeded': 'WRIST_UP'})
StateMachine.add("WRIST_UP", wrist_up_state, transitions={"succeeded": "CHANGE_2_BASE2"})
StateMachine.add("CHANGE_2_BASE2", change2base, transitions={'succeeded': "BACK2","aborted":"CHANGE_2_BASE2"})
StateMachine.add("BACK2", Forword_BacK_Global(dis=1), transitions={'succeeded': ''})
place_sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with place_sm:
StateMachine.add("SPEAK_PLACE",Speak(str='I am try to place it'),transitions={'succeeded': 'CHANGE_2_ARM1'})
StateMachine.add("CHANGE_2_ARM1", change2arm, transitions={'succeeded': "CHANGE_HEIGHT","aborted":"CHANGE_2_ARM1"})
StateMachine.add("CHANGE_HEIGHT", Lift_HEIGHT(height=0.38), transitions={"succeeded": "CHANGE_2_BASE2"})
StateMachine.add("CHANGE_2_BASE2", change2base, transitions={"succeeded": "CALCULATE_DIS2","aborted":"CHANGE_2_BASE2"})
# StateMachine.add("AR_TARGET2", MonitorState("ar_pose_marker", AlvarMarkers, self.get_mark_pos_cb),
# transitions={"valid": "AR_TARGET2", "invalid": "CALCULATE_DIS2"})
StateMachine.add("CALCULATE_DIS2", Calculate_length_place(), transitions={"succeeded": "FORWORD2"})
StateMachine.add("FORWORD2", Forword_BacK_Global(), transitions={"succeeded": "CHANGE_2_ARM2"})
StateMachine.add("CHANGE_2_ARM2", change2arm, transitions={'succeeded': "WRIST_DOWN","aborted":"CHANGE_2_ARM2"})
StateMachine.add("WRIST_DOWN", wrist_down_state, transitions={"succeeded": "GRIPER_OPEN"})
StateMachine.add("GRIPER_OPEN", gripper_open_state, transitions={"succeeded": "CHANGE_2_BASE3"})
StateMachine.add("CHANGE_2_BASE3", change2base, transitions={"succeeded": "","aborted":"CHANGE_2_BASE3"})
reinit_sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with reinit_sm:
StateMachine.add("BACK3", Forword_BacK_Global(dis=1), transitions={"succeeded": "INIT_ANGLE"})
StateMachine.add("INIT_ANGLE", ChangeAngularGlobal(), transitions={"succeeded": ""})
sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
sm.userdata.object_id=0
with sm:
StateMachine.add("FIND_OBJECT", find_object_sm, transitions={"succeeded": "READY"},remapping={"object":"object_id "})
StateMachine.add("READY", grasp_ready_sm, transitions={"succeeded": "GRASP"})
StateMachine.add("GRASP", grasp_sm, transitions={"succeeded": "PLACE"})
StateMachine.add("PLACE", place_sm, transitions={"succeeded": "REINIT"})
StateMachine.add("REINIT", reinit_sm, transitions={"succeeded": "INIT"})
StateMachine.add("INIT", init_sm_again, transitions={"succeeded": ""})
output=init_sm.execute()
rospy.sleep(1)
for id in id_list:
OBJECT_ID=id
output = sm.execute()
ANGLE_CHANGED = 0
rospy.sleep(1)
rospy.sleep(rospy.Duration(3))
class SMACHAI():
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()
def empty_cb(self, userdata, msg):
#rospy.loginfo("Empty message received.")
return False
# Gets called when ANY child state terminates
def useless_child_termination_cb(self, outcome_map):
rospy.loginfo("useless_child_termination_cb.")
return True
# Gets called when ALL child states are terminated
def useless_outcome_cb(self, outcome_map):
rospy.loginfo("useless_outcome_cb.")
return 'succeeded'
# Gets called when ANY child state terminates
def in_main_room_child_termination_cb(self, outcome_map):
#rospy.loginfo("daymode_child_termination_cb.")
return True
# Gets called when ALL child states are terminated
def in_main_room_outcome_cb(self, outcome_map):
#rospy.loginfo("daymode_outcome_cb.")
if outcome_map['GO_TO_KITCHEN'] == 'invalid':
rospy.loginfo("Going to the kitchen ")
return 'go_kitchen'
if outcome_map['GO_TO_BEDROOM'] == 'invalid':
rospy.loginfo("Going to the bedroom")
return 'go_bedroom'
if outcome_map['GO_TO_SLEEP'] == 'invalid':
rospy.loginfo("Going to sleep")
return 'go_sleep'
if outcome_map['GO_TO_POINT'] == 'succeeded':
rospy.loginfo("Arrived to point")
return 'succeeded'
else:
return 'aborted'
# Gets called when ANY child state terminates
def in_sleep_child_termination_cb(self, outcome_map):
#rospy.loginfo("daymode_child_termination_cb.")
return True
# Gets called when ALL child states are terminated
def in_sleep_outcome_cb(self, outcome_map):
#rospy.loginfo("daymode_outcome_cb.")
if outcome_map['GO_TO_KITCHEN'] == 'invalid':
rospy.loginfo("Going to the kitchen ")
return 'go_kitchen'
if outcome_map['GO_TO_BEDROOM'] == 'invalid':
rospy.loginfo("Going to the bedroom")
return 'go_bedroom'
if outcome_map['GO_TO_MAIN_ROOM'] == 'invalid':
rospy.loginfo("Going to main room")
return 'go_main_room'
if outcome_map['GO_TO_POINT'] == 'succeeded':
rospy.loginfo("Arrived to point")
return 'succeeded'
else:
return 'aborted'
# Gets called when ANY child state terminates
def in_bedroom_child_termination_cb(self, outcome_map):
#rospy.loginfo("daymode_child_termination_cb.")
return True
# Gets called when ALL child states are terminated
def in_bedroom_outcome_cb(self, outcome_map):
#rospy.loginfo("daymode_outcome_cb.")
if outcome_map['GO_TO_KITCHEN'] == 'invalid':
rospy.loginfo("Going to the kitchen ")
return 'go_kitchen'
if outcome_map['GO_TO_MAIN_ROOM'] == 'invalid':
rospy.loginfo("Going to the main room")
return 'go_main_room'
if outcome_map['GO_TO_SLEEP'] == 'invalid':
rospy.loginfo("Going to sleep")
return 'go_sleep'
if outcome_map['GO_TO_POINT'] == 'succeeded':
rospy.loginfo("Arrived to point")
return 'succeeded'
else:
return 'aborted'
# Gets called when ANY child state terminates
def in_kitchen_child_termination_cb(self, outcome_map):
#rospy.loginfo("daymode_child_termination_cb.")
return True
# Gets called when ALL child states are terminated
def in_kitchen_outcome_cb(self, outcome_map):
#rospy.loginfo("daymode_outcome_cb.")
if outcome_map['GO_TO_MAIN_ROOM'] == 'invalid':
rospy.loginfo("Going to the main room")
return 'go_main_room'
if outcome_map['GO_TO_BEDROOM'] == 'invalid':
rospy.loginfo("Going to the bedroom")
return 'go_bedroom'
if outcome_map['GO_TO_SLEEP'] == 'invalid':
rospy.loginfo("Going to sleep")
return 'go_sleep'
if outcome_map['GO_TO_POINT'] == 'succeeded':
rospy.loginfo("Arrived to point")
return 'succeeded'
else:
return 'aborted'
def time_cb(self, userdata, msg):
if msg.data < 2:
self.stopping = True
return False
else:
self.stopping = False
return True
def shutdown(self):
rospy.loginfo("Stopping carry automation...")
#self.sm_day_mode.request_preempt()
rospy.sleep(1)
def __init__(self):
rospy.init_node('deliver_food', anonymous=False)
self.initialize_destination()
self.lalala = 100
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
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(60.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states[room] = move_base_state
rospy.loginfo(room + " -> [" + str(round(self.room_locations[room].position.x,2)) + ", " + str(round(self.room_locations[room].position.y,2)) + "]" )
sm_rotate_search = Concurrence(outcomes=['find', 'not_find'],
default_outcome='not_find',
child_termination_cb=self.concurrence_child_termination_callback,
outcome_cb=self.concurrence_outcome_callback)
with sm_rotate_search:
Concurrence.add('ROTATE', Rotate360() )
Concurrence.add('SEARCH', SearchTable() )
sm_table1 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table1:
StateMachine.add('GOTO_TABLE1', nav_states['table1'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
sm_table2 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table2:
StateMachine.add('GOTO_TABLE2', nav_states['table2'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
sm_table3 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table3:
StateMachine.add('GOTO_TABLE3', nav_states['table3'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
sm_table4 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table4:
StateMachine.add('GOTO_TABLE4', nav_states['table4'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
sm_table5 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table5:
StateMachine.add('GOTO_TABLE5', nav_states['table5'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
sm_table6 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table6:
StateMachine.add('GOTO_TABLE6', nav_states['table6'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
# let's initialize the overall state machine
sm_deliverfood = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_deliverfood:
StateMachine.add('COMPUTER_VISION_TASK', ComputerVision(), transitions={'detect1':'TABLE_ONE_TASK',
'detect2':'TABLE_TWO_TASK',
'detect3':'TABLE_THREE_TASK',
'detect4':'TABLE_FOUR_TASK',
'detect5':'TABLE_FIVE_TASK',
'detect6':'TABLE_SIX_TASK'})
StateMachine.add('TABLE_ONE_TASK',sm_table1, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('TABLE_TWO_TASK',sm_table2, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('TABLE_THREE_TASK',sm_table3, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('TABLE_FOUR_TASK',sm_table4, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('TABLE_FIVE_TASK',sm_table5, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('TABLE_SIX_TASK',sm_table6, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('deliver_food', sm_deliverfood, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = sm_deliverfood.execute()
rospy.on_shutdown(self.shutdown)
service_states.make_bounding_boxes,
transitions={'succeeded': 'ANALYZE_CLOUD_COLOR'})
@cb_interface(input_keys=['clusters'])
def analyze_color_request_cb(userdata, request):
r = AnalyzeCloudColorRequest()
r.cloud = userdata.clusters[0]
return r
StateMachine.add('ANALYZE_CLOUD_COLOR',
ServiceState('analyze_cloud_color',
AnalyzeCloudColor,
request_cb=analyze_color_request_cb,
response_slots=['mean',
'median',
'points']),
transitions={'succeeded': 'COUNTER'})
StateMachine.add('COUNTER',
CounterState(int(args.confirm_every)),
transitions={'overflow': 'CONFIRM_OBJECT',
'counting': 'STORE_OBJECT'})
StateMachine.add('CONFIRM_OBJECT',
ConfirmState('Is the detected object correct?'),
transitions={'yes': 'STORE_OBJECT',
'no': 'ACCUMULATE_CLOUD'})
StateMachine.add('STORE_OBJECT',
StoreObject(args.dataset, args.object_id),
transitions={'stored': 'ACCUMULATE_CLOUD'})
rospy.loginfo('Starting data collection...')
outcome = sm.execute()
def __init__(self):
global target
rospy.init_node('object', anonymous=False)
rospy.on_shutdown(self.shutdown)
grasp_sm=StateMachine(outcomes=['succeeded','aborted','preempted'])
grasp_sm.userdata.target=Point(0,0,0)
grasp_sm.userdata.object=xm_Object
gripper_goal = xm_GripperCommandGoal()
gripper_goal.command.position = 0.15
# gripper_goal.command.torque = 0.5
# gripper_goal.header.frame
gripper_open_state = SimpleActionState('xm_move_gripper', xm_GripperCommandAction, goal=gripper_goal,
result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10),
server_wait_timeout=rospy.Duration(10))
#
gripper_goal = xm_GripperCommandGoal()
gripper_goal.command.position = 0.06
# gripper_goal.command.torque = 0.5
# gripper_goal.header.frame
gripper_close_state = SimpleActionState('xm_move_gripper', xm_GripperCommandAction, goal=gripper_goal,
result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10),
server_wait_timeout=rospy.Duration(10))
wrist_goal=xm_WristPositionGoal()
wrist_goal.angle=-1.047
wrist_state=SimpleActionState("xm_move_wrist", xm_WristPositionAction,goal=wrist_goal,
result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(20),
server_wait_timeout=rospy.Duration(20))
wrist_goal=xm_WristPositionGoal()
wrist_goal.angle=0
wrist_state_init=SimpleActionState("xm_move_wrist", xm_WristPositionAction,goal=wrist_goal,
result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(20),
server_wait_timeout=rospy.Duration(20))
change2base=ChangeMode("base")
change2arm=ChangeMode("arm")
rospy.loginfo("ready")
grasp_sm.userdata.target=Point(0.779,-0.03,-0.144)
grasp_sm.userdata.x1=float(0.6)
grasp_sm.userdata.x2=-0.6
grasp_sm.userdata.x3=0.3
# grasp_sm.userdata.distance_in=0.5
grasp_sm.userdata.angle_out=atan2(-0.03, 0.779)
grasp_sm.userdata.postions=Point(0.779,-0.03,-0.144)
with grasp_sm:
StateMachine.add("CHANGE_2_ARM0",change2arm,transitions={'succeeded':"WRIST_DOWN"})
# StateMachine.add("INIT",ServiceState('xm_move_arm_position/move_position', xm_ArmPosition,"init",response_cb=self.responce_cb),transitions={'succeeded':'WRIST_DOWN'})
StateMachine.add("WRIST_DOWN",wrist_state_init,transitions={"succeeded":"CHANGE_2_BASE0"})
StateMachine.add("CHANGE_2_BASE0",change2base,transitions={"succeeded":"CALCULATE_ANGLE"})
# StateMachine.add("Move_to_Pos", Forword_BacK(), transitions={'succeeded':'CALCULATE_ANGLE'},remapping={"distance_in":'x1'})
# StateMachine.add("FIND_OBJECT", ServiceState("Find_Object", xm_ObjectDetect,request_cb=self.find_object_request_cb,
# response_cb=self.find_object_cb,input_keys=['object'],output_keys=['target']),
# transitions={"succeeded":"CALCULATE_ANGLE",'aborted':'FIND_OBJECT'},
# remapping={'target':'labal'})
StateMachine.add("CALCULATE_ANGLE",Calculate_angle(),transitions={"succeeded":"CHANGE_ANGLE"},remapping={'waypoint_in':'target','angle':'angle_out'})
StateMachine.add("CHANGE_ANGLE",ChangeAngular(),transitions={"succeeded":"BACK"},remapping={"angle_in":'angle_out'})
StateMachine.add("BACK", Forword_BacK(), transitions={'succeeded':'CHANGE_2_ARM1'},remapping={"distance_in":"x2"})
StateMachine.add("CHANGE_2_ARM1",change2arm,transitions={'succeeded':"READY"})
StateMachine.add("READY",ServiceState('xm_move_arm_position/move_position', xm_ArmPosition,"prepare",response_cb=self.responce_cb),transitions={'succeeded':'OPEN_GRIPER'})
StateMachine.add("OPEN_GRIPER",gripper_open_state,transitions={"succeeded":"CHANGE_HEIGET"})
StateMachine.add("CHANGE_HEIGET",Lift(),transitions={"succeeded":"CHANGE_2_BASE1"},remapping={"position":"target"})
StateMachine.add("CHANGE_2_BASE1",change2base,transitions={"succeeded":"ALIGN_X"})
StateMachine.add("ALIGN_X",Forword_BacK(),transitions= {"succeeded" :"CHANGE_2_ARM2"},remapping={"distance_in":"x3"})
StateMachine.add("CHANGE_2_ARM2",change2arm,transitions={'succeeded':"CLOSE_GRIPPER"})
StateMachine.add("CLOSE_GRIPPER",gripper_close_state,transitions={'succeeded':'WRIST_UP'})
StateMachine.add("WRIST_UP",wrist_state,transitions={"succeeded":"CHANGE_2_BASE2"})
StateMachine.add("CHANGE_2_BASE2",change2base,transitions={'succeeded':"BACK2"})
StateMachine.add("BACK2", Forword_BacK(), transitions={'succeeded':''},remapping={"distance_in":'x2'})
outcome=grasp_sm.execute("CALCULATE_ANGLE")
rospy.sleep(rospy.Duration(3))
class Patrol():
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()
def nav_transition_cb(self, userdata, active_states, *cb_args):
self.last_nav_state = active_states
# Gets called when ANY child state terminates
def concurrence_child_termination_cb(self, outcome_map):
# If the current navigation task has succeeded, return True
if outcome_map['SM_NAV'] == 'succeeded':
return True
# If the MonitorState state returns False (invalid), store the current nav goal and recharge
if outcome_map['MONITOR_BATTERY'] == 'invalid':
rospy.loginfo("LOW BATTERY! NEED TO RECHARGE...")
if self.last_nav_state is not None:
self.sm_nav.set_initial_state(self.last_nav_state, UserData())
return True
else:
return False
# Gets called when ALL child states are terminated
def concurrence_outcome_cb(self, outcome_map):
# If the battery is below threshold, return the 'recharge' outcome
if outcome_map['MONITOR_BATTERY'] == 'invalid':
return 'recharge'
# Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop'
elif outcome_map['SM_NAV'] == 'succeeded':
self.patrol_count += 1
rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count))
# If we have not completed all our patrols, start again at the beginning
if self.n_patrols == -1 or self.patrol_count < self.n_patrols:
self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData())
return 'succeeded'
# Otherwise, we are finished patrolling so return 'stop'
else:
self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData())
return 'stop'
# Recharge if all else fails
else:
return 'recharge'
def battery_cb(self, userdata, msg):
if msg.data < self.low_battery_threshold:
self.recharging = True
return False
else:
self.recharging = False
return True
def recharge_cb(self, userdata, response):
return 'succeeded'
def move_base_result_cb(self, userdata, status, result):
if not self.recharging:
if status == actionlib.GoalStatus.SUCCEEDED:
self.n_succeeded += 1
elif status == actionlib.GoalStatus.ABORTED:
self.n_aborted += 1
elif status == actionlib.GoalStatus.PREEMPTED:
self.n_preempted += 1
try:
rospy.loginfo("Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted)))
except:
pass
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.sm_nav.request_preempt()
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
