基于激光雷达实现三边定位算法开发记录（十）——动态记录

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

开发平台为ubuntu 18.04 + ros melodic

日期：2022.8.1

本次实现：在《记录（六）——对拟定圆心进行保存和读取》中，已经初步完成了对于拟合圆心的保存，但那种方法只能保持最后一帧激光扫描到的但前反光柱的圆心数据。因此本次将会根据记录六中的函数功能进行改进，使之能够实现在机器人运动过程中对新出现的反光柱进行动态记录。

（一）代码编写

这里主要改进数据读写函数，原理是通过对文件的不断读写，当运动过程中有新的反光柱出现时，将新反光柱的拟合位置与之前文件中的反光柱位置坐标进行对比，当反光柱与文件内任一坐标点x轴y轴之间的距离小于0.1m时，则认为其为旧反光柱。反之，当反光柱的坐标与文件内任一坐标都不能匹配上时，则认为他是新反光柱，并将该拟合坐标记录到文件内。

//反光柱圆心数据文件读写
void fileOperation(std::string fileaddres,const std::vector< Point > point, std::vector< std::pair< float, float > > &result, bool fileoperation_state){
    std::string xy;
    std::string xxyy;
    std::string error_return;

    bool state = true;

    if(fileoperation_state){                                //将数据以[[x,y][x,y][x,y]]格式保存到文件                                    
    std::ifstream infile(fileaddres);                       //打开文件，获取预存数据
    if (!infile.is_open())                                  //检测是否打开，打不开请检查文件地址
        {
        std::cout << "infile cannot open file" << std::endl;
        }
    infile >> xy;
    std::cout << "当前保存的坐标数据：" << xy << std::endl;
    infile.close();
    // xxyy.pop_back();
    if(parseVVF(xy, result, error_return)){                  //将数据处理成(x,y)形式 
    for (long i = 0; i< point.size(); i++) {                 //遍历当前反光柱坐标  

        //转换为全局坐标
        float r_x = odom_point.Angle_x+(WHEELBASE*cos(odom_point.Angle_yaw))+(point[i].x*cos(odom_point.Angle_yaw))-(point[i].y*sin(odom_point.Angle_yaw)); 
        float r_y = odom_point.Angle_y+(WHEELBASE*sin(odom_point.Angle_yaw))+(point[i].x*sin(odom_point.Angle_yaw))+(point[i].y*cos(odom_point.Angle_yaw));                               
        
        for(long j = 0; j < result.size(); j++){             //遍历记录的坐标      
          if(fabs(r_x - result[j].first) < 0.1 && fabs(r_y - result[j].second)< 0.1) {  
            state = false;                                   //当x轴y轴差距都小于o.1时，认为是同一根反光柱，不加进去
            } 
          }
        if(state){                                           //否则不是同一根，保存

          std::ifstream infile(fileaddres);                  //重新读取，当有多根新反光柱时防止遗漏
          infile >> xxyy;
          infile.close();
          xxyy.pop_back();                                   //去除最后一个"]"，保持[[x,y][x,y][x,y]]格式
          
          std::ofstream outfile(fileaddres ,std::ios::trunc);         //打开CircleData.txt文件，将原来内容清零
          if (!outfile.is_open())                            //检测是否打开，打不开请检查文件地址
          {
            std::cout << "outfile cannot open file" << std::endl;
          }
          outfile << xxyy <<"[" << r_x << "," << r_y << "]" << "]";   //添加新反光柱，注意要把"]"加进去
          outfile.close();   
          std::cout << "Data transfer succeeded" << std::endl; 
          state = true; 
       }
     }
    }else{
          std::cout << error_return << std::endl;
      }
 
    }else{                                                    //读取文件数据
    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();
    }
}

 注意，当每次重新建图录入反光柱信息时，应该将记录文件清空，只剩下[]，可以通过脚本的形式快捷操作。Reset中为[]，将原来的文件删除，并将之复制过去即可。

#！/bin/bash

source ./setup.sh
cd src/HAL/trilateration
rm CircleData.txt
cp Reset.txt CircleData.txt
cd ~/lanhai_ws-DTEST/
roslaunch core view_laser.launch

（二）运行效果

在小车上的激光雷达范围只有270，因此将无根反光柱分布于小车周围，小车一次性只能扫描到3根反光柱，控制小车旋转一圈，结果如下，能将无根反光柱进行动态保存。

（三）问题和下一目标

问题：可以看出，每一次读取都要将文件内的内容全部取出进行操作，当工业现场实际应用时反光柱的数量将会有几百根，到时候该算法可能会出现错误。

目标：完善其余功能。