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

最新推荐文章于 2024-08-03 14:08:10 发布

慕羽★

最新推荐文章于 2024-08-03 14:08:10 发布

阅读量830

点赞数 13

分类专栏：运动规划和控制文章标签：机器人 ROS 运动规划未知环境自主探索自主建图 SLAM

本文链接：https://blog.csdn.net/qq_44339029/article/details/137438965

版权

运动规划和控制专栏收录该内容

66 篇文章 211 订阅

订阅专栏

本系列文章主要针对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中

在这里插入图片描述

十四、isNewFrontierCell函数

1、函数源码

bool FrontierSearch::isNewFrontierCell(unsigned int idx,
                                       const std::vector<bool>& frontier_flag)
{
  // check that cell is unknown and not already marked as frontier
  if (map_[idx] != NO_INFORMATION || frontier_flag[idx]) {
    return false;
  }

  // frontier cells should have at least one cell in 4-connected neighbourhood
  // that is free
  for (unsigned int nbr : nhood4(idx, *costmap_)) {
    if (map_[nbr] == FREE_SPACE) {
      return true;
    }
  }

  return false;
}

–

2、主要作用：

FrontierSearch::isNewFrontierCell函数的主要作用是判断给定的栅格单元（由idx索引指定）是否为一个新的前沿（frontier）单元。新的前沿单元是指那些未被探索的（即信息未知的）且至少有一个四连通邻域单元是无障碍空间的单元。–

3、详细介绍：

检查单元是否未知且未被标记为前沿

if (map_[idx] != NO_INFORMATION || frontier_flag[idx]) {
  return false;
}

【注】：在costmap_2d中，NO_INFORMATION代表未知区域的代价值，其定义为 255。这表示该区域的信息未知，机器人没有足够的信息来判断该区域是否安全或可通行。

首先，函数检查给定的单元是否满足两个条件：单元的信息状态是未知的（map_[idx] != NO_INFORMATION），且该单元尚未被标记为前沿（frontier_flag[idx]为false）。如果任一条件不满足，即单元已知或已被标记为前沿，则函数返回false。

检查四连通邻域内是否有无障碍空间

for (unsigned int nbr : nhood4(idx, *costmap_)) {
  if (map_[nbr] == FREE_SPACE) {
    return true;
  }
}

接着，函数遍历给定单元的四连通邻域（即上下左右相邻的单元）。这是通过nhood4函数实现的，它返回给定单元索引的四个邻域单元的索引列表。
对于每一个邻域单元，函数检查它是否为无障碍空间（map_[nbr] == FREE_SPACE）。如果找到至少一个邻域单元是无障碍的，这意味着给定的单元位于探索区域的边界上，即它是一个新的前沿单元，函数返回true。

函数返回值

如果上述循环完成后没有找到任何无障碍的邻域单元，这意味着给定单元不满足作为新前沿单元的条件，因此函数返回false。

总的来说，FrontierSearch::isNewFrontierCell函数是判断一个单元是否可以作为探索的新前沿的关键步骤，它通过检查单元的信息状态和邻域条件来实现。这个函数在自动探索算法中非常重要，它帮助算法识别那些位于已知空间和未知空间边界上的关键单元，从而有效地引导探索行为。

在这里插入图片描述

十五、buildNewFrontier函数

1、函数源码

Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell,
                                          unsigned int reference,
                                          std::vector<bool>& frontier_flag)
{
  // initialize frontier structure
  Frontier output;
  output.centroid.x = 0;
  output.centroid.y = 0;
  output.size = 1;
  output.min_distance = std::numeric_limits<double>::infinity();

  // record initial contact point for frontier
  unsigned int ix, iy;
  costmap_->indexToCells(initial_cell, ix, iy);
  costmap_->mapToWorld(ix, iy, output.initial.x, output.initial.y);

  // push initial gridcell onto queue
  std::queue<unsigned int> bfs;
  bfs.push(initial_cell);

  // cache reference position in world coords
  unsigned int rx, ry;
  double reference_x, reference_y;
  costmap_->indexToCells(reference, rx, ry);
  costmap_->mapToWorld(rx, ry, reference_x, reference_y);

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

    // try adding cells in 8-connected neighborhood to frontier
    for (unsigned int nbr : nhood8(idx, *costmap_)) {
      // check if neighbour is a potential frontier cell
      if (isNewFrontierCell(nbr, frontier_flag)) {
        // mark cell as frontier
        frontier_flag[nbr] = true;
        unsigned int mx, my;
        double wx, wy;
        costmap_->indexToCells(nbr, mx, my);
        costmap_->mapToWorld(mx, my, wx, wy);

        geometry_msgs::Point point;
        point.x = wx;
        point.y = wy;
        output.points.push_back(point);

        // update frontier size
        output.size++;

        // update centroid of frontier
        output.centroid.x += wx;
        output.centroid.y += wy;

        // determine frontier's distance from robot, going by closest gridcell
        // to robot
        double distance = sqrt(pow((double(reference_x) - double(wx)), 2.0) +
                               pow((double(reference_y) - double(wy)), 2.0));
        if (distance < output.min_distance) {
          output.min_distance = distance;
          output.middle.x = wx;
          output.middle.y = wy;
        }

        // add to queue for breadth first search
        bfs.push(nbr);
      }
    }
  }

  // average out frontier centroid
  output.centroid.x /= output.size;
  output.centroid.y /= output.size;
  return output;
}

–

2、主要作用：

FrontierSearch::buildNewFrontier函数的主要作用是构建一个新的前沿区域。这个过程从一个初始的栅格单元开始，通过广度优先搜索（BFS）探索邻近的未知区域，直到构建出一个完整的前沿区域。这个前沿区域包含了边界的所有点、其质心（centroid）、大小（size）、与给定参考点的最小距离以及最靠近参考点的中心点（middle）。

3、详细介绍：

初始化前沿结构

Frontier output;
output.centroid.x = 0;
output.centroid.y = 0;
output.size = 1;
output.min_distance = std::numeric_limits<double>::infinity();

创建一个Frontier结构体output，初始化质心坐标为(0,0)，前沿区域大小为1，与参考点的最小距离设置为无穷大。

记录前沿的初始接触点

unsigned int ix, iy;
costmap_->indexToCells(initial_cell, ix, iy);
costmap_->mapToWorld(ix, iy, output.initial.x, output.initial.y);

将初始单元格的地图索引转换为世界坐标，并记录为前沿的初始接触点initial。

初始化广度优先搜索队列

std::queue<unsigned int> bfs;
bfs.push(initial_cell);

初始化BFS队列，并将初始单元格加入队列。

缓存参考位置的世界坐标

unsigned int rx, ry;
double reference_x, reference_y;
costmap_->indexToCells(reference, rx, ry);
costmap_->mapToWorld(rx, ry, reference_x, reference_y);

将给定的参考点索引转换为世界坐标，用于后续计算前沿区域与参考点的距离。

执行广度优先搜索构建前沿区域

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

    for (unsigned int nbr : nhood8(idx, *costmap_)) {
      if (isNewFrontierCell(nbr, frontier_flag)) {
        frontier_flag[nbr] = true;
        unsigned int mx, my;
        double wx, wy;
        costmap_->indexToCells(nbr, mx, my);
        costmap_->mapToWorld(mx, my, wx, wy);

        geometry_msgs::Point point;
        point.x = wx;
        point.y = wy;
        output.points.push_back(point);

        output.size++;
        output.centroid.x += wx;
        output.centroid.y += wy;

        double distance = sqrt(pow((reference_x - wx), 2.0) + pow((reference_y - wy), 2.0));
        if (distance < output.min_distance) {
          output.min_distance = distance;
          output.middle.x = wx;
          output.middle.y = wy;
        }

        bfs.push(nbr);
      }
    }
}

使用BFS遍历栅格地图，扩展前沿区域。对于队列中的每个单元格，检查其8邻域中的单元格是否为新的前沿单元格。
如果是，将该单元格加入前沿区域，更新区域大小、质心、与参考点的最小距离以及最靠近参考点的中心点。
每找到一个新的前沿单元格，就将其加入BFS队列，直到队列为空。

计算质心平均值

output.centroid.x /= output.size;
output.centroid.y /= output.size;
return output;

最后，计算前沿区域质心的平均坐标，并返回构建的前沿区域。

FrontierSearch::buildNewFrontier通过这种方式，FrontierSearch::buildNewFrontier能够精确地识别并构建出从给定单元格开始的新前沿区域，为环境探索提供了基础信息。

在这里插入图片描述

十六、frontierCost函数

1、函数源码

double FrontierSearch::frontierCost(const Frontier& frontier)
{
  return (potential_scale_ * frontier.min_distance *
          costmap_->getResolution()) -
         (gain_scale_ * frontier.size * costmap_->getResolution());
}

–

2、主要作用：

FrontierSearch::frontierCost函数计算给定前沿（Frontier）的成本。这个成本基于前沿与机器人当前位置的最小距离（考虑了地图的分辨率）和前沿的大小，同时考虑了潜在的缩放因子（potential_scale_）和收益的缩放因子（gain_scale_）。

3、详细介绍：

计算前沿的成本

return (potential_scale_ * frontier.min_distance *
        costmap_->getResolution()) -
       (gain_scale_ * frontier.size * costmap_->getResolution());

成本计算公式包含两部分：距离成本和大小收益。
距离成本：potential_scale_ * frontier.min_distance * costmap_->getResolution()部分计算了从机器人到前沿的最小距离成本，乘以potential_scale_进行缩放。这里的min_distance是前沿到参考点（通常是机器人的当前位置）的最小距离。
大小收益：gain_scale_ * frontier.size * costmap_->getResolution()部分根据前沿的大小给出一个正收益，乘以gain_scale_进行缩放。这意味着更大的前沿区域被视为更有价值，因为它们可能代表着更大的未探索区域。

这个成本函数设计的目的是在距离和大小之间找到一个平衡，优先考虑那些既容易到达又足够大的前沿区域。通过这种方式，探索算法可以更有效地决定下一个探索目标，优化探索过程。

在这里插入图片描述

十七、FrontierSearch构造函数

1、函数源码

FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap,
                               double potential_scale, double gain_scale,
                               double min_frontier_size)
  : costmap_(costmap)
  , potential_scale_(potential_scale)
  , gain_scale_(gain_scale)
  , min_frontier_size_(min_frontier_size)
{
}

–

2、主要作用：

FrontierSearch::FrontierSearch构造函数的主要作用是初始化FrontierSearch对象，它为后续的前沿搜索提供必要的环境和参数。

3、详细介绍：

FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap,
                               double potential_scale, double gain_scale,
                               double min_frontier_size)
  : costmap_(costmap)
  , potential_scale_(potential_scale)
  , gain_scale_(gain_scale)
  , min_frontier_size_(min_frontier_size)
{
}

参数说明

costmap_2d::Costmap2D* costmap: 成本地图的指针，它提供了机器人周围环境的信息。这个地图被用来搜索前沿区域。
double potential_scale: 潜在缩放因子，用于调整前沿成本计算中距离的权重。距离越远，成本越高。
double gain_scale: 收益缩放因子，用于调整前沿成本计算中前沿大小的权重。前沿越大，收益越高。
double min_frontier_size: 前沿的最小大小，只有当前沿区域的大小超过这个阈值时，才会被考虑为有效的探索目标。

成员初始化列表

costmap_(costmap): 成本地图指针的初始化。
potential_scale_(potential_scale): 潜在缩放因子的初始化。
gain_scale_(gain_scale): 收益缩放因子的初始化。
min_frontier_size_(min_frontier_size): 前沿的最小大小的初始化。

这个构造函数通过接收成本地图和相关参数，为前沿搜索准备了所有必需的设置。这些参数允许自定义前沿搜索的行为，使得搜索过程可以根据特定的探索目标或环境条件进行调整。

在这里插入图片描述

十八、Costmap2DClient构造函数

1、函数源码

// static translation table to speed things up
std::array<unsigned char, 256> init_translation_table();
static const std::array<unsigned char, 256> cost_translation_table__ =
    init_translation_table();

Costmap2DClient::Costmap2DClient(ros::NodeHandle& param_nh,
                                 ros::NodeHandle& subscription_nh,
                                 const tf::TransformListener* tf)
  : tf_(tf)
{
  std::string costmap_topic;
  std::string footprint_topic;
  std::string costmap_updates_topic;
  param_nh.param("costmap_topic", costmap_topic, std::string("costmap"));
  param_nh.param("costmap_updates_topic", costmap_updates_topic,
                 std::string("costmap_updates"));
  param_nh.param("robot_base_frame", robot_base_frame_,
                 std::string("base_link"));
  // transform tolerance is used for all tf transforms here
  param_nh.param("transform_tolerance", transform_tolerance_, 0.3);

  /* initialize costmap */
  costmap_sub_ = subscription_nh.subscribe<nav_msgs::OccupancyGrid>(
      costmap_topic, 1000,
      [this](const nav_msgs::OccupancyGrid::ConstPtr& msg) {
        updateFullMap(msg);
      });
  ROS_INFO("Waiting for costmap to become available, topic: %s",
           costmap_topic.c_str());
  auto costmap_msg = ros::topic::waitForMessage<nav_msgs::OccupancyGrid>(
      costmap_topic, subscription_nh);
  updateFullMap(costmap_msg);

  /* subscribe to map updates */
  costmap_updates_sub_ =
      subscription_nh.subscribe<map_msgs::OccupancyGridUpdate>(
          costmap_updates_topic, 1000,
          [this](const map_msgs::OccupancyGridUpdate::ConstPtr& msg) {
            updatePartialMap(msg);
          });

  /* resolve tf prefix for robot_base_frame */
  std::string tf_prefix = tf::getPrefixParam(param_nh);
  robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_);

  // we need to make sure that the transform between the robot base frame and
  // the global frame is available
  /* tf transform is necessary for getRobotPose */
  ros::Time last_error = ros::Time::now();
  std::string tf_error;
  while (ros::ok() &&
         !tf_->waitForTransform(global_frame_, robot_base_frame_, ros::Time(),
                                ros::Duration(0.1), ros::Duration(0.01),
                                &tf_error)) {
    ros::spinOnce();
    if (last_error + ros::Duration(5.0) < ros::Time::now()) {
      ROS_WARN(
          "Timed out waiting for transform from %s to %s to become available "
          "before subscribing to costmap, tf error: %s",
          robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
      last_error = ros::Time::now();
    }
    // The error string will accumulate and errors will typically be the same,
    // so the last
    // will do for the warning above. Reset the string here to avoid
    // accumulation.
    tf_error.clear();
  }
}

–

2、主要作用：

Costmap2DClient类主要负责管理与成本地图（costmap）相关的操作，如订阅成本地图、更新成本地图以及获取机器人在成本地图中的位置等。此类作为探索和导航功能的基础组件，为上层应用提供了与成本地图交互的接口。

3、详细介绍：

初始化成本值转换表

std::array<unsigned char, 256> init_translation_table();
static const std::array<unsigned char, 256> cost_translation_table__ =
    init_translation_table();

init_translation_table函数初始化一个成本值转换表，该表将成本地图中的原始值（通常是占用概率）转换为用于路径规划和避障的内部成本值。这个转换表是为了加速成本地图值的处理。
cost_translation_table__是一个静态常量数组，存储了上述转换表，确保它在类的所有实例中只被初始化一次。

构造函数

Costmap2DClient::Costmap2DClient(ros::NodeHandle& param_nh,
                                 ros::NodeHandle& subscription_nh,
                                 const tf::TransformListener* tf)
  : tf_(tf)
{
  // 参数读取和成本地图订阅初始化
}

构造函数接收ROS节点句柄（用于参数获取和订阅）、成本地图的订阅节点句柄以及一个指向tf::TransformListener的指针（用于坐标变换）。
通过节点句柄读取成本地图相关的参数（如成本地图的话题名称）并进行相应的订阅操作。
成本地图的订阅分为两部分：完整成本地图的订阅和成本地图更新的订阅。对应的回调函数分别是updateFullMap和updatePartialMap，用于处理接收到的成本地图消息。

成本地图订阅和更新

costmap_sub_ = subscription_nh.subscribe<nav_msgs::OccupancyGrid>(
    costmap_topic, 1000,
    [this](const nav_msgs::OccupancyGrid::ConstPtr& msg) {
      updateFullMap(msg);
    });
costmap_updates_sub_ =
    subscription_nh.subscribe<map_msgs::OccupancyGridUpdate>(
        costmap_updates_topic, 1000,
        [this](const map_msgs::OccupancyGridUpdate::ConstPtr& msg) {
          updatePartialMap(msg);
        });

成本地图订阅操作初始化了对成本地图全图和成本地图更新的监听，一旦接收到新的成本地图或更新信息，相应的回调函数会被触发，更新内部的成本地图表示。

确保坐标变换可用

while (ros::ok() &&
       !tf_->waitForTransform(global_frame_, robot_base_frame_, ros::Time(),
                              ros::Duration(0.1), ros::Duration(0.01),
                              &tf_error)) {
  // 等待变换可用或输出警告
}

在订阅成本地图之前，构造函数中有一段逻辑确保了从机器人基座到成本地图全局坐标系的坐标变换是可用的。这对于后续获取机器人在成本地图中的位置非常关键。
使用tf::TransformListener等待所需的坐标变换成为可用状态，如果在指定时间内变换仍然不可用，则输出警告信息。

通过这些步骤，Costmap2DClient类在被实例化时配置了所有必需的参数和订阅，确保了成本地图数据可以被实时更新和使用，从而为机器人的导航和路径规划提供了基础支持。

在这里插入图片描述

慕羽★

关注

13
点赞
踩
13

收藏

觉得还不错? 一键收藏
打赏
0
评论
ROS机器人未知环境自主探索功能包explore_lite最全源码详细解析（四）

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

扫一扫