ROS导航包之costmap_2d

---

Costmap2DROS类

主要作用是完成对Costmap2D类的ROS接口包装，提供包括机器人在地图中的位置查询，机器人脚印（footprint）在地图中的位置查询； 地图的更新和停止接口； 提供Costmap2D的访问接口，通过它可以方便地访问地图数据信息。
关键函数接口：
Costmap2DROS(std::string name, tf::TransformListener& tf);
bool getRobotPose(tf::Stamped<tf::Pose>& global_pose) const;
Costmap2D* getCostmap()
std::vector<geometry_msgs::Point> getRobotFootprint()

---

LayeredCostmap类

该类是图层的管理类，负责管理图层插件的更新和并将信息汇总到根图层costmap上， 提供给Costmap2DROS的调用接口。

---

Costmap2D类

该类是删格地图的数据结构类，主要提供根据坐标或者索引设置和查询地图的值的方法。

---

Layer类

这是一个虚基类， 定义各种图层的公用接口。 
虚函数接口包括：
  virtual void onInitialize();
  virtual void activate();
  virtual void deactivate();
  virtual void reset();
  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,double* max_x, double* max_y);     
  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
  virtual void matchSize();

---

CostmapLayer类

顾名思义了，就是Layer和Costmap2D的结合类。 两个类功能的组合体。

---

插件类StaticLayer

继承自CostmapLayer类，实现了Layer类定义的虚函数。
其初始化函数中订阅了map消息，所以必须要有mapserver提供map给它才行，nh.param("map_topic", map_topic, std::string("map"));
nh.param("track_unknown_space", track_unknown_space_, true);
这个参数也尤其重要，它的意思是是否要将unknow认为是free的，若设置为false，那么所有的未知区域都将被认为是free的，这就是为什么有时候全局规划路径会穿过灰色的未知区域的原因，就是因为只是设置了nav_fn中的allow_unknown参数为false是不够的，还需要将这个地方设置成true。

关于该层数据的更新，如果不是滑动窗口类型,那么坐标框架就是和mapserver提供的map主题是一样的，只需要将订阅的数据copy过来更新自己的Costmap2D的数据成员就可以了， 如果是滑动窗口那么注意作者的注解如下：// If rolling window, the master_grid is unlikely to have same coordinates as this layer， Copy map data given proper transformations。 证明即使是滑动窗口也是会利用全局图map提供的数据的。

---

插件类ObstacleLayer

继承自CostmapLayer类，实现了Layer类定义的虚函数。
此外，还实现了对于传感器数据的监听与回调处理函数，支持类型包括sensor_msgs::LaserScan， sensor_msgs::PointCloud， sensor_msgs::PointCloud2三类，将它们压入observation_buffer中。
一个问题尚未弄清楚，为毛当局部动态图的frame设置成/map之后，就无法显示障碍物了。貌似必须是/odom才行，暂时没搞懂。验证后好像不是这样的QAQ

---

插件类InflationLayer

继承自Layer类，实现了其定义的虚函数接口。
总的来说，该类干了这样一件事情，读取父层LayeredCostmap上costmap的障碍物信息，并对范围内的每一个点更新其cost值，依据的是点离其最近障碍物点的距离来指定损失值。损失值确定方式如下：
   1. 若距离为0，则COST为LETHAL_OBSTACLE（254）
   2. 若距离小于机器人内半径，则COST为INSCRIBED_INFLATED_OBSTACLE（253）
   3. 若距离比内半径远， 则按照距离越远COST越低取值
   4. 距离障碍物越远应该cost越低，所以factor应该根据距离递减，weight_由外部的cost_scaling_factor决定，默认为10,所以要是想要离inflation近的损失更大，应该减小cost_scaling_factor的值252/(e^(dist*weight))。

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

---

    参数配置说明：
    1.ObstacleLayer和VoxelLayer在进行障碍物更新时会根据最小障碍物高度和最大障碍物高度，最小障碍物距离进行过滤，所以一定要确保自己想要让图层关注到的传感器信息在合理的范围内，也就是特别要注意雷达数据的z值不能想当然的设置为0，因为默认空间是（0,0.8）的开区间。在这上面吃过大亏。
    2.配置时要留心命名空间的问题，有时为了保证参数在正确的命名空间下，可以使用rosparam get ...来查看设置的参数是否起了作用。
    3.调试时，可以进行动态参数调整，避免反复修改文件，效率低下，命令为：`rosrun rqt_gui rqt_gui -s reconfigure`
    4.在确定pdist,gdist,occdist的相对大小关系时，在base_local_planner的设置中将publish_cost_grid设置为true,这样在rviz当中就可以查看相应的热力图来确定是否正确了。
    5.base_local_planner中速度限制不要设置太小，尤其是角速度和角加速度，否则有时候转弯很可能没法及时转过来而撞上障碍物。
    6.局部地图的窗口应该视环境复杂度而定，如果太大，有时候在转弯时，很可能以gdist目标为优先而撞上障碍物一般由小往大调，控制在1-3之内，调节时配合4中所述方法重点观察转弯时的轨迹。