3、行为规划器(Behavior Planner)调试：场景选择逻辑、状态机跟踪

行为规划器，说白了就是自动驾驶的「大脑皮层」。它决定了车什么时候该跟、什么时候该停、要不要变道。我刚开始接触Autoware时，总觉得这模块挺玄乎——明明感知和定位都没问题，车就是不走，或者突然急刹。后来才发现，十有八九是行为规划器在「犯迷糊」。

今天我们就来聊聊，怎么把Behavior Planner的调试搞明白。我个人习惯从两个维度入手：场景选择逻辑和状态机跟踪。这两个搞定了，80%的问题都能定位到。

Behavior Planner的核心，是一个场景管理器。它根据当前的路网信息和自车状态，从一堆场景里挑一个来执行。比如：

每个场景都有自己的触发条件。调试的第一步，就是搞清楚：当前时刻，它到底激活了哪个场景？

核心调试方法：打印场景切换日志。

在Autoware中，场景管理器会输出 /planning/scenario_planning/current_scenario 这个topic。你可以用 ros2 topic echo 实时看，或者录bag后回放。

我在项目中遇到过一个问题：车在直道上突然开始减速，然后往路边靠。我一开始以为是感知误检了障碍物，查了半天发现——场景管理器莫名其妙切到了PullOver。为什么？因为路网地图里有一段路被标记成了「终点区域」，而车刚好经过那里。嗯，地图数据的问题。

所以，当你发现车辆行为异常时，第一件事就是看当前场景。别急着怀疑控制或感知。

每个场景内部，其实是一个状态机。比如LaneFollow场景，它内部可能有：

状态机的跳转，决定了车辆的具体行为。调试时，我建议你关注两点：

举个例子。有一次，车在路口明明绿灯，却停着不走。我查了状态机，发现它卡在了APPROACHING状态。为什么？因为感知模块检测到了一个「虚拟障碍物」——其实是路边的金属护栏反光，被误识别成了行人。状态机认为前方有障碍物，就一直不切到DRIVING。

我的调试技巧：在代码里加一个状态机跳转的日志输出。每次状态变化时，打印出「从A状态 -> B状态，原因是XXX」。这样你回放bag时，就能精确知道是哪一帧、哪个条件触发了问题。

你想想看，如果没有这个日志，你只能看到车停了，但不知道为什么停。有了状态机跟踪，问题就变成了「哦，是感知误检导致状态机卡住了」。定位效率翻倍。

有时候，问题出在场景选择和状态机的配合上。比如：

我建议你画一张流程图，把场景选择和状态机的关系理清楚。下面是我自己常用的调试思路图：

这张图里，场景管理器负责选「大剧本」，每个场景内部的状态机负责演「小剧本」。调试时，你既要看大剧本选对了没，也要看小剧本演对了没。

假设你遇到一个问题：车在路口左转时，突然急刹。你会怎么查？

我的流程是这样的：

看当前场景：用 ros2 topic echo /planning/scenario_planning/current_scenario，发现是 Intersection。嗯，场景选对了。
看状态机：在Intersection场景内部，状态机当前是 WAITING。为什么在等？
查触发条件：状态机从 APPROACHING 跳到 WAITING 的条件是「检测到红灯或行人」。但当时明明是绿灯，也没有行人。
定位到感知：进一步查感知topic，发现有一个误检的「虚拟行人」在路口中间。问题找到了。

注意：状态机跳转条件，往往依赖多个模块的输出。比如：

任何一个输入异常，都可能导致状态机「抽风」。我曾经遇到过因为GPS信号跳变，导致状态机在DRIVING和STOPPED之间来回切换，车一抖一抖的。查了三天才发现是定位的问题。

最后，分享几个我常用的调试工具和方法：

一个小技巧：在代码里加一个 RCLCPP_INFO_THROTTLE，每秒只打印一次当前状态。这样既能看到状态变化，又不会刷屏。我一般设成500ms打印一次，刚刚好。

嗯，行为规划器的调试，说白了就是「看它选了啥场景，看它走到了哪个状态，看是什么条件让它跳的」。把这三点盯住了，再诡异的问题也能找到根因。别问我为什么知道——我踩过的坑，比你想象的多得多。