Marking and Clearing

Each sensor is used to either mark (insert obstacle information into the costmap),
clear (remove obstacle information from the costmap), or both.
A marking operation is just an index into an array to change the cost of a cell.
A clearing operation, however, consists of raytracing through a grid from the origin of the sensor outwards for each observation reported.

Occupied, Free, and Unknown Space

注意这里说的是列，那是不是意味着它使用的voxel，3维存储信息

Columns that have a certain number of occupied cells (see mark_threshold parameter)
are assigned a costmap_2d::LETHAL_OBSTACLE cost, columns that have a certain number of unknown cells (see unknown_threshold parameter)
are assigned a costmap_2d::NO_INFORMATION cost, and other columns are assigned a
costmap_2d::FREE_SPACE cost.

costmap更新的频率由参数update_frequency确定。注意costmap严重依赖于tf的信息，要是tf太久没有更新的话，navigation就会挂掉
这个时间tolerance是由参数transform_tolerance确定的

Map type

有两种方法初始化costmap_2d::Costmap2DROS对象：

• static map(map_server)

  这种情况下，costmap初始化的长度宽度和static map的一样,obstacle的信息也来自static map。
  一般用在amcl定位导航中，随着机器人的移动，使用传感器的信息更新costmap

• rolling window

  自己给一个长，宽值，设置rolling_window参数为true,这个参数设置机器人的位置在costmap的中心，丢弃离机器人比较远的obstacle信息。
  一般用在在里程计坐标下的移动，机器人只关心在他周边的障碍物信息。

注意提供plugin申明，还有在move_base node下面运行，不然有一些兼容性问题(可能是Hydro版本以前的初始化方式)

code

代码文档

A ROS wrapper for a 2D Costmap. Handles subscribing to topics that provide observations about obstacles in either the form of PointCloud or LaserScan messages

namespace costmap_2d
{
class Costmap2DROS
{
Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf){
    layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
    //发布costmap
    publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",
                                      always_send_full_costmap);
    timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);
    
    map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));

}

void Costmap2DROS::mapUpdateLoop(double frequency)
   
    updateMap();
    publisher_->publishCostmap();
}

void Costmap2DROS::updateMap()
{
    if (!stop_updates_)
    {
        // get global pose
        tf::Stamped < tf::Pose > pose;
        if (getRobotPose (pose))
        {
            double x = pose.getOrigin().x(),
                    y = pose.getOrigin().y(),
                    yaw = tf::getYaw(pose.getRotation());

            //对所有layer的costmap进行更新
            layered_costmap_->updateMap(x, y, yaw);
        }
    }
}
}
}