博客转载自:https://blog.csdn.net/u013158492/article/details/50492506

Costmap2D是存储地图数据的父类。真正的地图数据就存储在数据成员unsigned char *costmap_ 。

首先,分析类的构造函数:,默认构造函数:Costmap2D::Costmap2D() 

// just initialize everything to NULL by default
Costmap2D::Costmap2D() :
size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL)
{
access_ = new mutex_t();
}

带参数的构造函数:Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution, double origin_x, double origin_y, unsigned char default_value)

Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
double origin_x, double origin_y, unsigned char default_value) :
size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x),
origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
{
access_ = new mutex_t(); // create the costmap
initMaps(size_x_, size_y_);
resetMaps();
}

Copy 构造函数:Costmap2D::Costmap2D(const Costmap2D& map)

Costmap2D::Costmap2D(const Costmap2D& map) :
costmap_(NULL)
{
access_ = new mutex_t();
*this = map;
}

Assignment 构造函数:Costmap2D& Costmap2D::operator=(const Costmap2D& map)

Costmap2D& Costmap2D::operator=(const Costmap2D& map)
{
// check for self assignement
if (this == &map)
return *this; // clean up old data
deleteMaps(); size_x_ = map.size_x_;
size_y_ = map.size_y_;
resolution_ = map.resolution_;
origin_x_ = map.origin_x_;
origin_y_ = map.origin_y_; // initialize our various maps
initMaps(size_x_, size_y_); // copy the cost map
memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char)); return *this;
}

每次对costmap_ 操作都需要上锁access_=new mutex_t(), ‘mutex_t’ 实际定义是typedef boost::recursive_mutex mutex_t递归锁。

函数Costmap2D::setConvexPolygonCost

首先将机器人坐标系下的机器人轮廓点,全部转到地图坐标系下

 // we assume the polygon is given in the global_frame... we need to transform it to map coordinates
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);
}

然后通过下面的调用,得到在polygon内部的全部cell,存储在polygon_cells

std::vector<MapLocation> polygon_cells;
// get the cells that fill the polygon
// this function is to get all the cells inside the polygon
convexFillCells(map_polygon, polygon_cells);

然后获取这些内部cell的index,再对地图costmap_ 遍历进行赋值操作:

// set the cost of those cells
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;
}

那么问题 来了,convexFillCells(map_polygon, polygon_cells); 是怎么获取到的全部的内部点的呢?

// first get the cells that make up the outline of the polygon
// this function will get the edges along the polygon
polygonOutlineCells(polygon, polygon_cells);

首先获得轮廓点之间连线的cell的列表。然后对这些边缘点做一次排序:

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;
}

操作完成后得到的polygon_cells 的cell都按照x坐标从小到大排序好了。然后开始沿着x轴,对每个相同的x,检查y值,获取y值最大的和y值最小的polygoncell:

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;
}

最后将y最大的和y最小的整个列的所有cell全部都塞进polygon_cells去:

 MapLocation pt;
// loop though cells in the column
for (unsigned int y = min_pt.y; y < max_pt.y; ++y)
{
pt.x = x;
pt.y = y;
polygon_cells.push_back(pt);
}

回到刚才,根据轮廓点,就能获得轮廓点连线的全部的边缘点函数polygonOutlineCells

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);
}
}

主要的被调用的函数如下,它调用了bresenham2D 函数,这个算法实现了 对于离散的平面点,指定两个点,找到两点之间的其他点,使得这些中间组成一个尽可能趋近直线的点集。

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)

函数bool Costmap2D::saveMap(std::string file_name) 执行将costmap2D类中的costmap_这个指针指向的数据全部存储成文件。由于数据本身是一维的,所以需要在文件开头写入x,y的各自size值,另外加上一个分隔符0xff与地图数据分开。 
Costmap2D 类分析就是这么多,相比之前的简单得多,毕竟主要是作为父类,供obstacle ,inflation,static, voxel继承用的。

ROS naviagtion analysis: costmap_2d--Costmap2D的更多相关文章

  1. ROS naviagtion analysis: costmap_2d--ObstacleLayer

    博客转载自:https://blog.csdn.net/u013158492/article/details/50493676 构造函数 ObstacleLayer() { costmap_ = NU ...

  2. ROS naviagtion analysis: costmap_2d--StaticLayer

    博客转载自:https://blog.csdn.net/u013158492/article/details/50493246 从UML中能够看到,StaticLayer主要是在实现Layer层要求实 ...

  3. ROS naviagtion analysis: costmap_2d--Costmap2DROS

    博客转载自:https://blog.csdn.net/u013158492/article/details/50485418 在上一篇文章中moveBase就有关于costmap_2d的使用: pl ...

  4. ROS naviagtion analysis: move_base

    博客转载自:https://blog.csdn.net/u013158492/article/details/50483123 这是navigation的第一篇文章,主要通过分析ROS代码级实现,了解 ...

  5. ROS naviagtion analysis: costmap_2d--CostmapLayer

    博客转自:https://blog.csdn.net/u013158492/article/details/50493220 这个类是为ObstacleLayer StaticLayer voxelL ...

  6. ROS naviagtion analysis: costmap_2d--LayeredCostmap

    博客转自:https://blog.csdn.net/u013158492/article/details/50490490 在数据成员中,有两个重要的变量:Costmap2D costmap_和 s ...

  7. ROS naviagtion analysis: costmap_2d--Layer

    博客转载自:https://blog.csdn.net/u013158492/article/details/50493113 这个类中有一个LayeredCostmap* layered_costm ...

  8. costmap_2d 解析

    costmap_2d这个包提供了一种2D代价地图的实现方案,该方案利用输入的传感器数据,构建数据2D或者3D代价地图(取决于是否使用基于voxel的实现),并根据占用网格和用户定义的膨胀半径计算2D代 ...

  9. ROS 教程之 navigation :在 catkin 环境下创建costmap layer plugin

    在做机器人导航的时候,肯定见到过global_costmap和local_costmap.global_costmap是为了全局路径规划服务的,如从这个房间到那个房间该怎么走.local_costma ...

随机推荐

  1. 使用IntelliJ IDEA开发SpringMVC网站的学习

    最近开始了“使用IntelliJ IDEA开发SpringMVC网站”的学习,有幸看到一份非常完善的学习资料,笔者非常用心的详细注释了一份关于博客的开发过程和细节,并且在评论中回复大家提出的问题,非常 ...

  2. Django Rest Framework - Could not resolve URL for hyperlinked relationship using view name “user-detail”

    要把跟当前表相关的viewset定义出来 http://stackoverflow.com/questions/20550598/django-rest-framework-could-not-res ...

  3. Flex布局(转载)

    网页布局(layout)是 CSS 的一个重点应用. 布局的传统解决方案,基于盒状模型,依赖 display 属性 + position属性 + float属性.它对于那些特殊布局非常不方便,比如,垂 ...

  4. HPPTS SSL

    https加密.解密.及验证过程如下图: HTTPS怎么实现安全传输的? 建立安全传输 HTTPS中, 客户端首先打开一条到WEB服务器443端口的连接. 一旦建立了TCP连接 ,客户端和服务器就会初 ...

  5. SpringBean生命周期

    Spring作为当前Java最流行.最强大的轻量级框架,受到了程序员的热烈欢迎.准确的了解Spring Bean的生命周期是非常必要的.我们通常使用ApplicationContext作为Spring ...

  6. java 代码,练习ip,主机名的获取方法。InetAddress类

    package clientFrame; import java.io.IOException; import java.net.*; public class tai { public static ...

  7. redirect uri 参数错误 怎么办

    这种情况,多数是因为请求地址不合法所致. 去公众号中添加合法的地址. 这种地址需要满足一些条件. 设置地址 满足的条件 保证可以访问到安全文件 如果访问不到的话,将无法保存 这里是文件存放位置 经过这 ...

  8. 登陆验证系统实例-三种(cookie,session,auth)

    登陆验证 因为http协议是无状态协议,但是我们有时候需要这个状态,这个状态就是标识 前端提交from表单,后端获取对应输入值,与数据库对比,由此对象设置一个标识,该对象 在别的视图的时候,有此标识, ...

  9. web开发 那些年基于Redis的Provider库

    因为session基于本地cache,以前我们自己写分布式缓存,或者数据库存储,或者cookie加密存储,来保存用户状态信息,但较少的直接通过创建一个继承 SessionStateStoreProvi ...

  10. Java学习之Mysql结构优化

    背景:业务发展初期为了便于快速迭代,很多应用都采用集中式的架构,随着业务规模的扩展,系统变得越来越复杂,访问量越来越大,不得不进一步扩展系统的吞吐能力. 优化1.主从集群:通过数据库的复制策略,可以将 ...