机器人导航算法——Costmap地图ROS源码解析

本文主要是介绍机器人导航算法——Costmap地图ROS源码解析，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

文章目录

前言
一、move_base:Costmap程序的执行入口
二、costmap初始化和维护
三、costmap地图
- 1.costmap膨胀层InflationLayer
- 2.costmap障碍物obstacleLayer
- 3.costmap静态层staticLayer
四、如何使用地图用于逻辑处理（如规划）
总结

前言

本文将从move_base主程序开始，对costmap的原理进行详解，包括静态地图/障碍物地图/膨胀地图，本文主要侧重如何去理解costmap，由于作者水平有限，如有错误，谢谢指出。
costmap使用了一种分层地图的技术方案，即分别考虑障碍物层/膨胀层/静态障碍物层更新对应层的地图代价信息，然后再合并到主地图中，具体见我翻译的另一篇文章《costmap文献阅读—Layered Costmaps for Context-Sensitive Navigation》。

一、move_base:Costmap程序的执行入口

本部分，我们关心如何从move_base导航入口进入到costmap的运行程序，以及如何进行静态地图/障碍物层地图/膨胀层的处理。
代码路径：move_base/src/move_base.cpp
首先获取地图信息，内切圆半径inscribed_radius/外切圆形半径circumscribed_radius等信息。

 //we'll assume the radius of the robot to be consistent with what's specified for the costmapsprivate_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()开启。

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

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


// Start actively updating costmaps based on sensor data  planner_costmap_ros_->start();

这是导航模块move_base初始化costmap和维护地图的方法，其中Costmap2DROS是初始化costmap，planner_costmap_ros_->start()开始维护代价地图。

二、costmap初始化和维护

代码路径：costmap_2d/src/costmap_2d_ros.cpp
按照上述的move_base分析，我们首先调用Costmap2DROS的构造函数：


Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) :layered_costmap_(NULL), name_(name), tf_(tf), 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)

Costmap2DROS的构造函数进行了坐标变换和相关参数初始化。

 ros::NodeHandle private_nh("~/" + name);ros::NodeHandle g_nh;// get our tf prefix  //获取我们的tf前缀ros::NodeHandle prefix_nh;std::string tf_prefix = tf::getPrefixParam(prefix_nh);// get two framesprivate_nh.param("global_frame", global_frame_, std::string("/map"));private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));// make sure that we set the frames appropriately based on the tf_prefix  //确保我们根据tf_prefix适当地设置了框架global_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;// we need to make sure that the transform between the robot base frame and the global frame is availablewhile (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();}// check if we want a rolling window version of the costmapbool rolling_window, track_unknown_space, always_send_full_costmap;private_nh.param("rolling_window", rolling_window, false);//private_nh.param(“参数名称”,被赋值变量,默认值)，private_nh.param("track_unknown_space", track_unknown_space, false);private_nh.param("always_send_full_costmap", always_send_full_costmap, false);

大家都知道，代价地图是由不同层级的plugins逐层累加的，包括静态地图层、障碍层、膨胀层等等，那么通过LayeredCostmap的对象，创造了一个由指向plugin的共享指针组成的容器，通过配置文件，向里一一添加插件层。


layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);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){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()  );// plugin_loader_ 类型是　pluginlib::ClassLoader<Layer>，ROS插件机制boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);// 添加到容器 plugins_layered_costmap_->addPlugin(plugin);// 执行 Layer::initialize，它向各层地图通知主地图的存在，并调用// oninitialize方法（Layer类中的虚函数，被定义在各层地图中）plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);}
}

Costmap2DROS调用的接口addPlugin，则是将一个指针放入容器中，仅此而已。

对车体尺寸进行建模，这个文件中均是对不同形状的车体进行外壳的位置计算，然后计算的内切圆半径和外切圆半径同步更新到各个地图layer。


footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon, this);void Costmap2DROS::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)
{setUnpaddedRobotFootprint(toPointVector(footprint));
}void Costmap2DROS::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points)
{unpadded_footprint_ = points;padded_footprint_ = points;padFootprint(padded_footprint_, footprint_padding_);layered_costmap_->setFootprint(padded_footprint_);
}

这段代码是用于创建一个动态重新配置服务器，用于修改名为Costmap2DConfig的配置文件。动态重新配置服务器允许在运行时修改配置参数，而不需要重启机器人。

代码解释如下：

1.dsrv_=new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));：创建一个动态重新配置服务器，用于处理Costmap2DConfig类型的配置文件。ros::NodeHandle用于创建一个节点，其中包含动态重新配置服务器的地址。name是配置文件的名称，包含在地址中。

2.dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1, _2);：定义一个回调函数类型，用于处理配置更新事件。boost::bind将回调函数绑定到reconfigureCB成员函数，并将传入的参数传递给它。_1和_2是动态重新配置服务器传递的参数，分别对应配置文件的旧值和新值。

dsrv_->setCallback(cb);：将回调函数设置为动态重新配置服务器的回调函数，以便在接收到新的配置更新时调用它。

总之，这段代码创建了一个动态重新配置服务器，用于处理Costmap2DConfig类型的配置文件。它还定义了一个回调函数，用于在接收到配置更新时修改配置文件。最后，将回调函数设置为动态重新配置服务器的回调函数。这在机器人导航、控制等领域非常有用，允许在不重启机器人的情况下实时修改配置。


dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1,_2);dsrv_->setCallback(cb);

然后通过如下函数进行配置参数初始化并创建主线程&Costmap2DROS::mapUpdateLoop。


void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)

 map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));

mapUpdateLoop这个成员函数是用于不停刷新代价地图的，它在每个循环通过updateMap函数实现，而Costmap2DROS::updateMap函数是利用LayeredCostmap的updateMap函数进行更新。


void Costmap2DROS::mapUpdateLoop(double frequency)
{//mapUpdateLoop这个成员函数是用于不停刷新代价地图的，它在每个循环通过updateMap函数实现//，而Costmap2DROS::updateMap函数是利用LayeredCostmap的updateMap函数进行更新，在前面已经分析过。// the user might not want to run the loop every cycleupdateMap();//更新地图

更新地图信息，这个函数在Costmap2DROS的地图更新线程中被循环调用。它分为两步：第一步：更新bound，即确定地图更新的范围；第二步：更新cost，更新每层地图cell对应的cost值后整合到主地图上。

void Costmap2DROS::updateMap()
{if (!stop_updates_){// get global posetf::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);geometry_msgs::PolygonStamped footprint;footprint.header.frame_id = global_frame_;footprint.header.stamp = ros::Time::now();transformFootprint(x, y, yaw, padded_footprint_, footprint);//make coordiates to global coordiatesfootprint_pub_.publish(footprint);initialized_ = true;}}
}

layered_costmap_->updateMap(x, y, yaw)来自于如下代码：

void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
{// Lock for the remainder of this function, some plugins (e.g. VoxelLayer)// implement thread unsafe updateBounds() functions.boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));// if we're using a rolling buffer costmap... we need to update the origin using the robot's positionif (rolling_window_){double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;costmap_.updateOrigin(new_origin_x, new_origin_y);}if (plugins_.size() == 0)return;minx_ = miny_ = 1e30;maxx_ = maxy_ = -1e30;for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();++plugin){double prev_minx = minx_;double prev_miny = miny_;double prev_maxx = maxx_;double prev_maxy = maxy_;(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy){ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but ""is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",prev_minx, prev_miny, prev_maxx , prev_maxy,minx_, miny_, maxx_ , maxy_,(*plugin)->getName().c_str());}}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);ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);if (xn < x0 || yn < y0)return;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);}bx0_ = x0;bxn_ = xn;by0_ = y0;byn_ = yn;initialized_ = true;
}

rolling_window_默认为false，如果开启的话，地图是时刻跟随机器人中心移动的，这里需要根据机器人当前位置和地图大小计算出地图的新原点，设置给主地图。


// if we're using a rolling buffer costmap... we need to update the origin using the robot's position//rolling_window_默认为false，如果开启的话，地图是时刻跟随机器人中心移动的，//这里需要根据机器人当前位置和地图大小计算出地图的新原点，设置给主地图。if (rolling_window_){double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;costmap_.updateOrigin(new_origin_x, new_origin_y);}

设置好minx_、miny_、maxx_、maxy_的初始值，然后对每一层的子地图调用其updateBounds函数，传入minx_、miny_、maxx_、maxy_，函数将新的bound填充进去。updateBounds函数在Layer类中声明，在各层地图中被重载，第二步使用到的updateCosts函数也是如此。


for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();++plugin){double prev_minx = minx_;double prev_miny = miny_;double prev_maxx = maxx_;double prev_maxy = maxy_;(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy){ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but ""is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",prev_minx, prev_miny, prev_maxx , prev_maxy,minx_, miny_, maxx_ , maxy_,(*plugin)->getName().c_str());}}

需要指出的是，(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);成员函数updateBounds可能来自于三个类：(1)costmap_2d/plugins/inflation_layer.cpp (2)costmap_2d/plugins/obstacle_layer.cpp

(3) costmap_2d/plugins/static_layer.cpp

使用到了C++虚函数的多态！子类重写基类的方法，然后子类实例化指向基类即可使用。

接下来调用Costmap2D类的worldToMapEnforceBounds函数，将得到的bound转换到地图坐标系。这个函数可以防止转换后的坐标超出地图范围。

 //接下来调用Costmap2D类的worldToMapEnforceBounds函数，//将得到的bound转换到地图坐标系。这个函数可以防止转换后的坐标超出地图范围。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);//范围更新ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);

接下来，调用resetMap，将主地图上bound范围内的cell的cost恢复为默认值（track_unknown：255 / 否则：0），再对每层子地图调用updateCosts函数。updateCosts也是来源于不同的文件的（

(1)costmap_2d/plugins/inflation_layer.cpp (2)costmap_2d/plugins/obstacle_layer.cpp

(3) costmap_2d/plugins/static_layer.cpp

），也是虚函数实现的多态。updateCosts实现了对costmap_主地图的合并。


//接下来，调用resetMap，将主地图上bound范围内的cell的cost恢复为默认值（track_unknown：255 / 否则：0），//再对每层子地图调用updateCosts函数。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);}

其实上述完成了地图的初始化和循环更新地图的代码逻辑。通过上述的分析，我们知道有两个比较重要的接口：所有层都是继承layer类，这个类比较简单，是一个纯虚基函数，定定义了两个重要接口updateBounds、updateCosts，updateBounds用于计算栅格的权重，updateCosts用于将计算的地图合并到主地图上。具体类的继承关系可参见我的另一篇文章《机器人控制—ROS代价地图Costmap的层概述》。

三、costmap地图

1.costmap膨胀层InflationLayer

本文章主要是分析具体的膨胀层的算法具体是如何实现的。之前写过原理，可参见文章《机器人算法——costmap膨胀层InflationLayer》，总结下就是：
首先确认障碍物的栅格点，然后以障碍物栅格点位当前点，然后基于当前点的考察下一个点，即上下左右，当前障碍物会一直记录，后面用于距离比较。

如果在障碍物半径内，则继续循环到上述的“ unsigned char cost = costLookup(mx, my, sx, sy);为参考矩阵计算， master_array[index] = cost;为当前cell栅格赋值。” 如此循环即可，类似于A*算法的膨胀。

这些膨胀的操作都在 InflationLayer::updateCosts操作（costmap_2d/plugins/inflation_layer.cpp）并将将上述膨胀后的地图写入主地图中。

2.costmap障碍物obstacleLayer

障碍层地图通过订阅传感器话题，将传感器输出的障碍物信息存进buffer（剔除过高、过远的点），在本层地图上将观测到的点云标记为障碍物，将传感器到点云点连线上的点标记为FREE_SPACE。最后，在bound范围内，将本层地图合并到主地图上。

通过遍历传感器的数据，将符合要求的障碍点坐标从global系转换到map系，并在本层地图上标记致命障碍。

void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,double* min_y, double* max_x, double* max_y)
{if (rolling_window_)updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);if (!enabled_)return;useExtraBounds(min_x, min_y, max_x, max_y);//在接收到传感器数据后，buffer将被更新，同样，marking_buffers_和//clearing_buffers_也更新（即buffer中的内容），将二者分别提取到observations和clearing_observations中存储。bool current = true;std::vector<Observation> observations, clearing_observations;// get the marking observationscurrent = current && getMarkingObservations(observations);// get the clearing observationscurrent = current && getClearingObservations(clearing_observations);// update the global current statuscurrent_ = current;// raytrace freespacefor (unsigned int i = 0; i < clearing_observations.size(); ++i){raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);}
/***第二步是marking操作，即将点云中的点标记为障碍物。在标记时通过二重循环，外层迭代各观测轮次，内层迭代一次观测得到的点云点，
剔除本身太高（z坐标过大）、与传感器距离太远的点，将符合要求的障碍点坐标从global系转换到map系，并在本层地图上标记致命障碍。并调用touch函数，确保标记的障碍点包含在 bound内。作者：wanghuohuo0716
链接：https://www.jianshu.com/p/2984cdc99236
来源：简书
著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。*/// place the new obstacles into a priority queue... each with a priority of zero to begin withfor (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){const Observation& obs = *it;const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;for (unsigned int i = 0; i < cloud.points.size(); ++i){double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z;// if the obstacle is too high or too far away from the robot we won't add itif (pz > max_obstacle_height_){ROS_DEBUG("The point is too high");continue;}//计算点与点之间的距离平方// compute the squared distance from the hitpoint to the pointcloud's origindouble sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y)+ (pz - obs.origin_.z) * (pz - obs.origin_.z);// if the point is far enough away... we won't consider it  if (sq_dist >= sq_obstacle_range){ROS_DEBUG("The point is too far away");continue;}// now we need to compute the map coordinates for the observationunsigned int mx, my;if (!worldToMap(px, py, mx, my)){ROS_DEBUG("Computing map coords failed");continue;}//设置它为致命unsigned int index = getIndex(mx, my);costmap_[index] = LETHAL_OBSTACLE;touch(px, py, min_x, min_y, max_x, max_y);}}//在以上两步完成后，调用updateFootprint函数，//它的作用是基于机器人当前位置确定该位置下的足迹，并在内部调用touch函数保证足迹包含在bound范围内。updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}

这个函数就是将机器人足迹范围内设置为FREE_SPACE，并且在bound范围内将本层障碍地图的内容合并到主地图上。


void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{//这个函数就是将机器人足迹范围内设置为FREE_SPACE，并且在bound范围内将本层障碍地图的内容合并到主地图上。if (!enabled_)return;if (footprint_clearing_enabled_){setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);}switch (combination_method_){case 0:  // OverwriteupdateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);break;case 1:  // MaximumupdateWithMax(master_grid, min_i, min_j, max_i, max_j);break;default:  // Nothingbreak;}
}

3.costmap静态层staticLayer

获取接收到的静态地图的尺寸，当地图不随机器人移动时，若接收到的静态地图和主地图的尺寸/分辨率/起点不同，以接收到的地图为准，调整主地图的参数。


void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
{//获取接收到的静态地图的尺寸，当地图不随机器人移动时，//若接收到的静态地图和主地图的尺寸/分辨率/起点不同，以接收到的地图为准，调整主地图的参数。unsigned int size_x = new_map->info.width, size_y = new_map->info.height;ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);// resize costmap if size, resolution or origin do not match//如果master map的尺寸、分辨率或原点与获取到的地图不匹配，重新设置master mapCostmap2D* master = layered_costmap_->getCostmap();if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x ||master->getSizeInCellsY() != size_y ||master->getResolution() != new_map->info.resolution ||master->getOriginX() != new_map->info.origin.position.x ||master->getOriginY() != new_map->info.origin.position.y ||!layered_costmap_->isSizeLocked())){// Update the size of the layered costmap (and all layers, including this one)ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,new_map->info.origin.position.y, true);}else if (size_x_ != size_x || size_y_ != size_y ||resolution_ != new_map->info.resolution ||origin_x_ != new_map->info.origin.position.x ||origin_y_ != new_map->info.origin.position.y){ //若本层的数据和接收到的静态地图的参数不同时，继续以接收到的地图为准，调整本层参数。// only update the size of the costmap stored locally in this layerROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);resizeMap(size_x, size_y, new_map->info.resolution,new_map->info.origin.position.x, new_map->info.origin.position.y);}

将接收到的静态地图数据复制到本层地图，复制过程中调用interpretValue函数，进行“翻译”，内容后述。并设置好地图坐标系中的原点x_、y_，以及地图尺寸和标志位。若first_map_only_开启，则在第一次接收到地图后，话题将不再接收消息。

 //将接收到的静态地图数据复制到本层地图，复制过程中调用interpretValue函数，进行“翻译”，内容后述。//并设置好地图坐标系中的原点x_、y_，以及地图尺寸和标志位。//若first_map_only_开启，则在第一次接收到地图后，话题将不再接收消息。// initialize the costmap with static datafor (unsigned int i = 0; i < size_y; ++i){for (unsigned int j = 0; j < size_x; ++j){unsigned char value = new_map->data[index];costmap_[index] = interpretValue(value);++index;}}map_frame_ = new_map->header.frame_id;更新updateboundsvoid StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,double* max_x, double* max_y)
{//若非rolling地图，在有地图数据更新时更新边界，否则，根据静态层更新的区域的边界更新传入的边界。if( !layered_costmap_->isRolling() ){if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))return;}useExtraBounds(min_x, min_y, max_x, max_y);double wx, wy;
//将map系中的起点（x_， y_）与终点（x_ + width_,， y_ + height_）转换到世界系，
//确保传入的bound能包含整个map在世界系中的范围。mapToWorld(x_, y_, wx, wy);*min_x = std::min(wx, *min_x);*min_y = std::min(wy, *min_y);mapToWorld(x_ + width_, y_ + height_, wx, wy);*max_x = std::max(wx, *max_x);*max_y = std::max(wy, *max_y);has_updated_data_ = false;
}更新updateCosts  若为rolling地图，则找到静态地图和global系之间的坐标转换，通过主地图→global→静态地图的转换过程，找到主地图的cell在静态地图上对应的cost，赋值给主地图。void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{if (!map_received_)return;if (!layered_costmap_->isRolling()){// if not rolling, the layered costmap (master_grid) has same coordinates as this layerif (!use_maximum_)updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);elseupdateWithMax(master_grid, min_i, min_j, max_i, max_j);}else{// If rolling window, the master_grid is unlikely to have same coordinates as this layerunsigned int mx, my;double wx, wy;// Might even be in a different frametf::StampedTransform transform;try{tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0), transform);}catch (tf::TransformException ex){ROS_ERROR("%s", ex.what());return;}// Copy map data given proper transformationsfor (unsigned int i = min_i; i < max_i; ++i){for (unsigned int j = min_j; j < max_j; ++j){// Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinateslayered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);// Transform from global_frame_ to map_frame_tf::Point p(wx, wy, 0);p = transform(p);// Set master_grid with cell from mapif (worldToMap(p.x(), p.y(), mx, my)){if (!use_maximum_)master_grid.setCost(i, j, getCost(mx, my));elsemaster_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));}}}}
}

四、如何使用地图用于逻辑处理（如规划）

上述介绍了如何创建和维护更新地图：

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