Autoware实车测试记录（五）--实车测试遇到的问题以及解决方法

在实际测试中发现软件用实际硬件跑起来真的比在电脑上跑个demo遇到的问题多非常多，许多错误乍看起来比较邪门，但其实大部分也就是一些格式不对的问题。因为Autoware程序本身包含着非常多的功能节点，所以建议在检查错误的时候，先用rostopic确定信息输出是否有问题，然后善用rqt_graph这个命令来查看节点之间的关系是不是正确的，最后记得用录屏和rosbag录包进行记录和调试。这一套组合拳下来应该能解决大部分问题，至于剩下的问题就只能具体问题具体分析了。

下面我记录我在测试中遇到的问题，按时间顺序。

一、控制指令下发后，小车运动不连贯、顿挫。

这个问题出现在首次完成将Autoware控制指令转换为/cmd_vel时。

现象：在所有功能执行后，小车开始运动，但运动非常不连贯，一次只能走一小段几厘米左右，然后停一段时间，再走一段再停住。有点类似于毛毛虫那种蠕动。同时，小车只能直行，无法做出转向等动作。

查看话题，下发给小车的控制话题/cmd_vel输出数据不连续，混杂着大量0数据，有数据的和0数据各占一半，交错在一起。

---
linear:
  x:0.0
  y:0.0
  z:0.0
angular:
  x:0.0
  y:0.0
  z:0.0
---
linear:
  x:3.0
  y:0.0
  z:0.0
angular:
  x:0.0
  y:0.0
  z:0.227834879798
---
linear:
  x:3.0
  y:0.0
  z:0.0
angular:
  x:0.0
  y:0.0
  z:0.248274027847
---
linear:
  x:0.0
  y:0.0
  z:0.0
angular:
  x:0.0
  y:0.0
  z:0.0

出现原因：与小车底盘控制方式有关，初始有两种控制方式，一种是停用上位机使用手柄遥控。由于需要使用上位机软件控制，所以采用了上位机模式，禁用手柄通过启动小车自带驱动，使用键盘遥控。小车驱动中存在键盘控制的功能包，启动驱动launch文件时自动启动键盘控制节点。键盘控制节点实时发布/cmd_vel话题，同时控制转换节点也在发布这个话题，因此该话题有两个发布者，发布的内容混在一起导致了执行出现顿挫不连贯。

以至于无法转向产生的原因也是如此，底盘是差速底盘，不连贯的信息可以使小车短暂前进一下，但是转向无法通过这种不连贯消息实现。

问题解决起来也是相当简单，找到小车驱动的launch文件，将里面的键盘控制节点注释掉另存为一个新文件，测试的时候运行新的launch文件就可以了。

二、无法检测到障碍物（局部路径规划失效）

这个问题主要表现在使用局部路径避障的时候发现局部路径均为绿色，也即检测不到障碍物。同时从最开始到最终过程中后台均没有出现过报错情况。运行效果为小车可以正常沿着全局路线规划给出的路径前进，但是对于路上的一切障碍物全都无视，局部路径规划在rviz中全绿。

这个报错主要由于我粗心大意，本来是点云预处理部分的错误，由于前期工作正常对这些功能过分信任，导致后期工作异常的时候程序曾出现的一些不妙征兆都被我忽略了。当发现局部路径失效的时候，起初认为是哪个功能没正确启动，顺藤摸瓜一直找到最先启动的几个功能之一-点云预处理部分。

事实上问题出现在地面滤波功能，也就是点云预处理部分的ring_ground_filter这个功能。这个功能启动后，本应订阅/points_raw话题，处理后输出/points_no_ground话题，后续该话题被点云聚类节点和障碍物追踪节点所订阅。

其实这个节点出现问题是有征兆的，比如在启动障碍物追踪部分，因为没有启动ndt先启动的障碍物追踪，正常情况后台终端要疯狂输出报错信息：必要话题未发布。开启ndt之后会输出障碍物追踪节点的追踪信息。节点故障后后台静悄悄的什么也没有......

查到此处时检查话题，ring_ground_filter本应输出一个去地面的点云信息和只有地面的点云信息。实际上去地面的点云话题没有输出，而只有地面的点云话题输出的信息也是不对的。没有了去地面点云信息，后续障碍物追踪功能当然无法检测到障碍物，路径规划部分也就认为前方畅通无阻，沿着全局路径规划给出的路径一路向前。

明白了出错的原因后，直接换了一个地面滤波功能包，就是改用了ray_ground_Filter功能：

 开启后地面滤波节点有了/points_no_ground的输出信息，有了输出信息本来以为已经解决了，但实际上这个去地面的滤波节点输出的点云信息地面根本没有滤掉！这部分在下个问题中解决。

三、小车无法前进，停在原地，局部路径全线变红（局部路径规划异常）

这个问题是紧接着上一个问题的，在改用ray_ground_Filter节点后，出现的问题。主要现象是，小车偶尔可以前行，当走到某种特定地形时，小车停止运动，局部路径规划全线飘红。另外邪门的现象是，每当有车辆或者电动车从实验小车旁边路过时，小车突然恢复运动，当车辆等移动物体通过后，小车停车。

这个排错过程打脸了，发现错误的途径是我之前说过那个没用不建议开启的彩色矩形块，就是rviz中在By display type下找到BoundingBoxArray类型并添加，话题设为/op_planner_tracked_boxes后便可以看到的那个代表障碍物的彩色矩形块。错误情况下rviz显示如下：

 

 可以看出来地面上有一个红色线构成的多边形将小车围了起来，同时被检测为障碍物，小车认为自身被障碍物包围所以停在原地。导致这种情况的原因是地面滤波节点没有将地面过滤掉，环形的扫描线被检测为障碍物，所以导致了这种情况发生，这也解释了小车边上有移动物体经过时小车恢复运动，因为移动物体打破了环形扫描线，使闭环变为开环，不再识别为障碍物，小车移动；移动物体经过后，扫描线重新闭环再次识别为障碍物所以停车。

上文两个点云滤波问题如果使用的是velodyne雷达，是不会遇到的。我翻出以前使用velodyne雷达录制的rosbag进行测试发现确实工作的好好的。自从更换为rslidar后出现的问题，那多半就是雷达信息转换节点出现的问题。需要修改我在​​​​​​​Autoware实车测试记录（二）-- 实车NDT定位_站住前面的二哈的博客-CSDN博客这篇博客中使用的那个雷达转换功能包的源码。

在这里首先介绍下velodyne雷达的信息格式，默认情况下velodyne输出的点云信息包含XYZIRT这些信息，而我使用的rs雷达默认发送的XYZI信息格式。

由于雷达发送的何种信息格式无法通过rostopic info查看，无论什么类型都是sense::PointsCloud2格式。在这里介绍一种查看雷达输出信息的方法：

首先使录制一段雷达输出话题的rosbag包，使用命令：

//rosrun pcl_ros bag_to_pcd 录制的包名.bag /雷达话题 ./保存文件夹
//例：
rosrun pcl_ros bag_to_pcd rslidar.bag /rslidar_points ./pcd

在执行时终端会输出这六种信息中包含的数据：x、y、z、intensity、ring、timestamp

同时使用命令pcl_viewer XX.pcd查看通过上述命令产生的pcd文件也可以看到包含的信息。

其次是雷达信息转换节点，README中介绍的比较详细，XYZIRT可以转化为XYZI、XYZIR、XYZIRT中任何一种。但是XYZI只能转化为XYZIR格式。

所以首先要将雷达的输出格式从XYZI改为XYZIRT。我使用的rslidar驱动版本为v1.5.9.该版本的驱动可以通过修改rslidar_sdk-vi.5.9功能包下的Cmakelists文件进行修改：

cmake_minimum_required(VERSION 3.5)
cmake_policy(SET CMP0048 NEW)
project(rslidar_sdk)

#=======================================
# Custom Point Type (XYZI, XYZIRT)
#=======================================
set(POINT_TYPE XYZI)

#=======================================
# Compile setup (ORIGINAL, CATKIN, COLCON)
#=======================================
set(COMPILE_METHOD CATKIN)

option(ENABLE_TRANSFORM "Enable transform functions" OFF)
if(${ENABLE_TRANSFORM})
  add_definitions("-DENABLE_TRANSFORM")

修改第八行，将XYZI修改为XYZIRT，然后重新编译一遍，雷达就改为了输出XYZIRT格式。

回到雷达转换节点，根据内容输入命令：

rosrun rs_to_velodyne rs_to_velodyne XYZIRT XYZIRT

会发生报错：

Failed to find match for field 'intensity',
Failed to find match for field 'intensity',
Failed to find match for field 'intensity',
Failed to find match for field 'intensity',
Failed to find match for field 'intensity',
Failed to find match for field 'intensity',
Failed to find match for field 'intensity',
Failed to find match for field 'intensity',

实际测试发现使用该报错的功能包除了报错信息所指示的强度信息丢失外，其他功能均正常工作，ring地面滤波也可以正常工作了。

强度信息之所以报错是因为在转换源码中对于intensity的数据类型不同导致的无法读取，修改也不麻烦：

struct RsPointXYZIRT {
    PCL_ADD_POINT4D;
    //uint8_t intensity;
    PCL_ADD_INTENSITY;
    uint16_t ring = 0;
    double timestamp = 0;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(RsPointXYZIRT,
                                  (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)
                                          (uint16_t, ring, ring)(double, timestamp, timestamp))

按上面代码，将uint8_t intensity;修改为PCL_ADD_INTENSITY;。然后将下方(uint8_t, intensity, intensity)改为(float, intensity, intensity)。修改好后重新编译一下就可以了。

修改好后，启动ring_ground_filter地面滤波节点，最终可以正常运行了：