ROS导航相关消息（二十三）

目录

导航之地图

1.nav_msgs/MapMetaData地图元数据

2.nav_msgs/OccupancyGrid地图栅格数据

 地图内容数据

导航之里程计

 导航之坐标变换

 导航之定位

 导航之目标点与路径规划

 目标点设定

路径规划

导航之激光雷达

导航之相机

深度图像转激光数据 

1.depthimage_to_laserscan简介

1.1原理

1.2优缺点

1.3安装

2.depthimage_to_laserscan节点说明

2.1订阅的Topic

2.2发布的Topic

2.3参数

3.depthimage_to_laserscan使用

3.1编写launch文件

3.2修改URDF文件

3.3执行

4.SLAM应用

 7.4 本章小结

---

 

在导航功能包集中包含了诸多节点，毋庸置疑的，不同节点之间的通信使用到了消息中间件(数据载体)，在上一节的实现中，这些消息已经在rviz中做了可视化处理，比如:地图、雷达、摄像头、里程计、路径规划...的相关消息在rviz中提供了相关组件，本节主要介绍这些消息的具体格式。

导航之地图

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

nav_msgs/MapMetaData

• 地图元数据，包括地图的宽度、高度、分辨率等。

nav_msgs/OccupancyGrid

• 地图栅格数据，一般会在rviz中以图形化的方式显示。

1.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

 

2.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

跟上面比多了

 地图内容数据

int8[] data

1. 地图中的每一个像素取值在 [0,255] 之间，白色为 255，黑色为 0，该值设为 x；

这个就是地图内容数据

 

 这个节点读取nav.pgm

 把地图内容数据输出到txt里

此时就是会订阅话题/map然后输出到文件

其实这个txt就是之前的静态地图

 可以看到有大量的未知区域（-1）

白色是空闲（0）

黑色是占用（100）

 全局代价地图

启动仿真 

 启动导航完整实现

 输出话题数据

 

 0：未知

100：占用

0-100：越靠近障碍物代价越靠近100

导航之里程计

里程计相关消息是: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

 

 坐标变换是必须的，因为传感器感知到的信息必须转换为相对于底盘的

具体的在之前的ros常用组件用过

(1条消息) ROS中的TF坐标变换工具及实现、Rviz查看（十四）C++、python_啥也不是的py人的博客-CSDN博客https://blog.csdn.net/weixin_50920579/article/details/123966411

 导航之定位

 定位相关消息是: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

 

 坐标系id（就是一串独一无二的序列号）

 

目标点具体数据

 

 目标点位姿

(2条消息) 解决办法：WARNING: no messages received and simulated time is active. Is /clock being published?_weixin_43890711的博客-CSDN博客https://blog.csdn.net/weixin_43890711/article/details/94555166

路径规划

路径规划相关消息是: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 #扫描强度数据，如果设备不支持强度数据，该数组为空

 里面的参数和laser.xacro有关系

 ranges代表扫描到的障碍物与小车的距离

intensities是强度数据激光雷达不支持强度数据所以都是0

导航之相机

 

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

sensor_msgs/Image 对应的一般的图像数据，

sensor_msgs/CompressedImage 对应压缩后的图像数据，

sensor_msgs/PointCloud2 对应的是点云数据(带有深度信息的图像数据)。

启动仿真

启动kinect那个launch

 进行配置

 输出相机数据

 data中就是每个像素的值

调用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     #是否有无效点

 

 

 输出点云数据

 offset：对应xyz的偏移量（=0时代表原点）

datatype数据类型

count数据个数

 

 data由xyz、rgb组成的数组

point_step一个点的步长

row_step每行总步长

is_bigendian是否有无效点

深度图像转激光数据 

 

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

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

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

 

1.depthimage_to_laserscan简介

1.1原理

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

 

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

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

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

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

1.2优缺点

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

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

1.3安装

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

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

 

 

 

2.depthimage_to_laserscan节点说明

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

2.1订阅的Topic

image(sensor_msgs/Image)

• 输入图像信息。

camera_info(sensor_msgs/CameraInfo)

• 关联图像的相机信息。通常不需要重新映射，因为camera_info将从与image相同的命名空间中进行订阅。

2.2发布的Topic

scan(sensor_msgs/LaserScan)

• 发布转换成的激光雷达类型数据。

2.3参数

该节点参数较少，只有如下几个，一般需要设置的是: 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。

3.depthimage_to_laserscan使用

3.1编写launch文件

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

 

因为默认发布的图像信息是image所以要重映射为 /camera/depth/image_raw

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

 

 

3.2修改URDF文件

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

3.3执行

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

 

2.启动转换

 

 

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

 

 

4.SLAM应用

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

1.先启动 Gazebo 仿真环境；

2.启动转换节点；

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

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

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

5.在 rviz 中添加组件，显示栅格地图最后，就可以通过键盘控制gazebo中的机器人运动，同时，在rviz中可以显示gmapping发布的栅格地图数据了，

但是，前面也介绍了，由于精度和检测范围的原因，尤其再加之环境的特征点偏少，建图效果可能并不理想，建图中甚至会出现地图偏移的情况。

 7.4 本章小结

本章介绍了在仿真环境下的机器人导航实现，主要内容如下:

• 导航概念以及架构设计
• SLAM概念以及gmapping实现
• 地图的序列化与反序列化
• 定位实现
• 路径规划实现
• 导航中涉及的消息解释

导航整体设计架构中，包含地图、定位、路径规划、感知以及控制等实现，感知与控制模块在上一章机器人系统仿真中已经实现了，因此没有做过多介绍，其他部分，当前也是基于仿真环境实现的，后续，我们将搭建一台实体机器人并实现导航功能。