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 = 'base_footprint'
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 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 search_for_tags(
squares, marker,
cube): # type: (List[ParkingSquare], ARTag, ARCube) -> StateMachine
sm = StateMachine(outcomes=['ok'])
with sm:
StateMachine.add('GO_TO_AR_SEARCH_1',
NavigateToNamedPoseState('ar_search_1'),
transitions={
'ok': 'SEARCH1',
'err': 'ABSORB'
})
StateMachine.add('SEARCH1',
FindTags(squares, marker, cube),
transitions={
'ok': 'GO_TO_AR_SEARCH_2',
'found': 'ABSORB'
})
StateMachine.add('GO_TO_AR_SEARCH_2',
NavigateToNamedPoseState('ar_search_2'),
transitions={
'ok': 'SEARCH2',
'err': 'ABSORB'
})
StateMachine.add('SEARCH2',
FindTags(squares, marker, cube),
transitions={
'ok': 'GO_TO_AR_SEARCH_1',
'found': 'ABSORB'
})
StateMachine.add('ABSORB', AbsorbResultState())
return sm
def callback(message):
global flag
global sm
if flag:
kwargs = PoseStamped()
kwargs.pose = message.workpoint
# if message.statename == 'InitialState':
# kwargs.pose = message.workpoint
# elif message.statename == 'AfterGrasp':
# kwargs.pose = message.workpoint
# if message.keywords:
# pass
# kwargs = tuples_to_dict(message.keywords, message.arg)
# kwargs.pose = message.grasp1
with sm:
StateMachine.add(
message.id,
globals()[
message.statename](kwargs),
transitions=tuples_to_dict(
message.src,
message.dst))
if message.is_end is True:
flag = False
while True:
status = sm.execute()
if status == 'success':
print('===SUCCESS!===')
print('===RESTART STATE MACHINE===')
os._exit(1)
else:
print('###FAILED...###')
os._exit(1)
def __init__(self):
Concurrence(default_outcome='aborted',
input_keys=['text'],
output_keys=['text'],
outcomes=['succeeded', 'preempted', 'aborted'],
child_termination_cb=self.getfinish_Cb,
outcome_cb=self.outcome_Cb)
jointLoop = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with jointLoop:
StateMachine.add('NEXT_MOVE',
RandomSelectionFromPoolState(self._movementList),
output_keys={'joint_angles'},
transitions={'succeeded': 'MOVEMENT'}
)
StateMachine.add('MOVEMENT',
JointAngleState(['HeadPitch', 'HeadYaw']),
transitions={'succeeded': 'NEXT_MOVE'}
)
with self:
Concurrence.add('MOVING',
jointLoop,
transitions={'succeeded': 'succeeded'}
)
Concurrence.add('GET_USER_ANSWER',
GetUserAnswer(),
transitions={'succeeded': 'succeeded',
'aborted': 'aborted'},
remapping={'text': 'text'}
)
def main():
rospy.init_node("fsm_dummy")
sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with sm:
StateMachine.add('INITIAL_TURN',
SimpleActionState('/initial_turn', InitialTurnAction),
transitions={
'succeeded': 'EXPLORATION',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('EXPLORATION',
exploration.ExplorationContainer(),
transitions={
'victim_thermal': 'IDENTIFICATION_SIMPLE',
'victim_camera': 'IDENTIFICATION_TRACKING',
'aborted': 'aborted',
'preempted': 'preempted',
'time_out': 'AGGRESSIVE_EXPLORATION'
})
StateMachine.add('IDENTIFICATION_SIMPLE',
identification.IdentificationSimpleContainer(),
transitions={
'parked': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('IDENTIFICATION_TRACKING',
identification.IdentificationTrackingContainer(),
transitions={
'identification_finished': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('AGGRESSIVE_EXPLORATION',
SimpleActionState('/navigation/initial_turn',
InitialTurnAction),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/DUMMY_FSM')
sis.start()
smach_ros.set_preempt_handler(sm)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target=sm.execute)
smach_thread.start()
rospy.spin()
sis.stop()
def main():
rospy.init_node('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 __init__(self):
StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'])
with self:
StateMachine.add('CROUCH_POSE', GoToPostureState('Crouch', 0.5), transitions={'succeeded': 'DISABLE_STIFF'})
StateMachine.add('DISABLE_STIFF', DisableStiffnessState(), transitions={'succeeded': 'succeeded'})
def __init__(self, startPose='StandInit'):
StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'])
with self:
StateMachine.add('ENABLE_STIFF', EnableStiffnessState(), transitions={'succeeded': 'START_POSITION'})
StateMachine.add('START_POSITION', GoToPostureState(startPose, 0.5), transitions={'succeeded': 'succeeded'})
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 __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("fsm_dummy")
sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm:
StateMachine.add('INITIAL_TURN', SimpleActionState('/navigation/initial_turn', InitialTurnAction),
transitions={'succeeded':'EXPLORATION','aborted':'aborted','preempted':'preempted'})
StateMachine.add('EXPLORATION', exploration.ExplorationContainer(),
transitions={'victim_thermal':'IDENTIFICATION_SIMPLE',
'victim_camera':'IDENTIFICATION_TRACKING','aborted':'aborted','preempted':'preempted','time_out':'AGGRESSIVE_EXPLORATION'})
StateMachine.add('IDENTIFICATION_SIMPLE', identification.IdentificationSimpleContainer(),
transitions={'parked':'succeeded','aborted':'aborted','preempted':'preempted'})
StateMachine.add('IDENTIFICATION_TRACKING', identification.IdentificationTrackingContainer(),
transitions={'identification_finished':'succeeded','aborted':'aborted','preempted':'preempted'})
StateMachine.add('AGGRESSIVE_EXPLORATION', SimpleActionState('/navigation/initial_turn', InitialTurnAction),
transitions={'succeeded':'succeeded','aborted':'aborted','preempted':'preempted'})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/DUMMY_FSM')
sis.start()
smach_ros.set_preempt_handler(sm)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target = sm.execute)
smach_thread.start()
rospy.spin()
sis.stop()
def __init__(self):
StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['text'], output_keys=['text'])
with self:
StateMachine.add('START_LISTEN',
StartRecognitionState(),
transitions={'succeeded': 'ANSWER_DETECTION'}
)
StateMachine.add('ANSWER_DETECTION',
GetRecognizedWordNoEmptyList(timeout=30),
transitions={'succeeded': 'STOP_LISTEN',
'timeouted': 'STOP_LISTEN_ABORTED'},
remapping={'recognized_words': 'text'}
)
StateMachine.add('STOP_LISTEN',
StopRecognitionState(),
transitions={'succeeded': 'GET_SINGLE_RESPONSE'}
)
StateMachine.add('GET_SINGLE_RESPONSE',
CBState(self.list_cb, input_keys=['text'], output_keys=['text'], outcomes=['succeeded', 'aborted']),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'}
)
StateMachine.add('STOP_LISTEN_ABORTED',
StopRecognitionState(),
transitions={'succeeded': 'aborted'}
)
def __init__(self):
rospy.init_node('speech_recognition')
rospy.on_shutdown(self.shutdown)
self.find_people = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
self.test_bool = False
with self.find_people:
self.answer = Iterator(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['people_condition'],
output_keys=[],
it=lambda: range(0, 5),
exhausted_outcome='succeeded')
with self.answer:
Iterator.set_contained_state('ANSWER_STATE',
Anwser(),
loop_outcomes=['succeeded'])
StateMachine.add('ANSWER',
self.answer,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
})
out = self.find_people.execute()
if out == 'succeeded':
self.test_bool = True
rospy.logwarn('test finished, all thing is done well!')
else:
rospy.logerr('test failed ,something goes wrong')
def __init__(self):
rospy.init_node('test_vision',anonymous=False)
rospy.on_shutdown(self.shutdown)
rospy.loginfo('------------start test----------------')
self.test_bool = False
self.test_vision = StateMachine(outcomes=['succeeded','aborted','error'])
with self.test_vision:
StateMachine.add('RUN_NODE',
RunNode_obj(),
transitions={'succeeded':'WAIT','aborted':'aborted'})
self.test_vision.userdata.rec = 20.0
StateMachine.add('WAIT',
Wait(),
transitions={'succeeded':'succeeded','error':'error'})
# StateMachine.add('CLOSENODE',
# CloseKinect(),
# transitions={'succeeded':'succeeded','aborted':'aborted'})
# self.test_vision.userdata.pos_xm =Pose()
# StateMachine.add('GET_PEOPLE_POS',
# FindPeople().find_people_,
# transitions ={'invalid':'succeeded','valid':'error','preempted':'aborted'},
# remapping = {'pos_xm':'pos_xm'})
# rospy.loginfo(self.test_vision.userdata.pos_xm)
out = self.test_vision.execute()
# if out =='error':
# rospy.logerr('no position past!')
# self.test_bool = False
# else:
self.test_bool = True
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()
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 __init__(self):
StateMachine.__init__(self,
outcomes=['succeeded', 'aborted', 'preempted'])
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
pose = Pose()
pose.position = Point(8.2, -3.0, 0.0)
pose.orientation = Quaternion(0.0, 0.0, 1.0, 0.0)
nav_goal.target_pose.pose = pose
self.nav_docking_station = SimpleActionState('move_base',
MoveBaseAction,
goal=nav_goal)
with self:
StateMachine.add('NAVIGATE_TO_OUTLET',
self.nav_docking_station,
transitions={
'succeeded': 'DOCKING',
'aborted': 'NAVIGATE_TO_OUTLET'
})
StateMachine.add('DOCKING',
ServiceState(
'battery_simulator/set_battery_level',
SetBatteryLevel, 100),
transitions={'succeeded': 'succeeded'})
def __init__(self):
rospy.init_node('sm_Follow')
self.trace = Concurrence(
outcomes=['succeeded', 'aborted'],
default_outcome='aborted',
outcome_map={
'succeeded': {
'STOP': 'stop'
},
'aborted': {
'FOLLOW': 'aborted'
}
},
# outcome_cb = self.trace_out_cb,
child_termination_cb=self.trace_child_cb)
with self.trace:
self.meta_follow = StateMachine(['succeeded', 'aborted'])
with self.meta_follow:
StateMachine.add('FOLLOW',
SimpleFollow(),
transitions={
'succeeded': 'FINDPEOPLE',
'aborted': 'aborted'
})
Concurrence.add('RUNNODE', RunNode())
Concurrence.add('FOLLOW', self.meta_follow)
Concurrence.add('STOP', CheckStop())
out = self.trace.execute()
print out
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 cameraIdentificationSimple():
def _termination_cb(outcome_map):
return True
sm = StateMachine(outcomes=['victim_approached', 'aborted', 'preempted'])
with sm:
StateMachine.add('GET_VICTIM',
SelectTargetState('victim'),
transitions={
'succeeded': 'GO_TO_VICTIM',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('GO_TO_VICTIM',
MoveBaseState(),
transitions={
'succeeded': 'victim_approached',
'aborted': 'GET_VICTIM',
'preempted': 'preempted'
})
return sm
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 construct_sm():
sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with sm:
tutorial_it = Iterator(outcomes=['succeeded', 'preempted', 'aborted'],
it=lambda: range(0, 10),
it_label='index',
input_keys=[],
output_keys=[],
exhausted_outcome='succeeded')
with tutorial_it:
container_sm = utils.TargetSelectorContainer('explore')
#close container_sm
Iterator.set_contained_state('CONTAINER_STATE',
container_sm,
loop_outcomes=['target_sent'])
#close the tutorial_it
StateMachine.add('TUTORIAL_IT', tutorial_it, {
'succeeded': 'succeeded',
'aborted': 'aborted'
})
return sm
def ExplorationContainer():
cc = Concurrence(outcomes=['victim_thermal','victim_camera','aborted','preempted','time_out'],
default_outcome='aborted', outcome_map={'victim_thermal':{'VICTIM_MONITOR':'victim_thermal'},
'victim_camera':{'VICTIM_MONITOR':'victim_camera'},'preempted':{'EXPLORE':'preempted','VICTIM_MONITOR':'preempted'},
'aborted':{'EXPLORE':'aborted'}, 'time_out':{'EXPLORE':'time_out'}},
child_termination_cb=_termination_cb)
with cc:
#~ Concurrence.add('TARGET_CONTROLLER', utils.TargetSelectorContainer('explore'))
Concurrence.add('EXPLORE', utils.make_iterator(utils.TargetSelectorContainer('explore'), max_iter=100))
sm_victim_monitor = StateMachine(outcomes=['victim_thermal','victim_camera','preempted'])
sm_victim_monitor.userdata.victim_type = 0
with sm_victim_monitor:
StateMachine.add('VICTIM_MONITORING', MonitorVictimState(
input_keys=['victim_type'], output_keys=['victim_type']),
transitions={'invalid':'VICTIM_DECIDE', 'valid':'VICTIM_MONITORING', 'preempted':'preempted'},
remapping={'victim_type':'victim_type'})
StateMachine.add('VICTIM_DECIDE', DecideVictimState(),
transitions={'thermal':'victim_thermal','camera':'victim_camera'})
Concurrence.add('VICTIM_MONITOR', sm_victim_monitor)
return cc
def main():
rospy.init_node('exploration_fsm')
sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with sm:
StateMachine.add('INITIAL_TURN',
InitialTurnState(),
transitions={
'succeeded': 'EXPLORATION',
'aborted': 'EXPLORATION',
'preempted': 'preempted'
})
StateMachine.add('EXPLORATION',
simpleExplorationContainer(),
transitions={'preempted': 'preempted'})
sis = smach_ros.IntrospectionServer('fsm_introspection', sm,
'/EXPLORATION_FSM')
sis.start()
smach_ros.set_preempt_handler(sm)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target=sm.execute)
smach_thread.start()
rospy.spin()
sis.stop()
def __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()
def TargetSelectorContainer(target_type):
sm_target_selector = StateMachine(['target_sent', 'aborted', 'preempted'])
with sm_target_selector:
target_selection_goal = targetSelectionGoal(target_type)
StateMachine.add('GET_TARGET',
SimpleActionState('/select_target',
SelectTargetAction,
goal=target_selection_goal,
result_key='target_pose'),
transitions={
'succeeded': 'MOVE_BASE',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'target_pose': 'next_target'})
StateMachine.add('MOVE_BASE',
SimpleActionState('/move_base',
MoveBaseAction,
goal_key='next_target'),
transitions={
'succeeded': 'target_sent',
'aborted': 'GET_TARGET',
'preempted': 'preempted'
})
return sm_target_selector
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()
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 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 __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 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 __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 __init__(self, robot, cupboard_id, cupboard_navigation_area,
required_items):
StateMachine.__init__(self,
outcomes=["succeeded", "failed"],
output_keys=["item_picked"])
cupboard = EdEntityDesignator(robot=robot, id=cupboard_id)
with self:
StateMachine.add(
"NAVIGATE_TO_CUPBOARD",
NavigateToSymbolic(robot, {cupboard: cupboard_navigation_area},
cupboard),
transitions={
'arrived': 'PICK_ITEM_FROM_CUPBOARD',
'unreachable': 'failed',
'goal_not_defined': 'failed'
})
StateMachine.add("PICK_ITEM_FROM_CUPBOARD",
PickItemFromCupboardDrawer(
robot, cupboard_id, required_items),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
def build_state_machine(self):
'''
建立状态机
'''
if not self.sm_fetchtea is None:
self.sm_fetchtea = None
self.sm_fetchtea = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_fetchtea:
StateMachine.add('START_TIP',
SpeaktoTipStart(),
transitions={
'succeeded': 'GET_LOCATION',
'aborted': 'END_SPEAK_TO_TIP_ABORT'
})
StateMachine.add('GET_LOCATION',
GetLocation(),
transitions={
'succeeded': 'NAV_TO_WAYPOINT',
'aborted': 'END_SPEAK_TO_TIP_ABORT'
},
remapping={'waypoint_out': 'nav_waypoint'})
StateMachine.add('NAV_TO_WAYPOINT',
Nav2Waypoint(),
transitions={
'succeeded': 'END_SPEAK_TO_TIP_SUCESS',
'aborted': 'END_SPEAK_TO_TIP_ABORT'
},
remapping={'waypoint_in': 'nav_waypoint'})
StateMachine.add('END_SPEAK_TO_TIP_ABORT',
EndSpeaktoTipAbort(),
transitions={'succeeded': 'succeeded'})
StateMachine.add('END_SPEAK_TO_TIP_SUCESS',
EndSpeaktoTipSucess(),
transitions={'succeeded': 'succeeded'})
def __init__(self, robot):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
with self:
StateMachine.add("FIND_CUP",
CustomFindCup(robot),
transitions={'succeeded': 'succeeded',
'failed': 'FIND_CUP'})
def IdentificationTrackingContainer():
sm_identification_tracking = StateMachine(['identification_finished','aborted','preempted'])
with sm_identification_tracking:
StateMachine.add('DO_NOTHING', SimpleActionState('/navigation/initial_turn', InitialTurnAction),
transitions={'succeeded':'identification_finished','aborted':'aborted','preempted':'preempted'})
return sm_identification_tracking
def create_root_sm():
""" Create root State Machine with meta state (sleeping, awake) """
root_sm = StateMachine(
outcomes=[]
)
with root_sm:
StateMachine.add('SLEEPING', Sleeping(),
transitions={'sensing': 'AWAKE'})
StateMachine.add('AWAKE', create_awake_sm(),
transitions={'bored': 'SLEEPING'})
return root_sm
def __init__(self, robot):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
arm = Designator(robot.leftArm)
with self:
StateMachine.add("FIND_CUP", CustomFindCup(robot),
transitions={'succeeded': 'GRAB',
'failed': 'failed'})
StateMachine.add("GRAB", SimpleGrab(robot, arm),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
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()
def __init__(self):
StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'], input_keys=['user_answer'], output_keys=['text'])
with self:
# DONE integrate movements, speech and retrieval from akinator service
StateMachine.add('QUESTION',
AkinatorServiceState(),
transitions={'game_finished': 'aborted', 'continue_game': 'QUESTION_GESTURE'},
remapping={'resp_text': 'text', 'question_response': 'user_answer'}
)
AskingPool = ['CIR_Asking1', 'CIR_Asking2', 'CIR_Asking3', 'CIR_Asking4', 'CIR_Asking5', 'CIR_Asking6']
StateMachine.add('QUESTION_GESTURE',
SpeechGesture(wait_before_speak=1.5, behavior_pool=AskingPool),
transitions={'succeeded': 'succeeded'})
def __init__(self, robot, door_id):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
with self:
StateMachine.add("NAVIGATE_TO_DOOR",
NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=door_id)),
transitions={'arrived': 'OPEN_DOOR',
'unreachable': 'failed',
'goal_not_defined': 'failed'})
StateMachine.add("OPEN_DOOR",
OpenDoor(robot, door_id),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
def __init__(self, wait_time=3.0):
smach.StateMachine.__init__(self, [succeeded, preempted, aborted],
input_keys=[],
output_keys=[])
with self:
@smach.cb_interface(outcomes=[succeeded])
def wait(userdata):
rospy.sleep(wait_time)
return succeeded
StateMachine.add('WAIT_STATE', CBState(wait, outcomes=[succeeded]), transitions={succeeded: 'TAKE_SNAPSHOT'})
StateMachine.add('TAKE_SNAPSHOT', ServiceState('/xtion_snapshotter/snapshot', Empty),
transitions={succeeded: succeeded})
def add_states(self):
sm_nested = StateMachine(outcomes=self.outcomes, input_keys=self.input_keys, output_keys=self.output_keys)
machine_count = 0
state_count = 0
StateMachine.add(self.title,sm_nested,transitions=self.transitions, remapping=self.remapping)
with sm_nested:
for thing in self.things_to_add:
if thing == 's':
addition = self.state_machine_additions[state_count]
StateMachine.add(addition.title, addition.state, addition.transitions, remapping=addition.remapping)
state_count += 1
else:
machine = self.state_machines[machine_count]
machine.add_states()
machine_count += 1
def construct_sm():
sm = StateMachine(outcomes = ['succeeded','aborted','preempted'])
sm.userdata.nums = range(25, 88, 3)
sm.userdata.even_nums = []
sm.userdata.odd_nums = []
with sm:
## %Tag(ITERATOR)%
tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'],
input_keys = ['nums', 'even_nums', 'odd_nums'],
it = lambda: range(0, len(sm.userdata.nums)),
output_keys = ['even_nums', 'odd_nums'],
it_label = 'index',
exhausted_outcome = 'succeeded')
## %EndTag(ITERATOR)%
## %Tag(CONTAINER)%
with tutorial_it:
container_sm = StateMachine(outcomes = ['succeeded','preempted','aborted','continue'],
input_keys = ['nums', 'index', 'even_nums', 'odd_nums'],
output_keys = ['even_nums', 'odd_nums'])
with container_sm:
#test wether even or odd
StateMachine.add('EVEN_OR_ODD',
ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2,
input_keys=['nums', 'index']),
{'true':'ODD',
'false':'EVEN' })
#add even state
@smach.cb_interface(input_keys=['nums', 'index', 'even_nums'],
output_keys=['odd_nums'],
outcomes=['succeeded'])
def even_cb(ud):
ud.even_nums.append(ud.nums[ud.index])
return 'succeeded'
StateMachine.add('EVEN', CBState(even_cb),
{'succeeded':'continue'})
#add odd state
@smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'],
output_keys=['odd_nums'],
outcomes=['succeeded'])
def odd_cb(ud):
ud.odd_nums.append(ud.nums[ud.index])
return 'succeeded'
StateMachine.add('ODD', CBState(odd_cb),
{'succeeded':'continue'})
## %EndTag(CONTAINER)%
## %Tag(ADDCONTAINER)%
#close container_sm
Iterator.set_contained_state('CONTAINER_STATE',
container_sm,
loop_outcomes=['continue'])
## %EndTag(ADDCONTAINER)%
## %Tag(ADDITERATOR)%
#close the tutorial_it
StateMachine.add('TUTORIAL_IT',tutorial_it,
{'succeeded':'succeeded',
'aborted':'aborted'})
## %EndTag(ADDITERATOR)%
return sm
def __init__(self, robot, dishwasher_id, dishwasher_navigate_area):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
dishwasher = EdEntityDesignator(robot=robot, id=dishwasher_id)
with self:
StateMachine.add("NAVIGATE_TO_DISHWASHER",
NavigateToSymbolic(robot, {dishwasher: dishwasher_navigate_area}, dishwasher),
transitions={'arrived': 'OPEN_DISHWASHER',
'unreachable': 'failed',
'goal_not_defined': 'failed'})
StateMachine.add("OPEN_DISHWASHER",
OpenDishwasher(robot, dishwasher_id),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
def TargetSelectorContainer(target_type):
sm_target_selector = StateMachine(['target_sent','aborted','preempted'])
with sm_target_selector:
target_selection_goal = targetSelectionGoal(target_type)
StateMachine.add('GET_TARGET', SimpleActionState('/select_target', SelectTargetAction,
goal=target_selection_goal, result_key='target_pose'),
transitions={'succeeded':'MOVE_BASE','aborted':'aborted','preempted':'preempted'},
remapping={'target_pose':'next_target'})
StateMachine.add('MOVE_BASE', SimpleActionState('/navigation/move_base', MoveBaseAction, goal_key='next_target'),
transitions={'succeeded':'target_sent','aborted':'aborted','preempted':'preempted'})
return sm_target_selector
def get_move_around_smach():
sm = StateMachine(outcomes=['preempted'])
#sm.set_initial_state('CHECK_MOVEMENT')
with sm:
StateMachine.add('SLEEP_UNTIL_ENABLED', util.get_sleep_until_smach_enabled_smach(),
transitions={'succeeded':'MOVE_RANDOMLY'})
StateMachine.add('MOVE_RANDOMLY', move_base.get_random_goal_smach('/base_link'),
transitions={'succeeded':'SLEEP_UNTIL_ENABLED',
'aborted':'SLEEP_UNTIL_ENABLED'})
# StateMachine.add("ARM_LOOK_AROUND", look_around.get_lookaround_smach(util.SleepState(LOOKAROUND_SLEEP_DURATION)),
# StateMachine.add('ARM_LOOK_AROUND', util.SleepState(1), # mockup
return sm
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 __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 __init__(self, robot):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
@cb_interface(outcomes=['succeeded'], output_keys=['position'])
def send_point(ud):
ud.position = PointStamped(header=Header(frame_id='/amigo/base_link'), point=Point(0.5, 0, 0.7))
return 'succeeded'
arm = Designator(robot.leftArm)
with self:
StateMachine.add("SEND_POINT", CBState(send_point),
transitions={'succeeded': 'FIND_CUP'})
StateMachine.add("FIND_CUP", SimpleGrab(robot, arm),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
def __init__(self):
smach.StateMachine.__init__(self, outcomes = [succeeded,aborted,preempted], output_keys = {'name_of_person'})
with self:
def face_recognition(userdata, goal):
face_recog_goal = FaceRecognitionGoal()
face_recog_goal.order_id = 0
face_recog_goal.order_argument = 'Referee'
return face_recog_goal
class FaceProcessData(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded', 'face_not_found', 'preempted', 'aborted'],
input_keys=['process_names', 'process_confidence'])
def execute(self, userdata):
if len(userdata.process_names) > 0:
for confidence_value in userdata.process_confidence:
if confidence_value > MINIMUM_CONFIDENCE:
return 'succeeded'
return 'face_not_found'
intro_text = "Please stay still while I recognise you."
StateMachine.add('TTS_SAY_CALIB',
SpeakActionState(intro_text),
transitions={succeeded: 'FACE_RECOGNITION'})
StateMachine.add('FACE_RECOGNITION', SimpleActionState('face_recognition', FaceRecognitionAction, goal_cb=face_recognition,
result_slots=['order_id', 'names', 'confidence']),
transitions={'succeeded': 'PROCESS_FACEDATA'},
remapping={'names': 'LP_names', 'confidence': 'LP_confidence'})
smach.StateMachine.add('PROCESS_FACEDATA', FaceProcessData(),
transitions={'succeeded': 'TTS_SAY_FOUND',
'face_not_found': 'TTS_SAY_NOT_FOUND',
'aborted': 'TTS_SAY_CALIB'},
remapping={'process_names': 'LP_names',
'process_confidence': 'LP_confidence',
})
found_text = "I found you!"
StateMachine.add('TTS_SAY_FOUND',
SpeakActionState(found_text),
transitions={'succeeded':'succeeded'},
remapping = {'name_of_person':''})
not_found_text = "Sorry, I wasn't looking for you..."
StateMachine.add('TTS_SAY_NOT_FOUND',
SpeakActionState(not_found_text),
transitions={'succeeded': 'moveAround'})
def __init__(self):
State.__init__(self, outcomes=['succeeded','aborted','preempted'])
setup_task_environment(self);
# Initialize the navigation state machine
self.sm_nav = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_nav.userdata.waypoints = self.waypoints
with self.sm_nav:
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':''},
remapping={'waypoint_in':'patrol_waypoint'})
pass
def create_find_clusters_sm():
sm = StateMachine(outcomes=['succeeded','aborted'],
output_keys=['segmentation_result','cluster_information','action_index'])
with sm:
StateMachine.add('RESET_ROBOT',
ResetRobot(),
remapping={'action_index':'action_index'},
transitions={'succeeded':'SEGMENT_SCENE'})
StateMachine.add('SEGMENT_SCENE',
SegmentScene(),
remapping={'segmentation_result':'segmentation_result',
'cluster_information':'cluster_information'},
transitions={'found_clusters':'succeeded',
'no_clusters':'aborted'})
return sm
def __init__(self, keyPointManager):
State.__init__(self, outcomes=['succeeded','aborted','preempted'])
self.keyPointManager = keyPointManager
self.sm_nav = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_nav:
StateMachine.add('PICK_KEYPOINT', PickWaypointKeyPoint(self.keyPointManager),
transitions={'succeeded':'NAV_KEYPOINT'},
remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('NAV_KEYPOINT', Nav2Waypoint(),
transitions={'succeeded':'PICK_KEYPOINT',
'aborted':'PICK_KEYPOINT',
'preempted':''},
remapping={'waypoint_in':'patrol_waypoint'})
pass