ROS机器人未知环境自主探索功能包explore_lite最全源码详细解析（三）

   本系列文章主要针对ROS机器人常使用的未知环境自主探索功能包explore_lite展开全源码的详细解析，并进行概括总结。 本系列文章共包含六篇文章，前五篇文章主要介绍explore_lite功能包中 explore.cpp、costmap_tools.h、frontier_search.cpp、costmap_client.cpp等源码实现文件中全部函数的详细介绍，第六篇文章进行概括总结，包含自主探索功能包explore_lite的简介，实现未知环境自主探索功能的原理及流程总结，效果演示，使用功能包explore_lite时机器人一直在原地转圈的解决方法等内容。

---

   ☆☆☆本文系列文章索引及函数分布索引 【点击文章名可跳转】☆☆☆

文章一、ROS机器人未知环境自主探索功能包explore_lite最全源码详细解析（一）

   一、main    二、Explore构造函数    三、~Explore析构函数

   四、stop 函数    五、makePlan()函数 ☆☆☆☆☆

文章二、ROS机器人未知环境自主探索功能包explore_lite最全源码详细解析（二）

   六、visualizeFrontiers函数    七、goalOnBlacklist函数    八、reachedGoal函数

   九、start函数

文章三、ROS机器人未知环境自主探索功能包explore_lite最全源码详细解析（三）

   十、nhood4函数    十一、nhood8函数    十二、nearestCell函数

   十三、searchFrom函数

文章四、ROS机器人未知环境自主探索功能包explore_lite最全源码详细解析（四）

   十四、isNewFrontierCell函数    十五、buildNewFrontier函数    十六、frontierCost函数

   十七、FrontierSearch构造函数    十八、Costmap2DClient构造函数

文章五、ROS机器人未知环境自主探索功能包explore_lite最全源码详细解析（五）

   十九、updateFullMap函数    二十、updatePartialMap函数    二十一、getRobotPose函数

   二十二、init_translation_table函数

文章六、ROS机器人未知环境自主探索功能包explore_lite总结

   全系列文章的概括总结【强烈推荐】

   【注 1】：上述函数的颜色是我根据该函数对理解自主探索算法的工作原理及流程的重要程度划分的，红色的最重要，蓝色的次数，最后是绿色的，纯属个人观点，仅供参考。

   【注 2】：关于函数分布，函数一到九位于explore.cpp中，函数十到十二位于costmap_tools.h中，函数十三到十七位于frontier_search.cpp中，函数十八到二十二位于costmap_client.cpp中

---

  十、nhood4函数

   1、函数源码

std::vector<unsigned int> nhood4(unsigned int idx,
                                 const costmap_2d::Costmap2D& costmap)
{
  // get 4-connected neighbourhood indexes, check for edge of map
  std::vector<unsigned int> out;

  unsigned int size_x_ = costmap.getSizeInCellsX(),
               size_y_ = costmap.getSizeInCellsY();

  if (idx > size_x_ * size_y_ - 1) {
    ROS_WARN("Evaluating nhood for offmap point");
    return out;
  }

  if (idx % size_x_ > 0) {
    out.push_back(idx - 1);
  }
  if (idx % size_x_ < size_x_ - 1) {
    out.push_back(idx + 1);
  }
  if (idx >= size_x_) {
    out.push_back(idx - size_x_);
  }
  if (idx < size_x_ * (size_y_ - 1)) {
    out.push_back(idx + size_x_);
  }
  return out;
}

–

   2、主要作用：

   nhood4 函数的主要作用是确定一个给定单元格的索引值和在二维代价地图，然后查找该单元格中的4连通邻域（上、下、左、右四个方向），同时检查这些邻居是否在地图的边缘，简单点说就是 看一下 给定的单元格在地图范围内，在其上、下、左、右四个方向上是否有相邻的单元格，如果有，就返回这些相邻单元格的索引值。

–

   3、详细介绍：

std::vector<unsigned int> nhood4(unsigned int idx,
                                 const costmap_2d::Costmap2D& costmap)
{
  std::vector<unsigned int> out;

这段代码定义了 nhood4 函数，接受两个参数：idx（单元格的索引）和costmap（二维代价地图的引用）。函数返回一个 unsigned int 类型的向量，存储邻居单元格的索引。

  unsigned int size_x_ = costmap.getSizeInCellsX(),
               size_y_ = costmap.getSizeInCellsY();

这里获取了地图的尺寸，size_x_ 和 size_y_ 分别存储地图的宽度和高度（单元格数）。

  if (idx > size_x_ * size_y_ - 1) {
    ROS_WARN("Evaluating nhood for offmap point");
    return out;
  }

在这段代码中，首先检查给定的索引 idx 是否超出了地图的边界。如果超出，打印警告信息并返回空的邻居列表。

  if (idx % size_x_ > 0) {
    out.push_back(idx - 1);
  }

这一行检查给定单元格是否有左侧邻居，如果有（不在最左列），则将左侧邻居的索引添加到输出向量中。

  if (idx % size_x_ < size_x_ - 1) {
    out.push_back(idx + 1);
  }

这一行检查是否有右侧邻居，如果给定单元格不在最右列，右侧邻居的索引就会被添加到输出向量中。

  if (idx >= size_x_) {
    out.push_back(idx - size_x_);
  }

这行代码检查是否有上方的邻居，如果给定单元格不在最顶行，上方邻居的索引就会被添加到输出向量中。

  if (idx < size_x_ * (size_y_ - 1)) {
    out.push_back(idx + size_x_);
  }
  return out;
}

最后这段代码检查是否有下方邻居，如果给定单元格不在最底行，下方邻居的索引将被添加到输出向量中。函数最终返回包含所有4连通邻居索引的向量。

---

  十一、nhood8函数

   1、函数源码

std::vector<unsigned int> nhood8(unsigned int idx,
                                 const costmap_2d::Costmap2D& costmap)
{
  // get 8-connected neighbourhood indexes, check for edge of map
  std::vector<unsigned int> out = nhood4(idx, costmap);

  unsigned int size_x_ = costmap.getSizeInCellsX(),
               size_y_ = costmap.getSizeInCellsY();

  if (idx > size_x_ * size_y_ - 1) {
    return out;
  }

  if (idx % size_x_ > 0 && idx >= size_x_) {
    out.push_back(idx - 1 - size_x_);
  }
  if (idx % size_x_ > 0 && idx < size_x_ * (size_y_ - 1)) {
    out.push_back(idx - 1 + size_x_);
  }
  if (idx % size_x_ < size_x_ - 1 && idx >= size_x_) {
    out.push_back(idx + 1 - size_x_);
  }
  if (idx % size_x_ < size_x_ - 1 && idx < size_x_ * (size_y_ - 1)) {
    out.push_back(idx + 1 + size_x_);
  }

  return out;
}

–

   2、主要作用：

   nhood4 nhood8函数的主要作用是计算给定索引在成本地图中的8邻域（即该点周围的8个点）的索引，并确保这些邻域点不超过地图的边界。8邻域通常用于像素或网格中的图像处理和地图分析，因为它考虑了一个点周围所有可能的邻接点。

–

   3、详细介绍：

获取4邻域索引并初始化基本变量

std::vector<unsigned int> out = nhood4(idx, costmap);

unsigned int size_x_ = costmap.getSizeInCellsX(),
             size_y_ = costmap.getSizeInCellsY();

• 首先，调用nhood4函数获取给定索引的4邻域（即上、下、左、右四个方向的邻接点）的索引，并存储在out向量中。
• 然后，获取成本地图的宽度和高度（size_x_和size_y_），这些信息用于后续计算邻域索引时判断边界条件。

检查并添加额外的4个对角邻域点

if (idx > size_x_ * size_y_ - 1) {
  return out;
}

if (idx % size_x_ > 0 && idx >= size_x_) {
  out.push_back(idx - 1 - size_x_);
}
if (idx % size_x_ > 0 && idx < size_x_ * (size_y_ - 1)) {
  out.push_back(idx - 1 + size_x_);
}
if (idx % size_x_ < size_x_ - 1 && idx >= size_x_) {
  out.push_back(idx + 1 - size_x_);
}
if (idx % size_x_ < size_x_ - 1 && idx < size_x_ * (size_y_ - 1)) {
  out.push_back(idx + 1 + size_x_);
}

• 首先，如果给定的索引超出了地图的索引范围，直接返回已找到的4邻域索引。
• 然后，通过一系列条件判断，分别考虑左上、左下、右上、右下这四个对角方向的邻接点是否在地图范围内。如果这些点在地图范围内，则将它们的索引添加到out向量中。
  • 判断条件依赖于索引值与地图宽度的关系以及该索引在地图中的行位置，确保添加的邻接点不会跨越地图的边界。

  通过这种方法，nhood8函数能够为成本地图中的任一点提供一个完整的8邻域索引列表，这对于在地图上进行基于邻域的分析和操作非常有用。例如，在探索算法中，使用8邻域可以更准确地判断一个点是否为前沿的一部分，因为它考虑了点周围所有可能的邻接区域。

---

  十二、nearestCell函数

   1、函数源码

/**
 * @brief Find nearest cell of a specified value
 * @param result Index of located cell
 * @param start Index initial cell to search from
 * @param val Specified value to search for
 * @param costmap Reference to map data
 * @return True if a cell with the requested value was found
 */
bool nearestCell(unsigned int& result, unsigned int start, unsigned char val,
                 const costmap_2d::Costmap2D& costmap)
{
  const unsigned char* map = costmap.getCharMap();
  const unsigned int size_x = costmap.getSizeInCellsX(),
                     size_y = costmap.getSizeInCellsY();

  if (start >= size_x * size_y) {
    return false;
  }

  // initialize breadth first search
  std::queue<unsigned int> bfs;
  std::vector<bool> visited_flag(size_x * size_y, false);

  // push initial cell
  bfs.push(start);
  visited_flag[start] = true;

  // search for neighbouring cell matching value
  while (!bfs.empty()) {
    unsigned int idx = bfs.front();
    bfs.pop();

    // return if cell of correct value is found
    if (map[idx] == val) {
      result = idx;
      return true;
    }

    // iterate over all adjacent unvisited cells
    for (unsigned nbr : nhood8(idx, costmap)) {
      if (!visited_flag[nbr]) {
        bfs.push(nbr);
        visited_flag[nbr] = true;
      }
    }
  }

  return false;
}

–

   2、主要作用：

   nearestCell函数的主要作用是在成本地图中查找最近的、具有指定值的单元格。该函数从给定的起始单元格出发，使用广度优先搜索（BFS）算法遍历地图，直到找到匹配指定值的单元格为止。这个函数对于路径规划、避障和地图分析等功能十分重要，因为它允许算法基于地图上的实际情况（例如寻找最近的可通行区域或障碍物）进行决策。本程序中主要用来查找距离当前位置最近的未探索区域的一个边界点。

–

   3、详细介绍：

初始化变量并检查起始单元格合法性

const unsigned char* map = costmap.getCharMap();
const unsigned int size_x = costmap.getSizeInCellsX(),
                   size_y = costmap.getSizeInCellsY();

if (start >= size_x * size_y) {
  return false;
}

• 首先获取成本地图的字符表示（即每个单元格的值）以及地图的尺寸（宽度和高度）。
• 如果给定的起始单元格索引超出了地图的总大小，即起始单元格不在地图范围内，则函数立即返回false。

初始化广度优先搜索（BFS）

std::queue<unsigned int> bfs;
std::vector<bool> visited_flag(size_x * size_y, false);

// push initial cell
bfs.push(start);
visited_flag[start] = true;

• 初始化一个用于BFS的队列bfs，以及一个布尔向量visited_flag用来记录每个单元格是否已被访问过，以避免重复访问。
• 将起始单元格加入到队列中，并标记为已访问。

执行搜索

while (!bfs.empty()) {
  unsigned int idx = bfs.front();
  bfs.pop();

  // return if cell of correct value is found
  if (map[idx] == val) {
    result = idx;
    return true;
  }

  // iterate over all adjacent unvisited cells
  for (unsigned nbr : nhood8(idx, costmap)) {
    if (!visited_flag[nbr]) {
      bfs.push(nbr);
      visited_flag[nbr] = true;
    }
  }
}

• 使用BFS遍历成本地图。对于队列中的每个单元格，检查其是否具有所需的值val。如果找到匹配的单元格，将其索引赋给result，并返回true。
• 如果当前单元格不匹配，则检查该单元格的所有8个邻域单元格，将未访问过的邻域单元格加入队列继续搜索。
• 如果搜索完成但未找到匹配的单元格，则返回false。

  通过nearestCell函数，可以有效地在成本地图上基于特定的单元格值查找最近的单元格，这对于实现基于地图信息的导航和决策算法至关重要。本程序中主要用来查找距离当前位置最近的未探索区域的一个边界点。

---

  十三、searchFrom函数

   1、函数源码

std::vector<Frontier> FrontierSearch::searchFrom(geometry_msgs::Point position)
{
  std::vector<Frontier> frontier_list;

  // Sanity check that robot is inside costmap bounds before searching
  unsigned int mx, my;
  if (!costmap_->worldToMap(position.x, position.y, mx, my)) {
    ROS_ERROR("Robot out of costmap bounds, cannot search for frontiers");
    return frontier_list;
  }

  // make sure map is consistent and locked for duration of search
  std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getMutex()));

  map_ = costmap_->getCharMap();
  size_x_ = costmap_->getSizeInCellsX();
  size_y_ = costmap_->getSizeInCellsY();

  // initialize flag arrays to keep track of visited and frontier cells
  std::vector<bool> frontier_flag(size_x_ * size_y_, false);
  std::vector<bool> visited_flag(size_x_ * size_y_, false);

  // initialize breadth first search
  std::queue<unsigned int> bfs;

  // find closest clear cell to start search
  unsigned int clear, pos = costmap_->getIndex(mx, my);
  if (nearestCell(clear, pos, FREE_SPACE, *costmap_)) {
    bfs.push(clear);
  } else {
    bfs.push(pos);
    ROS_WARN("Could not find nearby clear cell to start search");
  }
  visited_flag[bfs.front()] = true;

  while (!bfs.empty()) {
    unsigned int idx = bfs.front();
    bfs.pop();

    // iterate over 4-connected neighbourhood
    for (unsigned nbr : nhood4(idx, *costmap_)) {
      // add to queue all free, unvisited cells, use descending search in case
      // initialized on non-free cell
      if (map_[nbr] <= map_[idx] && !visited_flag[nbr]) {
        visited_flag[nbr] = true;
        bfs.push(nbr);
        // check if cell is new frontier cell (unvisited, NO_INFORMATION, free
        // neighbour)
      } else if (isNewFrontierCell(nbr, frontier_flag)) {
        frontier_flag[nbr] = true;
        Frontier new_frontier = buildNewFrontier(nbr, pos, frontier_flag);
        if (new_frontier.size * costmap_->getResolution() >=
            min_frontier_size_) {
          frontier_list.push_back(new_frontier);
        }
      }
    }
  }

  // set costs of frontiers
  for (auto& frontier : frontier_list) {
    frontier.cost = frontierCost(frontier);
  }
  std::sort(
      frontier_list.begin(), frontier_list.end(),
      [](const Frontier& f1, const Frontier& f2) { return f1.cost < f2.cost; });

  return frontier_list;
}

–

   2、主要作用：

   FrontierSearch::searchFrom函数的主要作用是从给定的位置开始，在成本地图中搜索前沿区域（即未探索的边界区域）并对找到的前沿进行排序。

–

   3、详细介绍：

检查机器人是否在成本地图范围内

unsigned int mx, my;
if (!costmap_->worldToMap(position.x, position.y, mx, my)) {
  ROS_ERROR("Robot out of costmap bounds, cannot search for frontiers");
  return frontier_list;
}

• 在搜索前沿之前，首先需要确保给定的起始位置在成本地图的范围内。通过调用costmap_->worldToMap方法将世界坐标转换为地图坐标，并检查转换是否成功。如果失败，意味着机器人位于成本地图的边界之外，函数将输出错误信息并返回空的前沿列表。

锁定地图并初始化搜索

std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
map_ = costmap_->getCharMap();
size_x_ = costmap_->getSizeInCellsX();
size_y_ = costmap_->getSizeInCellsY();

• 为了确保在搜索过程中地图数据的一致性，使用std::lock_guard来锁定成本地图。这可以防止在搜索过程中地图数据被其他线程修改。
• 获取成本地图的原始字符映射以及地图的尺寸。

初始化访问和前沿标志数组

std::vector<bool> frontier_flag(size_x_ * size_y_, false);
std::vector<bool> visited_flag(size_x_ * size_y_, false);

• 初始化两个布尔数组，用于记录每个单元格是否被访问过以及是否被标记为前沿。这些数组对应于成本地图中的每个单元格。

执行广度优先搜索（BFS）查找前沿

std::queue<unsigned int> bfs;
unsigned int clear, pos = costmap_->getIndex(mx, my);
if (nearestCell(clear, pos, FREE_SPACE, *costmap_)) {
  bfs.push(clear);
} else {
  bfs.push(pos);
  ROS_WARN("Could not find nearby clear cell to start search");
}
visited_flag[bfs.front()] = true;

   【注】：在costmap_2d中，FREE_SPACE代表无障碍空间的代价值，其定义为 0。这意味着该区域是完全安全的，没有障碍物，机器人可以自由通过。

• 初始化一个广度优先搜索队列，并找到最接近给定位置的空闲单元格作为搜索的起点。如果找不到空闲单元格，则使用给定位置作为起点。将起点标记为已访问。

探索过程

while (!bfs.empty()) {
  unsigned int idx = bfs.front();
  bfs.pop();
  for (unsigned nbr : nhood4(idx, *costmap_)) {
    if (map_[nbr] <= map_[idx] && !visited_flag[nbr]) {
      visited_flag[nbr] = true;
      bfs.push(nbr);
    } else if (isNewFrontierCell(nbr, frontier_flag)) {
      frontier_flag[nbr] = true;
      Frontier new_frontier = buildNewFrontier(nbr, pos, frontier_flag);
      if (new_frontier.size * costmap_->getResolution() >= min_frontier_size_) {
        frontier_list.push_back(new_frontier);
      }
    }
  }
}

• 使用BFS遍历成本地图。对于队列中的每个单元格，检查其四连通邻域中的单元格。
• 如果邻域中的单元格是自由空间并且未被访问过，则将其加入队列继续搜索。
• 如果邻域中的单元格满足成为新前沿的条件（即，未被探索并且有自由空间邻居），则构建一个新的前沿并检查它的大小是否满足最小前沿大小要求。满足条件的前沿被加入到前沿列表中。

计算前沿成本并排序

for (auto& frontier : frontier_list) {


  frontier.cost = frontierCost(frontier);
}
std::sort(
    frontier_list.begin(), frontier_list.end(),
    [](const Frontier& f1, const Frontier& f2) { return f1.cost < f2.cost; });

• 计算每个前沿的成本，并根据成本对前沿列表进行排序。成本的计算考虑了前沿的大小和与机器人当前位置的距离，以便优先探索那些更接近且更大的前沿。

   通过这个函数，FrontierSearch类能够从给定位置开始搜索成本地图中的前沿区域，并根据一定的标准（如前沿的大小和距离）对它们进行排序，为自动探索提供了基础。

---

---