costmap_2d包详解及编写自定义图层插件

costmap_2d包

costmap_2d包提供了一个可配置的数据结构，该数据结构以栅格地图的形式维护机器人的导航信息。costmap使用的信息是传感器数据和静态地图。

主要接口是costmap_2d::Costmap2DROS，它维护了许多与ROS相关的函数。它包含一个costmap_2d::LayeredCostmap，用于跟踪每个层。每个层都在Costmap2DROS中使用pluginlib进行实例化，并添加到LayedCostmap中。每一层本身可以单独编译，允许通过C++接口对costmap进行任意更改。costmap_2d::Costmap2D类实现了用于存储和访问二维成本地图的基本数据结构。

costmap_2d::Costmap2DROS类是对costmap_2d::Costmap2D类的封装，函数接口转换为ROS C++的格式。它在初始化时指定的ROS命名空间内运行。

下例为costmap_2d::Costmap2DROS对象指定了my_costmapROS命名空间

#include <tf/transform_listener.h>
#include <costmap_2d/costmap_2d_ros.h>
...
tf2_ros::Buffer buffer;
costmap_2d::Costmap2DROS costmap("my_costmap", buffer);

三个最基础的类：Costmap2D Layer LayeredCostmap

Costmap2D::updateOrigin()函数

class Costmap2D{//一个二维数据结构，每一个元素存放着一个cost代价
public:
   /**
   * @brief  Constructor for a costmap
   * @param  cells_size_x The x size of the map in cells
   * @param  cells_size_y The y size of the map in cells
   * @param  resolution The resolution of the map in meters/cell
   * @param  origin_x The x origin of the map
   * @param  origin_y The y origin of the map
   * @param  default_value Default Value
   */
    Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
              double origin_x, double origin_y, unsigned char default_value = 0):
    size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x),
    origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
    /**
   * @brief  Turn this costmap into a copy of a window of a costmap passed in
   * @param  map The costmap to copy
   * @param win_origin_x The x origin (lower left corner) for the window to copy, in meters
   * @param win_origin_y The y origin (lower left corner) for the window to copy, in meters
   * @param win_size_x The x size of the window, in meters
   * @param win_size_y The y size of the window, in meters
   */
  	bool copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x,
                         double win_size_y);
    unsigned char getCost(unsigned int mx, unsigned int my) const;
    void setCost(unsigned int mx, unsigned int my, unsigned char cost);
    virtual void initMaps(unsigned int size_x, unsigned int size_y);//重新为costmap_申请size_x * size_y * uint8的内存
    virtual void resetMaps();//重设costmap_值为default_value_
    virtual void updateOrigin(double new_origin_x, double new_origin_y);
    //Sets the cost of a convex polygon to a desired value
    bool setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value);
protected:
    unsigned int size_x_;
    unsigned int size_y_;
    double resolution_;
    double origin_x_;
    double origin_y_;
    unsigned char* costmap_;
    unsigned char default_value_;
    /**
    * @brief  Copy a region of a source map into a destination map
    * @param  source_map The source map
    * @param sm_lower_left_x The lower left x point of the source map to start the copy
    * @param sm_lower_left_y The lower left y point of the source map to start the copy
    * @param sm_size_x The x size of the source map
    * @param  dest_map The destination map
    * @param dm_lower_left_x The lower left x point of the destination map to start the copy
    * @param dm_lower_left_y The lower left y point of the destination map to start the copy
    * @param dm_size_x The x size of the destination map
    * @param region_size_x The x size of the region to copy
    * @param region_size_y The y size of the region to copy
    */
  	template<typename data_type>
    void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y,
                       unsigned int sm_size_x, data_type* dest_map, unsigned int dm_lower_left_x,
                       unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
                       unsigned int region_size_y); 
}

class Layer{//层结构，地图层插件会继承此类，并通过layed_costmap汇总每一层
public: 
    void initialize(LayeredCostmap* parent, std::string name, tf2_ros::Buffer *tf){
        layered_costmap_ = parent;
        name_ = name;
        tf_ = tf;
        onInitialize();
	}
    //在LayeredCostmap类的updateMap函数中调用来轮询每一层插件要更新的costmap范围，每一层插件都可以扩张边界
    //在updateMap中的调用格式为(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
    //minx_,miny_,maxx_,maxy_为LayeredCostmap类的私有变量，保存costmap_在世界系的边界
    //之后LayeredCostmap类调用插件的updateCosts函数时会根据minx_,miny_,maxx_,maxy_边界转换为map坐标更新costmap的代价值
    virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                            double* max_x, double* max_y) {}//定义需要被更新的地图区域
    //Actually update the underlying costmap, only within the bounds calculated during UpdateBounds().
    //更新区域为[min_i/j,max_i/j)左闭右开
    virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {}
    /** @brief Stop publishers. */
    virtual void deactivate() {}
    /** @brief Restart publishers if they've been stopped. */
    virtual void activate() {}
    virtual void reset() {}
    /** @brief Implement this to make this layer match the size of the parent costmap. */
  	virtual void matchSize() {}
   
protected:
    /** @brief This is called at the end of initialize().  Override to
    * implement subclass-specific initialization.
    *
    * tf_, name_, and layered_costmap_ will all be set already when this is called. */
    virtual void onInitialize() {}
	LayeredCostmap* layered_costmap_;
    tf2_ros::Buffer *tf_;
}

class LayeredCostmap{//实例化不同的层插件并将其聚合为一个分数
public:
    LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown);
    void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x,double origin_y, bool size_locked);//调用costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);和每个插件的matchSize()函数重设地图大小
    /**
    * @brief  Update the underlying costmap with new data.
    * If you want to update the map outside of the update loop that runs, you can call this.
    */
    void updateMap(double robot_x, double robot_y, double robot_yaw);//核心函数，轮询各个layer插件，更新代价
    {
        for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();++plugin){
            //先轮询每个插件要更新的边界，扩张更新边界
            (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
        }
    	//将世界系边界转换到地图系
        int x0, xn, y0, yn;
        costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
        costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);

        x0 = std::max(0, x0);
        xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
        y0 = std::max(0, y0);
        yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
        //再将更新边界内的costmap初始化为默认值
        costmap_.resetMap(x0, y0, xn, yn);//关键点
        for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();++plugin){
            //轮询每个插件更新边界内的代价
            (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
        }
    }
    
    Costmap2D* getCostmap(){
        return &costmap_;
    }
    //添加地图层插件
    void addPlugin(boost::shared_ptr<Layer> plugin){
        plugins_.push_back(plugin);
    }
    //获取插件组指针
    std::vector<boost::shared_ptr<Layer> >* getPlugins()
    {
        return &plugins_;
    }
private:
    Costmap2D costmap_;
    std::vector<boost::shared_ptr<Layer> > plugins_;
    bool rolling_window_;  /// < @brief Whether or not the costmap should roll with the robot
}

派生类：

//StaticLayer和ObstacleLayer都派生自此类
class CostmapLayer : public Layer, public Costmap2D
{
    //清空地图区域(代价赋为NO_INFORMATION)，invert_area反转区域，
    //为0则清空区域为(start_x,start_y) (end_x,end_y)确定的矩形区域
    //为1则表示清空区域为除去矩形区域的其他区域
    void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y, bool invert_area)
    
    //取最大值更新
	void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)

    //主Costmap2D的下标[min_i/j,max_i/j)的元素覆盖重写
	void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
    
    //只有当不是cost值不是NO_INFORMATION(255)时，才覆盖重写
	void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)

    //增量式更新，将该costmap的值加到主costmap上(限幅252)，只对该costmap代价值非255(NO_INFORMATION)时才更新
	//主costmap值为NO_INFORMATION，则直接将该costmap的代价值赋给主costmap
	void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
}

最终ROS接口

class Costmap2DROS{
public:
    /**
    * @brief  Constructor for the wrapper
    * @param name The name for this costmap
    * @param tf A reference to a TransformListener
    */
    Costmap2DROS(const std::string &name, tf2_ros::Buffer& tf){
        //在该构造函数中读取参数服务器的参数，实例化LayeredCostmap，并加载Layer插件，调用Layer插件的initialize函数初始化
        layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
        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"]);
          boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
          layered_costmap_->addPlugin(plugin);
          //重点!!!!!初始化插件layer名字
          plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);   //name(global_costmap或local_costmap)/pname(static_layer或obstacle_layer等)
        }
    }
    void updateMap();//获取机器人位姿，调用layered_costmap_->updateMap(x, y, yaw)函数
    void start();//Subscribes to sensor topics if necessary and starts costmap updates, can be called to restart the costmap after calls to either stop() or pause();调用各Layer插件的activate()函数
    void stop();//Stops costmap updates and unsubscribes from sensor topics;调用各Layer插件的deactivate()函数
    void pause();//Stops the costmap from updating, but sensor data still comes in over the wire
    void resume();//Resumes costmap updates
    void resetLayers();//调用每个插件的reset函数，重设各layer的costmap为默认值
protected:
    LayeredCostmap* layered_costmap_;
}

global_costmap实例化一个``Costmap2DROS(“global_costmap”,tf_)`对象

local_costmap实例化一个Costmap2DROS("local_costmap",tf_)对象

在Costmap2DROS实例的构造函数中加载各插件实例，并调用插件layer的initialize初始化函数，传入每个插件layer的命名空间(global_costmap或local_costmap)/(stactic_layer或obstacle_layer等)，插件命名空间前缀有global_costmap或local_costmap。

插件的参数全部来自~/(global_costmap或local_costmap)/插件名 命名空间

地图更新流程

1、实例化Costmap2DROS对象，在构造函数里主要做以下三件事：

• 首先获得全局坐标系和机器人坐标系的转换
• 创建LayeredCostmap类型的对象layered_costmap_，并初始化costmap起点坐标和大小
• 加载各个Layer插件，初始化各层插件并加入到layered_costmap_里
• 设置机器人的轮廓setUnpaddedRobotFootprint
• 实例化一个Costmap2DPublisher来发布可视化数据
• 通过一个movementCB函数不断检测机器人是否在运动
• 开启动态参数配置服务，服务启动了更新costmap的线程mapUpdateLoop

2、在Costmap2DROS::updateMap函数里先获取机器人位姿，然后调用layered_costmap_的updateMap函数更新各层的costmap

3、在LayeredCostmap::updateMap里，做以下三件事

• 如果设置了rolling_window_为true，更新costmap的初始点坐标
• 依次调用各层插件的updateBounds函数，该函数会根据数据源数据先清空掉视野内已不存在的障碍物，然后再添加视野内新增的障碍物
• 依次调用各层插件的updateCosts函数，把每一层的costmap叠加成最终的costmap。

4、更新完成后，通过Costmap2DPublisher把costmap发送出去

编写地图层插件

必须继承直接或间接继承Layer类，至少重写最重要的三个函数

• onInitialize 初始化

• updateBounds 更新要更新的地图边界

• updateCosts 更新代价到LayedCostmap，汇总各层代价