def create(self): # x:1111 y:187, x:1077 y:300 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:365 _sm_follow_me_0 = OperatableStateMachine(outcomes=['completed'], input_keys=['tts_text', 'follow_me']) with _sm_follow_me_0: # x:43 y:153 OperatableStateMachine.add('hsr_FollowMe', hsr_FollowMe(), transitions={'continue': 'tts_Followme'}, autonomy={'continue': Autonomy.Off}, remapping={'request': 'follow_me'}) # x:204 y:150 OperatableStateMachine.add('tts_Followme', hsr_TtsInputKey(), transitions={'completed': 'completed'}, autonomy={'completed': Autonomy.Off}, remapping={'tts_text': 'tts_text'}) # x:575 y:100, x:568 y:39 _sm_taketheobject_1 = OperatableStateMachine(outcomes=['continue', 'failed']) with _sm_taketheobject_1: # x:209 y:40 OperatableStateMachine.add('tts_AllRight', hsr_TtsInputParameter(text="All right.", language="en", delay=2), transitions={'completed': 'CloseTheHand'}, autonomy={'completed': Autonomy.Off}) # x:385 y:38 OperatableStateMachine.add('CloseTheHand', hsr_GrippingObject(grasp_force=0.7, mode=True), transitions={'continue': 'continue', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'grasp_success': 'grasp_success'}) # x:1077 y:108 _sm_goto_2 = OperatableStateMachine(outcomes=['completed'], input_keys=['move']) with _sm_goto_2: # x:43 y:31 OperatableStateMachine.add('SpcoMoveEntrance', hsr_SpcofMove(), transitions={'move': 'tts_GoTo', 'failed': 'SpcoMoveEntrance'}, autonomy={'move': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'move_name': 'move', 'move_text': 'move_text', 'move_pose': 'move_pose', 'arrive_text': 'arrive_text'}) # x:221 y:31 OperatableStateMachine.add('tts_GoTo', hsr_TtsInputKey(), transitions={'completed': 'Move'}, autonomy={'completed': Autonomy.Off}, remapping={'tts_text': 'move_text'}) # x:406 y:29 OperatableStateMachine.add('Move', hsr_MoveBase(), transitions={'succeeded': 'tts_Arrived', 'failed': 'Limit_checker'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'request': 'move_pose'}) # x:232 y:161 OperatableStateMachine.add('tts_FailedMove', hsr_TtsInputParameter(text="I failed to make the planning. I will try again.", language="en", delay=2), transitions={'completed': 'SpcoMoveEntrance'}, autonomy={'completed': Autonomy.Off}) # x:615 y:28 OperatableStateMachine.add('tts_Arrived', hsr_TtsInputKey(), transitions={'completed': 'completed'}, autonomy={'completed': Autonomy.Off}, remapping={'tts_text': 'arrive_text'}) # x:480 y:168 OperatableStateMachine.add('Limit_checker', hsr_try_limit(), transitions={'give_up': 'tts_GiveupMove', 'try_again': 'tts_FailedMove'}, autonomy={'give_up': Autonomy.Off, 'try_again': Autonomy.Off}) # x:634 y:164 OperatableStateMachine.add('tts_GiveupMove', hsr_TtsInputParameter(text="Sorry. I gave up.", language="en", delay=2), transitions={'completed': 'completed'}, autonomy={'completed': Autonomy.Off}) # x:30 y:365 _sm_learnspatialconcept_3 = OperatableStateMachine(outcomes=['completed'], input_keys=['training']) with _sm_learnspatialconcept_3: # x:180 y:40 OperatableStateMachine.add('hsr_SpcofTraining', hsr_SpcofTraining(), transitions={'completed': 'tts_Spco_Result'}, autonomy={'completed': Autonomy.Off}, remapping={'request': 'training', 'spcof_text': 'spcof_text'}) # x:30 y:42 OperatableStateMachine.add('tts_Spco_Result', hsr_TtsInputKey(), transitions={'completed': 'completed'}, autonomy={'completed': Autonomy.Off}, remapping={'tts_text': 'spcof_text'}) # x:373 y:54 _sm_start_4 = OperatableStateMachine(outcomes=['completed']) with _sm_start_4: # x:30 y:43 OperatableStateMachine.add('hsr_MoveNeutral', hsr_MoveNeutral(), transitions={'continue': 'completed'}, autonomy={'continue': Autonomy.Off}) with _state_machine: # x:60 y:42 OperatableStateMachine.add('Start', _sm_start_4, transitions={'completed': 'hsr_SpeechRecognition'}, autonomy={'completed': Autonomy.Inherit}) # x:442 y:223 OperatableStateMachine.add('LearnSpatialConcept', _sm_learnspatialconcept_3, transitions={'completed': 'hsr_SpeechRecognition'}, autonomy={'completed': Autonomy.Inherit}, remapping={'training': 'training'}) # x:464 y:310 OperatableStateMachine.add('GoTo', _sm_goto_2, transitions={'completed': 'tts_next_command'}, autonomy={'completed': Autonomy.Inherit}, remapping={'move': 'move'}) # x:885 y:279 OperatableStateMachine.add('tts_FinishDemonstration', hsr_TtsInputParameter(text="本日はオープンキャンパスにご来場いただきありがとうございました。ぜひ立命館に来てくださいね!", language="ja", delay=0), transitions={'completed': 'finished'}, autonomy={'completed': Autonomy.Off}) # x:228 y:173 OperatableStateMachine.add('tts_next_command', hsr_TtsInputParameter(text="Please give me the next command", language="en", delay=2), transitions={'completed': 'hsr_SpeechRecognition'}, autonomy={'completed': Autonomy.Off}) # x:746 y:176 OperatableStateMachine.add('hsr_CourseDemo', hsr_CourseDemo(), transitions={'follow_me': 'Follow Me', 'retry_recognition': 'tts_RetryRecognition', 'data': 'tts_RetryRecognition', 'learn': 'LearnSpatialConcept', 'goto': 'GoTo', 'finish': 'tts_FinishDemonstration', 'start_position': 'tts_RetryRecognition', 'take_this': 'TakeTheObject'}, autonomy={'follow_me': Autonomy.Off, 'retry_recognition': Autonomy.Off, 'data': Autonomy.Off, 'learn': Autonomy.Off, 'goto': Autonomy.Off, 'finish': Autonomy.Off, 'start_position': Autonomy.Off, 'take_this': Autonomy.Off}, remapping={'recognition': 'recognition', 'follow_me': 'follow_me', 'training': 'training', 'tts_text': 'tts_text', 'move': 'move'}) # x:55 y:167 OperatableStateMachine.add('hsr_SpeechRecognition', hsr_SpeechRecognition(topic="/julius/speech2text/en"), transitions={'recognition': 'hsr_CourseDemo'}, autonomy={'recognition': Autonomy.Off}, remapping={'recognition': 'recognition'}) # x:237 y:417 OperatableStateMachine.add('hsr_MoveNeutralAgain', hsr_MoveNeutral(), transitions={'continue': 'hsr_SpeechRecognition'}, autonomy={'continue': Autonomy.Off}) # x:463 y:415 OperatableStateMachine.add('TakeTheObject', _sm_taketheobject_1, transitions={'continue': 'hsr_MoveNeutralAgain', 'failed': 'hsr_SpeechRecognition'}, autonomy={'continue': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:446 y:106 OperatableStateMachine.add('tts_RetryRecognition', hsr_TtsInputParameter(text="Could you speak again?", language="en", delay=2), transitions={'completed': 'tts_next_command'}, autonomy={'completed': Autonomy.Off}) # x:503 y:3 OperatableStateMachine.add('Follow Me', _sm_follow_me_0, transitions={'completed': 'hsr_SpeechRecognition'}, autonomy={'completed': Autonomy.Inherit}, remapping={'tts_text': 'tts_text', 'follow_me': 'follow_me'}) return _state_machine
def create(self): # x:7 y:121, x:11 y:204 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:43 y:25 OperatableStateMachine.add('hsr_MoveBaseClient1', hsr_MoveBaseClient(pose_position_x=0.5, pose_position_y=0.0, pose_orientation_z=0.0, pose_orientation_w=1.0), transitions={'completed': 'hsr_MoveBase1'}, autonomy={'completed': Autonomy.Off}, remapping={'pose': 'pose1'}) # x:227 y:22 OperatableStateMachine.add('hsr_MoveBase1', hsr_MoveBase(), transitions={'succeeded': 'hsr_MoveBaseClient2', 'failed': 'failed'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'request': 'pose1'}) # x:386 y:22 OperatableStateMachine.add('hsr_MoveBaseClient2', hsr_MoveBaseClient(pose_position_x=0.5, pose_position_y=0.5, pose_orientation_z=0.0, pose_orientation_w=1.0), transitions={'completed': 'hsr_MoveBase2'}, autonomy={'completed': Autonomy.Off}, remapping={'pose': 'pose2'}) # x:565 y:21 OperatableStateMachine.add('hsr_MoveBase2', hsr_MoveBase(), transitions={'succeeded': 'hsr_MoveBaseClient3', 'failed': 'failed'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'request': 'pose2'}) # x:401 y:101 OperatableStateMachine.add('hsr_MoveBase3', hsr_MoveBase(), transitions={'succeeded': 'hsr_MoveBaseClient4', 'failed': 'failed'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'request': 'pose3'}) # x:62 y:99 OperatableStateMachine.add('hsr_MoveBase4', hsr_MoveBase(), transitions={'succeeded': 'finished', 'failed': 'failed'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'request': 'pose4'}) # x:553 y:112 OperatableStateMachine.add('hsr_MoveBaseClient3', hsr_MoveBaseClient(pose_position_x=0.0, pose_position_y=0.5, pose_orientation_z=0.0, pose_orientation_w=1.0), transitions={'completed': 'hsr_MoveBase3'}, autonomy={'completed': Autonomy.Off}, remapping={'pose': 'pose3'}) # x:224 y:99 OperatableStateMachine.add('hsr_MoveBaseClient4', hsr_MoveBaseClient(pose_position_x=0.0, pose_position_y=0.0, pose_orientation_z=0.0, pose_orientation_w=1.0), transitions={'completed': 'hsr_MoveBase4'}, autonomy={'completed': Autonomy.Off}, remapping={'pose': 'pose4'}) return _state_machine
def create(self): # x:260 y:496, x:32 y:439 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:65 y:30 OperatableStateMachine.add( 'hsr_SpeechRecognition', hsr_SpeechRecognition(topic="julius"), transitions={'recognition': 'hsr_SpcofTraining'}, autonomy={'recognition': Autonomy.Off}, remapping={'recognition': 'recognition'}) # x:472 y:346 OperatableStateMachine.add('hsr_SpcofMove', hsr_SpcofMove(), transitions={ 'move': 'tts2', 'failed': 'hsr_SpcofMove' }, autonomy={ 'move': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'move_name': 'recognition', 'move_text': 'move_text', 'move_pose': 'move_pose', 'arrive_text': 'arrive_text' }) # x:630 y:463 OperatableStateMachine.add('hsr_MoveBase', hsr_MoveBase(), transitions={ 'succeeded': 'tts3', 'failed': 'hsr_MoveBase' }, autonomy={ 'succeeded': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'request': 'move_pose'}) # x:498 y:122 OperatableStateMachine.add( 'tts1', hsr_TtsInputKey(), transitions={'completed': 'hsr_SpeechRecognition'}, autonomy={'completed': Autonomy.Off}, remapping={'tts_text': 'spcof_text'}) # x:634 y:376 OperatableStateMachine.add( 'tts2', hsr_TtsInputKey(), transitions={'completed': 'hsr_MoveBase'}, autonomy={'completed': Autonomy.Off}, remapping={'tts_text': 'move_text'}) # x:631 y:560 OperatableStateMachine.add( 'tts3', hsr_TtsInputKey(), transitions={'completed': 'hsr_SpeechRecognition'}, autonomy={'completed': Autonomy.Off}, remapping={'tts_text': 'arrive_text'}) # x:483 y:27 OperatableStateMachine.add('hsr_SpcofTraining', hsr_SpcofTraining(), transitions={'completed': 'tts1'}, autonomy={'completed': Autonomy.Off}, remapping={ 'request': 'recognition', 'spcof_text': 'spcof_text' }) return _state_machine
def create(self): # x:849 y:279, x:1247 y:39 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:368 y:58 _sm_gotoexit_0 = OperatableStateMachine(outcomes=['completed'], input_keys=['exit_pose']) with _sm_gotoexit_0: # x:41 y:40 OperatableStateMachine.add( 'tts_EneterField', hsr_TtsInputParameter(text="I will go to the exit.", language="en", delay=1.0), transitions={'completed': 'hsr_MoveExit'}, autonomy={'completed': Autonomy.Off}) # x:222 y:38 OperatableStateMachine.add('hsr_MoveExit', hsr_MoveBase(), transitions={ 'succeeded': 'completed', 'failed': 'hsr_MoveExit' }, autonomy={ 'succeeded': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'request': 'exit_pose'}) # x:30 y:478 _sm_dooropen_1 = OperatableStateMachine(outcomes=['completed']) with _sm_dooropen_1: # x:30 y:45 OperatableStateMachine.add('hsr_DoorDetect', hsr_DoorDetect(), transitions={ 'open': 'tts_DoorOpened', 'close': 'hsr_DoorDetect' }, autonomy={ 'open': Autonomy.Off, 'close': Autonomy.Off }) # x:206 y:40 OperatableStateMachine.add('tts_DoorOpened', hsr_TtsInputParameter( text="Door opened.", language="en", delay=3), transitions={'completed': 'completed'}, autonomy={'completed': Autonomy.Off}) with _state_machine: # x:30 y:40 OperatableStateMachine.add('hsr_MoveNeutral', hsr_MoveNeutral(), transitions={'continue': 'tts_Ready'}, autonomy={'continue': Autonomy.Off}) # x:178 y:40 OperatableStateMachine.add('tts_Ready', hsr_TtsInputParameter(text="I'm ready!", language="en", delay=0), transitions={'completed': 'DoorOpen'}, autonomy={'completed': Autonomy.Off}) # x:363 y:38 OperatableStateMachine.add( 'DoorOpen', _sm_dooropen_1, transitions={'completed': 'hsr_Move_Ahead_Collision'}, autonomy={'completed': Autonomy.Inherit}) # x:846 y:36 OperatableStateMachine.add('hsr_ExitPose', hsr_GoalPoseDummy(px=3.93524549232, py=5.01833559259, pz=0.0, qx=0.0, qy=0.0, qz=0.687634546984, qw=0.726056974207), transitions={'continue': 'GoToExit'}, autonomy={'continue': Autonomy.Off}, remapping={'goal_pose': 'exit_pose'}) # x:1038 y:29 OperatableStateMachine.add( 'GoToExit', _sm_gotoexit_0, transitions={'completed': 'failed'}, autonomy={'completed': Autonomy.Inherit}, remapping={'exit_pose': 'exit_pose'}) # x:651 y:39 OperatableStateMachine.add( 'tts_EneterField', hsr_TtsInputParameter(text="I entered the field.", language="en", delay=1.0), transitions={'completed': 'hsr_ExitPose'}, autonomy={'completed': Autonomy.Off}) # x:496 y:132 OperatableStateMachine.add( 'hsr_Move_Ahead_Collision', hsr_Move_Ahead_Collision(speed=0.3, ros_rate=30, distance=1.1), transitions={'continue': 'tts_EneterField'}, autonomy={'continue': Autonomy.Off}) return _state_machine
def create(self): # x:511 y:532, x:610 y:460 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:55 y:26 OperatableStateMachine.add('tts_CollectLaserMicrophone', hsr_TtsInputParameter(text="OK. I will take something drink from dingin table.", language="en"), transitions={'completed': 'hsr_MoveBaseTakePlace'}, autonomy={'completed': Autonomy.Off}) # x:737 y:369 OperatableStateMachine.add('hsr_HandClose', hsr_GrippingObject(grasp_force=0.7, mode=True), transitions={'continue': 'hsr_MoveBack', 'failed': 'hsr_MoveBack'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'grasp_success': 'grasp_success'}) # x:254 y:27 OperatableStateMachine.add('hsr_MoveBaseTakePlace', hsr_MoveBaseClient(pose_position_x=2.93275682249, pose_position_y=-1.96998477423, pose_orientation_z=0.999996209364, pose_orientation_w=0.00275340834021), transitions={'completed': 'hsr_MoveTakePlace'}, autonomy={'completed': Autonomy.Off}, remapping={'pose': 'take_place'}) # x:434 y:28 OperatableStateMachine.add('hsr_MoveTakePlace', hsr_MoveBase(), transitions={'succeeded': 'hsr_HandOpen1', 'failed': 'hsr_MoveTakePlace'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'request': 'take_place'}) # x:1546 y:503 OperatableStateMachine.add('hsr_HandOver', hsr_JointPose(arm_lift_joint=0.0, arm_flex_joint=-3.14/4, arm_roll_joint=0.0, wrist_flex_joint=-3.14/4, wrist_roll_joint=0.0, head_pan_joint=0.0, head_tilt_joint=-0.2), transitions={'continue': 'tts_Hereyouare'}, autonomy={'continue': Autonomy.Off}) # x:1370 y:505 OperatableStateMachine.add('tts_Hereyouare', hsr_TtsInputParameter(text="Here you are.", language="en"), transitions={'completed': 'hsr_HandOpen'}, autonomy={'completed': Autonomy.Off}) # x:1188 y:506 OperatableStateMachine.add('hsr_HandOpen', hsr_GrippingObject(grasp_force=0.7, mode=False), transitions={'continue': 'hsr_JointPoseGo2', 'failed': 'hsr_HandOpen'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'grasp_success': 'grasp_success'}) # x:782 y:517 OperatableStateMachine.add('hsr_MoveBaseStandPlace', hsr_MoveBaseClient(pose_position_x=2.19663095949, pose_position_y=-0.107342765678, pose_orientation_z=-0.135386180174, pose_orientation_w=0.990792905817), transitions={'completed': 'hsr_MoveStandPlace'}, autonomy={'completed': Autonomy.Off}, remapping={'pose': 'standby_place'}) # x:1017 y:516 OperatableStateMachine.add('hsr_JointPoseGo2', hsr_JointPose(arm_lift_joint=0.0, arm_flex_joint=0.0, arm_roll_joint=-3.14/2, wrist_flex_joint=-3.14/2, wrist_roll_joint=0.0, head_pan_joint=0.0, head_tilt_joint=0.0), transitions={'continue': 'hsr_MoveBaseStandPlace'}, autonomy={'continue': Autonomy.Off}) # x:1084 y:363 OperatableStateMachine.add('hsr_JointPoseGo1', hsr_JointPose(arm_lift_joint=0.0, arm_flex_joint=0.0, arm_roll_joint=-3.14/2, wrist_flex_joint=-3.14/2, wrist_roll_joint=0.0, head_pan_joint=0.0, head_tilt_joint=0.0), transitions={'continue': 'hsr_MoveBasePartyRoom'}, autonomy={'continue': Autonomy.Off}) # x:1347 y:208 OperatableStateMachine.add('hsr_JointPoseNeutral', hsr_JointPose(arm_lift_joint=0.3, arm_flex_joint=-3.14/4, arm_roll_joint=0.0, wrist_flex_joint=-3.14/4, wrist_roll_joint=0.0, head_pan_joint=0.0, head_tilt_joint=0.0), transitions={'continue': 'hsr_ObjectName'}, autonomy={'continue': Autonomy.Off}) # x:593 y:27 OperatableStateMachine.add('hsr_HandOpen1', hsr_GrippingObject(grasp_force=0.7, mode=False), transitions={'continue': 'hsr_JointPose_xtion', 'failed': 'hsr_HandOpen1'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'grasp_success': 'grasp_success'}) # x:1261 y:19 OperatableStateMachine.add('hsr_ImageCapture', hsr_ImageCapture(rgb_topic="/hsrb/head_rgbd_sensor/rgb/image_rect_color", depth_topic="/hsrb/head_rgbd_sensor/depth_registered/image_rect_raw", camera_info_topic="/hsrb/head_rgbd_sensor/rgb/camera_info"), transitions={'completed': 'hsr_BoundingBox2D', 'failed': 'hsr_ImageCapture'}, autonomy={'completed': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'rgb_image': 'rgb_image', 'depth_image': 'depth_image', 'camera_info': 'camera_info'}) # x:1396 y:23 OperatableStateMachine.add('hsr_BoundingBox2D', hsr_BoundingBox2D(output_topic="/bounding_box_2d_monitor"), transitions={'completed': 'hsr_GraspingPointDetect', 'failed': 'hsr_BoundingBox2D'}, autonomy={'completed': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'rgb_image': 'rgb_image', 'depth_image': 'depth_image', 'camera_info': 'camera_info', 'detection': 'detection', 'grasping_point': 'grasping_point'}) # x:1558 y:21 OperatableStateMachine.add('hsr_GraspingPointDetect', hsr_GraspingPointDetect(output_topic="/bounding_box_2d_monitor"), transitions={'completed': 'hsr_Tf2', 'failed': 'hsr_GraspingPointDetect'}, autonomy={'completed': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'rgb_image': 'rgb_image', 'depth_image': 'depth_image', 'camera_info': 'camera_info', 'detection': 'detection', 'grasping_point': 'grasping_point'}) # x:1622 y:99 OperatableStateMachine.add('hsr_Tf2', hsr_Tf2(before="head_rgbd_sensor_rgb_frame", after="map"), transitions={'continue': 'hsr_JointPoseNeutral', 'failed': 'hsr_Tf2'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'before_pose': 'grasping_point', 'after_pose': 'after_pose'}) # x:308 y:265 OperatableStateMachine.add('hsr_PoseDecisionFinal', hsr_PoseDecisionConveni(offset=None), transitions={'continue': 'hsr_ViewMarker', 'plan_failed': 'tts_FailPlan', 'detect_failed': 'tts_FailDetection'}, autonomy={'continue': Autonomy.Off, 'plan_failed': Autonomy.Off, 'detect_failed': Autonomy.Off}, remapping={'target_poses': 'after_pose', 'target_object_names': 'object', 'failed_objects': 'object', 'selected_pose_approach': 'selected_pose_approach', 'selected_pose_grasp': 'selected_pose_grasp', 'selected_object_name': 'selected_object_name', 'selected_object_status': 'selected_object_status'}) # x:1111 y:206 OperatableStateMachine.add('hsr_ObjectName', hsr_StringDummy(text=["bottle"]), transitions={'continue': 'hsr_PoseDecisionFinal'}, autonomy={'continue': Autonomy.Off}, remapping={'name': 'object'}) # x:22 y:147 OperatableStateMachine.add('tts_FailPlan', hsr_TtsInputParameter(text="I failed to plan a trajectry, but I will try again.", language="en"), transitions={'completed': 'hsr_PlanTryCount'}, autonomy={'completed': Autonomy.Off}) # x:311 y:368 OperatableStateMachine.add('hsr_ViewMarker', hsr_ViewMarker(), transitions={'continue': 'hsr_MoveitToPoseGoalAction', 'failed': 'hsr_ViewMarker'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'view_pose': 'selected_pose_grasp'}) # x:492 y:368 OperatableStateMachine.add('hsr_MoveitToPoseGoalAction', hsr_MoveitToPoseGoalAction(move_group='whole_body', action_topic='/move_group'), transitions={'reached': 'hsr_HandClose', 'planning_failed': 'hsr_PlanTryCount', 'control_failed': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off}, remapping={'pose_goal': 'selected_pose_grasp', 'move_status': 'move_status'}) # x:917 y:366 OperatableStateMachine.add('hsr_MoveBack', hsr_MoveBase(), transitions={'succeeded': 'hsr_JointPoseGo1', 'failed': 'hsr_JointPoseGo1'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'request': 'take_place'}) # x:1279 y:360 OperatableStateMachine.add('hsr_MoveBasePartyRoom', hsr_MoveBaseClient(pose_position_x=4.78038886411, pose_position_y=1.95365056449, pose_orientation_z=-0.148683989838, pose_orientation_w=0.988884761317), transitions={'completed': 'hsr_MovePartyPlace'}, autonomy={'completed': Autonomy.Off}, remapping={'pose': 'bring_place'}) # x:348 y:165 OperatableStateMachine.add('hsr_PlanTryCount', hsr_Counting(max_count=5), transitions={'repetition': 'hsr_XtionMoveClient', 'end': 'tts_fail_take_object'}, autonomy={'repetition': Autonomy.Off, 'end': Autonomy.Off}) # x:596 y:522 OperatableStateMachine.add('hsr_MoveStandPlace', hsr_MoveBase(), transitions={'succeeded': 'finished', 'failed': 'hsr_MoveStandPlace'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'request': 'standby_place'}) # x:641 y:165 OperatableStateMachine.add('tts_fail_take_object', hsr_TtsInputParameter(text="I cannot detect the object, please handover the object.", language="en"), transitions={'completed': 'wait_5sec'}, autonomy={'completed': Autonomy.Off}) # x:835 y:277 OperatableStateMachine.add('wait_5sec', WaitState(wait_time=5.0), transitions={'done': 'hsr_HandClose'}, autonomy={'done': Autonomy.Off}) # x:1525 y:360 OperatableStateMachine.add('hsr_MovePartyPlace', hsr_MoveBase(), transitions={'succeeded': 'hsr_HandOver', 'failed': 'hsr_MovePartyPlace'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'request': 'bring_place'}) # x:966 y:22 OperatableStateMachine.add('hsr_XtionMoveClient', hsr_XtionMoveClient(point_x=0.792080036183, point_y=-1.96998477423, point_z=0.776622437874), transitions={'completed': 'hsr_XtionPoint'}, autonomy={'completed': Autonomy.Off}, remapping={'point': 'point'}) # x:1102 y:20 OperatableStateMachine.add('hsr_XtionPoint', hsr_XtionPoint(ref_frame_id="map"), transitions={'finish': 'hsr_ImageCapture'}, autonomy={'finish': Autonomy.Off}, remapping={'xtion_point': 'point'}) # x:769 y:23 OperatableStateMachine.add('hsr_JointPose_xtion', hsr_JointPose(arm_lift_joint=0.3, arm_flex_joint=-2.619, arm_roll_joint=0.0, wrist_flex_joint=-3.14/2, wrist_roll_joint=0.0, head_pan_joint=0.0, head_tilt_joint=0.0), transitions={'continue': 'hsr_XtionMoveClient'}, autonomy={'continue': Autonomy.Off}) # x:594 y:296 OperatableStateMachine.add('tts_FailDetection', hsr_TtsInputParameter(text="I cannot see the object, but I will try again.", language="en"), transitions={'completed': 'wait_5sec'}, autonomy={'completed': Autonomy.Off}) return _state_machine
def create(self): # x:30 y:365, x:130 y:365 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:412 y:45 OperatableStateMachine.add( 'hsr_SpeechRecognition', hsr_SpeechRecognition(topic="/julius/speech2text/en"), transitions={'recognition': 'hsr_SpcofInstruction'}, autonomy={'recognition': Autonomy.Off}, remapping={'recognition': 'recognition'}) # x:148 y:287 OperatableStateMachine.add( 'hsr_SpcofTraining', hsr_SpcofTraining(), transitions={'completed': 'hsr_training'}, autonomy={'completed': Autonomy.Off}, remapping={ 'request': 'output', 'spcof_text': 'spcof_text' }) # x:414 y:291 OperatableStateMachine.add('hsr_SpcofInstruction', hsr_SpcofInstruction(), transitions={ 'training': 'hsr_SpcofTraining', 'move': 'hsr_SpcofMove' }, autonomy={ 'training': Autonomy.Off, 'move': Autonomy.Off }, remapping={ 'input': 'recognition', 'output': 'output' }) # x:710 y:289 OperatableStateMachine.add('hsr_SpcofMove', hsr_SpcofMove(), transitions={ 'move': 'tts_move', 'failed': 'tts_cannot_move_2' }, autonomy={ 'move': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'move_name': 'output', 'move_text': 'move_text', 'move_pose': 'move_pose', 'arrive_text': 'arrive_text' }) # x:174 y:167 OperatableStateMachine.add( 'hsr_training', hsr_TtsInputKey(), transitions={'completed': 'hsr_SpeechRecognition'}, autonomy={'completed': Autonomy.Off}, remapping={'tts_text': 'spcof_text'}) # x:821 y:198 OperatableStateMachine.add( 'tts_move', hsr_TtsInputKey(), transitions={'completed': 'hsr_MoveBase'}, autonomy={'completed': Autonomy.Off}, remapping={'tts_text': 'move_text'}) # x:826 y:85 OperatableStateMachine.add('hsr_MoveBase', hsr_MoveBase(), transitions={ 'succeeded': 'tts_arrive', 'failed': 'tts_cannot_move_1' }, autonomy={ 'succeeded': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'request': 'move_pose'}) # x:630 y:17 OperatableStateMachine.add( 'tts_arrive', hsr_TtsInputKey(), transitions={'completed': 'hsr_SpeechRecognition'}, autonomy={'completed': Autonomy.Off}, remapping={'tts_text': 'arrive_text'}) # x:633 y:192 OperatableStateMachine.add( 'tts_cannot_move_2', hsr_TtsInputKey(), transitions={'completed': 'hsr_SpeechRecognition'}, autonomy={'completed': Autonomy.Off}, remapping={'tts_text': 'move_text'}) # x:626 y:101 OperatableStateMachine.add( 'tts_cannot_move_1', hsr_TtsInputParameter(text="I cannot move there.", language="en"), transitions={'completed': 'hsr_SpeechRecognition'}, autonomy={'completed': Autonomy.Off}) return _state_machine
def create(self): # x:686 y:318, x:620 y:319 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:514, x:130 y:514, x:230 y:514, x:330 y:514 _sm_grasp_phase_0 = OperatableStateMachine(outcomes=['succeeded', 'failed', 'planning_failed', 'control_failed'], input_keys=['move_pose', 'xtion_point', 'object', 'failed_objects'], output_keys=['selected_object_name', 'selected_object_status']) with _sm_grasp_phase_0: # x:57 y:44 OperatableStateMachine.add('hsr_MoveBase_grasp', hsr_MoveBase(), transitions={'succeeded': 'hsr_JointPose_xtion', 'failed': 'hsr_MoveBase_grasp'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'request': 'move_pose'}) # x:397 y:44 OperatableStateMachine.add('hsr_XtionPoint_grasp', hsr_XtionPoint(ref_frame_id="map"), transitions={'finish': 'hsr_ImageCapture'}, autonomy={'finish': Autonomy.Off}, remapping={'xtion_point': 'xtion_point'}) # x:776 y:42 OperatableStateMachine.add('hsr_ImageCapture', hsr_ImageCapture(rgb_topic="/hsrb/head_rgbd_sensor/rgb/image_rect_color", depth_topic="/hsrb/head_rgbd_sensor/depth_registered/image_rect_raw", camera_info_topic="/hsrb/head_rgbd_sensor/rgb/camera_info"), transitions={'completed': 'hsr_BoundingBox2D', 'failed': 'hsr_ImageCapture'}, autonomy={'completed': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'rgb_image': 'rgb_image', 'depth_image': 'depth_image', 'camera_info': 'camera_info'}) # x:935 y:41 OperatableStateMachine.add('hsr_BoundingBox2D', hsr_BoundingBox2D(output_topic="/bounding_box_2d_monitor"), transitions={'completed': 'hsr_GraspingPointDetect', 'failed': 'hsr_BoundingBox2D'}, autonomy={'completed': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'rgb_image': 'rgb_image', 'depth_image': 'depth_image', 'camera_info': 'camera_info', 'detection': 'detection', 'grasping_point': 'grasping_point'}) # x:1107 y:40 OperatableStateMachine.add('hsr_GraspingPointDetect', hsr_GraspingPointDetect(output_topic="/bounding_box_2d_monitor"), transitions={'completed': 'hsr_Tf2', 'failed': 'hsr_GraspingPointDetect'}, autonomy={'completed': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'rgb_image': 'rgb_image', 'depth_image': 'depth_image', 'camera_info': 'camera_info', 'detection': 'detection', 'grasping_point': 'grasping_point'}) # x:1289 y:40 OperatableStateMachine.add('hsr_Tf2', hsr_Tf2(before="head_rgbd_sensor_rgb_frame", after="map"), transitions={'continue': 'hsr_PoseDecision', 'failed': 'hsr_Tf2'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'before_pose': 'grasping_point', 'after_pose': 'after_pose'}) # x:1261 y:139 OperatableStateMachine.add('hsr_PoseDecision', hsr_PoseDecision(offset=None), transitions={'continue': 'hsr_ViewMarker', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'target_poses': 'after_pose', 'category': 'object', 'failed_objects': 'failed_objects', 'selected_pose_approach': 'selected_pose_approach', 'selected_pose_grasp': 'selected_pose_grasp', 'selected_object_name': 'selected_object_name', 'selected_object_status': 'selected_object_status'}) # x:931 y:140 OperatableStateMachine.add('hsr_OpenHand_grasp', hsr_GrippingObject(grasp_force=0.7, mode=False), transitions={'continue': 'hsr_JointPose_OpenHand', 'failed': 'hsr_OpenHand_grasp'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'grasp_success': 'grasp_success'}) # x:514 y:140 OperatableStateMachine.add('hsr_MoveitToPoseGoalAction_grasp', hsr_MoveitToPoseGoalAction(move_group='whole_body', action_topic='/move_group'), transitions={'reached': 'hsr_CloseHand', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off}, remapping={'pose_goal': 'selected_pose_grasp', 'move_status': 'selected_object_status'}) # x:318 y:142 OperatableStateMachine.add('hsr_CloseHand', hsr_GrippingObject(grasp_force=0.7, mode=True), transitions={'continue': 'hsr_MoveBase_grasp_back', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'grasp_success': 'selected_object_status'}) # x:243 y:43 OperatableStateMachine.add('hsr_JointPose_xtion', hsr_JointPose(arm_lift_joint=0.4, arm_flex_joint=-2.619, arm_roll_joint=0.0, wrist_flex_joint=-3.14/2, wrist_roll_joint=0.0, head_pan_joint=0.0, head_tilt_joint=0.0), transitions={'continue': 'hsr_XtionPoint_grasp'}, autonomy={'continue': Autonomy.Off}) # x:743 y:140 OperatableStateMachine.add('hsr_JointPose_OpenHand', hsr_JointPose(arm_lift_joint=0.4, arm_flex_joint=-3.14/4, arm_roll_joint=0.0, wrist_flex_joint=-3.14/4, wrist_roll_joint=0.0, head_pan_joint=0.0, head_tilt_joint=0.0), transitions={'continue': 'hsr_MoveitToPoseGoalAction_grasp'}, autonomy={'continue': Autonomy.Off}) # x:1110 y:139 OperatableStateMachine.add('hsr_ViewMarker', hsr_ViewMarker(), transitions={'continue': 'hsr_OpenHand_grasp', 'failed': 'hsr_ViewMarker'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'view_pose': 'selected_pose_grasp'}) # x:30 y:144 OperatableStateMachine.add('hsr_MoveBase_grasp_back', hsr_MoveBase(), transitions={'succeeded': 'succeeded', 'failed': 'failed'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'request': 'move_pose'}) # x:30 y:514, x:130 y:514, x:230 y:514 _sm_put_phase_1 = OperatableStateMachine(outcomes=['control_failed', 'succeeded', 'failed'], input_keys=['move_pose', 'xtion_point', 'arm_pose'], output_keys=['selected_object_name']) with _sm_put_phase_1: # x:30 y:152 OperatableStateMachine.add('hsr_MoveBase_put', hsr_MoveBase(), transitions={'succeeded': 'hsr_XtionPoint_put', 'failed': 'hsr_MoveBase_put'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'request': 'move_pose'}) # x:233 y:151 OperatableStateMachine.add('hsr_XtionPoint_put', hsr_XtionPoint(ref_frame_id="map"), transitions={'finish': 'hsr_MoveitToPoseGoalAction_put'}, autonomy={'finish': Autonomy.Off}, remapping={'xtion_point': 'xtion_point'}) # x:445 y:40 OperatableStateMachine.add('hsr_MoveitToPoseGoalAction_put', hsr_MoveitToPoseGoalAction(move_group='whole_body', action_topic='/move_group'), transitions={'reached': 'hsr_OpenHand_put', 'planning_failed': 'hsr_MoveitToPoseGoalAction_put', 'control_failed': 'control_failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off}, remapping={'pose_goal': 'arm_pose', 'move_status': 'selected_object_name'}) # x:233 y:43 OperatableStateMachine.add('hsr_OpenHand_put', hsr_GrippingObject(grasp_force=0.7, mode=False), transitions={'continue': 'hsr_MoveBase_put_back', 'failed': 'hsr_OpenHand_put'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'grasp_success': 'grasp_success'}) # x:30 y:43 OperatableStateMachine.add('hsr_MoveBase_put_back', hsr_MoveBase(), transitions={'succeeded': 'succeeded', 'failed': 'failed'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'request': 'move_pose'}) with _state_machine: # x:30 y:26 OperatableStateMachine.add('hsr_SpeechRecognition', hsr_SpeechRecognition(topic="/test"), transitions={'recognition': 'hsr_TidyUpDummy'}, autonomy={'recognition': Autonomy.Off}, remapping={'recognition': 'recognition'}) # x:37 y:237 OperatableStateMachine.add('hsr_TidyUpStage2', hsr_TidyUpStage2(), transitions={'retry': 'hsr_SpeechRecognition', 'grasp': 'grasp_phase', 'put': 'put_phase', 'kitchen': 'hsr_MoveBase_kitchen'}, autonomy={'retry': Autonomy.Off, 'grasp': Autonomy.Off, 'put': Autonomy.Off, 'kitchen': Autonomy.Off}, remapping={'recognition': 'recognition', 'grasp_object': 'selected_object_name', 'grasp_status': 'selected_object_status', 'move_pose': 'move_pose', 'xtion_point': 'xtion_point', 'arm_pose': 'arm_pose', 'object': 'object', 'failed_objects': 'failed_objects'}) # x:453 y:24 OperatableStateMachine.add('hsr_MoveBase_kitchen', hsr_MoveBase(), transitions={'succeeded': 'hsr_TtsInputParameter', 'failed': 'hsr_MoveBase_kitchen'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'request': 'move_pose'}) # x:241 y:25 OperatableStateMachine.add('hsr_TtsInputParameter', hsr_TtsInputParameter(text="discard or tidy up?", language="en"), transitions={'completed': 'hsr_SpeechRecognition'}, autonomy={'completed': Autonomy.Off}) # x:140 y:119 OperatableStateMachine.add('hsr_TidyUpDummy', hsr_TidyUpDummy(), transitions={'continue': 'hsr_TidyUpStage2'}, autonomy={'continue': Autonomy.Off}, remapping={'dummy1': 'selected_object_name', 'dummy2': 'selected_object_status'}) # x:336 y:425 OperatableStateMachine.add('put_phase', _sm_put_phase_1, transitions={'control_failed': 'failed', 'succeeded': 'hsr_TidyUpStage2', 'failed': 'hsr_TidyUpStage2'}, autonomy={'control_failed': Autonomy.Inherit, 'succeeded': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'move_pose': 'move_pose', 'xtion_point': 'xtion_point', 'arm_pose': 'arm_pose', 'selected_object_name': 'selected_object_name'}) # x:333 y:189 OperatableStateMachine.add('grasp_phase', _sm_grasp_phase_0, transitions={'succeeded': 'hsr_TidyUpStage2', 'failed': 'hsr_TidyUpStage2', 'planning_failed': 'hsr_TidyUpStage2', 'control_failed': 'failed'}, autonomy={'succeeded': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'planning_failed': Autonomy.Inherit, 'control_failed': Autonomy.Inherit}, remapping={'move_pose': 'move_pose', 'xtion_point': 'xtion_point', 'object': 'object', 'failed_objects': 'failed_objects', 'selected_object_name': 'selected_object_name', 'selected_object_status': 'selected_object_status'}) return _state_machine
def create(self): # x:686 y:318, x:620 y:319 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:514, x:130 y:514, x:230 y:514, x:330 y:514 _sm_grasp_phase_0 = OperatableStateMachine( outcomes=[ 'succeeded', 'failed', 'planning_failed', 'control_failed' ], input_keys=[ 'move_pose', 'xtion_point', 'object', 'failed_objects' ], output_keys=['selected_object_name', 'selected_object_status']) with _sm_grasp_phase_0: # x:32 y:41 OperatableStateMachine.add('hsr_MoveBase_grasp', hsr_MoveBase(), transitions={ 'succeeded': 'hsr_JointPose_initial', 'failed': 'hsr_MoveBase_grasp' }, autonomy={ 'succeeded': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'request': 'move_pose'}) # x:1331 y:34 OperatableStateMachine.add( 'hsr_XtionPoint_grasp', hsr_XtionPoint(ref_frame_id="map"), transitions={'finish': 'hsr_ImageCapture'}, autonomy={'finish': Autonomy.Off}, remapping={'xtion_point': 'xtion_point'}) # x:1510 y:35 OperatableStateMachine.add( 'hsr_ImageCapture', hsr_ImageCapture( rgb_topic="/hsrb/head_rgbd_sensor/rgb/image_rect_color", depth_topic= "/hsrb/head_rgbd_sensor/depth_registered/image_rect_raw", camera_info_topic="/hsrb/head_rgbd_sensor/rgb/camera_info" ), transitions={ 'completed': 'hsr_BoundingBox2D', 'failed': 'hsr_ImageCapture' }, autonomy={ 'completed': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'rgb_image': 'rgb_image', 'depth_image': 'depth_image', 'camera_info': 'camera_info' }) # x:1665 y:36 OperatableStateMachine.add( 'hsr_BoundingBox2D', hsr_BoundingBox2D(output_topic="/bounding_box_2d_monitor"), transitions={ 'completed': 'hsr_GraspingPointDetect', 'failed': 'hsr_BoundingBox2D' }, autonomy={ 'completed': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'rgb_image': 'rgb_image', 'depth_image': 'depth_image', 'camera_info': 'camera_info', 'detection': 'detection', 'grasping_point': 'grasping_point' }) # x:1832 y:36 OperatableStateMachine.add( 'hsr_GraspingPointDetect', hsr_GraspingPointDetect( output_topic="/bounding_box_2d_monitor", save=True), transitions={ 'completed': 'hsr_Tf2', 'failed': 'hsr_GraspingPointDetect' }, autonomy={ 'completed': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'rgb_image': 'rgb_image', 'depth_image': 'depth_image', 'camera_info': 'camera_info', 'detection': 'detection', 'grasping_point': 'grasping_point' }) # x:1861 y:139 OperatableStateMachine.add( 'hsr_Tf2', hsr_Tf2(before="head_rgbd_sensor_rgb_frame", after="map"), transitions={ 'continue': 'hsr_ClearBBoxOctomap_grasp', 'failed': 'hsr_Tf2' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'before_pose': 'grasping_point', 'after_pose': 'after_pose' }) # x:1261 y:139 OperatableStateMachine.add('hsr_PoseDecision', hsr_PoseDecision(offset=None), transitions={ 'continue': 'hsr_ViewMarker', 'failed': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'target_poses': 'after_pose', 'category': 'object', 'failed_objects': 'failed_objects', 'selected_pose_approach': 'selected_pose_approach', 'selected_pose_grasp': 'selected_pose_grasp', 'selected_object_name': 'selected_object_name', 'selected_object_status': 'selected_object_status' }) # x:931 y:140 OperatableStateMachine.add( 'hsr_OpenHand_grasp', hsr_GrippingObject(grasp_force=0.7, mode=False), transitions={ 'continue': 'hsr_JointPose_OpenHand', 'failed': 'hsr_OpenHand_grasp' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'grasp_success': 'grasp_success'}) # x:514 y:140 OperatableStateMachine.add( 'hsr_MoveitToPoseGoalAction_grasp', hsr_MoveitToPoseGoalAction(move_group='whole_body', action_topic='/move_group'), transitions={ 'reached': 'hsr_CloseHand', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off }, remapping={ 'pose_goal': 'selected_pose_grasp', 'move_status': 'selected_object_status' }) # x:318 y:142 OperatableStateMachine.add( 'hsr_CloseHand', hsr_GrippingObject(grasp_force=0.7, mode=True), transitions={ 'continue': 'hsr_MoveBase_grasp_back', 'failed': 'hsr_MoveBase_grasp2' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'grasp_success': 'selected_object_status'}) # x:1167 y:35 OperatableStateMachine.add( 'hsr_JointPose_xtion', hsr_JointPose(arm_lift_joint=0.4, arm_flex_joint=-2.619, arm_roll_joint=0.0, wrist_flex_joint=-3.14 / 2, wrist_roll_joint=0.0, head_pan_joint=0.0, head_tilt_joint=0.0), transitions={'continue': 'hsr_XtionPoint_grasp'}, autonomy={'continue': Autonomy.Off}) # x:743 y:153 OperatableStateMachine.add( 'hsr_JointPose_OpenHand', hsr_JointPose(arm_lift_joint=0.4, arm_flex_joint=-3.14 / 4, arm_roll_joint=0.0, wrist_flex_joint=-3.14 / 4, wrist_roll_joint=0.0, head_pan_joint=0.0, head_tilt_joint=0.0), transitions={'continue': 'hsr_MoveitToPoseGoalAction_grasp'}, autonomy={'continue': Autonomy.Off}) # x:1639 y:138 OperatableStateMachine.add( 'hsr_ClearBBoxOctomap_grasp', hsr_ClearBBoxOctomap(bbox_param_xy=0.15, bbox_param_z=0.3), transitions={ 'completed': 'hsr_PlanningSceneControl_grasp', 'failed': 'hsr_ClearBBoxOctomap_grasp' }, autonomy={ 'completed': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'object_point': 'after_pose'}) # x:348 y:38 OperatableStateMachine.add('hsr_ResetOctomap_grasp', hsr_ResetOctomap(play_octomap=True), transitions={ 'completed': 'hsr_JointPose_head_bellow_grasp', 'failed': 'hsr_ResetOctomap_grasp' }, autonomy={ 'completed': Autonomy.Off, 'failed': Autonomy.Off }) # x:1110 y:139 OperatableStateMachine.add( 'hsr_ViewMarker', hsr_ViewMarker(), transitions={ 'continue': 'hsr_OpenHand_grasp', 'failed': 'hsr_ViewMarker' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'view_pose': 'selected_pose_grasp'}) # x:1418 y:138 OperatableStateMachine.add( 'hsr_PlanningSceneControl_grasp', hsr_PlanningSceneControl(update_sw=False), transitions={ 'completed': 'hsr_PoseDecision', 'failed': 'hsr_PlanningSceneControl_grasp' }, autonomy={ 'completed': Autonomy.Off, 'failed': Autonomy.Off }) # x:30 y:144 OperatableStateMachine.add('hsr_MoveBase_grasp_back', hsr_MoveBase(), transitions={ 'succeeded': 'succeeded', 'failed': 'failed' }, autonomy={ 'succeeded': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'request': 'move_pose'}) # x:190 y:39 OperatableStateMachine.add( 'hsr_JointPose_initial', hsr_JointPose(arm_lift_joint=0.0, arm_flex_joint=0.0, arm_roll_joint=-3.14 / 2, wrist_flex_joint=-3.14 / 2, wrist_roll_joint=0.0, head_pan_joint=0.0, head_tilt_joint=0.0), transitions={'continue': 'hsr_ResetOctomap_grasp'}, autonomy={'continue': Autonomy.Off}) # x:533 y:39 OperatableStateMachine.add( 'hsr_JointPose_head_bellow_grasp', hsr_JointPose(arm_lift_joint=0.0, arm_flex_joint=0.0, arm_roll_joint=-3.14 / 2, wrist_flex_joint=-3.14 / 2, wrist_roll_joint=0.0, head_pan_joint=0.0, head_tilt_joint=-0.7), transitions={'continue': 'left_turn_grasp'}, autonomy={'continue': Autonomy.Off}) # x:750 y:39 OperatableStateMachine.add( 'left_turn_grasp', hsr_OmniBase(x=0.0, y=0.0, yaw=3.14 / 9, time_out=30.0), transitions={'finish': 'right_turn_grasp'}, autonomy={'finish': Autonomy.Off}) # x:898 y:36 OperatableStateMachine.add( 'right_turn_grasp', hsr_OmniBase(x=0.0, y=0.0, yaw=-3.14 / 4.5, time_out=30.0), transitions={'finish': 'front_turn_grasp'}, autonomy={'finish': Autonomy.Off}) # x:1032 y:39 OperatableStateMachine.add( 'front_turn_grasp', hsr_OmniBase(x=0.0, y=0.0, yaw=3.14 / 9, time_out=30.0), transitions={'finish': 'hsr_JointPose_xtion'}, autonomy={'finish': Autonomy.Off}) # x:191 y:285 OperatableStateMachine.add('hsr_MoveBase_grasp2', hsr_MoveBase(), transitions={ 'succeeded': 'failed', 'failed': 'hsr_MoveBase_grasp2' }, autonomy={ 'succeeded': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'request': 'move_pose'}) # x:543 y:350, x:46 y:336, x:254 y:359 _sm_put_phase_1 = OperatableStateMachine( outcomes=['control_failed', 'succeeded', 'failed'], input_keys=['move_pose', 'xtion_point', 'arm_pose'], output_keys=['selected_object_name']) with _sm_put_phase_1: # x:49 y:29 OperatableStateMachine.add('hsr_MoveBase_put', hsr_MoveBase(), transitions={ 'succeeded': 'hsr_XtionPoint_put', 'failed': 'hsr_MoveBase_put' }, autonomy={ 'succeeded': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'request': 'move_pose'}) # x:227 y:32 OperatableStateMachine.add( 'hsr_XtionPoint_put', hsr_XtionPoint(ref_frame_id="map"), transitions={'finish': 'hsr_ResetOctomap_put'}, autonomy={'finish': Autonomy.Off}, remapping={'xtion_point': 'xtion_point'}) # x:495 y:139 OperatableStateMachine.add( 'hsr_MoveitToPoseGoalAction_put', hsr_MoveitToPoseGoalAction(move_group='whole_body', action_topic='/move_group'), transitions={ 'reached': 'hsr_OpenHand_put', 'planning_failed': 'hsr_MoveitToPoseGoalAction_put', 'control_failed': 'control_failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off }, remapping={ 'pose_goal': 'arm_pose', 'move_status': 'selected_object_name' }) # x:275 y:141 OperatableStateMachine.add( 'hsr_OpenHand_put', hsr_GrippingObject(grasp_force=0.7, mode=False), transitions={ 'continue': 'hsr_MoveBase_put_back', 'failed': 'hsr_OpenHand_put' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'grasp_success': 'grasp_success'}) # x:66 y:142 OperatableStateMachine.add('hsr_MoveBase_put_back', hsr_MoveBase(), transitions={ 'succeeded': 'succeeded', 'failed': 'failed' }, autonomy={ 'succeeded': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'request': 'move_pose'}) # x:436 y:34 OperatableStateMachine.add('hsr_ResetOctomap_put', hsr_ResetOctomap(play_octomap=True), transitions={ 'completed': 'hsr_JointPose_head_bellow_put', 'failed': 'hsr_ResetOctomap_put' }, autonomy={ 'completed': Autonomy.Off, 'failed': Autonomy.Off }) # x:1071 y:135 OperatableStateMachine.add('hsr_ClearBBoxOctomap_put', hsr_ClearBBoxOctomap(bbox_param_xy=0.25, bbox_param_z=0.25), transitions={ 'completed': 'hsr_JointPose_put', 'failed': 'hsr_ClearBBoxOctomap_put' }, autonomy={ 'completed': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'object_point': 'arm_pose'}) # x:736 y:141 OperatableStateMachine.add( 'hsr_PlanningSceneControl_put', hsr_PlanningSceneControl(update_sw=False), transitions={ 'completed': 'hsr_MoveitToPoseGoalAction_put', 'failed': 'hsr_PlanningSceneControl_put' }, autonomy={ 'completed': Autonomy.Off, 'failed': Autonomy.Off }) # x:621 y:30 OperatableStateMachine.add( 'hsr_JointPose_head_bellow_put', hsr_JointPose(arm_lift_joint=0.0, arm_flex_joint=0.0, arm_roll_joint=-3.14 / 2, wrist_flex_joint=-3.14 / 2, wrist_roll_joint=0.0, head_pan_joint=0.0, head_tilt_joint=-0.7), transitions={'continue': 'left_turn_put'}, autonomy={'continue': Autonomy.Off}) # x:842 y:30 OperatableStateMachine.add( 'left_turn_put', hsr_OmniBase(x=0.0, y=0.0, yaw=3.14 / 9, time_out=30.0), transitions={'finish': 'right_turn_put'}, autonomy={'finish': Autonomy.Off}) # x:981 y:30 OperatableStateMachine.add( 'right_turn_put', hsr_OmniBase(x=0.0, y=0.0, yaw=-3.14 / 4.5, time_out=30.0), transitions={'finish': 'front_turn_put'}, autonomy={'finish': Autonomy.Off}) # x:1125 y:26 OperatableStateMachine.add( 'front_turn_put', hsr_OmniBase(x=0.0, y=0.0, yaw=3.14 / 9, time_out=30.0), transitions={'finish': 'hsr_ClearBBoxOctomap_put'}, autonomy={'finish': Autonomy.Off}) # x:934 y:144 OperatableStateMachine.add( 'hsr_JointPose_put', hsr_JointPose(arm_lift_joint=0.0, arm_flex_joint=-3.14 / 6, arm_roll_joint=0.0, wrist_flex_joint=-3.14 / 3, wrist_roll_joint=0.0, head_pan_joint=0.0, head_tilt_joint=0.0), transitions={'continue': 'hsr_PlanningSceneControl_put'}, autonomy={'continue': Autonomy.Off}) with _state_machine: # x:23 y:20 OperatableStateMachine.add( 'hsr_SpeechRecognitionByHeadMic', hsr_SpeechRecognitionByHeadMic(), transitions={'recognition': 'hsr_TidyUpDummy'}, autonomy={'recognition': Autonomy.Off}, remapping={'recognition': 'recognition'}) # x:453 y:24 OperatableStateMachine.add('hsr_MoveBase_kitchen', hsr_MoveBase(), transitions={ 'succeeded': 'hsr_SpeechRecognitionByHeadMic', 'failed': 'hsr_MoveBase_kitchen' }, autonomy={ 'succeeded': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'request': 'move_pose'}) # x:198 y:99 OperatableStateMachine.add( 'hsr_TidyUpDummy', hsr_TidyUpDummy(), transitions={'continue': 'hsr_TidyUpStage2'}, autonomy={'continue': Autonomy.Off}, remapping={ 'dummy1': 'selected_object_name', 'dummy2': 'selected_object_status' }) # x:311 y:311 OperatableStateMachine.add( 'hsr_PlanningSceneControl_True', hsr_PlanningSceneControl(update_sw=True), transitions={ 'completed': 'hsr_TidyUpStage2', 'failed': 'hsr_PlanningSceneControl_True' }, autonomy={ 'completed': Autonomy.Off, 'failed': Autonomy.Off }) # x:336 y:425 OperatableStateMachine.add('put_phase', _sm_put_phase_1, transitions={ 'control_failed': 'failed', 'succeeded': 'hsr_PlanningSceneControl_True', 'failed': 'hsr_PlanningSceneControl_True' }, autonomy={ 'control_failed': Autonomy.Inherit, 'succeeded': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'move_pose': 'move_pose', 'xtion_point': 'xtion_point', 'arm_pose': 'arm_pose', 'selected_object_name': 'selected_object_name' }) # x:333 y:189 OperatableStateMachine.add('grasp_phase', _sm_grasp_phase_0, transitions={ 'succeeded': 'hsr_PlanningSceneControl_True', 'failed': 'hsr_PlanningSceneControl_True', 'planning_failed': 'hsr_TidyUpStage2', 'control_failed': 'failed' }, autonomy={ 'succeeded': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'planning_failed': Autonomy.Inherit, 'control_failed': Autonomy.Inherit }, remapping={ 'move_pose': 'move_pose', 'xtion_point': 'xtion_point', 'object': 'object', 'failed_objects': 'failed_objects', 'selected_object_name': 'selected_object_name', 'selected_object_status': 'selected_object_status' }) # x:9 y:114 OperatableStateMachine.add( 'tts_RetrySpeechRecognition', hsr_TtsInputParameter(text="Could you repeat one more time?", language="en"), transitions={'completed': 'hsr_SpeechRecognitionByHeadMic'}, autonomy={'completed': Autonomy.Off}) # x:59 y:278 OperatableStateMachine.add('hsr_TidyUpStage2', hsr_TidyUpStage2(), transitions={ 'retry': 'tts_RetrySpeechRecognition', 'grasp': 'grasp_phase', 'put': 'put_phase', 'kitchen': 'hsr_MoveBase_kitchen' }, autonomy={ 'retry': Autonomy.Off, 'grasp': Autonomy.Off, 'put': Autonomy.Off, 'kitchen': Autonomy.Off }, remapping={ 'recognition': 'recognition', 'grasp_object': 'selected_object_name', 'grasp_status': 'selected_object_status', 'move_pose': 'move_pose', 'xtion_point': 'xtion_point', 'arm_pose': 'arm_pose', 'object': 'object', 'failed_objects': 'failed_objects' }) return _state_machine