ROS Navigation的costmap_2d类继承关系与实现方法

costmap_2d

costmap的计算

costmap表示的是地图中栅格的cost,cost的计算方法是:

上图将地图中栅格的cost分为五部分，其中红色多边形表示机器人的轮廓:
（1） Lethal （致命的):机器人的中心与该网格的中心重合，此时机器人必然与障碍物冲突。
（2） Inscribed（内切圆):网格的外切圆与机器人的轮廓内切，此时机器人也必然与障碍物冲突。
（3） Possibly circumscribed（外切圆):栅格与机器人的轮廓外切，此时机器人相当于靠在障碍物附近，所以不一定冲突。
（4） Freespace（自由空间）：没有障碍物的空间。
（5） Unknown（未知）：未知的空间。
对应的cost的计算:exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)

// costmap_2d/inflation_layer.h
  inline unsigned char computeCost(double distance) const
  {
    unsigned char cost = 0;
    if (distance == 0)
      cost = LETHAL_OBSTACLE;
    else if (distance * resolution_ <= inscribed_radius_)
      cost = INSCRIBED_INFLATED_OBSTACLE;                //254
    else
    {
      // make sure cost falls off by Euclidean distance cost 0~255
      double euclidean_distance = distance * resolution_;
      double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
      cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
    }
    return cost;
  }

costmap由使用的是layeredMap的方式，如下图:

图中红色栅格表示障碍物，绿色表示障碍物根据机器人的内切圆膨胀形成的，红色多边形表示机器人。机器人需要保证机器人的多边形轮廓不与红色栅格区域相交、机器人的中心不到达蓝色膨胀区域。