#include "init_inputs.h" #include <iostream> #include <fstream> #include "type_define.h" #include "jf_utils.hpp" #define TIME_BUFFER 0.01 /////////////////////////////////////////////////////////// // Pos_Atti_Ini_File bool Pos_Atti_Ini_FileReader::parseFromString(std::string line, pos_atti_ini_RawData &pos_atti_ini_data) { std::vector<std::string> list = split_string(line, ','); if (list.size() >= 6) { pos_atti_ini_data.pos_ini[0] = atof(list[0].c_str()); pos_atti_ini_data.pos_ini[1] = atof(list[1].c_str()); pos_atti_ini_data.pos_ini[2] = atof(list[2].c_str()); pos_atti_ini_data.atti_ini[0] = atof(list[3].c_str()); pos_atti_ini_data.atti_ini[1] = atof(list[4].c_str()); pos_atti_ini_data.atti_ini[2] = atof(list[5].c_str()); return true; } return false; } int Pos_Atti_Ini_FileReader::loadPos_Atti_Ini_File(std::string pos_atti_ini_File, double start_time, double end_time) { char buf[1024]; std::ifstream ifs(pos_atti_ini_File); struct pos_atti_ini_RawData pos_atti_ini_data; // buffer start_time -= TIME_BUFFER; end_time += TIME_BUFFER; if (!ifs.is_open()) { std::cout << "error when opening pos_atti_ini file, TERMINATED!!!\n" << std::endl; throw; } while (!ifs.eof()) { ifs.getline(buf, 1024); std::string line(buf); Pos_Atti_Ini_FileReader::parseFromString(line, pos_atti_ini_data); raw_data.push_back(pos_atti_ini_data); } ifs.close(); return raw_data.size(); } /////////////////////////////////////////////////////////// // imu2base_File bool Imu2BaseFileReader::parseFromString(std::string line, Imu2BaseRawData &Imu2BaseRaw_data) { std::vector<std::string> list = split_string(line, ','); if (list.size() >= 3) { Imu2BaseRaw_data.imu2base_calib[0] = atof(list[0].c_str()); Imu2BaseRaw_data.imu2base_calib[1] = atof(list[1].c_str()); Imu2BaseRaw_data.imu2base_calib[2] = atof(list[2].c_str()); return true; } return false; } int Imu2BaseFileReader::loadImu2BaseFile(std::string Imu2BaseFile) { char buf[1024]; std::ifstream ifs(Imu2BaseFile); struct Imu2BaseRawData imu2base_data; if (!ifs.is_open()) { std::cout << "error when opening odom file, TERMINATED!!!\n" << std::endl; throw; } while (!ifs.eof()) { ifs.getline(buf, 1024); std::string line(buf); Imu2BaseFileReader::parseFromString(line, imu2base_data); raw_data.push_back(imu2base_data); } ifs.close(); return raw_data.size(); } //////////////////////////////////////////////////////////// // imu2base_lever_armFile bool Imu2Base_LeverArmFileReader::parseFromString(std::string line, Imu2Base_LeverArmRawData &Imu2Base_LeverArmRaw_data) { std::vector<std::string> list = split_string(line, ','); if (list.size() >= 3) { Imu2Base_LeverArmRaw_data.imu2base_lever_arm[0] = atof(list[0].c_str()); Imu2Base_LeverArmRaw_data.imu2base_lever_arm[1] = atof(list[1].c_str()); Imu2Base_LeverArmRaw_data.imu2base_lever_arm[2] = atof(list[2].c_str()); return true; } return false; } int Imu2Base_LeverArmFileReader::loadImu2Base_LeverArmFile(std::string Imu2BaseleverarmFile) { char buf[1024]; std::ifstream ifs(Imu2BaseleverarmFile); struct Imu2Base_LeverArmRawData Imu2Base_LeverArmRaw_data; if (!ifs.is_open()) { std::cout << "error when opening odom file, TERMINATED!!!\n" << std::endl; throw; } while (!ifs.eof()) { ifs.getline(buf, 1024); std::string line(buf); Imu2Base_LeverArmFileReader::parseFromString(line, Imu2Base_LeverArmRaw_data); raw_data.push_back(Imu2Base_LeverArmRaw_data); } ifs.close(); return raw_data.size(); } ////////////////////////////////////////////////////////////