move_base参数 footprint 阅读

参数

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而不是将其看做一个质点，本身代价地图就无需膨胀了。