ROS naviagtion analysis: costmap_2d--Costmap2DROS

最新推荐文章于 2025-06-30 14:23:11 发布

惜缘865

最新推荐文章于 2025-06-30 14:23:11 发布

阅读量1.8k

点赞数 2

CC 4.0 BY-SA版权

分类专栏： ROS Navigation 文章标签： ROS navigation costmap_2d

本文链接：https://blog.youkuaiyun.com/u011608180/article/details/93636136

写在最前：

尊重原创，尊重他人劳动成果。原文地址：https://blog.youkuaiyun.com/u013158492/article/details/50485418

在上一篇文章中moveBase就有关于costmap_2d的使用： planner_costmap_ros_是用于全局导航的地图, controller_costmap_ros_是局部导航用的地图，地图类型为经过ROS封装的costmap_2d::Costmap2DROS*。

move_base.cpp中planner_costmap_ros_的代码如下：

//create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause();

move_base.cpp中controller_costmap_ros_代码如下：

//create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();

以下是这个ROS类的UML：

这个类的成员变量：LayeredCostmap* layered_costmap_; pluginlib::ClassLoader<Layer> plugin_loader_; 这两个最重要的成员变量，而LayeredCostmap类又包含了Costmap2D costmap_; 这个数据成员。
下面是这些类之间的关系：

绿色的是核心代码，从ROS用户的角度，只需要调用Costmap2DROS这个类，因为这个类已经把所有关于地图的操作都封装好了。不过我这里是分析底层算法实现，就不得不写得很长很长。

所以还是先回到对Costmap2DROS这个类的分析，然后再进一步一层一层的分析其他的类。这些类完成了对机器人地图的表示和操作，因此其数据结构和算法都很有分析的价值。

首先是构造函数Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) : 因此必须提供一个tf参数。tf参数需要提供以下两个坐标系的关系：

// get global and robot base frame names
private_nh.param("global_frame", global_frame_, std::string("map"));
private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));

如果没有找到这两个坐标系的关系或者超时，则构造函数会一直阻塞在这里：

  // we need to make sure that the transform between the robot base frame and the global frame is available
  while (ros::ok()
      && !tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), &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();
  }

然后加入各个层次的地图：

  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("%s: Using plugin \"%s\"", name_.c_str(), pname.c_str());

      copyParentParameters(pname, type, private_nh);

      boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
      layered_costmap_->addPlugin(plugin);
      plugin->initialize(laye