Autoware部署二：录制航迹点并进行航迹点导航

定位建图最终都是为导航服务的，当把地图建立出来并且机器人知道自己在地图的什么位置，这样才能更好更有针对性的规划一条较优的轨迹到达目标点，才不会盲目的搜索路径，影响规划效率。

1、录制航迹点

1.1 启动autoware和开启gazebo仿真模拟

roslaunch runtime_manager runtime_manager.launch
roslaunch vehicle_gazebo_simulation_launcher world_test.launch

注：记得要先开autoware再开gazebo，主要是为了统一时间戳，否则时间戳会出错，导致tf变换报错，无法进行后续工作。

1.2 定位

定位还是使用ndt-matching定位，与前一章一样。再写一遍，比较好理解。

1.2.1 base_link 到 velodyne坐标变换

选择setup页面，驱动Localizer选择的是velodyne，设置tf，tf是激光雷达相对于车体中心的坐标变换。这一步其实在打开gazebo的时候就已经自动生成了，其实可以不需要。但是如果是跑simulation的话这一步是需要的。

1.2.2. 加载地图+world到map坐标系变换

切换到Map页面，point cloud是加载建图生成的pcd文件，tf则是做world到map的坐标变换，vector map是加载矢量地图。后续会用到。

1.2.3.进行原始点云的滤波处理

Sensing最终配置如下图：

Voxel_grid_filter是体素滤波器，将一定范围内的点近似的看成是一个点，减少数据量，效率更高。Voxel Leaf Size参数设置为0.5，代表0.5米立方体内的电云都用一个重心点近似代替。Measurement Range参数为200，表示点云的有效距离为200米。

Ring ground filter作用是进行地面点的滤波，得到去除地面点的数据，发布的话题为/points_no_ground。Sensor Height为激光雷达距离地面的安装高度，Max Slope表示小于10°的坡会被认为是地面点，也会被滤除。Vertical Thres表示障碍物和地面的差异度，如果大于此值则被认为是障碍物。

1.2.4.启动ndt-matching定位

切换到computing页面，最终配置如下图：

点击lidar_localizer下ndt-matching进行配置，Initial Pos表示激光雷达初始位姿与建图时的初始位姿变换，如果有gnss数据可以使用gnss进行初始定位。其他默认。

启动vel_pose_connect，确定为勾选Simulation Mode

1.3.录制轨迹点

整个录制轨迹点的最终配置图：

切换到computing页面下，点击waypoint_maker下的waypoint_saver，里面可以配置保存的路径和名称。其中Save/current_velocity Interval表示采样的间隔，默认是1米的间隔。记得waypoint_saver重复勾选跟取消，因为他勾选是写入文件的动作，不是重新覆盖文件，重复勾选会导致得到的csv文件有重复的想，x,y,z,yaw等标识信息。会导致之后的解析csv文件有误。

1.4. 最终录制轨迹点显示

Rviz中选择左下角的Add按钮，选择By topic按钮，找到/waypoint_saver_marker话题。订阅此话题，rviz就能限制刚刚录制的轨迹点，可以看到每个点所保存的速度信息。

2.寻迹导航

根据上一节所录制的轨迹点进行导航。实现轨迹点跟踪导航。

2.1. 启动autoware和开启gazebo仿真模拟

roslaunch runtime_manager runtime_manager.launch
roslaunch vehicle_gazebo_simulation_launcher world_test.launch

注：记得要先开autoware再开gazebo，主要是为了统一时间戳，否则时间戳会出错，导致tf变换报错，无法进行后续工作。

2.2. 定位

world坐标系到map坐标系是由Map选项里的TF发布的，base_link到velodyne坐标系是由gazebo仿真发布的，map到base_link坐标系则是由ndt-matching发布的。

2.2.1.base_link 到 velodyne坐标变换

选择setup页面，驱动Localizer选择的是velodyne，设置tf，tf是激光雷达相对于车体中心的坐标变换。这一步其实在打开gazebo的时候就已经自动生成了，其实可以不需要。但是如果是跑simulation的话这一步是需要的。

2.2.2.加载地图+world到map坐标系变换

切换到Map页面，point cloud是加载建图生成的pcd文件，tf则是做world到map的坐标变换，vector map是加载矢量地图。后续会用到。

2.2.3.进行原始点云的滤波处理

Sensing最终配置如下图：

Voxel_grid_filter是体素滤波器，将一定范围内的点近似的看成是一个点，减少数据量，效率更高。Voxel Leaf Size参数设置为0.5，代表0.5米立方体内的电云都用一个重心点近似代替。Measurement Range参数为200，表示点云的有效距离为200米。

Ring ground filter作用是进行地面点的滤波，得到去除地面点的数据，发布的话题为/points_no_ground。Sensor Height为激光雷达距离地面的安装高度，Max Slope表示小于10°的坡会被认为是地面点，也会被滤除。Vertical Thres表示障碍物和地面的差异度，如果大于此值则被认为是障碍物。

2.2.4.启动ndt-matching定位

切换到computing页面，最终配置如下图：

点击lidar_localizer下ndt-matching进行配置，Initial Pos表示激光雷达初始位姿与建图时的初始位姿变换，如果有gnss数据可以使用gnss进行初始定位。其他默认。

启动vel_pose_connect，确定为勾选Simulation Mode

2.3. 加载轨迹点信息

选择刚刚保存的轨迹点路径，并勾选waypoint_loader选项。

2.4. 配置lane_planner相关信息

最终配置如下图所示：

2.4.1.Lane_planner模块

Lane_rule，lane_stop，lane_select都是跟车道线相关的配置。Lane_rule中Acceleration设置的是在停止线出现时启动跟停止时候的加速度，stopline search radius表示的是搜索以停止线为中心半径一米的导航点。Number of zeros ahead与number of zeros behind表示停止线前后的航迹点数目，用于修改停止线前后航迹点的速度或加速度。number_of_smoothing_count用于平滑航迹点路径的数目。

lane_stop是用于识别红绿灯的，根据红绿灯状态将停止线的航迹点变为红色或绿色，Traffic Light下的两个按钮用于调整当前路径处在红灯还是绿灯状态，Lane Change下的两个按钮用于变换车道，Current Lane下的Keep at表示在当前车道线上保持的速度，Stop in表示停止时距离停止线的最大距离。暂时还没用上

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

感觉上面上个简单的轨迹点跟踪是都用不上的，但是还是得勾选才能跑跟踪算法。所以都采用的是默认配置。

2.4.2.Waypoint_planner模块

此模块设置的是轨迹跟踪时候，遇到障碍物时的处理参数，是进行局部路径规划的参数，其中Astar_avoid_param中国enable avoidence如果打钩则表示在寻迹过程中遇到障碍物要避障。此时需要配合的打开semantics中costmap_generator，此按钮会发布话题semantics/costmap_generator/occupancy_grid，此话题会被astar_avoid接受并用于避障。若不打开避障按钮，则实现的是遇到障碍物停障，记得要实现停障功能，pure_pursuit一定要选择waypoint模式，否则车体不会动。其他参数就是字面上的意思。

Velocity_set参数内，首先是decelerate obstacle，从车辆位置开始，向后遍历一定数量的路径点(蓝色点)，对每个路径点，遍历所有的点云，如果该路径点decelerate_range半径内不存在点云，则切换到下一个路径点，再次遍历所有点云。当没有检测到decelerate obstacle但是突然在某一路径点的stop_range范围内检测到了点云，则将障碍物判定为stop_obstacle，需要停车处理。这种障碍物检测模式，如果是静止的障碍物，一定会被先检测为decelerate obstacle，优先减速处理。对于突然出现的障碍物才会被检测为stop_obstacle。

参数解释：
Stop distance for obstacle与detection range都表示停障距离。当二者有一个满足都会使得车辆停障。如下图。Detection range和deceleration range都是以每个路径点为圆形扩散半径，减速参数表示的是黄色阴影部分。红色粗线代表当激光雷达到达此位置也会触发停障Stop distance for obstacle代表的是激光距离障碍物多远的距离进行停止。二者满足其一就会触发停止。

2.4.3.勾选waypoint_follower

Pure_pursuit中有两种纯跟踪模式，一种还是waypoint，一种是dialog。如果是想实现录制点跟踪，那么就要选择waypoint模式。如果是要实现避障功能，那么可以选择dialog模式，选择Dialog时，可以通过下面的参数手动设置汽车速度。

Twist_filter选择默认设置即可。