足迹(footprint)

足迹表示机器人的轮廓，只能是一个多边形，或一个圆形。不论哪种形态，都用std::vector<geometry_msgs::Point>进行存储，对每个Point，z分量都是0。文中把这些Point称为顶点。对多边形，顶点数目是多边形中点的数目，圆形则固定用16个顶点。

一、配置和变量

配置涉及到两处变量，这些变量单位都是米。

param参数

多边形时
footprint = "[[-0.1, -0.06], [0.08, -0.06], [0.1, 0.0], [0.08, 0.06], [-0.1, 0.06]]";

“[[], ... []]”不是xml-prc内置格式，<costmap_2d>/src/array_parser.cpp中的parseVVF负责把这字符串解析成std::vector<std::vector<float> >。后者再转成std::vector<geometry_msgs::Point>。

至少需要3个顶点。

圆形时
robot_radius = 1.234;

轮廓半径，当中取16个点。

动态配置参数

footprint_padding = 0.01

在param参数的基础上，整个轮廓向外延伸footprint_padding米，延伸后轮廓将做为最终的footprint。

图1中的setUnpaddedRobotFootprint显示了如何从参数生成footprint，调用时机是在构造Costmap2DROS时。

• std::vector<geometry_msgs::Point> unpadded_footprint_。未填充过的足迹，即直接从param参数“footprint”或“robot_radius”算出的足迹。由于它不可直接用于计算导航，很少会用它。
• float footprint_padding_。来自动态配置参数footprint_padding。图1中值是0.01，即1厘米。
• std::vector<geometry_msgs::Point> padded_footprint_。填充过的足迹。unpadded_footprint_加上footprint_padding_后的足迹。它就是导航过程中机器人的footprint，读取它的api是getRobotFootprint()。

以上三个都是Costmap2DROS成员变量，另外LayeredCostmap.footprint_，ObstacleCostFunction.footprint_spec_，都等于填充过的足迹Costmap2DROS.padded_footprint_。

二、计算内切圆、外切圆

• footprint中心是base_footprint坐标系原点。
• 两个圆的圆点一定是footprint中心。
• 在计算时，会算出两种距离：原点到顶点的距离vertex_dist，原点到线段的距离edge_dist。
• 内切圆半径：所有距离的最小值。
• 外切圆半径：所有距离的最大值。

calculateMinAndMaxDistances用于计算这两个半径。输入参数footprint是填充过的足迹padded_footprint_，即图中红色五边形。输出参数min_dist存储着内切圆半径，max_dist则存储外切圆半径。

<libros>/costmap_2d/src/footprint.cpp
------
void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
{
  min_dist = std::numeric_limits<double>::max();
  max_dist = 0.0;
 
  if (footprint.size() <= 2)
  {
    return;
  }
 
  for (unsigned int i = 0; i < footprint.size() - 1; ++i)
  {
    // check the distance from the robot center point to the first vertex
    double vertex_dist = distance(0.0, 0.0, footprint[i].x, footprint[i].y);
    double edge_dist = distanceToLine(0.0, 0.0, footprint[i].x, footprint[i].y,
                                      footprint[i + 1].x, footprint[i + 1].y);
    min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
    max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
  }
 
  // we also need to do the last vertex and the first vertex
  double vertex_dist = distance(0.0, 0.0, footprint.back().x, footprint.back().y);
  double edge_dist = distanceToLine(0.0, 0.0, footprint.back().x, footprint.back().y,
                                      footprint.front().x, footprint.front().y);
  min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
  max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
}

distanceToLine用于计算原点(0, 0)到线段距离，代码注释见“costmap_2d中计算footprint的内切圆半径和外切圆半径的函数解析——点到线段的距离计算^[1] ”。

三、代价地图中足迹

• 黑色框是rivz收到footprint话题后画出的足迹。中间红色是机器人中心（base_footprint坐标系原点）所在栅格。
• 第一行；航偏角(yaw)0度，背后放一个宽度9cm的障碍物。机器人、障碍物都不动时，然后不同时刻从rviz截出来的三张图。
• 第二行；航偏角(yaw)49度。机器人不动时，不同时刻从rviz截出来的两张图。
• 青色是Costmap2D::setConvexPolygonCost会涉及到的栅格（包括1-2、2-2障碍物突进的那个栅格。
• 1-1、1-2、1-3左侧和2-2左下红色是9cm宽障碍物。

rviz画足迹框不是以栅格为单位，是按world坐标系的米。

机器人中心栅格在足迹框中的位置不是固定的。以图中yaw=0为例，在x方向，1-1和1-2不一样。类似的，y方向也会不一样。

即使足迹是矩形，代价地图出来的足迹可能不是矩形。示例见1-3。

有障碍物毗邻机器人时，虽然现实看到它们没有相互交织，但在代价地图会出现，即共同栅格。要理解这个，栅格对应现实世界5cmx5cm面积，如果机器人和障碍物都有出现在这面积，它们自然要共用。于是可得出个结论：Costmap2D::setConvexPolygonCost(..., FREE_SPACE)会清除毗邻障碍物。

代价地图中栅格数并不严格等于按footprint数值算出的栅格数。以图中yaw=0为例，按footprint数值算的话，水平、垂直都6个栅格，但3张图的重直方向都是7个栅格。

由于激光雷达存在随机误差，导致9cm障碍物并不会总是占用两栅格。

四、transformFootprint

<libros>/costmap_2d/src/footprint.cpp
------
namespace costmap_2d
{
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
                        geometry_msgs::PolygonStamped& oriented_footprint)
{
  // build the oriented footprint at a given location
  oriented_footprint.polygon.points.clear();
  double cos_th = cos(theta);
  double sin_th = sin(theta);
  for (unsigned int i = 0; i < footprint_spec.size(); ++i)
  {
    geometry_msgs::Point32 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.polygon.points.push_back(new_pt);
  }
}
}

oriented_footprint存储着把机器人中心放在(x, y)，并且旋转theta角度后，size()个顶点在x、y所属的坐标系下的坐标。costmap_2d::transformFootprint(...)有着一样代码。