Navigation源码阅读之costmap_2d

最新推荐文章于 2024-08-06 08:50:00 发布

若愚和小巧

最新推荐文章于 2024-08-06 08:50:00 发布

阅读量2.1k

点赞数 4

分类专栏：导航算法阅读

本文链接：https://blog.csdn.net/weixin_42048023/article/details/83992863

版权

导航算法阅读专栏收录该内容

8 篇文章 11 订阅

订阅专栏

costmap可谓是激光slam的商标一般，它直接决定了小车的路径设计与速度计算，不把它搞清楚心里都发虚。当然，作为一个只分析代码、不贴图也不进行科普的人，还是从导航的核心模块——move_base开始：

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

......

controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();

在move_base的构造函数中，调用了全局代价地图和局部代价地图的构造函数Costmap2DROS()，那就从这个类开始阅读，从头文件看来，它的构造函数接受一个名字和tf变换，这个类提供给外界的接口有很多，相互之间的调用的关系也很零散，我们随后慢慢来看。

void start();
void stop();
void pause();
void resume();
void updateMap();
void resetLayers();
bool isCurrent()
bool getRobotPose(tf::Stamped<tf::Pose>& global_pose) const;
std::string getName() const
double getTransformTolerance() const
Costmap2D* getCostmap()
std::string getGlobalFrameID()
std::string getBaseFrameID()
LayeredCostmap* getLayeredCostmap()
geometry_msgs::Polygon getRobotFootprintPolygon()
std::vector<geometry_msgs::Point> getRobotFootprint()
std::vector<geometry_msgs::Point> getUnpaddedRobotFootprint()
void getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const;
void setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points);
void setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint);

一、Costmap2DROS类（costmap_2d_ros.cpp）

１．构造函数里声明了一个global_frame_和robot_base_frame_，全局代价地图默认的参数如下，而局部代价地图的参数默认为base_link与odom，这也符合tf树的层级转化关系。

private_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
global_frame_ = tf::resolve(tf_prefix, global_frame_);
robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_);

２．接着构造函数反复检查两个frame的tf变换是否正常，报的这个警告应该大家也不陌生，但是tf不稳定这种事情，我一直也很抓狂，除了重启节点没什么好办法……

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();
  }
  tf_error.clear();
}

３．接下来是costmap的重头戏——layers。大家都知道，代价地图是由不同层级的plugins逐层累加的，包括静态地图层、障碍层、膨胀层等等，那么通过LayeredCostmap的对象，创造了一个由指向plugin的共享指针组成的容器，通过配置文件，向里一一添加插件层，若我们需要用其他传感器的信息制作代价地图，可以通过自定义插件层，加入即可。

layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);

下面是通过XmlRpc机制，阅读配置文件进行插件层初始化的过程。

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());

    boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
    layered_costmap_->addPlugin(plugin);
    plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
  }
}

接下来转到layerd_costmap.cpp，看看LayeredCostmap是如何一一创造层级的。

（１）转到头文件layer_costmap.h，私有成员中包含了一个容器plugins_：

std::vector<boost::shared_ptr<Layer> > plugins_;

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

（２）LayeredCostmap::resizeMap是调用了Costmap2D::resizeMap，这个函数是将costmap_这个数组重置，并赋予默认值（为什么要取一个如此容易引起歧义的名字）。Costmap2D这个类，都是围绕costmap_这个代价数组进行。

①Costmap2D::mapToWorld是利用数组下标mx，my，将数组中特定位置的点转化到世界坐标系下。

②Costmap2D::updateOrigin是利用copyMapRegion这个保护成员函数，将改变原点之后的数组储存下来并再次赋予costmap_。

（３）LayeredCostmap::updateMap是依次调用了plugins_中各插件层的updateBounds与updateCosts函数，对地图的最大边界与代价值进行重新赋值。

（４）LayeredCostmap::setFootprint则是通过footprint.cpp中的工具函数calculateMinAndMaxDistances，对车体尺寸进行建模，这个文件中均是对不同形状的车体进行外壳的位置计算。

４．回到Costmap2DROS的构造函数，这里还使用了一个Costmap2DPublisher的对象，那进入costmap_2d_publisher.cpp研究一下。

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

（１） Costmap2DPublisher::prepareGrid其实就是用一个nav_msgs::OccupancyGrid的topic，将代价地图中的cost依次按坐标装入，这个很有意思，类似于点云变图片那种方法～

５．接下来看看成员函数：

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

（２）Costmap2DROS::start和Costmap2DROS::stop是一对开启和关闭代价地图的函数，这两个函数都是通过插件层自身的成员函数activate和deactivate来实现的，我们转到plugins文件夹下的obstaclie_layer.cpp，看看它的具体流程。

①ObstacleLayer::activate是通过observation_subscribers_这个容器中的订阅过滤器来实现的，所谓订阅过滤器是指对某一话题的信息进行“过滤”，具体请戳http://docs.ros.org/jade/api/message_filters/html/c++/classmessage__filters_1_1Subscriber.html。因此它调用了subscribe()这个api将关闭的subscriber重新启动进行接收。ObstacleLayer::deactivate同理，是关闭接收功能。

（３）Costmap2DROS::getRobotPose是通过tf变换实现的获取当前位姿。在这里有可能会出现tf的问题，这个警告我也有看到过。。。

if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_)
{
  ROS_WARN_THROTTLE(1.0,"Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",current_time.toSec(),global_pose.stamp_.toSec(), transform_tolerance_);
  return false;
}

代价地图大致的实现过程就是这样，当然它还调用了另一些接口，例如用visualization marker表示代价地图的体素网格，这个是在rviz上观看代价地图的实现方式。

通过对这一大块代码的研读，可以做到心里有数，以后依靠costmap再搭建别的功能就不会心虚了～