costmap_2d详解3：costmap_2d_ros.cpp

整体概述：

    Costmap2DROS 类，它是move_base 调用costmap_2d 生成代价地图的入口，整体的逻辑都在这里面。

Costmap2DROS构造函数，实例化LayeredCostmap的指针，根据yaml配置文件中plugins参数生成相应类型的各个层Layer plugin，并添加到LayeredCostmap 的plugins_指针列表中后面使用，调用到各层的初始化函数，调用重配置函数reconfigureCB，创建多线程，同时更新地图

reconfigureCB 创建多线程，并调用mapUpdateLoop，在它中调用updateMap() 函数更新各层costmap 边界和cost值，之后在发布出去

在updateMap()中，得到global_frame_ 与robot_base_frame_ tf转换信息中的 translation 坐标信息，并调用LayeredCostmap 的updateMap(x, y, yaw) 函数，更新各层costmap 边界和cost 值

 

关键变量：

LayeredCostmap* layered_costmap_; //LayeredCostmap 类指针，管理各层costmap
global_frame_;
robot_base_frame_;
tf_;
plugin_loader_; //加载各层costmap 到 LayeredCostmap 中

 

关键函数

/*作用: 构造函数，

*name : costmap 名称，如global_costmap和local_costmap

* tf :  从move_base 传过来的tf 转换

*/

Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf)

{

  //在这里可以区分global_costmap 和local_costmap
  ros::NodeHandle private_nh("~/" + name);

/*如果是global_costmap,两参数分别是
*   global_frame: /map
*  robot_base_frame: /base_footprint
*****************************************************
* 如果是local_costmap 两参数分别是
*   global_frame: /odom_combined
*  robot_base_frame: /base_footprint
*/
  private_nh.param("global_frame", global_frame_, std::string("/map"));
  private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));


/*如果是global_costmap，rolling_window=false
*如果是local_costmap,rolling_window=true
*track_unknown_space 都默认是false
*/
private_nh.param("rolling_window", rolling_window, false);private_nh.param("track_unknown_space", track_unknown_space, false);

//实例化LayeredCostmap的指针，也初始化来master costmap
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);

/*
*判断导入的参数是否有plugins
*/
if (private_nh.hasParam("plugins"))
{
XmlRpc::XmlRpcValue my_list;

//把要加载的costmap 插件层添加进来
private_nh.getParam("plugins", my_list);
for (int32_t i = 0; i < my_list.size(); ++i)
{
//创建一个以 type为类类型的实例变量，然后让plugin这个指针指向这个实例
/* 两个值例如为：
* name: static_layer,       type: "costmap_2d::StaticLayer”
*/
std::string pname = static_cast(my_list[i]["name"]);
std::string type = static_cast(my_list[i]["type"]);

/* 创建一个以 type为类类型的实例变量，然后让plugin这个指针指向这个实例
并把这层地图加入到layered_costmap_ 中统一管理
*/

//定义Layer 的智能指针，并实例化
boost::shared_ptr plugin = plugin_loader_.createInstance(type);

//添加到 LayeredCostmap 中的plugins_ 列表末尾
layered_costmap_->addPlugin(plugin);

/*这里就会调用到Layer的initialize 函数，initialize 在父类layer.cpp 中，它会调用虚函数onInitialize，
* 最后各个plugin 层会实现该方法
*/
plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);


/*还没详细看*/    
setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));


//把地图更新过的部分发布出去

  publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",always_send_full_costmap);

  // create a thread to handle updating the map
  stop_updates_ = false;
  initialized_ = true;
  stopped_ = false;

  // Create a time r to check if the robot is moving
  robot_stopped_ = false;
  /*回调函数movementCB 实现，是通过比较前后两个pose的差，判断机器人是否在移动*/
  timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);
  dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~/" + name));

  /*除了对一些类成员的配置值做赋值以外，还会开启一个更新map的线程*/

  dynamic_reconfigure::Server::CallbackType \

cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);

}
}
}

 

/*根据参数对地图进行调整，另外还会重新启动地图更新线程mapUpdateLoop（因此，地图更新线程在这里启动）。*/

void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)
{
 //都是1.0
  transform_tolerance_ = config.transform_tolerance;

//如果已经更新则不用更新
  if (map_update_thread_ != NULL)
  {
    map_update_thread_shutdown_ = true;
    map_update_thread_->join();
    delete map_update_thread_;
  }
  map_update_thread_shutdown_ = false;
  //配置地图更新频率
  double map_update_frequency = config.update_frequency;

  double map_publish_frequency = config.publish_frequency;
/*
如果是global_costmap:
map_width_meters = config.width  //暂时不知
map_height_meters = config.height  //暂时不知
resolution = config.resolution //0.05
origin_x = config.origin_x  //应该是地图左下角坐标(即像素坐标系原点)
origin_y = config.origin_y

**************************************
如果是local_costmap:
map_width_meters = config.width  //3.0
map_height_meters = config.height  //3.0
resolution = config.resolution //0.05
origin_x = config.origin_x  //应该是机器人局部costmap左下角
origin_y = config.origin_y      
*/

double map_width_meters = config.width, map_height_meters = config.height, resolution = config.resolution, origin_x =config.origin_x,origin_y = config.origin_y;

//如果已经膨胀填充，重新计算填充一次
  if (footprint_padding_ != config.footprint_padding)
  {
    footprint_padding_ = config.footprint_padding;
    setUnpaddedRobotFootprint(unpadded_footprint_);
  }

  readFootprintFromConfig(config, old_config_);
//记录现在的配置为旧的配置，可能用于恢复以前的配置
  old_config_ = config;

  //启动地图更新线程mapUpdateLoop
  map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));
}

 

void Costmap2DROS::mapUpdateLoop(double frequency)
{
  while (nh.ok() && !map_update_thread_shutdown_)
  {
    //更新边界和地图
    updateMap();
  }
}

 

/*updateMap函数是地图信息更新的地方，其内部调用了LayeredCostmap->updateMap(double robot_x, double robot_y, double robot_yaw)函数。

 * 在该函数中，先依据各层的更新情况，判断地图更新过的范围的边界。然后用初始值重置全局地图更新

 * 边界范围内的地图信息，并用各层的信息在更新边界内部更新地图信息

*/

void Costmap2DROS::updateMap()
{
  if (!stop_updates_)
  {
tf::Stamped < tf::Pose > pose;
//得到global_frame_ 与robot_base_frame_ tf转换信息中的 translation 坐标信息
    if (getRobotPose (pose))
    {
      //获取当前机器人位置，必须要弄清楚这个x，y 代表什么意思
      double x = pose.getOrigin().x(),
      y = pose.getOrigin().y(),
      yaw = tf::getYaw(pose.getRotation());

/*调用LayeredCostmap 类中的updateMap 更新边界和cost值
*先依据各层的更新情况，判断地图更新过的范围的边界。
* 然后用初始值重置全局地图更新边界范围内的地图信息，
* 并用各层的信息在更新边界内部更新地图信息
*/
      layered_costmap_->updateMap(x, y, yaw);

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

bool Costmap2DROS::getRobotPose(tf::Stamped& global_pose) const
{
  //local_costmap和global_costmap的robot_base_frame_都是是/base_footprint
  robot_pose.frame_id_ = robot_base_frame_;

  try
  {
    /*
    *如果是local_costmap,global_frame_是/map
    * 如果是global_costmap 是/odom_combined
    * 然后根据tf信息，转换的到global_pose，即机器人当前位置？？
    *得到global_frame_ 与robot_base_frame_ tf转换信息中的 translation 坐标信息
    */
    tf_.transformPose(global_frame_, robot_pose, global_pose);
  }
  return true;
}

 

/*启动地图运行，内部包括激活各层、启动地图更新循环。可以在stop或者

 * pause调用之后用于重新启动地图

*/

void Costmap2DROS::start()

 

/*停止地图运行，内部包括去激活各层、停止地图更新循环*/

void Costmap2DROS::stop()

/*暂停地图运行，内部包括停止地图更新循环*/

void Costmap2DROS::pause()

/*复位地图运行，内部包括恢复地图更新循环*/

void Costmap2DROS::resume()

/*重置地图，内部包括重置总地图、重置地图各层*/

void Costmap2DROS::resetLayers()