在上一篇文章中moveBase就有关于costmap_2d的使用:
planner_costmap_ros_
是用于全局导航的地图,controller_costmap_ros_
是局部导航用的地图,地图类型为经过ROS封装的costmap_2d::Costmap2DROS*
。
以下是这个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 two frames
private_nh.param("global_frame", global_frame_, std::string("/map"));
private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
如果没有找到这两个坐标系的关系或者超时,则构造函数会一直阻塞在这里:
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())
{
last_error = ros::Time::now();
}
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("Using plugin \"%s\"", pname.c_str());
boost::shared_ptr<Layer> plugin =lugin_loader_.createInstance(type);
layered_costmap_->addPlugin(plugin);
plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
}
}
boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
这行会创建一个以 type为类类型的实例变量,然后让plugin这个指针指向这个实例。
` layered_costmap_->addPlugin(plugin);`
然后layered_costmap_
将这些类型的地图都加入,addPlugin
实现:
void addPlugin(boost::shared_ptr<Layer> plugin)
{
plugins_.push_back(plugin);
}
这里的关系是:Costmap2DROS
有一个layered_costmap_
数据成员,然后