本篇记录局部规划器生成路径时用到的几个“辅助工具”,内容简单也比较少。MapCell类与MapGrid类用于获取轨迹点与目标点/全局路径之间的距离,为路径打分提供参考,CostmapModel类则能够获取点、连线、多边形边缘(机器人足迹)的cost,是局部规划器与costmap间的一个桥梁。
【结构示意图】
【相关文件】
- base_local_planner/src/trajectory_planner_ros.cpp
- base_local_planner/src/trajectory_planner.cpp
- base_local_planner/src/map_grid.cpp
- base_local_planner/src/map_cell.cpp
- base_local_planner/src/costmap_model.cpp
本篇记录对MapCell、MapGrid类和CostmapModel类阅读和理解。
【代码分析】
一、 MapCell类与MapGrid类
这两个类是局部规划器专门用来确定各cell与目标点和全局规划路径的距离的,它们是TrajectoryPlanner的成员,在为路径打分时被使用。
- MapGrid类数据成员:
public:
double goal_x_, goal_y_; /**< @brief The goal distance was last computed from */
unsigned int size_x_, size_y_; ///< @brief The dimensions of the grid
private:
std::vector<MapCell> map_; ///< @brief Storage for the MapCells
MapGrid是“地图”,它包含一个由MapCell类对象数组的成员,即cell数组,地图大小为size_x_×size_y_。goal_map和path_map都是MapGrid类实例,这里就分别称这两张地图为 “目标地图” 和 “路径地图”。
- MapCell类数据成员:
public:
unsigned int cx, cy; ///< @brief Cell index in the grid map
double target_dist; ///< @brief Distance to planner's path
bool target_mark; ///< @brief Marks for computing path/goal distances
bool within_robot; ///< @brief Mark for cells within the robot footprint
MapCell就代表地图上的一个cell,它记录x、y坐标(索引)。target_dist表示目标距离,它被初始化为无穷大,经过path_map和goal_map的计算后,它可以表示该cell距目标点/全局路径的距离。target_mark是该点已更新过的flag。within_robot表示该点在机器人足迹范围内。
二、MapGrid::setTargetCells 处理路径地图
首先调用类内sizeCheck函数,检查路径地图尺寸是否和costmap相同,若不同,则按照costmap尺寸对其重新设置,并且检查MapCell在MapGrid中的index是否和它本身的坐标索引一致。
void MapGrid::setTargetCells(const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan) {
sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
bool started_path = false;
queue<MapCell*> path_dist_queue; //用于储存全局路径上的MapCell
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
传入的全局规划是global系下的,调用adjustPlanResolution函数对其分辨率进行一个简单的调整,使其达到costmap的分辨率,方法也很简单,当相邻点之间距离>分辨率,在其间插入点,以达到分辨率要求。
// 调整全局路径的分辨率使其能够达到cost_map的分辨率
adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
if (adjusted_global_plan.size() != global_plan.size()) {
ROS_DEBUG("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
}
unsigned int i;
接下来将全局路径的点转换到“路径地图”上,并将对应cell的target_dist设置为0,标记已计算过。由于分辨率已经过调整,故不会出现路径不连续的情况。在“路径地图”上得到一条在地图范围内、且不经过costmap上未知cell的全局路径,每个点的target_dist都为0。
for (i = 0; i < adjusted_global_plan.size(); ++i) {
double g_x = adjusted_global_plan[i].pose.position.x;
double g_y = adjusted_global_plan[i].pose.position.y;
unsigned int map_x, map_y;
// 如果成功把一个全局规划上的点的坐标转换到地图坐标(map_x, map_y)上,且在代价地图上这一点不是未知的
if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
// 用current来引用这个cell
MapCell& current = getCell(map_x, map_y);
// 将这个点的target_dist设置为0,即在全局路径上
current.target_dist = 0.0;
// 标记已经计算了距离
current.target_mark = true;
// 把该点放进path_dist_queue队列中
path_dist_queue.push(¤t);
// 标记已经开始把点转换到地图坐标
started_path = true;
}
// 当代价地图上这一点的代价不存在了(规划路径已经到达了代价地图的边界)
// 并且标记了已经开始转换,退出循环
else if (started_path) {
break;
}
}
// 如果循环结束后并没有确定任何在地图坐标上的点
if (!started_path) {
ROS_ERROR("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
i, adjusted_global_plan.size(), global_plan.size());
return;
}
接下来调用类内computeTargetDistance函数,开始“填充”,最终得到一张完整的路径地图。
// 计算本地地图上的每一个cell与规划路径之间的距离
computeTargetDistance(path_dist_queue, costmap);
}
三、MapGrid::setLocalGoal 处理目标地图
同样,先调整全局规划的分辨率。
void MapGrid::setLocalGoal(const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan) {
sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
int local_goal_x = -1;
int local_goal_y = -1;
bool started_path = false;
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
然后,通过迭代找到全局路径的终点,即目标点,但如果迭代过程当中到达了局部规划costmap的边际或经过障碍物,立即退出迭代,将上一个有效点作为终点。
// skip global path points until we reach the border of the local map
for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i) {
double g_x = adjusted_global_plan[i].pose.position.x;
double g_y = adjusted_global_plan[i].pose.position.y;
unsigned int map_x, map_y;
if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
// 不断迭代local_goal_x、local_goal_y,尝试找到终点
local_goal_x = map_x;
local_goal_y = map_y;
started_path = true;
} else {
if (started_path) {
break;
}// else we might have a non pruned path, so we just continue
}
}
if (!started_path) {
ROS_ERROR("None of the points of the global plan were in the local costmap, global plan points too far from robot");
return;
}
将迭代得到的目标点对应在“目标地图”上的cell的target_dist标记为0。setTargetCells和setLocalGoal这两个函数,前者在“路径地图”上将全局路径点target_dist标记为0,后者在“目标地图”上将目标点target_dist标记为0。最后同样调用computeTargetDistance函数。
queue<MapCell*> path_dist_queue;
// 如果找到的目标点横纵坐标都大于0
if (local_goal_x >= 0 && local_goal_y >= 0) {
MapCell& current = getCell(local_goal_x, local_goal_y);
costmap.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push(¤t);
}
computeTargetDistance(path_dist_queue, costmap);
}
依次检查“父cell”四周的cell并标记它已计算,对它调用updatePathCell函数,得到该cell的值。若有效,加入循环队列里,弹出“父cell”,四周的cell成为新的“父cell”, 循环,直到所有cell更新完毕。
check_cell其实是children_cell,是current_cell的子节点,computeTargetDistance其实是个dijstra算法
// 如果该点的横坐标大于0
if(current_cell->cx > 0){
// 检查它左侧相邻的点check_cell
check_cell = current_cell - 1;
// 如果check_cell未被标记
if(!check_cell->target_mark){
// 标记它
check_cell->target_mark = true;
// updatePathCell功能:如果check_cell不是障碍物,将target_dist设置为current_cell.target_distance+1;否则,设置为无穷
if(updatePathCell(current_cell, check_cell, costmap)) {
// check_cell入队
dist_queue.push(check_cell);
}
}
}
// 如果该点的横坐标小于图像宽度-1
if(current_cell->cx < last_col){
// 检查它右侧相邻的点check_cell
check_cell = current_cell + 1;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
// 如果该点的纵坐标>0
if(current_cell->cy > 0){
// 检查它上面的点
check_cell = current_cell - size_x_;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
// 如果该点的纵坐标<图像长度-1
if(current_cell->cy < last_row){
// 检查它下面的点
check_cell = current_cell + size_x_;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
// 所以已经计算过的点不断被加入,然后利用它找到周围点后,弹出它,直到地图上所有点的距离都被计算出来
}
}
};
关于updatePathCell函数,先获取该cell在costmap上的值,若发现它是障碍物或未知cell或它在机器人足迹内,直接返回false,该cell将不会进入循环队列,也就是不会由它继续向下传播;若非以上情况,让它的值=邻点值+1,若<原值,用它更新。
inline bool MapGrid::updatePathCell(MapCell* current_cell, MapCell* check_cell,
const costmap_2d::Costmap2D& costmap){
unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
if(! getCell(check_cell->cx, check_cell->cy).within_robot &&
(cost == costmap_2d::LETHAL_OBSTACLE ||
cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
cost == costmap_2d::NO_INFORMATION)){
check_cell->target_dist = obstacleCosts();
return false;
}
// 不是障碍物的话,比较它的target_dist(本来通过resetPathDist被设置为unreachableCellCosts)与>current_cell->target_dist + 1
double new_target_dist = current_cell->target_dist + 1;
if (new_target_dist < check_cell->target_dist) {
check_cell->target_dist = new_target_dist;
}
return true;
}
这和全局规划器使用Dijkstra算法时用costarr生成potarr很类似,过程中的“累加”能够反映各cell与初始cell之间的距离,这样,最终得到完整的path_map和goal_map。
四、CostmapModel类
这个类帮助局部规划器在Costmap上进行计算,footprintCost、lineCost、pointCost三个类方法分别能通过Costmap计算出机器人足迹范围、两个cell连线、单个cell的代价,并将值返回给局部规划器。这里简单看一下footprintCost函数。
若预设的足迹点数<3,考虑足迹的形状没有意义,这时只计算机器人位置点在costmap上的代价;
若预设的足迹点数≥3,把足迹视为多边形,循环调用lineCost计算多边形各边的cell,注意首尾闭合,* 最后返回代价。
返回值:-1.0 :覆盖至少一个障碍cell
-2.0 :覆盖至少一个未知cell
-3.0 :不在地图上
其他正cost
为什么这里只要多边形边缘不经过障碍就认为足迹不经过障碍呢?而不是多边形内部所有的点都要检测呢?
1.因为认为机器人是实心的,一定是最外面的边缘线碰到障碍物,所以是检测多边形的边缘
- 因为这个函数是在轨迹点生成过程中,对新生成的轨迹点调用的,而既然能新生成轨迹点,说明该轨迹点必不是障碍物,当多边形边缘不经过障碍、中心也不是障碍的时候,如果机器人比较小,可以认为足迹不经过障碍。
double CostmapModel::footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius){
//声明地图坐标系上的坐标
unsigned int cell_x, cell_y;
/****************************************考虑机器人中心******************************************/
//得到机器人中心点的cell坐标,存放在cell_x cell_y中
//如果得不到坐标,说明不在地图上,直接返回-3
if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
return -3.0;
//如果脚印(预先输入的)点数小于三,默认机器人形状为圆形,不考虑脚印,只考虑中心
if(footprint.size() < 3){
unsigned char cost = costmap_.getCost(cell_x, cell_y);
//如果中心位于未知代价的cell上,返回-2
if(cost == NO_INFORMATION)
return -2.0;
//如果中心位于致命障碍cell上,返回-1
if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE) //这几个宏是在costvalue中定义的,是灰度值
return -1.0;
//如果机器人位置既不是未知也不是致命,返回它的代价
return cost;
}
/****************************************考虑机器人脚印******************************************/
//接下来在costmap上考虑脚印
unsigned int x0, x1, y0, y1;
double line_cost = 0.0; //初始化
double footprint_cost = 0.0; //初始化0
//对footprint进行光栅化算法处理得到栅格点
//计算各个脚印点连成的多边形的边缘上是否碰撞到障碍物
//i是索引,大小是脚印的size,第一次,i=1,下面得到footprint[1]和footprint[2]的连线的cost,后面以此类推
for(unsigned int i = 0; i < footprint.size() - 1; ++i){
//得到脚印中第一个点的坐标x0, y0
if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
//如果无法转换,说明第一个脚印点不在地图上,直接返回-3
return -3.0;
//得到脚印中下一个点的坐标x1, y1
if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
//如果无法转换,说明下一个脚印点不在地图上,直接返回-3
return -3.0;
//得到连线的代价
line_cost = lineCost(x0, x1, y0, y1);
//不断用最大的连线代价迭代footprint_cost
footprint_cost = std::max(line_cost, footprint_cost);
//如果某条边缘线段代价<0,直接停止生成代价,返回这个负代价
if(line_cost < 0)
return line_cost;
}
//再把footprint的最后一个点和第一个点连起来,形成闭合
if(!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
return -3.0;
if(!costmap_.worldToMap(footprint.front().x, footprint.front().y, x1, y1))
return -3.0;
line_cost = lineCost(x0, x1, y0, y1);
footprint_cost = std::max(line_cost, footprint_cost);
if(line_cost < 0)
return line_cost;
//如果所有边缘线的代价都是合法的,那么返回足迹的代价
return footprint_cost;
}
转载:https://blog.csdn.net/Neo11111/article/details/104720103#commentBox