def __init__(self): smach.StateMachine.__init__(self, [succeeded, preempted, aborted]) with self: # move arm filename = MOTION_FOLDER_PATH + "hang.xml" hang_motion_goal = MotionManagerGoal() hang_motion_goal.plan = False hang_motion_goal.filename = filename hang_motion_goal.checkSafety = False hang_motion_goal.repeat = False hang_motion_action = SimpleActionState('/motion_manager', MotionManagerAction, goal=hang_motion_goal) smach.StateMachine.add('MOVE_TO_HANG_POSE', hang_motion_action, transitions={succeeded: 'RELEASE_OBJECT_FROM_HAND1', preempted: preempted, aborted: aborted}) # release smach.StateMachine.add( 'RELEASE_OBJECT_FROM_HAND1', OpenReemHand(), transitions={succeeded: 'MOVE_TO_FINISH_POSE', preempted: preempted, aborted: aborted}) smach.StateMachine.add( 'RELEASE_OBJECT_FROM_HAND2', OpenReemHand2(), transitions={succeeded: 'MOVE_TO_FINISH_POSE', preempted: preempted, aborted: aborted}) #move arm home filename = MOTION_FOLDER_PATH + "final.xml" finish_motion_goal = MotionManagerGoal() finish_motion_goal.plan = False finish_motion_goal.filename = filename finish_motion_goal.checkSafety = False finish_motion_goal.repeat = False finish_motion_action = SimpleActionState('/motion_manager', MotionManagerAction, goal=finish_motion_goal) smach.StateMachine.add('MOVE_TO_FINISH_POSE', finish_motion_action, transitions={succeeded: 'RELEASE_OBJECT_FROM_HAND3', preempted: preempted, aborted: aborted}) # close gripper #smach.StateMachine.add( # 'CLOSE_HAND', # CloseReemHand(), # transitions={succeeded: 'MOVE_TO_FINISH_POSE', preempted: preempted, aborted: aborted}) smach.StateMachine.add( 'RELEASE_OBJECT_FROM_HAND3', OpenReemHand2(), transitions={succeeded: succeeded, preempted: preempted, aborted: aborted})
def __init__(self): smach.StateMachine.__init__(self, [succeeded, preempted, aborted]) with self: # move arm to point position filename = MOTION_FOLDER_PATH + "point_at.xml" point_at_motion_goal = MotionManagerGoal() point_at_motion_goal.plan = False point_at_motion_goal.filename = filename point_at_motion_goal.checkSafety = False point_at_motion_goal.repeat = False point_at_motion_action = SimpleActionState( "/motion_manager", MotionManagerAction, goal=point_at_motion_goal ) smach.StateMachine.add( "POINT", point_at_motion_action, transitions={succeeded: "OPEN_HAND", preempted: preempted, aborted: aborted}, ) # open smach.StateMachine.add( "OPEN_HAND", OpenReemHand2(), transitions={succeeded: "TELL_HERE", preempted: preempted, aborted: aborted}, ) # Tell pointed smach.StateMachine.add( "TELL_HERE", SpeakActionState(text="Here it is"), transitions={succeeded: "MOVE_TO_FINISH_POSE", preempted: preempted, aborted: aborted}, ) # move arm home filename = MOTION_FOLDER_PATH + "final_pose_point_at.xml" finish_motion_goal = MotionManagerGoal() finish_motion_goal.plan = False finish_motion_goal.filename = filename finish_motion_goal.checkSafety = False finish_motion_goal.repeat = False finish_motion_action = SimpleActionState("/motion_manager", MotionManagerAction, goal=finish_motion_goal) smach.StateMachine.add( "MOVE_TO_FINISH_POSE", finish_motion_action, transitions={succeeded: succeeded, preempted: preempted, aborted: aborted}, )
def __init__(self, filename, text, checksafety=True, plan=True, wait_before_speaking=0): smach.Concurrence.__init__(self, outcomes=['succeeded', 'aborted'], default_outcome='aborted', outcome_map={'succeeded': {'MOTION_ACTION': 'succeeded', 'SPEAK_ACTION': 'succeeded'}}) with self: motion_goal = MotionManagerGoal() motion_goal.plan = plan motion_goal.filename = filename motion_goal.checkSafety = checksafety motion_goal.repeat = False smach.Concurrence.add('MOTION_ACTION', SimpleActionState('/motion_manager', MotionManagerAction, goal=motion_goal)) smach.Concurrence.add('SPEAK_ACTION', SpeakActionState(text=text, wait_before_speaking=wait_before_speaking))
def __init__(self): smach.StateMachine.__init__(self, [succeeded, preempted, aborted]) with self: # move arms to cool pose filename = MOTION_FOLDER_PATH + "cool_cloth_pose.xml" cool_motion_goal = MotionManagerGoal() cool_motion_goal.plan = False cool_motion_goal.filename = filename cool_motion_goal.checkSafety = False cool_motion_goal.repeat = False cool_motion_action = SimpleActionState('/motion_manager', MotionManagerAction, goal=cool_motion_goal) smach.StateMachine.add('MOVE_TO_COOL_POSE', cool_motion_action, transitions={succeeded: succeeded, preempted: preempted, aborted: aborted})
def arms_out_of_self_colision(self): self._print_title("ARMS OUT OF SELF COLISION") if self.using_the_robot: if self.__check_action(ARMS_ACTION_NAME) == aborted: return aborted else: self._print_warning("Not checking. ROS_MASTER_URI nor COMPUTER_NAME contains %s" % ROBOTS_NAME) return aborted robot = os.environ.get('PAL_ROBOT') ros_master_uri = os.environ.get('ROS_MASTER_URI') remotelly_executing = ros_master_uri.rfind('localhost') rospy.loginfo('remotelly_executing: %s' % remotelly_executing) # FIXME: Remove this line after test in the robot MOTION_FOLDER_PATH = '' plan = False checkSafety = False if robot == 'rh2c' or robot == 'rh2m' or robot == 'reemh3c' or robot == 'reemh3m' or robot == 'reemh3' or remotelly_executing == -1: check_for_door = True # FIXME: This variable is not being used. MOTION_FOLDER_PATH = "/mnt_flash/stacks/reem_alive/alive_engine/config/database/Stopped/interact_1.xml" if remotelly_executing == -1: MOTION_FOLDER_PATH = packages.get_pkg_dir('pal_smach_utils') + '/config/interact_1.xml' plan = True checkSafety = False motion_goal = MotionManagerGoal() motion_goal.plan = plan motion_goal.filename = MOTION_FOLDER_PATH motion_goal.checkSafety = checkSafety motion_goal.repeat = False motion_manager_action = SimpleActionState(ARMS_ACTION_NAME, MotionManagerAction, goal=motion_goal) userdata_hacked = {} status = motion_manager_action.execute(userdata_hacked) if status == succeeded: rospy.loginfo(self.colors.GREEN_BOLD + "Arms out of self colisin: OK " + self.colors.NATIVE_COLOR) else: self.ALL_OK = False rospy.loginfo(self.colors.RED_BOLD + "Arms out of self colisin: Failed " + self.colors.NATIVE_COLOR)
def main(): rospy.init_node("sm_pick_up_cloths") # Create a SMACH state machine sm = smach.StateMachine(outcomes=["FINISH_OK", "FAILED_VISION", "FAILED_MOVE_ARM", "FAILED_MOVE_BASE"]) # Open the container with sm: # Add states to the container # smach.StateMachine.add('GetPCL', GetPCL('/camera/rgb/points'), # TODO SALUDO # TODO RETRYING smach.StateMachine.add( "GetPCL", GetPCL("/head_mount_xtion/depth_registered/points"), transitions={"success": "GetGraspingPointAction", "fail": "FAILED_VISION"}, remapping={"pcl_RGB": "pcl_RGB"}, ) smach.StateMachine.add( "GetGraspingPointAction", SimpleActionState( "/skills/bow_detector/bow_object_detector/get_grasping_point", GetGraspingPointAction, # goal_slots=['pcl_RGB'], goal_cb=grasping_point_goal_cb, input_keys=["pcl_RGB"], result_slots=["grasping_point"], ), # server_wait_timeout=rospy.Duration(10.0)), transitions={"succeeded": "ConvertGraspPoint", "preempted": "FAILED_VISION", "aborted": "FAILED_VISION"}, remapping={"grasping_point": "sm_grasp_point"}, ) # prepare pose smach.StateMachine.add( "ConvertGraspPoint", CBState(build_grasp_pose_from_point_cb), transitions={"done": "TransformPoseStamped"}, remapping={ "grasp_point": "sm_grasp_point", "grasp_pose_st": "sm_grasp_pose", "target_frame": "sm_target_frame", }, ) # transform pose ¿needed? smach.StateMachine.add( "TransformPoseStamped", TransformPoseStamped(), transitions={ "succeeded": "ReemGraspStateMachine", "preempted": "FAILED_VISION", "aborted": "FAILED_VISION", }, remapping={"target_pose": "target_pose_stamped"}, ) # grasp smach.StateMachine.add( "ReemGraspStateMachine", SMReemClothGraspStateMachine(), transitions={"succeeded": "MOVE_TO_COOL_POSE", "preempted": "FAILED_MOVE_ARM", "aborted": "GetPCL"}, ) # move arms to cool pose filename = MOTION_FOLDER_PATH + "cool_cloth_pose.xml" cool_motion_goal = MotionManagerGoal() cool_motion_goal.plan = True cool_motion_goal.filename = filename cool_motion_goal.checkSafety = True cool_motion_goal.repeat = False cool_motion_action = SimpleActionState("/motion_manager", MotionManagerAction, goal=cool_motion_goal) smach.StateMachine.add( "MOVE_TO_COOL_POSE", cool_motion_action, transitions={"succeeded": "MOVE_TO_HANGER", "preempted": "FAILED_MOVE_ARM", "aborted": "FAILED_MOVE_ARM"}, ) # move to hanger sm.userdata.hanger = HANGER_NAME smach.StateMachine.add( "MOVE_TO_HANGER", MoveToRoomStateMachine(announcement=None), transitions={ "succeeded": "MOVE_TO_HANG_POSE", "preempted": "FAILED_MOVE_BASE", "aborted": "FAILED_MOVE_BASE", }, remapping={"room_name": "hanger"}, ) # move arm filename = MOTION_FOLDER_PATH + "hang.xml" hang_motion_goal = MotionManagerGoal() hang_motion_goal.plan = True hang_motion_goal.filename = filename hang_motion_goal.checkSafety = True hang_motion_goal.repeat = False hang_motion_action = SimpleActionState("/motion_manager", MotionManagerAction, goal=hang_motion_goal) smach.StateMachine.add( "MOVE_TO_HANG_POSE", hang_motion_action, transitions={ "succeeded": "RELEASE_OBJECT_FROM_HAND", "preempted": "FAILED_MOVE_ARM", "aborted": "FAILED_MOVE_ARM", }, ) # release smach.StateMachine.add( "RELEASE_OBJECT_FROM_HAND", OpenReemHand(), transitions={"succeeded": "CLOSE_HAND", "preempted": "FAILED_MOVE_ARM", "aborted": "FAILED_MOVE_ARM"}, ) # close gripper smach.StateMachine.add( "CLOSE_HAND", CloseReemHand(), transitions={ "succeeded": "MOVE_TO_FINISH_POSE", "preempted": "FAILED_MOVE_ARM", "aborted": "FAILED_MOVE_ARM", }, ) # move arm home filename = MOTION_FOLDER_PATH + "final.xml" finish_motion_goal = MotionManagerGoal() finish_motion_goal.plan = True finish_motion_goal.filename = filename finish_motion_goal.checkSafety = True finish_motion_goal.repeat = False finish_motion_action = SimpleActionState("/motion_manager", MotionManagerAction, goal=finish_motion_goal) smach.StateMachine.add( "MOVE_TO_FINISH_POSE", finish_motion_action, transitions={"succeeded": "FINISH_OK", "preempted": "FAILED_MOVE_ARM", "aborted": "FAILED_MOVE_ARM"}, ) # finish # Execute SMACH plan outcome = sm.execute()
import rospy import actionlib from std_srvs.srv import Empty, EmptyResponse from pal_control_msgs.msg import MotionManagerGoal, MotionManagerAction def hand_initialize(name): rospy.wait_for_service(name) try: hand_init = rospy.ServiceProxy(name, Empty) resp1 = hand_init() return resp1 except rospy.ServiceException, e: print "Service call failed: %s"%e if __name__ == "__main__": rospy.init_node('calibrator') client = actionlib.SimpleActionClient('/motion_manager', MotionManagerAction) client.wait_for_server() goal = MotionManagerGoal() goal.filename = '/mnt_flash/etc/control/robot/reemh3/motion/start_reem.xml' goal.checkSafety = True goal.repeat = False goal.priority = 0 goal.queueTimeout = 0 client.send_goal(goal) client.wait_for_result() hand_initialize ('/rightHandDeviceInitializerStart') hand_initialize ('/leftHandDeviceInitializerStart')
def __init__(self, distance=1.5, orient_after_passing=math.pi/2, openDoor=False): """ Look for a door, open it (if needed) and enter the room. :param distance: The distance in meters to go into the room. :param orientation: 0 to look to the front, 1 to rotate towards the door :return output_keys: the position to the door (in abs. coordinates) """ smach.StateMachine.__init__(self, [succeeded, preempted, aborted]) self.door_position = -1 with self: '''if openDoor: smach.StateMachine.add('SM_OPEN_DOOR', EnterDoorStateMachine(), transitions={succeeded: succeeded, aborted: 'SAY_CANT_OPEN'}) smach.StateMachine.add('SAY_CANT_OPEN', SpeakActionState(text="I can't open the door. Can you please open it for me?"), transitions={succeeded: 'CROSSING_DOOR_POS', aborted: 'CROSSING_DOOR_POS'})''' if check_for_door: plan = True checkSafety = False rest_goal = MotionManagerGoal() rest_goal.plan = plan rest_goal.filename = MOTION_FOLDER_PATH + 'rest.xml' rest_goal.checkSafety = checkSafety rest_goal.repeat = False smach.StateMachine.add( "STOP_ROBOT_CONTROLLERS", StopRobotControllers(head=True, left=True, right=True), transitions={succeeded: "DISABLE_REEM_ALIVE", preempted: "DISABLE_REEM_ALIVE", aborted: "DISABLE_REEM_ALIVE" } ) # if aborts maybe is because they are already stopped. smach.StateMachine.add('DISABLE_REEM_ALIVE', ServiceState('/alive_engine/stop', Empty), transitions={succeeded: 'CROSSING_DOOR_POS'}) smach.StateMachine.add('CROSSING_DOOR_POS', SimpleActionState('/motion_manager', MotionManagerAction, goal=rest_goal), transitions={succeeded: 'CHECK_CAN_PASS', aborted: 'CROSSING_DOOR_POS'}) @smach.cb_interface(outcomes=[succeeded, aborted, preempted, 'door_too_far']) def check_range(userdata, message): length_ranges = len(message.ranges) alpha = math.atan((self.WIDTH/2)/distance) n_elem = int(math.ceil(length_ranges*alpha/(2*message.angle_max))) # Num elements from the 0 angle to the left or right middle = length_ranges/2 cut = [x for x in message.ranges[middle-n_elem:middle+n_elem] if x > 0.01] print "cut: " + str(cut) minimum = min(cut) if self.door_position == -1: if message.ranges[middle] <= self.MAX_DIST_TO_DOOR: self.door_position = message.ranges[middle] else: return 'door_too_far' if (minimum >= distance+self.door_position): return succeeded rospy.loginfo("Distance in front of the robot is too small: " + str(distance+self.door_position) + ". Minimum distance: " + str(minimum)) return aborted smach.StateMachine.add('CHECK_CAN_PASS', TopicReaderState(topic_name=LASER_TOPIC, msg_type=LaserScan, timeout=self.READ_TIMEOUT, callback=check_range, outcomes=[succeeded, aborted, preempted, 'door_too_far']), remapping={'message': 'range_readings'}, transitions={succeeded: 'ENTER_ROOM', aborted: 'CHECK_CAN_PASS', 'door_too_far': 'SAY_TOO_FAR_FROM_DOOR'}) smach.StateMachine.add('SAY_TOO_FAR_FROM_DOOR', SpeakActionState(text="I'm too far from the door."), transitions={succeeded: 'CHECK_CAN_PASS', aborted: 'CHECK_CAN_PASS'}) def move_enter_room_cb(userdata, nav_goal): pose = Pose() pose.position = Point(distance+self.door_position, 0, 0) pose.orientation = Quaternion(*quaternion_from_euler(0, 0, orient_after_passing)) nav_goal.target_pose.pose = transform_pose(pose, 'base_link', 'map') return nav_goal smach.StateMachine.add('ENTER_ROOM', MoveActionState("/map", "/move_base", goal_cb=move_enter_room_cb), transitions={succeeded: 'INTERACT_POS', aborted: 'ENTER_ROOM'} if check_for_door else {aborted: 'ENTER_ROOM'}) # transitions={succeeded: 'succeeded', aborted: 'ENTER_ROOM'} if check_for_door else {aborted: 'ENTER_ROOM'}) if check_for_door: # It's safer to navigate with the arms like that interact_goal = MotionManagerGoal() interact_goal.plan = plan interact_goal.checkSafety = checkSafety interact_goal.repeat = False interact_goal.filename = MOTION_FOLDER_PATH + 'interact.xml' smach.StateMachine.add('INTERACT_POS', SimpleActionState('/motion_manager', MotionManagerAction, goal=interact_goal), transitions={succeeded: succeeded})
def __init__(self): smach.StateMachine.__init__(self, [succeeded, preempted, aborted]) with self: # desactivate controllers smach.StateMachine.add( 'DeactivateControllersInit', StopRobotControllers(head=True, left=True, right=True), transitions={succeeded: 'MOVE_TO_NO_COLLISION_POSE', preempted: preempted, aborted: aborted}) # move arms to cool pose filename = MOTION_FOLDER_PATH + "interact_head_up.xml" cool_motion_goal = MotionManagerGoal() cool_motion_goal.plan = False cool_motion_goal.filename = filename cool_motion_goal.checkSafety = False cool_motion_goal.repeat = False cool_motion_action = SimpleActionState('/motion_manager', MotionManagerAction, goal=cool_motion_goal) smach.StateMachine.add('MOVE_TO_NO_COLLISION_POSE', cool_motion_action, transitions={succeeded: 'GetPCL', preempted: preempted, aborted: succeeded}) smach.StateMachine.add('GetPCL', GetPCL('/head_mount_xtion/depth_registered/points'), transitions={'success':'GetGraspingPointAction', 'fail':aborted}, remapping={'pcl_RGB':'pcl_RGB'}) smach.StateMachine.add('GetGraspingPointAction', SimpleActionState('/skills/bow_detector/bow_object_detector/get_grasping_point', GetGraspingPointAction, #goal_slots=['pcl_RGB'], goal_cb=grasping_point_goal_cb, input_keys=['pcl_RGB'], result_slots=['grasping_point']), #server_wait_timeout=rospy.Duration(10.0)), transitions={'succeeded':'ConvertGraspPoint', 'preempted':preempted, 'aborted':aborted}, remapping={'grasping_point':'sm_grasp_point'}) # prepare pose smach.StateMachine.add('ConvertGraspPoint', CBState(build_grasp_pose_from_point_cb), transitions={'done':'TransformPoseStamped'}, remapping={'grasp_point' : 'sm_grasp_point', 'grasp_pose_st' : 'sm_grasp_pose', 'target_frame' : 'sm_target_frame'}) # transform pose ¿needed? smach.StateMachine.add('TransformPoseStamped', TransformPoseStamped(), transitions={'succeeded':'ActivateControllers', 'preempted':preempted, 'aborted':aborted}, remapping={'target_pose':'target_pose_stamped'}) # activate controllers smach.StateMachine.add( 'ActivateControllers', StartRobotControllers(head=False, left=False, right=True), transitions={succeeded: 'ReemGraspStateMachine', preempted: preempted, aborted: aborted}) # grasp smach.StateMachine.add( 'ReemGraspStateMachine', SMReemClothGraspStateMachine(), transitions={'succeeded': 'DeactivateControllers', 'preempted': preempted, 'aborted': aborted}) # desactivate controllers smach.StateMachine.add( 'DeactivateControllers', StopRobotControllers(head=False, left=False, right=True), transitions={succeeded: succeeded, preempted: preempted, aborted: aborted})
def __init__(self, distToHuman=0.9): smach.StateMachine.__init__( self, outcomes=[succeeded, preempted, aborted], input_keys=["in_learn_person"]) with self: if remotelly_executing == -1 or inside_robot: '''def disable_cb(userdata, request): disableEmerg = DisableEmergencyRequest() disableEmerg.second = 600 disableEmerg.useLEDs = True return disableEmerg smach.StateMachine.add('DISABLE_EMERGENCY_STOP', ServiceState('/disable_emergency_stop', DisableEmergency, request_cb=disable_cb), transitions={succeeded: 'DISABLE_REEM_ALIVE'}) smach.StateMachine.add('DISABLE_REEM_ALIVE', ServiceState('/alive_engine/stop', Empty), transitions={succeeded: 'DISABLE_FACE_TRACKING'})''' smach.StateMachine.add('DISABLE_FACE_TRACKING', ServiceState('/personServer/faceTracking/stop', Empty), transitions={succeeded: 'START_HEAD_CONTROLLERS'}) smach.StateMachine.add( "START_HEAD_CONTROLLERS", StartRobotControllers(head=True, left=False, right=False), transitions={succeeded: "CROSSING_DOOR_POS", aborted: "START_HEAD_CONTROLLERS", preempted: "START_HEAD_CONTROLLERS"}) motion_goal = MotionManagerGoal() motion_goal.plan = False motion_goal.filename = MOTION_FOLDER_PATH motion_goal.checkSafety = False motion_goal.repeat = False smach.StateMachine.add('CROSSING_DOOR_POS', SimpleActionState('/motion_manager', MotionManagerAction, goal=motion_goal), transitions={succeeded: 'FIX_HEAD_POSITION', aborted: 'FIX_HEAD_POSITION'}) @smach.cb_interface(outcomes=[succeeded, preempted, aborted]) def fixHeadPosition(userdata): head_goal = PointHeadGoal() head_goal.target.header.frame_id = "base_link" head_goal.target.point.x = 1.0 head_goal.target.point.y = 0.0 head_goal.target.point.z = 1.65 head_goal.pointing_frame = "stereo_link" head_goal.pointing_axis.x = 1.0 head_goal.pointing_axis.y = 0.0 head_goal.pointing_axis.z = 0.0 head_goal.max_velocity = 1.5 head_goal.min_duration.secs = 1.5 client = SimpleActionClient("/head_traj_controller/point_head_action", PointHeadAction) client.wait_for_server(rospy.Duration(5.0)) client.send_goal(head_goal) return succeeded smach.StateMachine.add('FIX_HEAD_POSITION', CBState(fixHeadPosition), transitions={succeeded: "LEARN_PERSON", preempted: "LEARN_PERSON", aborted: "LEARN_PERSON"}) smach.StateMachine.add('LEARN_PERSON', LearnPerson(), transitions={succeeded: 'TRACK_OPERATOR', aborted: aborted}, remapping={'out_personTrackingData': 'out_personTrackingData', "in_learn_person": "in_learn_person"}) smach.StateMachine.add('TRACK_OPERATOR', TrackOperator(distToHuman), remapping={'in_personTrackingData': 'out_personTrackingData'}, transitions={succeeded: succeeded, preempted: preempted, aborted: aborted})
def main(): rospy.init_node("robot_inspection") # Create a SMACH state machine sm = smach.StateMachine(["succeeded", "aborted", "preempted"]) with sm: # FIXME Do all the speak actions and motion actions simultanously?? smach.StateMachine.add( "CLOSE_HAND", CloseReemHand(), transitions={"succeeded": "STARTING_SPEECH", "aborted": "STARTING_SPEECH"} ) ## Indicate that we are starting smach.StateMachine.add( "STARTING_SPEECH", SpeakActionState( text="I am ready to start the registration. I will now go to the registration desk.", wait_before_speaking=Duration(1.0), ), transitions={"succeeded": "ENTER_ROOM", "aborted": "ENTER_ROOM"}, ) # Enters into the room through the door smach.StateMachine.add( "ENTER_ROOM", EnterRoomStateMachine(distance=DOOR_DISTANCE, orient_after_passing=0.0), transitions={"succeeded": "ENABLE_REEM_ALIVE"}, ) smach.StateMachine.add( "ENABLE_REEM_ALIVE", ServiceState("/alive_engine/start", Empty), transitions={"succeeded": "MOVE_TO_REG_TABLE", "aborted": "MOVE_TO_REG_TABLE"}, ) # Go to table sm.userdata.registration_table = REGISTRATION_TABLE_NAME smach.StateMachine.add( "MOVE_TO_REG_TABLE", MoveToRoomStateMachine(announcement=None), transitions={"succeeded": "DISABLE_REEM_ALIVE", "aborted": "MOVE_TO_REG_TABLE"}, remapping={"room_name": "registration_table"}, ) smach.StateMachine.add( "DISABLE_REEM_ALIVE", ServiceState("/alive_engine/stop", Empty), transitions={"succeeded": "MOVE_TO_BOW", "aborted": "MOVE_TO_BOW"}, ) # Bow to face filename = MOTION_FOLDER_PATH + "bow.xml" bow_motion_goal = MotionManagerGoal() bow_motion_goal.plan = False bow_motion_goal.filename = filename bow_motion_goal.checkSafety = False bow_motion_goal.repeat = False bow_motion_goal.priority = 0 bow_action = SimpleActionState("/motion_manager", MotionManagerAction, goal=bow_motion_goal) smach.StateMachine.add( "MOVE_TO_BOW", bow_action, transitions={"succeeded": "PRESENT_REEM", "aborted": "PRESENT_REEM"} ) # Present the robot and the team present_text = "Hi everybody! My name is REEM. I came here with my teammates from the REEM_@_EERI team!" smach.StateMachine.add( "PRESENT_REEM", SpeakActionState(text=present_text, wait_before_speaking=Duration(0.2)), transitions={"succeeded": "START_PRESENT", "aborted": "PRESENT_REEM"}, ) # Deliver the form and say something funny take_it_text = "Oh, you look really good today! This is my registration form. Please, take it." smach.StateMachine.add( "SAY_HERE_CV", SpeakActionState(text=take_it_text, wait_before_speaking=Duration(0.10)), transitions={"succeeded": "OPEN_HAND", "aborted": "SAY_HERE_CV"}, ) smach.StateMachine.add( "START_PRESENT", SpeakAndMotionActionConcurrentSM( filename=MOTION_FOLDER_PATH + "robocup_start_presentation.xml", checksafety=False, plan=False, text=take_it_text, ), transitions={"succeeded": "OPEN_HAND", "aborted": "SAY_HERE_CV"}, ) smach.StateMachine.add( "OPEN_HAND", OpenReemHand2(), transitions={"succeeded": "SAY_READ_AND_MID", "aborted": "SAY_READ_AND_MID"} ) read_this = "Please, can you read the form and tell my mates if there's anything wrong." smach.StateMachine.add( "SAY_READ_AND_MID", SpeakAndMotionActionConcurrentSM( filename=MOTION_FOLDER_PATH + "robocup_middle_presentation.xml", checksafety=False, plan=False, text=read_this, wait_before_speaking=Duration(1.0), ), transitions={"succeeded": "END_PRESENTATION_AND_THANK", "aborted": "SAY_HERE_CV"}, ) thank_you = "Thank you. Tell me when I should leave the arena." smach.StateMachine.add( "END_PRESENTATION_AND_THANK", SpeakAndMotionActionConcurrentSM( filename=MOTION_FOLDER_PATH + "robocup_end_presentation.xml", checksafety=False, plan=False, text=thank_you, wait_before_speaking=Duration(0.8), ), transitions={"succeeded": "RE_ENABLE_REEM_ALIVE", "aborted": "SAY_HERE_CV"}, ) smach.StateMachine.add( "RE_ENABLE_REEM_ALIVE", ServiceState("/alive_engine/start", Empty), transitions={"succeeded": "WAIT_LISTEN_ORDER", "aborted": "WAIT_LISTEN_ORDER"}, ) smach.StateMachine.add( "WAIT_LISTEN_ORDER", RecogCommand( GRAMMAR_NAME="robocup/room", command_key="location", command_value=LEAVING_DOOR_NAME, ask_for_confirmation=False, ), transitions={"succeeded": "BABY_DONT_HURT_ME", "aborted": "ASK_TO_REPEAT"}, ) repeat_pool = [ "Pardon?", "Can you repeat the name, please?", "I'm sorry I didn't understand you. Can you repeat that?", "I didn't get it. Can you please repeat?", ] smach.StateMachine.add( "ASK_TO_REPEAT", SpeakActionFromPoolStateMachine(repeat_pool), transitions={"succeeded": "WAIT_LISTEN_ORDER", "aborted": "WAIT_LISTEN_ORDER"}, ) baby_dont_hurt_me = "Okay. Please, Don't hurt me when you hit the emergency button." smach.StateMachine.add( "BABY_DONT_HURT_ME", SpeakActionState(text=baby_dont_hurt_me, wait_before_speaking=Duration(0.20)), transitions={"succeeded": "SAY_AND_DO_BYE", "aborted": "SAY_HERE_CV"}, ) # Say Bye bye and move the hand smach.StateMachine.add( "SAY_AND_DO_BYE", SpeakAndMotionActionConcurrentSM( filename=MOTION_FOLDER_PATH + "wave.xml", checksafety=False, plan=True, text="Bye Bye!", wait_before_speaking=Duration(2.1), ), transitions={"succeeded": "INTERACT_POS", "aborted": "INTERACT_POS"}, ) interact_goal = MotionManagerGoal() interact_goal.plan = True interact_goal.filename = filename interact_goal.checkSafety = False interact_goal.repeat = False interact_goal.priority = 0 interact_goal.filename = MOTION_FOLDER_PATH + "interact.xml" smach.StateMachine.add( "INTERACT_POS", SimpleActionState("/motion_manager", MotionManagerAction, goal=interact_goal), transitions={"succeeded": "SLOW_SPEED", "aborted": "INTERACT_POS"}, ) def slow_speed(userdata): node_to_reconfigure = "/move_base/PalLocalPlanner" client = dynamic_reconfigure.client.Client(node_to_reconfigure) new_params = {"max_vel_x": 0.2} client.update_configuration(new_params) return "succeeded" smach.StateMachine.add( "SLOW_SPEED", smach.CBState(slow_speed, outcomes=["succeeded"]), transitions={"succeeded": "GO_PRE_EXIT"} ) # Go to the leaving door sm.userdata.pre_exit_door = PRE_EXIT_NAME smach.StateMachine.add( "GO_PRE_EXIT", MoveToRoomStateMachine(announcement=None), transitions={"succeeded": "STOP_REEM_ALIVE", "aborted": "CANT_LEAVE"}, remapping={"room_name": "pre_exit_door"}, ) smach.StateMachine.add( "STOP_REEM_ALIVE", ServiceState("/alive_engine/stop", Empty), transitions={"succeeded": "CROSSING_DOOR_POS", "aborted": "CROSSING_DOOR_POS"}, ) rest_goal = MotionManagerGoal() rest_goal.plan = True rest_goal.filename = filename rest_goal.checkSafety = False rest_goal.repeat = False rest_goal.priority = 0 rest_goal.filename = MOTION_FOLDER_PATH + "rest.xml" smach.StateMachine.add( "CROSSING_DOOR_POS", SimpleActionState("/motion_manager", MotionManagerAction, goal=rest_goal), transitions={"succeeded": "LEAVE_ARENA", "aborted": "CROSSING_DOOR_POS"}, ) sm.userdata.exit_door = LEAVING_DOOR_NAME smach.StateMachine.add( "LEAVE_ARENA", MoveToRoomStateMachine(announcement=None), transitions={"succeeded": "succeeded", "aborted": "LEAVE_ARENA"}, remapping={"room_name": "exit_door"}, ) cant_leave = "Emergency button was pressed, retrying to leave the arena." smach.StateMachine.add( "CANT_LEAVE", SpeakActionState(text=cant_leave, wait_before_speaking=Duration(0.0)), transitions={"succeeded": "GO_PRE_EXIT", "aborted": "GO_PRE_EXIT"}, ) # Create and start the introspection server sis = smach_ros.IntrospectionServer("server_name", sm, "/SM_ROOT") sis.start() # Execute the state machine sm.execute() # Wait for ctrl-c to stop the application rospy.spin() sis.stop()