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('/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 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('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, 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, arm):
StateMachine.__init__(self, outcomes=["succeeded", "failed"], input_keys=['position'])
@cb_interface(outcomes=['succeeded'])
def prepare_grasp(ud):
# Open gripper (non-blocking)
arm.resolve().send_gripper_goal('open', timeout=0)
# Torso up (non-blocking)
robot.torso.high()
# Arm to position in a safe way
arm.resolve().send_joint_trajectory('prepare_grasp', timeout=0)
# Open gripper
arm.resolve().send_gripper_goal('open', timeout=0.0)
return 'succeeded'
with self:
StateMachine.add("PREPARE_GRASP", CBState(prepare_grasp),
transitions={'succeeded': 'SIMPLE_NAVIGATE_TO_GRASP'})
StateMachine.add("SIMPLE_NAVIGATE_TO_GRASP", SimpleNavigateToGrasp(robot, arm),
transitions={'unreachable': 'failed',
'arrived': 'SIMPLE_PICKUP',
'goal_not_defined': 'failed'})
StateMachine.add("SIMPLE_PICKUP", SimplePickup(robot, arm),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
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 __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 __init__(self, distance_threshold):
StateMachine.__init__(self, outcomes=['detected','failed','preempted'], input_keys=['sm_distance_threshold'], output_keys=['sm_pose'])
#self.userdata.sm_distance_threshold =
#self.userdata.required_still_checks =
with self:
smach.StateMachine.add('DETECT_CLOSE_PERSON', DetectClosePersonState(distance_threshold),
transitions = {'check_again':'DETECT_CLOSE_PERSON', 'done':'WAIT_FOR_STILL_PERSON', 'failed':'failed', 'preempted':'preempted'},
remapping = {'pose':'sm_pose'})
smach.StateMachine.add('WAIT_FOR_STILL_PERSON', WaitForStillPersonState(2),
transitions = {'check_again':'DETECT_CLOSE_PERSON','done':'detected','failed':'DETECT_CLOSE_PERSON','preempted':'preempted'},
remapping = {'pose':'sm_pose'})
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 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
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 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
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 __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):
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):
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 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):
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 __init__(self):
smach.StateMachine.__init__(self,[succeeded,aborted,preempted])
with self:
def move_around(userdata, goal):
moveGoal = MoveBaseGoal()
moveGoal.target_pose.header.stamp = rospy.Time.now()
moveGoal.target_pose.header.frame_id = 'base_link'
moveGoal.target_pose.pose.position.x = random.randint(-2, 2)
moveGoal.target_pose.pose.position.y = random.randint(-2, 2)
moveGoal.target_pose.pose.position.z = 0
rotationAngle = random.random()
moveGoal.target_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, rotationAngle))
print moveGoal
return moveGoal
# 2_1 is unnecessary
smach.StateMachine.add(
'FINDING',
FindPersonStateMachine(),
transitions ={'aborted':'MOVE_AROUND_DUMMY','succeeded':'THERE_IS_A_PERSON'})
smach.StateMachine.add(
'MOVE_AROUND_DUMMY',
MoveAroundCounter(),
transitions ={'aborted':'aborted','succeeded':'MOVE_AROUND'})
StateMachine.add('MOVE_AROUND',
SimpleActionState('move_base', MoveBaseAction, goal_cb=move_around),
transitions={'aborted': 'aborted','succeeded':'FINDING'})
smach.StateMachine.add(
'THERE_IS_A_PERSON',
SpeakActionState("I found a person."),
transitions={'succeeded': 'MOVE', 'aborted': 'MOVE'})
smach.StateMachine.add(
'MOVE',
MoveToPerson(),
transitions = {'succeeded':'succeeded'})
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
def __init__(self):
super(PatrolController, self).__init__('PatrolServer')
self.__keyListener = rospy.Subscriber('keys', String, self.__keysCallback)
#TODO make sure shuffling works and thet the first node stays the same
# Or try a flag to block the reissue of patrol
self.__waypoints = self.__loadWaypointsFromCsv()
self.__homeGoal = self.__getGoal(self.__waypoints[0])
self.__StateMachine = StateMachine(['succeded', 'aborted', 'preempted'])
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 __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 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 poly(sides):
poly = StateMachine(outcomes=['success'])
for i in range(0,sides-1):
StateMachine.add('SIDE{0}'.format(i+1),Drive(1),transitions={'success':'TURN{0}'.format(i+1)})
StateMachine.add('TURN{0}'.format(i+1),Turn(360.0/sides),transitions={'success':'SIDE{0}'.format(i+2)})
StateMachine.add('SIDE3',Drive(1),transitions={'success':'success'})
return poly
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):
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
# State machine for Action1
self.sm_action1 = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['waypoint_in'],
output_keys=['waypoint_out'])
self.sm_action1.userdata.speed = 0.1
self.sm_action1.userdata.distance = 1.0
with self.sm_action1:
StateMachine.add('NAV_WAYPOINT',
Nav2Waypoint(),
transitions={
'succeeded': 'FORWARD',
'aborted': 'aborted'
})
StateMachine.add('FORWARD',
MoveForward(),
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.5
with self.sm_action2:
StateMachine.add('NAV_WAYPOINT',
Nav2Waypoint(),
transitions={
'succeeded': 'SIDE',
'aborted': 'aborted'
})
StateMachine.add('SIDE',
MoveSide(),
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'])
with self.sm_action3:
StateMachine.add('NAV_WAYPOINT',
Nav2Waypoint(),
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'])
with self.sm_action4:
StateMachine.add('NAV_WAYPOINT',
Nav2Waypoint(),
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'])
with self.sm_action5:
StateMachine.add('NAV_WAYPOINT',
Nav2Waypoint(),
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'])
with self.sm_action6:
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',
'aborted', 'succeeded',
'preempted'
]),
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('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('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:
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()
if __name__ == '__main__':
rospy.init_node('HUMABOT_SHOPPING_LIST_TEST')
# Define needed nodes
# Nodes names to check
TOPIC_LIST_NAMES = ['/nao_robot/camera/top/camera/image_raw']
SERVICES_LIST_NAMES = [
'/nao_shopping_list/checkObjects', '/nao_shopping_list/trainObjects',
'/cmd_pose_srv'
]
ACTION_LIST_NAMES = ['/speech', '/joint_angles_action']
PARAMS_LIST_NAMES = []
# Create a SMACH state machine
sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
shoplistsm = DoShoppingListSM()
with sm:
StateMachine.add('CHECK_NODES',
CheckNodesState(TOPIC_LIST_NAMES, SERVICES_LIST_NAMES,
ACTION_LIST_NAMES, PARAMS_LIST_NAMES),
transitions={
'succeeded': 'START_THE_TEST',
'aborted': 'aborted'
})
StateMachine.add('START_THE_TEST',
StartTest(testName='Shopping list',
dist_m_to_square=0.4,
def __init__(self, cart_area, cart_sub_area, action_req, action_server):
StateMachine.__init__(self, outcomes=['done', 'failed'])
# get cart pose state params
map_frame_name = rospy.get_param('~map_frame_name', 'map')
# get cart setpoint state params
robot_length_m = float(rospy.get_param('~robot_length_m', '0.73'))
cart_length_m = float(rospy.get_param('~cart_length_m', '0.81'))
distance_to_cart_m = float(
rospy.get_param('~distance_to_cart_m', '1.0'))
# align and approach cart state params
offset_to_approach_pose_m = float(
rospy.get_param('~offset_to_approach_pose_m', '0.55'))
backward_vel_docking_ms = float(
rospy.get_param('~backward_vel_docking_ms', '0.1'))
max_rot_vel_docking_rads = float(
rospy.get_param('~max_rot_vel_docking_rads', '0.1'))
approach_x_thresh_m = float(
rospy.get_param('~approach_x_thresh_m', '0.05'))
approach_y_thresh_m = float(
rospy.get_param('~approach_y_thresh_m', '0.05'))
approach_yaw_thresh_rad = float(
rospy.get_param('~approach_yaw_thresh_rad', '0.1'))
# couple to cart state params
max_coupling_attempts = int(
rospy.get_param('max_coupling_attempts', '3'))
# post dock params
post_dock_forward_distance_m = float(
rospy.get_param('~post_dock_forward_distance_m', '0.2'))
self.userdata.cart_pose = None
self.userdata.pre_dock_setpoint = None
self.userdata.post_dock_setpoint = None
self.userdata.cart_area = cart_area
self.userdata.cart_sub_area = cart_sub_area
self.userdata.action_req = action_req
self.userdata.action_server = action_server
self.userdata.area_shape = None
self.userdata.sub_area_shape = None
with self:
StateMachine.add('GET_CART_POSE',
GetCartPose(map_frame_name=map_frame_name),
transitions={
'cart_found': 'GET_SETPOINT_IN_PRE_DOCK_AREA',
'cart_not_found': 'LOOK_FOR_CART',
'timeout': 'failed'
})
StateMachine.add('GET_SETPOINT_IN_PRE_DOCK_AREA',
GetSetpointInPreDockArea(
robot_length_m=robot_length_m,
cart_length_m=cart_length_m,
distance_to_cart_m=distance_to_cart_m),
transitions={
'setpoint_found': 'GO_TO_PRE_DOCK_SETPOINT',
'setpoint_unreachable': 'failed',
'timeout': 'failed'
})
StateMachine.add('GO_TO_PRE_DOCK_SETPOINT',
GoToPreDockSetpoint(),
transitions={
'reached_setpoint': 'ALIGN_AND_APPROACH_CART',
'setpoint_unreachable': 'failed',
'timeout': 'failed'
})
StateMachine.add(
'ALIGN_AND_APPROACH_CART',
AlignAndApproachCart(
offset_to_approach_pose_m=offset_to_approach_pose_m,
backward_vel_docking_ms=backward_vel_docking_ms,
max_rot_vel_docking_rads=max_rot_vel_docking_rads,
approach_x_thresh_m=approach_x_thresh_m,
approach_y_thresh_m=approach_y_thresh_m,
approach_yaw_thresh_rad=approach_yaw_thresh_rad),
transitions={
'approach_succeeded': 'COUPLE_TO_CART',
'cart_not_found': 'GET_CART_POSE',
'cart_pose_publisher_not_available': 'failed',
'timeout': 'GO_TO_PRE_DOCK_SETPOINT'
})
StateMachine.add(
'COUPLE_TO_CART',
CoupleToCart(max_coupling_attempts=max_coupling_attempts),
transitions={
'coupling_succeeded': 'GET_SETPOINT_IN_POST_DOCK_AREA',
'coupling_failed': 'GO_TO_PRE_DOCK_SETPOINT',
'cannot_switch_to_load_mode': 'failed'
})
StateMachine.add(
'GET_SETPOINT_IN_POST_DOCK_AREA',
GetSetpointInPostDockArea(
distance_to_move=post_dock_forward_distance_m),
transitions={
'setpoint_found': 'GO_TO_POST_DOCK_SETPOINT',
# we consider the action a success even if it doesn't move forward post docking
'setpoint_unreachable': 'done',
'timeout': 'done'
})
StateMachine.add('GO_TO_POST_DOCK_SETPOINT',
GoToPostDockSetpoint(),
transitions={
'reached_setpoint': 'done',
'setpoint_unreachable': 'done',
'timeout': 'done'
})
#############################################################################################################
## Non-nominal states; i.e. states to execute when the nominal execution fails
StateMachine.add('LOOK_FOR_CART',
LookForCart(map_frame_name=map_frame_name,
robot_length_m=robot_length_m),
transitions={
'cart_found': 'GET_SETPOINT_IN_PRE_DOCK_AREA',
'cart_not_found': 'failed',
'timeout': 'failed'
})
def create_statemachine(self):
try:
response = self.relative_work_point_srv()
except rospy.ServiceException as e:
rospy.logerr('servive call failed:{}'.format(e))
relative_grasp_pose = PoseStamped()
relative_grasp_pose2 = PoseStamped()
relative_grasp_pose3 = PoseStamped()
relative_grasp_pose4 = PoseStamped()
relative_grasp_pose5 = PoseStamped()
relative_grasp_pose6 = PoseStamped()
relative_assemble_pose = PoseStamped()
relative_assemble_pose2 = PoseStamped()
relative_assemble_pose3 = PoseStamped()
relative_assemble_pose4 = PoseStamped()
relative_assemble_pose5 = PoseStamped()
relative_assemble_pose6 = PoseStamped()
relative_grasp_pose.pose = response.grasp[0]
relative_grasp_pose2.pose = response.grasp[1]
relative_grasp_pose3.pose = response.grasp[2]
relative_grasp_pose4.pose = response.grasp[3]
relative_grasp_pose5.pose = response.grasp[4]
relative_grasp_pose6.pose = response.grasp[5]
relative_assemble_pose.pose = response.assemble[0]
relative_assemble_pose2.pose = response.assemble[1]
relative_assemble_pose3.pose = response.assemble[2]
relative_assemble_pose4.pose = response.assemble[3]
relative_assemble_pose5.pose = response.assemble[4]
relative_assemble_pose6.pose = response.assemble[5]
sm_top = StateMachine(outcomes=['success', 'failed'])
with sm_top:
StateMachine.add(
"InitialState",
InitialState(relative_grasp_pose),
transitions={
'success': 'SUB1',
'failed': 'failed'})
sm_sub1 = StateMachine(outcomes=['success', 'failed'])
with sm_sub1:
StateMachine.add(
"BeforeGrasp",
BeforeGrasp(),
transitions={
'success': 'AfterGrasp',
'failed': 'failed'})
StateMachine.add(
"AfterGrasp",
AfterGrasp(relative_assemble_pose),
transitions={
'success': 'BeforeAssemble',
'failed': 'failed'})
StateMachine.add(
"BeforeAssemble",
BeforeAssemble(relative_assemble_pose),
transitions={
'success': 'AfterAssembleRepetition',
'failed': 'failed'})
StateMachine.add(
"AfterAssembleRepetition",
AfterAssembleRepetition(relative_grasp_pose2),
transitions={
'success': 'success',
'failed': 'failed'})
StateMachine.add(
"SUB1", sm_sub1, transitions={
'success': 'SUB2', 'failed': 'failed'})
sm_sub2 = StateMachine(outcomes=['success', 'failed'])
with sm_sub2:
StateMachine.add(
"BeforeGrasp",
BeforeGrasp(),
transitions={
'success': 'AfterGrasp',
'failed': 'failed'})
StateMachine.add(
"AfterGrasp",
AfterGrasp(relative_assemble_pose2),
transitions={
'success': 'BeforeAssemble',
'failed': 'failed'})
StateMachine.add(
"BeforeAssemble",
BeforeAssemble(relative_assemble_pose2),
transitions={
'success': 'AfterAssembleRepetition',
'failed': 'failed'})
StateMachine.add(
"AfterAssembleRepetition",
AfterAssembleRepetition(relative_grasp_pose3),
transitions={
'success': 'success',
'failed': 'failed'})
StateMachine.add(
"SUB2", sm_sub2, transitions={
'success': 'SUB3', 'failed': 'failed'})
sm_sub3 = StateMachine(outcomes=['success', 'failed'])
with sm_sub3:
StateMachine.add(
"BeforeGrasp",
BeforeGrasp(),
transitions={
'success': 'AfterGrasp',
'failed': 'failed'})
StateMachine.add(
"AfterGrasp",
AfterGrasp(relative_assemble_pose3),
transitions={
'success': 'BeforeAssemble',
'failed': 'failed'})
StateMachine.add(
"BeforeAssemble",
BeforeAssemble(relative_assemble_pose3),
transitions={
'success': 'AfterAssembleRepetition',
'failed': 'failed'})
StateMachine.add(
"AfterAssembleRepetition",
AfterAssembleRepetition(relative_grasp_pose4),
transitions={
'success': 'success',
'failed': 'failed'})
StateMachine.add(
"SUB3", sm_sub3, transitions={
'success': 'SUB4', 'failed': 'failed'})
sm_sub4 = StateMachine(outcomes=['success', 'failed'])
with sm_sub4:
StateMachine.add(
"BeforeGrasp",
BeforeGrasp(),
transitions={
'success': 'AfterGrasp',
'failed': 'failed'})
StateMachine.add(
"AfterGrasp",
AfterGrasp(relative_assemble_pose4),
transitions={
'success': 'BeforeAssemble',
'failed': 'failed'})
StateMachine.add(
"BeforeAssemble",
BeforeAssemble(relative_assemble_pose4),
transitions={
'success': 'AfterAssembleRepetition',
'failed': 'failed'})
StateMachine.add(
"AfterAssembleRepetition",
AfterAssembleRepetition(relative_grasp_pose5),
transitions={
'success': 'success',
'failed': 'failed'})
StateMachine.add(
"SUB4", sm_sub4, transitions={
'success': 'SUB5', 'failed': 'failed'})
sm_sub5 = StateMachine(outcomes=['success', 'failed'])
with sm_sub5:
StateMachine.add(
"BeforeGrasp",
BeforeGrasp(),
transitions={
'success': 'AfterGrasp',
'failed': 'failed'})
StateMachine.add(
"AfterGrasp",
AfterGrasp(relative_assemble_pose5),
transitions={
'success': 'BeforeAssemble',
'failed': 'failed'})
StateMachine.add(
"BeforeAssemble",
BeforeAssemble(relative_assemble_pose5),
transitions={
'success': 'AfterAssembleRepetition',
'failed': 'failed'})
StateMachine.add(
"AfterAssembleRepetition",
AfterAssembleRepetition(relative_grasp_pose6),
transitions={
'success': 'success',
'failed': 'failed'})
StateMachine.add(
"SUB5", sm_sub5, transitions={
'success': 'SUB6', 'failed': 'failed'})
sm_sub6 = StateMachine(outcomes=['success', 'failed'])
with sm_sub6:
StateMachine.add(
"BeforeGrasp",
BeforeGrasp(),
transitions={
'success': 'AfterGrasp',
'failed': 'failed'})
StateMachine.add(
"AfterGrasp",
AfterGrasp(relative_assemble_pose6),
transitions={
'success': 'BeforeAssemble',
'failed': 'failed'})
StateMachine.add(
"BeforeAssemble",
BeforeAssemble(relative_assemble_pose6),
transitions={
'success': 'AfterAssemble',
'failed': 'failed'})
StateMachine.add(
"AfterAssemble",
AfterAssemble(),
transitions={
'success': 'success',
'failed': 'failed'})
StateMachine.add(
"SUB6",
sm_sub6,
transitions={
'success': 'FinalState',
'failed': 'failed'})
StateMachine.add(
"FinalState",
FinalState(),
transitions={
'success': 'success',
'failed': 'failed'})
sis = IntrospectionServer('p_and_p_example', sm_top, '/SM_ROOT')
sis.start()
sm_top.execute()
rospy.spin()
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 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 polygonial():
# Construct static goals
polygon_big = turtle_actionlib.msg.ShapeGoal(edges=11, radius=4.0)
polygon_small = turtle_actionlib.msg.ShapeGoal(edges=6, radius=0.5)
##########################
# Create a SMACH state machine
sm0 = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Open the container
with sm0:
# Reset turtlesim
StateMachine.add('RESET', ServiceState('reset', std_srvs.srv.Empty),
{'succeeded': 'SPAWN'})
# Create a second turtle
StateMachine.add(
'SPAWN',
ServiceState('spawn',
turtlesim.srv.Spawn,
request=turtlesim.srv.SpawnRequest(
0.0, 0.0, 0.0, 'turtle2')),
{'succeeded': 'TELEPORT1'})
# Teleport turtle 1
StateMachine.add(
'TELEPORT1',
ServiceState('turtle1/teleport_absolute',
turtlesim.srv.TeleportAbsolute,
request=turtlesim.srv.TeleportAbsoluteRequest(
5.0, 1.0, 0.0)), {'succeeded': 'DRAW_SHAPES'})
# Draw some polygons
shapes_cc = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='aborted',
outcome_map={
'succeeded': {
'BIG': 'succeeded',
'SMALL': 'succeeded'
}
})
StateMachine.add('DRAW_SHAPES', shapes_cc)
with shapes_cc:
# Draw a large polygon with the first turtle
Concurrence.add(
'BIG',
SimpleActionState('turtle_shape1',
turtle_actionlib.msg.ShapeAction,
goal=polygon_big))
# Draw a small polygon with the second turtle
small_shape_sm = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
Concurrence.add('SMALL', small_shape_sm)
with small_shape_sm:
# Teleport turtle 2
StateMachine.add(
'TELEPORT2',
ServiceState('turtle2/teleport_absolute',
turtlesim.srv.TeleportAbsolute,
request=turtlesim.srv.TeleportAbsoluteRequest(
9.0, 5.0, 0.0)),
{'succeeded': 'DRAW_WITH_MONITOR'})
# Construct a concurrence for the shape action and the monitor
draw_monitor_cc = Concurrence(
['succeeded', 'aborted', 'preempted', 'interrupted'],
'aborted',
child_termination_cb=lambda so: True,
outcome_map={
'succeeded': {
'DRAW': 'succeeded'
},
'preempted': {
'DRAW': 'preempted',
'MONITOR': 'preempted'
},
'interrupted': {
'MONITOR': 'invalid'
}
})
StateMachine.add('DRAW_WITH_MONITOR', draw_monitor_cc,
{'interrupted': 'WAIT_FOR_CLEAR'})
with draw_monitor_cc:
Concurrence.add(
'DRAW',
SimpleActionState('turtle_shape2',
turtle_actionlib.msg.ShapeAction,
goal=polygon_small))
def turtle_far_away(ud, msg):
"""Returns True while turtle pose in msg is at least 1 unit away from (9,5)"""
if sqrt(pow(msg.x - 9.0, 2) +
pow(msg.y - 5.0, 2)) > 2.0:
return True
return False
Concurrence.add(
'MONITOR',
MonitorState('/turtle1/pose',
turtlesim.msg.Pose,
cond_cb=turtle_far_away))
StateMachine.add(
'WAIT_FOR_CLEAR',
MonitorState(
'/turtle1/pose',
turtlesim.msg.Pose,
cond_cb=lambda ud, msg: not turtle_far_away(ud, msg)),
{
'valid': 'WAIT_FOR_CLEAR',
'invalid': 'TELEPORT2'
})
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Set preempt handler
smach_ros.set_preempt_handler(sm0)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
# smach_thread = threading.Thread(target = sm0.execute)
# smach_thread.start()
##################################
# Construct action server wrapper
asw = ActionServerWrapper(
'my_action_server_name',
MachineAction,
wrapped_container=sm0,
succeeded_outcomes=['did_something', 'did_something_else'],
aborted_outcomes=['aborted'],
preempted_outcomes=['preempted'])
# Run the server in a background thread
asw.run_server()
def __init__(self):
rospy.init_node('follow')
rospy.logerr('gogogo')
self.trace = Concurrence(outcomes=['remeber', 'stop', 'aborted'],
default_outcome='stop',
outcome_map={
'remeber': {
'STOP': 'remeber'
},
'stop': {
'STOP': 'stop'
},
'aborted': {
'FOLLOW': 'aborted'
}
},
child_termination_cb=self.trace_child_cb,
input_keys=['PT_LIST', 'mission'],
output_keys=['PT_LIST', 'mission'])
with self.trace:
self.meta_follow = StateMachine(
['succeeded', 'aborted', 'preempted'])
with self.meta_follow:
StateMachine.add('FIND',
FindPeople().find_people_,
transitions={
'invalid': 'META_NAV',
'valid': 'FIND',
'preempted': 'preempted'
},
remapping={'pos_xm': 'pos_xm'})
self.meta_nav = Concurrence(
outcomes=['time_over', 'get_pos', 'aborted'],
default_outcome='aborted',
outcome_map={
'time_over': {
'WAIT': 'succeeded'
},
'get_pos': {
'NAV': 'succeeded'
},
'aborted': {
'NAV': 'aborted'
}
},
child_termination_cb=self.nav_child_cb,
input_keys=['pos_xm'])
with self.meta_nav:
Concurrence.add('NAV',
NavStack(),
remapping={'pos_xm': 'pos_xm'})
Concurrence.add('WAIT', Wait_trace())
StateMachine.add('META_NAV',
self.meta_nav,
transitions={
'get_pos': 'FIND',
'time_over': 'FIND',
'aborted': 'FIND'
})
Concurrence.add('FOLLOW', self.meta_follow)
Concurrence.add('STOP',
CheckStop(),
remapping={
'PT_LIST': 'PT_LIST',
'mission': 'mission'
})
self.shopping = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.shopping:
self.shopping.userdata.PT_LIST = {}
self.shopping.userdata.mission = {}
self.shopping.userdata.task = list()
self.shopping.userdata.rec = 5.0
StateMachine.add('RUNNODE',
RunNode(),
transitions={
'succeeded': 'WAIT',
'aborted': 'aborted'
})
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'TRACE',
'error': 'error'
},
remapping={'rec': 'rec'})
StateMachine.add("TRACE",
self.trace,
transitions={
'remeber': 'succeeded',
'stop': 'succeeded',
'aborted': 'aborted'
})
intro_server = IntrospectionServer('shopping', self.shopping,
'SM_ROOT')
intro_server.start()
out = self.shopping.execute()
intro_server.stop()
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
# State machine for Action1
self.sm_action1 = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['waypoint_in'],
output_keys=['waypoint_out'])
self.sm_action1.userdata.speed = 0.1
self.sm_action1.userdata.distance = 1.0
with self.sm_action1:
StateMachine.add('NAV_WAYPOINT',
Nav2Waypoint(),
transitions={
'succeeded': 'FORWARD',
'aborted': 'aborted'
})
StateMachine.add('FORWARD',
MoveForward(),
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.5
with self.sm_action2:
StateMachine.add('NAV_WAYPOINT',
Nav2Waypoint(),
transitions={
'succeeded': 'SIDE',
'aborted': 'aborted'
})
StateMachine.add('SIDE',
MoveSide(),
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'])
with self.sm_action3:
StateMachine.add('NAV_WAYPOINT',
Nav2Waypoint(),
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'])
with self.sm_action4:
StateMachine.add('NAV_WAYPOINT',
Nav2Waypoint(),
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'])
with self.sm_action5:
StateMachine.add('NAV_WAYPOINT',
Nav2Waypoint(),
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'])
with self.sm_action6:
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',
'aborted', 'succeeded',
'preempted'
]),
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('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('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:
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 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'
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 Follow():
def __init__(self):
rospy.init_node('follow')
rospy.logerr('gogogo')
self.trace = Concurrence(outcomes=['remeber', 'stop', 'aborted'],
default_outcome='stop',
outcome_map={
'remeber': {
'STOP': 'remeber'
},
'stop': {
'STOP': 'stop'
},
'aborted': {
'FOLLOW': 'aborted'
}
},
child_termination_cb=self.trace_child_cb,
input_keys=['PT_LIST', 'mission'],
output_keys=['PT_LIST', 'mission'])
with self.trace:
self.meta_follow = StateMachine(
['succeeded', 'aborted', 'preempted'])
with self.meta_follow:
StateMachine.add('FIND',
FindPeople().find_people_,
transitions={
'invalid': 'META_NAV',
'valid': 'FIND',
'preempted': 'preempted'
},
remapping={'pos_xm': 'pos_xm'})
self.meta_nav = Concurrence(
outcomes=['time_over', 'get_pos', 'aborted'],
default_outcome='aborted',
outcome_map={
'time_over': {
'WAIT': 'succeeded'
},
'get_pos': {
'NAV': 'succeeded'
},
'aborted': {
'NAV': 'aborted'
}
},
child_termination_cb=self.nav_child_cb,
input_keys=['pos_xm'])
with self.meta_nav:
Concurrence.add('NAV',
NavStack(),
remapping={'pos_xm': 'pos_xm'})
Concurrence.add('WAIT', Wait_trace())
StateMachine.add('META_NAV',
self.meta_nav,
transitions={
'get_pos': 'FIND',
'time_over': 'FIND',
'aborted': 'FIND'
})
Concurrence.add('FOLLOW', self.meta_follow)
Concurrence.add('STOP',
CheckStop(),
remapping={
'PT_LIST': 'PT_LIST',
'mission': 'mission'
})
self.shopping = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.shopping:
self.shopping.userdata.PT_LIST = {}
self.shopping.userdata.mission = {}
self.shopping.userdata.task = list()
self.shopping.userdata.rec = 5.0
StateMachine.add('RUNNODE',
RunNode(),
transitions={
'succeeded': 'WAIT',
'aborted': 'aborted'
})
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'TRACE',
'error': 'error'
},
remapping={'rec': 'rec'})
StateMachine.add("TRACE",
self.trace,
transitions={
'remeber': 'succeeded',
'stop': 'succeeded',
'aborted': 'aborted'
})
intro_server = IntrospectionServer('shopping', self.shopping,
'SM_ROOT')
intro_server.start()
out = self.shopping.execute()
intro_server.stop()
def trace_child_cb(self, outcome_map):
if outcome_map['STOP'] == 'stop':
rospy.logwarn('get the stop signal, stop tracing ........')
pid = get_pid("people_tracking")
subprocess.call('kill ' + str(pid[0]), shell=True)
return True
elif outcome_map['STOP'] == 'remeber':
rospy.logwarn('ready to remeber the position')
return True
if outcome_map['FOLLOW']:
rospy.logerr('the follow state meet error!')
return True
return False
def nav_child_cb(self, outcome_map):
if outcome_map['WAIT'] == 'succeeded':
rospy.logwarn('get the pos again')
return True
elif outcome_map['NAV'] == 'succeeded':
return True
elif outcome_map['NAV'] == 'aborted':
return True
else:
print(outcome_map)
return False
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(40.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 build_state_machine(self):
'''
建立状态机
'''
if not self.sm_carry_object is None:
self.sm_carry_object = None
self.sm_carry_object = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_carry_object:
StateMachine.add('GRASP_OBJECT', GraspObject(), transitions={'succeeded':'GET_LOCATION',
'aborted':'END_ABORT'})
StateMachine.add('GET_LOCATION', GetLocation(), transitions={'succeeded':'NAV_TO_WAYPOINT',
'aborted':'END_ABORT'},
remapping={'waypoint_out':'nav_waypoint'})
StateMachine.add('NAV_TO_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'RELEASE_OBJECT',
'aborted':'END_ABORT'},
remapping={'waypoint_in':'nav_waypoint'})
StateMachine.add('RELEASE_OBJECT', ReleaseObject(), transitions={'succeeded':'TURN_BODY',
'aborted':'END_ABORT'})
StateMachine.add('TURN_BODY', TurnBody(), transitions={'succeeded':'GRASP_OBJECT',
'aborted':'END_ABORT'})
StateMachine.add('END_ABORT', EndAbort(), transitions={'succeeded':'succeeded'})
StateMachine.add('END_SUCESS', EndSuccess(), transitions={'succeeded':'succeeded'})
StateMachine.add('START_POSITION', GoToPostureState(startPose, 0.5), transitions={'succeeded': 'succeeded'})
class HomeOff_SM(StateMachine):
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'})
# TEST
if __name__ == '__main__':
rospy.init_node('ON_OFF_TEST')
sm = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
# Wrap command line arguments
if len(sys.argv) == 1:
init=None
startPose=None
elif len(sys.argv) == 2:
init=str(sys.argv[1])
startPose=None
else:
init=str(sys.argv[1])
startPose=str(sys.argv[2])
with sm:
StateMachine.add('HOME', HomeState(init, startPose), transitions={'succeeded': 'succeeded'})
sm.execute()
def __init__(self):
rospy.init_node('ware_house', anonymous=False)
# rospy.on_shutdown(self.shutdown)
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
self.move_base.wait_for_server(rospy.Duration(15))
rospy.loginfo("Connected to move_base action server")
self.nav_goal = None
quaternions = list()
euler_angles = (pi / 2, pi, 3 * pi / 2, 0)
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
self.waypoints = list()
self.waypoints.append(Pose(Point(-2.92, 11.8, 0.0025), quaternions[3]))
self.waypoints.append(Pose(Point(-2.7, 19.7, 0.0035), quaternions[0]))
self.waypoints.append(Pose(Point(12.6, 18.9, 0.0183), quaternions[1]))
self.waypoints.append(Pose(Point(11.5, 7.17, 0.0121), quaternions[1]))
room_locations = (('starting_point', self.waypoints[0]),
('loading_point1', self.waypoints[1]),
('loading_point2', self.waypoints[2]),
('unloading_point', self.waypoints[3]))
self.room_locations = OrderedDict(room_locations)
nav_states = {}
def smach(self):
for room in self.room_locations.iterkeys():
def nav_goal_cb(userdata, goal):
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.waypoints[room]
return nav_goal
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=self.nav_goal,
goal_cb=self.nav_goal_cb,
exec_timeout=rospy.Duration(15.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states[room] = move_base_state
sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with sm:
StateMachine.add('POWER_ON',
PowerOnRobot(),
transitions={'succeeded': 'WAITING'})
StateMachine.add('WAITING',
Waiting(),
transitions={'succeeded': 'STARTING'})
StateMachine.add('STARTING',
nav_states['starting_point'],
transitions={
'succeeded': 'LOADING_POINT',
'aborted': 'LOADING_POINT',
'preempted': 'LOADING_POINT'
})
StateMachine.add('LOADING_POINT',
nav_states['loading_point'],
transitions={
'succeeded': 'UNLOADING_POINT',
'aborted': 'UNLOADING_POINT',
'preempted': 'UNLOADING_POINT'
})
StateMachine.add('UNLOADING_POINT',
nav_states['unloading_point'],
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
intro_server = IntrospectionServer('restaurant', sm, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = sm.execute()
intro_server.stop()
def shutdown(self):
rospy.loginfo("Stopping the robot...")
rospy.sleep(1)
class Navigation(Iterator):
def __init__(self):
Iterator.__init__(self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['log_mission'],
it=[],
output_keys=['log_mission'],
it_label='waypoint_id',
exhausted_outcome='succeeded')
self.register_start_cb(self.set_iterator_list)
with self:
self.sm_nav = StateMachine(
outcomes=['succeeded', 'preempted', 'aborted', 'continue'],
input_keys=['waypoint_id', 'log_mission'],
output_keys=['log_mission'])
self.sm_nav.register_start_cb(self.start_cb_nav)
self.sm_nav.register_termination_cb(self.termination_cb_nav)
with self.sm_nav:
StateMachine.add('MOVE_BASE',
SimpleActionState(
'move_base',
MoveBaseAction,
input_keys=['waypoint_id', 'log_mission'],
goal_cb=self.move_base_goal_cb),
transitions={
'succeeded': 'SAVE_DATA',
'aborted': 'continue'
})
self.save_data_service = ServiceState(
'make_data_acquisition',
RequestSaveData,
response_cb=self.save_data_response_cb)
StateMachine.add('SAVE_DATA',
self.save_data_service,
transitions={'succeeded': 'continue'})
# Close the sm_nav machine and add it to the iterator
Iterator.set_contained_state('FOLLOW_PATH',
self.sm_nav,
loop_outcomes=['continue'])
def set_iterator_list(self, userdata, initial_states):
with self:
waypoints = userdata.log_mission['waypoints']
start_index = userdata.log_mission['patrol']['index_waypoint']
Iterator.set_iteritems(range(start_index, len(waypoints)),
'waypoint_id')
def save_data_response_cb(self, userdata, response):
if self.save_data_service.preempt_requested():
self.save_data_service.service_preempt()
return 'preempted'
return 'succeeded'
def move_base_goal_cb(self, ud, goal):
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
waypoint = next(wp for wp in ud.log_mission['waypoints']
if wp["id"] == ud.waypoint_id)
position = Point(waypoint['x'], waypoint['y'], 0.0)
q_angle = quaternion_from_euler(0, 0, waypoint['theta'], axes='sxyz')
orientation = Quaternion(*q_angle)
nav_goal.target_pose.pose.position = position
nav_goal.target_pose.pose.orientation = orientation
return nav_goal
def start_cb_nav(self, userdata, initial_states):
add_marker_rviz(
[userdata.log_mission['waypoints'][userdata.waypoint_id]], 4)
def termination_cb_nav(self, userdata, terminal_states, container_outcome):
status = None
if container_outcome == 'continue' or container_outcome == 'succeeded':
if terminal_states == ['MOVE_BASE']:
status = 'aborted'
add_marker_rviz(
[userdata.log_mission['waypoints'][userdata.waypoint_id]],
3)
else:
status = 'succeeded'
add_marker_rviz(
[userdata.log_mission['waypoints'][userdata.waypoint_id]],
2)
else:
deleteall_marker_rviz()
pym.cycle.update_one({"_id": userdata.log_mission['_id']}, {
"$set": {
"data.patrol.index_waypoint": userdata.waypoint_id
},
"$push": {
"data.navigation": status
}
})
# def execute(self, userdata):
# print 'two'
# sleep(1)
# if (userdata.counter % 2 == 0):
# userdata.counter = userdata.counter + 1
# return 'visible'
# else:
# userdata.counter = userdata.counter + 1
# return 'not_visible'
if __name__ == '__main__':
rospy.init_node("behaviour_output")
discovered = []
rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
sm = StateMachine(outcomes=['success', 'failure', 'out_of_tags'])
sm.userdata.tags = []
sm.userdata.furthest_tag_id = -1
with sm:
# def result(userdata, sta)
StateMachine.add('LOOKING_FOR_FIRST_TARGET',
Looking_for_first_target(),
transitions={
'found': 'APPROACHING_TARGET',
'not_found': 'failure'
},
remapping={
'tags': 'tags',
'furthest_tag_id': 'furthest_tag_id'
})
StateMachine.add('APPROACHING_TARGET',
def __init__(self):
rospy.init_node('GpsrSmach')
rospy.on_shutdown(self.shutdown)
rospy.logerr('you are all sillyb')
self.smach_bool = False
# xm_arm_moveit_name('nav_pose')#最开始时，把机械臂降下来
self.sm_EnterRoom = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_EnterRoom:
# if use arm, use this state
# self.sm_EnterRoom.userdata.arm_mode_1 =0
# self.sm_EnterRoom.userdata.arm_ps = PointStamped()
# StateMachine.add('NAV_POSE',
# ArmCmd(),
# transitions={'succeeded':'DOOR_DETECT','aborted':'NAV_POSE','error':'error'},
# remapping ={'arm_ps':'arm_ps','mode':'arm_mode_1'})
#first detect the door is open or not
StateMachine.add('DOOR_DETECT',
DoorDetect().door_detect_,
transitions={
'invalid': 'WAIT',
'valid': 'DOOR_DETECT',
'preempted': 'aborted'
})
# after clear the costmap, wait for some seconds
self.sm_EnterRoom.userdata.rec = 3.0
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'SIMPLE_MOVE',
'error': 'error'
},
remapping={'rec': 'rec'})
# before executing the nav-stack, move directly through the door may make the nav-stack easier
self.sm_EnterRoom.userdata.go_point = Point(0.1, 0.0, 0.0)
StateMachine.add('SIMPLE_MOVE',
SimpleMove_move(),
remapping={'point': 'go_point'},
transitions={
'succeeded': 'NAV_1',
'aborted': 'NAV_1',
'error': 'error'
})
# go to the known Pose in the map to get the gpsr-command
self.sm_EnterRoom.userdata.start_waypoint = gpsr_target['speaker'][
'pos']
StateMachine.add('NAV_1',
NavStack(),
transitions={
'succeeded': 'succeeded',
'aborted': 'NAV_1',
'error': 'error'
},
remapping={'pos_xm': 'start_waypoint'})
self.sm_Find = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
# 寻找人
with self.sm_Find:
# for the people-finding task, pay attention that at the end of the task,
# run a simple executable file to make the cv-node release the KinectV2
# otherwise, the find-object task may throw error and all the cv-node may
# break down
self.sm_Person = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_Person:
self.sm_Person.userdata.rec = 2.0
# run the people_tracking node
StateMachine.add('RUNNODE',
RunNode(),
transitions={
'succeeded': 'WAIT',
'aborted': 'succeeded'
})
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'GET_PEOPLE_POS',
'error': 'error'
},
remapping={'rec': 'rec'})
#the data should be PointStamped()
# 这里必须先运行节点，也就是用RunNode()状态
self.sm_Person.userdata.pos_xm = Pose()
StateMachine.add('GET_PEOPLE_POS',
FindPeople().find_people_,
transitions={
'invalid': 'NAV_PEOPLE',
'valid': 'SPEAK',
'preempted': 'aborted'
},
remapping={'pos_xm': 'pos_xm'})
# this state will use the userdata remapping in the last state
StateMachine.add('NAV_PEOPLE',
NavStack(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'NAV_PEOPLE',
'error': 'error'
},
remapping={'pos_xm': 'pos_xm'})
self.sm_Person.userdata.sentences = 'I find you'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'CLOSEKINECT',
'error': 'error'
},
remapping={'sentences': 'sentences'})
# close the KinectV2
StateMachine.add('CLOSEKINECT',
CloseKinect(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
self.sm_Pos = StateMachine(
outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
with self.sm_Pos:
# this command may be foolish, but it may be used in the next competition
# get the name of the target
# in the target_gpsr.py we define some Pose() , they are all named as ${room_name}+'_table_1(2)'
# because each room we have some best detecting positions, we should make xm go to these places
# for object-find tasks
self.sm_Pos.userdata.pose = Pose()
self.sm_Pos.userdata.mode_1 = 1
StateMachine.add('GET_POS',
GetPos(),
remapping={
'target': 'target',
'current_task': 'current_task',
'pose': 'pose',
'mode': 'mode_1'
},
transitions={
'succeeded': 'NAV_HEHE',
'aborted': 'aborted',
'error': 'error'
})
StateMachine.add('NAV_HEHE',
NavStack(),
remapping={'pos_xm': 'pose'},
transitions={
'succeeded': 'GET_TARGET',
'aborted': 'NAV_HEHE',
'error': 'error'
})
self.sm_Pos.userdata.target_mode = 0
self.sm_Pos.userdata.name = ''
StateMachine.add('GET_TARGET',
GetTarget(),
remapping={
'target': 'target',
'current_task': 'current_task',
'mode': 'target_mode',
'current_target': 'name'
},
transitions={
'succeeded': 'FIND_OBJECT',
'aborted': 'aborted',
'error': 'error'
})
self.sm_Pos.userdata.object_pos = PointStamped()
StateMachine.add('FIND_OBJECT',
FindObject(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'GET_POS_1',
'error': 'error'
},
remapping={
'name': 'name',
'object_pos': 'object_pos'
})
# if xm not finds the object in the first detecting-place, she should go to the second
# specified place for detect-task, but as U can see in the target_gpsr.py, some room are
# only one best detecting-place
self.sm_Pos.userdata.pose_1 = Pose()
self.sm_Pos.userdata.mode_2 = 2
StateMachine.add('GET_POS_1',
GetPos(),
remapping={
'target': 'target',
'current_task': 'current_task',
'pose': 'pose_1',
'mode': 'mode_2'
},
transitions={
'succeeded': 'NAV_HAHA',
'aborted': 'aborted',
'error': 'error'
})
StateMachine.add('NAV_HAHA',
NavStack(),
remapping={'pos_xm': 'pose_1'},
transitions={
'succeeded': 'FIND_OBJECT_1',
'aborted': 'NAV_HAHA',
'error': 'error'
})
self.sm_Pos.userdata.object_pos = PointStamped()
StateMachine.add('FIND_OBJECT_1',
FindObject(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'SPEAK',
'error': 'error'
},
remapping={
'name': 'name',
'object_pos': 'object_pos'
})
self.sm_Pos.userdata.sentences = 'I find it'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'succeeded',
'error': 'error'
},
remapping={'sentences': 'sentences'})
# this state is used for swithing the mode, finding people or finding object?
StateMachine.add('PERSON_OR_POS',
PersonOrPosition(),
transitions={
'person': 'PERSON',
'position': 'POS',
'error': 'error'
},
remapping={
'target': 'target',
'current_task': 'current_task'
})
StateMachine.add('PERSON',
self.sm_Person,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
})
StateMachine.add('POS',
self.sm_Pos,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'target': 'target',
'current_task': 'current_task'
})
#nav tasks
self.sm_Nav = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
with self.sm_Nav:
self.sm_Nav.userdata.pos_xm = Pose()
self.sm_Nav.userdata.target_mode = 1
StateMachine.add('GETTARGET',
GetTarget(),
transitions={
'succeeded': "NAV_GO",
'aborted': 'aborted',
'error': 'error'
},
remapping={
'target': 'target',
'current_task': "current_task",
'current_target': 'pos_xm',
'mode': 'target_mode'
})
# StateMachine.add('FINDWAY',
# FindWay(),
# transitions = {'succeeded':'NAV_PATH1','aborted':'NAV_GO','error':'NAV_GO'},
# remapping={'way_path1':'way_path1','way_path2':'way_path2'})
# StateMachine.add('NAV_PATH1',
# NavStack(),
# transitions={'succeeded':'NAV_PATH2','aborted':'NAV_PATH1','error':'error'},
# remapping={'pos_xm':'way_path1'})
# StateMachine.add('NAV_PATH2',
# NavStack(),
# transitions = {'succeeded':'NAV_GO','aborted':'NAV_PATH2','error':'error'},
# remapping={'pos_xm':'way_path2'})
StateMachine.add('NAV_GO',
NavStack(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'NAV_GO',
'error': 'error'
},
remapping={'pos_xm': 'pos_xm'})
self.sm_Nav.userdata.sentences = 'I arrive here'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'succeeded',
'error': 'error'
},
remapping={'sentences': 'sentences'})
#follow task
# we will use the concurrence fun
# the tutorials in the wiki too easy, it is no help for you to understand the concurrence
# please see the test-snippet of the source of executive-smach
# https://github.com/ros/executive_smach/blob/indigo-devel/smach_ros/test/concurrence.py
self.sm_Follow = Concurrence(outcomes=['succeeded', 'aborted'],
default_outcome='succeeded',
outcome_map={
'succeeded': {
'STOP': 'stop'
},
'aborted': {
'FOLLOW': 'aborted'
}
},
child_termination_cb=self.child_cb)
with self.sm_Follow:
self.meta_follow = StateMachine(outcomes=['succeeded', 'aborted'])
with self.meta_follow:
StateMachine.add('FOLLOW',
SimpleFollow(),
transitions={
'succeeded': 'FOLLOW',
'aborted': 'aborted',
'preempt': 'succeeded'
})
Concurrence.add('FOLLOW', self.meta_follow)
Concurrence.add('STOP', CheckStop())
Concurrence.add('RUNNODE', RunNode())
# self.meta_Follow = StateMachine(outcomes =['succeeded','aborted','error'])
# with self.meta_Follow:
# StateMachine.add('RUNNODE',
# RunNode(),
# transitions={'succeeded':'FIND_PEOPLE','aborted':'succeeded'})
# self.meta_Follow.userdata.pos_xm = Pose()
# StateMachine.add('FIND_PEOPLE',
# FindPeople().find_people_,
# remapping ={'pos_xm':'pos_xm'},
# transitions ={'invalid':'MOVE','valid':'FIND_PEOPLE','preempted':'aborted'})
# StateMachine.add('MOVE',
# NavStack(),
# remapping ={'pos_xm':'pos_xm'},
# transitions={'succeeded':'FIND_PEOPLE','aborted':'MOVE','error':'error'})
# self.meta_Stop = StateMachine(outcomes =['succeeded','aborted'])
# with self.meta_Stop:
# StateMachine.add('STOP',
# StopFollow(),
# transitions ={'succeeded':'succeeded','aborted':'STOP'})
# Concurrence.add('FOLLOW',
# self.meta_Follow)
# Concurrence.add('STOP',
# self.meta_Stop)
# speak task
self.sm_Talk = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.sm_Talk:
self.sm_Talk.userdata.people_condition = list()
StateMachine.add('SPEAK',
Anwser(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
self.sm_Pick = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
with self.sm_Pick:
self.sm_Pick.userdata.name = ''
self.sm_Pick.userdata.target_mode = 0
self.sm_Pick.userdata.objmode = -1
StateMachine.add('RUNNODE_IMG',
RunNode_img(),
transitions={
'succeeded': 'GETNAME',
'aborted': 'aborted'
})
StateMachine.add('GETNAME',
GetTarget(),
remapping={
'target': 'target',
'current_task': 'current_task',
'mode': 'target_mode',
'current_target': 'name'
},
transitions={
'succeeded': 'FIND_OBJECT',
'aborted': 'aborted',
'error': 'error'
})
self.sm_Pick.userdata.object_pos = PointStamped()
StateMachine.add('FIND_OBJECT',
FindObject(),
transitions={
'succeeded': 'POS_JUSTFY',
'aborted': 'FIND_OBJECT',
'error': 'SPEAK'
},
remapping={
'name': 'name',
'object_pos': 'object_pos',
'objmode': 'objmode'
})
#making the xm foreward the object may make the grasping task easier
self.sm_Pick.userdata.pose = Pose()
StateMachine.add('POS_JUSTFY',
PosJustfy(),
remapping={
'object_pos': 'object_pos',
'pose': 'pose'
},
transitions={
'succeeded': 'NAV_TO',
'aborted': 'aborted',
'error': 'error'
})
StateMachine.add('NAV_TO',
NavStack(),
transitions={
'succeeded': 'RUNNODE_IMG2',
'aborted': 'NAV_TO',
'error': 'error'
},
remapping={"pos_xm": 'pose'})
StateMachine.add('RUNNODE_IMG2',
RunNode_img(),
transitions={
'succeeded': 'FIND_AGAIN',
'aborted': 'aborted'
})
StateMachine.add('FIND_AGAIN',
FindObject(),
transitions={
'succeeded': 'PICK',
'aborted': 'FIND_AGAIN',
'error': 'SPEAK'
},
remapping={
'name': 'name',
'object_pos': 'object_pos',
'objmode': 'objmode'
})
self.sm_Pick.userdata.arm_mode_1 = 1
StateMachine.add('PICK',
ArmCmd(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'arm_ps': 'object_pos',
'mode': 'arm_mode_1'
})
self.sm_Pick.userdata.sentences = 'xiao meng can not find things'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
})
self.sm_Place = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_Place:
# place_ps please specified due to the scene
self.sm_Place.userdata.place_ps = PointStamped()
self.sm_Place.userdata.place_ps.header.frame_id = 'base_link'
self.sm_Place.userdata.place_ps.point.x = 0.8
self.sm_Place.userdata.place_ps.point.y = 0.0
self.sm_Place.userdata.place_ps.point.z = 0.6
self.sm_Place.userdata.objmode = 2
# without moveit, if is just place it in a open space
self.sm_Place.userdata.arm_mode_1 = 2
StateMachine.add('PLACE',
ArmCmd(),
transitions={
'succeeded': 'succeeded',
'aborted': 'PLACE',
'error': 'error'
},
remapping={
'arm_ps': 'place_ps',
'mode': 'arm_mode_1'
})
self.sm_GPSR = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.sm_GPSR:
self.sm_GPSR.userdata.target = list()
self.sm_GPSR.userdata.action = list()
self.sm_GPSR.userdata.task_num = 0
self.sm_GPSR.userdata.current_task = -1
self.sm_GPSR.userdata.current_turn = 1
self.sm_GPSR.userdata.turn = 3
self.sm_GPSR.userdata.pos_xm_door = gpsr_target['speaker']['pos']
self.sm_GPSR.userdata.sentences = 'please command me'
StateMachine.add('SPEAK_RESTART',
Speak(),
transitions={
'succeeded': 'RECEIVE_TASKS',
'aborted': 'RECEIVE_TASKS',
'error': 'RECEIVE_TASKS'
})
# get the task due to the speech-node
StateMachine.add('RECEIVE_TASKS',
GetTask(),
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'RECEIVE_TASKS',
'error': 'error'
},
remapping={
'target': 'target',
'action': 'action',
'task_num': 'task_num'
})
#get the task in order and execute the task
# if want to go to the door at the end of the task, please modify:
# succeeded->succeeded --> succeeded->BYEBYE
StateMachine.add('GET_NEXT_TASK',
NextDo(),
transitions={
'succeeded': 'BACK_DOOR',
'aborted': 'aborted',
'error': 'error',
'find': 'FIND',
'go': 'GO',
'follow': 'FOLLOW',
'pick': 'PICK',
'place': 'PLACE',
'talk': 'TALK',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'action': 'action',
'current_task': 'current_task',
'task_num': 'task_num'
})
StateMachine.add('BACK_DOOR',
NavStack(),
transitions={
'succeeded': 'CHECK_TURN',
'aborted': 'BACK_DOOR',
'error': 'error'
},
remapping={'pos_xm': 'pos_xm_door'})
StateMachine.add('CHECK_TURN',
CheckTurn(),
transitions={
'succeeded': 'succeeded',
'continue': 'SPEAK_RESTART',
'error': 'error'
})
# all the task-group
StateMachine.add('GO',
self.sm_Nav,
remapping={
'target': 'target',
'current_task': 'current_task'
},
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'GO'
})
StateMachine.add('FIND',
self.sm_Find,
remapping={
'target': 'target',
'current_task': 'current_task'
},
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'FIND'
})
StateMachine.add('FOLLOW',
self.sm_Follow,
transitions={
'succeeded': 'CLOSE',
'aborted': 'FOLLOW'
})
StateMachine.add('CLOSE',
CloseKinect(),
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'GET_NEXT_TASK'
})
StateMachine.add('PICK',
self.sm_Pick,
remapping={
'target': 'target',
'current_task': 'current_task'
},
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'PICK'
})
StateMachine.add('TALK',
self.sm_Talk,
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'TALK'
})
StateMachine.add('PLACE',
self.sm_Place,
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'PLACE'
})
self.sm_GPSR.userdata.string = 'I finish all the three tasks, I will go out the stage'
StateMachine.add('BYEBYE',
Speak(),
transitions={
'succeeded': 'GOOUT',
'error': 'error'
},
remapping={'sentences': 'string'})
#no need
self.sm_GPSR.userdata.waypoint = gpsr_target['end']['pos']
StateMachine.add('GOOUT',
NavStack(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={"pos_xm": 'waypoint'})
intro_server = IntrospectionServer('enter_room', self.sm_EnterRoom,
'/SM_ROOT')
intro_server.start()
out_1 = self.sm_EnterRoom.execute()
intro_server.stop()
#只运行一次
intro_server = IntrospectionServer('sm_gpsr', self.sm_GPSR, '/SM_ROOT')
intro_server.start()
out_2 = self.sm_GPSR.execute()
intro_server.stop()
self.smach_bool = True
import rospy
from smach import StateMachine
import sys
import os
sys.path.append(os.path.join(os.path.dirname(__file__), "states"))
from wait import Wait
from explore import Explore
from go_back import GoBack
from report import Report
if __name__ == "__main__":
rospy.init_node("fsm", anonymous=True)
fsm = StateMachine(
outcomes=["done", "exit"]
)
with fsm:
StateMachine.add(
"WAIT",
Wait(),
transitions={
"commanded": "EXPLORE",
},
)
StateMachine.add(
"EXPLORE",
Explore(),
transitions={
"finished": "GOBACK",
},
def setup_statemachine(robot):
state_machine = StateMachine(outcomes=['done'])
state_machine.userdata['item_picked'] = None
with state_machine:
# Intro
# StateMachine.add('START_CHALLENGE_ROBUST',
# Initialize(robot),
# transitions={'initialized': 'SAY_START', 'abort': 'done'})
StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, "initial_pose"), # ToDo: in knowledge
transitions={'Done': 'SAY_START',
'Aborted': 'done',
'Failed': 'SAY_START'})
StateMachine.add('SAY_START',
Say(robot,
"Let's set the table baby! If there are any chairs near the kitchen_table, please remove them",
block=False),
transitions={'spoken': 'NAVIGATE_AND_OPEN_CUPBOARD'})
# The pre-work
StateMachine.add('NAVIGATE_AND_OPEN_CUPBOARD',
NavigateToAndOpenCupboard(robot, "kitchen_cabinet", "in_front_of"),
transitions={'succeeded': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER',
'failed': 'SAY_OPEN_FAILED'})
StateMachine.add('SAY_OPEN_FAILED',
Say(robot, "I failed to open the cupboard drawer"),
transitions={'spoken': 'WAIT_OPEN'})
StateMachine.add('WAIT_OPEN',
WaitTime(robot, 5),
transitions={'waited': 'SAY_OPEN_THANKS', 'preempted': 'done'})
StateMachine.add('SAY_OPEN_THANKS',
Say(robot, "Thank you darling"),
transitions={'spoken': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER'})
# The loop
StateMachine.add('NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER',
NavigateToAndPickItemFromCupboardDrawer(robot, "kitchen_cabinet", "in_front_of",
required_items),
transitions={'succeeded': 'PLACE_ITEM_ON_TABLE',
'failed': 'CHECK_IF_WE_HAVE_IT_ALL'})
StateMachine.add('PLACE_ITEM_ON_TABLE',
NavigateToAndPlaceItemOnTable(robot, "kitchen_table", "right_of", "right_of_close"),
transitions={'succeeded': 'CHECK_IF_WE_HAVE_IT_ALL',
'failed': 'WAIT'})
StateMachine.add('WAIT',
WaitTime(robot, 2),
transitions={'waited': 'CHECK_IF_WE_HAVE_IT_ALL', 'preempted': 'done'})
StateMachine.add('CHECK_IF_WE_HAVE_IT_ALL',
CBState(check_if_we_have_it_all, cb_args=[robot]),
transitions={'we_have_it_all': 'SAY_END_CHALLENGE',
'keep_going': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER'})
# Outro
StateMachine.add('SAY_END_CHALLENGE',
Say(robot, "That was all folks, have a nice meal!"),
transitions={'spoken': 'done'})
StateMachine.add('NAVIGATE_AND_CLOSE_CUPBOARD',
NavigateToAndCloseCupboard(robot, "cupboard", "in_front_of"),
transitions={'succeeded': 'done',
'failed': 'done'})
return state_machine
def __init__(self):
rospy.init_node('operating_mode', anonymous=False)
global mb
mb = MongoBridge()
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Create the top level SMACH state machine
self.sm_top = StateMachine(outcomes=['stop'])
# Open the container
with self.sm_top:
# ===== SETUP State =====
StateMachine.add('SETUP',
Setup(),
transitions={
'succeeded': 'TOP_CONTROL',
'preempted': 'stop',
'aborted': 'stop'
})
StateMachine.add('RECHARGE',
Recharge(),
transitions={
'succeeded': 'SETUP',
'preempted': 'stop',
'aborted': 'stop'
})
# ===== TOP_CONTROL State =====
self.sm_battery_concurrence = Concurrence(
outcomes=[
'restart', 'recharge', 'preempted', 'aborted', 'stop'
],
default_outcome='recharge',
child_termination_cb=self.child_termination_cb,
outcome_cb=self.outcome_cb)
self.sm_battery_concurrence.userdata.mode_change_infos = {
'mode': None,
'change_mode': False
}
# Open the container
with self.sm_battery_concurrence:
# Add states to the container
Concurrence.add(
'BATTERY_MONITOR',
MonitorState("/battery_level", Float32,
self.battery_monitor_cb))
Concurrence.add(
'CHANGE_MODE_MONITOR',
MonitorState("/change_mode",
String,
self.change_mode_monitor_cb,
input_keys=['mode_change_infos'],
output_keys=['mode_change_infos']))
self.operating_mode = StateMachine(
outcomes=['aborted', 'preempted', 'standby'],
input_keys=['mode_change_infos'])
with self.operating_mode:
StateMachine.add(
'MODE_MANAGEMENT',
OperatingMode(),
transitions={'succeeded': 'MODE_MANAGEMENT'})
Concurrence.add('OPERATING_MODE', self.operating_mode)
StateMachine.add('TOP_CONTROL',
self.sm_battery_concurrence,
transitions={
'restart': 'TOP_CONTROL',
'recharge': 'RECHARGE',
'aborted': 'stop',
'preempted': 'stop'
})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('mode', 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 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
class Main():
def __init__(self):
rospy.init_node('operating_mode', anonymous=False)
global mb
mb = MongoBridge()
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Create the top level SMACH state machine
self.sm_top = StateMachine(outcomes=['stop'])
# Open the container
with self.sm_top:
# ===== SETUP State =====
StateMachine.add('SETUP',
Setup(),
transitions={
'succeeded': 'TOP_CONTROL',
'preempted': 'stop',
'aborted': 'stop'
})
StateMachine.add('RECHARGE',
Recharge(),
transitions={
'succeeded': 'SETUP',
'preempted': 'stop',
'aborted': 'stop'
})
# ===== TOP_CONTROL State =====
self.sm_battery_concurrence = Concurrence(
outcomes=[
'restart', 'recharge', 'preempted', 'aborted', 'stop'
],
default_outcome='recharge',
child_termination_cb=self.child_termination_cb,
outcome_cb=self.outcome_cb)
self.sm_battery_concurrence.userdata.mode_change_infos = {
'mode': None,
'change_mode': False
}
# Open the container
with self.sm_battery_concurrence:
# Add states to the container
Concurrence.add(
'BATTERY_MONITOR',
MonitorState("/battery_level", Float32,
self.battery_monitor_cb))
Concurrence.add(
'CHANGE_MODE_MONITOR',
MonitorState("/change_mode",
String,
self.change_mode_monitor_cb,
input_keys=['mode_change_infos'],
output_keys=['mode_change_infos']))
self.operating_mode = StateMachine(
outcomes=['aborted', 'preempted', 'standby'],
input_keys=['mode_change_infos'])
with self.operating_mode:
StateMachine.add(
'MODE_MANAGEMENT',
OperatingMode(),
transitions={'succeeded': 'MODE_MANAGEMENT'})
Concurrence.add('OPERATING_MODE', self.operating_mode)
StateMachine.add('TOP_CONTROL',
self.sm_battery_concurrence,
transitions={
'restart': 'TOP_CONTROL',
'recharge': 'RECHARGE',
'aborted': 'stop',
'preempted': 'stop'
})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('mode', 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 child_termination_cb(self, outcome_map):
if outcome_map['BATTERY_MONITOR'] == 'invalid' or \
outcome_map['CHANGE_MODE_MONITOR'] == 'invalid' or \
outcome_map['OPERATING_MODE'] == 'aborted' or \
outcome_map['OPERATING_MODE'] == 'preempted':
return True
else:
return False
def outcome_cb(self, outcome_map):
if outcome_map['BATTERY_MONITOR'] == 'invalid':
return 'recharge'
elif outcome_map['CHANGE_MODE_MONITOR'] == 'invalid' or \
outcome_map['OPERATING_MODE'] == 'aborted' or \
outcome_map['OPERATING_MODE'] == 'preempted':
return 'restart'
else:
return 'stop'
def battery_monitor_cb(self, userdata, msg):
if msg.data < 20.0:
return False
else:
return True
def change_mode_monitor_cb(self, userdata, msg):
if msg.data:
data = json.loads(msg.data)
if not hasattr(self, '_last_msg_stamp'
) or self._last_msg_stamp != data['stamp']:
print('Entrou!')
mb.add_data({
'class_name': 'mode',
'item_name': 'mode',
'data': data
})
mb.send_data_and_wait()
mode_id = pym.mode.find_one({}, {"_id": 1},
sort=[("data.timestamp", order_des)
])["_id"]
if data.has_key(
'default_mode') and data['default_mode'] == True:
mb.add_data({
'class_name': 'mode',
'item_name': 'mode',
'params': {
'default_patrol': mode_id
}
})
mb.send_data_and_wait()
userdata.mode_change_infos['mode'] = data['mode']
userdata.mode_change_infos['mode_id'] = mode_id
userdata.mode_change_infos['change_mode'] = True
self._last_msg_stamp = data['stamp']
return False
else:
return True
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.sm_top.request_preempt()
rospy.sleep(1)
def __init__(self):
StateMachine.__init__(self,
outcomes=['succeeded', 'preempted', 'aborted'])
self.userdata.shopping_list = [
'bouillon cubes', 'clove', 'coffe', 'muesli', 'popcorn', 'tea'
]
self.executed_times = 0
self.execute_nTimes = 6
self.waitTime = 2.0
with self:
text = "I'm ready to do the shopping list."
StateMachine.add('SAY_DO_SHOPPING_LIST',
SpeechState(text=text, blocking=True),
transitions={'succeeded': 'MOVE_RIGHT'})
StateMachine.add('MOVE_RIGHT',
MoveToState(
Pose2D(0.0, -DISTANCE_TO_OBJECT_RECOGNITION,
0.0)),
transitions={'succeeded': 'LOOK_UP'})
StateMachine.add('LOOK_UP',
JointAngleState(['HeadPitch'], [-0.5]),
transitions={'succeeded': 'WAIT1'})
StateMachine.add('WAIT1',
TimeOutState(self.waitTime),
transitions={'succeeded': 'CONTROL_CHECK1'})
def check_obj(ud):
if self.executed_times < self.execute_nTimes:
self.executed_times += 1
print self.executed_times
return 'succeeded'
else:
self.executed_times = 0
return 'ended'
StateMachine.add('CONTROL_CHECK1',
CBState(check_obj,
outcomes=['succeeded', 'ended'],
input_keys=['in_list', 'in_detected'],
output_keys=['out_list']),
transitions={
'succeeded': 'CHECK_OBJECTS1',
'ended': 'SAY_CROUCH'
},
remapping={
'in_list': 'shopping_list',
'out_list': 'shopping_list'
})
StateMachine.add('CHECK_OBJECTS1',
ShoppingListState(),
transitions={
'succeeded': 'CONTROL_CHECK1',
'aborted': 'CHECK_OBJECTS1'
})
StateMachine.add(
'SAY_CROUCH',
SpeechState('I will bend down to look the lower cupboard.',
blocking=False),
transitions={'succeeded': 'CROUCH_POSE'})
StateMachine.add('CROUCH_POSE',
GoToPostureState('Crouch', 0.5),
transitions={'succeeded': 'LOOK_DOWN_LEFT'})
StateMachine.add('LOOK_DOWN_LEFT',
JointAngleState(['HeadPitch', 'HeadYaw'],
[0.5, 0.5]),
transitions={'succeeded': 'WAIT2'})
StateMachine.add('WAIT2',
TimeOutState(self.waitTime),
transitions={'succeeded': 'CONTROL_CHECK2'})
StateMachine.add('CONTROL_CHECK2',
CBState(check_obj,
outcomes=['succeeded', 'ended'],
input_keys=['in_list', 'in_detected'],
output_keys=['out_list']),
transitions={
'succeeded': 'CHECK_OBJECTS2',
'ended': 'LOOK_CENTER'
},
remapping={
'in_list': 'shopping_list',
'out_list': 'shopping_list'
})
StateMachine.add('CHECK_OBJECTS2',
ShoppingListState(),
transitions={
'succeeded': 'CONTROL_CHECK2',
'aborted': 'CHECK_OBJECTS2'
})
StateMachine.add('LOOK_CENTER',
JointAngleState(['HeadPitch', 'HeadYaw'],
[0.0, 0.0]),
transitions={'succeeded': 'SAY_GOING_LEFT'})
StateMachine.add('SAY_GOING_LEFT',
SpeechState(
'I will go to look to the upper cupboard.',
blocking=False),
transitions={'succeeded': 'MOVE_LEFT'})
StateMachine.add('MOVE_LEFT',
MoveToState(
Pose2D(0.0,
2 * DISTANCE_TO_OBJECT_RECOGNITION,
0.0)),
transitions={'succeeded': 'LOOK_UP2'})
StateMachine.add('LOOK_UP2',
JointAngleState(['HeadPitch'], [-0.5]),
transitions={'succeeded': 'WAIT3'})
StateMachine.add('WAIT3',
TimeOutState(self.waitTime),
transitions={'succeeded': 'CONTROL_CHECK3'})
StateMachine.add('CONTROL_CHECK3',
CBState(check_obj,
outcomes=['succeeded', 'ended'],
input_keys=['in_list', 'in_detected'],
output_keys=['out_list']),
transitions={
'succeeded': 'CHECK_OBJECTS3',
'ended': 'LOOK_FRONT'
},
remapping={
'in_list': 'shopping_list',
'out_list': 'shopping_list'
})
StateMachine.add('CHECK_OBJECTS3',
ShoppingListState(),
transitions={
'succeeded': 'CONTROL_CHECK3',
'aborted': 'CHECK_OBJECTS3'
})
StateMachine.add('LOOK_FRONT',
JointAngleState(['HeadPitch', 'HeadYaw'],
[0.0, 0.0]),
transitions={'succeeded': 'PREPARE_TEXT'})
def prep_text(ud):
text_pre = "I've to buy "
if len(self.userdata.shopping_list) == 1:
ud.out_text = text_pre + str(self.userdata.shopping_list)
elif len(self.userdata.shopping_list) == 0:
ud.out_text = "I don't have to buy anything"
else:
ud.out_text = text_pre + str(
self.userdata.
shopping_list[:len(self.userdata.shopping_list) - 1]
) + ' and ' + self.userdata.shopping_list[-1]
return 'succeeded'
StateMachine.add(
'PREPARE_TEXT',
CBState(prep_text,
outcomes=['succeeded'],
input_keys=['shopping_list'],
output_keys=['out_text']),
transitions={'succeeded': 'SAY_SHOPPING_LIST_DONE1'},
remapping={'out_text': 'text'})
StateMachine.add('SAY_SHOPPING_LIST_DONE1',
SpeechState(text=None, blocking=True),
transitions={'succeeded': 'WAIT4'})
StateMachine.add(
'WAIT4',
TimeOutState(1.0),
transitions={'succeeded': 'SAY_SHOPPING_LIST_DONE2'})
StateMachine.add('SAY_SHOPPING_LIST_DONE2',
SpeechState(text=None, blocking=True),
transitions={'succeeded': 'WAIT5'})
StateMachine.add('WAIT5',
TimeOutState(1.0),
transitions={'succeeded': 'SAY_FINISH'})
text = 'I am done. Sending the shopping list to AMAZON.'
StateMachine.add('SAY_FINISH',
SpeechState(text=text, blocking=False),
transitions={'succeeded': 'succeeded'})
def transition_to(name, target_state):
StateMachine.add(name, states.NoOp(), transitions={"succeeded": target_state})
#if rospy.Time.now() > state_change_time:
while rospy.Time.now() < state_change_time:
twist = Twist()
twist.angular.z = 1
twist.linear.x = 0
cmd_vel_pub.publish(twist)
state_change_time = rospy.Time.now() + rospy.Duration(30)
return 'success'
if __name__ == '__main__':
g_range_ahead = 1 # anything to start
scan_sub = rospy.Subscriber('scan', LaserScan, scan_callback)
cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
rospy.init_node('wanderSM')
state_change_time = rospy.Time.now()
#driving_forward = True
rate = rospy.Rate(10)
sm = StateMachine(outcomes=['success'])
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
with sm:
StateMachine.add('Forward', Forward(), transitions={'success':'Spin'})
StateMachine.add('Spin', Spin(), transitions={'success':'Forward'})
outcome = sm.execute()
def location3(
cam_pixel_to_point): # type: (CamPixelToPointServer) -> StateMachine
sm = StateMachine(outcomes=['ok'], input_keys=['green_shape'])
with sm:
StateMachine.add('MOVETO1',
move_to_stop_line(lower=True),
transitions={'ok': 'TURN1_1'})
StateMachine.add('TURN1_1',
RotateState(np.pi / 2),
transitions={'ok': 'LOCATION3_1'})
StateMachine.add('LOCATION3_1',
Location3State(cam_pixel_to_point),
transitions={
'ok': 'TURN1_2',
'err': 'TURN1_2',
'match': 'EXIT_TURN1'
})
StateMachine.add('TURN1_2',
RotateState(-np.pi / 2),
transitions={'ok': 'MOVETO2'})
StateMachine.add('MOVETO2',
move_to_stop_line(lower=True),
transitions={'ok': 'TURN2_1'})
StateMachine.add('TURN2_1',
RotateState(np.pi / 2),
transitions={'ok': 'LOCATION3_2'})
StateMachine.add('LOCATION3_2',
Location3State(cam_pixel_to_point),
transitions={
'ok': 'TURN2_2',
'err': 'TURN2_2',
'match': 'EXIT_TURN2'
})
StateMachine.add('TURN2_2',
RotateState(-np.pi / 2),
transitions={'ok': 'MOVETO3'})
StateMachine.add('MOVETO3',
move_to_stop_line(lower=True),
transitions={'ok': 'TURN3_1'})
StateMachine.add('TURN3_1',
RotateState(np.pi / 2),
transitions={'ok': 'LOCATION3_3'})
StateMachine.add('LOCATION3_3',
Location3State(cam_pixel_to_point),
transitions={
'ok': 'TURN3_2',
'err': 'TURN3_2',
'match': 'EXIT_TURN3'
})
StateMachine.add('TURN3_2', RotateState(-np.pi / 2))
StateMachine.add('EXIT_TURN1',
RotateState(-np.pi / 2),
transitions={'ok': 'EXIT_MOVETO2'})
StateMachine.add('EXIT_TURN2',
RotateState(-np.pi / 2),
transitions={'ok': 'EXIT_MOVETO3'})
StateMachine.add('EXIT_TURN3', RotateState(-np.pi / 2))
StateMachine.add('EXIT_MOVETO2',
move_to_stop_line(lower=True),
transitions={'ok': 'EXIT_MOVETO3'})
StateMachine.add('EXIT_MOVETO3', move_to_stop_line(lower=True))
return sm
if (mc.detach_objects(self.object_name) == 'succeeded'):
if (hc.release()): return 'succeeded'
else: return 'aborted'
else: return 'aborted'
#==================================
#==================================
if __name__ == '__main__':
rospy.init_node('scenario_node')
na = NaviAction()
hc = HandController()
mc = MoveitCommand()
go_to_shelf = StateMachine(outcomes=['succeeded', 'aborted'])
with go_to_shelf:
StateMachine.add('DOWN LIFTER', MOVE_LIFTER(0,0.6),\
transitions={'succeeded':'MOVE','aborted':'aborted'})
StateMachine.add('MOVE', GO_TO_PLACE(1),\
transitions={'succeeded':'succeeded','aborted':'aborted'})
go_to_start_point = StateMachine(outcomes=['succeeded', 'aborted'])
with go_to_start_point:
StateMachine.add('DOWN LIFTER', MOVE_LIFTER(0,0.6),\
transitions={'succeeded':'MOVE','aborted':'aborted'})
StateMachine.add('MOVE', GO_TO_PLACE(0),\
transitions={'succeeded':'succeeded','aborted':'aborted'})
pick_place_1 = StateMachine(outcomes=['succeeded', 'aborted'])
x = mc.box1.pose.position.x
def __init__(self, get_one=True, ask_for_repetition=True, timeout=30):
''' If get_one is True, recognition_result is just the topic information. If it's false is a string with the most confident word.
If ask_for_repetition is True, the robot asks the user to repeat the response (for now it does it forever)
'''
StateMachine.__init__(self,
outcomes=['succeeded', 'aborted', 'preempted'],
output_keys=['recognition_result'])
self.userdata.recognition_result = None
with self:
StateMachine.add('START_LISTEN',
StartRecognitionState(),
transitions={'succeeded': 'ANSWER_DETECTION'})
StateMachine.add(
'ANSWER_DETECTION',
GetRecognizedWordNoEmptyList(timeout=timeout),
transitions={
'succeeded': 'STOP_LISTEN',
'timeouted': 'STOP_LISTEN_ABORTED',
'preempted': 'preempted'
},
remapping={'recognized_words': 'recognition_result'})
StateMachine.add(
'STOP_LISTEN',
StopRecognitionState(),
transitions={
'succeeded':
'GET_SINGLE_RESPONSE' if get_one else 'succeeded'
})
def get_most_confident_word(ud):
sorted_result = sorted(zip(
ud.in_recognition_result.confidence_values,
ud.in_recognition_result.words),
reverse=True)
ud.out_recognition_result = sorted_result[0][
1] # Get the most confident [0] word [1]
rospy.loginfo('---Get user answer recognized word: ' +
sorted_result[0][1])
return 'succeeded'
StateMachine.add('GET_SINGLE_RESPONSE',
CBState(get_most_confident_word,
input_keys=['in_recognition_result'],
output_keys=['out_recognition_result'],
outcomes=['succeeded']),
remapping={
'in_recognition_result': 'recognition_result',
'out_recognition_result': 'recognition_result'
},
transitions={'succeeded': 'succeeded'})
StateMachine.add(
'STOP_LISTEN_ABORTED',
StopRecognitionState(),
transitions={
'succeeded':
'aborted' if not ask_for_repetition else 'ASK_TO_REPEAT'
})
repeat_pool = [
'I could not hear you. Repeat it please.',
'Did you say anything? Please repeat it.',
'I did not understand anything. Answer my question.',
'Please, answer my question.'
]
StateMachine.add('ASK_TO_REPEAT',
SpeechFromPoolSM(pool=repeat_pool, blocking=True),
transitions={'succeeded': 'START_LISTEN'})
def __init__(self):
rospy.init_node('smach_home_status', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
#####################################
# JO IS AWAKE
#####################################
# State machine for Jo-awake-go-sleep
self.sm_jo_awake_sleep = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo_awake_sleep:
StateMachine.add('LOOK_WAKE',
MonitorState("/JO/sleep", Empty, self.empty_cb),
transitions={
'valid': 'GOING_SLEEP',
'preempted': 'preempted',
'invalid': 'GOING_SLEEP'
})
StateMachine.add('GOING_SLEEP',
JoGoingSleep(),
transitions={'succeeded': 'LOOK_IN_BED'})
StateMachine.add('LOOK_IN_BED',
MonitorState("/myo_disconnected", Empty,
self.empty_cb),
transitions={
'valid': 'IN_BED',
'preempted': 'preempted',
'invalid': 'IN_BED'
})
StateMachine.add('IN_BED',
JoInBed(),
transitions={'succeeded': 'succeeded'})
# State machine for Jo-awake-bothgo-sleep
self.sm_jo_awake_bothsleep = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo_awake_bothsleep:
StateMachine.add('LOOK_WAKE',
MonitorState("/BOTH/sleep", Empty, self.empty_cb),
transitions={
'valid': 'GOING_SLEEP',
'preempted': 'preempted',
'invalid': 'GOING_SLEEP'
})
StateMachine.add('GOING_SLEEP',
BothGoingSleep(),
transitions={'succeeded': 'LOOK_IN_BED'})
StateMachine.add('LOOK_IN_BED',
MonitorState("/myo_disconnected", Empty,
self.empty_cb),
transitions={
'valid': 'IN_BED',
'preempted': 'preempted',
'invalid': 'IN_BED'
})
StateMachine.add('IN_BED',
BothInBed(),
transitions={'succeeded': 'succeeded'})
# State machine for Jo-awake-go-out
self.sm_jo_awake_out = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo_awake_out:
StateMachine.add('LOOK_OUT',
MonitorState("/JO/go_out", Empty, self.empty_cb),
transitions={
'valid': 'PAUSE',
'preempted': 'preempted',
'invalid': 'PAUSE'
})
StateMachine.add('PAUSE',
Pause(),
transitions={'succeeded': 'succeeded'})
# State machine for Jo-awake
self.sm_jo_awake = Concurrence(
outcomes=['succeeded', 'stop', 'go_sleep', 'go_out'],
default_outcome='succeeded',
child_termination_cb=self.jo_awake_child_termination_cb,
outcome_cb=self.jo_awake_outcome_cb)
with self.sm_jo_awake:
Concurrence.add('SM_GO_TO_SLEEP', self.sm_jo_awake_sleep)
Concurrence.add('SM_BOTH_GO_TO_SLEEP', self.sm_jo_awake_bothsleep)
Concurrence.add('SM_GO_OUT', self.sm_jo_awake_out)
#####################################
# JO IS SLEEPING
#####################################
# State machine for Jo-sleep-waking
self.sm_jo_sleep_waking = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo_sleep_waking:
StateMachine.add('WAIT_WAKE_UP',
MonitorState("/JO/wake_up", Empty, self.empty_cb),
transitions={
'valid': 'WAKING_UP',
'preempted': 'preempted',
'invalid': 'WAKING_UP'
})
StateMachine.add('WAKING_UP',
JoWakingUp(),
transitions={'succeeded': 'succeeded'})
# State machine for Jo-sleep-bothwaking
self.sm_jo_sleep_bothwaking = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo_sleep_bothwaking:
StateMachine.add('WAIT_WAKE_UP',
MonitorState("/BOTH/wake_up", Empty,
self.empty_cb),
transitions={
'valid': 'WAKING_UP',
'preempted': 'preempted',
'invalid': 'WAKING_UP'
})
StateMachine.add('WAKING_UP',
BothWakingUp(),
transitions={'succeeded': 'succeeded'})
# State machine for Jo-awake
self.sm_jo_sleep = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted', 'wake_up'],
default_outcome='succeeded',
child_termination_cb=self.jo_sleep_child_termination_cb,
outcome_cb=self.jo_sleep_outcome_cb)
with self.sm_jo_sleep:
Concurrence.add('SM_WAKE_UP', self.sm_jo_sleep_waking)
Concurrence.add('SM_BOTH_WAKE_UP', self.sm_jo_sleep_bothwaking)
#####################################
# JO IS OUT TODO
#####################################
# State machine for Jo-out-back
self.sm_jo_out_back = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo_out_back:
StateMachine.add('WAIT_BACK_HOME',
MonitorState("/JO/back_home", Empty,
self.empty_cb),
transitions={
'valid': 'WAIT_MYO',
'preempted': 'preempted',
'invalid': 'WAIT_MYO'
})
StateMachine.add('WAIT_MYO',
MonitorState("/myo_connected", Empty,
self.empty_cb),
transitions={
'valid': 'COMING_BACK',
'preempted': 'preempted',
'invalid': 'COMING_BACK'
})
StateMachine.add('COMING_BACK',
JoBackHome(),
transitions={'succeeded': 'succeeded'})
# State machine for Jo-out-bothback
self.sm_jo_out_bothback = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo_out_bothback:
StateMachine.add('WAIT_BACK_HOME',
MonitorState("/BOTH/back_home", Empty,
self.empty_cb),
transitions={
'valid': 'WAIT_MYO',
'preempted': 'preempted',
'invalid': 'WAIT_MYO'
})
StateMachine.add('WAIT_MYO',
MonitorState("/myo_connected", Empty,
self.empty_cb),
transitions={
'valid': 'COMING_BACK',
'preempted': 'preempted',
'invalid': 'COMING_BACK'
})
StateMachine.add('COMING_BACK',
BothBackHome(),
transitions={'succeeded': 'succeeded'})
# State machine for Jo-out
self.sm_jo_out = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted', 'back_home'],
default_outcome='succeeded',
child_termination_cb=self.jo_out_child_termination_cb,
outcome_cb=self.jo_out_outcome_cb)
with self.sm_jo_out:
Concurrence.add('SM_BACK_HOME', self.sm_jo_out_back)
Concurrence.add('SM_BOTH_BACK_HOME', self.sm_jo_out_bothback)
#####################################
# TOP LVL JO SM
#####################################
# State machine for JO
self.sm_jo = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo:
StateMachine.add('AWAKE',
self.sm_jo_awake,
transitions={
'succeeded': 'succeeded',
'stop': 'aborted',
'go_sleep': 'SLEEP',
'go_out': 'OUT'
})
StateMachine.add('SLEEP',
self.sm_jo_sleep,
transitions={
'succeeded': 'succeeded',
'wake_up': 'AWAKE'
})
StateMachine.add('OUT',
self.sm_jo_out,
transitions={
'succeeded': 'succeeded',
'back_home': 'AWAKE'
})
#####################################
# TOP LVL CAROLE SM TODO
#####################################
# State machine for CAROLE
self.sm_carole = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_carole:
StateMachine.add('WAIT3',
MonitorState("/TEST/wait3", Empty, self.empty_cb),
transitions={
'valid': 'PAUSE',
'preempted': 'preempted',
'invalid': 'PAUSE'
})
StateMachine.add('PAUSE',
Pause(),
transitions={
'succeeded': 'WAIT3',
'aborted': 'aborted'
})
#####################################
# TOP LVL EAT SM TODO
#####################################
# State machine for EAT
self.sm_eat = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_eat:
StateMachine.add('WAIT2',
MonitorState("/TEST/wait2", Empty, self.empty_cb),
transitions={
'valid': 'PAUSE',
'preempted': 'preempted',
'invalid': 'PAUSE'
})
StateMachine.add('PAUSE',
Pause(),
transitions={
'succeeded': 'WAIT2',
'aborted': 'aborted'
})
#####################################
# TOP LVL SHOWER SM
#####################################
# State machine for SHOWER
self.sm_shower = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_shower:
StateMachine.add('WAIT_SHOWER',
MonitorState("/HOME/go_shower", Empty,
self.empty_cb),
transitions={
'valid': 'PREPARING_SHOWER',
'preempted': 'preempted',
'invalid': 'PREPARING_SHOWER'
})
StateMachine.add('PREPARING_SHOWER',
PreparingShower(),
transitions={
'succeeded': 'GO_SHOWER',
'aborted': 'WAIT1'
})
StateMachine.add('GO_SHOWER',
GoShower(),
transitions={
'succeeded': 'STOP_SHOWER',
'aborted': 'aborted'
})
StateMachine.add('STOP_SHOWER',
StopShower(),
transitions={
'succeeded': 'WAIT1',
'aborted': 'aborted'
})
#####################################
# TOP LVL SM
#####################################
# Create the top level state machine
self.sm_top = Concurrence(
outcomes=['succeeded', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
Concurrence.add('SM_JO', self.sm_jo)
Concurrence.add('SM_CAROLE', self.sm_carole)
Concurrence.add('SM_EAT', self.sm_eat)
Concurrence.add('SM_SHOWER', self.sm_shower)
# 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()
polygon = StateMachine(outcomes=['success'])
with polygon:
# Add all but the final side
for i in xrange(sides - 1):
StateMachine.add('SIDE_{0}'.format(i + 1),
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
if __name__ == '__main__':
triangle = polygon(3)
square = polygon(4)
shapes = StateMachine(outcomes=['success'])
with shapes:
StateMachine.add('TRIANGLE', triangle, transitions={'success':'SQUARE'})
StateMachine.add('SQUARE', square, transitions={'success':'success'})
shapes.execute()
