Changes

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 搜索失败。

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

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

fix: SSEP: 1. 解决威海多段倒车拉直搜索会反复往前的问题。2. 修复杨浦码头面直接进堆场在超过pass point的位置重规划时会规划失败的问题。

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 方法以处理软回退逻辑

fix: Keb: 改进低贝位进堆场时距离上层障碍物较近的问题

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

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

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

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

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

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

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: 添加 UpdateAllSoftFallback 方法以处理软回退逻辑

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

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

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

fix:  Keb: 参考线发生变化时禁用warm start，调用cold start重新优化路径

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

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

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

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

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

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

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

场景决策: 修复弯道 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

feat: SSEP: 多段倒车支持无pass point模式。解决到达pass point后重规划会继续往前的问题，解决停车角度歪的问题

fix: imv wbca crash bug

fix: disable wbca during esdf_map = nullptr

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

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

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/动态切出边界仍可触发绕行的原行为。

边界限速修误限: 贴边路沿点云归 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 检查, 防空容器
未定义行为。