本系列文章主要针对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中
一、自主探索功能包explore_lite
1、简介
explore_lite是一个轻量级的自主探索软件包，主要用于移动机器人的自主导航和环境探索。这个软件包通常是用于机器人操作系统（ROS）的环境中，提供了一系列的工具和算法来支持机器人在未知环境中的自主探索任务。explore_lite的目标是使机器人能够在不需要人类直接控制的情况下，自动地映射其周围的环境并有效地导航，自主完成未知环境的探索以及地图构建。
2、主要特点和功能
轻量级设计：如其名，explore_lite是一个轻量级的探索包，这意味着它旨在尽量减少对计算资源的需求，使得它可以在计算能力较低的机器人系统上运行。
自主探索：explore_lite使机器人能够自主地探索未知环境，通过不断地更新已知环境的地图，并寻找新的、未探索的区域前进。
集成导航功能：它通常与ROS的导航栈一起使用，可以利用导航栈提供的路径规划和避障功能来指导机器人避开障碍物，安全地探索环境。
动态环境适应性：explore_lite能够在一定程度上适应动态变化的环境，通过持续更新地图信息来应对环境中的变化。
可配置性：虽然是轻量级的，但explore_lite提供了一定程度的可配置性，允许用户根据特定的应用场景和机器人能力调整探索策略和参数。
3、开源源码链接
https://github.com/hrnr/m-explore/tree/noetic-devel
二、实现未知环境自主探索功能的原理及流程总结
1、初始化：
在主函数main创建了Explore类的实例 explore，从而执行了Explore类的构造函数，完成了算法初始化。 具体而言，在该构造函数中，先设置了探索相关的参数、然后初始化用于搜索未知区域边界的对象search_、创建发布可视化的发布者，最后，启动定时器以周期性地调用makeplan函数执行探索任务。
2、算法核心流程
算法的核心流程在makeplan函数中执行，首先他通过getRobotPose函数获取机器人的当前位置，然后调用searchFrom函数，基于机器人当前位置使用BFS广度优先搜索与未知区域的所有边界，并将其按照成本从小到大排序，存放在frontiers 中，然后判断frontiers 是否为空，若为空，则没有找到任何与未知区域的边界，调用stop函数，结束探索。 若不为空，则根据设定参数决定是否发布找到的边界的可视化标记。然后对frontiers 中存放的边界进行遍历，依次判断每个边界的质心是否在黑名单中，直至找到第一个未被放入黑名单的边界质心，作为当前目标点。
【补充说明----边界质心和设立黑名单的作用】：这里的边界质心，其中就是该条边界中所有边界点的坐标的平均值，判断其是否在黑名单中的程序在goalOnBlacklist函数中，黑名单中的目标点是那些之前尝试探索但失败或因某种原因决定不再探索的点。这个功能对于避免重复尝试无法到达或效果不佳的目标非常重要。
【补充说明----是否在黑名单中的判断方法】：具体的判断方法是通过遍历黑名单上的每一个目标点（frontier_goal），计算给定点与黑名单上每个点的x轴和y轴差值的绝对值。如果给定点与黑名单中某个点的这些差值都小于通过tolerace（容差）和成本地图分辨率计算出的阈值，则认为给定的目标点已在黑名单中，函数返回true。如果遍历完成后没有找到任何符合条件的黑名单点，则认为给定的目标点不在黑名单中。
然后，检查当前目标是否与之前的目标相同。如果不同或者机器人与当前目标点对应的那条边界的最小距离小于之前目标点对应的边界的最小距离，则认为有进展，进行时间更新和距离更新。并将新的目标点发送出去，从而使得机器人前往与未知区域的边界处进行探索。如果目标点相同，则说明机器人正在前往上一次选定的边界的过程中，无需发布新的目标点，但要进行进展检查，如果长时间没有进展（超过progress_t imeout_），则将当前目标加入黑名单并调用makeplan函数重新生成计划。此外，在move_base服务的回调函数中，会判断在设定的最大时间内是否成功到达目标，在目标未成功达成时同样将其添加到黑名单中，并调用makePlan函数重新生成计划。
【补充说明----关于进展检查】：如果目标点发生变化的话，就直接更新与边界的距离和时间了，认为有进展，如果目标点没有发生变化，则检测当前与边界的距离是否小于上一次与边界的距离，如果是，则说明机器人是否正有效的朝着边界运动，认为有进展，这里的进展检测是为了来防止以某个边界点为目标点时，机器人因为某些情况，长时间无法到达该点或者卡死等情况发生，是很有必要的。一但这些情况发生，可以及时的放弃该点将其加入黑名单，然后前往其他边界进行探索，一定程度上保证了探索效率和质量。
3、核心流程重点补充
上面的核心流程中，如何实现基于机器人当前位置找到与未知区域的所有边界是核心中核心，接下来对其展开详细的介绍，该部分程序在searchFrom函数中。
在正式介绍前，需要补充以下基础知识：
在ROS的costmap_2d包中，栅格地图用来表示机器人周围的环境，其中每个栅格（cell）可以表示为无障碍（free space）、障碍（obstacle）或未知区域（unknown）。这些栅格的代价值（cost values）用于路径规划和避障决策。costmap_2d定义了几个特定的代价值来表示这些不同类型的栅格：
- **无障碍点（Free Space）**的代价值通常是 0。这表示该区域是完全安全的，机器人可以自由通过，没有障碍物。
【补充说明----FREE_SPACE】在costmap_2d中，FREE_SPACE代表无障碍空间的代价值，其定义为 0。这意味着该区域是完全安全的，没有障碍物，机器人可以自由通过。
-
**障碍点（Lethal Obstacle）**的代价值通常是 254。这表示该区域有一个障碍物，机器人不应该尝试穿过这个区域。
-
**未知区域（Unknown）**的代价值是 255。这表示该区域的信息未知，机器人没有足够的信息来判断该区域是否安全。
【补充说明----NO_INFORMATION】在costmap_2d中，NO_INFORMATION代表未知区域的代价值，其定义为 255。这表示该区域的信息未知，机器人没有足够的信息来判断该区域是否安全或可通行。
除了这些特定的值，costmap_2d还可以有介于0和254之间的代价值，这些代价值表示区域的通行成本。成本越高，表示该区域越难以通过（例如，可能是松软的地面或斜坡等）。路径规划算法使用这些成本来寻找从起点到终点成本最低的路径。
了解以上内容后，现在开始正式介绍，首先，基于机器人当前到的位置，调用nearestCell函数，尝试找到当前位置附近的一个代价值为FREE_SPACE点，查找的方式依然是使用BFS广度优先算法，如果成功找到，则以该点作为查找与未知区域边界的起点，如果未能找到，则以机器人的当前位置，作为查找与未知区域边界的起点。
确定了搜索的起点后，同样采用BFS广度优先搜索算法，基于该起点开始搜索，将其存放到BFS队列中，进行循环。在每次循环中会从BFS队列的头部弹出一个点作为本轮循环的扩展点，并将其从队列中移除。依次对该扩展点的上下左右四个相邻点进行检查，将相邻点中的代价值小于等于当前扩展点（自由空间点）且未被访问过的点，加入BFS探索队列中。若相邻点中存在未被访问过的代价值为NO_INFORMATION点，且该点的上下左右四联通邻域内至少有一个代价值为FREE_SPACE的点，则该点为新发现的前沿点（该过程通过调用isNewFrontierCell函数实现），然后基于新发现的前沿点，调用buildNewFrontier函数，进一步获取一整条与未知区域的边界，然后判断获取的边界大小是否大于设定的阈值min_frontier_size_，若是，则认为找到了一条新的与未知区域的边界，将其添加到边界列表frontier_list中，循环执行以上过程，直至BFS探索队列为空，探索完成。
【补充说明----成为新前沿点的条件】成为新的前沿点需要同时满足，未被访问过、代价值为NO_INFORMATION、且四邻域内至少存在一个代价值为FREE_SPACE的点，这三个条件。
【补充说明----由前沿点获取其所在的整条与未知区域的边界的方法】由一个前沿点获取其所在的整条与未知区域的边界，也是通过BFS广度优先搜索实现的，具体来说，先将该前沿点放入该过程的BFS探索队列，然后进入循环探索，每轮循环中，先从队列的头部弹出一个点，作为当前扩展点，然后调用isNewFrontierCell函数检查其八邻域的邻居中是否存在可以成为新的前沿点的点，并将新找的的前沿点添加到该条边界中，并更新该条边界的区域大小、质心、与参考点的最小距离以及最靠近参考点的中心点。此外， 还会将新找的的前沿点加入到BFS探索队列中，循环执行以上探索过程，直至BFS探索队列为空，此时已经找出了与最初给定的前沿点连通的一整条与未知区域的边界，返回该边界。
探索完成后，计算边界列表frontier_list中找到到所有前沿边界的成本，并按成本从小到大对其进行排序，成本的计算考虑了前沿的大小和与机器人当前位置的距离，以便优先探索那些更接近且更大的前沿，返回按成本排序的前沿边界列表。
三、效果演示
1、长时间未到达当前目标边界时将其添加到黑名单演示，如下图中的红色边界所示：
–
2、当机器人移动后，更新未知区域边界，黑名单边界被取代演示：
–
3、正常进行未知区域自主探索及建图效果演示：
–
–
–
四、使用功能包explore_lite时机器人一直在原地转圈的解决方法
我在使用功能包explore_lite进行未知环境自动探索及建图时，遇到了差速机器人一直在原地转圈，无法前往目标边界，从而导致探索失败的问题，如本文第三部分的第一个动态示意图所示，最初，我认为时局部路径规划器dwa的参数没有调好，然而根据我以往参加比赛的调参经验，调了半天，并没有什么效果。
这个时候，我注意到终端时不时会打印如下的报错信息
[ WARN] [1712023104.265382383, 269.304000000]: Could not transform the global plan to the frame of the controller
[ERROR] [1712023104.265411012, 269.304000000]: Could not get local plan
[DEBUG] [1712023104.303940944, 269.335000000]: Getting status over the wire.
[ INFO] [1712023104.398306569, 269.404000000]: Got new plan
[ERROR] [1712023104.398545889, 269.404000000]: Extrapolation Error: Lookup would require extrapolation -0.015000000s into the future. Requested time 269.372000000 but the latest data is at time 269.357000000, when looking up transform from frame [odom] to frame [map]
[ERROR] [1712023104.398585837, 269.404000000]: Global Frame: odom Plan Frame size 117: map
[ WARN] [1712023104.398602316, 269.404000000]: Could not transform the global plan to the frame of the controller
[ERROR] [1712023104.398628230, 269.404000000]: Could not get local plan
[ INFO] [1712023104.513341551, 269.504000000]: Got new plan
[ERROR] [1712023104.513538569, 269.504000000]: Extrapolation Error: Lookup would require extrapolation -0.014000000s into the future. Requested time 269.472000000 but the latest data is at time 269.458000000, when looking up transform from frame [odom] to frame [map]
从其中提取的关键报错信息为：
Extrapolation Error: Lookup would require extrapolation -0.015000000s into the future. Requested time 269.372000000 but the latest data is at time 269.357000000, when looking up transform from frame [odom] to frame [map]
这个错误是ROS尝试在两个不同的坐标帧（odom和map）之间进行变换查询时出现的。这个错误通常表明试图查询的时间点超出了变换数据的最新时间点，也就是说，试图查找的时间点比系统中变换数据的最新时间还要晚。
在纯电脑仿真情况下，大概率应该不是时间不同步的问题，很有可能是AMCL中odom到map的tf变换太慢，达不到导航的要求，也就是设定的激光雷达的采样频率太低造成的。
第一种解决方法是，在雷达的xacro文件中调高雷达扫描频率 。第二种方法是采用偷懒的办法，既然odom到map的变化达不到导航要求，那就是直接不变了，将local_costmap参数配置文件中的global_frame改为和global_costmap的global_frame一样，都用map，不再使用odom。
我采用的是上面第二种解决方法，成功解决了该问题成功运行了未知环境的自主探索和建图，如第三部分的第3个动态演示图所示。
五、ROS机器人未知环境自主探索大地图测试效果演示
ROS机器人未知环境自主探索大地图测试效果演示【点击此处可跳转】
https://www.bilibili.com/video/BV15m42177fG/
1321
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
