def load_obstacle_detection_params(filename=None):
if filename:
params_file = get_package_path('mdr_wrs_tidy_up', 'config', filename)
else:
params_file = get_package_path('mdr_wrs_tidy_up', 'config',
'object_detection_params.yaml')
params = load_yaml_file(params_file)
if params:
return params
else:
rospy.logerr(
"Unable to load object_detection_params from file {0}".format(
params_file))
return None
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(
self,
'select_storage_location',
save_sm_state=save_sm_state,
input_keys=['grasped_object'],
output_keys=['destination_locations', 'storage_location'],
outcomes=['succeeded'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'select_storage_location')
self.storage_locations_file = kwargs.get('storage_locations_file',
'storage_location.yaml')
self.available_trays = kwargs.get('available_trays',
['tray_1', 'tray_2'])
self.tray_locations = kwargs.get('tray_locations', {
'tray_1': 'long_table_a_trays',
'tray_2': 'long_table_a_trays'
})
self.available_bins = kwargs.get('available_bins', ['bin_a', 'bin_b'])
self.bin_locations = kwargs.get('bin_locations', {
'bin_a': 'bin_a',
'bin_b': 'bin_b'
})
self.default_storage_location = kwargs.get('default_storage_location',
'bin')
storage_locations_path = get_package_path('mdr_wrs_tidy_up', 'config',
self.storage_locations_file)
if not self.storage_locations_file:
raise ValueError(
'[{0}] Argument "storage_locations_file" cannot be empty'.
format(self.state_name))
self.storage_locations_map = load_yaml_file(storage_locations_path)
def get_environment_objects(planning_scene_map_file):
object_list = ObjectList()
scene_file_path = get_package_path('mdr_wrs_tidy_up', 'config',
planning_scene_map_file)
scene_data = load_yaml_file(scene_file_path)
frame_id = scene_data['frame_id']
for obj_data in scene_data['objects']:
obj = Object()
obj.name = obj_data['name']
obj.pose.header.frame_id = frame_id
euler_orientation = obj_data['orientation']
quaternion = tf.transformations.quaternion_from_euler(
*euler_orientation)
obj.pose.pose = Pose(Point(*obj_data['position']),
Quaternion(*quaternion))
obj.dimensions.header.frame_id = frame_id
obj.dimensions.vector = Vector3(*obj_data['dimensions'])
obj.bounding_box.center = Point(*obj_data['position'])
obj.bounding_box.dimensions = Vector3(*obj_data['dimensions'])
object_list.objects.append(obj)
return object_list
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(
self,
'receive_information',
save_sm_state=save_sm_state,
input_keys=[],
output_keys=['target_entity'],
outcomes=['succeeded', 'failed', 'failed_after_retrying'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'receive_information')
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.timeout = kwargs.get('timeout', 25)
self.threshold = kwargs.get('threshold', 0.68)
# Load the rasa model
model_directory = get_package_path('rasa_nlu_models', 'common',
'where_is_this', 'nlu')
self.interpreter = Interpreter.load(model_directory)
# wait for listen action server
self.listen_client = actionlib.SimpleActionClient(
"listen_server", ListenAction)
listen_wait_result = self.listen_client.wait_for_server(
timeout=rospy.Duration(self.timeout))
if not listen_wait_result:
raise RuntimeError('failed to wait for "listen_server" action')
# Speech Matcher
self.speech_matcher = SpeechMatching()
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self,
'retrieve_information',
save_sm_state=save_sm_state,
outcomes=['succeeded', 'failed'],
input_keys=['command'])
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.timeout = kwargs.get('timeout', 120.)
self.threshold = kwargs.get('threshold', 0.68)
# Load rasa nlu model
model_directory = get_package_path('rasa_nlu_models', 'common',
'mdr_hri_behaviours_models', 'nlu')
self.interpreter = Interpreter.load(model_directory)