做自动驾驶这套东西，最容易被人低估的环节不是感知也不是控制，而是路径点。前两篇日记里我把 Autoware 的环境、地图、定位都折腾通了，真正卡住我的却是“车该沿着哪条线走”这件事——地图给的是静态环境，ROSBAG 给的是动态数据，中间那条把两者串起来的纽带，就是一份看起来平平无奇的 CSV 路径点文件。这篇就聊聊我是怎么用 Autoware 自带的工具，从一段录好的 ROSBAG 里把车辆轨迹“榨”成可用路径点的，中间踩过的坑一个不落地写出来，包括 yaw 弧度陷阱、速度字段全零、bag 里缺 TF 导致工具静默不输出这些让人抓狂的问题。如果你也在 Autoware 1.x 这套体系里折腾，或者只是好奇一份路径点文件到底怎么来的，这篇应该能帮你少熬两个晚上。

1. 路径点在 Autoware 里到底算个什么东西

1.1 它不是“地图”，但比地图更贴近车的行为

刚开始接触的时候我一直有个误解，觉得路径点等于地图的一部分。实际上完全不是。Autoware 里的地图（vector map 也好，点云地图也好）描述的是环境：车道线在哪、路口在哪、限速牌在哪。而路径点描述的是车的意图：从 A 点到 B 点，车打算沿着哪条折线走，每个点上大概跑多快，哪里要停一下。

这两者的关系有点像导航软件里的“路网数据”和“你这次要走的路线”。路网是公共的、全量的，路线是你这次私有的、带偏好的。路径点就是 Autoware 眼中那条“你这次要走的路”。

所以路径点文件里带的信息很有意思：x, y, z是位置，yaw是车头朝向，velocity是这个点的期望速度，change_flag一般用来标记换挡点或者停车点。你看，位置和朝向是“空间”，速度和标志位是“行为”。这也解释了为什么路径点质量直接决定车跑起来是丝滑还是抽搐——它不只是几何，它还是决策的输入。

我见过有人把路径点当成纯几何折线来理解，随便拿地图上的车道中心线导出一份 CSV 就往车里灌，结果车在弯道里画龙。原因很简单：车道中心线的点间距可能是 5 米一个，曲率变化根本体现不出来；而且速度字段要么没有要么是常量，规划模块拿到的是一份“没有意图”的几何数据，自然跑不出人的驾驶感。

1.2 三条造点路线，为什么我最后选了“从 ROSBAG 里榨”

在实际操作里，造路径点大致有三条路可走，我先把它们的取舍摆出来，后面的操作你就知道为什么我要这么绕。

我选第三条的根本原因是：人的驾驶轨迹本身就是一份高质量的“参考解”。你让一个熟练的人开一段路，他自然会在进弯前减速、出弯时提前给油、路口前松油门滑行，这些行为模式如果你靠手工编速度曲线，几乎不可能编得像。而 ROSBAG 记录的正好是这套行为在时空上的投影。

代价也是真实的。第一，定位必须稳，map 到 base_link 的变换一旦跳变，抽出来的点就是一团麻花；第二，录制的时候得有点“表演意识”，不能猛地急刹急加速，否则轨迹抽稀之后全是噪声；第三，bag 处理这一步需要你懂一点 ROS 的回放机制，不然工具跑起来没反应你还以为是编译问题。

提示：如果你手上一时没有实车条件，仿真环境里跑一段带定位的录制同样有效。关键不在于车是真车还是虚拟车，而在于map→base_link 这条 TF 链路在那个 bag 里是连续且可信的。

1.3 先把工具链的角色分清楚

在 Autoware 1.x 这套体系里，和路径点相关的工具主要是三个，名字很像但分工完全不一样，初学者极容易搞混：

• waypoint_loader：负责读。把 CSV 读进来，发布成路径话题，同时把速度信息一起发布出去。它是消费端。
• waypoint_saver：负责写。订阅实时的位姿话题，按一定间隔把位置、朝向、速度存成 CSV。它是生产端。
• waypoint_maker：负责编。在 RViz 里提供交互，可以手动加点、拖点、改速度。它是编辑器。

我一开始就是没分清这三个，以为waypoint_saver能直接吃 bag 文件。实际上它订阅的是实时的位姿话题，所以正确姿势是：先把 bag 回放起来，让位姿话题“活”过来，然后waypoint_saver在旁边安静地把流过的位姿记下来。

这个“回放 + 旁听”的模式是整件事的核心机制，理解了这个，后面所有操作都顺了。

2. 录制之前，先把“位姿可信”这件事坐实

2.1 map→base_link 的连续性比精度更重要

很多人纠结定位精度是 5 厘米还是 20 厘米，但在生成路径点这个场景下，连续性比绝对精度重要得多。原因很直白：路径点最终是一串点，如果相邻两帧的位姿突然跳了半米，那抽出来的点就会在局部形成一个诡异的折角，车跑到那里会突然打方向。

所以录制前我会先做一件事：把车停在地图里的起点，打开 RViz，盯着map坐标系下的车模型看 30 秒。判断标准很朴素——车模型在 RViz 里应该是静止的，或者只有极其轻微的抖动。如果你看到它每隔几秒轻微“呼吸”一下，或者方向角在缓慢漂移，那就说明定位还没收敛好，这时候录出来的 bag 全是垃圾。

还有一个更隐蔽的问题：定位模块在启动初期往往会有一段“从粗到精”的收敛过程。我踩过一次坑，车刚启动就开录，前 20 秒的位姿是飘的，结果生成的路径开头有十几米的歪线。后来我养成了习惯——先原地静止 10 秒再起步，这 10 秒在后续处理时直接裁掉，宁可多录不要少录。

2.2 一份够用但不臃肿的话题清单

录制 bag 最容易犯的错是“全都录”。rosbag record -a一时爽，回头 bag 文件几个 GB，回放的时候硬盘跟不上，位姿话题掉帧，抽出来的点间距忽大忽小。

我的做法是精确指定话题。以 Autoware 1.x 的常见话题命名为例，核心是这几类：

rosbag record -O waypoint_raw.bag \ /tf \ /tf_static \ /current_pose \ /vehicle_status \ /gnss/fix \ /gnss/pose_with_covariance

逐个说一下为什么：

• /tf和/tf_static是必须的。waypoint_saver内部要做坐标变换，如果 bag 里只有位姿没有 TF 树，工具会一直等变换，表现就是“启动了但什么也不输出”。这是最常见的静默失败。
• /current_pose是 Autoware 里常用的当前位姿话题（geometry_msgs/PoseStamped类型）。有些配置下它由定位模块发布，有些配置下需要你自己用 TF 转出来。
• /vehicle_status或者底盘反馈话题，用来记录真实车速。如果你打算用“按里程间隔抽点 + 用真实车速填 velocity 字段”这套流程，这个必须有。
• GNSS 相关的原始话题可以作为备份，万一后面想用 GNSS 轨迹做交叉验证。

注意：话题名在不同分支和不同车辆配置下差异很大。不要照抄，先用rostopic list确认你车上实际在发什么，再决定录哪些。我见过有人照着别人的命令录，结果录了一堆不存在的空话题，关键话题反而漏了。

2.3 驾驶动作本身就是数据质量的一部分

这段听起来有点玄，但是真的。同样的路线，不同的人开，抽出来的路径点质量能差出一个档次。我总结了几个录制时的原则：

第一，速度要稳。别一脚油门一脚刹车，匀速巡航为主，速度控制在你能从容处理弯道的水平。原因在于waypoint_saver是按固定间隔（时间或距离）抽点的，如果速度剧烈变化，抽出来的点在空间上就是疏密不均的——直道上稀稀拉拉，急减速区挤成一团。

第二，弯道提前减速，但方向要一次到位。人的自然反应是边转边调，但这对路径点不友好，会让弯道里的点呈现出锯齿状的横向摆动。稍微刻意一点，把弯走圆。

第三，路口和停止点要“停顿一下”。如果你希望生成的路径里有明确的停车点，就在该停的位置真正停稳一到两秒。这样抽点的时候这个位置会有多个几乎重合的点，后处理时很容易识别成停车段，change_flag也就有了依据。

第四，跑完别急着关。到终点后再静置几秒，让轨迹有个明确的收尾。

我把这套原则叫做“为数据而驾驶”。听起来有点反人类，但做过一次你就会发现，录制时多花的那点心思，在后处理阶段能省掉大量返工。

3. 用 waypoint_saver 把一段 bag 榨成 CSV

3.1 它的工作机制其实很朴素

waypoint_saver的逻辑用一句话概括：订阅位姿，攒够一个间隔就写一行。

它内部维护一个计时器或者距离累计器，每当满足间隔条件，就把当前位姿的 x/y/z/yaw 写下来，同时把速度也填进去。间隔到了就写，不管你有没有在动——这一点很关键，意味着如果你在录制时停车等待，它照样会写出一堆重复点。

它的核心参数一般是这几项（不同分支命名可能略有差异，以你本地 launch 文件里的实际字段为准）：

waypoint_saver: save_dir: /home/user/waypoints/ # 输出目录，文件自动命名 interval: 1.0 # 抽点间隔，单位取决于实现（秒或米） velocity: 5.0 # 位姿话题里没有速度时使用的默认速度 accel_limit: 1.0 decel_limit: 1.0

这里有两个坑值得单独说。

第一个坑是interval的单位。不同实现里它可能是秒，也可能是米。如果是秒，1.0 意味着每 1 秒写一个点——你开 30 km/h 的话，点间距就是 8 米多，对于弯道来说太粗了。如果是米，1.0 意味着每 1 米一个点，这个密度对城市道路比较合适但文件会很大。务必先确认单位再调，否则你调了半天参数发现方向反了。

第二个坑是velocity字段的来源。如果位姿话题里不带速度信息，工具会退化成用这个固定的velocity值填所有点。这就是为什么很多人生成的 CSV 里速度全是同一个数——不是工具坏了，是它没拿到真实速度。想要真实速度，要么位姿话题里带 twist 信息，要么后处理时用/vehicle_status的时间戳对齐补上。

3.2 离线回放的完整操作顺序

我现在的标准流程是这样的，顺序不能乱：

第一步，单独起一个终端，把 Autoware 的运行时框架拉起来，确保 TF 树是完好的。如果只是离线处理，不需要启动感知、控制这些模块，但定位和地图相关的部分要起来，因为waypoint_saver做坐标变换时依赖map帧存在。

第二步，把 bag 用暂停模式起播：

rosbag play --pause waypoint_raw.bag

加--pause是为了给自己留出准备时间。我曾经犯过一个低级错误——直接rosbag play，然后手忙脚乱地去启动waypoint_saver，结果最精彩的那段起点被跳过了，白录。

第三步，检查话题是否正常：

rostopic hz /current_pose

看到频率稳定输出，说明位姿流是通的。这一步千万别省，我至少三次是因为位姿话题其实没发出来，白等半天。

第四步，启动waypoint_saver，然后在播放终端按空格继续播放。

第五步，盯着输出目录，确认文件在增长。看到文件大小在变，心里就踏实了。

第六步，bag 播完后，等几秒再关waypoint_saver，让它把缓冲区写完。

提示：回放速度可以用-r参数调慢。如果你的机器性能一般，用-r 0.5半速回放，位姿话题的连续性会好很多，抽出来的点也更均匀。这个技巧在处理大 bag 时特别有用。

3.3 生成结果的第一眼检查

文件生成出来别急着用，先在终端里瞄一眼：

head -5 waypoint_20240101_120000.csv wc -l waypoint_20240101_120000.csv

正常情况下应该是每行 6 列（有些版本是 7 列，多一个转向字段）。用wc -l看行数，心里有个估算：如果你跑了一公里，点间距 1 米，那大概一千行左右。如果只有几十行，说明抽点间隔太大或者中途工具挂了；如果有几十万行，说明间隔太小或者你在原地停了很久。

我还习惯用一个小脚本快速画一下轨迹，肉眼判断形状对不对：

import csv import matplotlib.pyplot as plt xs, ys = [], [] with open("waypoint.csv") as f: for row in csv.reader(f): xs.append(float(row[0])) ys.append(float(row[1])) plt.plot(xs, ys, '-o', markersize=1) plt.axis('equal') plt.show()

图一出来，什么问题都藏不住：轨迹是不是一段完整的路线、有没有突然的折返、有没有一根莫名其妙的直线拉到远处。我见过最离谱的一次是轨迹里出现一条笔直跨越半个地图的线，排查半天发现是 bag 中途断开重连，位姿从终点跳回了起点。

4. CSV 字段里的三个隐藏地雷

4.1 yaw 单位：弧度还是角度，这是个哲学问题

waypoint_saver输出的 yaw 通常是弧度，范围在 -π 到 π 之间。但很多人脑子里想的是角度，看到1.57会觉得“这车头朝向才 1.57 度？”，实际上那是 90 度。

这个坑的真正危害不在读的时候，而在你手工编辑文件的时候。我有一次想微调某个停车点的朝向，凭直觉填了个90，结果车到位之后原地转了 20 多圈——因为 90 弧度相当于转了 14 圈多。

更隐蔽的问题是角度回绕。当车头朝向从接近 π 转到接近 -π 时，数值会从 3.14 直接跳到 -3.14。你自己写脚本做平滑滤波的时候，如果直接对 yaw 序列做滑动平均，这个跳变会污染整段数据，导致某个点的朝向变成 0 附近，车在那里会突然摆正。

处理办法是做角度展开（unwrap），把跳变拉平：

import numpy as np yaws = np.unwrap(np.array(yaws))

numpy.unwrap会自动处理 ±π 的跳变，让序列连续。这一行代码救过我无数次。

4.2 velocity 字段：全 0 和全 30 都是一场灾难

速度字段是最容易被忽略、又最容易出事的地方。

如果全是 0，规划模块拿到的是一条“要求车一直停着”的路径，表现就是规划失败或者车原地不动。如果全是同一个比较大的值，车在弯道里不会减速，你会看到一个非常刺激的画面。

我的做法是后处理时按曲率重新生成速度曲线，而不是信任录制时抽出来的原始速度。原因在于：录制时的速度包含了人的判断，但抽点后的点序列已经变了，原来那个速度对应的空间位置可能已经不精确了；而且抽稀之后点变少了，速度曲线会变得阶梯化。

重算速度的逻辑其实不复杂，核心思想是“曲率大的地方限速低”：

import numpy as np def curvature(x, y, z, i): # 用前后各两个点估算局部曲率 p0 = np.array([x[max(i-2, 0)], y[max(i-2, 0)]]) p1 = np.array([x[i], y[i]]) p2 = np.array([x[min(i+2, len(x)-1)], y[min(i+2, len(y)-1)]]) a = np.linalg.norm(p1 - p0) b = np.linalg.norm(p2 - p1) c = np.linalg.norm(p2 - p0) if a * b * c == 0: return 0.0 s = (a + b + c) / 2 area = max(s * (s-a) * (s-b) * (s-c), 0) ** 0.5 return 4 * area / (a * b * c) v_max = 8.0 # 直道最高速，m/s k_limit = 0.15 # 曲率阈值 v = [] for i in range(len(x)): k = curvature(x, y, z, i) v.append(min(v_max, v_max * k_limit / max(k, 1e-6)))

这段代码不完美，但对于城市道路这种曲率不极端的场景够用了。生成完记得做一次前向和后向的加速度约束，避免速度突变，否则车的纵向控制会很难受。

4.3 change_flag：不写也能跑，写对了才好用

change_flag这个字段很多人直接全填 0，确实也能跑。但它其实承担着一个挺重要的职责：标记行为切换点。

常见的约定是：0 表示正常行驶，1 表示这个点需要停车或者换挡。比如你在一条路线的终点标记一个 1，规划和控制模块就知道这里要减速停止，而不是傻乎乎地冲过去。

我一般的做法是：起点附近标一个 1（确保车辆从静止状态开始），终点标一个 1，中间如果有明确的停车点（比如某个路口需要等待）也标 1。其余全 0。

这里有个细节：标 1 的位置要留提前量。如果你的停车点是在路径的最后一行，车很可能来不及减速就冲过头了。我通常会在目标停车点前面 10 到 15 米的位置提前标一个减速意图，让纵向控制有足够的距离。

5. 把糙点修成能跑的路径：后处理实操

5.1 抽稀和重采样，先把密度调舒服

waypoint_saver抽出来的点，密度往往不均匀。直道上因为速度快，点间距大；转弯和减速时因为速度慢，点挤在一起。这种不均匀对下游的规划算法不友好。

我做两步处理。第一步是按固定距离重采样：把原始折线当成一条连续的曲线，每隔固定的弧长取一个点，比如 1 米取一个。这样输出就是等间距的，后续所有计算都简单了。

import numpy as np def resample(x, y, z, yaw, step=1.0): pts = np.column_stack([x, y, z]) seg = np.linalg.norm(np.diff(pts, axis=0), axis=1) s = np.concatenate([[0], np.cumsum(seg)]) total = s[-1] s_new = np.arange(0, total, step) x_new = np.interp(s_new, s, x) y_new = np.interp(s_new, s, y) z_new = np.interp(s_new, s, z) yaw_new = np.interp(s_new, s, np.unwrap(yaw)) return x_new, y_new, z_new, yaw_new

第二步是平滑。等间距之后，用一个简单的移动平均或者高斯滤波去毛刺。但这里要小心：平滑会削掉弯道的顶点，让转弯变得“提前”和“滞后”。我的经验是窗口不要超过 3 个点，宁可少平滑也不要过度。

平滑之后还有一个必须做的动作：重新计算 yaw。因为你动过点的位置了，原来的朝向就不准了。正确做法是用相邻点连线的方向角重算：

yaw_new = np.arctan2(np.gradient(y_new), np.gradient(x_new))

这个细节很多人漏掉，结果就是路径点位置改了、朝向还是旧的，车沿着路径走的时候车头一直斜着。

5.2 自交和回环，人工检查绕不开

等间距重采样有个副作用：如果原始轨迹里有回头、绕圈或者倒车，重采样之后可能出现路径自交或者重叠。可视化的时候一眼就能看出问题，但很多人只做数值检查不画图。

我的标准动作是把路径画成图，叠加在点云地图的俯视图上，肉眼看三件事：

第一，路径是不是贴着你想要的车道走。有时候录制时车稍微偏了一点，抽出来的路径就整体偏移了半米，虽然能跑但不好看。

第二，有没有突然的折角。正常的轨迹应该是连续弯曲的，出现尖角一定是数据有问题。

第三，起终点的位置对不对。我遇到过生成的路径开头多了几米“倒车”，原因是录制时车先往后退了一点再前进。

注意：如果发现路径整体偏移，不要用平移去“修”。整体平移会让路径和地图里的车道线、路口关系全部错位。正确做法是回到录制环节重新跑一遍，或者用编辑工具逐点修正局部。

5.3 一个实用的小检查清单

在后处理完、准备灌进车里之前，我会跑一份检查脚本，输出几个关键指标：

这份清单看起来啰嗦，但它帮我抓到过好几次“文件看起来正常、一跑就出问题”的隐蔽错误。最典型的一次是速度字段里混进了一个负数，原因是某行数据少了一列导致整体错位，肉眼完全看不出来。

6. 加载回去跑一遍才算真正验证

6.1 waypoint_loader 的启动与显示

路径点文件做好了，第一件事不是直接上车，而是在 RViz 里加载看效果。启动waypoint_loader的时候要注意配置里的文件路径和文件格式：

waypoint_loader: lane_csv: /home/user/waypoints/final.csv velocity_max: 10.0

启动后 RViz 里应该能看到一条彩色的路径线，颜色的含义通常和速度相关——速度高的地方偏暖色，低的地方偏冷色。如果整条线颜色一致，说明速度字段没被正确读取，大概率是列数不对或者分隔符问题。

另外一个必须检查的点：路径的朝向箭头。RViz 里显示的箭头方向应该和车实际行驶方向一致。如果箭头反了，说明 yaw 的基准差了 180 度，车加载后会朝着反方向规划。

我踩过的一个坑是 CSV 用了中文逗号或者全角空格，工具解析时跳过了一部分行却不报错，表现就是路径中间缺了一段。保存文件时确认是纯 ASCII 的逗号分隔，这个细节太容易被忽略。

6.2 上车前必须做的三项确认

第一项，原地加载不报错。把车停在路径起点附近，加载路径点，观察规划模块有没有异常输出。如果规划一直失败，先检查路径的起点是不是离车太远——很多规划器要求车辆初始位置和路径起点在某个距离阈值内。

第二项，低速跟一遍。用很低的速度（比如 5 km/h）让车沿着路径走一小段，重点看两件事：车是不是贴在路径上、转弯的时候方向盘动作是不是平滑。我一般只跑前 30 米，够判断了。

第三项，检查停车点行为。把车开到靠近终点或者某个标记了change_flag的位置，观察它是不是按预期减速。这一步最容易被跳过，但恰恰是最容易出问题的——减速太晚冲过头、减速太早停在半路，都是常见现象。

提示：如果车在路径上左右轻微摆动，先别怀疑控制参数，八成是路径点太密或者 yaw 抖动。把点间距从 1 米放宽到 2 米试试，很多摆动会自然消失。这个反直觉的经验我验证过好几次。

6.3 一套流程复用起来之后

把这套流程走顺之后，我现在的节奏是：跑一趟录制大概 20 分钟，回放抽点 5 分钟，后处理脚本一键跑完 1 分钟，RViz 检查 5 分钟，整个过程半小时左右能出一条可用的路径。相比一开始手工在 RViz 里一个个点、一条两公里的路点了一下午，效率完全是两个量级。

更重要的变化是可复现性。手工打点这件事，换个人来做结果完全不同；而录制加脚本处理这套流程，只要录制条件一致、脚本版本一致，出来的结果基本一致。这对团队协作来说价值很大。

7. 我在这个环节攒下的几点私人体会

最后说几个不成体系但挺有用的经验。

一是永远保留原始 bag。路径点文件出了问题，第一反应应该是回到 bag 重新抽一次，而不是在 CSV 上修修补补。bag 是唯一的真值来源，CSV 是派生物，把派生物当宝贝是舍本逐末。我的习惯是每段路线一个目录，里面放原始 bag、抽点参数、后处理脚本和最终 CSV，版本对得上才敢往车里灌。

二是脚本比工具可靠。waypoint_saver这种工具受话题命名、TF 状态、参数单位影响太大，稍微一个环境差异就静默失败。我后来干脆写了个小节点，直接订阅位姿话题自己攒点，逻辑简单到不可能出错。工具用不顺的时候，自己写二十行代码往往更快。

三是别小看静止段。录制时那些原地等待的几秒钟，在后处理时是识别起终点的绝佳锚点——连续多个几乎重合的点，一眼就能定位。所以我现在录制时会刻意在起点和终点各停三秒，专门留给后处理用。

四是速度曲线值得单独调。路径几何对了只是及格，速度曲线才是决定乘坐体验的部分。我的做法是先按曲率生成一版，然后实际跑的时候坐在车里感受，哪里觉得“该慢没慢”或者“该走不走”，就单独回去调那一段。这个调优过程没有捷径，就是跑几遍的事。

五是记录版本信息。CSV 文件我一般会在文件名里带日期，同时在同目录放一个说明文件，写清楚这段路径对应哪条路线、录制时用的是什么定位配置、后处理用了哪个版本的脚本。隔两周回来再看，没有这些信息你根本分不清哪份是哪份。