参数
movebase costmap中关于机器人关于机器人模型的描述中包含两个参数,robot_radius或者footprint,robot_radius没得说,当机器人是圆形时机器人的半径,而机器人是多边形时才用footprint参数。
首先,我们拿一个简单的矩形机器人来做示例,假如机器人长1.0米宽0.5米,首先我们参数设置为:
footprint: [[0.5, 0.25], [0.5, -0.25], [-0.5, -0.25], [-0.5, 0.25]]
首先在costmap_2d功能包costmap_2d_ros.cpp中get param然后将string转geometry_msgs::Point类型并将其保存在padded_footprint_和unpadded_footprint_,并通过
void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
函数计算出其内接和外接半径,内接半径就不用说了,长方形的宽/2 = 0.25m,外接半径则是长方形对角线/2 = 0.559。
内接半径用于膨胀地图,以障碍物为中心膨胀以内接半径为半径画圆,园内去心全部标记为INSCRIBED_INFLATED_OBSTACLE(代价地图中的青色部分).
static const unsigned char NO_INFORMATION = 255;
static const unsigned char LETHAL_OBSTACLE = 254;
static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
static const unsigned char FREE_SPACE = 0;
内接半径只用于全局路径规划,首先用内接半径肯定是不对的,例如矩形长为机器人x轴,机器人目标点离墙内接半径距离朝向墙,全局可以规划,但是如果到达该点机器人肯定撞了墙。
base_local_planner
那么就来到了局部路径规划中base_local_planner功能包的CostmapModel这个类。
可以直接检测多边形的cost,
其实原理很简单,就是检测闭合多边形每一条边的cost,找出最大值。
首先查看world_model.h下的函数
double footprintCost(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec, double inscribed_radius = 0.0, double circumscribed_radius=0.0)
参数footprint_spec及为我们在上面设置的多边形顶点,xytheta即机器人位姿
通过下面代码将多边形顶点转换到map坐标系下
for(unsigned int i = 0; i < footprint_spec.size(); ++i){
geometry_msgs::Point new_pt;
new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
oriented_footprint.push_back(new_pt);
}
然后调用costmap_model.cpp中的
double CostmapModel::footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius);
函数,该函数返回值
// returns:
// -1 if footprint covers at least a lethal obstacle cell, or
// -2 if footprint covers at least a no-information cell, or
// -3 if footprint is [partially] outside of the map, or
// a positive value for traversable space
footprint即为多边形机器人顶点在map坐标系下的所有坐标
首先从第一个点开始,计算每一条边上最大的cost
//we need to rasterize each line in the footprint
for(unsigned int i = 0; i < footprint.size() - 1; ++i){
//get the cell coord of the first point
if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
return -3.0;
//get the cell coord of the second point
if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
return -3.0;
line_cost = lineCost(x0, x1, y0, y1);
footprint_cost = std::max(line_cost, footprint_cost);
//if there is an obstacle that hits the line... we know that we can return false right away
if(line_cost < 0)
return line_cost;
}
下面再计算最后一个点到起点的边。
求出最大的cost,如果边上有障碍物LETHAL_OBSTACLE会返回-1
多边形只会计算障碍物层而忽略了膨胀层。
因为计算多边形每一条边的cost而不是将其看做一个质点,本身代价地图就无需膨胀了。