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
