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

最新推荐文章于 2023-10-20 16:40:42 发布

VIP文章不学好学什么

最新推荐文章于 2023-10-20 16:40:42 发布

阅读量1.3k

点赞数

分类专栏：移动机器人 slam的原理

本文链接：https://blog.csdn.net/Nksjc/article/details/78820524

版权

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;
  }