机器人导航（3）：导航相关消息

地图

地图相关的消息主要有两个:

nav_msgs/MapMetaData: 地图元数据，包括地图的宽度、高度、分辨率等。
nav_msgs/OccupancyGrid: 地图栅格数据，一般会在rviz中以图形化的方式显示。

nav_msgs/MapMetaData

调用rosmsg info nav_msgs/MapMetaData显示消息内容如下:

time map_load_time
float32 resolution #地图分辨率
uint32 width #地图宽度
uint32 height #地图高度
geometry_msgs/Pose origin #地图位姿数据
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

nav_msgs/OccupancyGrid

调用 rosmsg info nav_msgs/OccupancyGrid显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
#--- 地图元数据
nav_msgs/MapMetaData info
  time map_load_time
  float32 resolution
  uint32 width
  uint32 height
  geometry_msgs/Pose origin
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
#--- 地图内容数据，数组长度 = width * height
int8[] data

里程计

里程计相关消息是:nav_msgs/Odometry，调用rosmsg info nav_msgs/Odometry 显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose #里程计位姿
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist
  geometry_msgs/Twist twist #速度
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z    
  # 协方差矩阵
  float64[36] covariance

坐标变换

坐标变换相关消息是: tf/tfMessage，调用rosmsg info tf/tfMessage 显示消息内容如下:

geometry_msgs/TransformStamped[] transforms #包含了多个坐标系相对关系数据的数组
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  string child_frame_id
  geometry_msgs/Transform transform
    geometry_msgs/Vector3 translation
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion rotation
      float64 x
      float64 y
      float64 z
      float64 w

定位

定位相关消息是:geometry_msgs/PoseArray，调用rosmsg info geometry_msgs/PoseArray显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Pose[] poses #预估的点位姿组成的数组
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

目标点与路径规划

目标点相关消息是:move_base_msgs/MoveBaseActionGoal，调用rosmsg info move_base_msgs/MoveBaseActionGoal显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
actionlib_msgs/GoalID goal_id
  time stamp
  string id
move_base_msgs/MoveBaseGoal goal
  geometry_msgs/PoseStamped target_pose
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    geometry_msgs/Pose pose #目标点位姿
      geometry_msgs/Point position
        float64 x
        float64 y
        float64 z
      geometry_msgs/Quaternion orientation
        float64 x
        float64 y
        float64 z
        float64 w

路径规划相关消息是:nav_msgs/Path，调用rosmsg info nav_msgs/Path显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/PoseStamped[] poses #由一系列点组成的数组
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w

激光雷达

激光雷达相关消息是:sensor_msgs/LaserScan，调用rosmsg info sensor_msgs/LaserScan显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 angle_min #起始扫描角度(rad)
float32 angle_max #终止扫描角度(rad)
float32 angle_increment #测量值之间的角距离(rad)
float32 time_increment #测量间隔时间(s)
float32 scan_time #扫描间隔时间(s)
float32 range_min #最小有效距离值(m)
float32 range_max #最大有效距离值(m)
float32[] ranges #一个周期的扫描数据
float32[] intensities #扫描强度数据，如果设备不支持强度数据，该数组为空

相机

深度相机相关消息有:sensor_msgs/Image、sensor_msgs/CompressedImage、sensor_msgs/PointCloud2

sensor_msgs/Image 对应的一般的图像数据，sensor_msgs/CompressedImage 对应压缩后的图像数据，sensor_msgs/PointCloud2 对应的是点云数据(带有深度信息的图像数据)。

调用rosmsg info sensor_msgs/Image显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height #高度
uint32 width  #宽度
string encoding #编码格式:RGB、YUV等
uint8 is_bigendian #图像大小端存储模式
uint32 step #一行图像数据的字节数，作为步进参数
uint8[] data #图像数据，长度等于 step * height

调用rosmsg info sensor_msgs/CompressedImage显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string format #压缩编码格式(jpeg、png、bmp)
uint8[] data #压缩后的数据

调用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      #存储点云的数组，总长度为 row_step * height
bool is_dense     #是否有无效点

深度图像转激光数据

本节介绍ROS中的一个功能包:depthimage_to_laserscan，顾名思义，该功能包可以将深度图像信息转换成激光雷达信息，应用场景如下:

在诸多SLAM算法中，一般都需要订阅激光雷达数据用于构建地图，因为激光雷达可以感知周围环境的深度信息，而深度相机也具备感知深度信息的功能，且最初激光雷达价格比价比较昂贵，那么在传感器选型上可以选用深度相机代替激光雷达吗？

答案是可以的，不过二者发布的消息类型是完全不同的，如果想要实现传感器的置换，那么就需要将深度相机发布的三维的图形信息转换成二维的激光雷达信息，这一功能就是通过depthimage_to_laserscan来实现的。

depthimage_to_laserscan简介

depthimage_to_laserscan将实现深度图像与雷达数据转换的原理比较简单，雷达数据是二维的、平面的，深度图像是三维的，是若干二维(水平)数据的纵向叠加，如果将三维的数据转换成二维数据，只需要取深度图的某一层即可，为了方面理解，请看官方示例:

图一:深度相机与外部环境(实物图)

图二:深度相机发布的图片信息，图中彩线对应的是要转换成雷达信息的数据

图三:将图二以点云的方式显示更为直观，图中彩线对应的仍然是要转换成雷达信息的数据

图四:转换之后的结果图(俯视)

优缺点
优点:深度相机的成本一般低于激光雷达，可以降低硬件成本；

缺点: 深度相机较之于激光雷达无论是检测范围还是精度都有不小的差距，SLAM效果可能不如激光雷达理想。

安装
使用之前请先安装,命令如下:

sudo apt-get install ros-melodic-depthimage-to-laserscan

depthimage_to_laserscan节点说明

depthimage_to_laserscan 功能包的核心节点是:depthimage_to_laserscan ，为了方便调用，需要先了解该节点订阅的话题、发布的话题以及相关参数。

订阅的Topic

image(sensor_msgs/Image)：输入图像信息。
camera_info(sensor_msgs/CameraInfo)：关联图像的相机信息。通常不需要重新映射，因为camera_info将从与image相同的命名空间中进行订阅。

发布的Topic

scan(sensor_msgs/LaserScan)：发布转换成的激光雷达类型数据。

参数

该节点参数较少，只有如下几个，一般需要设置的是: output_frame_id。

~scan_height(int, default: 1 pixel)

设置用于生成激光雷达信息的象素行数。
~scan_time(double, default: 1/30.0Hz (0.033s))

两次扫描的时间间隔。
~range_min(double, default: 0.45m)

返回的最小范围。结合range_max使用，只会获取 range_min 与 range_max 之间的数据。
~range_max(double, default: 10.0m)

返回的最大范围。结合range_min使用，只会获取 range_min 与 range_max 之间的数据。
~output_frame_id(str, default: camera_depth_frame)

激光信息的ID。

depthimage_to_laserscan使用

编写launch文件
编写launch文件执行，将深度信息转换成雷达信息

<launch>
    <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
        <remap from="image" to="/camera/depth/image_raw" />
        <param name="output_frame_id" value="camera"  />
    </node>
</launch>

订阅的话题需要根据深度相机发布的话题设置，output_frame_id需要与深度相机的坐标系一致。

修改URDF文件
经过信息转换之后，深度相机也将发布雷达数据，为了不产生混淆，可以注释掉 xacro 文件中的关于激光雷达的部分内容。

执行
1.启动gazebo仿真环境，如下:

2.启动rviz并添加相关组件(image、LaserScan)，结果如下:

SLAM应用

现在我们已经实现并测试通过深度图像信息转换成激光雷达信息了，接下来是实践阶段，通过深度相机实现SLAM，流程如下:

1.先启动 Gazebo 仿真环境；

2.启动转换节点；

3.再启动地图绘制的 launch 文件；

4.启动键盘键盘控制节点，用于控制机器人运动建图；

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

5.在 rviz 中添加组件，显示栅格地图最后，就可以通过键盘控制gazebo中的机器人运动，同时，在rviz中可以显示gmapping发布的栅格地图数据了，但是，前面也介绍了，由于精度和检测范围的原因，尤其再加之环境的特征点偏少，建图效果可能并不理想，建图中甚至会出现地图偏移的情况。