关于Move_base的框架梳理参考： 此链接

一. costmap_2d包介绍

costmap_2d包提供了一种可配置框架来维护机器人在代价地图上应该如何导航的信息。实际使用的时候，可以分为global_costmap和local_costmap两部分，每一种都可以配置多个图层，包括以下几种：

• static_layer :静态地图层，基本不变的地图层，通常都是SLAM建图完成的静态地图，用于路径规划
• obstacle_layer：障碍地图层，用于动态的记录传感器感知的障碍物信息，用于路径规划和避障。
• inflation_layer: 膨胀层，在以上两层地图进行膨胀，以避免机器人的撞上障碍物，用于路径规划。
• Other Layers：可以通过插件的形式自己实现costmap，目前已有Social Costmap Layer、Range Sensor Layer等开源插件。

costmap_2d包提供的ROS化功能接口：costmap_2d::Costmap2DROS。它使用costmap_2d::LayeredCostmap 来跟踪每一层。 每一层在Costmap2DROS中以插件方式被实例化，并被添加到LayeredCostmap。 每一层可以独立编译，且可使用C++接口实现对代价地图的随意修改。costmap_2d::Costmap2D 类中实现了用来存储和访问2D代价地图的的基本数据结构。其中包含或者涉及到的类如下：

• Costmap2DROS类

  是对整个代价地图内容的封装。

• LayeredCostmap类

  是Costmap2DROS的类成员，它是“主地图”，也能管理各层地图，因为它含有指向各层子地图的指针，能够调用子地图的类方法，开启子地图的更新。并且各层子地图最后都会合并到主地图上，提供给规划器的使用。它含有Costmap2D类成员，这个类就是底层地图，用于记录地图成员。

• Layer类

  含有子地图层用到的一些函数，如更新size、更新bound、和主地图合并等；

• Costmap2D类

  存储该层维护的地图数据,包括地图的x,y方向的尺寸，地图的分辨率，地图原点位姿。

• CostmapLayer类

  派生自Layer类和Costmap2D类，里面提供了 用主地图尺寸设置该层地图尺寸matchSize、用当前子地图数据更新对应区域updateWithMax等函数，相当于函数模版，在相关层进行继承和调用。

• Staticlayer类和Obsaclelayer类

  由CostmapLayer类派生出Staticlayer类和Obsaclelayer类，即静态层和障碍层，前者获取静态地图，后者通过传感器数据不断更新，获得能反应障碍物信息的子地图。

• InflationLayer类

  由Layer类单独派生InflationLayer类，即膨胀层，用它来反映障碍物在地图上向周边的膨胀。由于它的父类中不含有Costmap2D类，所以其实膨胀层自身没有栅格地图要维护，这一点和另外两层有区别。

类与类的继承关系如下图：

二. Costmap包的执行入口-- move_base中调用

代码路径：move_base/src/move_base.cpp

首先获取地图信息，内切圆半径inscribed_radius/外接圆形半径circumscribed_radius等信息。

private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);

下面通过模块costmap_2d构建了全局代价地图对象，并通过其成员函数pause()暂停运行，将在必要的功能模块初始化结束之后通过成员接口start()开启。

planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause();

加载的全局代价地图是由静态地图层、 障碍层和膨胀层构成，如下面下边的代码片段所示。而局部代价地图只有障碍层和膨胀层。

# global_costmap_params.yaml- {name: static_layer,type: "costmap_2d::StaticLayer"}- {name: obstacle_layer,type: "costmap_2d::VoxelLayer"}- {name: inflation_layer,type: "costmap_2d::InflationLayer"}
# lobal_costmap_params.yaml- {name: obstacle_layer,type: "costmap_2d::VoxelLayer"}- {name: inflation_layer,type: "costmap_2d::InflationLayer"}

构建了必要的功能模块之后，就可以开启代价地图的计算了。

planner_costmap_ros_->start();

三. Costmap包的初始化以及维护

3.1 Costmap2DROS类

代码路径：costmap_2d/src/costmap_2d_ros.cpp

大致的运行关系如下图：

3.1.1 构造函数 Costmap2DROS::Costmap2DROS

首先是一些参数的获取：

• 循环等待直到获得机器人底盘坐标系和global系之间的坐标转换；
• 获取rolling_window、track_unknown_space、always_send_full_costmap的参数，默认为false；

Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) :layered_costmap_(NULL),name_(name),tf_(tf),transform_tolerance_(0.3),map_update_thread_shutdown_(false),stop_updates_(false),initialized_(true),stopped_(false),robot_stopped_(false),map_update_thread_(NULL),last_publish_(0),plugin_loader_("costmap_2d", "costmap_2d::Layer"),publisher_(NULL),dsrv_(NULL),footprint_padding_(0.0)
{//初始化old poseold_pose_.setIdentity();old_pose_.setOrigin(tf::Vector3(1e30, 1e30, 1e30));ros::NodeHandle private_nh("~/" + name);ros::NodeHandle g_nh;//获取tf前缀ros::NodeHandle prefix_nh;std::string tf_prefix = tf::getPrefixParam(prefix_nh);//两个坐标系private_nh.param("global_frame", global_frame_, std::string("/map"));private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));//确保基于tf前缀正确设置了坐标系//注：tf::resolve或者TransformListener::resolve方法的作用就是使用tf_prefix参数将frame_name解析为frame_idglobal_frame_ = tf::resolve(tf_prefix, global_frame_);robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_);ros::Time last_error = ros::Time::now();std::string tf_error;//确保机器人底盘坐标系和世界坐标系之间的有效转换//否则一直停留在这里阻塞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()){ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());last_error = ros::Time::now();}// The error string will accumulate and errors will typically be the same, so the last// will do for the warning above. Reset the string here to avoid accumulation.tf_error.clear();}//检测是否需要“窗口滚动”，从参数服务器获取以下参数bool rolling_window, track_unknown_space, always_send_full_costmap;private_nh.param("rolling_window", rolling_window, false);private_nh.param("track_unknown_space", track_unknown_space, false);private_nh.param("always_send_full_costmap", always_send_full_costmap, false);

接下来创建一个LayeredCostmap类实例layered_costmap_，作为规划器使用的主地图，并通过它管理各层地图。

通过LayeredCostmap的对象创造了一个由指向plugin的共享指针组成的容器，通过配置文件，向里逐个添加插件层。

//初始化一个layered_costmap
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);

接着，在参数服务器上获取“plugins”参数，这里得到的插件即为各层子地图。每层子地图使用Pluginlib（ROS插件机制）来实例化，各个层可以被独立的编译，且允许使用C++接口对Costmap做出任意的改变。循环将每个plugin即每层子地图添加进layered_costmap_类的指针组对象中，并对每层地图调用其initialize类方法，初始化各层地图。这个函数定义在Layer类中，它向各层地图“通知”主地图的存在，并调用oninitialize类方法（Layer类中的虚函数，被定义在各层地图中）。

  //如果没有这一项参数，重新设置旧参数if (!private_nh.hasParam("plugins")){resetOldParameters(private_nh);}//加入各个层次的地图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){ //从my_list里获取地图名称和种类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());//创建一个以type为类类型的实例变量，然后让plugin这个指针指向这个实例boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);layered_costmap_->addPlugin(plugin);//实际执行的是plugin调用的父类Layer的方法initializeplugin->initialize(layered_costmap_, name + "/" + pname, &tf_);}}

订阅footprint话题，回调函数为setUnpaddedRobotFootprintPolygon。当话题上收到footprint时，回调函数会将接收到的footprint根据参数footprint_padding的值进行“膨胀”，得到“膨胀”后的padded_footprint，传递给各级地图。

创建另一个话题，该话题上将发布的内容是根据机器人当前位置计算出来的实时footprint的位置。

  std::string topic_param, topic;if (!private_nh.searchParam("footprint_topic", topic_param)){topic_param = "footprint_topic";}private_nh.param(topic_param, topic, std::string("footprint"));footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon, this);if (!private_nh.searchParam("published_footprint_topic", topic_param)){topic_param = "published_footprint";}private_nh.param(topic_param, topic, std::string("oriented_footprint"));footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>("footprint", 1);setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));

创建地图的发布器实例，Costmap2DPublisher类是用于地图发布的封装类。

//发布Costmap2Dpublisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",always_send_full_costmap);//创建地图更新线程的控制量stop_updates_ = false;initialized_ = true;stopped_ = false;//创建一个timer去检测机器人是否在移动robot_stopped_ = false;//回调函数movementCB通过比较前后两个pose的差来判断机器人是否在移动timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);

开启动态参数配置，它的回调函数Costmap2DROS::reconfigureCB会在节点启动时运行一次，加载.cfg文件的配置参数。这个函数给对应参数赋值，并创建了一个Costmap2DROS::mapUpdateLoop函数的线程，即地图更新线程。

//开启参数动态配置dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));//回调函数reconfigureCB 除了对一些类成员的配置值做赋值以外，还会开启一个更新map的线程 dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1,_2);dsrv_->setCallback(cb);
}

3.1.2 地图更新线程 Costmap2DROS::mapUpdateLoop

这个函数循环调用UpdateMap函数，更新地图，并以publish_cycle为周期，发布更新后的地图。它分为两步：

• 第一步：更新bound，即确定地图更新的范围；
• 第二步：更新cost，更新每层地图cell对应的cost值后整合到主地图上。

void Costmap2DROS::mapUpdateLoop(double frequency)
{// the user might not want to run the loop every cycleif (frequency == 0.0)return;ros::NodeHandle nh;ros::Rate r(frequency);while (nh.ok() && !map_update_thread_shutdown_){struct timeval start, end;double start_t, end_t, t_diff;gettimeofday(&start, NULL);updateMap();gettimeofday(&end, NULL);start_t = start.tv_sec + double(start.tv_usec) / 1e6;end_t = end.tv_sec + double(end.tv_usec) / 1e6;t_diff = end_t - start_t;//计算地图更新时间ROS_DEBUG("Map update time: %.9f", t_diff);//更新地图边界及发布if (publish_cycle.toSec() > 0 && layered_costmap_->isInitialized()){unsigned int x0, y0, xn, yn;layered_costmap_->getBounds(&x0, &xn, &y0, &yn);publisher_->updateBounds(x0, xn, y0, yn);ros::Time now = ros::Time::now();if (last_publish_ + publish_cycle < now){publisher_->publishCostmap();last_publish_ = now;}}r.sleep();// make sure to sleep for the remainder of our cycle timeif (r.cycleTime() > ros::Duration(1 / frequency))ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency,r.cycleTime().toSec());}
}

3.1.3 地图更新 Costmap2DROS::updateMap()

这个函数首先调用类内getRobotPose函数，通过tf转换，将机器人底盘系的坐标转换到global系，得到机器人的位姿。然后通过layered_costmap_调用LayeredCostmap类的updateMap函数，更新地图。

void Costmap2DROS::updateMap()
{if (!stop_updates_){//获取机器人在地图上的位置和姿态tf::Stamped < tf::Pose > pose;if (getRobotPose (pose)){double x = pose.getOrigin().x(),y = pose.getOrigin().y(),yaw = tf::getYaw(pose.getRotation());//调用layered_costmap_的updateMap函数，参数是机器人位姿layered_costmap_->updateMap(x, y, yaw);

这里更新机器人的实时足迹，通过footprint_pub_发布。

//更新机器人足迹geometry_msgs::PolygonStamped footprint;footprint.header.frame_id = global_frame_;footprint.header.stamp = ros::Time::now();transformFootprint(x, y, yaw, padded_footprint_, footprint);footprint_pub_.publish(footprint);initialized_ = true;}}
}

3.1.4 激活各层地图 Costmap2DROS::start()

start函数在Movebase中被调用，这个函数对各层地图插件调用activate函数，激活各层地图。

void Costmap2DROS::start()
{std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();//检查地图是否暂停或者停止if (stopped_){//如果停止，需要重新订阅话题//“Layer”是一个类，active是其中一个类方法for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();++plugin){(*plugin)->activate();}stopped_ = false;}stop_updates_ = false;//在costmap被重新初始化前阻塞..meaning one update cycle has runros::Rate r(100.0);while (ros::ok() && !initialized_)r.sleep();
}

3.2 Costmap2D类

Costmap2D类是记录地图数据的底层，它记录地图的x、y方向的尺寸，地图的分辨率，地图原点位置，以及用unsigned char类型记录地图内容。并且，Costmap2D类提供了一些对地图进行基本操作的函数，如：地图复制、用index/点坐标来设置/获取地图上该点的cost值、地图坐标和世界坐标之间的转换、获取地图大小/分辨率/原点、设置多边形边缘及内部点的cost值。

它充当一种以模版形式存在的类，Layer类则是调用它的工具人形式。

代码路径：costmap_2d/src/costmap_2d.cpp

大致的运行关系如下图：

3.3 LayeredCostmap类

LayeredCostmap类是Costmap2DROS的成员，含有主地图，并能通过它操作各层子地图。LayeredCostmap类的地图更新函数主要分为两步，先更新bound，再更新cost，它调用Layer类方法，它在各层子地图中被重载。

代码路径：costmap_2d/src/layered_costmap.cpp

大致的运行关系如下图：

3.4 CostmapLayer类

这个类继承自Layer类和Costmap2D类，它是地图插件（静态层和障碍层）的基类。它的类方法主要用于处理bound和用几种不同的策略合并子地图和主地图。