costmap_2d详解2：Costmap 代码整体分析

最新推荐文章于 2024-09-25 13:36:41 发布

feidaji

最新推荐文章于 2024-09-25 13:36:41 发布

阅读量2.3k

点赞数 4

CC 4.0 BY-SA版权

分类专栏： ros机器人和slam从入门到精通 ros 文章标签： ros costmap_2d

本文链接：https://blog.youkuaiyun.com/feidaji/article/details/103178619

ros机器人和slam从入门到精通同时被 2 个专栏收录

29 篇文章

订阅专栏

ros

24 篇文章

订阅专栏

本文深入解析了ROS中的costmap_2d模块结构与工作原理，详细介绍了其代码文件组织、类继承关系及整体逻辑流程。从mobe_base调用costmap_2d_ros接口开始，至LayeredCostmap的实例化与插件加载，直至地图更新过程，全面展示了costmap如何通过不同插件层处理静态与动态障碍物，为机器人路径规划提供关键信息。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

代码文件结构：

costmap_2d
├── plugins
│   ├── beifen-virtualWall_layer.cpp
│   ├── inflation_layer.cpp  //膨胀层
│   ├── obstacle_layer.cpp  //动态障碍物层
│   ├── simple_layer.cpp
│   ├── static_layer.cpp   //静态障碍物层
│   ├── virtualWall_layer.cpp
│   └── voxel_layer.cpp //将动态障碍物层分成三维障碍物层
├── src
│   ├── array_parser.cpp
│   ├── costmap_2d_cloud.cpp
│   ├── costmap_2d.cpp
│   ├── costmap_2d_markers.cpp
│   ├── costmap_2d_node.cpp
│   ├── costmap_2d_publisher.cpp  //发布master costmap
│   ├── costmap_2d_ros.cpp  //与ros 对接的入口
│   ├── costmap_layer.cpp
│   ├── costmap_math.cpp
│   ├── footprint.cpp   //足迹
│   ├── layer.cpp  //各种costmap层的基类
│   ├── layered_costmap.cpp  //管理master costmap
│   └── observation_buffer.cpp  //动态障碍物数据信息保存数组

代码类继承关系及整体逻辑流程

mobe_base 中调用costmap_2d_ros 接口

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

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

costmap_2d_ros.cpp 中调用LayeredCostmap

//实例化LayeredCostmap的指针,track_unknown_space is true
  layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);

//创建一个以 type为类类型的实例变量，然后让plugin这个指针指向这个实例
      boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);     
      layered_costmap_->addPlugin(plugin);      

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

另外一个线程mapUpdateLoop，去更新地图

//更新地图
    updateMap(); => layered_costmap_->updateMap(x, y, yaw);

然后更新各个插件层地图范围，和cost 值
    (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
    (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);

LayeredCostmap 中更新每一costmap 范围和cell栅格cost 值
//根据各层的更新情况确定地图更新范围的边界
(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
//更新该层costmap 大小
costmap_.resetMap(x0, y0, xn, yn);

//调用各层的updateCosts函数用各层的信息去更新更新范围内的地图信息
//x0, y0, xn, yn是该层costmap地图左下角和右上角坐标，以像素表示
(*plugin)->updateCosts(costmap_, x0, y0, xn, yn);