costmap_2d代码解析之二：静态层StaticLayer和障碍层ObstacleLayer插件

ROS

一、静态层StaticLayer
参考https://blog.csdn.net/Neo11111/article/details/104849757

//StaticLayer继承CostmapLayer类，CostmapLayer继承Layer类
class StaticLayer : public CostmapLayer

开启地图的订阅，以及bound和cost的更新。静态地图默认只更新一次，地图的边界也只更新一次，对于rolling地图，需要随着机器人的运动位置不断更新附近的cost，而对于非rolling地图则不需要。

具体看代码注释。

二、障碍层ObstacleLayer
参考https://blog.csdn.net/Neo11111/article/details/104852657

//StaticLayer继承CostmapLayer类，CostmapLayer继承Layer类
class ObstacleLayer : public CostmapLayer

1.关于消息
sensor_msg::LaserScan消息定义如下:
#
# 测量的激光扫描角度，逆时针为正
# 设备坐标帧的0度面向前（沿着X轴方向）
#

Header header
float32 angle_min        # scan的开始角度 [弧度]
float32 angle_max        # scan的结束角度 [弧度]
float32 angle_increment  # 每次扫描增加的角度（角度分辨率） [弧度]
float32 time_increment   # 每次扫描时间间隔 [秒],比如10Hz，2000个点，则time_increment = 1/10/2000；即1s扫描10次（10圈），每1圈需要发射2000次光束。
float32 scan_time        # 扫描的时间间隔[秒]
float32 range_min        # 最小的测量距离 [米]
float32 range_max        # 最大的测量距离 [米]
float32[] ranges         # 测量的距离数据数值（最主要数据） [米] (注意: 值 < range_min 或 > range_max 应当被丢弃),不在测距范围内的用inf填充
float32[] intensities    # 强度数据 [device-specific units],ranges有数据则非0,返回光的强度，代表了反射面的情况。

注意：1）ranges，intensities数组大小应该是根据 angle_min: 0.0 angle_max: 6.26573181152 angle_increment: 0.0174532923847 算出来的个数，并不是固定长度。size = (angle_max-angle_min)/angle_increment；2)https://www.ncnynl.com/archives/201702/1330.html实现生成虚拟数据发布传感器话题

sensor_msgs::PointCloud 消息,这个消息支持将三维空间中的点的数组以及任何保存在一个信道中的相关数据。

#This message holds a collection of 3d points, plus optional additional information about each point.
#Each Point32 should be interpreted as a 3d point in the frame given in the header

Header header
geometry_msgs/Point32[] points  #Array of 3d points
ChannelFloat32[] channels       #Each channel should have the same number of elements as   points array, and the data in each channel should correspond 1:1 with each point

sensor_msgs::PointCloud2是一类点云数据结构，消息定义如下
$ rosmsg info sensor_msgs/PointCloud2

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height
uint32 width
sensor_msgs/PointField[] fields
  uint8 INT8=1
  uint8 UINT8=2
  uint8 INT16=3
  uint8 UINT16=4
  uint8 INT32=5
  uint8 UINT32=6
  uint8 FLOAT32=7
  uint8 FLOAT64=8
  string name
  uint32 offset
  uint8 datatype
  uint32 count
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense

sensor_msgs::LaserScan转pcl::PointCloud

首先我们需要订阅激光雷达topic（如/scan）获取到sensor_msgs::LaserScan的message．然后使用ROS提供的laser_geometry包将其转化为sensor_msgs::PointCloud2格式的message．接着将sensor_msgs::PointCloud2的message转换为PCL点云库所需的数据格式．有两种方法，一种方法是使用ROS提供的pcl_conversions包，另一种方法是直接订阅之前转化的PointCloud2的数据，这样可以自动完成两种类型的转换，需要注意的是这种方法需要引入pcl_ros/point_cloud.h．
如需要代码则参考https://www.guyuehome.com/12913

2.该类涉及的需要看的其它文件还有：
costmap_2d/observation.h
costmap_2d/observation_buffer.h及.cpp
costmap_2d/footprint.h
costmap_2d/costmap_2d.h
3.costmap_2d: obstacle_layer中关于激光雷达障碍物清除不干净的解决
https://blog.csdn.net/xinmei4275/article/details/88760505