Autoware 1.12学习整理--05--航迹点循迹导航

〇、引言

本文将使用《Autoware 1.12学习整理–04–录制航迹点》文章中得到的航迹点进行循迹导航，也就是控制汽车尽可能的沿着航迹点路线进行行驶。本文首先使用bag数据完成循迹，然后使用gazebo仿真环境中的实时激光雷达数据完成仿真汽车的循迹。

一、bag数据循迹

1.1、加载bag文件
启动autoware，切换到Simulation页面，点击Ref按钮加载bag文件，Start Time设置为0，如果之前已经加载过bag文件再次运行时则不用重复加载，直接点击Play然后点击Pause暂停播放，如下图所示。为了保证autoware的正常运行，在启动autoware后首先要做的就是加载bag文件，否则会出现时间戳同步问题。

1.2、完成从base_link到velodyne的坐标系转换
切换到autoware的Setup页面下，确定Localizer选的是Velodyne，设置Baselink to Localizer的x、y、z、yaw、pitch、roll数值，其为雷达中心点与车身后轴中心点的相对位置关系，接着点击TF按钮，最后点击Vehicle Model按钮，如果其后为空，那么会加载一个默认模型，也可以加载其他模型，上篇博客已提到。

1.3、完成从world到map的坐标系转换并加载点云地图
切换到autoware的Map页面下，点击TF右侧的Ref按钮，加载如下路径的launch文件，其中是从/world坐标系转换到/map坐标系的tf变换，最后点击TF按钮，autoware能够记录上一次运行程序时的路径，所以只点击TF按钮即可。

autoware.ai/src/autoware/documentation/autoware_quickstart_examples/launch/tf_local.launch

点击Point Cloud右侧的Ref按钮，加载pcd点云地图，并点击Point Cloud按钮，进度条显示OK，则加载完毕，如下所示：

1.4、点云降采样
切换到autoware的Sensing页面下，点击Points Downsampler下voxel_grid_filter右侧的app按钮，在弹出的界面中设置点云话题名Points topic为/points_raw，下面两个参数默认即可，最后勾选voxel_grid_filter。voxel_grid_filter滤波原理和参数意义在此篇博客有作说明。

1.5、点云预处理
在Sensing页面中，Points Preprocessor下第一个是ring_ground_filter，其作用是对激光雷达数据进行滤波，能够区分地面与非地面雷达数据，点击ring_ground_filter右侧的app按钮，在弹出的界面中设置点云话题名input_point_topic为/points_raw，Sensor Model是激光雷达扫描线数，本文用的是32线雷达，所以选32，Sensor Height参数是激光雷达距地面高度，Max Slope参数是最大坡度，大于此值将被识别为非地面数据，Vertical Thres表示障碍物和地面的差异度，如果大于此值则被认为是障碍物。设置完参数后点击OK退出，最后勾选ring_ground_filter。


启动ring_ground_filter滤波前后对比如下，滤波后去掉了扫到地面上的激光点云。

1.6、完成map到base_link的坐标系转换
切换到autoware的Computing页面下，点击lidar_localizer下ndt_matching右侧的app按钮，在弹出的页面中topic:/config/ndt选择Initial Pos选项，x，y，z，roll，pitch，yaw的值表示激光数据的初始位姿，如果有GNSS设备可以选择GNSS进行初始定位。Method Type选择pcl_anh_gpu，即使用GPU进行点云匹配计算，然后点击OK，最后勾选ndt_matching，完成ndt定位。

1.7、启动vel_pose_connect
在autoware的Computing页面下，点击autoware_connector下vel_pose_connect右侧的app按钮，在弹出的界面中确定没有勾选Simulation_Mode，退出并勾选vel_pose_connect。

1.8、勾选lane_planner相关选项
在autoware的Computing页面右侧，Mission Planning下可以看到lane_planner，点击lane_rule右侧app按钮，弹出如下界面，界面中参数默认即可，Acceleration参数表示在停止线启动或停止的加速度，Stopline Search Radius参数表示搜索以停止线为圆心，此参数为半径圆形区域内的航迹点，参数Number of zeros ahead和Number of zeros behind表示停止线前后的航迹点数目，用于修改停止线前后航迹点的速度或加速度，number_of_smoothing_count用于平滑航迹点路径。最后勾选lane_ruler。

点击lane_stop右侧app按钮，弹出如下界面，界面中参数默认即可，lane_stop用于识别红绿灯，根据红绿灯状态将停止线的航迹点变为红色或绿色，Traffic Light下的两个按钮用于调整当前路径处在红灯还是绿灯状态，Lane Change下的两个按钮用于变换车道，Current Lane下的Keep at表示在当前车道线上保持的速度，Stop in表示停止时距离停止线的最大距离，退出界面勾选lane_stop。

点击lane_select右侧app按钮，弹出如下界面，界面中参数默认即可，Distance threshold to neighbor lanes参数表示检测当前路径周围有效通道的阈值， 超过此阈值的车道将不会被识别为车道，Lane Change Interval After Lane merge参数表示在已变换车道中行驶多少米后可以再次变换车道，Lane Change Target minimum(m)参数表示到变换车道中目标点的最小距离，Vector Length of Hermite Curve(m)参数表示Hermite曲线上航迹点的向量长度，退出界面勾选lane_select。


1.9、勾选waypoint_planner相关选项
在autoware的Computing页面右侧，Motion Planning下可以看到waypoint_planner，分别点击astar_avold和velocity_set右侧的app按钮，弹出如下界面，参数默认即可。


勾选astar_avold和velocity_set

1.10、加载航迹点文件
在autoware的Computing页面右侧，点击waypoint_maker下waypoint_loader右侧的app按钮，在弹出的界面中点击Ref按钮加载上篇博客录制的航迹点文件，点击OK按钮后勾选waypoint_loader。


1.11、勾选waypoint_follower相关选项
在autoware的Computing页面右侧，Motion Planning下可以看到waypoint_follower，点击pure_puesuit右侧的app按钮，弹出如下界面，路点跟随的速度选择为Waypoint时就是航迹点文件中记录的速度，选择Dialog时，可以通过下面的参数手动设置汽车速度。点击OK按钮后勾选pure_puesuit。

点击twist_filter右侧的app按钮，弹出如下界面，对速度进行滤波，参数默认即可，点击OK按钮后勾选twist_filter。


1.12、航迹点循迹
在autoware界面中点击RViz按钮打开rviz，依次点击file->open config加载配置文件：

autoware.ai/src/autoware/documentation/autoware_quickstart_examples/launch/rosbag_demo/default.rviz

切回到autoware的Simulation界面，点击Pause按钮开始播放数据，Rviz界面显示如下：

整个运行过程如下：

运行过程中的节点图如下：

从节点图中可以看到，输入节点有points_map_loader，waypoint_loader和play_164...622节点，分别用于加载点云地图、航迹点和bag点云数据，输出话题是/twist_cmd速度话题。若将bag点云数据换成gazebo中实时仿真的点云数据，仿真汽车订阅/twist_cmd速度话题，则可实现仿真汽车的循迹导航。

二、gazebo仿真环境中的循迹

2.1、输入以下指令启动包含汽车模型的gazebo仿真环境，《Autoware 1.12学习整理–02–构建点云地图》文章中介绍了仿真环境的配置。

roslaunch vehicle_sim_launcher world_test.launch

运行命令后会启动gazebo和rviz，在rviz中加载1.12小节中的default.rviz配置文件，如下图所示：

2.2、输入以下指令启动启动autoware

roslaunch runtime_manager runtime_manager.launch 

2.3、与bag数据循迹的不同
打开autoware界面后，不用在Simulation页面加载bag文件，也不用点击Setup页面的TF按钮和Vehicle Model按钮，因为启动的gazebo完成了base_link到velodyne的坐标系转换，并加载了汽车模型。

2.4、后续操作步骤与bag数据循迹的相同
同1.3小节，完成从world到map的坐标系转换并加载点云地图
同1.4小节，点云降采样
同1.5小节，点云预处理
同1.6小节，完成map到base_link的坐标系转换
同1.7小节，启动vel_pose_connect
同1.8小节，勾选lane_planner相关选项
同1.9小节，勾选waypoint_planner相关选项
同1.10小节，加载航迹点文件
同1.11小节，勾选waypoint_follower相关选项

2.5、gazebo仿真汽车循迹
在勾选pure_puesuit和twist_filter后，仿真汽车开始循迹导航，如下图所示：

整个运行过程如下：

在汽车前放置障碍物，当检测到时rviz中能够显示出红色屏障：

运行过程中的节点图如下：

通过节点图可以看出各节点之间的连接关系，当程序不能正常运行时可以对比一下节点图，找到问题所在，我在运行程序时遇到过以下问题：
①不能通过autoware界面启动ndt_matching，可以使用命令行启动：

roslaunch lidar_localizer ndt_matching.launch

②在航迹点路线上没有出现绿色可通行区域，汽车也不前进，后来发现是没有启动astar_avoid，可以使用命令行启动：

roslaunch waypoint_planner astar_avoid.launch

文章不妥之处还望指正

参考文章：
https://www.cnblogs.com/hgl0417/p/11147109.html
https://blog.csdn.net/Travis_X/article/details/105455195