上一次，我们使用lio-sam、ndt-mapping分别尝试构建小区和公园的地图，今天我们继续下一步，地图来进行定位和导航的验证。

一、地图定位

1. 启动底盘+传感器+Autoware

# 1. 启动底盘+传感器
roslaunch turn_on_wheeltec_robot open_autoware.launch

# 检查topic帧率
rostopic hz /imu_raw /points_raw /vehicle/odom /gps/fix
# 检查topic时间戳是否正常
rostopic echo /imu_raw /gps/fix -p

# 2. 启动Autoware
cd ~/autoware.ai.x86/
./open.sh
source ~/Autoware/install/setup.bash
roslaunch runtime_manager runtime_manager.launch

2. 加载从base_link到Lidar坐标系的TF、小车模型

点击进入【Setup】菜单，分别设置Localizer、TF、Vehicle Model：

• 在【Localizer】中选择Velodyne。
• 在【Baselink to Localizer】中设置好各个参数，其中 x、y、z、yaw、pitch、roll表示真车雷达中心点与车身后轴中心点的 TF 位置关系，可根据雷达实际位置进行修改（对于镭神 16 线雷达z参数设置为0.38，yaw 参数设置成-1.57），设置好之后点击 【TF】 按钮。
• docker版本的urdf小车模型在 autoware_shared_dir/urdf 目录下面，选择后点击【Vehicle Model】加载模型（如果Vehicle Model为空，那么会加载一个默认模型，在 rviz 显示时，如果有激光雷达数据，车辆会显示为黑色）。

3. 设置从 world 到 map 转换

点击【Map】菜单，加载 world 到 map 的坐标转换：

• 点击【TF】右侧的【ref】按钮，选择~/autoware_shared_dir/lgsvl-tf.launch 文件后
• 点击【TF】按钮加载该文件，以加载 world 到 map 的坐标转换

4. 加载点云地图 PCD 文件

• 打开【map】页面，点击【point cloud】右侧的【ref】按钮，选择上一步建图保存的3D点云地图.pcd文件
• 点击【point cloud】按钮，此时下方会出现一个进度条，当进度条显示加载了 100％并出现[OK]字样时，证明 3D 点云地图已经成功加载完毕

5. 点云降采样过滤

打开【sensing】页面，找到【points_downsampler】下的【voxel_grid_filter】选项，设置[app]的一些参数后，勾选[voxel_grid_filter]。 [voxel_grid_filter]有一些参数设置如下图：

• Voxel Leaf size：体素降采样算法中的体素叶大小，室内可设置小点，室外可设置大点

6. 地面过滤

打开【sensing】页面，找到【points_preprocessor】下的【ring_ground_filter】选项，设置【app】的一些参数后，并勾选。【ring_ground_filter】有一些参数设置如下图：

• Sensor model：16、32、64 线雷达，这里如果使用镭神 16 线雷达勾选[16]
• Sensor height：雷达距离地面高度，我这里是0.438
• Max slope：地面最大坡度，单位°，如果不给出坡度，容易将坡识别为障碍物
• Vertical Thres：表示障碍物和地面的差异度，如果大于这个值则被认为是障碍物

7. 启动ndt_matching（从 map 到 base_link 的转换）

找到【Computing】左菜单栏下的【ndt_matching】选项，设置【app】里面的参数后，勾选【ndt_matching】，一些参数设置如下所示：

• 使用RTK时，【topic:/config/ndt】 勾选【GNSS】。没有使用 GNSS时，【topic:/config/ndt】勾选 【Initial_Pose】，x，y，z，roll，pitch，yaw 的值表示小车的初始位置（默认建图起点位置）。
• 在[method type]栏中，无 GPU 加速的选【pcl_generic】，有 GPU 加速的选[pcl_anh_gpu]。
• 用到 imu 辅助定位，需要把【useimu】选项勾选上，其余参数参考默认值就可以了。

在[Compulting]页面找到[ndt_matching_monitor]选项，勾选节点

ndt_matching参数在autoware中是通过runtime manager以/config/ndt话题的方式发布的，话题通过界面触发，每切换界面按钮、修改参数并确定就触发一次话题发布，来实时改变程序中的NDT配置参数，类似于动态参数配置，后续可改为ros参数在程序开始时加载。

rostopic echo /config/ndt

参数说明：

• init_pos_gnss：1通过GNSS来确定初值，0通过初始位姿确定初值；如果使用GNSS可参考GNSS+NDT联合定位
• use_predict_pose：1使用预测位姿，0不使用预测位姿；
• error_threshold: （1.0）该参数不知道干嘛的，好像没用；
• resolution: （1.0）网格大小设置；
• step_size: (0.1) 迭代步长；
• trans_epsilon: （0.01）算法收敛条件；
• max_iterations: （30）最大迭代次数；

8. 启动定位vel_pose_connect

• 找到【Computing】 左菜单栏下的 【vel_pose_connect】。
• 打开 【app】 并确保选项 [Simulation_Mode] 没有被勾选。
• 退出并勾选 【vel_pose_connect】。

9. 打开rviz

点击界面右下角的[rviz] 打开rviz，相关配置在~/autoware_shared_dir/default.rviz 。

若是小车不在建图起点位置，或者是定位不准确，可以点击 rviz 菜单栏中的【2D pose estimate】，然后在地图上鼠标左击拖动，重新设置小车的位置。

 

二、雷达目标识别

1. 完成地图定位

完成上一步地图定位步骤。

2. 欧几里得聚类

点云聚类是将三维点云数据进行分组或分类，将距离较近的点归为一类的过程。

• 障碍物检测与跟踪：点云聚类可以帮助识别和区分环境中的不同物体，特别是障碍物。通过聚类，可以将同一个障碍物的点归为一类，并估计其位置、形状和大小，从而实现对障碍物的检测和跟踪。
• 建立环境模型：通过点云聚类，可以将点云数据分割为不同的物体群组，并将这些群组的特征提取出来。这些特征可以用于建立环境模型，包括道路、建筑物、交通标志等，为自动驾驶车辆提供更准确和详细的环境信息。
• 动态物体检测：点云聚类可以帮助识别和区分稳定物体和动态物体。通过将点云数据进行聚类，可以发现移动的物体，并将其与稳定的环境进行区分。这对于自动驾驶来说至关重要，因为它可以帮助车辆预测和适应动态物体的行为。
• 传感器融合：自动驾驶系统通常会使用多种不同类型的传感器，如激光雷达、摄像头等。点云聚类可以将多个传感器获取的点云数据进行融合，提供更全面和一致的环境感知。

总的来说，点云聚类在自动驾驶感知层中的意义在于帮助将三维点云数据转化为更易处理的物体信息，并为障碍物检测、环境建模、动态物体检测和传感器融合等任务提供基础数据和准确性。它是实现自动驾驶环境感知与决策的关键步骤之一。

欧式聚类是一种基于欧氏距离度量的聚类算法。采用基于KD-Tree的近邻查询算法是加速欧式聚类。

打开[Computing]页面，找到[lidar_detector]下的[lidar_euclidean_cluster_detect]选项，设置[app]的一些参数后，并勾选。[lidar_euclidean_cluster_detect]有一些参数设置如下图：

• use_gpu：是否使用GPU，选择使用，否则聚类十分慢；
• output_frame：输出坐标系，改为map；
• pose_estimation：需要估计聚类点云最小面积边界矩形的姿态；
• downsample_cloud：点云通过VoxelGrid滤波器进行下采样；
• input_points_node：输入点云topic，这里使用过滤地面后的雷达话题/points_no_ground；
• leaf size：下采样体素网格大小；
• cluster size minimum：聚类的最少点数（调小一点能聚到更远的类）；
• cluster size maximum：聚类的最多点数；
• clustering distance：聚类公差（同一类两点最大距离阈值，调大一点能聚到更远的类，但是太大会造成明显的不同类聚到一起m）；
• clip_min_height：裁剪的最小高度；
• clip_max_height：裁剪的最大高度（高于雷达多少）；
• use_vector_map：是否使用矢量地图；
• vectormap_frame：矢量地图坐标系；
• remove_points_upto：距离激光雷达这个距离过近的点将被移除（最大只能2.5，因此车身过大不要用，需要输入去除地面点点云）；
• keep_only_lanes_points：行驶线边（远）滤波；
• keep_lane_left_distance：向左移除该距离以外的点 (m)（算力强可以设大点）；
• keep_lane_right_distance：向右移除该距离以外的点 (m)；
• cluster_merge_threshold：聚类簇间的距离阈值(m)；
• use_multiple_thres：是否使用多级阈值进行聚类（使用下面两个参数）；
• clustering_ranges：不同的距离有不同的聚类阈值 (m)；
• clustering_distances：聚类公差(同一类两个点的最大距离阈值，与clustering_ranges对应)；
• remove ground：地平面滤波（移除属于地面的点），推荐设置为0；
• use_diffnormals：采用正态差值滤波再去除一次地面点。

3. 跟踪障碍物

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

打开[Computing]页面，找到[lidar_tracker]下的[lidar_kf_contour_track]选项，设置[app]的一些参数后，勾选[lidar_kf_contour_track]。[lidar_kf_contour_track]有一些参数设置如下图：

• Vector map distance：需要修改为 0，其他参数默认即可。

我们可以通过 rviz 订阅/detection/lidar_detector/objects_markers 话题可以看到聚类及跟踪障碍物得到的结果，也可以通过图片中的步骤订阅/bounding_boxes话题使结果以方框的形式显示出来。

三、高精地图绘制

在开始导航之前，单靠点云地图已经不够了，我们必须先绘制矢量地图再继续后续的导航环节。

具体参考：使用Unity+MapToolBox绘制VectorMap地图

 

四、open_planner导航

OpenPlanner是名古屋大学开发的开源自动驾驶框架Autoware中使用的运动规划算法。它是通过对全局路径进行采样生成一系列的候选路径，结合矢量地图、传感器信息以及碰撞、交通规则等约束和优化目标，选择出最优的运动轨迹。OpenPlanner通用性强，只需相应的调整参数即可与任何的移动机器人配合使用。

图是OpenPlanner的一般体系结构，其主要三个组成部分是全局规划器、行为状态生成器和局部规划器。

OpenPlanner旨在用于移动机器人的自主导航，更确切地说是应用于自动驾驶领域。它结合了矢量地图或道路网络地图中包含的所有离散信息（例如交通信号灯，交通标志，十字路口，停车线等的位置），这相对于其他的开源的导航系统OMPL和Navigation Stack来说是主要的优势。 使用矢量地图可以更加轻松、快速地进行全局和局部规划。当然，像RRT * 和 Hybrid A *这些自由空间中全局规划器对于停车和越野情况也是很重要的。在这种情况下，我们可以切换到自由空间规划器进行全局规划，仍需要使用Open Planner的行为状态机和本地规划器。

OpenPlanner适合于遵守交通规则的自主移动机器人导航系统，它仅需要矢量地图和目标位置即可进行全局规划，而对于局部规划和行为状态生成，则仅需要当前位置和检测到的障碍即可。

1. 完成雷达目标识别

完成上一步雷达目标识别步骤。

2. 加载高精地图

高精地图地图具体制作方法参考第 3 章内容。

打开[map]页面，点击[vector map]的[ref]，选中全部的高精地图文件，点击[vector map]加载高精地图，此时可以在 rviz 中看到我们画的高精地图。

3. 全局路径规划

打开[Computing]页面，找到[openplanner_global planning]下的[op_global_planner]选项，设置[app]的一些参数后，并勾选。然后在 rviz 中点击[2Dnav goal]，选择你所要导航的目标点位置（小车起点位置和设定的目标点位置需要放在行驶线 lane 附近）rviz 中会出现一条蓝色路径即为全局路线。

• Replanning：用于到达目标点后，可以重新规划到下一个目标点。如果有了两个目标点会循环
• Smooth：用于平滑全局路径
• Path density：由每一个小点组合成全局路径，此参数为每一个点的间隔距离。

4. 局部路径规划

打开[Computing]页面，找到[openplanner_local planning]栏目，设置[app]的一些参数后，需要依次勾选[openplanner_local planning]栏目下的全部节点。可以在rviz 中点击[add]，添加[local trajectories_evel_rviz]话题， rviz 中会出现几条路径即为可行驶的几条局部路线，红色为不可行驶的路线，粉红色为当前选择行驶的局部路线，黄色为可行驶的路线。相关一些参数设置如下图：

局部路径规划勾选节点：

op_commom_params 节点相关参数：

• Path distance：用于设置主路径两侧的衍生出的局部轨迹的长度
• Path Density 用于设置一条局部轨迹上两个轨迹点的距离
• Horizontal Density：用于设置两个局部轨迹的间距
• Rollouts Number：用于设置局部轨迹的数量
• Max velocity：小车的行驶速度，不在中心轨迹上的小车行驶速度减半，单位 m/s
• Accelration/deceleration：加速度/减加速度
• Avoiding distance：距离障碍物多远才开始避障
• Avoidance Limit：停车时，障碍物距离多远可以开车
• Lateral Safety 与 Longitudinal Safty：设置安全框尺寸，分别代表车辆安全框宽与长，障碍物在小车安全框内时，小车停止运动
• Enable following 和 enable avoidance：需要勾选上，打开跟随和避障功能

op_trajectory_generator 相关参数：

Tip Margin 与 Roll In Margin 代表意义：

• Tip Margin：用于设置从车辆中心到开始路径分叉点横向采样点的距离，其长度决定了车辆切换到不同轨迹的平滑度。（建议设置为车辆中心到车辆最前方的距离+轮子转弯半径可以满足路线切换的距离，简单粗暴点就直接设置为车辆长度*0.8，我这里设置的车长0.45*0.8=0.36）
• Roll In Margin：水平横向采样点到平行横向采样点的距离，其长度与车辆速度成正比。车辆行驶的速度越快，此区域产生平滑变化的时间就越长。（由于默认速度因子为0.25，这里可以设置为TipMargin的4倍，我这里设置的1.44。注意它对应到参数里是samplingOutMargin）
• Roll out：平行横向采样点到最大规划距离，通过从全局路径垂直移动固定的距离（称为Roll out密度）来执行直接横向采样。

op_motion_predictor 参数

• Detect curbs from map：用于将路沿判定为障碍物

op_trajectory_evaluator 相关参数：

• Enable prediction：打开预测功能

op_behavior_selector行为选择器状态机：（状态机输出topic：/current_behavior）

• 停止状态:[#停止状态]
• 任务完成状态:[#任务完成状态]
• 转向状态:[*前进状态,#转向状态]
• 停止信号停止状态:[*停止信号等待状态,#停止信号停止状态]
• 前进状态 :[*目标状态, #前进状态, 转向状态, 停止信号停止状态, 交通灯停止状态, 跟随状态, ]
• 目标状态 :[*任务完成状态,#目标状态, 前进状态]
• 停止信号等待状态:[*前进状态,#停止信号等待状态,停止信号停止状态, 目标状态]
• 交通灯停止状态:[*前进状态, #交通灯停止状态, 交通灯等待状态]
• 交通灯等待状态:[*前进状态, #交通灯等待状态, 交通灯停止状态, 目标状态]
• 跟随状态:[*前进状态, #跟随状态, 转向状态, 停止信号停止状态, 交通灯停止状态, 目标状态]
• 初始状态:[*前进状态, #初始状态]

5. 速度输出，小车开始导航

打开[Computing]页面，找到 [waypoint_follower] 下 [pure_pursuit] 与[twist_filter]，设置[app]的一些参数后，勾选 [pure_pursuit] 与 [twist_filter] 。相关一些参数设置如下图：

速度输出勾选节点：

5.1 pure_pursuit 巡迹节点

主要作用是接收op_behavior_select节点发布的“/final_waypoints”目标路径，根据纯跟踪算法，计算自车下一时刻的速度、角速度指令，并发布到“/twist_raw”x轴线速度和z轴角速度、”/ctrl_raw”加速度以及转角topic：

相关参数如下：

• 勾选 waypoint 选项，使用路径点的速度进行输出
• Lookahead_ratio：前视距离系数
• Minimum_lookahead_distance：最小前视距离限制，这里设置为小车的最小转弯半径
• Linear interpolation：是否线性插值
• 小车的预瞄点距离小车的距离是小车的前视距离，预瞄点是小车在导航中一直在“追”的点，当前行驶速度乘以系数 Lookahead_ratio 即为小车的前视距离。

5.2 twist_filter平滑节点

主要功能是对pure_suit节点输出的汽车运动速度进行低通滤波，消除杂波，使速度更加平滑。

参数如下：

• Lateral_accel_limit 和 Lateral_jerk_limit：横向加减速限制，默认即可

5.3 twist_gate 控车节点

这里单独说下这个节点，它其实是在twist_filter平滑滤波节点启动的，但它实际负责了跟底盘的控车通信。它通过订阅平滑后的控车/twist_cmd、/ctrl_cmd topic，将最终的控车消息通过timerCallback发送到了/vehicle_cmd。

*注：若是导航中小车定位容易乱，请运行 rostopic hz /points_raw 观察小车雷达频率多少，推荐雷达频率为 20hz 左右，太小如 10hz 小车运动时容易定位偏移。

 

五、绕障

按照上述方法导航时，遇到障碍物会停止下来，但是不会绕过去，需要调整些地方才可以做到

1. 障碍物跟踪与聚类的关联

# 订阅的cloud_clusters topic名需要与聚类topic对应上
vim Autoware/src/autoware/core_perception/lidar_kf_contour_track/nodes/lidar_kf_contour_track/lidar_kf_contour_track_core.cpp
    sub_cloud_clusters     = nh.subscribe("/detection/lidar_detector/cloud_clusters", 1, &ContourTracker::callbackGetCloudClusters, this);

# 欧几里得聚类输出的topic
vim Autoware/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp
  _pub_clusters_message = h.advertise<autoware_msgs::CloudClusterArray>("/detection/lidar_detector/cloud_clusters", 1);

# 重新编译
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=lidar_kf_contour_track

# 检查节点订阅关系
rosrun rqt_graph rqt_graph

确保lidar_kf_contour_track跟踪节点订阅了lidar_euclidean_cluster_detect聚类节点输出的/detection/lidar_detector/cloud_clusters topic：

2. 调整发布frame

此时障碍物可以被op_planer识别和跟踪，但是显示的位置在地图坐标原点，原因是从源头上lidar检测的障碍物就是基于雷达坐标系的，需要调整为map坐标系，并将聚类结果进行坐标系的同步

# 检查聚类的frame
rostopic echo /detection/lidar_detector/cloud_clusters |grep frame_id
    frame_id: "map"

# 检查障碍物跟踪的frame
rostopic echo /tracked_objects |grep frame_id
    frame_id: "map"

# 修改障碍物跟踪的frame
vim Autoware/src/autolaunch/launch/detection.launch
    <arg name="output_frame" default="map" />
    <arg name="sync" default="true" /> <!-- 将聚类结果从velodyne坐标系同步到map坐标系 -->

# 修改影响避障的几个参数
vim Autoware/src/autolaunch/launch/planner.launch
   <include file="$(find op_local_planner)/launch/op_common_params.launch">
      <arg name="horizontalSafetyDistance" default="0.1" /> <!-- 车辆安全框-横向 -->
      <arg name="verticalSafetyDistance" default="0.3" /> <!-- 车辆安全框-纵向 -->
      <arg name="width" default="0.35" /> <!-- 车宽 -->
      <arg name="length" default="0.45" /> <!-- 车长 -->
      <arg name="wheelBaseLength" default="0.322" /> <!-- 车辆轴距 -->
      <arg name="turningRadius" default="0.75" /> <!-- 车辆转弯半径 -->
      
      <arg name="enableSwerving" default="true" /> <!-- 打开避障功能 -->
      <arg name="enableFollowing" default="true" /> <!-- 打开跟随功能 -->
      <arg name="minFollowingDistance" default="3.0" /> <!-- 需要大于跟随距离 -->
      <arg name="minDistanceToAvoid" default="2.0" /> <!-- 距离障碍物多远才开始避障，需要 <minFollowingDistance 并且 >maxDistanceToAvoid -->
      <arg name="maxDistanceToAvoid" default="0.8" /> <!-- 停车时，障碍物距离多远可以开车 -->
   </include>
   <include file="$(find op_local_planner)/launch/op_trajectory_evaluator.launch">
      <arg name="enablePrediction" default="true" /> <!-- 打开预测功能 -->
      <arg name="horizontalSafetyDistance" default="0.1" />
      <arg name="verticalSafetyDistance" default="0.3" />
   </include>

rviz添加/detected_polygons、/op_planner_tracked_boxes topic的输出，可以看到障碍物被跟踪，位置正常，并且开始干预局部路径规划：

• 红色：不可行驶的路线
• 黄色：可行驶的路线
• 紫色：为当前选择行驶的局部路线

3. 其他绕障效果优化

实车测试，可以避障和绕障了，但是还有几个问题需要解决：

1. 到终点时，总是会使劲靠往路边走，即使有车或者树丛也使劲上撞：感觉像是op_planer到终点时有特殊的靠边停车策略，并且此时避障/绕障是不生效的，需要排查下；
   • 障碍物这块可以先增加安全框来限制车辆碰撞，并强迫车辆走选择其他局部路线绕行
   • 修改后此问题消失，可能跟下一个问题里的imu偏移有关
2. 车走的时候，从左侧绕行时，喜欢贴着障碍物走，从右侧绕行时，喜欢离障碍物很远：像是车自身左右方向有点偏似的，需要排查下车身和障碍物物的偏移关系是不是哪里不对
   • 应该是imu相对车后轴中心的位置不对，这个车imu安装在左侧，相对车身tf设置为0，那么车辆就会以为imu的位置才是车辆中心，从而导致两侧的绕行距离出现差异
   • 调整imu与base_link的tf，发布的tf里即可
3. 离车辆很近时突然阻挡车辆，车辆不会避障/绕障，但是站在3米以外就能正常绕
   • 精细测量并调整下车辆的准确长宽、轴距、转弯半径、最大转向角等车辆参数
   • 调整局部路径规划tip/in margin、规划路径条数/间距/路径点距离等，确保覆盖可行进的路宽足以规划绕行路线
   • 调整避障参数，避障距离、安全框大小
4. 用gps初始化定位容易飘，途中飘不容易恢复定位
   • 经过仔细观察，关键在gps的航向上，一旦gps航向准确，修正成功的概率就会大大增加；
   • 初始化时增加旋转航向尝试match机制，改造后，基本秒初始化定位成功；
   • 行进途中增加横向平移尝试match机制，改造后，实车验证效果尚可；
   • 尽量避免在信号不好时上电，此时车辆移动到信号逐渐变好位置的过程中，定位会偏移，尽量在空旷位置重新上电，这样初始定位位置会更加准确；
   • 后续可考虑更换具有准确航向的imu以方便确定初始航向；增加激光雷达线数，从而提高match特征点。
5. 局部规划的路径，集中在狭窄的行驶线上
   • 局部规划路线设置太少，或者路线间间距过小，调大覆盖可行进的整个路宽就好了
6. 路径规划会规划到非道路上
   • 修改地图，增加路沿限制
7. 过减速带时，容易match定位失败
   • 修改地图，增加减速带标记，降低颠簸
8. 中途容易在安全的情况下减速或暂停
   • 目前看以上修改调整后实车验证，此问题基本消失，应该跟imu tf偏移导致障碍物距离偏移有关

 

六、多点连续导航

目前实车测试看，open planner导航并不支持连续导航，在导航到第一个目标点后，需要重新运行全局路径规划、局部路径规划、以及pure persuit控制节点才能导航到下一个点。我们这里可以修改部分代码，让它能够在上一导航结束后，直接设定导航到下一个点，而不需要重启任何节点。

0. op_global_planner_core.cpp的callbackGetGoalPose()

打印m_GoalsPos目标点列表的添加记录

void GlobalPlanner::callbackGetGoalPose(const geometry_msgs::PoseStampedConstPtr &msg)
{
  PlannerHNS::WayPoint wp = PlannerHNS::WayPoint(msg->pose.position.x+m_OriginPos.position.x, msg->pose.position.y+m_OriginPos.position.y, msg->pose.position.z+m_OriginPos.position.z, tf::getYaw(msg->pose.orientation));
  m_GoalsPos.push_back(wp);
  ROS_INFO("Received Goal Pose: GoalsPosSize=%ld NewPose=%f,%f,%f OrignPos=%f,%f,%f", m_GoalsPos.size(), msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, m_OriginPos.position.x, m_OriginPos.position.y, m_OriginPos.position.z);
}

1. op_global_planner_core.cpp的MainLoop()

接近当前目标点距离时，开始下一个目标点的全局路径规划：

void GlobalPlanner::MainLoop()
{
 
        if(m_GoalsPos.size() > 0)
        {
            if(m_GeneratedTotalPaths.size() > 0 && m_GeneratedTotalPaths.at(0).size() > 3)
            {
                // 支持在导航到第一个目标点后，不重启的情况下支持直接导航到下一个点 yan 24.12.10
                if(m_params.bEnableReplanning && m_iCurrentGoalIndex<m_GoalsPos.size()-1)
                {
                    PlannerHNS::RelativeInfo info;
                    // 车辆是否在当前目的地规划路径上，如在的话，获得所在link位置信息
                    bool ret = PlannerHNS::PlanningHelpers::GetRelativeInfoRange(m_GeneratedTotalPaths, m_CurrentPose, 0.75, info); 
                    ROS_INFO("Goal Pose Enable Replanning Check: GetRelativeInfoRange=%d iGlobalPath=%d m_GeneratedTotalPaths.size=%ld info.iFront=%d m_GeneratedTotalPaths.at(info.iGlobalPath).size=%ld", ret, info.iGlobalPath, m_GeneratedTotalPaths.size(), info.iFront, m_GeneratedTotalPaths.at(info.iGlobalPath).size());
                    if(ret == true && info.iGlobalPath >= 0 && info.iGlobalPath < m_GeneratedTotalPaths.size() && info.iFront > 0 && info.iFront < m_GeneratedTotalPaths.at(info.iGlobalPath).size())
                    {
                        // 车辆当前位置距离当前目的地的距离（<=30米时，自动开始下一目的地的全局路径规划）
                        double remaining_distance = m_GeneratedTotalPaths.at(info.iGlobalPath).at(m_GeneratedTotalPaths.at(info.iGlobalPath).size()-1).cost - (m_GeneratedTotalPaths.at(info.iGlobalPath).at(info.iFront).cost + info.to_front_distance);
                        ROS_INFO("Goal Pose Remaining Distance %f <= %d ", remaining_distance, REPLANNING_DISTANCE);
                        if(remaining_distance <= REPLANNING_DISTANCE)
                        {
                            bMakeNewPlan = true;
                            if(m_GoalsPos.size() > 0){
                                ROS_WARN("m_iCurrentGoalIndex = %d", m_iCurrentGoalIndex);
                                m_iCurrentGoalIndex = (m_iCurrentGoalIndex + 1) % m_GoalsPos.size();
                            }
                            ROS_INFO("Change Goal Pose: Index=%d, GoalsPosSize=%ld ", m_iCurrentGoalIndex, m_GoalsPos.size());
                        }
                    }
                }
            }
 
}

2. BehaviorStateMachine.cpp的MissionAccomplishedStateII::GetNextState()

BehaviorStateMachine* MissionAccomplishedStateII::GetNextState()
{
    // 注释原有代码
    // return FindBehaviorState(this->m_Behavior);
    // 改成以下的代码
    PreCalculatedConditions* pCParams = GetCalcParams();
    if(pCParams->currentGoalID == -1) {
        return FindBehaviorState(this->m_Behavior);
    } else {
        pCParams->prevGoalID = pCParams->currentGoalID;
        return FindBehaviorState(FORWARD_STATE);
    }
}

3. pure_pursuit模块在final_waypoints为空时会出异常

先在pure_pursuit_core.h中添加 ros::Subscriber sub_ndt_stat定义，并增加回调函数声明：

private:
  ros::Subscriber sub_ndt_stat;
  ...
  // callbacks
  void loopWaypointsCallback(const autoware_msgs::LaneConstPtr &msg);

pure_pursuit_core.cpp添加sub_ndt_stat的定义：

void PurePursuitNode::initForROS()
{
  // setup subscriber
  sub_ndt_stat = nh_.subscribe("loop_waypoints", 10, &PurePursuitNode::loopWaypointsCallback, this); //add
}

pure_pursuit_core.cpp 中添加sub_ndt_stat的回调函数：

static autoware_msgs::Lane loopWaypoints;
void PurePursuitNode::loopWaypointsCallback(const autoware_msgs::LaneConstPtr &msg)
{
  loopWaypoints = *msg;
}

pure_pursuit_core.cpp 中修改callbackFromWayPoints函数，改成以下样子：

void PurePursuitNode::callbackFromWayPoints(
  const autoware_msgs::LaneConstPtr& msg)
{
  autoware_msgs::Lane::Ptr msg_output(new autoware_msgs::Lane);
  *msg_output = *msg;
  if (!loopWaypoints.waypoints.empty())
  {
    msg_output->waypoints.insert(msg_output->waypoints.end(), loopWaypoints.waypoints.begin(), loopWaypoints.waypoints.end());
    loopWaypoints.waypoints.clear();
  }
  if (msg_output->waypoints.empty())    return;  // 如果waypoints为空，pp_会出异常      
 
  if (!msg_output->waypoints.empty())
    command_linear_velocity_ = msg_output->waypoints.at(0).twist.twist.linear.x;   // 从航点中得到速度
  else
    command_linear_velocity_ = 0;
 
  pp_.setCurrentWaypoints(msg_output->waypoints);
  is_waypoint_set_ = true;
}
 

4. 自动设置多个连续的目标点

vim Robot/goals.py

#! /usr/bin/env python3

# 发送目标点
# python3 goals.py


import rospy
import time
import math
from geometry_msgs.msg import PoseStamped



def InitXiangyansiGoals():
    """初始化目标位置列表"""
    goals = []

    t = time.time()
    secs = int(t)
    nsecs = int((t - secs) * 1000000000)
    # print(t, secs, nsecs)

    # 西南角
    pose_westsourth = PoseStamped()
    pose_westsourth.header.frame_id = 'world'
    pose_westsourth.header.stamp.secs = secs
    pose_westsourth.header.stamp.nsecs = nsecs
    pose_westsourth.pose.position.x = -36.194183349609375
    pose_westsourth.pose.position.y = -3.115070343017578
    pose_westsourth.pose.position.z = 0.0
    pose_westsourth.pose.orientation.x = 0.0
    pose_westsourth.pose.orientation.y = 0.0
    pose_westsourth.pose.orientation.z = 0.9140234511288408
    pose_westsourth.pose.orientation.w = 0.4056613498800741
    goals.append(pose_westsourth)


    # 中间道路
    pose_middlerow = PoseStamped()
    pose_middlerow.header.frame_id = 'world'
    pose_middlerow.header.stamp.secs = secs
    pose_middlerow.header.stamp.nsecs = nsecs
    pose_middlerow.pose.position.x = -1.769686222076416
    pose_middlerow.pose.position.y = 52.081050872802734
    pose_middlerow.pose.position.z = 0.0
    pose_middlerow.pose.orientation.x = 0.0
    pose_middlerow.pose.orientation.y = 0.0
    pose_middlerow.pose.orientation.z = 0.3816013240128726
    pose_middlerow.pose.orientation.w = 0.9243270143794471
    goals.append(pose_middlerow)

    # 公寓楼
    pose_apartment = PoseStamped()
    pose_apartment.header.frame_id = 'world'
    pose_apartment.header.stamp.secs = secs
    pose_apartment.header.stamp.nsecs = nsecs
    pose_apartment.pose.position.x = 36.168060302734375
    pose_apartment.pose.position.y = 35.61585998535156
    pose_apartment.pose.position.z = 0.0
    pose_apartment.pose.orientation.x = 0.0
    pose_apartment.pose.orientation.y = 0.0
    pose_apartment.pose.orientation.z =  0.96506640968254
    pose_apartment.pose.orientation.w = -0.26200539097975056
    goals.append(pose_apartment)

    # # 停车场
    # pose_parkinglot = PoseStamped()
    # pose_parkinglot.header.frame_id = 'world'
    # pose_parkinglot.header.stamp.secs = secs
    # pose_parkinglot.header.stamp.nsecs = nsecs
    # pose_parkinglot.pose.position.x = -14.472017288208008
    # pose_parkinglot.pose.position.y = 79.07386779785156
    # pose_parkinglot.pose.position.z = 0.0
    # pose_parkinglot.pose.orientation.x = 0.0
    # pose_parkinglot.pose.orientation.y = 0.0
    # pose_parkinglot.pose.orientation.z = -0.4085631512503463
    # pose_parkinglot.pose.orientation.w = 0.9127300539811246
    # goals.append(pose_parkinglot)


    # 西北角道路
    pose_westnorth = PoseStamped()
    pose_westnorth.header.frame_id = 'world'
    pose_westnorth.header.stamp.secs = secs
    pose_westnorth.header.stamp.nsecs = nsecs
    pose_westnorth.pose.position.x = -41.374229431152344
    pose_westnorth.pose.position.y = 85.13764953613281
    pose_westnorth.pose.position.z = 0.0
    pose_westnorth.pose.orientation.x = 0.0
    pose_westnorth.pose.orientation.y = 0.0
    pose_westnorth.pose.orientation.z = 0.3484739589582032
    pose_westnorth.pose.orientation.w = 0.9373184623851152
    goals.append(pose_westnorth)

    # 寺庙门口左侧
    pose_templeleft = PoseStamped()
    pose_templeleft.header.frame_id = 'world'
    pose_templeleft.header.stamp.secs = secs
    pose_templeleft.header.stamp.nsecs = nsecs
    pose_templeleft.pose.position.x = 83.72945404052734
    pose_templeleft.pose.position.y = 171.56243896484375
    pose_templeleft.pose.position.z = 0.0
    pose_templeleft.pose.orientation.x = 0.0
    pose_templeleft.pose.orientation.y = 0.0
    pose_templeleft.pose.orientation.z = 0.3451665329359535
    pose_templeleft.pose.orientation.w = 0.9385414559522522
    goals.append(pose_templeleft)

    # 小卖部
    pose_store = PoseStamped()
    pose_store.header.frame_id = 'world'
    pose_store.header.stamp.secs = secs
    pose_store.header.stamp.nsecs = nsecs
    pose_store.pose.position.x = 315.1929626464844
    pose_store.pose.position.y = 318.74285888671875
    pose_store.pose.position.z = 0.0
    pose_store.pose.orientation.x = 0.0
    pose_store.pose.orientation.y = 0.0
    pose_store.pose.orientation.z = 0.7824470064057306
    pose_store.pose.orientation.w = 0.622717176707621
    goals.append(pose_store)

    # 寺庙大门
    pose_temple = PoseStamped()
    pose_temple.header.frame_id = 'world'
    pose_temple.header.stamp.secs = secs
    pose_temple.header.stamp.nsecs = nsecs
    pose_temple.pose.position.x = 102.62203979492188
    pose_temple.pose.position.y = 163.55311584472656
    pose_temple.pose.position.z = 0.0
    pose_temple.pose.orientation.x = 0.0
    pose_temple.pose.orientation.y = 0.0
    pose_temple.pose.orientation.z = -0.47270840074021886
    pose_temple.pose.orientation.w = 0.8812189102996058
    goals.append(pose_temple)

    # 公寓楼
    pose_apartment.header.stamp.secs = secs
    pose_apartment.header.stamp.nsecs = nsecs
    goals.append(pose_apartment)

    return goals


if __name__ ==  '__main__':
    """main"""
    # init
    rospy.init_node('goal_pose')
    pub_goal = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size = 1)
    
    pose = PoseStamped()
    pub_goal.publish(pose)
    time.sleep(1)

    # send goal pose
    goals = InitXiangyansiGoals()
    for goal in goals:
        pub_goal.publish(goal)
        time.sleep(5)

5. 编译验证

重新编译，当上车到达上一目标点后，直接设置下一目标点，车辆即可自动运行：

cd ~/Autoware
AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=op_global_planner
or
./build.sh op_global_planner

也可以直接根据预先设定好的一组途径点，一键自动设定目标（用于固定巡逻点路线巡逻）：

cd Robot/
python3 goals.py

 

七、变道

OpenPlanner规划全局路径，如果要支持实时根据环境变道和改变路径，需要在op_global_planner中启用enableLaneChange、enableReplan、enableDynamicMapUpdate，同时在vector_map绘制多条并行Lane行驶线（需要用golbal planner验证下所有的路口转弯，同时注意所有交叉路口的地方Lane必须切断，用独立的衔接Lane连接不同的路，否则切换路线会有问题）。

整个变道的流程如下：

播包单独验证路线切换和各路线的连通型：

# 加载地图等其他模块
roslaunch autolaunch run.launch use_gnss:=1 rviz:=true map_name:=jingheyuan bag_file:=/home/autoware/shared_dir/bag/run-4-jhy-bz/2024-12-11-22-26-17.bag planner:=false

# 单独debug路径规划
roslaunch autolaunch planner.launch

上车验证，在原有行驶路线上放置障碍物，即可自动切换并行的其他行驶线。

八、安全停车

1. 定位质量差场景

前几天调车时，遇到一次定位飘后车辆依然在移动导致碰撞的情况，我们这里在control逻辑里添加一 个安全保护：当定位质量差时，停止控车指令指令的输出。

考虑到发出移动指令但车辆未移动时，pnc会增加转速，为避免发生意外，我们直接在控制的源头拦截，即pure_pursuit 路径跟踪节点，拦截原始控制指令/twist_raw、/ctrl_raw的发出。

vim Autoware/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp
    #include "autoware_msgs/NDTStat.h"

    void PurePursuitNode::initForROS() {
        ...
        sub6_ = nh_.subscribe("ndt_stat", 10, &PurePursuitNode::callbackFromNdtStat, this);
        ...
    }

    // 订阅ndt_stat状态，用于根据score决定是否停下车辆 yan 24.12.25
    void PurePursuitNode::callbackFromNdtStat(const autoware_msgs::NDTStatConstPtr &msg) {
      if (msg->score != 0.0 && msg->score < 1.0) {
          good_location_quality_ = true;
      } else {  // 出现1帧定位质量飘，即立即停止移动车辆
        good_location_quality_ = false;
      }
    }

    void PurePursuitNode::run() {
        ...
        bool can_get_curvature = pp_.canGetCurvature(&kappa);
        if (!good_location_quality_)  // 当定位质量不好时，不允许控车
          can_get_curvature = false;
        ...
    }

修改后编译并运行，监控在定位飘时，控车速度以及转角topic是否变为0，并在定位恢复后继续控车：

# build
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=pure_pursuit

# check
rostopic echo /ctrl_raw

2. 定错位置场景

除了上述场景时，还存在一种情况，就是定位质量显示很好，但实际让它定错位置了，此时它会基于错误的位置规划路线，而障碍物基于map坐标系转换后，也会出现在错误的位置上，导致此时车辆碰撞问题。这里有个简单粗暴的方法，就是增加lidar近距离点云检测，即lidar检测到自身坐标系附近距离有障碍物，那么就必须安全停车。

同上，我们直接在控制的源头拦截原始控制指令/twist_raw、/ctrl_raw的发出。

vim Autoware/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit.launch
    <arg name="safety_distance" default="0.5" />            <!-- 车辆安全距离（车身坐标系下，lidar如果探测到此距离内的障碍物，则认为车辆不安全，暂时停止控车） -->
    <arg name="points_topic" default="/points_no_ground" /> <!-- ，车辆安全检查输入点云topic，这里使用过滤地面后的雷达话题/points_no_ground -->
vim Autoware/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h
    #include <sensor_msgs/PointCloud2.h>
    #include <pcl_conversions/pcl_conversions.h>
    #include <pcl/PCLPointCloud2.h>
    #include <pcl/conversions.h>
    #include <pcl_ros/transforms.h>
    #include <pcl_ros/point_cloud.h>
    ros::Subscriber sub_lidar_points;
    ...
    // 车身周围障碍物检查 yan 25.9.23
    bool safe_near_vehicle_ = false;      // 车身坐标系附近是否安全（无近距离障碍物）
    double safety_distance_ = 0.5;         // 车辆安全距离（车身坐标系下，lidar如果探测到此距离内的障碍物，则认为车辆不安全，暂时停止控车）
    std::string points_topic_ = "/points_no_ground"; //安全检查输入点云topic，这里使用过滤地面后的雷达话题
    void callbackLidarPoints(const sensor_msgs::PointCloud2ConstPtr& msg);

vim Autoware/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp
    void PurePursuitNode::initForROS() {
        private_nh_.param("points_topic", points_topic_);
        sub_lidar_points = nh_.subscribe(points_topic_, 10, &PurePursuitNode::callbackLidarPoints, this);
    }

    void PurePursuitNode::callbackLidarPoints(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromROSMsg(*msg, *cloud_ptr);
        // 检查过近的点云（默认不移除）
        int near_points = 0;
        for (unsigned int i = 0; i < cloud_ptr->points.size(); i++) {
            double origin_distance = sqrt(pow(cloud_ptr->points[i].x, 2) + pow(cloud_ptr->points[i].y, 2));
            if (origin_distance < safety_distance_) { // 小于距离阈值的技术
                near_points++;
            }
        }
        if (near_points == 0) { // 车身附近近距离没有障碍物点云，允许移动车辆
            safe_near_vehicle_ = true;
        } else { // 出现非常近距离的障碍物时，立即停止移动车辆
            ROS_WARN("Unsafe! Lidar detect %d points neer vehicle!", near_points);
            safe_near_vehicle_ = false;
        }
    }

    void PurePursuitNode::run(){
        if (!good_location_quality_ || !safe_near_vehicle_){  // 当定位质量不好或车身附近有障碍物时，不允许控车 yan 24.12.25
          can_get_curvature = false;
          ROS_WARN("Unsafe! Pause moving Vehicle! %d %d", good_location_quality_, safe_near_vehicle_);
        }
    }
    }

修改后编译并运行，监控在靠近激光雷达1米内时，控车速度以及转角topic是否变为0，并在远离后继续控车：

# build
cd ~/Autoware
./build.sh pure_pursuit

# check
rostopic echo /ctrl_raw
    cmd: 
        linear_velocity: 0.0
        linear_acceleration: 0.0
        steering_angle: 0.0

 

参考：

基于已知点云地图的NDT的激光SLAM定位

Autoware自动驾驶使用教程v1.3

基于SC-LeGO-LOAM的建图和ndt_localizer的定位

autoware下ndt_mapping节点解读

Autoware实车测试记录（二）– 实车NDT定位

Autoware实车测试记录（一）–前期准备以及使用Autoware Maptool插件进行矢量地图的绘制

多传感融合定位（六）—— autoware NDT单独测试

基于已知点云地图的NDT的激光SLAM定位

Autoware 笔记 No. 5——基于GNSS的定位

Autoware定位与建图模块(一)——定位 Label: Research

Open Planner代码分析

Autoware感知02—欧氏聚类（lidar_euclidean_cluster_detect）源码解析

Autoware小问题之二——避障的问题（不能避障的可以参考一下）

Autoware 笔记 No. 6，基于openplanner的避障

Autoware感知02—欧氏聚类（lidar_euclidean_cluster_detect）源码解析

【运动规划算法】OpenPlanner

autoware实车实战之一：如何快速适配环境、让自己的小车跑起来

Autoware小问题之一——雷达目标检测不能显示/bounding_boxes矩形框

Autoware小问题之三——open planner不能多点连续导航

ROS之四元数、欧拉角以及旋转矩阵之间的相互转换（tf库）

Autoware1.15 + OpenPlanner2.5 下的laneChange解析（1）

Autoware 软件开发指南

AutoWare源码解析——twist_filter节点

Autoware中pure pursuit纯跟踪算法的代码分析（一）