Autoware实车测试记录（三）--障碍物检测及追踪节点的启用以及测试过程遇到的问题

在正式内容开始前补充一点，前文说过由于各种原因导致每次测试前需要启动多个launch文件，细数一下包括底盘、雷达、雷达格式转换、autoware主程序、控制指令转换足足五个，而且由于分属不同工作空间下还没有办法写入一个launch内。启动起来确实麻烦所以采用了一个脚本文件一次性启动所有launch文件，在这里分享出来给有相同问题困扰的提供一个思路。

我使用的脚本文件代码粘贴在下面：

#!/bin/bash

#底盘
if [ ! -e "/dev/autolabor_pro1" ]; then
  zenity --error --text="底层连接失败，请检查机器人是否连接至上位机，并确保上位>机控制开关打开以及急停按钮未按下。"
  exit 0
fi

source /opt/autolabor/ros/catkin_ws/devel[表情]tup.bash
IP=`ifconfig wlan0 | grep "inet 地址:" | awk '{print $2}' | cut -c 8-`

if [ -z "$IP" ]; then
  IP="127.0.0.1"
fi

export ROS_MASTER_URI=http://$IP:11311/
export ROS_IP=$IP


#启动底盘节点（原启动文件）
#roslaunch autolabor_test_launch car_test.launch

#启动底盘节点（不启动rviz）

#gnome-terminal -t "car" -x bash -c "roslaunch autolabor_test_launch #car_test.launch;exec bash;"
#sleep 5s

#启动底盘节点（不启动键盘控制节点）
gnome-terminal -t "car" -x bash -c "roslaunch autolabor_test_launch WithoutKeyboard.launch;exec bash;"
sleep 5s


#启动激光雷达
gnome-terminal -t "lidar" -x bash -c "roslaunch rslidar_sdk start.launch;exec bash;"
#roslaunch velodyne_pointcloud VLP16_points.launch
sleep 5s
#转换节点
gnome-terminal -t "turn" -x bash -c "rosrun rs_to_velodyne rs_to_velodyne XYZI XYZIR;exec bash;"
sleep 5s

#启动imu
#gnome-terminal -t "imu" -x bash -c "roslaunch fdilink_ahrs ahrs_data.launch;exec bash;"
#sleep 5s

#启动autoware
roslaunch runtime_manager runtime_manager.launch

#启动速度转换节点
#rosrun vel_transform twist_transform

上面的代码从第20行开始看起就可以，前面的代码是和小车底盘启动相关的代码。

通过脚本文件可以通过一个窗口的多个选项卡中分别启动这些launch文件，同时可以通过注释不同的代码来更改启动的文件。总之就是非常好使。这个文件的启动方式是用命令启动：

//脚本是没有后缀的，双击进编辑模式，需要使用下面的命令启动
//这个脚本是可以转化为可执行文件双击打开的，但是不知道具体操作方法
source 脚本文件名

-----------------------------------------------------------------------------------------------

好题外话暂时讲到这里，接上篇文章定位成功的部分，上文完成了定位的工作，主要是通过启用ndt匹配节点完成的。

定位完成后接下来的工作主要围绕路径规划以及避障，要实现这两部分，需要检测障碍物以及追踪他们，这项工作主要由lidar_detector下的lidar_euclidean_cluster_detect以及lidar_tracker下的lidar_kf_contour_track两个功能完成。

 先不要急着加载这两个功能块，lidar_kf_contour_track的功能是追踪lidar_euclidean_cluster_detect聚类后的结果，所以他需要订阅聚类后的结果，但是追踪功能订阅的话题是/cloud_clusters，这个话题没有被前一个功能块发布，聚类结果实际上发布的节点话题名为/detection/lidar_detector/cloud_clusters。所以这里需要修改一下源码，找到lidar_kf_contour_track_core.cpp文件，修改订阅的话题，然后重新编译一下就可以了。

​​​​​​​ 

//可以使用这行命令单独编译这个修改过的功能包
AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select lidar_kf_contour_track

修改编译完成之后，在Autoware中，打开lidar_euclidean_cluster_detect的设置，将app参数中的output_frame由velodyne改为map，然后将lidar_kf_contour_track中功能中的Vector Map Filter参数设置为0。

正常情况下，在ndt定位成功后启动这两个功能，等待一小会就会在rviz上面实时显示出聚类结果以及追踪的结果信息（我这个图聚类结果不是很清楚，但是可以隐约看见粉色色块）：

​​​​​​​

如果想查看障碍物所组成的矩形块，可以在rviz左下角点击Add按钮，在By display type下找到BoundingBoxArray类型并添加，话题设置为/op_planner_tracked_boxes，便可以看到代表障碍物的彩色矩形块。但实际不推荐可视化这个矩形块，因为矩形块显示在rviz中总是与障碍物垂直，观感非常不好而且挡视线，建议不要添加这个。

然而事情通常不按套路出牌，我实测无法出现这样的“正常情况”。在实测中的这一阶段我遇到了两个报错，一个为tf的报错，另一个为rviz的报错。第一个报错导致程序不能正常工作，第二个报错导致rviz闪退。下面介绍一下两个错误出现的可能原因以及如何解决的。

第一个错误：tf报错：

[ERROR] [1678243435.42366616]: publishCloudClusters:Could not find a connection between 'map' and 'velodyne' because they are not part of the same tree Tf has two or more unconnected trees.
[ERROR] [1678243435.42501118]: publishCloudClusters:Could not find a connection between 'map' and 'velodyne' because they are not part of the same tree Tf has two or more unconnected trees.
[ERROR] [1678243435.42533050]: publishCloudClusters:Could not find a connection between 'map' and 'velodyne' because they are not part of the same tree Tf has two or more unconnected trees.

之所以出现这样的报错是因为tf树出现了问题，autoware前期跑给定的数据集，tf树如下，这里车体应该是使用的base_link，由于是播放的rosbag，所以有了gps，实车测试是没有这个的。

 但是由于实车，车自身启动的节点是将odom（车坐标系）与base_link绑定，在不启动ndt节点之前的tf树是这样的：

 启动ndt节点后，map指向base_link，导致tf冲突，导致最后的tf树变为这样：

 解决办法是修改了底盘的tf发布。本来单独启动底盘后的tf为odom->base_link。修改一下顺序，改为base_link->odom，这样最终的tf树会变为一个，这样消除掉tf的报错。感觉上是在tf树内可以一指多，但是不能多指一，对tf没有花时间细致学过如有错误欢迎指正。

第二个错误：rviz的md5校验码错误：

[ERROR] [1678436716.978564889]:Client[/rviz_1678436714543865795]wants topic /local_trajectories to have datatype/md5sum [visualization_msgs/MarkerArray/d155b188fbaf89745847fd5882d7].but our version has [autoware_msgs/LaneArrary/23abbdfa4ee58530bb628c974c2a].Dropping connection.
[ERROR] [1678436716.978526579]:Client[/rviz_1678436714543865795]wants topic /behavior_state to have datatype/md5sum [visualization_msgs/Marker/4048c9de2a16f40538085ebf1b97].but our version has [visualization_msgs/MarkerArray/d155b188fbaf89745847fd5882d7].Dropping connection.

这个问题可能解决起来比较麻烦，这是一种在校验过程中遇到的问题，在网上搜索有按照命令行提示的报错信息修改md5校验码的方法，但是我的报错信息显示的貌似不仅仅只有一种校验码错误，甚至消息格式都不统一。很让人头大，出现这种情况最有可能是出现了程序移植这一操作。

按照网上正常的操作流程安装测试通过的autoware程序可能不会出现这样的问题。但是问题在于autoware安装比较繁琐而且非常容易报错，当时在安装时由于网络原因迟迟无法安装成功。索性将之前安装在其他设备上的autoware拷贝过来，本想着在这里再编译安装一遍，但没曾想编译没通过不过反倒是可以直接运行起来的。当时我就觉得给自己挖坑了，测试到这才掉进去。

虽然解决这个问题比较麻烦，但也不是完全躺在这了。我发现只有按正常顺序启动时才会出现rviz的闪退，同时出现上面的报错信息。也就是启动ndt->启动聚类->启动追踪这个顺序。每当启动lidar_kf_contour_track也就是追踪部分时就会发生闪退，重启rviz也是一样的，能看见画面而且显示正常但是rviz马上中断了，很可惜后面需要在rviz里面给目标点所以还是需要解决这个问题的。

最终尝试出了一种方法，可以绕开（不是解决）这个问题。这个方法比较玄学我也搞不懂他是怎么成功的但他就是可以正常工作了（有懂的大佬可以在线指点）。

方法是在前面的步骤都完成后，到了启动ndt匹配部分的时候，先不启动他，先启动lidar_euclidean_cluster_detect以及lidar_kf_contour_track这两个部分。等待一段时间后rviz显示计算结果，同时如果autoware后台终端被必要话题未发布之类的红字报错信息刷屏，之后，启动ndt定位，rviz就绕过了这个报错，三个功能和rviz都正常工作。