沉默杀手HomeArchivesTagsCategories
代价地图的层(一) 概述
|路径规划代价地图|
Word count: 505|Reading time: 2 min

所有的层都是继承Layer类，这个类本身比较简单，是一个虚基类，定义了两个重要接口：
updateBounds、updateCosts

1
2
3
4
5
6
7
8
9
10
11
12
13
// 每层的初始化
void Layer::initialize(LayeredCostmap* parent, std::string name, 
                          tf::TransformListener *tf)
{
    layered_costmap_ = parent;
    name_ = name;
    tf_ = tf;
    onInitialize(); 	//虚函数，空
}
const std::vector<geometry_msgs::Point>& Layer::getFootprint() const
{
    return layered_costmap_->getFootprint();
}

调用在Costmap2DROS的构造函数:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
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_);
    }
}

这里得到的插件即为各层子地图。如果是新定义的层，至少需要对虚函数onInitialize覆盖

Layer的所有虚函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
/* called by the LayeredCostmap to poll this plugin as to how
   much of the costmap it needs to update. 
   Each layer can increase  the size of this bounds.
 */
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, 
      double* min_x, double* min_y, double* max_x, double* max_y) {}

/** Actually update the underlying costmap, only within the bounds
 * calculated during UpdateBounds(). */
virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {}

/* Stop publishers. */
virtual void deactivate() {}
/* Restart publishers if they've been stopped. */
virtual void activate() {}
virtual void reset() {}
  /* make this layer match the size of the parent costmap. */
virtual void matchSize() {}
  /** LayeredCostmap calls this whenever the footprint there
 * changes (via LayeredCostmap::setFootprint()).  
  Override to be notified of changes to the robot's footprint. */
virtual void onFootprintChanged() {}
virtual void onInitialize() {}

  • 聚合layer的功能：LayeredCostmap* layered_costmap_
  • 此layer是否被使能，是否当前(更新信息)：bool enabled_，current
  • layer名：std::string name_，
  • 小车： std::vector< geometry_msgs::Point> footprint_spec_，

三层的加载过程

Costmap2DROS::mapUpdateLoop —— Costmap2DROS::updateMap —— LayeredCostmap::updateMap ——　每一层调用updateBounds　——　每一层调用updateCosts(核心函数)