wap 网站,网站做pc,贵阳做网站,东莞中赢网站建设公司怎么样本系列文章主要针对ROS机器人常使用的未知环境自主探索功能包explore_lite展开全源码的详细解析#xff0c;并进行概括总结。 本系列文章共包含六篇文章#xff0c;前五篇文章主要介绍explore_lite功能包中 explore.cpp、costmap_tools.h、frontier_search.cpp、costmap_clie…   本系列文章主要针对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::vectorunsigned int nhood4(unsigned int idx,const costmap_2d::Costmap2D costmap) {// get 4-connected neighbourhood indexes, check for edge of mapstd::vectorunsigned 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::vectorunsigned int nhood4(unsigned int idx,const costmap_2d::Costmap2D costmap) {std::vectorunsigned 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::vectorunsigned int nhood8(unsigned int idx,const costmap_2d::Costmap2D costmap) {// get 8-connected neighbourhood indexes, check for edge of mapstd::vectorunsigned 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::vectorunsigned 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 searchstd::queueunsigned int bfs;std::vectorbool visited_flag(size_x * size_y, false);// push initial cellbfs.push(start);visited_flag[start] true;// search for neighbouring cell matching valuewhile (!bfs.empty()) {unsigned int idx bfs.front();bfs.pop();// return if cell of correct value is foundif (map[idx] val) {result idx;return true;}// iterate over all adjacent unvisited cellsfor (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::queueunsigned int bfs; std::vectorbool 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 foundif (map[idx] val) {result idx;return true;}// iterate over all adjacent unvisited cellsfor (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::vectorFrontier FrontierSearch::searchFrom(geometry_msgs::Point position) {std::vectorFrontier frontier_list;// Sanity check that robot is inside costmap bounds before searchingunsigned 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 searchstd::lock_guardcostmap_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 cellsstd::vectorbool frontier_flag(size_x_ * size_y_, false);std::vectorbool visited_flag(size_x_ * size_y_, false);// initialize breadth first searchstd::queueunsigned int bfs;// find closest clear cell to start searchunsigned 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 neighbourhoodfor (unsigned nbr : nhood4(idx, *costmap_)) {// 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);if (new_frontier.size * costmap_-getResolution() min_frontier_size_) {frontier_list.push_back(new_frontier);}}}}// set costs of frontiersfor (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_guardcostmap_2d::Costmap2D::mutex_t lock(*(costmap_-getMutex())); map_ costmap_-getCharMap(); size_x_ costmap_-getSizeInCellsX(); size_y_ costmap_-getSizeInCellsY();为了确保在搜索过程中地图数据的一致性使用std::lock_guard来锁定成本地图。这可以防止在搜索过程中地图数据被其他线程修改。获取成本地图的原始字符映射以及地图的尺寸。 初始化访问和前沿标志数组 std::vectorbool frontier_flag(size_x_ * size_y_, false); std::vectorbool visited_flag(size_x_ * size_y_, false);初始化两个布尔数组用于记录每个单元格是否被访问过以及是否被标记为前沿。这些数组对应于成本地图中的每个单元格。 执行广度优先搜索BFS查找前沿 std::queueunsigned 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类能够从给定位置开始搜索成本地图中的前沿区域并根据一定的标准如前沿的大小和距离对它们进行排序为自动探索提供了基础。