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

在这里插入图片描述

  一、main函数

   1、函数源码

int main(int argc, char** argv)
{
  ros::init(argc, argv, "explore");
  if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
                                     ros::console::levels::Debug)) {
    ros::console::notifyLoggerLevelsChanged();
  }
  explore::Explore explore;
  ros::spin();

  return 0;
}

–

   2、主要作用：

  该函数主要用于启动一个名为"explore"的探索任务，这也是在ROS框架中常用的节点启动方式。

  该main函数是探索程序的入口点，其主要作用是初始化ROS环境、设置日志级别，并创建explore::Explore实例来执行探索任务。在创建Explore实例后，它调用ros::spin()进入ROS消息处理循环，直到接收到关闭信号或者程序被手动终止为止。

–

   3、详细介绍：

初始化ROS节点

ros::init(argc, argv, "explore");
  • 这行代码初始化ROS系统，并指定节点名称为"explore"。argc和argv参数是从程序的主函数传入的，包含了命令行启动程序时传入的参数。这允许ROS通过命令行参数进行配置。

设置日志级别

if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) {
  ros::console::notifyLoggerLevelsChanged();
}
  • 这段代码尝试将ROS控制台的日志级别设置为Debug。如果日志级别成功设置，它会调用notifyLoggerLevelsChanged()函数来通知ROS系统日志级别已更改。这样做可以帮助开发者获得更详细的程序运行信息，有助于调试。

创建Explore实例

explore::Explore explore;

  - 这里创建了Explore类的一个实例。Explore类封装了自动探索环境的所有功能。构造函数中会进行必要的初始化操作，如读取参数、连接到move_base服务器、启动定时器等。

进入ROS消息处理循环

ros::spin();
  • ros::spin()函数调用将程序置入等待状态，等待并处理ROS的回调函数，如订阅的消息和服务请求。这是ROS程序中常见的模式，让主线程在这个循环中阻塞，直到接收到ROS的关闭信号（例如，通过Ctrl+C或者其他节点请求关闭）。

函数返回

return 0;
  • 最后，程序返回0，表示正常退出。这是操作系统期待的程序成功完成的标准退出代码。

  总之，main函数通过初始化ROS，设置日志级别，创建Explore实例，并进入消息处理循环，完成了整个自动探索程序的启动过程。这个结构是编写ROS节点程序的典型模式，其中Explore类封装了主要的功能逻辑，而main函数则处理初始化和事件循环。

在这里插入图片描述

  二、Explore构造函数

   1、函数源码

Explore::Explore()
  : private_nh_("~")
  , tf_listener_(ros::Duration(10.0))
  , costmap_client_(private_nh_, relative_nh_, &tf_listener_)
  , move_base_client_("move_base")
  , prev_distance_(0)
  , last_markers_count_(0)
{
  double timeout;
  double min_frontier_size;
  private_nh_.param("planner_frequency", planner_frequency_, 1.0);
  private_nh_.param("progress_timeout", timeout, 30.0);
  progress_timeout_ = ros::Duration(timeout);
  private_nh_.param("visualize", visualize_, false);
  private_nh_.param("potential_scale", potential_scale_, 1e-3);
  private_nh_.param("orientation_scale", orientation_scale_, 0.0);
  private_nh_.param("gain_scale", gain_scale_, 1.0);
  private_nh_.param("min_frontier_size", min_frontier_size, 0.5);

  search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(),
                                                 potential_scale_, gain_scale_,
                                                 min_frontier_size);

  if (visualize_) {
    marker_array_publisher_ =
        private_nh_.advertise<visualization_msgs::MarkerArray>("frontiers", 10);
  }

  ROS_INFO("Waiting to connect to move_base server");
  move_base_client_.waitForServer();
  ROS_INFO("Connected to move_base server");

  exploring_timer_ =
      relative_nh_.createTimer(ros::Duration(1. / planner_frequency_),
                               [this](const ros::TimerEvent&) { makePlan(); });
}

–

   2、主要作用：

   Explore::Explore()是Explore类的构造函数，在创建Explore对象时被自动调用以初始化对象。这个构造函数的主要职责是设置探索相关的参数、初始化资源和启动定时器以周期性地执行探索任务。

–

   3、详细介绍：

Explore::Explore()
  : private_nh_("~")
  , tf_listener_(ros::Duration(10.0))
  , costmap_client_(private_nh_, relative_nh_, &tf_listener_)
  , move_base_client_("move_base")
  , prev_distance_(0)
  , last_markers_count_(0)
  • 初始化列表中，private_nh_是一个节点句柄，用于访问私有命名空间内的参数。
  • tf_listener_是一个tf监听器，用于在ROS中解决不同坐标系之间的变换问题，这里设置了10秒的缓存时间。
  • costmap_client_是用于获取成本地图信息的客户端，它依赖于tf_listener_来解析地图数据中的坐标变换。
  • move_base_client_是与move_base节点通信的客户端，用于发送导航目标。
  • prev_distance_和last_markers_count_是用于内部逻辑的变量，分别初始化为0。
{
  double timeout;
  double min_frontier_size;
  private_nh_.param("planner_frequency", planner_frequency_, 1.0);
  private_nh_.param("progress_timeout", timeout, 30.0);
  progress_timeout_ = ros::Duration(timeout);
  private_nh_.param("visualize", visualize_, false);
  private_nh_.param("potential_scale", potential_scale_, 1e-3);
  private_nh_.param("orientation_scale", orientation_scale_, 0.0);
  private_nh_.param("gain_scale", gain_scale_, 1.0);
  private_nh_.param("min_frontier_size", min_frontier_size, 0.5);
  • 这部分代码通过private_nh_从ROS的参数服务器获取探索任务的配置参数，例如探索计划的频率、进展超时时间、是否可视化探索结果等，并将它们存储在类的成员变量中。
  search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(),
                                                 potential_scale_, gain_scale_,
                                                 min_frontier_size);

   - 初始化search_对象，这是一个FrontierSearch对象，用于在成本地图上搜索未探索区域（即前沿）。这个对象依据提供的参数来配置搜索算法的行为。

  if (visualize_) {
    marker_array_publisher_ =
        private_nh_.advertise<visualization_msgs::MarkerArray>("frontiers", 10);
  }
  • 如果启用了可视化(visualize_为true)，则创建一个发布者(marker_array_publisher_)，用于向"frontiers"主题发布前沿的可视化标记。
  ROS_INFO("Waiting to connect to move_base server");
  move_base_client_.waitForServer();
  ROS_INFO("Connected to move_base server");
  • 输出信息表示正在等待与move_base服务器的连接，并使用waitForServer函数等待直到连接建立。
  exploring_timer_ =
      relative_nh_.createTimer(ros::Duration(1. / planner_frequency_),
                               [this](const ros::TimerEvent&) { makePlan(); });
}

   - 创建一个定时器exploring_timer_，根据planner_frequency_设置的频率定期调用makePlan方法。这个定时器是探索逻辑的核心，它周期性地触发探索计划的生成和执行。

   总的来说，Explore::Explore()构造函数通过一系列的初始化操作和配置读取准备了探索任务所需的环境，并通过定时器周期性地触发探索计划的生成，从而实现自动探索的功能。

在这里插入图片描述

  三、~Explore析构函数

   1、函数源码

Explore::~Explore()
{
  stop();
}

–

   2、主要作用：

   Explore::~Explore()是Explore类的析构函数，在Explore对象被销毁时自动调用，用于执行清理工作。

   这里调用的stop()方法是Explore类中定义的一个成员函数，其主要功能是停止探索过程。它通过取消所有当前激活的导航目标并停止内部使用的定时器来实现这一点。

在这里插入图片描述

  四、stop 函数

   1、函数源码

void Explore::stop()
{
  move_base_client_.cancelAllGoals();
  exploring_timer_.stop();
  ROS_INFO("Exploration stopped.");
}

–

   2、主要作用：

   Explore::stop()函数是Explore类的一个成员函数，负责终止探索过程。这个函数的主要目的是确保在停止探索时，所有与探索相关的活动和资源都被适当地管理和终止。

–

   3、详细介绍：

{
  move_base_client_.cancelAllGoals();
  exploring_timer_.stop();
  ROS_INFO("Exploration stopped.");
}

  • 取消所有导航目标：move_base_client_.cancelAllGoals();

    • 这一行调用move_base客户端的cancelAllGoals方法，取消所有当前激活的导航目标。这是为了确保在停止探索时，机器人不会继续尝试前往之前设定的任何目标点。

  • 停止探索定时器：exploring_timer_.stop();

    • exploring_timer_是一个ROS定时器，用于周期性地触发探索计划的生成。通过调用stop方法，定时器被停止，这意味着不会再有新的探索计划被自动触发。

  • 输出日志信息：ROS_INFO("Exploration stopped.");

    • 这行代码使用ROS的日志功能输出一条信息，表明探索过程已经被停止。这对于监控程序的状态和调试非常有用。

   总的来说，Explore::stop()函数通过取消所有激活的导航目标和停止探索定时器，安全地终止探索过程，并通过日志信息告知用户探索已经停止。这个函数的设计体现了在资源管理和程序控制方面的良好实践，确保了程序在终止探索时的稳定性和可靠性。

在这里插入图片描述

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

   1、函数源码

void Explore::makePlan()
{
  // find frontiers
  auto pose = costmap_client_.getRobotPose();
  // get frontiers sorted according to cost
  auto frontiers = search_.searchFrom(pose.position);
  ROS_DEBUG("found %lu frontiers", frontiers.size());
  for (size_t i = 0; i < frontiers.size(); ++i) {
    ROS_DEBUG("frontier %zd cost: %f", i, frontiers[i].cost);
  }

  if (frontiers.empty()) {
    stop();
    return;
  }

  // publish frontiers as visualization markers
  if (visualize_) {
    visualizeFrontiers(frontiers);
  }

  // find non blacklisted frontier
  auto frontier =
      std::find_if_not(frontiers.begin(), frontiers.end(),
                       [this](const frontier_exploration::Frontier& f) {
                         return goalOnBlacklist(f.centroid);
                       });
  if (frontier == frontiers.end()) {
    stop();
    return;
  }
  geometry_msgs::Point target_position = frontier->centroid;

  // time out if we are not making any progress
  bool same_goal = prev_goal_ == target_position;
  prev_goal_ = target_position;
  if (!same_goal || prev_distance_ > frontier->min_distance) {
    // we have different goal or we made some progress
    last_progress_ = ros::Time::now();
    prev_distance_ = frontier->min_distance;
  }
  // black list if we've made no progress for a long time
  if (ros::Time::now() - last_progress_ > progress_timeout_) {
    frontier_blacklist_.push_back(target_position);
    ROS_DEBUG("Adding current goal to black list");
    makePlan();
    return;
  }

  // we don't need to do anything if we still pursuing the same goal
  if (same_goal) {
    return;
  }

  // send goal to move_base if we have something new to pursue
  move_base_msgs::MoveBaseGoal goal;
  goal.target_pose.pose.position = target_position;
  goal.target_pose.pose.orientation.w = 1.;
  goal.target_pose.header.frame_id = costmap_client_.getGlobalFrameID();
  goal.target_pose.header.stamp = ros::Time::now();
  move_base_client_.sendGoal(
      goal, [this, target_position](
                const actionlib::SimpleClientGoalState& status,
                const move_base_msgs::MoveBaseResultConstPtr& result) {
        reachedGoal(status, result, target_position);
      });
}

–

   2、主要作用：

   Explore::makePlan函数的主要作用是生成和执行一个探索计划，它通过寻找和导航至未探索的前沿（frontier）来实现。该过程包括获取机器人当前位置、搜索前沿、筛选出未被黑名单的前沿、检查进展并更新目标，以及向move_base发送新的导航目标。

–

   3、详细介绍：

获取机器人当前位置并搜索前沿

auto pose = costmap_client_.getRobotPose();
auto frontiers = search_.searchFrom(pose.position);
ROS_DEBUG("found %lu frontiers", frontiers.size());
for (size_t i = 0; i < frontiers.size(); ++i) {
  ROS_DEBUG("frontier %zd cost: %f", i, frontiers[i].cost);
}
  • 首先，通过costmap_client_获取机器人当前的位置（pose）。
  • 然后，使用search_对象（一个FrontierSearch实例）从当前位置搜索前沿，结果是一组按成本排序的前沿。
  • 使用ROS_DEBUG打印发现的前沿数量和每个前沿的成本，有助于调试和性能分析。

判断前沿是否为空并可视化

if (frontiers.empty()) {
  stop();
  return;
}

if (visualize_) {
  visualizeFrontiers(frontiers);
}
  • 如果没有找到前沿，则调用stop()方法停止探索并直接返回。
  • 如果启用了可视化（由visualize_标志控制），则调用visualizeFrontiers方法将前沿以可视化标记的形式发布。

筛选未被黑名单过滤的前沿

auto frontier =
    std::find_if_not(frontiers.begin(), frontiers.end(),
                     [this](const frontier_exploration::Frontier& f) {
                       return goalOnBlacklist(f.centroid);
                     });
if (frontier == frontiers.end()) {
  stop();
  return;
}
geometry_msgs::Point target_position = frontier->centroid;
  • 通过遍历前沿列表，寻找第一个未被黑名单过滤的前沿作为目标位置。
  • 如果所有前沿都被黑名单过滤，则调用stop()停止探索并返回。

附：c++语法补充：

这段代码使用了C++11标准中的std::find_if_not算法，配合一个lambda表达式，来搜索一个满足特定条件的元素在容器中的位置。具体来说，它在frontiers容器（假设为一个包含frontier_exploration::Frontier类型元素的容器）中查找第一个不在黑名单中的前沿（frontier）。

语句详细解读

auto frontier =
    std::find_if_not(frontiers.begin(), frontiers.end(),
                     [this](const frontier_exploration::Frontier& f) {
                       return goalOnBlacklist(f.centroid);
                     });

  • frontiers.begin(), frontiers.end(): 这部分指定了std::find_if_not算法的搜索范围，从容器frontiers的开始到结束。

  • [this](const frontier_exploration::Frontier& f) { return goalOnBlacklist(f.centroid); }: 这是一个lambda表达式，它被作为std::find_if_not的第三个参数传递。这个lambda捕获当前对象（通过this指针），以便能够访问成员函数goalOnBlacklist。对于每个迭代的frontier对象f，lambda表达式调用goalOnBlacklist(f.centroid)，检查f的中心点（centroid）是否在黑名单上。

  • goalOnBlacklist(f.centroid): 这个调用返回一个布尔值，指示给定的centroid是否在黑名单上。如果在黑名单上，则返回true；否则返回false。

  • std::find_if_not: 这个算法遍历指定范围的每个元素，并对每个元素执行lambda表达式。它返回第一个使lambda表达式返回false的元素的迭代器。也就是说，它寻找第一个不在黑名单中的前沿的迭代器。如果所有的前沿都在黑名单上，或者frontiers容器为空，则返回frontiers.end()。

使用场景

这段代码通常用于在自动化探索或路径规划算法中，从一组候选前沿中排除那些已知为不可达或不希望再次访问的目标。通过找到第一个不在黑名单上的前沿，算法可以继续处理或移向这个新的探索目标。

检查进展并更新目标

bool same_goal = prev_goal_ == target_position;
prev_goal_ = target_position;
if (!same_goal || prev_distance_ > frontier->min_distance) {
  last_progress_ = ros::Time::now();
  prev_distance_ = frontier->min_distance;
}

if (ros::Time::now() - last_progress_ > progress_timeout_) {
  frontier_blacklist_.push_back(target_position);
  ROS_DEBUG("Adding current goal to black list");
  makePlan();
  return;
}
  • 检查当前目标是否与之前的目标相同。如果不同或者已经接近前一个目标，则更新进展时间和距离。
  • 如果长时间没有进展（超过progress_timeout_），则将当前目标加入黑名单并重新生成计划。

向move_base发送新的导航目标

if (same_goal) {
  return;
}

move_base_msgs::MoveBaseGoal goal;
goal.target_pose.pose.position = target_position;
goal.target_pose.pose.orientation.w = 1.;
goal.target_pose.header.frame_id = costmap_client_.getGlobalFrameID();
goal.target_pose.header.stamp = ros::Time::now();
move_base_client_.sendGoal(
    goal, [this, target_position](
              const actionlib::SimpleClientGoalState& status,
              const move_base_msgs::MoveBaseResultConstPtr& result) {
      reachedGoal(status, result, target_position);
    });
  • 如果当前目标与之前的目标相同，则不需要执行任何操作。
  • 否则，构造一个新的导航目标goal并发送给move_base。
  • 当目标达成时，调用reachedGoal回调函数处理后续逻辑。

   通过这个函数，Explore`类能够自动发现环境中的未探索区域，并指导机器人进行探索，同时考虑了进展监控和目标更新的逻辑，以优化探索效率。

# 机器人
Logo
腾讯云开发者社区

腾讯云面向开发者汇聚海量精品云计算使用和开发经验，营造开放的云计算技术生态圈。

更多推荐

cover

动物识别系统 python实现+UI

avatar 腾讯云开发者社区

Linux下安装MySQL8.0(超详细)

Linux下安装MySQL8.0(超详细)

avatar 腾讯云开发者社区
cover

光模块问题查看并保证光模块收发功率

avatar 腾讯云开发者社区