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

从UML中能够看到,StaticLayer主要是在实现Layer层要求实现的接口。

virtual void onInitialize();
virtual void activate();
virtual void deactivate();
virtual void reset();
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,double* max_x, double* max_y);
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual void matchSize();

函数virtual void activate()

void StaticLayer::activate()
{
onInitialize();
}

而函数onInitialize() 中,首先初始化了一堆参数,然后调用

map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this);//一旦收到topic 是“map”的消息,就调用`incomingMap`

  

 while (!map_received_ && g_nh.ok())
{
ros::spinOnce();
r.sleep();
}//如果map_received_一直是false,则阻塞在这里。而更新map_received_的地方在回调函数incomingMap

接下来判断是否接受static map的更新,如果是则开启对topic为map_topic + "_updates" 的更新。最后开启参数动态配置服务。 

函数matchSize 中的操作依然是根据master map的尺寸,更新本层的尺寸:

void StaticLayer::matchSize()
{
// If we are using rolling costmap, the static map size is
// unrelated to the size of the layered costmap
if (!layered_costmap_->isRolling())
{
Costmap2D* master = layered_costmap_->getCostmap();
resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
master->getOriginX(), master->getOriginY());
}
}

函数interpretValue 则是将参数根据阈值,设定为NO_INFORMATION  FREE_SPACE LETHAL_OBSTACLE FREE_SPACE 或者其他值。

unsigned char StaticLayer::interpretValue(unsigned char value)
{
// check if the static value is above the unknown or lethal thresholds
if (track_unknown_space_ && value == unknown_cost_value_)
return NO_INFORMATION;
else if (!track_unknown_space_ && value == unknown_cost_value_)
return FREE_SPACE;
else if (value >= lethal_threshold_)
return LETHAL_OBSTACLE;
else if (trinary_costmap_)
return FREE_SPACE; double scale = (double) value / lethal_threshold_;
return scale * LETHAL_OBSTACLE;
}

以下分析回调函数incomingMap

void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
{
unsigned int size_x = new_map->info.width, size_y = new_map->info.height; ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution); // resize costmap if size, resolution or origin do not match
//这里判断master map的尺寸是否和获取到的static map一致,如果不一致,则应该修改master map
Costmap2D* master = layered_costmap_->getCostmap();
if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x ||
master->getSizeInCellsY() != size_y ||
master->getResolution() != new_map->info.resolution ||
master->getOriginX() != new_map->info.origin.position.x ||
master->getOriginY() != new_map->info.origin.position.y ||
!layered_costmap_->isSizeLocked()))
{
// Update the size of the layered costmap (and all layers, including this one)
ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x, new_map->info.origin.position.y, true);//修改了master map
}
//如果本层的数据和订阅到的map尺寸不一致,则更新本层的尺寸
else if (size_x_ != size_x || size_y_ != size_y ||
resolution_ != new_map->info.resolution ||
origin_x_ != new_map->info.origin.position.x ||
origin_y_ != new_map->info.origin.position.y)
{
// only update the size of the costmap stored locally in this layer
ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
resizeMap(size_x, size_y, new_map->info.resolution,new_map->info.origin.position.x, new_map->info.origin.position.y);
} unsigned int index = 0; // initialize the costmap with static data
//这里将订阅拿到的map数据拷贝到了本层static map的数据成员`costmap_`
for (unsigned int i = 0; i < size_y; ++i)
{
for (unsigned int j = 0; j < size_x; ++j)
{
unsigned char value = new_map->data[index];
costmap_[index] = interpretValue(value);
++index;
}
}
map_frame_ = new_map->header.frame_id; // we have a new map, update full size of map
x_ = y_ = 0;
width_ = size_x_;
height_ = size_y_;
map_received_ = true;
has_updated_data_ = true; // shutdown the map subscrber if firt_map_only_ flag is on
if (first_map_only_)
{
map_sub_.shutdown();
}
}

函数 updateBounds 这里设定为整张static map的大小:

void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y)
{ if( !layered_costmap_->isRolling() ){
if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
return;
} useExtraBounds(min_x, min_y, max_x, max_y); double wx, wy; mapToWorld(x_, y_, wx, wy);
*min_x = std::min(wx, *min_x);
*min_y = std::min(wy, *min_y); mapToWorld(x_ + width_, y_ + height_, wx, wy);
*max_x = std::max(wx, *max_x);
*max_y = std::max(wy, *max_y); has_updated_data_ = false;
}

函数 updateCosts

void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!map_received_)
return; if (!layered_costmap_->isRolling())
{
// if not rolling, the layered costmap (master_grid) has same coordinates as this layer这里如果不是rolling 选项,则直接将本层数据copy到master map,因为它们尺寸也一样
if (!use_maximum_)
updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
else
updateWithMax(master_grid, min_i, min_j, max_i, max_j);
}
else
{
// If rolling window, the master_grid is unlikely to have same coordinates as this layer
unsigned int mx, my;
double wx, wy;
// Might even be in a different frame
//首先获得map坐标系相对于global坐标系的位置,这个时候的map坐标系是随着机器人运动而运动的。
tf::StampedTransform transform;
try
{
tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0), transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s", ex.what());
return;
}
// Copy map data given proper transformations
for (unsigned int i = min_i; i < max_i; ++i)
{
for (unsigned int j = min_j; j < max_j; ++j)
{
// Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
// Transform from global_frame_ to map_frame_
tf::Point p(wx, wy, 0);
p = transform(p);
// Set master_grid with cell from map
if (worldToMap(p.x(), p.y(), mx, my))
{
if (!use_maximum_)
master_grid.setCost(i, j, getCost(mx, my));
else
master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
}
}
}
}
}

重点是这两句:

if (worldToMap(p.x(), p.y(), mx, my))
{
if (!use_maximum_)
master_grid.setCost(i, j, getCost(mx, my));

将static map层的每一点(i,j),都找到对应的master map的(mx,my),这样就可以直接更改master map的对应点了。

OK,静态地图分析到此为止~接下来分析Obstacle 层,这个略微要难点。

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

  1. ROS naviagtion analysis: costmap_2d--Costmap2DROS

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

  2. ROS naviagtion analysis: move_base

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

  3. ROS naviagtion analysis: costmap_2d--LayeredCostmap

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

  4. ROS naviagtion analysis: costmap_2d--ObstacleLayer

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

  5. ROS naviagtion analysis: costmap_2d--CostmapLayer

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

  6. ROS naviagtion analysis: costmap_2d--Layer

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

  7. ROS naviagtion analysis: costmap_2d--Costmap2D

    博客转载自:https://blog.csdn.net/u013158492/article/details/50492506 Costmap2D是存储地图数据的父类.真正的地图数据就存储在数据成员u ...

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

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

  9. ROS探索总结(十三)——导航与定位框架

    导航与定位是机器人研究中的重要部分.         一般机器人在陌生的环境下需要使用激光传感器(或者深度传感器转换成激光数据),先进行地图建模,然后在根据建立的地图进行导航.定位.在ROS中也有很多 ...

随机推荐

  1. 【ACM非算法部分】scanf()函数

    scanf函数 格式说明符: %c    字符型 %d    十进制数 %i     读入十进制,八进制,十六进制数.输入的时候正常是十进制数,若输入0前缀的数将读入八进制,0x前缀读入16进制.若输 ...

  2. HDU - 6166:Senior Pan(顶点集合最短路&二进制分组)

    Senior Pan fails in his discrete math exam again. So he asks Master ZKC to give him graph theory pro ...

  3. word中如何将空格变成换行

    大家在工作和学习中可能会遇到文字替换或符号替换,大家要学会txt.doc.xls之间的切换,替换好之后放到最终的文件中,txt好处是没有格式,doc个好处是有格式,而xls主要是分配到单元格中. 那么 ...

  4. opencv 学习笔记集锦

    整理了我所了解的有关OpenCV的学习笔记.原理分析.使用例程等相关的博文.排序不分先后,随机整理的.如果有好的资源,也欢迎介绍和分享. 1:OpenCV学习笔记 作者:CSDN数量:55篇博文网址: ...

  5. 为什么要使用ORM技术?和 JDBC 有何不一样?

    为什么要使用ORM技术?和 JDBC 有何不一样?        1.繁琐的代码问题:        用JDBC的API编程访问数据库,代码量较大,特别是访问字段较多的表的时候,代码显得繁琐.累赘,容 ...

  6. 选择第n大的数(分治法和排列实现)

    个人心得:在买的书上看到的一个经典分治题,题目意思就是给定一个数组,求第k小的数. 第一反应就是排序,然后返回第k-1位置的数就可以了,这样算法的复杂度是nlongn,在快速排序的基础下还是挺不错的. ...

  7. WCF+上传+大文件处理

    大文件处理的方式拆分读取,此文只为记录文件处理方式,供日后查阅. 源码来自http://blog.csdn.net/lywust/article/details/7009248 经过改编将源码改编为文 ...

  8. fn project 对象模型

    Applications At the root of everything are applications. In fn, an application is essentially a grou ...

  9. numpy之通用函数ufunc

    通用函数-元素级数组函数 通用函数(ufunc)是一种对ndarray执行元素级运算的函数. 一元ufunc import numpy as np arr = np.arange(-10,10,2) ...

  10. asp select count(*) 用 open还是excute

    dSql1="select count(*) from test_hist where uid="&cid  'dRs1.open dSql1,tConn,1,1  'dS ...