全覆盖路径规划开源项目Clean-robot-turtlebot3关键函数解析（上）

  本系列文章主要介绍全覆盖路径规划开源项目Clean-robot-turtlebot3的相关内容，包含如下四篇文章，分别介绍了开源项目Clean-robot-turtlebot3流程的概括总结、ROS坐标系常用坐标及其变换、Clean-robot-turtlebot3关键函数解析等内容。

  1、全覆盖路径规划开源项目Clean-robot-turtlebot3原理及流程概括总结

  https://blog.csdn.net/qq_44339029/article/details/136880603

  2、ROS坐标系常用坐标及其变换详细梳理

  https://blog.csdn.net/qq_44339029/article/details/136671867

  3、全覆盖路径规划开源项目Clean-robot-turtlebot3关键函数解析（上）

  https://blog.csdn.net/qq_44339029/article/details/136879605

  4、全覆盖路径规划开源项目Clean-robot-turtlebot3关键函数解析（下）

  https://blog.csdn.net/qq_44339029/article/details/136660766

---

---

   函数目录上篇：

   一、CleaningPathPlanning()

   二、initializeMats()

   三、getCellMatAndFreeSpace()

   四、initializeNeuralMat()

   五、initializeCoveredGrid()

   函数目录下篇：

   六、GetPathInROS()

   七、GetPathInCV()

   八、mainPlanningLoop()

   九、Boundingjudge()

   十、next_goal.cpp中的主函数

---

   一、CleaningPathPlanning()

$\ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast$

   写在前面：该函数是类的构造函数，用于初始化全覆盖路径规划算法的各项参数和数据结构。就算法理解而言，该函数中重点注意，使用costmap栅格地图来初始化化了srcMap_，其尺寸和代价值均与costmap栅格地图相同

$\ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast$

  1. 函数源码:

CleaningPathPlanning::CleaningPathPlanning(costmap_2d::Costmap2DROS *costmap2d_ros)
{
 //temp solution.
 costmap2d_ros_ = costmap2d_ros;
 //costmap2d_ros_->updateMap();
 costmap2d_ = costmap2d_ros->getCostmap();

 ros::NodeHandle private_nh("~/cleaning_plan_nodehandle");
 plan_pub_ = private_nh.advertise<nav_msgs::Path>("cleaning_path", 1);
 grid_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("covered_grid", 1);

 string sizeOfCellString, coveredValueStr;

 SIZE_OF_CELL = 3;
 if (private_nh.searchParam("size_of_cell", sizeOfCellString)) //搜索参数,根据名称"size of cell"搜索参数，将对应名称下的参数值赋给sizeOfCellString.
     private_nh.param("size_of_cell", SIZE_OF_CELL, 3);   //设置机器人占据n*n的栅格，决定规划的稀疏   

 GRID_COVERED_VALUE = 0;
 if (private_nh.searchParam("grid_covered_value", coveredValueStr))
     private_nh.param("grid_covered_value", GRID_COVERED_VALUE, 0);

 int sizex = costmap2d_->getSizeInCellsX(); //获取地图尺寸
 int sizey = costmap2d_->getSizeInCellsY();
 cout << "The size of map is " << sizex << "  " << sizey << endl;
 resolution_ = costmap2d_->getResolution(); //分辨率

 srcMap_ = Mat(sizey, sizex, CV_8U);
 for (int r = 0; r < sizey; r++)
 {
     for (int c = 0; c < sizex; c++)
     {
         srcMap_.at<uchar>(r, c) = costmap2d_->getCost(c, sizey - r - 1); //??sizey-r-1 caution: costmap's origin is at left bottom ,while opencv's pic's origin is at left-top.
         //getCost（）:获取代价值
     }
 }

 initializeMats();
 initializeCoveredGrid();

 //imshow("debugMapImage",srcMap_);
 //imshow("debugCellMatImage",cellMat_);
 //waitKey(0);
 //imwrite("debug_srcmap.jpg",srcMap_);

 if (!srcMap_.empty())
     initialized_ = true; //这句话説明srcMap_里面得有东西才能说明初始化成功。
 else
     initialized_ = false;
}

  2. 功能概述:

   -（1）初始化成员变量，包括Costmap2DROS对象、发布路径和覆盖网格的ROS节点、栅格尺寸和覆盖值等。

   -（2）从ROS参数服务器中获取配置参数，如栅格尺寸和覆盖值。

   - （3）获取地图的尺寸和分辨率。

   - （4）创建地图的OpenCV表示，并将Costmap2DROS对象中的地图数据复制到OpenCV中。

   - （5）初始化路径规划所需的其他数据结构，如cellMat和coveredGrid。

   - （6）根据初始化结果，设置initialized_标志表示初始化是否成功。

  3. 详细解释:

• 成员变量初始化:

  costmap2d_ros_ = costmap2d_ros;
  costmap2d_ = costmap2d_ros->getCostmap();
  

  初始化Costmap2DROS对象和Costmap2D对象，以便后续使用。

• ROS节点初始化:

  ros::NodeHandle private_nh("~/cleaning_plan_nodehandle");
  plan_pub_ = private_nh.advertise<nav_msgs::Path>("cleaning_path", 1);
  grid_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("covered_grid", 1);
  

  初始化ROS节点，并创建用于发布规划路径和覆盖网格的ROS话题发布器。

• 参数获取:

  string sizeOfCellString, coveredValueStr;
  // ...
  if (private_nh.searchParam("size_of_cell", sizeOfCellString))
      private_nh.param("size_of_cell", SIZE_OF_CELL, 3);   
  if (private_nh.searchParam("grid_covered_value", coveredValueStr))
      private_nh.param("grid_covered_value", GRID_COVERED_VALUE, 0);
  

  从ROS参数服务器中获取规划参数，如栅格尺寸和覆盖值。如果参数不存在，则使用默认值。

• 地图初始化:

  int sizex = costmap2d_->getSizeInCellsX();
  int sizey = costmap2d_->getSizeInCellsY();
  resolution_ = costmap2d_->getResolution();
  

  获取地图的尺寸和分辨率。

• OpenCV地图初始化:

  srcMap_ = Mat(sizey, sizex, CV_8U);
  

  创建一个OpenCV的Mat对象，用于存储地图的数据。

• 地图数据复制:

  for (int r = 0; r < sizey; r++)
  {
      for (int c = 0; c < sizex; c++)
      {
          srcMap_.at<uchar>(r, c) = costmap2d_->getCost(c, sizey - r - 1);
      }
  }
  

  将Costmap2DROS对象中的地图数据复制到OpenCV的Mat对象中，需要注意坐标系的转换。

• 数据结构初始化:

  initializeMats();
  initializeCoveredGrid();
  

  初始化其他路径规划所需的数据结构，如cellMat和coveredGrid。

• 初始化结果:

  if (!srcMap_.empty())
      initialized_ = true;
  else
      initialized_ = false;
  

  根据地图数据是否为空，设置initialized_标志表示初始化是否成功。

   4. 总体流程:

   - 初始化ROS节点和发布器。

   - 从ROS参数服务器中获取规划参数。

   - 获取地图的尺寸和分辨率，并创建OpenCV地图对象。

   - 将Costmap2DROS对象中的地图数据复制到OpenCV地图对象中。

   - 初始化其他路径规划所需的数据结构。

   - 根据初始化结果，设置initialized_标志表示初始化是否成功。

   该构造函数的作用是对全覆盖路径规划算法进行初始化，准备好后续的路径规划过程所需的参数和数据结构。

   二、initializeMats()

$\ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast$

   写在前面： 该函数用于初始化路径规划中的数据结构，返回一个布尔值表示是否成功。具体而言，调用getCellMatAndFreeSpace()函数将costmap栅格地图按覆盖直径转换成稀疏的新地图cellMat_，并得到该新地图中的无障碍区域，存储在freeSpaceVec_中，然后调用initializeNeuralMat()函数使用新地图cellMat_来计算初始化活力值地图neuralizedMat_。

$\ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast$

  1. 函数源码:

bool CleaningPathPlanning::initializeMats()
{
 //initialize the member variables.
 if (srcMap_.empty())
     return false;
 getCellMatAndFreeSpace(srcMap_, cellMat_, freeSpaceVec_);

 neuralizedMat_ = Mat(cellMat_.rows, cellMat_.cols, CV_32F);
 //Astarmap = Mat(cellMat_.rows, cellMat_.cols, CV_32F);
 initializeNeuralMat(cellMat_, neuralizedMat_);
 //Astarmap = neuralizedMat_;
 return true;
}

  2. 功能概述:

• 检查地图数据是否为空，如果为空则返回false。
• 根据地图数据初始化路径规划所需的数据结构，包括cellMat（表示地图的栅格化矩阵）和neuralizedMat（表示神经网络化后的矩阵）。
• 将初始化后的数据结构赋值给相应的成员变量。
• 返回初始化结果。

  3. 详细解释:

• 地图数据检查:

  if (srcMap_.empty())
      return false;
  

  检查地图数据srcMap_是否为空，如果为空则无法进行初始化，直接返回false表示初始化失败。

• 地图数据初始化:

  getCellMatAndFreeSpace(srcMap_, cellMat_, freeSpaceVec_);
  

  调用getCellMatAndFreeSpace函数，根据原始地图数据srcMap_生成栅格化矩阵cellMat_和自由空间向量freeSpaceVec_。这些数据结构将用于路径规划的计算。

• 神经网络化矩阵初始化:

  neuralizedMat_ = Mat(cellMat_.rows, cellMat_.cols, CV_32F);
  initializeNeuralMat(cellMat_, neuralizedMat_);
  

  创建一个与栅格化矩阵cellMat_相同大小的矩阵neuralizedMat_，用于表示神经网络化后的地图。调用initializeNeuralMat函数对neuralizedMat_进行初始化，该函数将根据栅格化矩阵中的信息，计算每个栅格的神经网络化值。

• 返回结果:

  return true;
  

  如果初始化成功，则返回true，表示路径规划的数据结构已经成功初始化。

  4. 总体流程:

• 检查地图数据是否为空，如果为空则返回初始化失败。
• 根据地图数据生成栅格化矩阵和自由空间向量。
• 初始化神经网络化矩阵。
• 返回初始化结果。

该函数的作用是初始化路径规划算法中的数据结构，为后续的路径规划过程做好准备。

   三、getCellMatAndFreeSpace()

$\ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast$

   写在前面： 这个函数的作用是将原始地图图像按照指定的大小划分成栅格，同时标记出自由空间和障碍物。即将使用costmap栅格地图初始化的与栅格地图相同分辨率的srcMap_地图，划分成以SIZE_OF_CELL为单位的栅格，存储到cellMat_中。也就是将原有地图稀疏化，认为机器人每走过一个栅格就完成了对其周围SIZE_OF_CELL个栅格的覆盖，所以将原有地图稀疏SIZE_OF_CELL倍得到新地图存放在cellMat_中，也就是原地图srcMap_中每个SIZE_OF_CELLxSIZE_OF_CELL个栅格的正方形区域被转换成新地图cellMat_中的一个栅格，且若原地图中这个SIZE_OF_CELLxSIZE_OF_CELL个栅格的正方形区域中每个栅格均不存在障碍物，则认为转换后得到的新地图中的这个栅格处没有障碍物，否则认为新地图中的这个栅格处有障碍物。遍历结束后，就得到了新地图cellMat_，且标记出新地图的自由空间（无障碍物）的栅格，并将其坐标存储到freeSpaceVec_中。

$\ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast$

  1. 函数源码:

void CleaningPathPlanning::getCellMatAndFreeSpace(Mat srcImg, Mat &cellMat, vector<cellIndex> &freeSpaceVec)
{
 cellMat = Mat(srcImg.rows / SIZE_OF_CELL, srcImg.cols / SIZE_OF_CELL, srcImg.type()); //cellMat是以之前规定的cell为最小单位重新划分的矩阵

 freeSpaceVec.clear();
 bool isFree = true;
 int r = 0, c = 0, i = 0, j = 0;
 for (r = 0; r < cellMat.rows; r++)
 {
     for (c = 0; c < cellMat.cols; c++)
     {
         isFree = true;
         for (i = 0; i < SIZE_OF_CELL; i++)
         {
             for (j = 0; j < SIZE_OF_CELL; j++)
             {
                 if (srcImg.at<uchar>(r * SIZE_OF_CELL + i, c * SIZE_OF_CELL + j) != costmap_2d::FREE_SPACE)
                 {
                     isFree = false;
                     i = SIZE_OF_CELL;
                     break;
                 }
             }
         }
         if (isFree)
         {
             cellIndex ci;
             ci.row = r;
             ci.col = c;
             ci.theta = 0;
             freeSpaceVec.push_back(ci);
             cellMat.at<uchar>(r, c) = costmap_2d::FREE_SPACE; //0
         }
         else
         {
             cellMat.at<uchar>(r, c) = costmap_2d::LETHAL_OBSTACLE;
         } //254
     }
 }
 cout << "freespace size:" << freeSpaceVec.size() << endl;
 //imwrite("cellMat.jpg",cellMat);
 return;
}

这是一个void函数，用于生成栅格矩阵和自由空间向量。

  2. 功能概述:

• 将原始地图srcImg划分成以SIZE_OF_CELL为单位的栅格，存储到cellMat中。
• 标记出自由空间的栅格，并将其坐标存储到freeSpaceVec中。

  3. 详细解释:

• 栅格矩阵初始化:

  cellMat = Mat(srcImg.rows / SIZE_OF_CELL, srcImg.cols / SIZE_OF_CELL, srcImg.type());
  

  创建一个与原始图像大小相适应的栅格矩阵cellMat，栅格大小由SIZE_OF_CELL确定。

• 自由空间向量清空:

  freeSpaceVec.clear();
  

  清空自由空间向量，准备存储新的自由空间坐标。

• 栅格划分:

  for (r = 0; r < cellMat.rows; r++) {
      for (c = 0; c < cellMat.cols; c++) {
          // 判断栅格内是否全为自由空间
          // 如果是，则将该栅格标记为自由空间，并将其坐标存入自由空间向量
          // 否则，将该栅格标记为障碍物
      }
  }
  

  使用两层循环遍历栅格矩阵，对每个栅格进行如下处理：

  • 内部再次使用两层循环遍历栅格内的像素，判断是否全为自由空间。
  • 如果栅格内全为自由空间，则将该栅格标记为自由空间，并将其坐标存入自由空间向量。
  • 否则，将该栅格标记为障碍物。

• 自由空间向量输出:

  cout << "freespace size:" << freeSpaceVec.size() << endl;
  

  打印自由空间向量的大小，用于调试和监测自由空间的数量。

  4. 总体流程:

• 初始化栅格矩阵和自由空间向量。
• 遍历栅格矩阵，对每个栅格判断其中的像素情况，标记自由空间和障碍物。
• 输出自由空间向量的大小。

该函数的主要作用是将原始地图进行栅格化处理，并标记出其中的自由空间，以便后续路径规划算法使用。

   四、initializeNeuralMat()

$\ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast$

   写在前面： 这个函数的作用是使用稀疏后的新地图cellMat_来初始化活力值地图neuralizedMat_，如果新地图中某个栅格处有障碍物，则其对应的活力值地图中存储一个较大的负数-100000，如果没有障碍物，则活力值地图中对应的位置存储的活力值为50/j，其中j是其所在的列数，这种活力值设置模式使得同一列无障碍物点的初始活力值相同，而随着所在列编号的增加，活力值依次降低，在后续的步骤中，机器人会倾向于向活力值高的区域移动，这样初始化活力值，将引导机器人按列形成从弓字型的覆盖路径，且避开障碍物。

$\ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast$

  1. 函数源码:

void CleaningPathPlanning::initializeNeuralMat(Mat cellMat, Mat neuralizedMat)
{
 int i = 0, j = 0;
 for (i = 0; i < neuralizedMat.rows; i++)
 {
     for (j = 0; j < neuralizedMat.cols; j++)
     {
         if (cellMat.at<uchar>(i, j) == costmap_2d::LETHAL_OBSTACLE)
             neuralizedMat.at<float>(i, j) = -100000.0; 
         else
             neuralizedMat.at<float>(i, j) = 50.0 / j; //这里的1.0/j的用意是什么？这里有没有考虑到j=0时刻的问题呢？   hjr注
     }
 }
 return;
}

  2. 功能概述:

• 将栅格矩阵转换为活力值地图表示，其中障碍物被标记为一个非常小的负值，而自由空间则根据其列数位置分配一个相对较小的正值。

  3. 详细解释:

• 矩阵遍历:

  for (i = 0; i < neuralizedMat.rows; i++) {
      for (j = 0; j < neuralizedMat.cols; j++) {
          // 根据栅格矩阵中的值，初始化活力值地图中的值
      }
  }
  

  使用两层循环遍历活力值地图，对每个元素进行如下处理：

  • 根据栅格矩阵中的值，将活力值地图中的对应元素初始化为适当的值。

• 初始化值赋予:

  if (cellMat.at<uchar>(i, j) == costmap_2d::LETHAL_OBSTACLE)
      neuralizedMat.at<float>(i, j) = -100000.0; 
  else
      neuralizedMat.at<float>(i, j) = 50.0 / j;
  

  • 如果栅格矩阵中的元素表示障碍物，则将活力值地图中的对应元素初始化为一个非常小的负值（这里是-100000.0）。
  • 如果栅格矩阵中的元素表示自由空间，则将活力值地图中的对应元素初始化为一个相对较小的正值（这里是50.0除以列数j）。

• 返回:

  return;
  

  函数执行完毕后返回。

  4. 总体流程:

• 遍历活力值地图，对每个元素根据栅格矩阵中的值进行初始化。
• 障碍物对应的元素初始化为一个非常小的负值。
• 自由空间对应的元素初始化为一个相对较小的正值。

该函数的主要作用是将栅格矩阵转换为活力值地图表示，为后续路径规划算法中的活力值查询做好准备。

---

   五、initializeCoveredGrid()

$\ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast$

   写在前面： 这个函数的作用是初始化覆盖栅格地图，用于记录已经覆盖过的区域。该函数使用costmap栅格地图来初始化覆盖栅格地图，覆盖栅格地图的作用是记录机器人已经覆盖过的区域，方便辨别，以及在RVIZ中进行可视化显示。

$\ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast \ast$

  1. 函数源码:

bool CleaningPathPlanning::initializeCoveredGrid() //在这里我对CoverGrid的理解为覆盖栅格。
{
 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap2d_->getMutex()));
 double resolution = costmap2d_->getResolution(); //分辨率

 covered_path_grid_.header.frame_id = "map"; //covered_path_grid_是costmap库中的占据栅格地图消息。
 covered_path_grid_.header.stamp = ros::Time::now();
 covered_path_grid_.info.resolution = resolution;

 covered_path_grid_.info.width = costmap2d_->getSizeInCellsX();
 covered_path_grid_.info.height = costmap2d_->getSizeInCellsY();

 double wx, wy;
 costmap2d_->mapToWorld(0, 0, wx, wy); //从地图坐标系转换至世界坐标系。
 covered_path_grid_.info.origin.position.x = wx - resolution / 2;
 covered_path_grid_.info.origin.position.y = wy - resolution / 2;
 covered_path_grid_.info.origin.position.z = 0.0;
 covered_path_grid_.info.origin.orientation.w = 1.0;

 covered_path_grid_.data.resize(covered_path_grid_.info.width * covered_path_grid_.info.height); //这里可以理解为一共有多少个栅格，所以长×宽

 unsigned char *data = costmap2d_->getCharMap(); //返回一个指针，指向一个底层无符号字符数组，数组中存储着代价值，这个数组貌似还可以用来作为代价地图。
 for (unsigned int i = 0; i < covered_path_grid_.data.size(); i++)
 {
     /*if(data[i]==costmap_2d::FREE_SPACE)
         covered_path_grid_.data[i] = costmap_2d::FREE_SPACE;
     else
         covered_path_grid_.data[i] = 0;*/
     covered_path_grid_.data[i] = data[i]; //这里我理解为将代价值赋予到栅格地图的每个对应栅格当中去。
 }
 return true;
}

这是一个bool函数，用于初始化覆盖栅格地图。

  2. 功能概述:

• 设置覆盖栅格地图的元数据，包括分辨率、大小、原点等。
• 将实际栅格地图中的代价值（costmap）复制到覆盖栅格地图中。

  3. 详细解释:

• 锁定地图:

  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap2d_->getMutex()));
  

  使用boost::unique_lock对地图的mutex进行加锁，确保在地图操作过程中不会被其他线程中断。

• 元数据设置:

  double resolution = costmap2d_->getResolution(); // 获取地图的分辨率
  
  covered_path_grid_.header.frame_id = "map"; // 设置地图坐标系为map
  covered_path_grid_.header.stamp = ros::Time::now(); // 设置时间戳为当前时间
  covered_path_grid_.info.resolution = resolution; // 设置地图分辨率
  
  covered_path_grid_.info.width = costmap2d_->getSizeInCellsX(); // 设置地图宽度（以栅格单元为单位）
  covered_path_grid_.info.height = costmap2d_->getSizeInCellsY(); // 设置地图高度（以栅格单元为单位）
  
  double wx, wy;
  costmap2d_->mapToWorld(0, 0, wx, wy); // 将地图左下角的栅格坐标转换为世界坐标
  covered_path_grid_.info.origin.position.x = wx - resolution / 2; // 设置地图原点x坐标
  covered_path_grid_.info.origin.position.y = wy - resolution / 2; // 设置地图原点y坐标
  covered_path_grid_.info.origin.position.z = 0.0; // 设置地图原点z坐标
  covered_path_grid_.info.origin.orientation.w = 1.0; // 设置地图原点方向为默认值
  

• 数据赋值:

  covered_path_grid_.data.resize(covered_path_grid_.info.width * covered_path_grid_.info.height); // 调整栅格地图数据大小
  
  unsigned char *data = costmap2d_->getCharMap(); // 获取地图的代价值数组
  for (unsigned int i = 0; i < covered_path_grid_.data.size(); i++)
  {
      covered_path_grid_.data[i] = data[i]; // 将地图的代价值复制到覆盖栅格地图中
  }
  

• 返回:

  return true;
  

  函数执行完毕后返回true，表示初始化成功。

  4. 总体流程:

• 锁定地图，确保地图操作的原子性。
• 设置覆盖栅格地图的元数据，包括分辨率、大小、原点等。
• 将实际栅格地图中的代价值（costmap）复制到覆盖栅格地图中。

该函数的主要作用是在全覆盖路径规划算法中初始化覆盖栅格地图，用于记录已经覆盖过的区域。

---

   上述这五个函数（一~五），是算法类的构造函数及其调用的函数，共同完成对算法的初始化和一些准备工作

---