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

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

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

函数virtual void activate()

  1. void StaticLayer::activate()
  2. {
  3. onInitialize();
  4. }

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

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

  

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

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

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

  1. void StaticLayer::matchSize()
  2. {
  3. // If we are using rolling costmap, the static map size is
  4. // unrelated to the size of the layered costmap
  5. if (!layered_costmap_->isRolling())
  6. {
  7. Costmap2D* master = layered_costmap_->getCostmap();
  8. resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
  9. master->getOriginX(), master->getOriginY());
  10. }
  11. }

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

  1. unsigned char StaticLayer::interpretValue(unsigned char value)
  2. {
  3. // check if the static value is above the unknown or lethal thresholds
  4. if (track_unknown_space_ && value == unknown_cost_value_)
  5. return NO_INFORMATION;
  6. else if (!track_unknown_space_ && value == unknown_cost_value_)
  7. return FREE_SPACE;
  8. else if (value >= lethal_threshold_)
  9. return LETHAL_OBSTACLE;
  10. else if (trinary_costmap_)
  11. return FREE_SPACE;
  12.  
  13. double scale = (double) value / lethal_threshold_;
  14. return scale * LETHAL_OBSTACLE;
  15. }

以下分析回调函数incomingMap

  1. void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
  2. {
  3. unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
  4.  
  5. ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);
  6.  
  7. // resize costmap if size, resolution or origin do not match
  8. //这里判断master map的尺寸是否和获取到的static map一致,如果不一致,则应该修改master map
  9. Costmap2D* master = layered_costmap_->getCostmap();
  10. if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x ||
  11. master->getSizeInCellsY() != size_y ||
  12. master->getResolution() != new_map->info.resolution ||
  13. master->getOriginX() != new_map->info.origin.position.x ||
  14. master->getOriginY() != new_map->info.origin.position.y ||
  15. !layered_costmap_->isSizeLocked()))
  16. {
  17. // Update the size of the layered costmap (and all layers, including this one)
  18. ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
  19. 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
  20. }
  21. //如果本层的数据和订阅到的map尺寸不一致,则更新本层的尺寸
  22. else if (size_x_ != size_x || size_y_ != size_y ||
  23. resolution_ != new_map->info.resolution ||
  24. origin_x_ != new_map->info.origin.position.x ||
  25. origin_y_ != new_map->info.origin.position.y)
  26. {
  27. // only update the size of the costmap stored locally in this layer
  28. ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
  29. resizeMap(size_x, size_y, new_map->info.resolution,new_map->info.origin.position.x, new_map->info.origin.position.y);
  30. }
  31.  
  32. unsigned int index = 0;
  33.  
  34. // initialize the costmap with static data
  35. //这里将订阅拿到的map数据拷贝到了本层static map的数据成员`costmap_`
  36. for (unsigned int i = 0; i < size_y; ++i)
  37. {
  38. for (unsigned int j = 0; j < size_x; ++j)
  39. {
  40. unsigned char value = new_map->data[index];
  41. costmap_[index] = interpretValue(value);
  42. ++index;
  43. }
  44. }
  45. map_frame_ = new_map->header.frame_id;
  46.  
  47. // we have a new map, update full size of map
  48. x_ = y_ = 0;
  49. width_ = size_x_;
  50. height_ = size_y_;
  51. map_received_ = true;
  52. has_updated_data_ = true;
  53.  
  54. // shutdown the map subscrber if firt_map_only_ flag is on
  55. if (first_map_only_)
  56. {
  57. map_sub_.shutdown();
  58. }
  59. }

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

  1. void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
  2. double* max_x, double* max_y)
  3. {
  4.  
  5. if( !layered_costmap_->isRolling() ){
  6. if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
  7. return;
  8. }
  9.  
  10. useExtraBounds(min_x, min_y, max_x, max_y);
  11.  
  12. double wx, wy;
  13.  
  14. mapToWorld(x_, y_, wx, wy);
  15. *min_x = std::min(wx, *min_x);
  16. *min_y = std::min(wy, *min_y);
  17.  
  18. mapToWorld(x_ + width_, y_ + height_, wx, wy);
  19. *max_x = std::max(wx, *max_x);
  20. *max_y = std::max(wy, *max_y);
  21.  
  22. has_updated_data_ = false;
  23. }

函数 updateCosts

  1. void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
  2. {
  3. if (!map_received_)
  4. return;
  5.  
  6. if (!layered_costmap_->isRolling())
  7. {
  8. // if not rolling, the layered costmap (master_grid) has same coordinates as this layer这里如果不是rolling 选项,则直接将本层数据copy到master map,因为它们尺寸也一样
  9. if (!use_maximum_)
  10. updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
  11. else
  12. updateWithMax(master_grid, min_i, min_j, max_i, max_j);
  13. }
  14. else
  15. {
  16. // If rolling window, the master_grid is unlikely to have same coordinates as this layer
  17. unsigned int mx, my;
  18. double wx, wy;
  19. // Might even be in a different frame
  20. //首先获得map坐标系相对于global坐标系的位置,这个时候的map坐标系是随着机器人运动而运动的。
  21. tf::StampedTransform transform;
  22. try
  23. {
  24. tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0), transform);
  25. }
  26. catch (tf::TransformException ex)
  27. {
  28. ROS_ERROR("%s", ex.what());
  29. return;
  30. }
  31. // Copy map data given proper transformations
  32. for (unsigned int i = min_i; i < max_i; ++i)
  33. {
  34. for (unsigned int j = min_j; j < max_j; ++j)
  35. {
  36. // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
  37. layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
  38. // Transform from global_frame_ to map_frame_
  39. tf::Point p(wx, wy, 0);
  40. p = transform(p);
  41. // Set master_grid with cell from map
  42. if (worldToMap(p.x(), p.y(), mx, my))
  43. {
  44. if (!use_maximum_)
  45. master_grid.setCost(i, j, getCost(mx, my));
  46. else
  47. master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
  48. }
  49. }
  50. }
  51. }
  52. }

重点是这两句:

  1. if (worldToMap(p.x(), p.y(), mx, my))
  2. {
  3. if (!use_maximum_)
  4. 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. Windows操作系统及其安全机制

    kali视频学习请看 http://www.cnblogs.com/lidong20179210/p/8909569.html Windows操作系统及其安全机制 Windows文件系统 FAT (F ...

  2. alpine docker 镜像 时区问题

    1. 时区处理 RUN apk update && apk add curl bash tree tzdata \ && cp -r -f /usr/share/zon ...

  3. COGS 2259 异化多肽——生成函数+多项式求逆

    题目:http://cogs.pro:8080/cogs/problem/problem.php?pid=2259 详见:https://www.cnblogs.com/Zinn/p/10054569 ...

  4. (装)Android高性能编程基本规范

    最近总结了一些,Android应用开发中,需要注意的一些事项,与大家分享     1.尽量少的声明全局变量   2.声明全局静态变量,一定要加final声明   3.声明非静态的全局变量,最好不要初始 ...

  5. sqlserver sql语句附加 分离数据库

    当使用 sp_attach_db 系统存储过程附加数据库时- - Tag: 当使用 sp_attach_db 系统存储过程附加数据库时 //附加数据库 sp_attach_db 当使用 sp_atta ...

  6. 蓝桥杯 算法训练 ALGO-114 黑白无常

    算法训练 黑白无常   时间限制:1.0s   内存限制:256.0MB 问题描述 某寝室的同学们在学术完之后准备玩一个游戏:游戏是这样的,每个人头上都被贴了一张白色或者黑色的纸,现在每个人都会说一句 ...

  7. Java-Maven-Runoob:Maven教程

    ylbtech-Java-Maven-Runoob:Maven教程 1.返回顶部 1. Maven 教程 Maven 翻译为"专家"."内行",是 Apache ...

  8. tp5操作mongo

    1.通过composer安装 composer require mongodb/mongodb 2.使用 <?php /** * @author: jim * @date: 2017/11/17 ...

  9. tomcat 1字节的UTF-8序列的字节1无效

    微信支付时, 命名返回支付成功, 但是成功后却返回如下的错误, 在测试环境都是没有任何问题, 到客户现场后, 可能客户现场使用的4G网络, 用微信支付时一直报这样的错误 错误现象: com.sun.o ...

  10. C#多线程List的非线程安全性

    背景:最近在做多线程方面的工作,工作中发现多线程中很多坑,这里就有一个List添加对象的误区,这里做个分享跟大家讲讲这个坑是怎么形成的怎么样避免. 示例: 代码及错误: 如果单单只从程序逻辑上看,应该 ...