costmap_2d 中计算footprint 的内切圆半径和外切圆半径的函数解析——点到线段的距离计算

计算footprint 的内切圆半径和外切圆半径，用到点到线段的距离，计算方法

两个向量的点积等于一个向量在另一个向量的投影 乘以 另一个向量的模

点到线段的三种情况如下:

/*作用：计算点到线段的距离
*参数：pX，pY 是中心点坐标
* x0，y0，x1，y2 线段两端的坐标，求点到线段的距离
*/
/*
*用向量点乘 来计算
*/
double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
{
  double A = pX - x0; //向量AP的x值
  double B = pY - y0; //向量AP 的y中
  double C = x1 - x0; //向量AB的x值
  double D = y1 - y0; //向量AB的y值
  //向量AP与向量AB的点积
  double dot = A * C + B * D;
  //向量AB的模的平方
  double len_sq = C * C + D * D;
  //r 的值
  double param = dot / len_sq;

  double xx, yy;

  if (param < 0)
  {
    xx = x0;
    yy = y0;
  }
  else if (param > 1)
  {
    xx = x1;
    yy = y1;
  }
  else
  {
    //向量AC=r*向量AB
    xx = x0 + param * C;
    yy = y0 + param * D;
  }
  //计算AC的距离即可
  return distance(pX, pY, xx, yy);
}