예제 #1
0
def convert_tf_to_csv(yalm_file_dir, filename, s_grasp):
    """
    Read the tf yaml file, calculate transforms for links, create a header for the file and save it to a csv
    :param yalm_file_dir: directory of the yaml file
    :param filename: name of the yaml file
    :param s_grasp: successful grasp value
    :return: None - create a csv file and saves the information to it
    """

    # header
    header = get_header(filename)

    # read file
    yalm_object = open(yalm_file_dir)
    print 'reading {0}'.format(filename)

    # variable to keep all the data points
    dataset = np.empty(shape=(0, 0), dtype='object')

    # binary flag used to know when to start collecting data
    val_flag = False
    # variable used to keep all the information for the row of the file
    data_point = np.zeros(shape=(1, 9), dtype='object')
    # keep track of initial time
    time_step = list()

    for row_index, line in enumerate(yalm_object):

        # if reading on line for link2, turn on flag in order to start collecting data
        if 'child_frame_id: staubli_rx60l_link2' in line:
            transform_data = Transform()
            val_flag = True

        elif 'secs' in line and val_flag:

            val = re.search('\s*secs:\s(.*)\\n', line).group(1)

            time_step.append(val)

            if len(time_step) == 2:
                # switch the nsecs with the secs values in order to get the right datetime
                time_step[0], time_step[1] = time_step[1], time_step[0]
                timestep = ''.join(time_step)
                # seconds have to go before all the other fingers and palm values
                data_point[0, 0] = Decimal(timestep)

        elif ('w:' in line or 'x:' in line or 'y:' in line
              or 'z:' in line) and val_flag:
            # extract and collect the sensor value
            val = Decimal(re.search('\s *.:\s(.*)\\n', line).group(1))
            transform_data.add(val)

            # check if all the necessary values have been added
            if transform_data.all_data_included():
                tmp_data_point = list(
                    transform_data.get_rotation_translation())
                data_point[0, 1:-1] = np.array(tmp_data_point, dtype='object')
                data_point[0, -1] = s_grasp
                dataset = append_to_dataset(data_matrix=dataset,
                                            r_data=data_point,
                                            filename=filename,
                                            n_row=row_index,
                                            n_cols=9)

                # reset variables
                val_flag = False
                time_step = list()
                data_point = np.zeros(shape=(1, 9), dtype='object')
                transform_data = Transform()

    return header, dataset