ROS naviagtion analysis: costmap_2d--Costmap2DROS

在上一篇文章中moveBase就有关于costmap_2d的使用：
planner_costmap_ros_是用于全局导航的地图，controller_costmap_ros_是局部导航用的地图，地图类型为经过ROS封装的costmap_2d::Costmap2DROS*。
以下是这个ROS类的UML：

看看这个类的成员变量：LayeredCostmap* layered_costmap_; pluginlib::ClassLoader<Layer> plugin_loader_; 这两个最重要的成员变量，而LayeredCostmap类又包含了Costmap2D costmap_; 这个数据成员。
下面是这些类之间的关系：

绿色的是核心代码，从ROS用户的角度，只需要调用Costmap2DROS这个类，因为这个类已经把所有关于地图的操作都封装好了。不过我这里是分析底层算法实现，就不得不写得很长很长。

所以还是先回到对Costmap2DROS这个类的分析，然后再进一步一层一层的分析其他的类。这些类完成了对机器人地图的表示和操作，因此其数据结构和算法都很有分析的价值。
首先是构造函数Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) : 因此必须提供一个tf参数。tf参数需要提供以下两个坐标系的关系：

  // get two frames
  private_nh.param("global_frame", global_frame_, std::string("/map"));
  private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));

如果没有找到这两个坐标系的关系或者超时，则构造函数会一直阻塞在这里：

while (ros::ok()
&& !tf_.waitForTransform(global_frame_,robot_base_frame_,ros::Time(), ros::Duration(0.1), ros::Duration(0.01),&tf_error))
  {
    ros::spinOnce();
    if (last_error + ros::Duration(5.0) < ros::Time::now())
    {
      last_error = ros::Time::now();
    }
    tf_error.clear();
  }

然后加入各个层次的地图：

 if (private_nh.hasParam("plugins"))
  {
    XmlRpc::XmlRpcValue my_list;
    private_nh.getParam("plugins", my_list);
    for (int32_t i = 0; i < my_list.size(); ++i)
    {
      std::string pname = static_cast<std::string>(my_list[i]["name"]);
      std::string type = static_cast<std::string>(my_list[i]["type"]);
      ROS_INFO("Using plugin \"%s\"", pname.c_str());

      boost::shared_ptr<Layer> plugin =lugin_loader_.createInstance(type);
      layered_costmap_->addPlugin(plugin);
      plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
    }
  }

boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type); 这行会创建一个以 type为类类型的实例变量，然后让plugin这个指针指向这个实例。

 ` layered_costmap_->addPlugin(plugin);`

然后layered_costmap_ 将这些类型的地图都加入，addPlugin 实现：

  void addPlugin(boost::shared_ptr<Layer> plugin)
  {
    plugins_.push_back(plugin);
  }

这里的关系是：Costmap2DROS 有一个layered_costmap_ 数据成员，然后