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:38 y:102
OperatableStateMachine.add(
'hsr_JointPoseMemo',
hsr_JointPose(arm_lift_joint=0.4,
arm_flex_joint=-3.14 / 3 * 2,
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.35),
transitions={'continue': 'hsr_ImageCaptureMemo'},
autonomy={'continue': Autonomy.Off})
# x:604 y:44
OperatableStateMachine.add(
'hsr_LabelArrangement',
hsr_LabelArrangement(
path=
'/root/HSR/catkin_ws/src/em_spco_formation/training_data/default/place2object.txt'
),
transitions={
'continue': 'finished',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'detection_result': 'detection_memo'})
# x:386 y:44
OperatableStateMachine.add(
'hsr_BoundingBox2DMemo',
hsr_BoundingBox2D(output_topic="/bounding_box_2d_monitor"),
transitions={
'completed': 'hsr_LabelArrangement',
'failed': 'failed'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'rgb_image': 'rgb_image_memo',
'depth_image': 'depth_image_memo',
'camera_info': 'camera_info_memo',
'detection': 'detection_memo'
})
# x:195 y:46
OperatableStateMachine.add(
'hsr_ImageCaptureMemo',
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_BoundingBox2DMemo',
'failed': 'failed'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'rgb_image': 'rgb_image_memo',
'depth_image': 'depth_image_memo',
'camera_info': 'camera_info_memo'
})
return _state_machine
def create(self):
# x:1258 y:287, x:1255 y:399
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:54 y:72
OperatableStateMachine.add(
'hsr_JointPose_FindObject',
hsr_JointPose(arm_lift_joint=0.0,
arm_flex_joint=0.0,
arm_roll_joint=0.0,
wrist_flex_joint=-3.14 / 2,
wrist_roll_joint=0.0,
head_pan_joint=-1.3,
head_tilt_joint=-0.8),
transitions={'continue': 'hsr_SpeechRecognition'},
autonomy={'continue': Autonomy.Off})
# x:1046 y:79
OperatableStateMachine.add(
'hsr_ViewMarkerArray_1',
hsr_ViewMarkerArray(),
transitions={'continue': 'hsr_ImageCapture'},
autonomy={'continue': Autonomy.Off},
remapping={'object_points': 'greeeedy'})
# x:272 y:163
OperatableStateMachine.add('hsr_SetObjectState_1',
hsr_SetObjectState(),
transitions={
'completed':
'hsr_CountingAddReset_1',
'failed': 'hsr_SetObjectState_1'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'object_name': 'tidy_order',
'object_pose': 'tidy_pose'
})
# x:1046 y:332
OperatableStateMachine.add(
'hsr_SpaCoTyInstruction',
hsr_SpaCoTyInstruction(save_folder="sim_st1"),
transitions={
'datadata': 'finished',
'training': 'finished',
'tidy_baseline_nearly': 'hsr_MakeFile_Near',
'tidy_baseline_randomly': 'hsr_MakeFile_Random',
'tidy_greedy': 'hsr_MakeFile_Greedy',
'failed': 'failed',
'end': 'finished'
},
autonomy={
'datadata': Autonomy.Off,
'training': Autonomy.Off,
'tidy_baseline_nearly': Autonomy.Off,
'tidy_baseline_randomly': Autonomy.Off,
'tidy_greedy': Autonomy.Off,
'failed': Autonomy.Off,
'end': Autonomy.Off
},
remapping={
'sentence': 'sentence',
'move_name': 'move_name',
'save_folder': 'save_folder'
})
# x:914 y:483
OperatableStateMachine.add(
'hsr_GetObjectState_2',
hsr_GetObjectState(yolo_yaml="yolov3-isb-tidy.json"),
transitions={
'completed': 'hsr_ViewMarkerArray_2',
'failed': 'hsr_GetObjectState_2'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'load_file': 'load_file',
'object_states': 'neaaaar',
'object_states_init': 'init_neaaar'
})
# x:1055 y:644
OperatableStateMachine.add(
'hsr_GetObjectState_3',
hsr_GetObjectState(yolo_yaml="yolov3-isb-tidy.json"),
transitions={
'completed': 'hsr_ViewMarkerArray_3',
'failed': 'hsr_GetObjectState_3'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'load_file': 'load_file',
'object_states': 'randommmm',
'object_states_init': 'init_randommm'
})
# x:723 y:482
OperatableStateMachine.add(
'hsr_ViewMarkerArray_2',
hsr_ViewMarkerArray(),
transitions={
'continue': 'hsr_SelectTidyObjectandPosefromDatasetUltra'
},
autonomy={'continue': Autonomy.Off},
remapping={'object_points': 'neaaaar'})
# x:805 y:644
OperatableStateMachine.add(
'hsr_ViewMarkerArray_3',
hsr_ViewMarkerArray(),
transitions={
'continue': 'hsr_SelectTidyObjectandPoseRandomlyUltra'
},
autonomy={'continue': Autonomy.Off},
remapping={'object_points': 'randommmm'})
# x:254 y:478
OperatableStateMachine.add('hsr_SetObjectState_2',
hsr_SetObjectState(),
transitions={
'completed':
'hsr_CountingAddReset_2',
'failed': 'hsr_SetObjectState_2'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'object_name': 'tidy_name_db',
'object_pose': 'tidy_pose_db'
})
# x:316 y:642
OperatableStateMachine.add('hsr_SetObjectState_3',
hsr_SetObjectState(),
transitions={
'completed':
'hsr_CountingAddReset_3',
'failed': 'hsr_SetObjectState_3'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'object_name': 'tidy_name_random',
'object_pose': 'tidy_pose_random'
})
# x:1051 y:157
OperatableStateMachine.add(
'hsr_GetObjectState_1',
hsr_GetObjectState(yolo_yaml="yolov3-isb-tidy.json"),
transitions={
'completed': 'hsr_ViewMarkerArray_1',
'failed': 'hsr_GetObjectState_1'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'load_file': 'load_file',
'object_states': 'greeeedy',
'object_states_init': 'init_greeedy'
})
# x:510 y:643
OperatableStateMachine.add(
'hsr_SelectTidyObjectandPoseRandomlyUltra',
hsr_SelectTidyObjectandPoseRandomlyUltra(),
transitions={'completed': 'hsr_SetObjectState_3'},
autonomy={'completed': Autonomy.Off},
remapping={
'load_file': 'load_file',
'detect_objects': 'randommmm',
'tidy_name_random': 'tidy_name_random',
'tidy_pose_random': 'tidy_pose_random'
})
# x:424 y:481
OperatableStateMachine.add(
'hsr_SelectTidyObjectandPosefromDatasetUltra',
hsr_SelectTidyObjectandPosefromDatasetUltra(),
transitions={'completed': 'hsr_SetObjectState_2'},
autonomy={'completed': Autonomy.Off},
remapping={
'load_file': 'load_file',
'detect_objects': 'neaaaar',
'tidy_name_db': 'tidy_name_db',
'tidy_pose_db': 'tidy_pose_db'
})
# x:523 y:163
OperatableStateMachine.add('hsr_Switcher',
hsr_Switcher(),
transitions={
'true_load': 'hsr_SetObjectState_1',
'false_load': 'hsr_SetObjectState_1'
},
autonomy={
'true_load': Autonomy.Off,
'false_load': Autonomy.Off
},
remapping={'switch': 'name2place'})
# x:258 y:83
OperatableStateMachine.add('hsr_SpaCoTyUpdateTidyPose',
hsr_SpaCoTyUpdateTidyPose(),
transitions={
'completed': 'hsr_SetObjectState_1',
'failed':
'hsr_SpaCoTyUpdateTidyPose'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'object_name': 'tidy_order',
'tidy_pose_bef': 'tidy_pose',
'tidy_pose_aft': 'tidy_pose'
})
# x:579 y:329
OperatableStateMachine.add(
'hsr_OutputNum',
hsr_OutputNum(num=0),
transitions={'end': 'hsr_SpaCoTyInstruction'},
autonomy={'end': Autonomy.Off},
remapping={'count_num': 'init_num'})
# x:707 y:247
OperatableStateMachine.add('hsr_CountingAddReset_1',
hsr_CountingAddReset(max_count=10),
transitions={
'repetition':
'hsr_GetObjectState_1',
'end':
'hsr_ResetObjectStateRandomly_1'
},
autonomy={
'repetition': Autonomy.Off,
'end': Autonomy.Off
},
remapping={
'start_num': 'init_num',
'count_num': 'count_num'
})
# x:583 y:401
OperatableStateMachine.add('hsr_CountingAddReset_2',
hsr_CountingAddReset(max_count=10),
transitions={
'repetition':
'hsr_GetObjectState_2',
'end':
'hsr_ResetObjectStateRandomly_2'
},
autonomy={
'repetition': Autonomy.Off,
'end': Autonomy.Off
},
remapping={
'start_num': 'init_num',
'count_num': 'count_num'
})
# x:308 y:553
OperatableStateMachine.add('hsr_CountingAddReset_3',
hsr_CountingAddReset(max_count=10),
transitions={
'repetition':
'hsr_GetObjectState_3',
'end':
'hsr_ResetObjectStateRandomly_3'
},
autonomy={
'repetition': Autonomy.Off,
'end': Autonomy.Off
},
remapping={
'start_num': 'init_num',
'count_num': 'count_num'
})
# x:44 y:642
OperatableStateMachine.add(
'hsr_ResetObjectStateRandomly_3',
hsr_ResetObjectStateRandomly(),
transitions={
'completed': 'hsr_SpeechRecognition',
'failed': 'hsr_ResetObjectStateRandomly_3'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'object_states': 'init_randommm'})
# x:211 y:399
OperatableStateMachine.add(
'hsr_ResetObjectStateRandomly_2',
hsr_ResetObjectStateRandomly(),
transitions={
'completed': 'hsr_SpeechRecognition',
'failed': 'hsr_ResetObjectStateRandomly_2'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'object_states': 'init_neaaar'})
# x:256 y:244
OperatableStateMachine.add(
'hsr_ResetObjectStateRandomly_1',
hsr_ResetObjectStateRandomly(),
transitions={
'completed': 'hsr_SpeechRecognition',
'failed': 'hsr_ResetObjectStateRandomly_1'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'object_states': 'init_greeedy'})
# x:58 y:326
OperatableStateMachine.add(
'hsr_SpeechRecognition',
hsr_SpeechRecognition(topic="julius/speech2text/en"),
transitions={'recognition': 'hsr_OutputNum'},
autonomy={'recognition': Autonomy.Off},
remapping={'recognition': 'sentence'})
# x:921 y:403
OperatableStateMachine.add(
'hsr_MakeFile_Near',
hsr_MakeFile(save_file="sim_near_ultra"),
transitions={'completed': 'hsr_GetObjectState_2'},
autonomy={'completed': Autonomy.Off},
remapping={'load_file': 'load_file'})
# x:1051 y:553
OperatableStateMachine.add(
'hsr_MakeFile_Random',
hsr_MakeFile(save_file="sim_random_ultra"),
transitions={'completed': 'hsr_GetObjectState_3'},
autonomy={'completed': Autonomy.Off},
remapping={'load_file': 'load_file'})
# x:1054 y:235
OperatableStateMachine.add(
'hsr_MakeFile_Greedy',
hsr_MakeFile(save_file="plus_sim_greedy_ultra"),
transitions={'completed': 'hsr_GetObjectState_1'},
autonomy={'completed': Autonomy.Off},
remapping={'load_file': 'load_file'})
# x:900 y:79
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_GraspingPointDetectIsbPlus',
'failed': 'hsr_CaptureTime'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'rgb_image': 'rgb_image',
'depth_image': 'depth_image',
'camera_info': 'camera_info'
})
# x:902 y:15
OperatableStateMachine.add('hsr_CaptureTime',
hsr_Counting(max_count=2),
transitions={
'repetition': 'hsr_CaptureTime',
'end': 'hsr_ImageCapture'
},
autonomy={
'repetition': Autonomy.Off,
'end': Autonomy.Off
},
remapping={'count_num': 'capture_num'})
# x:689 y:13
OperatableStateMachine.add('hsr_DetectErrorIgnoreOnce',
hsr_Counting(max_count=2),
transitions={
'repetition':
'hsr_DetectErrorIgnoreOnce',
'end':
'hsr_GraspingPointDetectIsbPlus'
},
autonomy={
'repetition': Autonomy.Off,
'end': Autonomy.Off
},
remapping={'count_num': '_num'})
# x:679 y:83
OperatableStateMachine.add(
'hsr_GraspingPointDetectIsbPlus',
hsr_GraspingPointDetectIsbPlus(
output_topic="/bounding_box_2d_monitor",
resnet_model="resnet50",
save=True),
transitions={
'completed': 'hsr_SpaCoTyPlanningPlus',
'failed': 'hsr_GraspingPointDetectIsbPlus',
'yolo_failed': 'hsr_DetectErrorIgnoreOnce'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off,
'yolo_failed': Autonomy.Off
},
remapping={
'rgb_image': 'rgb_image',
'depth_image': 'depth_image',
'camera_info': 'camera_info',
'grasping_point': 'grasping_point',
'grasping_point_feature': 'grasping_point_feature'
})
# x:496 y:83
OperatableStateMachine.add(
'hsr_SpaCoTyPlanningPlus',
hsr_SpaCoTyPlanningPlus(),
transitions={'completed': 'hsr_Switcher'},
autonomy={'completed': Autonomy.Off},
remapping={
'load_file': 'load_file',
'detect_objects': 'greeeedy',
'detect_objects_feature': 'grasping_point_feature',
'tidy_pose': 'tidy_pose',
'tidy_order': 'tidy_order',
'name2place': 'name2place'
})
return _state_machine
def create(self):
# x:967 y:198, x:232 y:377
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:478, x:130 y:478
_sm_visualize_0 = OperatableStateMachine(
outcomes=['failed', 'continue'])
with _sm_visualize_0:
# x:30 y:41
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': 'failed'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'rgb_image': 'rgb_image',
'depth_image': 'depth_image',
'camera_info': 'camera_info'
})
# x:251 y:40
OperatableStateMachine.add(
'hsr_BoundingBox2D',
hsr_BoundingBox2D(output_topic="/bounding_box_2d_monitor"),
transitions={
'completed': 'hsr_GraspingPointDetect',
'failed': 'failed'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'rgb_image': 'rgb_image',
'depth_image': 'depth_image',
'camera_info': 'camera_info',
'detection': 'detection'
})
# x:440 y:43
OperatableStateMachine.add(
'hsr_GraspingPointDetect',
hsr_GraspingPointDetect(
output_topic="/bounding_box_2d_monitor", save=True),
transitions={
'completed': 'hsr_Tf2',
'failed': 'failed'
},
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:645 y:41
OperatableStateMachine.add('hsr_Tf2',
hsr_Tf2(
before="head_rgbd_sensor_rgb_frame",
after="map"),
transitions={
'continue': 'hsr_ViewDetection',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'before_pose': 'grasping_point',
'after_pose': 'after_pose'
})
# x:794 y:40
OperatableStateMachine.add(
'hsr_ViewDetection',
hsr_ViewDetection(),
transitions={'continue': 'continue'},
autonomy={'continue': Autonomy.Off},
remapping={'object_points': 'after_pose'})
with _state_machine:
# x:314 y:82
OperatableStateMachine.add('Visualize',
_sm_visualize_0,
transitions={
'failed': 'failed',
'continue': 'finished'
},
autonomy={
'failed': Autonomy.Inherit,
'continue': Autonomy.Inherit
})
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:478, x:130 y:478
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:478
_sm_newest_product_0 = OperatableStateMachine(
outcomes=['continue'],
input_keys=['object'],
output_keys=['selected_object_status'])
with _sm_newest_product_0:
# x:194 y:31
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:388 y:33
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'
})
# x:548 y:31
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:740 y:31
OperatableStateMachine.add('hsr_Tf2',
hsr_Tf2(
before='head_rgbd_sensor_rgb_frame',
after='map'),
transitions={
'continue': 'continue',
'failed': 'hsr_Tf2'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'before_pose': 'grasping_point',
'after_pose': 'after_pose'
})
with _state_machine:
# x:23 y:114
OperatableStateMachine.add(
'hsr_StringDummy',
hsr_StringDummy(text="bottle"),
transitions={'continue': 'Newest Product'},
autonomy={'continue': Autonomy.Off},
remapping={'name': 'object'})
# x:216 y:109
OperatableStateMachine.add('Newest Product',
_sm_newest_product_0,
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Inherit},
remapping={
'object':
'object',
'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: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
def create(self):
# x:225 y:613, x:1131 y:611
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:14 y:478
_sm_group_0 = OperatableStateMachine(outcomes=['continue'])
with _sm_group_0:
# x:78 y:63
OperatableStateMachine.add(
'hsr_JointPoseXtion',
hsr_JointPose(arm_lift_joint=0.4,
arm_flex_joint=-2.169,
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.35),
transitions={'continue': 'hsr_OpenHand'},
autonomy={'continue': Autonomy.Off})
# x:617 y:41
OperatableStateMachine.add(
'hsr_BoundingBox2D',
hsr_BoundingBox2D(output_topic="/bounding_box_2d_monitor"),
transitions={
'completed': 'hsr_GraspingPointDetect',
'failed': 'hsr_MoveNeutral'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'rgb_image': 'rgb_image',
'depth_image': 'depth_image',
'camera_info': 'camera_info',
'detection': 'detection'
})
# x:973 y:46
OperatableStateMachine.add('hsr_tf2',
hsr_Tf2(
before="head_rgbd_sensor_rgb_frame",
after="map"),
transitions={
'continue': 'hsr_PoseDummy',
'failed': 'hsr_MoveNeutral'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'before_pose': 'grasping_point',
'after_pose': 'target_poses'
})
# x:1049 y:220
OperatableStateMachine.add('hsr_PoseDecision',
hsr_PoseDecision(),
transitions={
'continue': 'hsr_Move2Center',
'failed': 'hsr_MoveNeutral'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'target_poses':
'target_poses',
'category':
'dummy_object',
'failed_objects':
'dummy_failed_objects',
'selected_pose_grasp':
'selected_pose_grasp',
'selected_object_name':
'selected_object_name',
'selected_object_status':
'selected_object_status'
})
# x:59 y:411
OperatableStateMachine.add(
'hsr_MoveitToPoseGoalAction',
hsr_MoveitToPoseGoalAction(move_group='whole_body',
action_topic='/move_group',
tolerance=0.001,
orien_tolerance=True),
transitions={
'reached': 'hsr_GrippingObject_close',
'planning_failed': 'hsr_MoveNeutral',
'control_failed': 'hsr_MoveNeutral'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'pose_goal': 'selected_pose_grasp',
'move_status': 'move_status'
})
# x:764 y:42
OperatableStateMachine.add(
'hsr_GraspingPointDetect',
hsr_GraspingPointDetect(
output_topic="/bounding_box_2d_monitor", save=True),
transitions={
'completed': 'hsr_tf2',
'failed': 'hsr_MoveNeutral'
},
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:1151 y:42
OperatableStateMachine.add(
'hsr_PoseDummy',
hsr_PoseDummy(),
transitions={'continue': 'hsr_InitDecision'},
autonomy={'continue': Autonomy.Off},
remapping={
'dummy_pose': 'dummy_pose',
'dummy_object': 'dummy_object',
'dummy_failed_objects': 'dummy_failed_objects'
})
# x:429 y:40
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_MoveNeutral'
},
autonomy={
'completed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'rgb_image': 'rgb_image',
'depth_image': 'depth_image',
'camera_info': 'camera_info'
})
# x:36 y:298
OperatableStateMachine.add(
'hsr_ViewMaker_grasp_point',
hsr_ViewMarker(),
transitions={
'continue': 'hsr_MoveitToPoseGoalAction',
'failed': 'hsr_MoveNeutral'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'view_pose': 'selected_pose_grasp'})
# x:59 y:489
OperatableStateMachine.add(
'hsr_GrippingObject_close',
hsr_GrippingObject(grasp_force=0.7, mode=True),
transitions={
'continue': 'hsr_JointPose_initial',
'failed': 'hsr_MoveNeutral'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'grasp_success': 'grasp_success'})
# x:60 y:566
OperatableStateMachine.add('hsr_JointPose_initial',
hsr_JointPose(arm_lift_joint=0.05,
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': 'continue'},
autonomy={'continue': Autonomy.Off})
# x:458 y:234
OperatableStateMachine.add(
'hsr_CollisionBox',
hsr_CollisionBox(offset_z=0.1,
offset_dist=0.1,
width=1.2,
mode='C'),
transitions={'continue': 'hsr_CreateApproach'},
autonomy={'continue': Autonomy.Off},
remapping={'box_pose': 'selected_pose_grasp'})
# x:978 y:533
OperatableStateMachine.add('hsr_MoveNeutral',
hsr_MoveNeutral(),
transitions={'continue': 'continue'},
autonomy={'continue': Autonomy.Off})
# x:217 y:72
OperatableStateMachine.add(
'hsr_OpenHand',
hsr_GrippingObject(grasp_force=0.7, mode=False),
transitions={
'continue': 'hsr_ImageCapture',
'failed': 'hsr_MoveNeutral'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'grasp_success': 'grasp_success'})
# x:30 y:226
OperatableStateMachine.add(
'hsr_MoveitToPoseAppAction',
hsr_MoveitToPoseGoalAction(move_group='whole_body',
action_topic='/move_group',
tolerance=0.001,
orien_tolerance=True),
transitions={
'reached': 'hsr_ViewMaker_grasp_point',
'planning_failed': 'hsr_MoveNeutral',
'control_failed': 'hsr_MoveNeutral'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'pose_goal': 'selected_pose_approach',
'move_status': 'move_status'
})
# x:604 y:239
OperatableStateMachine.add(
'hsr_JointPoseLine',
hsr_JointPoseLine(line_z=0.0),
transitions={'continue': 'hsr_CollisionBox'},
autonomy={'continue': Autonomy.Off},
remapping={'pose_goal': 'selected_pose_grasp'})
# x:1259 y:247
OperatableStateMachine.add(
'hsr_RemoveBox',
hsr_CollisionBox(offset_z=0.3,
offset_dist=0.1,
width=0.7,
mode='R'),
transitions={'continue': 'hsr_PoseDecision'},
autonomy={'continue': Autonomy.Off},
remapping={'box_pose': 'dummy_pose'})
# x:211 y:186
OperatableStateMachine.add(
'hsr_ViewMarker',
hsr_ViewMarker(),
transitions={
'continue': 'hsr_MoveitToPoseAppAction',
'failed': 'hsr_MoveNeutral'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'view_pose': 'selected_pose_approach'})
# x:757 y:237
OperatableStateMachine.add(
'hsr_ArmInit',
hsr_MoveNeutral(),
transitions={'continue': 'hsr_JointPoseLine'},
autonomy={'continue': Autonomy.Off})
# x:889 y:233
OperatableStateMachine.add(
'hsr_Move2Center',
hsr_Move2Center(before='map', after='base_footprint'),
transitions={
'continue': 'hsr_ArmInit',
'failed': 'hsr_MoveNeutral'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target_pose': 'selected_pose_grasp'})
# x:320 y:214
OperatableStateMachine.add('hsr_CreateApproach',
hsr_CreateApproach(offset=0.3),
transitions={
'continue': 'hsr_ViewMarker',
'failed': 'hsr_MoveNeutral'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'target_pose':
'selected_pose_grasp',
'selected_pose_approach':
'selected_pose_approach',
'selected_pose_grasp':
'selected_pose_grasp'
})
# x:1243 y:154
OperatableStateMachine.add(
'hsr_InitDecision',
hsr_MoveNeutral(),
transitions={'continue': 'hsr_RemoveBox'},
autonomy={'continue': Autonomy.Off})
with _state_machine:
# x:444 y:346
OperatableStateMachine.add('Group',
_sm_group_0,
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Inherit})
return _state_machine