本文系列深入解析ROS机器人环境未知时使用的explore_lite库，涵盖源码中关键函数如updateFullMap和updatePartialMap，介绍了成本地图的处理与更新机制，以及如何进行自主探索和障碍物处理。
本系列文章主要针对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中
十九、updateFullMap函数
1、函数源码
void Costmap2DClient::updateFullMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
global_frame_ = msg->header.frame_id;
unsigned int size_in_cells_x = msg->info.width;
unsigned int size_in_cells_y = msg->info.height;
double resolution = msg->info.resolution;
double origin_x = msg->info.origin.position.x;
double origin_y = msg->info.origin.position.y;
ROS_DEBUG("received full new map, resizing to: %d, %d", size_in_cells_x,
size_in_cells_y);
costmap_.resizeMap(size_in_cells_x, size_in_cells_y, resolution, origin_x,
origin_y);
// lock as we are accessing raw underlying map
auto* mutex = costmap_.getMutex();
std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
// fill map with data
unsigned char* costmap_data = costmap_.getCharMap();
size_t costmap_size = costmap_.getSizeInCellsX() * costmap_.getSizeInCellsY();
ROS_DEBUG("full map update, %lu values", costmap_size);
for (size_t i = 0; i < costmap_size && i < msg->data.size(); ++i) {
unsigned char cell_cost = static_cast<unsigned char>(msg->data[i]);
costmap_data[i] = cost_translation_table__[cell_cost];
}
ROS_DEBUG("map updated, written %lu values", costmap_size);
}
–
2、主要作用：
Costmap2DClient::updateFullMap函数的主要作用是处理完整的成本地图（costmap）更新。当收到一个新的nav_msgs::OccupancyGrid消息时，此函数将使用该消息更新内部的成本地图表示，这对于机器人的导航和路径规划至关重要。
3、详细介绍：
更新成本地图的基本信息
global_frame_ = msg->header.frame_id;
unsigned int size_in_cells_x = msg->info.width;
unsigned int size_in_cells_y = msg->info.height;
double resolution = msg->info.resolution;
double origin_x = msg->info.origin.position.x;
double origin_y = msg->info.origin.position.y;
ROS_DEBUG("received full new map, resizing to: %d, %d", size_in_cells_x,
size_in_cells_y);
costmap_.resizeMap(size_in_cells_x, size_in_cells_y, resolution, origin_x,
origin_y);
- 首先，函数更新了全局坐标系名称（
global_frame_）以匹配接收到的成本地图消息的帧ID。 - 然后，它读取并设置了成本地图的尺寸、分辨率和原点位置。
- 使用
ROS_DEBUG打印接收到的成本地图的尺寸信息，并调用resizeMap函数调整内部成本地图的大小以匹配接收到的成本地图。
加锁并更新成本地图数据
auto* mutex = costmap_.getMutex();
std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
unsigned char* costmap_data = costmap_.getCharMap();
size_t costmap_size = costmap_.getSizeInCellsX() * costmap_.getSizeInCellsY();
ROS_DEBUG("full map update, %lu values", costmap_size);
for (size_t i = 0; i < costmap_size && i < msg->data.size(); ++i) {
unsigned char cell_cost = static_cast<unsigned char>(msg->data[i]);
costmap_data[i] = cost_translation_table__[cell_cost];
}
ROS_DEBUG("map updated, written %lu values", costmap_size);
- 在更新内部成本地图之前，通过获取成本地图的互斥锁来确保线程安全。
- 获取成本地图的原始数据指针和成本地图的大小。
- 遍历接收到的成本地图数据，将每个单元格的成本值通过之前定义的成本值转换表
cost_translation_table__转换后，更新到内部成本地图中。 - 使用
ROS_DEBUG打印更新的成本地图数据量信息。
通过updateFullMap函数的处理，Costmap2DClient能够确保内部成本地图始终保持最新，反映了环境的当前状态，为机器人提供准确的导航和规划信息。
二十、updatePartialMap函数
1、函数源码
void Costmap2DClient::updatePartialMap(
const map_msgs::OccupancyGridUpdate::ConstPtr& msg)
{
ROS_DEBUG("received partial map update");
global_frame_ = msg->header.frame_id;
if (msg->x < 0 || msg->y < 0) {
ROS_ERROR("negative coordinates, invalid update. x: %d, y: %d", msg->x,
msg->y);
return;
}
size_t x0 = static_cast<size_t>(msg->x);
size_t y0 = static_cast<size_t>(msg->y);
size_t xn = msg->width + x0;
size_t yn = msg->height + y0;
// lock as we are accessing raw underlying map
auto* mutex = costmap_.getMutex();
std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
size_t costmap_xn = costmap_.getSizeInCellsX();
size_t costmap_yn = costmap_.getSizeInCellsY();
if (xn > costmap_xn || x0 > costmap_xn || yn > costmap_yn ||
y0 > costmap_yn) {
ROS_WARN("received update doesn't fully fit into existing map, "
"only part will be copied. received: [%lu, %lu], [%lu, %lu] "
"map is: [0, %lu], [0, %lu]",
x0, xn, y0, yn, costmap_xn, costmap_yn);
}
// update map with data
unsigned char* costmap_data = costmap_.getCharMap();
size_t i = 0;
for (size_t y = y0; y < yn && y < costmap_yn; ++y) {
for (size_t x = x0; x < xn && x < costmap_xn; ++x) {
size_t idx = costmap_.getIndex(x, y);
unsigned char cell_cost = static_cast<unsigned char>(msg->data[i]);
costmap_data[idx] = cost_translation_table__[cell_cost];
++i;
}
}
}
–
2、主要作用：
Costmap2DClient::updatePartialMap函数负责处理成本地图的部分更新。当收到map_msgs::OccupancyGridUpdate消息时，此函数将更新内部成本地图的一部分，而不是整个成本地图，这通常用于实时更新成本地图中的变化区域，提高更新效率。
3、详细介绍：
接收部分地图更新
ROS_DEBUG("received partial map update");
global_frame_ = msg->header.frame_id;
if (msg->x < 0 || msg->y < 0) {
ROS_ERROR("negative coordinates, invalid update. x: %d, y: %d", msg->x,
msg->y);
return;
}
- 首先，打印接收到部分更新的调试信息，并更新
global_frame_为消息头中指定的帧ID。 - 然后，检查更新消息中的坐标是否为负值，负坐标表示无效更新，如果是这种情况，函数将打印错误信息并返回。
准备更新数据
size_t x0 = static_cast<size_t>(msg->x);
size_t y0 = static_cast<size_t>(msg->y);
size_t xn = msg->width + x0;
size_t yn = msg->height + y0;
- 接下来，计算更新区域的起始点
(x0, y0)和终点(xn, yn)坐标，这些坐标标识了需要更新的成本地图区域。
加锁并检查更新范围
auto* mutex = costmap_.getMutex();
std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
size_t costmap_xn = costmap_.getSizeInCellsX();
size_t costmap_yn = costmap_.getSizeInCellsY();
if (xn > costmap_xn || x0 > costmap_xn || yn > costmap_yn ||
y0 > costmap_yn) {
ROS_WARN("received update doesn't fully fit into existing map, "
"only part will be copied. received: [%lu, %lu], [%lu, %lu] "
"map is: [0, %lu], [0, %lu]",
x0, xn, y0, yn, costmap_xn, costmap_yn);
}
- 使用互斥锁保证在更新成本地图数据时的线程安全。
- 计算内部成本地图的尺寸，并检查接收到的更新是否完全适合现有的成本地图范围内。如果更新超出了现有地图的范围，则只会更新适合的部分，并打印警告信息。
更新成本地图数据
unsigned char* costmap_data = costmap_.getCharMap();
size_t i = 0;
for (size_t y = y0; y < yn && y < costmap_yn; ++y) {
for (size_t x = x0; x < xn && x < costmap_xn; ++x) {
size_t idx = costmap_.getIndex(x, y);
unsigned char cell_cost = static_cast<unsigned char>(msg->data[i]);
costmap_data[idx] = cost_translation_table__[cell_cost];
++i;
}
}
- 遍历指定的更新区域，在内部成本地图中更新相应单元格的成本值。成本值通过
cost_translation_table__转换表转换，以匹配内部使用的成本表示。 - 更新操作根据更新消息中的数据逐个单元格进行，只更新消息指定区域内的单元格。
通过这种方式，Costmap2DClient::updatePartialMap能够高效地更新成本地图中的特定区域，从而确保成本地图能够快速响应环境的变化，为机器人的导航和规划提供最新的信息。
二十一、getRobotPose函数
1、函数源码
geometry_msgs::Pose Costmap2DClient::getRobotPose() const
{
tf::Stamped<tf::Pose> global_pose;
global_pose.setIdentity();
tf::Stamped<tf::Pose> robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
robot_pose.stamp_ = ros::Time();
ros::Time current_time =
ros::Time::now(); // save time for checking tf delay later
// get the global pose of the robot
try {
tf_->transformPose(global_frame_, robot_pose, global_pose);
} catch (tf::LookupException& ex) {
ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot "
"pose: %s\n",
ex.what());
return {};
} catch (tf::ConnectivityException& ex) {
ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n",
ex.what());
return {};
} catch (tf::ExtrapolationException& ex) {
ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n",
ex.what());
return {};
}
// check global_pose timeout
if (current_time.toSec() - global_pose.stamp_.toSec() >
transform_tolerance_) {
ROS_WARN_THROTTLE(1.0, "Costmap2DClient transform timeout. Current time: "
"%.4f, global_pose stamp: %.4f, tolerance: %.4f",
current_time.toSec(), global_pose.stamp_.toSec(),
transform_tolerance_);
return {};
}
geometry_msgs::PoseStamped msg;
tf::poseStampedTFToMsg(global_pose, msg);
return msg.pose;
}
–
2、主要作用：
Costmap2DClient::getRobotPose函数的主要作用是获取机器人在成本地图所在的全局坐标系下的位置和姿态。这个过程通过查询ROS的tf变换系统来实现，确保了获取的机器人位置信息是与成本地图正确对齐的。
3、详细介绍：
初始化和设置变换查询
tf::Stamped<tf::Pose> global_pose;
global_pose.setIdentity();
tf::Stamped<tf::Pose> robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
robot_pose.stamp_ = ros::Time();
ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
- 首先，初始化两个
tf::Stamped<tf::Pose>对象：global_pose和robot_pose。global_pose用于存储转换后的全局位置，robot_pose表示机器人的当前位置。 robot_pose的帧ID被设置为机器人基座的坐标系（robot_base_frame_），时间戳设置为当前时间。
获取全局位置
try {
tf_->transformPose(global_frame_, robot_pose, global_pose);
} catch (tf::LookupException& ex) {
ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
return {};
} catch (tf::ConnectivityException& ex) {
ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
return {};
} catch (tf::ExtrapolationException& ex) {
ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
return {};
}
- 使用
tf::TransformListener的transformPose方法尝试将robot_pose从机器人基座坐标系转换到全局坐标系（global_frame_）。 - 这个转换过程可能会抛出几种异常，包括查找变换时的异常、连接问题或数据外推错误。遇到任何异常时，函数将输出错误信息并返回空的位置信息。
检查变换延迟
if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_) {
ROS_WARN_THROTTLE(1.0, "Costmap2DClient transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f", current_time.toSec(), global_pose.stamp_.toSec(), transform_tolerance_);
return {};
}
- 在成功获取全局位置后，检查变换信息的时效性，确保获取的位置信息没有超过预定的容忍延迟（
transform_tolerance_）。
返回机器人位置
geometry_msgs::PoseStamped msg;
tf::poseStampedTFToMsg(global_pose, msg);
return msg.pose;
- 最后，将
global_pose转换为geometry_msgs::PoseStamped类型，并返回其中的pose部分，即机器人在全局坐标系下的位置和姿态。
通过这种方式，Costmap2DClient::getRobotPose为机器人提供了一种可靠的方法来获取其在成本地图全局坐标系下的精确位置，这对于路径规划和导航决策非常关键。
二十二、init_translation_table函数
1、函数源码
std::array<unsigned char, 256> init_translation_table()
{
std::array<unsigned char, 256> cost_translation_table;
// lineary mapped from [0..100] to [0..255]
for (size_t i = 0; i < 256; ++i) {
cost_translation_table[i] =
static_cast<unsigned char>(1 + (251 * (i - 1)) / 97);
}
// special values:
cost_translation_table[0] = 0; // NO obstacle
cost_translation_table[99] = 253; // INSCRIBED obstacle
cost_translation_table[100] = 254; // LETHAL obstacle
cost_translation_table[static_cast<unsigned char>(-1)] = 255; // UNKNOWN
return cost_translation_table;
}
–
2、主要作用：
Costmap2DClient::updateFullMap函数的主要作用是处理完整的成本地图（costmap）更新。当收到一个新的nav_msgs::OccupancyGrid消息时，此函数将使用该消息更新内部的成本地图表示，这对于机器人的导航和路径规划至关重要。
3、详细介绍：
init_translation_table函数的主要作用是初始化一个成本转换表，这个表用于将成本地图中的原始值转换为用于路径规划和障碍物避让的内部值。这种转换是必要的，因为成本地图中的原始值通常表示障碍物的占据概率（例如，0到100的范围），而在路径规划中需要将这些值转换为特定的成本值。
初始化成本转换表
std::array<unsigned char, 256> cost_translation_table;
// lineary mapped from [0..100] to [0..255]
for (size_t i = 0; i < 256; ++i) {
cost_translation_table[i] =
static_cast<unsigned char>(1 + (251 * (i - 1)) / 97);
}
- 首先，创建一个
std::array<unsigned char, 256>类型的数组cost_translation_table，用于存储转换后的成本值。 - 通过循环遍历0到255的所有可能的输入值，使用线性映射公式
1 + (251 * (i - 1)) / 97计算对应的成本值。这个公式将原始值的范围（0到100）映射到一个更宽的范围（1到252），以便在内部使用。
设置特殊成本值
cost_translation_table[0] = 0; // NO obstacle
cost_translation_table[99] = 253; // INSCRIBED obstacle
cost_translation_table[100] = 254; // LETHAL obstacle
cost_translation_table[static_cast<unsigned char>(-1)] = 255; // UNKNOWN
- 接着，手动设置几个特殊的成本值：
- 将数组的第一个元素（代表没有障碍物的情况）的值设置为0。
- 将代表接触障碍物（inscribed obstacle）的值设置为253。
- 将代表致命障碍物（lethal obstacle）的值设置为254。
- 将代表未知区域的值设置为255。这里使用
static_cast<unsigned char>(-1)来访问数组的最后一个元素，因为-1转换为无符号字符类型时会变为255。
返回成本转换表
return cost_translation_table;
- 最后，函数返回填充完成的成本转换表。
通过这个成本转换表，Costmap2DClient可以将成本地图中的原始数据转换为实际用于路径规划和障碍物避让的成本值。这个转换过程对于实现更高级的导航和探索策略至关重要。
扫一扫
2458
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
