基于激光雷达实现三边定位算法开发记录（六）——对拟定圆心进行保存和读取

22年暑假实习过程中第一个开发任务，基于激光雷达实现三边定位功能

开发平台为ubuntu 18.04 + ros melodic

日期：2022.7.26

本次实现：在完成圆心拟定后，下一步是进行反光柱的匹配，匹配的原理是将预先录入系统的反光柱之间的边长一一去和当前能识别到的反光柱之间的边长匹配，识别出所需要的反光柱。本次完成将上次通过三角函数法拟定圆心坐标数据保存到txt文件当中作为预先录入系统的反光柱坐标，然读取保存的文本，将其还原为坐标信息，以便后期去对比匹配。

参考：

ofstream的使用方法--超级精细

ifstream的使用

（一）代码编写

修改处理激光信息函数，将拟定的圆心数据传入文件读写函数。

//处理激光信息
void scan_CB(sensor_msgs::LaserScan scan) {                                          
  std::vector< Point > circle;                            //当前反光柱数据
  std::vector< std::pair< float, float > > coordinate;   //保存的反光柱坐标
  std::string fileaddress = "/home/ding/test_ws/src/HAL/trilateration/src/CircleData.txt";   //反光柱坐标数据文件地址

  bool fileoperation = false;  //true;            //文件读写标志为，true为写，false为读

  for (int i = 0; i < 5; i++) {
    circle = findCircle(scan);                            //获取各个反光柱圆心
  }
  FileOperation(fileaddress,circle,coordinate,fileoperation);        //对获取的反光柱数据进行读写文件处理
  for(long i = 0; i< coordinate.size(); i++){
  std::cout <<  coordinate[i].first <<"             " << coordinate[i].second << std::endl;
  }
}

新建文件读写函数，当写文件状态下时，将当前拟合圆心坐标数据存入文件；当读文件状态下时，读取文件内容，并将字符串信息还原为folat格式坐标信息，并通过指针传出以便后期处理。

//反光柱圆心数据文件读写函数
void FileOperation(std::string fileaddres,const std::vector< Point > point, std::vector< std::pair< float, float > > &result, bool operation){
    if(operation){                                                    //将数据以[[x,y][x,y][x,y]]格式保存到文件                                    
      std::ofstream outfile(fileaddres,std::ios::trunc );//CircleData.txt里面的内容会被清空,重新开始写
      if (!outfile.is_open())                                   //检测是否打开，打不开请检查文件地址
      {
      std::cout << "cannot open file" << std::endl;
      }
      outfile << "[";
      for (long i = 0; i< point.size(); i++) {          
        outfile << "[" << point[i].x << "," << point[i].y << "]" ;
      }
      outfile << "]";
      std::cout << "Data transfer succeeded" << std::endl;
      outfile.close();
    }else{                                                                     //读取文件数据
      std::string xy;
      std::string  error_return;
      std::ifstream infile(fileaddres); 

      if (!infile.is_open())                                       //检测是否打开，打不开请检查文件地址
      {
      std::cout << "cannot open file" << std::endl;
      }
      infile >> xy;
      if(parseVVF(xy, result, error_return)){   //将数据处理成(x,y)形式
        std::cout << error_return << std::endl;
      }else{
        std::cout << error_return << std::endl;
      }
      infile.close();
    }
}

将文本传入的字符串数据转为[x,y]坐标的解析函数，来自于工友

//字符串转[x,y]坐标解析函数
bool parseVVF(const std::string &input, std::vector< std::pair< float, float > > &result, std::string &error_return) {
  result.clear();
  std::stringstream input_ss(input);
  int depth = 0;

  std::pair< float, float > current_vector;
  size_t index = 0;

  while (!!input_ss && !input_ss.eof()) {
    switch (input_ss.peek()) {
      case EOF:
        break;
      case '[':
        depth++;
        if (depth > 2) {
          error_return = "Array depth greater than 2";
          return false;
        }
        input_ss.get();
        // current_vector.clear();
        break;
      case ']':
        depth--;
        if (depth < 0) {
          error_return = "More close ] than open [";
          return false;
        }
        if (depth == 1 && index != 2) {
          error_return = "Numbers at depth 2 mast be have only 2 value..";
          return false;
        }
        index = 0;
        input_ss.get();
        if (depth == 1) {
          result.push_back(current_vector);
        }
        break;
      case ',':
      case ' ':
      case '\t':
        input_ss.get();
        break;
      default: // All other characters should be part of the numbers.
        if (depth != 2) {
          std::stringstream err_ss;
          err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'.";
          error_return = err_ss.str();
          return false;
        }
        float value;
        input_ss >> value;
        if (!!input_ss) {
          switch (index) {
            case 0:
              current_vector.first = value;
              break;
            case 1:
              current_vector.second = value;
              break;
            default:
              error_return = "Numbers at depth 2 mast be have only 2 value.";
              return false;
          }
          index++;
        }
        break;
    }
  }

  if (depth == 0) {
    error_return = "";
    return true;
  } else {
    error_return = "Unterminated vector string.";
    return false;
  }
}

（二）运行效果

将坐标写入文件

 读取文件信息，先输出到屏幕上，后面再去做处理。

（三）问题和下一目标

问题：暂时没有

目标：处理预设数据，多加两个反光柱去扫描作为当前反光柱数据，通过匹配找出原来的三根反光柱，并加上可视化标记。