Ejemplo n.º 1
0
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)
Ejemplo n.º 3
0
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
Ejemplo n.º 4
0
    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()
Ejemplo n.º 5
0
    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)