Autoware 1.12学习整理--06--open_planner路径规划与避障

〇、前言

本文将在gazebo仿真环境中使用open_planner完成局部路径规划与避障，open_planner需要输入矢量地图(vector map)，可以使用unity map toolbox绘制矢量地图，本文使用的矢量地图和点云地图已上传到百度网盘，链接如下，其中矢量地图是由北京石油化工学院的常彦文同学绘制的。

链接: https://pan.baidu.com/s/17Oei64SxL9h79OfzTJDZjg
提取码: y5zw

一、运行open_planner前的准备工作

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

roslaunch vehicle_sim_launcher world_test.launch

运行命令后会启动gazebo和rviz，在rviz中依次点击file->open config加载配置文件：

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

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

roslaunch runtime_manager runtime_manager.launch 

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

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、加载矢量地图
点击Vector Map右侧的Ref按钮，选择绘制的矢量地图文件中的所有文件进行加载，最后Vector Map按钮。


在rviz左下角点击Add按钮，弹出的显示框中点击By topic按钮，找到话题/vector_map的MakerArray并添加，则rviz中出现了矢量地图。


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

1.6、点云预处理
在Sensing页面中，点击Points Preprocessor下ring_ground_filter右侧的app按钮，在弹出的界面中设置点云话题名input_point_topic为/points_raw，Sensor Model选32，参数含义在上篇博客中有作说明。设置完参数后点击OK退出，最后勾选ring_ground_filter。


1.7、完成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.8、启动vel_pose_connect
在autoware的Computing页面下，点击autoware_connector下vel_pose_connect右侧的app按钮，在弹出的界面中确定没有勾选Simulation_Mode，退出并勾选vel_pose_connect。


1.9、开启欧几里得聚类
在autoware的Computing页面下，点击lidar_detector下lidar_euclidean_cluster_detect右侧的app按钮，在弹出的界面中，设置output_frame为map，input_points_node为/point_no_ground，clip_min_height和clip_max_height表示检测障碍物的高度范围，然后点击OK退出后勾选lidar_euclidean_cluster_detect，等待几秒钟后rviz中出现障碍物点云簇 /points_cluster。



1.10、跟踪障碍物
多线激光雷达数据从传感器出来后，经过传统的聚类+高精地图object filter，或者learning-based的方法之后，得到一个目标列表，此目标列表含有二维位置信息和角度信息，没有速度信息，同时基于每一帧的检测方法，可能会出现“目标消失”的问题，因此需要滤波器来跟踪这些目标。

lidar_kf_contour_track用于跟踪障碍物，在autoware1.12版本中需要将其订阅的话题名修改为lidar_euclidean_cluster_detect发布的/detection/lidar_detector/cloud_clusters，然后重新编译一下，可以使用以下指令单独编译此功能包：

AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select lidar_kf_contour_track

在autoware的Computing页面中，点击lidar_tracker下lidar_kf_contour_track右侧的app按钮，在弹出的界面中，参数Vector Map Filter Distance设置为0，点击OK退出后勾选lidar_kf_contour_track。


在rviz左下角点击Add按钮，弹出的显示框中点击By display type按钮，找到BoundingBoxArray类型并添加，话题设置为/op_planner_tracked_boxes，此时rviz中出现了彩色矩形块。

二、运行open_planner

2.1、全局路径规划
在autoware的Computing页面中，点击OpenPlanner-Global Planning下op_global_planner后面的app按钮，勾选 Replanning、Smooth和Rviz Goals选项， Replanning用于到达目标点后，可以重新规划到下一个目标点，Smooth用于平滑全局路径，Rviz Goals用于在rviz中设置导航目标点，点击OK退出后勾选op_global_planner。


在rviz左下角点击Add按钮，弹出的显示框中点击By topic按钮，找到话题/vector_map_center_lines_rviz的MakerArray并添加，则rviz中汽车正前方出现了一条中线。


在rviz中点击2D Nav Goal按钮，在出现的中线远离汽车一端上设置导航目标点（目标点尽量靠近中线），然后就可以看到规划出来的全局路径：

2.2、局部路径规划
在autoware的Computing页面中，点击OpenPlanner-Local planning下op_common_params右侧的app按钮，弹出如下界面，参数Plan Ditance是局部路径长度，Path Density是局部路径上两个路径点的间距，Horizontal Density是相邻两个局部路径的间距，Rollouts Number是局部路径的条数，Max Velocity是路径上的执行速度，避障时速度减半，Follow DIstance为沿着路径从多远开始检测障碍物，Avoid Distance为沿着路径从多远开始避障，Lateral Safety和Longitudinal Safty是汽车安全框尺寸，设置完参数点击OK退出，勾选op_common_params。

点击OpenPlanner-Local planning下op_trajectory_generator右侧的app按钮，弹出如下界面，参数Tip Margin是车辆中心到横向采样点的距离，其长度决定了在两条轨迹之间切换时转向的平稳性，Roll In Margin是指从Car tip到外部极限的平行横向采样点的距离，这部分的长度和速度成正比，设置完参数点击OK退出，勾选op_trajectory_generator。


接下来点击op_motion_predictor和op_trajectory_evaluator的app按钮查看参数，如下所示：


最后勾选op_motion_predictor、op_trajectory_evaluator和op_behavior_selector。

在rviz左下角点击Add按钮，弹出的显示框中点击By topic按钮，找到话题/local_trajectories_eval_rviz的MakerArray并添加，则rviz中出现了多条局部路径。


2.3、路径跟踪
在autoware的Computing页面中，Motion Planning下可以看到waypoint_follower，点击pure_puesuit右侧的app按钮，弹出如下界面，路点跟随的速度选择为Waypoint，点击OK按钮后勾选pure_puesuit。

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


此时汽车开始移动，如下所示：

整个运行过程如下：

放入障碍物，汽车避障过程如下：

运行过程中的节点图如下，通过节点图能够看出节点间连接关系和程序运行流程：

文章不妥之处还望指正

参考文章：
https://blog.csdn.net/YP233/article/details/116708489
https://www.cnblogs.com/hgl0417/p/12522266.html
https://blog.csdn.net/weixin_42396991/article/details/117418973