追赶法是专门用来解三对角线性方程组的高斯消元法。什么是三对角？upper →┌ ┐└ ┘lower = [*, l1, l2, l3, l4] // 下对角线（i 行对应 M[i-1] 的系数）main_diag = [m0, m1, m2, m3, m4] // 主对角线（i 行对应 M[i] 的系数）upper = [u0, u1, u2, u3, * ] // 上对角线（i 行对应 M[i+

参考线上每一个点 R，都可以用弧长 s 来定位。切向量 τ：沿着参考线的方向。如果参考线在 s 处的航向角是 θ，那么法向量 n：垂直于 τ，指向参考线左侧。s：P 沿着参考线的投影位置（走了多远）l：P 偏离参考线的距离，左正右负这就是 Frenet 坐标(s, l)的定义。上面这个展开式其实也就是Frenet → 笛卡尔的转换公式——有 s 有 l，直接代进去就出 (x, y)。对应到我的代码

本文记录了ROS2+EMPlanner开发中EgoVehicle模块的实现细节。该模块作为自车代理，主要功能包括存储位姿、执行运动学计算和广播TF变换。采用模块化设计，将位姿信息(Pose)与车辆完整状态(VehicleInfo)解耦，便于不同模块调用。开发过程中解决了四元数默认值导致的RViz2显示异常问题，通过显式设置四元数值确保TF变换正确显示。模块采用50Hz定时器驱动，实现自行车模型积分

背后的机制是 RViz2 内部维护了一个"黑板"——你把 Marker 写上去，它就存在那里。我的场景就是典型的"先发后收"——main 函数里在初始化完成后立刻发布，但这时候我还没打开 RViz2。——消息发了就发了。如果发布的时候没有订阅者，消息直接丢弃，后来连上来的订阅者什么也收不到。的含义是：发布者把最后一条消息保存起来，之后有新的订阅者连上来，自动补发一份。后面把这段替换成 EM Pla

背后的机制是 RViz2 内部维护了一个"黑板"——你把 Marker 写上去，它就存在那里。我的场景就是典型的"先发后收"——main 函数里在初始化完成后立刻发布，但这时候我还没打开 RViz2。——消息发了就发了。如果发布的时候没有订阅者，消息直接丢弃，后来连上来的订阅者什么也收不到。的含义是：发布者把最后一条消息保存起来，之后有新的订阅者连上来，自动补发一份。后面把这段替换成 EM Pla