Costmap通过各层地图订阅话题、接收传感器数据,维护各层地图数据,并最终整合出一张用于路径规划的主地图。
Costmap2DROS类是对整个代价地图内容的封装。
LayeredCostmap类是Costmap2DROS的类成员,它是“主地图”,也能够管理各层地图,因为它含有指向各层子地图的指针,能够调用子地图的类方法,开启子地图的更新。并且,各层子地图最后都会合并到主地图上,提供给规划器的使用。它含有Costmap2D类成员,这个类就是底层地图,用于记录地图数据。
CostmapLayer类派生自Layer类和Costmap2D类。Layer类中含有子地图层用到的一些函数,如更新size、更新bound、和主地图合并等;Costmap2D类存储该层维护的地图数据。由CostmapLayer类派生出StaticLayer类和ObstacleLayer类,即静态层和障碍层,前者获取静态地图,后者通过传感器数据不断更新,获得能反映障碍物信息的子地图。
由Layer类单独派生出InflationLayer类,即膨胀层,用它来反映障碍物在地图上向周边的膨胀。由于它的父类中不含Costmap2D类,所以其实膨胀层自身没有栅格地图要维护,这一点和另外两层有区别。
本篇记录对Costmap2DROS类和Costmap2D类的阅读和理解。
这里关注一下Costmap2DROS类的构造函数以及它在Movebase中被调用的函数。
首先是一些参数的获取。循环等待直到获得机器人底盘坐标系和global系之间的坐标转换。
并获取rolling_window、track_unknown_space、always_send_full_costmap的参数,默认为false。
Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) :
layered_costmap_(NULL),
name_(name),
tf_(tf),
transform_tolerance_(0.3),
map_update_thread_shutdown_(false),
stop_updates_(false),
initialized_(true),
stopped_(false),
robot_stopped_(false),
map_update_thread_(NULL),
last_publish_(0),
plugin_loader_("costmap_2d", "costmap_2d::Layer"),
publisher_(NULL),
dsrv_(NULL),
footprint_padding_(0.0)
{
//初始化old pose
old_pose_.setIdentity();
old_pose_.setOrigin(tf::Vector3(1e30, 1e30, 1e30));
ros::NodeHandle private_nh("~/" + name);
ros::NodeHandle g_nh;
//获取tf前缀
ros::NodeHandle prefix_nh;
std::string tf_prefix = tf::getPrefixParam(prefix_nh);
//两个坐标系
private_nh.param("global_frame", global_frame_, std::string("/map"));
private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
//确保基于tf前缀正确设置了坐标系
//注:tf::resolve或者TransformListener::resolve方法的作用就是使用tf_prefix参数将frame_name解析为frame_id
global_frame_ = tf::resolve(tf_prefix, global_frame_);
robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_);
ros::Time last_error = ros::Time::now();
std::string tf_error;
//确保机器人底盘坐标系和世界坐标系之间的有效转换
//否则一直停留在这里阻塞
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();
}
// 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();
}
//检测是否需要“窗口滚动”,从参数服务器获取以下参数
bool rolling_window, track_unknown_space, always_send_full_costmap;
private_nh.param("rolling_window", rolling_window, false);
private_nh.param("track_unknown_space", track_unknown_space, false);
private_nh.param("always_send_full_costmap", always_send_full_costmap, false);
接下来创建一个LayeredCostmap类实例layered_costmap_
,作为规划器使用的主地图,并通过它管理各层地图。
//初始化一个layered_costmap
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
接着,在参数服务器上获取“plugins”参数,这里得到的插件即为各层子地图。每层子地图使用Pluginlib(ROS插件机制)来实例化,各个层可以被独立的编译,且允许使用C++接口对Costmap做出任意的改变。循环将每个plugin即每层子地图添加进layered_costmap_类的指针组对象中,并对每层地图调用其initialize类方法,初始化各层地图。这个函数定义在Layer类
中,它向各层地图“通知”主地图的存在,并调用oninitialize类方法(Layer类中的虚函数,被定义在各层地图中)。
//如果没有这一项参数,重新设置旧参数
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)
{
//从my_list里获取地图名称和种类
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());
//这行会创建一个以type为类类型的实例变量,然后让plugin这个指针指向这个实例
boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
layered_costmap_->addPlugin(plugin);
//实际执行的是plugin调用的父类Layer的方法initialize
plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
}
}
订阅footprint话题,回调函数为setUnpaddedRobotFootprintPolygon。当话题上收到footprint时,回调函数会将接收到的footprint根据参数footprint_padding_的值进行“膨胀”,得到“膨胀”后的padded_footprint_,传递给各级地图。
创建另一个话题,该话题上将发布的内容是根据机器人当前位置计算出来的实时footprint的位置。
std::string topic_param, topic;
if (!private_nh.searchParam("footprint_topic", topic_param))
{
topic_param = "footprint_topic";
}
private_nh.param(topic_param, topic, std::string("footprint"));
footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon, this);
if (!private_nh.searchParam("published_footprint_topic", topic_param))
{
topic_param = "published_footprint";
}
private_nh.param(topic_param, topic, std::string("oriented_footprint"));
footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>("footprint", 1);
setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));
创建地图的发布器实例,Costmap2DPublisher类是用于地图发布的封装类。
//发布Costmap2D
publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",
always_send_full_costmap);
//创建地图更新线程的控制量
stop_updates_ = false;
initialized_ = true;
stopped_ = false;
//创建一个timer去检测机器人是否在移动
robot_stopped_ = false;
//回调函数movementCB通过比较前后两个pose的差来判断机器人是否在移动
timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);
开启动态参数配置,它的回调函数Costmap2DROS::reconfigureCB会在节点启动时运行一次,加载.cfg文件的配置参数。这个函数给对应参数赋值,并创建了一个Costmap2DROS::mapUpdateLoop函数
的线程,即地图更新线程。
//开启参数动态配置
dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
//回调函数reconfigureCB 除了对一些类成员的配置值做赋值以外,还会开启一个更新map的线程
dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1,
_2);
dsrv_->setCallback(cb);
}
这个函数循环调用UpdateMap函数
,更新地图。并以publish_cycle为周期,发布更新后的地图。
void Costmap2DROS::mapUpdateLoop(double frequency)
{
// the user might not want to run the loop every cycle
if (frequency == 0.0)
return;
ros::NodeHandle nh;
ros::Rate r(frequency);
while (nh.ok() && !map_update_thread_shutdown_)
{
struct timeval start, end;
double start_t, end_t, t_diff;
gettimeofday(&start, NULL);
updateMap();
gettimeofday(&end, NULL);
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
t_diff = end_t - start_t;
//计算地图更新时间
ROS_DEBUG("Map update time: %.9f", t_diff);
//更新地图边界及发布
if (publish_cycle.toSec() > 0 && layered_costmap_->isInitialized())
{
unsigned int x0, y0, xn, yn;
layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
publisher_->updateBounds(x0, xn, y0, yn);
ros::Time now = ros::Time::now();
if (last_publish_ + publish_cycle < now)
{
publisher_->publishCostmap();
last_publish_ = now;
}
}
r.sleep();
// make sure to sleep for the remainder of our cycle time
if (r.cycleTime() > ros::Duration(1 / frequency))
ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency,
r.cycleTime().toSec());
}
}
这个函数首先调用类内getRobotPose函数,通过tf转换,将机器人底盘系的坐标转换到global系,得到机器人的位姿。然后通过layered_costmap_调用LayeredCostmap类
的updateMap函数,更新地图。
void Costmap2DROS::updateMap()
{
if (!stop_updates_)
{
//获取机器人在地图上的位置和姿态
tf::Stamped < tf::Pose > pose;
if (getRobotPose (pose))
{
double x = pose.getOrigin().x(),
y = pose.getOrigin().y(),
yaw = tf::getYaw(pose.getRotation());
//调用layered_costmap_的updateMap函数,参数是机器人位姿
layered_costmap_->updateMap(x, y, yaw);
这里更新机器人的实时足迹,通过footprint_pub_发布。
//更新机器人足迹
geometry_msgs::PolygonStamped footprint;
footprint.header.frame_id = global_frame_;
footprint.header.stamp = ros::Time::now();
transformFootprint(x, y, yaw, padded_footprint_, footprint);
footprint_pub_.publish(footprint);
initialized_ = true;
}
}
}
start函数在Movebase中被调用,这个函数对各层地图插件调用activate函数,激活各层地图。
void Costmap2DROS::start()
{
std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
//检查地图是否暂停或者停止
if (stopped_)
{
//如果停止,需要重新订阅话题
//“Layer”是一个类,active是其中一个类方法
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
++plugin)
{
(*plugin)->activate();
}
stopped_ = false;
}
stop_updates_ = false;
//在costmap被重新初始化前阻塞..meaning one update cycle has run
ros::Rate r(100.0);
while (ros::ok() && !initialized_)
r.sleep();
}
Costmap2D类是记录地图数据的底层,它记录地图的x、y方向的尺寸,地图的分辨率,地图原点位置,以及用unsigned char类型记录地图内容。
protected:
unsigned int size_x_;
unsigned int size_y_;
double resolution_;
double origin_x_;
double origin_y_;
unsigned char* costmap_;
并且,Costmap2D类提供了一些对地图进行基本操作的函数,如:地图复制、用index/点坐标来设置/获取地图上该点的cost值、地图坐标和世界坐标之间的转换、获取地图大小/分辨率/原点、设置多边形边缘及内部点的cost值。
前面的函数内容都很简单,这里关注一下多边形cost的设置,它主要是用来设置机器人足迹内cell的cost
先将世界系下的多边形顶点转换到地图坐标系,并存放进map_polygon数组中。
bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
{
std::vector<MapLocation> map_polygon;
for (unsigned int i = 0; i < polygon.size(); ++i)
{
MapLocation loc;
if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y))
{
// ("Polygon lies outside map bounds, so we can't fill it");
return false;
}
map_polygon.push_back(loc);
}
接着调用convexFillCells函数
,通过机器人顶点坐标数组map_polygon得到多边形边缘及内部的全部cell,存放在polygon_cells中,并通过循环对多边形边缘及内部各cell的cost赋值。
std::vector<MapLocation> polygon_cells;
// get the cells that fill the polygon
convexFillCells(map_polygon, polygon_cells);
//把多边形边缘及内部的所有cell的cost设置为cost_value
for (unsigned int i = 0; i < polygon_cells.size(); ++i)
{
unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);
costmap_[index] = cost_value;
}
return true;
}
首先确保给定的多边形顶点不少于3个,接着调用类内polygonOutlineCells函数
,通过给定的顶点提取多边形边上的cell。
void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
{
// we need a minimum polygon of a triangle
if (polygon.size() < 3)
return;
//首先获得轮廓点之间连线的列表,存放在polygon_cells中
polygonOutlineCells(polygon, polygon_cells);
对边上的cell点的x做排序,使其按x坐标升序排列。
MapLocation swap;
unsigned int i = 0;
while (i < polygon_cells.size() - 1)
{
if (polygon_cells[i].x > polygon_cells[i + 1].x)
{
swap = polygon_cells[i];
polygon_cells[i] = polygon_cells[i + 1];
polygon_cells[i + 1] = swap;
if (i > 0)
--i;
}
else
++i;
}
遍历所有x,对每个相同的x,检查y,获得y最大和最小的polygon cell,将范围内的所有cell填充进polygon_cells,获得多边形边缘及内部的所有cell。
i = 0;
MapLocation min_pt;
MapLocation max_pt;
unsigned int min_x = polygon_cells[0].x;
unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x;
for (unsigned int x = min_x; x <= max_x; ++x)
{
if (i >= polygon_cells.size() - 1)
break;
if (polygon_cells[i].y < polygon_cells[i + 1].y)
{
min_pt = polygon_cells[i];
max_pt = polygon_cells[i + 1];
}
else
{
min_pt = polygon_cells[i + 1];
max_pt = polygon_cells[i];
}
i += 2;
while (i < polygon_cells.size() && polygon_cells[i].x == x)
{
if (polygon_cells[i].y < min_pt.y)
min_pt = polygon_cells[i];
else if (polygon_cells[i].y > max_pt.y)
max_pt = polygon_cells[i];
++i;
}
MapLocation pt;
//填充
for (unsigned int y = min_pt.y; y < max_pt.y; ++y)
{
pt.x = x;
pt.y = y;
polygon_cells.push_back(pt);
}
}
}
这个函数循环调用raytraceLine函数
,不断获取相邻之间的连线,最终组成多边形边上的cell,需要注意的是需要将最后一点和第一点连接起来,形成闭合。
void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
{
PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
for (unsigned int i = 0; i < polygon.size() - 1; ++i)
{
raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);
}
if (!polygon.empty())
{
unsigned int last_index = polygon.size() - 1;
// we also need to close the polygon by going from the last point to the first
raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y);
}
}
对于离散的平面点,指定两个点,这个函数可以找到两个点之间的其他点,使得这些中间组成一个尽可能趋近直线的点集。
template<class ActionType>
inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1,
unsigned int max_length = UINT_MAX)
{
int dx = x1 - x0;
int dy = y1 - y0;
unsigned int abs_dx = abs(dx);
unsigned int abs_dy = abs(dy);
int offset_dx = sign(dx);
int offset_dy = sign(dy) * size_x_;
unsigned int offset = y0 * size_x_ + x0;
// we need to chose how much to scale our dominant dimension, based on the maximum length of the line
double dist = hypot(dx, dy);
double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);
// if x is dominant
if (abs_dx >= abs_dy)
{
int error_y = abs_dx / 2;
bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
return;
}
// otherwise y is dominant
int error_x = abs_dy / 2;
bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
}
Neo 2020.3