【自主探索】explore_lite 源码解析

本文主要是介绍【自主探索】explore_lite 源码解析，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

各文件运行顺序：

\explore_lite\launch\explore.launch
\explore_lite\src\explore.cpp
\explore_lite\src\frontier_search.cpp

文章目录

- 一、explore.launch
- 二、explore.cpp
- 三、frontier_search.cpp

一、explore.launch

<launch><node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen"><param name="robot_base_frame" value="base_footprint"/><param name="costmap_topic" value="map"/><param name="costmap_updates_topic" value="map_updates"/><param name="visualize" value="true"/><param name="planner_frequency" value="0.33"/><param name="progress_timeout" value="30.0"/><!-- <param name="progress_timeout" value="60.0"/> --><param name="potential_scale" value="3.0"/><param name="orientation_scale" value="0.0"/><param name="gain_scale" value="1.0"/><param name="transform_tolerance" value="0.3"/><param name="min_frontier_size" value="0.75"/><!-- <param name="min_frontier_size" value="3"/> --></node>
</launch>

二、explore.cpp

// 包含 explore 软件包的头文件，这可能包含了 Explore 类的定义以及其他与探索功能相关的内容
#include <explore/explore.h>// 该文件提供了线程相关的功能
#include <thread>// 定义了一个静态的、内联的相等运算符，用于比较两个 geometry_msgs::Point 类型的对象是否相等。
inline static bool operator==(const geometry_msgs::Point& one,const geometry_msgs::Point& two)
{double dx = one.x - two.x;double dy = one.y - two.y;double dist = sqrt(dx * dx + dy * dy);   // 计算两个点之间的欧氏距离return dist < 0.01;  // 如果两个点的距离小于 0.01，则认为它们相等
}

int main(int argc, char** argv)
{// 接受命令行参数 argc 和 argv，并为节点命名为 "explore"。这是ROS节点的初始化步骤。ros::init(argc, argv, "explore");if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,ros::console::levels::Debug)) {ros::console::notifyLoggerLevelsChanged();}// 创建了explore对象，该对象属于explore::Explore类。这个对象的创建触发了explore::Explore类的构造函数。explore::Explore explore;// 这一行启动ROS事件循环。它使ROS节点一直运行，以便能够接收和处理来自ROS系统的事件，例如订阅的消息、服务调用等。// ros::spin()会一直运行，直到节点被关闭ros::spin();return 0;
}

namespace explore
{......}  // namespace explore

  // Explore类的构造函数开始。初始化了成员变量和对象Explore::Explore(): private_nh_("~")  // 使用~私有命名空间初始化一个ros::NodeHandle对象 private_nh_。, tf_listener_(ros::Duration(10.0)) //使用10秒的缓存时长初始化tf::TransformListener对象 tf_listener_。, costmap_client_(private_nh_, relative_nh_, &tf_listener_) // 使用私有和相对命名空间以及tf_listener_初始化explore::CostmapClient对象 costmap_client_。, move_base_client_("move_base")  //以 "move_base" 为参数初始化 actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> 对象 move_base_client_。, prev_distance_(0) //将prev_distance_初始化为0。, last_markers_count_(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);// 使用 costmap_client_.getCostmap() 取得代价地图，以及前面获取的参数值，// 初始化 frontier_exploration::FrontierSearch 对象 search_。search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(),potential_scale_, gain_scale_,min_frontier_size);//  如果 visualize_ 为真，则创建 marker_array_publisher_ 发布器，发布可视化的前沿。if (visualize_) {marker_array_publisher_ =private_nh_.advertise<visualization_msgs::MarkerArray>("frontiers", 10);}ROS_INFO("Waiting to connect to move_base server"); // 输出ROS信息，表示正在等待连接到 move_base 服务器。move_base_client_.waitForServer();ROS_INFO("Connected to move_base server");   //输出ROS信息，表示已连接到 move_base 服务器。// 创建一个定时器，用于定期调用 makePlan() 函数。这里使用了相对命名空间。exploring_timer_ =relative_nh_.createTimer(ros::Duration(1. / planner_frequency_),[this](const ros::TimerEvent&) { makePlan(); });}

  Explore::~Explore() //Explore类的析构函数开始{stop(); //调用 stop() 函数，可能是该类的一个私有成员函数，用于停止某些正在进行的操作。}

  // 1. makePlan() 函数的主要作用是找到前沿，选择一个未被列入黑名单的前沿，并将其作为目标发送给 move_basevoid Explore::makePlan(){// find frontiersauto pose = costmap_client_.getRobotPose(); //通过 costmap_client_.getRobotPose() 获取机器人的当前姿态。// get frontiers sorted according to costauto frontiers = search_.searchFrom(pose.position); //使用前沿搜索算法（search_）查找从机器人当前位置开始的前沿。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 可视化前沿,如果设置了 visualize_ 为真，则可视化前沿。if (visualize_) {visualizeFrontiers(frontiers);}// find non blacklisted frontier    寻找非黑名单的前沿//在找到的前沿中查找第一个不在黑名单中的前沿//std::find_if_not 是 STL 提供的一个通用查找算法，用于在给定范围内查找第一个不满足指定条件的元素。auto frontier =    //auto frontier 使用自动类型推导，将找到的前沿的迭代器赋值给 frontier。std::find_if_not(frontiers.begin(), frontiers.end(),  //分别是前沿列表的开始和结束迭代器，它们定义了查找范围。[this](const frontier_exploration::Frontier& f) {return goalOnBlacklist(f.centroid);});if (frontier == frontiers.end()) {    //如果找到的前沿迭代器等于 frontiers.end()，说明在整个前沿列表中没有找到符合条件的前沿，结束当前的 makePlan() 函数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 progresslast_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 发送目标move_base_msgs::MoveBaseGoal goal;goal.target_pose.pose.position = target_position;goal.target_pose.pose.orientation.w = 1.; //这一行设置目标的朝向。在这里，机器人被要求直接朝向目标。因此，将四元数的 w 分量设置为 1.0，表示不进行旋转，保持与目标位置的朝向一致goal.target_pose.header.frame_id = costmap_client_.getGlobalFrameID();  //获取全局坐标系的ID，确保目标位置的正确性goal.target_pose.header.stamp = ros::Time::now(); move_base_client_.sendGoal(   //通过 move_base_client_.sendGoal() 函数将目标发送给 move_basegoal, [this, target_position](  //这是一个回调函数，用于处理机器人到达目标时的状态。const actionlib::SimpleClientGoalState& status,const move_base_msgs::MoveBaseResultConstPtr& result) {reachedGoal(status, result, target_position); //reachedGoal 函数会在目标到达时被调用，并传递当前目标的状态和结果以及目标位置。});}

  // 2. visualizeFrontiers() 函数用于可视化前沿，它将前沿的信息转化为可视化标记，并发布到 frontiers 主题void Explore::visualizeFrontiers(const std::vector<frontier_exploration::Frontier>& frontiers){// 定义颜色常量std_msgs::ColorRGBA blue;blue.r = 0;blue.g = 0;blue.b = 1.0;blue.a = 1.0;std_msgs::ColorRGBA red;red.r = 1.0;red.g = 0;red.b = 0;red.a = 1.0;std_msgs::ColorRGBA green;green.r = 0;green.g = 1.0;green.b = 0;green.a = 1.0;ROS_DEBUG("visualising %lu frontiers", frontiers.size()); // 输出调试信息// 创建 MarkerArray 用于存储所有标记visualization_msgs::MarkerArray markers_msg;std::vector<visualization_msgs::Marker>& markers = markers_msg.markers;visualization_msgs::Marker m;// 设置 Marker 的基本属性m.header.frame_id = costmap_client_.getGlobalFrameID();m.header.stamp = ros::Time::now();m.ns = "frontiers";m.scale.x = 1.0;m.scale.y = 1.0;m.scale.z = 1.0;m.color.r = 0;m.color.g = 0;m.color.b = 255;m.color.a = 255;// lives forever  // 永不过期m.lifetime = ros::Duration(0);m.frame_locked = true;// weighted frontiers are always sorted 权重前沿总是排序的double min_cost = frontiers.empty() ? 0. : frontiers.front().cost;// 将前沿信息转换为可视化标记m.action = visualization_msgs::Marker::ADD;size_t id = 0;for (auto& frontier : frontiers) {// 点标记m.type = visualization_msgs::Marker::POINTS;m.id = int(id);m.pose.position = {};m.scale.x = 0.1;m.scale.y = 0.1;m.scale.z = 0.1;m.points = frontier.points;// 根据是否在黑名单中设置颜色if (goalOnBlacklist(frontier.centroid)) {m.color = red;} else {m.color = blue;}markers.push_back(m);++id;// 球标记m.type = visualization_msgs::Marker::SPHERE;m.id = int(id);m.pose.position = frontier.initial;// scale frontier according to its cost (costier frontiers will be smaller) 根据前沿的成本设置标记的缩放大小double scale = std::min(std::abs(min_cost * 0.4 / frontier.cost), 0.5);m.scale.x = scale;m.scale.y = scale;m.scale.z = scale;m.points = {};m.color = green;markers.push_back(m);++id;}size_t current_markers_count = markers.size();// delete previous markers, which are now unused  删除之前的标记，它们现在未被使用m.action = visualization_msgs::Marker::DELETE;for (; id < last_markers_count_; ++id) {m.id = int(id);markers.push_back(m);}last_markers_count_ = current_markers_count;marker_array_publisher_.publish(markers_msg); // 发布标记数组}

  // 3.用于检查给定的目标位置是否在黑名单中//这个函数采用机器人当前计划的前沿目标的坐标，并遍历黑名单中的目标，检查是否有目标接近当前计划的前沿目标。//这个函数的作用是帮助过滤掉那些在黑名单中的目标，以确保机器人不会一直尝试无法到达的目标。容忍范围的设置可以控制允许目标与黑名单中目标的接近程度。bool Explore::goalOnBlacklist(const geometry_msgs::Point& goal){constexpr static size_t tolerace = 5; //表示容忍的范围，即允许的目标与黑名单中目标的距离阈值。costmap_2d::Costmap2D* costmap2d = costmap_client_.getCostmap();  //获取 costmap_2d::Costmap2D* 类型的指针，该指针指向 costmap_client_ 中的地图信息。// check if a goal is on the blacklist for goals that we're pursuing  遍历黑名单中的目标，对比每个目标与当前计划的前沿目标的坐标for (auto& frontier_goal : frontier_blacklist_) {double x_diff = fabs(goal.x - frontier_goal.x);double y_diff = fabs(goal.y - frontier_goal.y);if (x_diff < tolerace * costmap2d->getResolution() &&y_diff < tolerace * costmap2d->getResolution())return true;}return false;}

  // 4.reachedGoal() 函数是一个回调函数，用于处理机器人是否已经达到目标的情况// 如果机器人未能成功到达目标，则将当前计划的前沿目标添加到黑名单中，并触发重新规划。void Explore::reachedGoal(const actionlib::SimpleClientGoalState& status, //表示机器人达到目标的状态，是一个 actionlib::SimpleClientGoalState 类型的对象。const move_base_msgs::MoveBaseResultConstPtr&,  //表示机器人到达目标时的结果，这里没有使用，因此使用占位符 _。const geometry_msgs::Point& frontier_goal)  //表示机器人当前计划的前沿目标的坐标。{ROS_DEBUG("Reached goal with status: %s", status.toString().c_str()); //输出调试信息，显示机器人达到目标的状态。// 检查机器人达到目标的状态是否为 ABORTED，如果是，表示机器人未能成功到达目标。// 在这种情况下，将当前计划的前沿目标的坐标添加到黑名单 (frontier_blacklist_) 中，并输出调试信息。if (status == actionlib::SimpleClientGoalState::ABORTED) {  frontier_blacklist_.push_back(frontier_goal);ROS_DEBUG("Adding current goal to black list");}// find new goal immediatelly regardless of planning frequency.// execute via timer to prevent dead lock in move_base_client (this is// callback for sendGoal, which is called in makePlan). the timer must live// until callback is executed.// 创建一个一次性定时器 (oneshot_)，在定时器触发时调用 makePlan() 函数。// 这是为了确保在 sendGoal 的回调中调用 makePlan() 不会导致死锁。// 定时器被设置为一次性的，因此它会在执行一次 makePlan() 后自动销毁。oneshot_ = relative_nh_.createTimer(ros::Duration(0, 0), [this](const ros::TimerEvent&) { makePlan(); },true);}

三、frontier_search.cpp

#include <explore/frontier_search.h>#include <mutex>#include <costmap_2d/cost_values.h>
#include <costmap_2d/costmap_2d.h>
#include <geometry_msgs/Point.h>#include <explore/costmap_tools.h>

namespace frontier_exploration
{......
}

  using costmap_2d::LETHAL_OBSTACLE;  //使用 costmap_2d 命名空间中的常量，并将它们引入到当前的命名空间中using costmap_2d::NO_INFORMATION;using costmap_2d::FREE_SPACE;FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap,  // 这是 FrontierSearch 类的构造函数。它接受四个参数double potential_scale, double gain_scale,double min_frontier_size): costmap_(costmap) // costmap_2d::Costmap2D* costmap：指向 Costmap2D 对象的指针，表示用于搜索前沿的成本地图。, potential_scale_(potential_scale) // double potential_scale：前沿潜在分数的比例尺度。, gain_scale_(gain_scale) // double gain_scale：前沿增益的比例尺度。, min_frontier_size_(min_frontier_size) // double min_frontier_size：前沿的最小大小。{}

  // 1. 这个函数的目标是找到机器人当前位置附近的前沿。// 在这个过程中，它通过广度优先搜索遍历地图，找到未被访问的自由空间单元，并将其标记为前沿。// 然后，它构建新的前沿，计算前沿的成本，并将前沿按成本排序。最后，返回前沿列表。std::vector<Frontier> FrontierSearch::searchFrom(geometry_msgs::Point position){// 函数返回一个 std::vector，其中包含 Frontier 对象的前沿列表。// 该函数接受一个参数 geometry_msgs::Point position，表示机器人的当前位置std::vector<Frontier> frontier_list;  //创建一个空的前沿列表，该列表将在函数中被填充。// Sanity check that robot is inside costmap bounds before searchingunsigned int mx, my;  //定义两个无符号整数变量 mx 和 my，用于存储机器人在地图上的离散坐标。// 使用 worldToMap 函数将机器人的世界坐标位置转换为地图坐标。如果机器人的位置超出了成本地图的范围，输出错误信息并返回空的前沿列表。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 searchstd::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getMutex()));  //使用 std::lock_guard 对地图的互斥锁进行上锁，确保在搜索期间地图是一致的map_ = costmap_->getCharMap();  //获取成本地图的字符形式，表示地图上每个单元格的状态size_x_ = costmap_->getSizeInCellsX();  //获取地图在 X 轴上的单元格数量size_y_ = costmap_->getSizeInCellsY();  //获取地图在 Y 轴上的单元格数量// initialize flag arrays to keep track of visited and frontier cells// 创建两个标志数组 frontier_flag 和 visited_flag，分别用于跟踪前沿单元和已访问单元的状态std::vector<bool> frontier_flag(size_x_ * size_y_, false);std::vector<bool> visited_flag(size_x_ * size_y_, false);// initialize breadth first search  创建一个用于广度优先搜索的队列 bfsstd::queue<unsigned int> bfs;// find closest clear cell to start search// 这一行声明了两个无符号整数变量 clear 和 pos，同时计算机器人当前位置在成本地图中的索引，并将结果存储在 pos 中。unsigned int clear, pos = costmap_->getIndex(mx, my);//nearestCell 函数的目的是在机器人当前位置附近找到最近的自由空间单元//使用 nearestCell 函数查找最近的自由空间单元，将其索引存储在 clear 中if (nearestCell(clear, pos, FREE_SPACE, *costmap_)) { bfs.push(clear);  //如果成功找到最近的自由空间单元，将其索引加入广度优先搜索队列 bfs 中} else {  //如果未找到最近的自由空间单元（可能由于机器人位置不在地图范围内）bfs.push(pos);  // 将机器人当前位置的索引加入广度优先搜索队列 bfs 中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// 这段代码是广度优先搜索（BFS）的核心循环，用于在地图上搜索前沿for (unsigned nbr : nhood4(idx, *costmap_)) { //这是一个 for 循环，用于遍历当前单元格 idx 的4邻域。nhood4 是一个函数，它返回一个包含当前单元格四个相邻单元格的容器。// add to queue all free, unvisited cells, use descending search in case// initialized on non-free cellif (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);  //根据新的前沿单元格的信息构建一个 Frontier 对象。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.检查一个单元格是否是新的前沿单元格// 这个函数的目的是通过检查一个单元格的状态和其4邻域中是否有自由空间单元格来确定是否将该单元格标记为新的前沿。// 如果单元格是未知状态且未被标记为前沿，并且其4邻域中至少有一个自由空间单元格，那么就认为这是一个新的前沿单元格。bool FrontierSearch::isNewFrontierCell(unsigned int idx,const std::vector<bool>& frontier_flag) //函数定义，接收一个单元格索引 idx 和一个标志数组 frontier_flag。{// check that cell is unknown and not already marked as frontierif (map_[idx] != NO_INFORMATION || frontier_flag[idx]) {  //如果单元格不是未知状态或已被标记为前沿，则返回 false。这表示该单元格不是新的前沿。return false;}// frontier cells should have at least one cell in 4-connected neighbourhood// that is freefor (unsigned int nbr : nhood4(idx, *costmap_)) { //遍历当前单元格 idx 的4邻域。if (map_[nbr] == FREE_SPACE) {  //如果4邻域中存在至少一个自由空间单元格，表示当前单元格是新的前沿单元格，返回 true。return true;}}return false;}

  // 3.用于构建新的前沿// 该函数的主要目的是通过广度优先搜索找到与初始单元格相连的前沿单元格，构建并返回一个包含前沿信息的 Frontier 结构体。Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell,  //接收初始单元格索引 initial_cellunsigned int reference, //参考单元格索引 referencestd::vector<bool>& frontier_flag) //前沿标志数组 frontier_flag{// initialize frontier structureFrontier output;  //创建 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 frontierunsigned 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 coordsunsigned 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 遍历当前单元格的8邻域。for (unsigned int nbr : nhood8(idx, *costmap_)) {// check if neighbour is a potential frontier cell  检查邻域中的单元格是否是潜在的前沿单元格。if (isNewFrontierCell(nbr, frontier_flag)) {// mark cell as frontierfrontier_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; //创建一个包含单元格坐标的 geometry_msgs::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;}

  // 4.用于计算前沿的总代价// 这个代价用于评估前沿的优先级，其中代价越低，优先级越高。// 这种代价的设计旨在同时考虑到机器人距离前沿的距离和前沿的尺寸，以便更有效地选择探索目标。double FrontierSearch::frontierCost(const Frontier& frontier)// 这个函数接收一个 Frontier 结构体的实例 frontier，然后计算并返回前沿的代价{return (potential_scale_ * frontier.min_distance * costmap_->getResolution()) -  //这一部分表示机器人到前沿的最小距离的代价//potential_scale_ 是一个比例因子，frontier.min_distance 表示前沿的最小距离，costmap_->getResolution() 表示地图的分辨率。(gain_scale_ * frontier.size * costmap_->getResolution());  //这一部分表示前沿的尺寸的代价//gain_scale_ 是一个比例因子，frontier.size 表示前沿的尺寸，costmap_->getResolution() 表示地图的分辨率。}