ROS naviagtion analysis: costmap_2d--StaticLayer
博客转载自: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的更多相关文章
- ROS naviagtion analysis: costmap_2d--Costmap2DROS
博客转载自:https://blog.csdn.net/u013158492/article/details/50485418 在上一篇文章中moveBase就有关于costmap_2d的使用: pl ...
- ROS naviagtion analysis: move_base
博客转载自:https://blog.csdn.net/u013158492/article/details/50483123 这是navigation的第一篇文章,主要通过分析ROS代码级实现,了解 ...
- ROS naviagtion analysis: costmap_2d--LayeredCostmap
博客转自:https://blog.csdn.net/u013158492/article/details/50490490 在数据成员中,有两个重要的变量:Costmap2D costmap_和 s ...
- ROS naviagtion analysis: costmap_2d--ObstacleLayer
博客转载自:https://blog.csdn.net/u013158492/article/details/50493676 构造函数 ObstacleLayer() { costmap_ = NU ...
- ROS naviagtion analysis: costmap_2d--CostmapLayer
博客转自:https://blog.csdn.net/u013158492/article/details/50493220 这个类是为ObstacleLayer StaticLayer voxelL ...
- ROS naviagtion analysis: costmap_2d--Layer
博客转载自:https://blog.csdn.net/u013158492/article/details/50493113 这个类中有一个LayeredCostmap* layered_costm ...
- ROS naviagtion analysis: costmap_2d--Costmap2D
博客转载自:https://blog.csdn.net/u013158492/article/details/50492506 Costmap2D是存储地图数据的父类.真正的地图数据就存储在数据成员u ...
- ROS 教程之 navigation :在 catkin 环境下创建costmap layer plugin
在做机器人导航的时候,肯定见到过global_costmap和local_costmap.global_costmap是为了全局路径规划服务的,如从这个房间到那个房间该怎么走.local_costma ...
- ROS探索总结(十三)——导航与定位框架
导航与定位是机器人研究中的重要部分. 一般机器人在陌生的环境下需要使用激光传感器(或者深度传感器转换成激光数据),先进行地图建模,然后在根据建立的地图进行导航.定位.在ROS中也有很多 ...
随机推荐
- Windows操作系统及其安全机制
kali视频学习请看 http://www.cnblogs.com/lidong20179210/p/8909569.html Windows操作系统及其安全机制 Windows文件系统 FAT (F ...
- alpine docker 镜像 时区问题
1. 时区处理 RUN apk update && apk add curl bash tree tzdata \ && cp -r -f /usr/share/zon ...
- COGS 2259 异化多肽——生成函数+多项式求逆
题目:http://cogs.pro:8080/cogs/problem/problem.php?pid=2259 详见:https://www.cnblogs.com/Zinn/p/10054569 ...
- (装)Android高性能编程基本规范
最近总结了一些,Android应用开发中,需要注意的一些事项,与大家分享 1.尽量少的声明全局变量 2.声明全局静态变量,一定要加final声明 3.声明非静态的全局变量,最好不要初始 ...
- sqlserver sql语句附加 分离数据库
当使用 sp_attach_db 系统存储过程附加数据库时- - Tag: 当使用 sp_attach_db 系统存储过程附加数据库时 //附加数据库 sp_attach_db 当使用 sp_atta ...
- 蓝桥杯 算法训练 ALGO-114 黑白无常
算法训练 黑白无常 时间限制:1.0s 内存限制:256.0MB 问题描述 某寝室的同学们在学术完之后准备玩一个游戏:游戏是这样的,每个人头上都被贴了一张白色或者黑色的纸,现在每个人都会说一句 ...
- Java-Maven-Runoob:Maven教程
ylbtech-Java-Maven-Runoob:Maven教程 1.返回顶部 1. Maven 教程 Maven 翻译为"专家"."内行",是 Apache ...
- tp5操作mongo
1.通过composer安装 composer require mongodb/mongodb 2.使用 <?php /** * @author: jim * @date: 2017/11/17 ...
- tomcat 1字节的UTF-8序列的字节1无效
微信支付时, 命名返回支付成功, 但是成功后却返回如下的错误, 在测试环境都是没有任何问题, 到客户现场后, 可能客户现场使用的4G网络, 用微信支付时一直报这样的错误 错误现象: com.sun.o ...
- C#多线程List的非线程安全性
背景:最近在做多线程方面的工作,工作中发现多线程中很多坑,这里就有一个List添加对象的误区,这里做个分享跟大家讲讲这个坑是怎么形成的怎么样避免. 示例: 代码及错误: 如果单单只从程序逻辑上看,应该 ...