Changes

Update orinBuild.groovy

fix:优化动态下码头面搜索终点选取的问题

refactor: 优化Publish和UpdateDrivingStatus函数，简化代码逻辑，解决openspace 区域，方向灯不对的bug

fix: del mode=0

fix: 调整最大速度阈值以优化后向Ogm忽略逻辑

feat: 添加核心对象库并优化可执行文件链接逻辑

feat: add smooth_mode_wbca_params

fix: smooth_mode_wbca_params bug

fix: lock historical cloud point in replayer.

fix: add FORWARD_BEFORE_BACKWARD

fix: ssep replan for  reversing truck.

fix: refine deviation_stop params.

fix: path disapearing issue.

fix: trailer_path_ego match issue.

refact: straighten then reverse.

fix: using map lane width for single line lane detection

feat: stop for localization switching.

fix: log msf localization switching status.

fix: road segment splitting bug.

refact: GetRoadSegmentFirstSplitPos

fix: (临时处理)注释掉接收目标命令中的拖车标签和大小处理逻辑

fix: 解决西联IMV进停车位搜索过慢的问题

feat: 普通卡车多段倒车入库时适配拉直点逻辑

fix: SSEP: 进出散货堆场-解决后处理过于严格导致搜索失败的问题。

fix: 调整路径重叠的起始和结束点设置，确保不超出有效范围

feat: add position check for reversing

fix: disable revsersing deviation.

feat: limit straithen times.

feat: SSEP: 普通卡车倒车搜索加长最后一段拉直距离

fix: reversing times bug

fix: checkout feat_keb_task

fix: reversing ssep planner bug

fix: reversitong activation position thr 10.0

feat: add lbfgs prevous version.

fix: curvature over limit filer.

feat:合并ssep

fix: checkout ssep from feat_keb_task

feat: checkout relate_offset from feat_straddle_v2

feat: add DeviationStatus

feat: add update_end_position for ssep replan.

feat: deviation for align

fix: ssep replan for bulk align.

fix: reverse bulk offset heading

feat: limit reversing align times

fix: log align info; serialize bulk_align_offset

fix: log stop_deviation_for_algn

fix: SSEP: 提升太阳纸业自卸车出堆场成功率

fix: pnc_map splitting error.

fix: SSEP: 提升太阳纸业自卸车出堆场成功率

chore: SSEP: 提升CAR多段倒车pass point距离终点较远时的搜索速度

fix: SSEP: 修复多段倒车路径异常问题

feat: set up max_forward_distance

fix: SSEP: 修复自卸车进卸料区路径有点斜的问题

fix: SSEP: 解决多段搜索除数为0可能导致崩溃的问题

fix: SSEP: 解决多段搜索除数为0可能导致崩溃的问题

fix: SSEP: 解决精准倒车在pass point附近进行多段搜索时搜索失败的问题

fix: straight lane near crane and lock_station

feat: ssep replan for localization exception.

feat: waiting for lift traile; using heading error for reversing align

fix: checkout feat_keb_v2 ssep

fix: add max_forward_distance_for_straighten_trailer

fix: deviation_status bug

fix: ignore path optimizor in park_in state.

chore: SSEP: 集卡多段倒车接挂适配超窄库位

refactor:优化界面耗时

fix: using straighten replan logic for path_direction_change replan type

fix: SSEP: 解决威海多段倒车距离og墙太近的问题

fix: checkout feat_keb_v2

feat: 添加不可以区域Z层处理逻辑，优化地图绘制功能

fix: 修复威海车型不匹配的问题

chore: SSEP: 减小集卡多段倒车的最大曲率

fix:修复快照播放崩溃的bug

fix:修复快照播放崩溃的bug

fix: check out feat_keb_v2 ssep

fix: add ssep_forward_ec_tail_straight_distance

fix: ssep replan bug

fix: wbca warm start

feat: side-pass 决策 gap-graph 重构 + 决策可观测化

- CompareTo 纳入全分项软代价(path_l/dl/ddl/his/end), 修旧逻辑只比
  safety+smoothness 致调参失效(PNC_DP_LEGACY_COMPARE 可回退)
- BuildGaps 的 side 绑真实障碍(STATIC_OBS), 纯几何偏置不报绕行
- label 加位移阈值门控 + 搜索失败 dp_search_ok_ gating, 直行/退化不误报左右绕行
- 终点后持住 corridor 边界, 治末端假 OOB 致"先右再左"虚假 S
- path_l 目标取 gap 中心(ref_l) + 权重 200->800, 绕障后正常回目标车道
- 碰撞 cost 收口 box-to-SDF + 稠密采样; s_base_ego 提到构造期(~500->20ms)
- 新增 decision_trace + HMI"DP决策过程"窗口(可视化决策)
- 清理两段式 DP 死代码(RefineDlAlongPath/CalculateDrivingCorridorCost/旧 mean-filter)

注: OOB/OOL 硬约束当前停用, 仅 collision 为硬约束, 下一版评估重启。

fix: 扩大 data_render 地图渲染范围边距 (10->15m)

fix: path_l 的 ref_l 取 gap 内离车道中心最近的 l(非几何中心)

- ref_l = clamp(0, gap_lower, gap_upper): 障碍轻微侵入只付最小偏移清障,
  不再被拽到 gap 中间过度绕行; 障碍走后 gap 含 0 自动回正
- dl/ddl 物理化车速上限 3.3->8.0m/s

fix: 下码头面偏离参考线时扩大 path boundary 以避免 PJPO 求解失败

在 GetBoundaryFromKinetics 中, 除 IsInIntoYardPlanner 外, 海侧路口区域
(IsInSeasideJunctionRegion) 同样触发 ssep_into_yard_replan 路径边界扩大,
解决下码头面车辆偏离参考线时 PJPO 因边界过窄求解失败的问题。

附带补充 SmoothTask 起点平滑日志(start_ref_point/overlap_distance)便于排查。

fix: 开放空间区域同样扩大 path boundary 避免 PJPO 求解失败

GetBoundaryFromKinetics 触发条件再补充 IsInOpenSpaceRegion, 使开放空间
区域偏离参考线时也走 ssep_into_yard_replan 边界扩大逻辑。

fix: 大榭普通堆高机场地放宽终点约束距离, 修复进堆场 SSEP 搜索失败

GetEndStraightDis 中 DAXIE 区域且 is_normal_stacking_yard_ 时, terminal_constrain_dis
直接取 dst_accu_distance_after_end_stop_line_(不再扣除 5m straighten_extend_buffer),
避免 end_straighten_dis_ 被过度压缩导致进堆场 SSEP 搜索失败。

fix: low speed issues in wbca smooth mode.

feat: 拖挂进堆场无 passing 途径点时改用单段倒车

TT 进堆场场景下, 当搜索不到 passing 途径点
(is_have_search_passing_pt_ 为 false)时, 改用
kTractorTrailerSingleSegmentParking 单段倒车规划;
并恢复 ScenarioPlannerFactory 中单段倒车 planner 的真正构造
(原先复用 MultiSegmentParkingPlanner)。

feat: 只有当任务是倒车去货堆的时候，才用单段倒车搜索

fix: speed down unexpectly in wbca smooth mode.

feat: HMI 新增上层/下层点云显示切换开关

feat: 在 ExecPreparation 中为 LIANG_SHAN_GANG 区域添加新的场景类型处理

fix: warm-start bug

feat: PJPO 增加软曲率约束并优化起点处理

- piecewise_jerk 支持软曲率约束(kappa_ref/kappa_max/slack)及 all_soft 回退
- pjpo 起点 init_dl/init_ddl 限幅收紧, 消除起点曲率尖峰与外扩自激
- 补充 pjpo 曲率约束相关测试
- 清理 PJPO 调试 trace 日志

fix: 修复优化函数中的逻辑错误并注释掉历史记录的日志输出

feat: 在 HandleMap 中添加地图包络和中心线信息的条件判断，优化路径处理

feat: 添加车辆最大曲率计算函数，优化运动学处理

feat: 在 GetBoundaryFromKinetics 中添加场景管理器判断，优化路径边界计算

feat: 在 GetLatPoint 中添加起点曲率限制，优化车辆运动学处理

feat: 绕行边界按窄区逐段限速

- driving_corridor 沿路径找出所有狭窄区段, 每段输出 (限速, 段入口距离),
  带帧间非对称 EMA 滤波(降速即时跟手, 升速平滑防抖)
- constrain_decider 逐段生成 BOUNDARY_SPEED_LIMIT 约束, 替代原来只取单一最紧段
- max_boundary_distance_thr 默认值 0.5 -> 1.0

feat: 在 SamplePathWaypoints 中添加居中候选点处理，优化路径平滑性

feat: 变道安全动态障碍物碰撞判定引入 s-t 时空耦合

- CheckObjSafe 对速度足够的动态障碍物, 要求自车比其晚到冲突点至少 dt_gate
  (跟在其后变道), 过滤"障碍物未来才驶入路径"的纯空间重叠误判
- 障碍物过慢(< obj_min_v_kmh)时退回空间包络判定
- 新增开关 use_func_lane_change_st_time_gate_ 及 dt_gate / obj_min_v 阈值

feat: 变道 OGM 碰撞检测点 HMI 标红可视化

CheckOgmSafe 收集落进安全检测区的 OGM 占用栅格点存入 switch_data(不序列化),
DrawUnsafeArea 单独用 DrawOgmCrashPoints 标红, 直观显示哪块碰撞导致变道不安全。
upper/underlying 两层都判断, 下层碰撞时短路跳过上层。

feat: 锁站变道安全校验距离引入可配 buffer 并修正近距障碍物低速阈值

新增 lock_station_lane_change_safety_check_distance_buffer_ 配置项,
锁站(NEAR_SEASIDE_PASSING_PT)变道时用该 buffer 替代固定 +5.0;
修正 obstacle_near_speed_lower_thr_ 误写成 upper_thr 的赋值 bug。

feat: 更新 CloudTrajectory 中 curr_time_ 的初始化方式为当前时间的纳秒表示

feat: 添加 UpdateAllSoftFallback 方法以处理软回退逻辑

feat: 堆场偏移区配置改用 YardType 枚举, 新增入口高处障碍物类型并延长拉直距离

重构: 抽取搜索起点车头车挂框构造为静态函数并复用于入堆场区域

- 新增 OnlineRegionContourCreator::AddStartPostureBoxes 静态函数, 集中生成膨胀后的车头(及车挂)框
- open_space_ec 起点框内联逻辑改为调用该函数
- into_yard_junction_ec 构建 plan_region_contour 时合并起点框, 并集失败则保持原轮廓兜底

修复任务选择器组内多条件匹配语义: AND 改为 OR, 命中任一条件即选中该 group

feat:调整参考线权重，从 0.0 修改为 3.0，以增强路径优化效果

场景决策: 修复弯道 NORMAL 兜底与直行上下桥中误判

- U/S 弯 NORMAL 兜底判据统一为 BendForwardIntersection, 让桥后及海侧弯后短桥的弯能进 *_NORMAL_*_BEFORE(原 CurrentBendOut<LAT/LONG> 门挡住桥上游拓扑, 卡在 seaside INSIDE)
- 直行上/下桥中判据 StraightUp/DownBridgeInside 补前方 IsBend 早退, 与 Before 版对齐; 前方有弯道时不再误判为直行下桥中, 修复 BRIDGE_MULTI_LANES 被 STRAIGHT_INTO_BRIDGE_DOWN_*_INSIDE 遮蔽

BTActRunPlanner: 进入海侧路口区域时退出常规 planner

KEB 补充路边点云(TYPE_ROADSIDE)作为障碍规避

碰撞检测不需要路边点, 故 prepare 阶段未写入 OGM 栅格。KEB 在栅格副本上
补画下层路边采样点再抽取轮廓线, 不污染碰撞用栅格。新增开关
use_func_keb_add_roadside_segments_(默认 false), 仅下层补充。

path_bounds: 出库 planner 也纳入 kinetics 边界特殊处理

修复 GetRoadType 调用点未校验 lane 空指针导致的崩溃隐患

GetRoadType 内部 CHECK_NOTNULL, 传 null 直接 FATAL。
- routing_data GetOpenSpaceShapeMode: lane/pre_lane/suc_lane 判空后返回 PARALLEL
- into_yard_junction_ec: 修正 lane_ptr 判空顺序倒置
- reference_road_section/map_envelop: .lane 为 null 时降级为 ROAD_TYPE_UNKNOWN

修复 replayer 换图后 DrawMap 缓存 offset_ptr 悬空导致的崩溃

lane/road/junction 等指针是 offset_ptr(内存映射), 换图重建 HDMap 后旧指针
悬空。DrawMap 跨帧缓存这些指针, 但刷新只看车位移、漏了换图判断, 换图后车未
大幅移动时解引用悬空指针崩溃。DrawBaseMap 增加 map_path 变化检测, 换图时强制
清空重取。

修复变道临停标志位在非变道场景残留导致的幽灵停车线

req_stop_to_wait_change_lane_ 是跨帧锁存位, 原仅在 LaneChangeDecider 内复位,
而 OPEN_SPACE/BULKLOAD/倒车等场景不调度该 decider, 标志位残留会被 constrain_decider
读到并一直在车前挂临停停车线. 改为在 decision_reaction_ 每帧 FromHis 流转处按
planner 边界兜底清除.

修复 DrawMap last_map_path_ 从未赋值导致每帧全量重取地图

last_map_path_ 声明后从未被赋值, 恒为空串, 使换图检测 map_changed 恒为
true, 每帧强制重取 lanes/roads/junctions 等, 架空 IsNeedGetMap 的位移缓存。
在重取分支开头记录当前 map_path(放在后续 Get* 之前, 保证即便中途异常也已
记下, 不会下一帧误判换图)。

OGM 距离场增加最近占用来源标注, mask 3→5, 查询索引对齐写入端截断约定

- distanceTransform 带 DIST_LABEL_PIXEL 输出 label_map, 按占用灰度建
  label→OccSource(道路边界/沟/上下层点云/实体/预测框) 查表, 供走廊与
  DP 按来源分级处理
- mask 3→5: 3×3 掩模斜向 2~5% 低估对小障碍边界定位影响放大
- GetNearestObstacleDistance/GetNearestSource 先截断 x/res 再加 center,
  与写入端同约定, 消除负半平面 1 格系统性偏移

走廊 gap 扫描升级: 占用按来源分型写入, 断面间小障碍防丢, EMA 端点双条件

- BuildOgm 按危险度排序写占用灰度(道路边界200/沟248/下层253/上层255/
  obj251/预测250), 道路边界墙沿 shift 边界左右分开画折线, 防 uturn 宽
  边界闭合多边形自相交
- gap 扫描顺存与 SDF 同索引的最近占用来源行(set_sdf_source/
  GetSdfSourceByIdx), 供 DP/限速按来源分级
- 幻影挖窗: free 段内 SDF 局部极小 < interval/2 判为断面间小障碍, 按等
  值弦宽挖占用窗, 治小障碍随车前进可见/消失闪烁; 大障碍边界仍由细等值
  线定位不损通道宽
- gap 帧间 EMA 改端点双条件匹配, 防新障碍切出的子 gap 错配上帧大 gap
  被拉走边界
- 限速: 最近占用为道路边界/沟点的角点不触发降速, 只对障碍类降速

DP 代价修复: SDF 罚按来源分型, path_l/end 去归一化米制, 终点罚 off-by-one, 首碰 s 记录

治绕行后不回目标车道的三层叠加缺陷 + 低速晚急拐形态:
- SDF 软罚预留量按占用来源分档(cfg dp_poly_sdf_margin_*, 道路边界 0.3
  其余 1.0): 原统一 1m 使普通车道纯居中也背 safety, 路径无故偏心 ~0.8m
- SDF 碰撞硬判只统计非道路来源: 车身斜穿时角点横甩 1~2m 必扫进道路边
  界硬判带,致回中心必碰整列 HAS_COLLISION; 道路越界由 gap 硬判+
  deficit+软罚兜底
- path_l/path_end 偏离量改米制: 原 /(left+right) 归一化使回正拉力随走
  廊宽度稀释, 高速压不过 v⁴ 级 ddl 机动代价; 权重换算 800→50/1e4
- 末层判定 curr_level+1==total_level: 原 ==total_level 永不成立, 终点
  回正罚从未生效
- dl/ddl 等效车速下限 cfg 化(dp_poly_lat_v_floor): 低速急拐不再近乎免
  费, 恢复早而缓的绕行形态偏好
- ComparableCost 新增 first_coll_s 随碰撞位传播(取链上最小), 供 trace/
  HMI 把红×画在真实碰撞位置
- CfgLatDecide 序列化 bump v4(新字段分版本 gate, 旧 dump 可回放)

DP 采样与绕行决策: dl 5 档+双重剪枝, side_pass_range 真实区间, 回正保持

- dl 档 {±0.2,0}→{±0.2,±0.1,0}: 补中间档使匀速缓爬可表达, 治低速"贴 0
  到临近才集中爬完"的晚急形态
- 转移剪枝: 端点 dl 档与段平均斜率偏差 >0.15 的曲线不构不算(直行区只留
  |dl|≤0.1 档), 把 5 档转移量压回 3 档量级; level1 起点真实 dl 不剪
- 边界邻近 dl 档过滤: 到 gap 车中心可行边界余量 < 档位×5m 的"冲向边界"
  档不生成节点, 贴边死节点连同其全部入边转移一并省掉
- side_pass_range 改真实绕行区间[起坡点,回正点](沿路径回溯/延伸到偏移
  回落 ≤0.3m); 旧 {触发s, +0.5} 起点晚报且终点无意义
- 回正保持 KeepSidePassUntilCentered: 上帧在绕行且车实际仍偏 >0.5m 时
  延续 label 直到回落, 三个无触发出口(路径空/dp_search_ok=false/前方无
  障碍侧 gap)统一接入; 失败帧只延续不新发起, 依据车辆实际位置而非失败
  路径形状, 治障碍甩到车后或单帧搜索失败时状态机提前退出绕行态

side-pass trace/HMI 可观测增强: gap 边界来源分型, 红×画真实首碰位置

- HMI "DP决策过程" gap 边界按 Gap::Source 分 5 类散点(道路/静障/OGM/
  动态/未知), 空类型不占图例
- trace 节点带 coll_s(链上首碰实际 s), 红×画在首碰位置而非节点 s(节点
  记账有层间距+半车长视觉偏差, 易误读为障碍定位错), 红×聚成障碍轮廓;
  tooltip 增加首碰 s 与节点 s 对照
- dl 档摊开系数 0.75→2.5 适配 5 档(相邻档 s 偏 0.25m 可分辨/可分别 hover)
- DumpTraceJson 对 inf/nan clamp(不可达节点原写出裸 inf 致 JSON 非法)

wbca 边界: path_bound 查找改最近邻断面, 修右边界误用左半宽

- 查 driving_corridor path_bound() 原取"右邻"断面(单向 0~0.5m s 偏),
  叠加 path_bound 标签在段中点(+0.25)后小障碍凹口与障碍 s 明显错位;
  改取左右邻中更近者, 偏差降为 ±0.25(0.5m 断面结构下限)
- 右边界原误加 UnderlyingHeadBase2left(复制粘贴), 左右半宽不同车型
  右侧凹口深度偏差, 改 Base2right

gap 边界来源分型改查 SDF 真实占用来源, 修贴路沿 obj 被误标道路

BuildGaps 端点来源原是几何猜测(距 shift 边界 0.3m 内标 ROAD, 否则一律
STATIC_OBS), 贴路沿的 obj 会被误标成道路; 道路边界被滤波/挖窗内侵超
0.3m 时也会被误标静障、凑出假绕行 side。

改为优先查 source_at_l(往占用侧外探半步防窄 gap 查到对侧), 按 OccSource
映射: ROAD/DITCH→道路, UNDERLYING/UPPER→OGM, OBJ→静障, PRED_OBJ→动态;
SDF 无信息才回退几何判定。side 触发判定同步从 ==STATIC_OBS 放宽为
!=ROAD, 保持 OGM/动态切出边界仍可触发绕行的原行为。

fix: SSEP: 集卡单段倒车裁剪不必要的短轨迹

边界限速修误限: 贴边路沿点云归 ROAD, 兜底分支不吞豁免, 限速原因日志

修"贴道路边界就降速": 真实路沿的下层/上层点云叠在/紧邻几何边界墙,
按 253/255 写入后最近占用查出"障碍", ROAD/DITCH 豁免形同虚设。BuildOgm
画完墙后对墙单独求距离场, 距墙 0.75m 带内的下层/上层点改标 200(路沿
观测归道路边界); band 覆盖地图偏差(实测 0.46m 例)+截断离散 1 格。
obj/预测框后画覆盖不受影响。

修兜底误限: 轮廓顶点全被豁免时 clearance==DBL_MAX 被误判"轮廓不可用",
退静态半宽兜底把已豁免占用重新罚回。contour_clearance 回传 any_valid,
轮廓有效但全豁免=余量充足, 不再退兜底。

可观测: 每段限速输出 [boundary_limit] 日志(最窄余量/来源/触发角点/
占用像素 Frenet 位置); OgmDistance 新增 label_pos_ 表与 GetNearestOccPos
反查最近占用位置。限速上限逐点取该 s 处地图限速(原用 ego 处一刀切)。

边界限速调参: 距离阈值下限 0.25, 约束点去 10m 提前量

min_boundary_distance_thr 0→0.25: 余量低于 0.25m 即压到最低限速,
线性段从 0.25 起算, 贴障碍时限速更果断。
CreateBoundaryConstrain 约束位置去掉固定 -10m 提前量, 按窄区真实
入口下发(原提前量叠加窄区入口, 减速点过早)。

移除边界限速调试日志与占用反查诊断

问题已定位修复([boundary_limit] 日志完成使命), 摘除诊断链路:
flush_segment 的日志与 occ 反查、NarrowInfo 收集、OgmDistance 的
label_pos_ 表与 GetNearestOccPos。保留功能性修复不动: 贴墙点云改标
ROAD、兜底 any_valid(轮廓全豁免不退兜底)。需要时从 116049d3a 找回。

边界限速约束点再去车头偏置, 按窄区入口原值下发

CreateBoundaryConstrain 的 distance 去掉 -HeadBase2front: 窄区入口
距离本就是沿路径到该断面的量, 再减车头偏置会把减速点提前一个车头。

修 UseHis 拼接膨胀: frenet_frame_path 先清空再替换, 触发条件改回 USE_HIS 状态

两半同一 bug: ① IsNeedUseHis 手误写成 status_==RIGHT_SIDE_PASS → 右绕行
期间每帧触发 UseHis; ② UseHis 不清空直接 append, his 又是上帧拼接结果
→ frenet_frame_path 每帧净增约一条路径长, 线性膨胀且 s 失序(下游
GenerateBoundary/EvaluateByS 按 s 有序二分, 边界被带乱)。

修: UseHis 先 clear 再整体替换(s 平移到本帧参考系); 触发条件改回
status_==USE_HIS(状态机判定本帧 DP 结果不可信时才沿用上帧)。附带行为
修正: 右绕行恢复与左绕一致、每帧使用 DP 新结果, 不再执行首帧旧路径的
平移拼接版。

wbca 热启动: route_info 空容器防护

WarmStartFromHistory 访问 route_info.front() 前补 empty 检查, 防空容器
未定义行为。

chore: SSEP: 集卡多段倒车增加30厘米碰撞软约束

feat: SSEP: 调整集卡多段倒车拉直逻辑

fix: build error

桥区重规划: 上桥切下桥触发重规划并重置搜索状态

- ReplanDecider 增加 JudgeBridgeSearchReplan: BRIDGE_JUNCTION 规划器下, 历史帧在上桥区、当前帧进入下桥区时触发 CONTINUOUS_NORMAL_INTERSECTION 重规划
- SsepReplanUpdate 早退门加入桥区, 使桥区不再被直接清空 replan 状态
- BridgeJunctionSearchDecider::SubProcessReuse 响应该重规划类型, 重置搜索元素与回退计数

feat: 添加<cstddef>头文件以支持大小类型

桥区重规划: 上桥切下桥触发重规划并重置搜索状态

- ReplanDecider 增加 JudgeBridgeSearchReplan: BRIDGE_JUNCTION 规划器下, 历史帧在上桥区、当前帧进入下桥区时触发 CONTINUOUS_NORMAL_INTERSECTION 重规划
- SsepReplanUpdate 早退门加入桥区, 使桥区不再被直接清空 replan 状态
- BridgeJunctionSearchDecider::SubProcessReuse 响应该重规划类型, 重置搜索元素与回退计数

refactor: 后继任务重置逻辑抽取到 Stage 基类 ResetSuccessorTasks

任务失败后重置后继任务的循环在 18 个 stage 中重复, 统一抽到基类;
其中 MOTION_FSM_TASK/CONSTRAIN_DECIDER 不做 Reset 而继续 RunTask 执行,
以维持状态机与约束输出, 该行为随基类函数推广到所有场景 stage

桥区重规划: last path 无效时回退自车位姿起搜, 不再放弃搜索

ConstructDefaultStartPosture 中 last_path_ 不可用原先直接 return false,
改为用自车 x/y/yaw(挂角有效时含挂角)构造起点位姿并置 is_search_from_ego_,
日志同步改为 search from ego pose

scenario BT: 出桥区进入普通路口区时立即退出 BRIDGE_JUNCTION planner

fix: multi-frames trigger

block: 过滤本车自己上报的堆高机作业区(stackerWork_*_本车ID)

ssep: 进场景计划区域合并起点框时校验面积, 防止区域轮廓被 sanitize 丢弃后只剩起点框

boundary: 路口边界改用多边形 SL 投影, 修复异形路口边界错误(灰度默认关)

旧逻辑逐点垂线射线与手工过滤的多边形边集求交, 对凹形/异形路口取
最近交点语义错误, 门边剔除脆弱, 单点失败兜底值无限传播。

新增 PolygonSLProjector: 将路口多边形/envelope 轮廓离散投影到
guide line SL 系逐格聚合左右边界。要点:
- 门区(出入口 ±gate_window)样本一律不采信, 防异形门口的门柱顶点
  把 l≈0 砸进门口格子(实车日志实证)
- 横跨 guide line 且交叉点落在门区内的门边整条剔除
- max_slope=3.0 限坡磨尖刺, 不削直角外凸的可行驶区
- max_lateral=50 覆盖大路口敞开区, 避免人为截断走廊
- 出入口不与车道宽度混合, 门口宽度台阶是放宽方向, 与旧逻辑一致

顺手修复 Path::GetProjection s_range 窗口两处隐藏 bug(上界 fmax
应为 fmin; 平方距离误与 50.0 比较), 新增显式窗口投影接口
GetProjectionInRange(不回退全线扫描)。

开关 use_func_junction_boundary_sl_projection 默认关闭; 开启时
每帧输出 [sl_bd] 新旧对比与边界表摘要日志, 供灰度期 replay 排查,
默认打开后随旧射线路径一并清理。

单测 pnc_polygon_sl_projector_test(-DPNC_BUILD_TESTS=ON) 覆盖
矩形/凹形/直角外凸/门柱坑/单侧无约束/退化输入与窗口投影。

fix:出堆场 直接open sapce上码头面搜索起点不合适的bug

ssep: 删除路口终点位姿的车头点框内回退校验

终点区域已扩大且终点支持动态调整, 该直矩形点包含校验失去保护意义,
且在出口车道大曲率时误杀(回退到停止线得到未拉直的终点位姿)。

feat:删除打印的日志

fix: 禁用前向搜索设置中的 theta 功能

fix: wbca velocity profile bug

boundary: OGM 真值入栅格+贴边位编码, 修无障碍走廊边界内缩

旧的贴边点云改标 200 方案用无符号距离带, 边界内侧真障碍点云也被吞成
道路(限速被豁免+不发绕行+HMI 显示错), 且写入时销毁真源。改为:

- 点云一律写真实灰度(253/255); BuildOgm 构建有符号边界带 mask(外侧
  0.75m+内侧 2 格离散容差), ConstructDistanceMap 给占用像素打
  kOccInBandBit 高位, 低 7 位保真源, 消费端各自解释: 限速豁免
  in_band 亦豁免; DP 软罚按真源档(贴边点云 underlying 1.0m > 几何墙
  road 0.3m, 观测优先于几何), in_band 只豁免硬判; gap 分型贴边点云
  归 ROAD 不发绕行
- Gap::Source 扩 7 值与 OccSource 数值一一对齐, side 判定改
  IsBoundarySource(ROAD||DITCH), HMI DP 窗 7 桶分色, trace 数值语义
  同步(旧 dump 不兼容)
- 修无障碍下走廊边界内缩(路口张开区不跟随/直线段锯齿/加点反而外扩):
  挖窗豁免 ROAD(墙斜连线平坦垂距区被误判局部极小整片挖占用);
  墙折线宽度突变阶梯化(斜连线贴线带啃噬 ~0.2*Δl);
  扫描端点带 0.3m 内纯几何墙豁免(封边由扫描范围表达, 消像素基线锯齿);
  EMA 匹配容差 0.6→0.35(防历史系统性偏差持续混入不收敛)
- HMI DP 窗加 shift 边界白点线(gap 端点对比定位内缩来源)

feat: 云端pnc config适配access line转换, 补HMI绘制与收包打印

fix: 修正云端访问线路的yard_id赋值

fix: 修正云端配置中access line的yard_id赋值

fix: Align FSM超距退出3/4阶段时更新need_spreader_align_计数, 防止重回3阶段

fix: truck reversing path kappa calculation

perf: OGM精细碰撞采样点分级去重, 控制序列化体积

精细碰撞开启时采样点不再全量保留:
- 引导线路径带(半径12m, 1m粗块掩码)内按1/4 cell子格去重,
  位置误差上界约3.5cm, 每cell至多16点
- 路径带外及无引导线时退化为每cell仅保留首点(等同旧抽稀密度)
- 栅格写入逻辑不变, 仅瘦身采样点集; 精细开关关闭时行为不变
- 顺手收拢龙须/上下层四处重复写入分支为AddPoint

fix: extending distance from current ego position.

fix: Guardian锁站区域退出改为锚定车尾(带挂取挂车尾), 途径点单点前后扩展近似锁站区间, 防止车尾未过锁站障碍物提前退出

fix: 恢复union region异步计算, latch结果消费即释放治理内存驻留

- CreateUnionRegion恢复std::async异步, polygons改move捕获少一份拷贝
- latch结果消费一次即move取走并shrink_to_fit释放, 不再常驻AsyncUnionRegionState
- 重建路径不再预清union_region_/重建async_state, 异步未就绪时沿用上一帧轮廓

perf: 回放同帧降频节流, 暂停/停在末帧时避免循环满频重复计算, 改参数仍0.5s内重算生效

fix: 远程单次直线步进END后不退出循环重回MOVE

UpdateFSMState 每周期被调两次(pilot_update 一次, 失败 stage 经
ResetSuccessorTasks 仍强制执行的 MOTION_FSM_TASK 一次), fsm_interface
的 last_motion_sub_state_type 被第二次快照拍平, REMOTE_MOVE->REMOTE_END
边沿被吞, is_need_increase_remote_idx_ 永不置位, cur_remote_idx_ 不推进,
finishReqRemoteCtrl 不触发, REMOTE_END 停 remote_align_stop_sec 后经
RemoteInit 无条件重回 REMOTE_MOVE 直至超时.

prepare 改为自持上周期 remote 子状态判 END 进入沿, 不再依赖
fsm_interface 的 last_*; REMOTE_STOP 跳过判断同步替换恢复原语义.

feat: 走廊边界按gap端点来源差异化内缩(corridor_buffer_*), 钳制保DP路径可行

- GenerateBoundary: 选中gap端点按来源查buffer内缩(道路/沟点小, 点云/实体/预测框大),
  钳制到容得下DP路径(车中心±半宽+0.3 slack, 覆盖WBCA Dilated每侧0.2m侵蚀), 只缩不反转;
  贴障/起步时要求的预留自适应压到实际余量, 不做近自车渐变(避免越接近障碍预留越衰减);
  DP无路径降级帧不内缩, 保持原始边界
- 新增raw_path_bound(未内缩原始端点): path_bounds/trailer_path_bounds_decider的
  区域相等性判定(bound==shift宽)改读raw, 防内缩翻转is_in_obstacle_region分支
- cfg_lat_plan新增6个corridor_buffer_*参数(rosparam, 序列化v5), 类内初始化0兜底
  老dump反序列化(v<5跳过字段落0=不内缩=复现老行为)
- BuildGaps拆出BuildGapAt(单gap构建), 免每断面整表分类; 挂车半宽提TrailerAwareHalfWidths
  与GetSpeedLimitsAndDistances共用

fix: park子状态机收普通任务卡死PARK_MOVE无法退出

bulk分支(FSM_TYPE_B)在PARK_LOCK收到无散货途径点的普通任务时,
Lock2Init/Init2Move仅凭routing标志放行进入PARK_MOVE, 而该态下
Move2Adjust(需bulk_in点)与Park_Bulk2Travel(需bulk_out点)恒假,
只能靠再次取消任务解锁(实车卡死近4小时):
- Lock2Init/Init2Move补散货途径点检查, 普通任务不再进入死区
- Park_Bulk2Travel新增逃生: 新任务无任何散货途径点且地图可用时退回TRAVEL

同修停车场分支(FSM_TYPE_A)同类隐患: Park_Park2Travel对PARK_MOVE
放行新routing逃生, 但带双卡死判据(switch恒KEEP+停留超20s),
正常进位途中刷新停车任务不受影响

另补EnumType2String缺失的E_TASK_FSM/E_MOTION_FSM映射,
日志不再把正常事件误打成E_NO_STATE

fix: 远程单次直线步进END后不退出循环重回MOVE

UpdateFSMState 每周期被调两次(pilot_update 一次, 失败 stage 经
ResetSuccessorTasks 仍强制执行的 MOTION_FSM_TASK 一次), fsm_interface
的 last_motion_sub_state_type 被第二次快照拍平, REMOTE_MOVE->REMOTE_END
边沿被吞, is_need_increase_remote_idx_ 永不置位, cur_remote_idx_ 不推进,
finishReqRemoteCtrl 不触发, REMOTE_END 停 remote_align_stop_sec 后经
RemoteInit 无条件重回 REMOTE_MOVE 直至超时.

prepare 改为自持上周期 remote 子状态判 END 进入沿, 不再依赖
fsm_interface 的 last_*; REMOTE_STOP 跳过判断同步替换恢复原语义.

fix: park子状态机收普通任务卡死PARK_MOVE无法退出

bulk分支(FSM_TYPE_B)在PARK_LOCK收到无散货途径点的普通任务时,
Lock2Init/Init2Move仅凭routing标志放行进入PARK_MOVE, 而该态下
Move2Adjust(需bulk_in点)与Park_Bulk2Travel(需bulk_out点)恒假,
只能靠再次取消任务解锁(实车卡死近4小时):
- Lock2Init/Init2Move补散货途径点检查, 普通任务不再进入死区
- Park_Bulk2Travel新增逃生: 新任务无任何散货途径点且地图可用时退回TRAVEL

同修停车场分支(FSM_TYPE_A)同类隐患: Park_Park2Travel对PARK_MOVE
放行新routing逃生, 但带双卡死判据(switch恒KEEP+停留超20s),
正常进位途中刷新停车任务不受影响

另补EnumType2String缺失的E_TASK_FSM/E_MOTION_FSM映射,
日志不再把正常事件误打成E_NO_STATE

fix: 空Path取点断言与退化lane segment构造CHECK线上崩溃, 改优雅降级

- Path::GetIndexFromS/GetSmoothPoint 空路径不再fatal, 返回默认值并记ERROR日志
- PncMapPath lane segment构造函数点数不足2时清空path_points_留无效path,
  下游经IsAvailable安全降级, 不再CHECK崩进程

fix: Path/DiscretizedPath 空与单点场景既有隐患修复

- Path::Reset 补清 length_/num_sample_points_, 防复位后陈旧值残留
- path.hpp 给 num_sample_points_/length_ 加类内默认值, 防默认构造读未初始化垃圾
- Path::InitPoints 单点路径不再越界访问 path_pts_[-1]
- DiscretizedPath::GetPathPtFromS/GetPathPtIdxFromS 空容器 front() UB 改空判降级,
  Evaluate/EvaluateReverse 的 CHECK(!empty()) fatal 同步改 LOG(ERROR)+默认点

fix: 堆场内open space场景误报REGION_OUT_BRIDGE提前降安全阈值, 改按guide line查挂尾所在路段类型, 挂出堆场后再切换

fix: MatchPtByS 对超末端 s 解引用 end() 迭代器的未定义行为

lower_bound 落在 end() 时原代码用 *end() 参与外插, 读垃圾内存, 结果不可信
且可崩溃; 与 MatchPtByt 的同款历史修复对齐, 夹到末点返回, 并补 size<2 的
begin 分支越界保护。动态边界收窄的 arrive_t 反查每帧命中该分支, 既有
cloud_trajectory/trajectory_combiner 调用点同受益。

feat: 障碍决策模块化, 运动状态迟滞+label消抖+准入谓词单一源

新增 data/obstacle/decision/: ObstacleDecider 循环体收敛为 obstacle_decision::
Update(决策态继承→运动状态估计→Base/Label 决策→label 消抖)。

- 运动状态估计治动静误判误绕行: 感知 is_static 是 1km/h 单帧阈值, 测速置零
  即翻转, 走静态标签分支引发误绕行。改用 his_states 15帧均值速度 + 3s 窗
  净位移证据(≥0.8m, 滤 bbox 遮挡抖动) + 非对称迟滞(判动3帧/判静10帧);
  UNDETERMINED 回落感知 is_static, 真静障首帧即走静态通道。
- direction 修复: 投影失败返 0 误判 REVERSE(同向车被当对向画反向延伸/污染
  长决策)。改 +0.3/-0.15 非对称死区, 死区内保持上帧值。
- label 消抖: 收紧立即生效, 放松需连续 5 帧确认。
- 决策态继承走 Update 内 InheritDecisionState 而非 Obstacles::FromHis:
  后者全 stage 无条件跑, 会在不调度 OBSTACLE_DECIDER 的 stage 残留旧值。
- 准入谓词统一: IsDpDynamicCandidate/IsCorridorGridObstacle 单一事实源,
  DP 动态盒与 KEB 走廊消费端只调用不自判。
- ObstacleHisStates 加 velocity_filter_available(): MeanFilter 空缓存取均值
  LOG_ASSERT 直接崩, 且该滤波器不参与序列化(回放载入恒空), 消费前必查。
- debug_str 增加运动状态标记(U_/S_/M_)。attr 序列化升 v3。

feat: 动态障碍时间对齐边界收窄+会车旁路(灰度 use_dynamic_bound_clamp)

DP 有时间参数化的动态碰撞 cost 但 path_bound 只由静态化 OGM 切出, 动态避让
仅经 ref_l 软引导进 WBCA, 平滑权重把绕行量拉回。GenerateBoundary 在静态内缩
后追加动态收窄:

- 同向车时间对齐投影: 自车到达时刻按上帧轨迹反查(与 DP 同口径), 回放/首帧
  退化匀速映射(下限步速 1.5m/s, 防起步覆盖塌缩); 预测横向漂移限幅采信在
  当前实测 l ± min(0.5·t, 1m), 防邻道预测线漂移把直道路径挤偏。
- 对向车会车旁路(dyn_clamp_meeting_bypass): 常被弯道/航向门打成 IGNORE 但
  会车交互仍在, REVERSE+FRONT/SIDE+确认运动独立于 label 进收窄(不进 DP 盒
  不改绕行决策); 弃时间对齐用 6s 扫掠包络(低速会车两车到达时刻天然错开),
  豁免横向限幅(弯道对向车 Frenet 横移是真实几何), 收窄深度封顶半富余。
- 安全阀: 只收窄不翻转绕行侧, 下限=DP路径±半宽+slack, 车前 5m 起步豁免,
  K=3/M=5 帧锁存消抖(锁存迁移不受单帧降级门控, 防会车中途边界抖动)。
- 障碍 SL 半宽取 fabs: GetSLBoundary start/end 顺序未归一化, 裸减得负宽会
  把占用区间整段丢弃(根治待源头归一, 见审计记录)。
- 观测: 障碍 debug_str 追加 _D<C|B><H><A>:<n> 漏斗标记(幂等, 防回放重算
  累积); trailer 走廊共用对象但不参与收窄不触碰锁存。
- 灰度默认关(rosparam use_dynamic_bound_clamp, 验证需显式开)。

feat: side-pass trace 增加动态收窄漏斗与逐障碍投影明细

dyn_dbg 漏斗行(enabled→cand/bypass→arrive→hit→active→clamped, 哪层归零卡
哪层) + dyn_notes 逐障碍投影统计(预测 s/l 范围/swept/inter/被拒原因) +
断面级 dyn_* 收窄快照; dump 移到 GenerateBoundary 之后以带出收窄结果。
排查'为什么没收窄'先看这两处。

feat: SSEP: 集卡多段倒车场景前进后退使用不同曲率上限

fix: revering in straight line

fix: Station2Align 补齐准入过滤, 防不该进align场景绕道从station复入

feat: 绕行决策加固一轮(误绕行治理+类型策略+动作承诺+限速补全)

- DP 动态碰撞时域修复: num=5.0/0.5 却按 0.1 步进致实际只看 1s, 收成
  编译期单源 kDynEvalDtS/kDynHorizonS(5s), 让 DP 及早为对向车主动让行
- 动态障碍边界时间对齐收窄 + 会车 MEETING label 化 + 投影策略集中
  (对向扫掠包络/收窄深度封顶 dyn_clamp_meeting_depth_cap 可调) + trace 观测
- 狭窄会车识别: 收窄后可用宽度不足→降速+缩跟车距离(narrow_meeting)
- 误绕行治理:
  * 跟踪成熟度门(reserved_int_1 未初始化 + age), 修 age>0 放过新 track 的洞
  * 动态障碍足迹掩膜: 动车漏聚类只剩 OGM 点云时不切 gap(仅横向, 纵向照常)
  * 类型策略 SidePassPolicy: 车辆类居中跟车等/VRU 居中停让/靠边绕/场桥不碰,
    VRU 靠边无条件绕(解耦动静), 车辆类居中抛锚超时逃生口
  * 动作承诺: 一旦绕行锁到动作完成(甩到车后/VRU挡前硬退出/超时), 承诺期
    旁路各软门, 防超车并排时半途回摆; 进入侧连续 N 帧确认防单帧误判固化
- 走廊限速失真治理(零新栅格): 静态非绕行障碍横向余量按 frenet 补进限速;
  同向并行/超车贴身降速(空间足够不降速), 复用 PARALLEL_OBSTACLE_SPEED_LIMIT
- SL 约定全仓统一(start_l=左缘/end_l=右缘)+语义访问器
- 所有功能使能开关集中到 CfgFuncSw, 数值调参留各域 config;
  序列化迁移用占位模式保旧 dump 对齐(CfgFuncSw/CfgLatPlan/CfgSafeConst/
  ObstacleDeciderAttr/PerceptionObstacle 各升版本)

fix: is_in_straight_lane = false during park state

fix: 绕行DP转移预剪放宽, 治偏置起点+畸短首段致l=0直行走廊不可达误绕行

dl剪枝带用弦斜率avg_dl=Δl/seg_len当机动激进度代理, 在短基线上被天然放大;
init锚车辆真实s防采样抖导致首段常畸短, 偏置起点回正弦斜率超带→l=0在DP图上
不可达(留默认inf非碰撞)→被迫朝障碍侧大拱误绕行。改: 短段按基线比例放宽带宽
(cap 5x), 回正方向(|cur_l|<|prev_l|)一律放行打破方向锁; 激进度仍由IsValidCurve兜底。
不动s锚定/采样。

feat: 绕行动作承诺加实际横向执行门槛(防空承诺固化误绕行)

- 进入承诺除 label 意图外, 增加自车已实际横向偏移(|ego_match l|>0.5,
  与 DP KeepSidePassUntilCentered 同源、对齐 kKeepLThr)作为必要条件,
  防 l≈0 纯意图被单帧抖动瞬时锁成 30s 承诺
- 退出新增"空承诺撤销": 已承诺但未实际横向执行且已不满足准入即释放,
  零回摆风险; 已绕到一半(lateral_engaged)的仍走硬承诺完成
- ego_match 不可用时证据无效, 进入保守拒绝、已建承诺不误撤

feat: 类型策略灰度默认开启(use_sidepass_type_policy 默认 true)

feat: 直道正面逆向车走会车收窄让行, 不再 BLOCK 死等

会车 MEETING 原只挂在弯道/航向差大两门上, 直道正面对开车掉头后与参考线
反平行(r_diff≈0)且在直线, 两门都不触发, 掉进余宽判定两侧不足即 BLOCK
停等。在 LabelDecider !treat_as_static 块【先于】两门加会车短路:
IsMeetingScenario(确认运动+逆向+车前/侧)一律走 MEETING——本体不 BLOCK/
不写实体栅格, 只按预测轨迹收窄边界让行; 空间真不够时收窄切不出 gap,
纵向 ST/guardian 照常停, 安全不丢。灰度仍 dyn_clamp_meeting_bypass。

chore: 会车收窄封顶 dyn_clamp_meeting_depth_cap 默认 0.8→0.9(收窄更紧)

fix: 补 NARROW_MEETING_SPEED_LIMIT 登记, 修 CreateNarrowMeetingConstrain 断言崩溃

NARROW_MEETING_SPEED_LIMIT 实装时只登记了 IsSpeedLimitType, 漏了
LongitudeSpeedLimitConstrain::Initialize 的类型断言白名单。窄会车识别一旦
触发 CreateNarrowMeetingConstrain 就 LOG_ASSERT 崩溃(Signal 6)。补三处:
断言白名单 / TraceBack switch 分组(同 PARALLEL 走减速曲线) / ToString case
(原走 default 显示 UNKNOWN)。走软限速, 未加 IsHardConstrainType。

feat: 会车逆向道窄时不借道让行(纯逻辑判场景+走廊左界渐变钳)

窄路正面会车时, 本车道被 OGM 点堵住需绕行, 原会向左借进逆向道, 与对向
来车正面冲突。改为在决策层纯逻辑判场景, 执行侧钳走廊左界, 不动 DP 搜索。

参考线预计算逆向车道几何(PncMapPath::InitWidth):
- 新增 ReverseLaneGeo{valid,num,near_bound_l,far_bound_l,gap}, 逐采样点
  存左/右逆向道近远边界与缝宽(可为负), 复用原有 GetToRoadBoundaryDistance
  /GetProjection 结果不重复查图; 不序列化。
- 访问器 GetReverseLaneGeo(s, is_left, out)。

场景判定(obstacle_decision::IsMeetingBorrowYieldScenario, 帧级):
对向逆向车快速逼近(速度取 his_states 15 帧均值, 不用裸 speed_km_h——感知
单帧测速置零会误撤让行门, 是危险方向的失效) + 逆向道过窄 → 宁让不借。

执行(DrivingCorridor::Init) 三层门控:
1. 场景成立;
2. 已承诺左借(ego 中心越过车道左边线)则不钳——中途拉回比走完更危险,
   让它尽快清空逆向道; 让行只拦尚未起步的借行;
3. 本车道确实被堵才钳——用 his 走廊 drivable_seg 判(原始 OGM 堵塞, 与本帧
   钳制无关, 不震荡); 本车道畅通则完全不动边界。
命中后左界按 ego 左边缘锚点沿 12m 线性渐变钳到 lane_left_l, 避免硬钳导致
方向盘猛打; 车道被堵死→无 gap→复用纵向停, 不硬挤右侧。

停住的对向车写入栅格(IsCorridorGridObstacle):
对方停下后让行解除放行绕道, 若它不在图里就是"图上空、实际有车"。故确认
静止+逆向+heading 对齐者写图, 能否通过交真实几何判定。heading 对齐判定
(30°+3 帧防抖)从 meeting_fallback 分支提到动静分支之外, 否则静态车只能
读到上帧陈旧值。

其他:
- 逆向车绕行策略直接 ALLOW: pulled_over 相对自车参考线判, 会把侵入自车道
  的逆向车误判为"居中"→跟一辆迎面来的车。
- 走廊左钳与堵塞判据统一用 TrailerAwareHalfWidths(挂车感知/左右非对称),
  该函数上移至 Init 之前, 保持各处 footprint 口径一致。
- GetBoundaryFromS 内是 CHECK(!empty()), 新增调用在断面循环之前, 加空守卫。
- 灰度 use_meeting_borrow_yield_(cfg_func_sw v14), 观测日志 LOG_EVERY_N 降频。

feat: 烟台车内/柱子/房子点云及下沉坑点适配

新增 107/108/110/111 四种点云类型: 前三者(车辆内部/柱子/房子点云)标记
为无效点云, 碰撞与侧避均不考虑; 下沉坑点(111)按路边点云同等逻辑处理,
参与侧避绕行(限速不因它降速, 但 gap/碰撞仍当墙, KEB 补画为障碍规避)。

fix: 会车让行逆向道几何改用 guide 系断面, 钳制线改为逆向道近边界

坐标系纠错:
障碍物 sl_boundary 由 GenerateRefSlBoundary(guide_line) 生成, 是 guide 系,
原实现却拿它的 s 去查 reference_line().pnc_map_path() 的 ref 系逆向道表,
两系不重合时取到错误断面; 且硬编码主参考线, standby 断面直接查错线。
改为在 GenerateRoadBoundary 建表时用 point_info.s() 从该断面对应的那条
参考线取, 存进 BoundaryPt, 与 lane_left_l 同源, 消费侧全在 guide 系查。
顺带补 rev_s<0 早退(GetBoundaryFromS 对负 s 退化返回最远断面)。

钳制线纠错:
原钳到自车道左边线, 而"不借道"的硬线是逆向道近边界, 二者之间的本方向
其余车道与道间缝隙被白白没收; 本方向多车道时还会拦住合法的同向换道绕行。
改为 lane_left_l + reverse_lane_gap(自车道边线到逆向道近边界的距离),
承诺判据线同步取同一条, 否则同向换道会被误判成"已借逆向道"而撤销钳制。
地图两 road 重叠(gap<0)时该线比车道边线更靠右, 反而堵住侵入漏洞。

只搬差值(宽度、缝隙)不搬 near/far 绝对量: 差值的参考系零点相消, 绝对量
搬进 guide 系必须先减 point_info.l()。

SSEP 断面排除:
该段无对应参考线, lane_left_l 停在"半车宽+0.15"兜底值而本身是 left_l=10
的开阔区; meeting_yield 是帧级开关而 guide line 多段拼接, 不排除会把开阔
的 SSEP 段硬缩到贴车身。

fix: disable replan during path direction changing for straightening

feat: SSEP: 集卡多段倒车支持GPU搜索，搜索速度提升5-10倍

feat: HMI快照按钮触发发SNAP录包(关掉决策结果保存)

- DecisionReaction 加一次性标志 hmi_req_record_snap_(不序列化/不入FromHis)
- logic plan循环消费快照按钮标志并当场复位, 修从不复位每帧触发问题
- 快照按钮不再置 save_decision_result
- RosSinkRecordTrigger 按钮标志与自动条件一并发 SNAP Trigger

feat: 窄会车主动贴右让行(走廊左界钳到对向车近侧)+修急停判定两处笔误

- 走廊 Init 注入 narrow_hug: 距会车点 kHugLeadS 内把左界钳到"对向车近侧-余量",
  逼 DP/PJPO 贴右, 横向让出空间给对向车; 读 his 帧结果同时喂 DP 与 PJPO
- 判窄改用交互过滤前的几何净空(raw_l_intervals), 不再要求收窄锁存 active,
  避免"对向车已压上路径才判窄"(该刹车非靠边)的时机错位
- 逐断面存对向车近侧 l(narrow_hug_near_l_), 贴对向车而非道路右界免疫弯道 SL 畸变
- GenerateBoundary 贴右时右侧障碍源保留最小膨胀(kNarrowRightObsKeep), 防啃 og 点
- 缩跟车距离加距会车点门(kNarrowFollowLeadS), 与贴右提前量口径一致
- side_margin 默认 0.4→0.6
- 修 constrain_decider 两处: fabs(end_l()<=3.0) 括号错位; 横向切入判定
  M_2_PI(36°)误用应为 M_PI_2(90°), 致同向并行车误急停/斜插漏拦

perf: 窄会车贴右就近查改单调游标(O(n²)→均摊O(n)), 诊断日志降频

- Init 内 his_hug/his_pb 逐断面线性扫描改只前进的单调游标: 二者沿行驶方向
  单调且与本帧 while 同向, start_s 单调推进→最近邻单调推进, 均摊 O(n);
  仅采信半个 interval 内最近点, 阈值语义与原线性扫描一致
- [narrow_hug]/[narrow_hug_dbg]/[narrow_meet] 三条诊断日志频率降到 EVERY_N 50

feat: 新增镇石(ZHEN_SHI)站点地理围栏, IMV前进进出站复用西联场景

fix: 极限变道弯道tunnel沿参考线折线化, 补挂车offtracking裕量与急弯降级

极限变道的 tunnel 约束原本用单条长直线段近似弯曲车道, 弦误差为 L^2*kappa/8
(40m 直线在 R=100m 弯道上偏 2.0m), 而 end tunnel 的 HARD 管道半宽只有车道半宽
加 5cm(约2m), 弯道上管道与真实车道几乎不重叠, 起点/终点被墙关在外面, 导致
搜索必然失败。

- 折线化: 新增 AppendOffsetPolylineTunnel(沿参考线 2m 等 l 偏移折线) /
  AppendCrossLineTunnel(等 s 断面法向封口线) / CalcMaxCurvature(theta 差分
  带符号曲率), 替换四处弦近似: ConstructStartTunnel 两条侧墙+横切线、
  ConstructEndTunnel 左右管道+后端封口、ConstructObsTunnel、
  GetRealObstacleTunnel。横向量统一为 SL 约定(左正右负), 与旧
  GetParallelLine "线段方向右侧为正"、且纵向线反向构造致法向翻转的隐式约定等价
- 弯道裕量: end tunnel 补挂车 offtracking(L^2*kappa/2, 上限 1.2m, 按曲率符号
  只放宽弯道内侧, 仅带挂车型); 后向管道长度按曲率收缩(kappa>0.01 起, 最短 8m)
- 急弯兜底: kappa>0.02(R<50m) 且端约束为 HARD 且带挂时降级为 SOFT, IMV 不降级
- 顺带修: s 超出参考线长度时线段退化的隐患; SL 法向外推在 |l|*kappa 接近 1 时
  跨过曲率中心翻折成幽灵墙, 按局部曲率钳制横向跨度; 曲率采样改用插值的
  GetReferencePoint, 避免最近离散点致曲率被静默低估为 0

灰度 use_func_lc_tunnel_curvature_fit / use_func_lc_tunnel_curvature_margin,
cfg_func_sw 序列化 version 15。replay 与实车均未验证。

chore: SSEP:  解决集卡倒车距离障碍物较近时会倒着进的问题

feat: 逆向车道快速接近凸包停车防护(cluster兜底)

对向来车未被感知分类为obj、只输出TYPE_CLUSTER时, PNC纵向决策对它完全无反应:
cluster不进interested因而无decider_attr, FromHis跳过它因而无历史, ST按静态建
且不进path_time_obstacles_, 唯一影响是写OGM当前占用->只限速不停车。对"点云少+
速度快"的逆向来车, 等它进自车path时已无刹停距离。

cluster无id概念、前后帧id不对应, 因此判据做成不依赖id的标量时序:
- 逆向车道链: 自车已驶入转弯lane(无逆向邻车道)时沿参考线回退到弯道前直道作
  锚点, 再沿直道predecessor往上游延伸, 终止条件是"自车上游累计长度够不够",
  不是级数/链总长(单条lane就可能几百米, 按总长封顶会一次都延伸不进去)
- 冲突区: 自车走廊与对向道路判定带的重叠段, 不用两条中心线求交 —— 车体有
  宽度, 中心线不相交时车照样扫过去
- 判据: cluster投影到逆向链自身量弧长(不碰guide的SL, 规避弯道投影问题),
  帧级批量关联维持航迹, 两车占用冲突区的时间窗交叠即触发

防误停三道门: 路沿侧边界内缩、接近速度死区、航迹成熟度。
停止线保持2s防判据抖动导致点头; 车头横向切入对向道路超阈则整体释放 —— 进区
后再停等于横在对向道上挡路, 比抢过去更危险。

灰度use_reverse_approach_stop默认关, 12项参数均可rosparam调。
HMI"逆向车道接近"菜单画链/道路边界/判定带/冲突区/命中cluster, 状态行给出各道
门的通过情况。

chore: 逆向车道接近防护灰度默认打开

hpp 类内初始化与 cpp 的 rosparam 默认值一并改 true —— 前者在回放
version<16 的老 dump 时生效, 只改后者会让老包回放仍是关的。

fix: 窄会车限速HMI补绘制颜色, 原落到default暗红

feat: 红绿灯实时灯态停车, 含倒计时通行判断与红灯缓行

从 feat_traffic 移植实时灯态接入, 原逻辑只按地图判"路口有灯就停",
现改为按灯色放行:
- 新增 TrafficLightData 与 ROS 源, 订阅 traffic_light_det
- 按路口转向选相位判定停走, 补齐右转灯(未检出右转灯时维持放行)
- HMI 诊断显示灯态与倒计时

倒计时(感知复用 conf 字段承载)相关:
- 绿灯剩余不足以让车头驶出路口时提前停等; 已无法在停止线前停住
  则继续通过, 避免明知停不住仍急刹
- 红灯将结束时压低接近速度缓行; 停止线约束保留, 不会闯红灯

移植修正:
- FromMsg 补 clear(), 原实现跨帧累积会让历史绿灯命中 IsHaveStatus
- ROS 源补 mutex, 与其余数据源一致
- 未移植 NeedTrafficLightStop 里的调试用 return true(会架空灰度开关)

灰度 use_func_traffic_light_countdown / use_func_traffic_light_creep
默认关; PlanFrame 序列化升至 version 6, 新字段按版本追加。

style: 灯相位改指定初始化器, 防字段顺序变动时静默错位

feat: 地图标注红绿灯相位与倒计时, 挂动态层避免静态缓存不刷新

build: Debug 调试信息拆到 .dwo, 链接改用 lld

-fstandalone-debug 让每个 .o 都带完整调试信息, 全量产出 819 个 .o 共 4.7GB、
可执行文件各 1.35GB, 结果 ccache 装不下一次全量(命中率仅 17%), 链接也很慢。
追加 -gsplit-dwarf 把调试信息拆到独立 .dwo, 不再进 .o 和最终二进制。两个 flag
各管一件事(前者决定类型完整性, 后者决定存放位置), 必须同时保留, 调试能力不变。
改动后 .o 总量降到 1.25GB, pnc_node 降到 768MB。

链接器在探测到 lld 时改用 lld, 链接阶段耗时占比从瓶颈降到 0.3%。lld 是独立包
不随 clang 一起装, 所以先问编译器能否使用再启用, 没装的机器回落系统默认链接器。

注意: .dwo 按相对路径记录, 调试器需从 build/ 目录启动才能定位。

fix: BETWEEN_YARD 条件增加前进方向判断, 倒车时不启用

feat: 锁站方向判定增加终点在车后倒车分支

终点在车后且对齐余量超过 1m 时直接置为倒车方向, 避免落到
req_step_align 分支后按前进方向规划。

fix: 途径点匹配改按横向距离绝对值取最近中心线

引桥/岸桥/park/buffer/lane_change 五处筛选中心线时, min_l 初值为 DBL_MAX
却用带符号的 l() 比较, 实际选出的是横向偏移最负(最靠右)的那条, 而非横向
最近的。外层已按 fabs(l()) < 2.0 过滤, 内层比较与之矛盾。改为与
UpdateDestAttr / UpdateCurrentAttr 一致的 fabs 写法。

fix: center_line 深度 review 一轮(判定修正+越界防护+去冗余拷贝+清死代码)

判定:
- 变道左右邻的 distance 跨越数不同, 原 check_range 被右邻覆盖后左侧实线
  检查也复用它, 拆成 left/right 各算各的
- 引桥途径点 s 只算不用, 结果取到最后一个而非最近的, 补上筛选
- seaside/lane_change 比较用 start_match.s() 却存 end_match.s(), 下一轮
  比较基准错位, 统一回 start

越界与崩溃防护:
- RelationOfRefByIdx 两个重载空容器时 size()-1 下溢, 后面 CHECK 必失败,
  改优雅返回 INT32_MAX(46 处调用点已按此值处理)
- UpdateSwitchAttr 开头补 empty() 早退, 原先 back() 无保护
- JudgeIsInBlackLane 查的是邻车道, 补 idx==0 查左邻的边界判断
- GetBlackLaneDistanceFromEgo 的 count 是变道相对关系可为负, idx+count
  走 size_t 会下溢, 改 int64_t 中转后校验
- SwitchData::curr_lane_idx_/target_lane_idx_ 无类内初始化也不在构造列表,
  补 SIZE_MAX

冗余拷贝(均为每帧热路径, CenterLine 含完整点列):
- UpdateBlackLaneDistance 深拷贝整个 MltCenterLines
- GetBlackLaneDistanceFromEgo 首次 at(idx) 拷贝立即被覆盖
- JudgeIsInBlackLane 按值拷贝邻车道

死代码:
- 136 行注释掉的 lane_change 旧实现
- LaneChangePtIsInCurPath(条件括号错位致语义反转, 仅注释块内调用)
- buffer 途径点在 center_line 侧的链路(赋值处早已注释, 无消费者);
  AppReaction::buffer_passing_pts_ 仍在用, 未动

feat: HMI 显示 road boundary 的 shift type

在 road boundary 左边界点旁标注每段的 shift type, 类型跳变处各标一个,
同类型连续断面不重复标。

文字画在 cv::flip 之后(flip 前画会上下颠倒), 沿用 DrawReverseApproach
的两段式: Draw 阶段只缓存 UTM 锚点, DrawText 做坐标变换并出字。

fix: 修复 TrajectoryPoints 三处按弧长插值的参数误用

MatchPtByt/MatchPtByS/MatchMultiPtsByMultiS 都把 [0,1] 的比例传给
InterpPtByLinearS, 而后者第三参是绝对弧长, 于是恒命中其 "p0.s() >= s - 0.01"
早退分支 —— 轨迹中段 s 是几十米量级、恒大于比例值, 插值从未真正生效, 取点一直
退化成按离散点向下取整。

- MatchPtByt: 把时间比例换算成对应弧长再传
- MatchPtByS / MatchMultiPtsByMultiS: 本就按 s 查, 直接传 s
- MatchMultiPtsByMultiS: 顺带修解引用 end() 的 UB 与 l/u 写反(该函数无调用方)
- InterpPtByLinearS: z_ 是被重载的字段(存挂车横摆角, 另有 10000 终止哨兵),
  插值真正生效后不能再盲目 lerp —— 任一端越界或两端相差超过 pi 时取下界点

影响面较大: obstacle 预测采样、DP 动态代价、走廊 s<->t 反查、lattice 停车轨迹、
cloud_trajectory 都从"取整"变为"真插值", 数值会有变化, 目前只验证过变道场景。

fix: 修复 PJPO 变道横移走不完与跨两车道路径扭曲

一、变道横移走不完(车挪 1~3m 就停住, 路径起点段变直)
车的横移速率 ≈ init_dl × v(每帧只执行路径最前约 0.5m), 所以起点斜率一旦被压到
0, 路径形状再对车也不动。两处把它压到 0:
- 起点 dl 在变道场景恢复硬约束(新增 soften_init_dl)。dcf2390c1 为治 side-pass
  的起点 kappa kink 把 init_dl 软化, 但变道恰恰需要起点持续保持朝目标的横移
  斜率, 软化后 QP 每帧把它拉回 0(kSoftInitPenalty 相对 l_ref 权重极低, 拉回几乎
  无代价), 实测逐帧 0.113 -> 0.0137 -> 0.00065。
- kInitDlOutwardMax 的"向外"判据改为相对 path_reference_l。原按 init_l 符号判,
  会把 PREPARE 段"正朝目标车道靠拢"误判成向外漂移(实测 ref_l=1.57/init_l=0.82/
  init_dl=0.06), 起点斜率被夹死在 0.05。
另新增 BoostLaneChangeInitDl: QP 每帧的解从起点起 dl 就单调递减, 而车只执行起点
段, 构成一阶惯性环节(指数收敛、尾巴无限长), 只能在入口给定横移速率下限打破。

二、跨两车道路径扭曲(变一道正常, 变两道路径扭、控制跟不住而触发曲率限速)
- 变道 dl 上限放宽到 0.8。单车道 S 曲线峰值仅 0.26, 跨两车道需 ~0.5, 恰好顶死
  ±0.5 上限, QP 在约束边界上滑动导致解非光滑。用同一帧 dump 离线扫描, dl 拐点
  数与触限程度严格对应: 0.3->7, 0.5->3, 0.8->1; 放宽后实际只用到 0.505。
- config_local 的 lane_change_weight.dl_weight 20->5000(不在本仓库, 需单独同步)
  压 dl 峰值治"急": 峰值 0.42->0.30, 拐点 2->0, 收敛距离 18m->44.5m。

三、连带修复
- init_dl 为硬约束时补上夹进 dl 边界。piecewise_jerk_problem 把 dx[0] 上下界同设
  为 init 值(硬等式), 与 dx 的 box 约束矛盾时 OSQP 直接 primal infeasible, 而变道
  关闭了 all_soft 兜底, 会一路掉到 last_path fallback。
- 起点 dl/ddl 增加 Frenet 直传(参考线几何未变时从上一帧解按 s 取), 绕开 Cartesian
  往返; 失败自动回退反算。
- PjpoConstraintOptions 序列化补 enable_all_soft_fallback(版本 2->3)。生产置 false
  而结构体默认 true, 之前从不入档, 离线回放会多挂 all_soft 档致复现不可信。
- 新增离线工具 AnalyzeDlBoundEffect(扫 dl 上限/权重, 打印 opt_dl 曲线与拐点数)与
  SidePassOutwardClampRegression(比对新旧向外判据并校验解是否退化)。

验证: 实车变道正常、曲率连续; side-pass 回归 72 例 0 退化, 其中 1 例判据差异是对
旧判据误夹的纠正(车朝走廊中点走却被当成向外漂移)。

feat: 梁山港进料场景也使用通用牵引挂车泊车场景

原先仅出料(BulkOut)时设为 kTractorTrailerGeneral, 现进料(BulkIn)同样适用。

feat: 云端轨迹下发各 lane 的剩余长度与总长度

Traj.msg 的 length_s/total_length_s 与 lane_ids 逐项对齐, 取值来自
第一条引导线的 line_infos: 自车所在 lane 只算未走完的部分, 其后的
lane 取引导线覆盖的全长; 变道备用引导线不下发长度。

feat: 逆向车道接近支持 Replayer 回放诊断

fix: 修正逆向车道防护区域构建

按 section real_lane_ids 过滤反向生成的重叠车道，统一初始候选和前驱链校验。

防护区域靠自车侧使用逆向车道左 lane boundary，按自车方向的左侧外沿使用逆向车道右 road boundary，避免正反向 road boundary 重叠造成区域越界。

冲突区域改为与自车走廊求交，并补充纵向 padding 与去重采样，避免垂直相交时 polygon 退化；同步更新 HMI 边界显示。

feat: 增加堆高机侧向吊具高度防护

按堆高机与车身的横纵向位置及垂直朝向识别侧向作业场景。

场景内采用可配置的 5.5 米吊具高度阈值，并追加角度、横向距离和纵向容错配置及回放序列化。

feat: 显示障碍物朝向与速度方向

修正速度方向访问器，并为外部聚类障碍物初始化速度方向。

HMI 使用较浅、较短的箭头区分 heading 和速度方向，同时过滤场桥虚拟障碍物及聚类障碍物。

fix: 禁止跳过动态场桥临停约束

在 Range 中增加可跳过属性，并将属性传递到临停约束。\n\n区分动态场桥与静态场桥、未聚类障碍物，动态场桥始终保持停车且拒绝 SS 跳过请求，同时记录告警日志。\n\n修正出堆场场桥障碍物索引读取错误。

feat: 支持真石 IMV 按途经点多段停车

真石 IMV 进场且携带搜索途经点时，切换到 IMV 多段停车规划器并构建 pass posture。

收紧途经点支持范围，避免真石 CAR/TT 意外启用随机途经点采样，同时保持其他港区原有逻辑不变。

fix: 修复直线步进指令立即中断

区分远控命令尚未进入与已退出 MOTION_REMOTE 的状态，保留 STOP/DISABLE 启动阶段请求，使 stop2remote 能在后续周期正常触发。\n\n进入 MOTION_REMOTE 后退出仍立即中断，并保留原有超时保护；同步升级命令序列化版本。

fix: 提前窄路靠边车辆绕行判定

缩短动态障碍转静态时的确认帧数和近期位移窗口，降低停车后开始绕行的等待时间。\n\n靠边判断增加障碍外缘到真实道路边界的距离证据，使窄路上绝对横向偏移较小但实际已贴边的车辆能够及时放行；道路边界无效时保留原中心带判据。

fix: 防止DP搜索中断后绕行路径错误回正

DP完整路径无解时，从最深层的最优可行节点回溯，仅保留无碰撞、无越界的搜索前缀。

保持失败帧不发起新绕行决策，并让后续路径边界维持中断前的绕行侧位置，避免采用硬约束节点之后的虚假回目标车道段。

fix: 稳定动态障碍绕行边界切换

隔离 UseHis 最终执行路径与动态交互路径，并改用原始可行 gap 计算窄会车空间，消除历史 DP 反灌造成的边界闪烁。

横向切入采用让速不让道，切入完成或障碍后缘超过自车后释放来源侧车道边界，同时保留 KEB 碰撞防护。

增加 OG 到 obj 的占用交接标签，以历史原始 OG 作为新 box 的成熟度证据；仅旁路 age，侧向与 KEB 构图均保留原始 bbox 且不额外膨胀。

fix: 稳定窄路会车的 DP 采样边界

在自车纵向范围内延后边界收缩，并结合历史可行 gap、当前道路边界、安全缓冲和栅格余量保证最小通行空间。

锁存贴右进入与退出状态，对各采样断面的对向车几何独立做短时保持，避免快速来车时边界逐帧收缩和放开。

fix: 过滤未沿道路对齐的会车收边

MEETING 障碍只有通过沿当前道路逆向对齐确认后才能进入动态边界收窄，避免弯道外侧、支路横穿或调头车辆误触发狭窄会车。

补充 along 诊断字段，便于回放区分标签命中与沿路对开确认。

fix: 修正 DP 动态障碍横向范围过滤

使用障碍物 SL 横向边界与走廊横向宽度进行比较，修复原先混用 s 边界造成的量纲错误。\n\n避免前方动态障碍被提前过滤，恢复 DP 动态碰撞代价的有效性。

feat: 支持受控低速车辆超车

新增清扫车和洒水车类型，支持明确类型授权与可配置速度门控的低速超车候选。\n\n实现单目标锁定、左侧单向 DP 采样、目标扫掠与短时掉帧占用、动作承诺及超车限速，并在切入、会车、路口、倒车和目标失效时统一退出。\n\n按道路姿态区分静止车辆绕行：纵向对齐车辆使用短等待，斜停或横停车辆仅在确认静止并长时间堵塞后开放，避免方向和靠边判据旁路姿态保护。\n\n补充配置、诊断标记和快照序列化兼容。

fix: 收紧快速对向会车边界

快速对向车改用到达时刻对齐和局部参考线投影，投影异常时按当前状态匀速回退。

窄会车仅保留真实不足的断面，并按整组更新历史贴右区域，避免移动预测留下长尾收边和跨支路误收缩。

fix: 保持绕行路径连续性

绕行未获得足够回中空间或本帧 DP 无解时，请求沿用历史路径，避免车辆提前回目标车道后陷入首层无解。

历史路径复用增加参考线锚点与当前可行走廊校验；校验失败时交由上层停车兜底，并修正拖挂路径使用错误参考线的问题。

fix: 修正录包触发模块标识

将快照录包触发消息的模块名由 decision 统一为 pnc，与当前节点其他对外消息保持一致。

chore: SSEP: 集卡多段倒车自适应CPU/GPU模式，回退部分CPU参数

chore: SSEP: 集卡多段倒车自适应CPU/GPU模式，回退部分CPU参数

fix: synchronize straighten hold with SSEP search

fix: 延迟普通出堆场区域切换

普通出堆场场景按挂尾所在路段确定反馈区域，挂尾仍在堆场或堆垛区时保持对应安全档位，完全离开后再切换为出堆场路口。

复用按需计算的挂尾路段查询，避免非相关场景执行额外地图查询，并保持 open space 原有逻辑不变。

fix: 稳定任务切换期间区域状态

车辆静止且 PNC local map 尚未稳定时沿用上一帧区域状态，地图恢复或车辆起步后退出保持。

通过新 routing 接收标志和 task id 变化识别任务首帧，避免场景状态尚未更新时短暂回退到普通道路或路口区域；保留 PARK_LOCK 专用判定优先级。

fix: serialize WBCA max cost

fix: 低速出桥场景适配锁站安全档位

启用低速安全阈值调整时，将 REGION_OUT_BRIDGE 纳入锁站安全档位转换范围，避免该枚举因数值位于 REGION_STACKER 之后而被遗漏。

fix: plan from history in low speed

fix: 修正场景规划器过早退出判断

修正横向偏差与航向角阈值表达式的括号错误，避免将比较结果传入 fabs 导致左右判断不对称。

补齐出堆场区域切换判断，并校验搜索路径坐标长度，防止错误退出及无效路径访问。

fix: update WBCA for truck and imv

fix: Keb: 修复IMV重规划时起点曲率异常的问题

fix: stabilize STEP_ALIGN trajectory

fix: use history path for IMV direct link

fix: 修复缓刹遇障碍后无法进入停止态

NStop 执行期间进入 MOVE_BRAKE 后保持障碍制动，避免 MOVE_NSTOP 与 MOVE_BRAKE 循环切换。

车辆静止退出 MOVE_BRAKE 时恢复 NN_STOP 停车原因，使 MotionStopFsm 直接进入 STOP_CTRL。

fix: Keb: 修复堆场转场时避障距离过大的问题

fix: Keb: 修复出堆场时挂车距离障碍物太近的问题

feat: SSEP: 前港清除access line附近的地图障碍物

fix: 修复普通场景缓冲区数据失效问题

将缓冲区有效标记和距离更新移出堆高机场景判断，使通过基础路径匹配筛选的缓冲区点均能正常生效。

保留堆高机障碍物对缓冲区停车距离的动态调整逻辑。

fix: 修正纵向轨迹基础计算边界

修正轨迹点投影的绝对 s 计算、可行域减速边界和加速段距离公式。

补齐终点采样，允许数值稳定的微小时长 jerk 段，并实现 PiecewiseJerkTraj 深拷贝。

feat: 增强纵向碰撞连续扫掠

按时间和空间双阈值补齐轨迹点间的连续碰撞检测，并统一牵引车与挂车扫掠轨迹。

细化 OGM 首碰位置，修正碰撞距离重复扣除 ego s 的问题。

fix: 统一纵向停车规划与限速约束

建立统一停车上下文与运行状态，明确普通规划继承规划曲线起点、FSM 安全制动继承车辆反馈。

将严格停车改为 jerk 受限的多候选采样并支持上一条安全曲线续接，统一零速硬约束、动态限速和碰撞检查。

压缩停车诊断序列化，在回放端重建可派生数据，并补齐停车、重入、重载、限速及连续扫掠回归测试。