From 675f934649d3b99f67f9ec5f72eb5cb3afcf9767 Mon Sep 17 00:00:00 2001 From: Dame-time-0034 <812793632@qq.com> Date: Wed, 18 Nov 2026 14:27:08 +0800 Subject: [PATCH] =?UTF-8?q?feat(scenario):=20=E6=96=B0=E5=A2=9E=E5=AF=B9?= =?UTF-8?q?=E5=90=91=E8=BD=A6=E5=80=9F=E9=81=93=E4=BE=B5=E5=85=A5=E6=9C=AC?= =?UTF-8?q?=E8=BD=A6=E9=81=93=E5=9C=BA=E6=99=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 新增 oncoming_intrusion 模板(城市双向路)与语义路由; 修复播放器借道变道前瞻方向与城市车道识别。 --- llm/src/scenario_intents.py | 15 +++++ llm/src/scenario_templates.py | 116 ++++++++++++++++++++++++++++++++++ llm/src/xosc_player.py | 78 +++++++++++++++++++---- 3 files changed, 196 insertions(+), 13 deletions(-) diff --git a/llm/src/scenario_intents.py b/llm/src/scenario_intents.py index 72aed31..6b98791 100644 --- a/llm/src/scenario_intents.py +++ b/llm/src/scenario_intents.py @@ -103,6 +103,14 @@ "examples": ["借对向车道超车时对面有车驶来,只好放弃超车", "超车到一半发现对向来车,赶紧回到原车道"], }, + "oncoming_intrusion": { + "aliases": ["对向车借道", "对向借道侵入", "对向车侵入", "对向车占道", + "对面车占道", "迎面占道", "越过中心线", "越线迎面", + "对向车跑到我车道", "对向车迎面驶来", "迎面对撞风险", + "占了我的车道", "占我车道", "对面来车占道"], + "examples": ["对向车借道超车,越过中心线朝我迎面开过来", + "对面来车占了我的车道,只能紧急刹车"], + }, "signal_phase": { "aliases": ["红绿灯", "信号配时", "绿灯起步", "黄灯", "红灯停车", "信号灯变化"], @@ -143,6 +151,12 @@ (("超车", "对向"), "overtake_oncoming", 8.0), (("超车", "来车"), "overtake_oncoming", 6.0), (("借道", "对向"), "overtake_oncoming", 6.0), + (("对向车", "借道"), "oncoming_intrusion", 8.0), + (("对向车", "占道"), "oncoming_intrusion", 8.0), + (("越过", "中心线"), "oncoming_intrusion", 8.0), + (("迎面", "占道"), "oncoming_intrusion", 8.0), + (("对向车", "本车道"), "oncoming_intrusion", 6.0), + (("占", "车道"), "oncoming_intrusion", 6.0), (("闯红灯",), "junction_light", 6.0), (("左转",), "unprotected_turn", 3.0), (("汇入",), "merge_turn", 3.0), @@ -152,6 +166,7 @@ _REQUIRED_MAP = { "unprotected_turn": "Town10HD_Opt", "merge_turn": "Town10HD_Opt", + "oncoming_intrusion": "Town03", } _CN_NUM = {"零": 0, "一": 1, "二": 2, "两": 2, "三": 3, "四": 4, "五": 5, diff --git a/llm/src/scenario_templates.py b/llm/src/scenario_templates.py index 1eaf74d..7af5d55 100644 --- a/llm/src/scenario_templates.py +++ b/llm/src/scenario_templates.py @@ -883,6 +883,92 @@ def _named(b: BehaviorSpec, name: str) -> BehaviorSpec: ) +# ===================================================================== +# 模板 10c:对向车借道侵入本车道(城市双向路) +# 自车在城市双向路上正常行驶;对向车道的来车为超越同向慢车,越过 +# 中心线驶入自车车道,形成迎面冲突。自车制动停车,来车在临近时 +# 放弃借道、退回原车道。 +# Town03 road 67 从 s≈174 起有 136m 直线无路口,两个方向各有车道。 +# ===================================================================== +def build_oncoming_intrusion(p: Dict[str, Any]) -> ScenarioSpec: + s0 = float(p.get("s0", 174.0)) + road_id = str(p.get("road_id", "67")) + lane = str(p.get("lane", LANE_R)) + ego_speed = float(p.get("ego_speed", 12.0)) + slow_speed = float(p.get("slow_speed", 6.0)) + slow_gap = float(p.get("slow_gap", 45.0)) + oncoming_speed = float(p.get("oncoming_speed", 12.0)) + oncoming_gap = float(p.get("oncoming_gap", 95.0)) + lc_delay = float(p.get("lc_delay", 0.5)) + lc_time = float(p.get("lc_time", 1.2)) + brake_gap = float(p.get("brake_gap", 70.0)) + brake_time = float(p.get("brake_time", 2.5)) + abort_gap = float(p.get("abort_gap", 45.0)) + abort_time = float(p.get("abort_time", 1.2)) + abort_speed = float(p.get("abort_speed", 5.0)) + duration = float(p.get("duration", 10.0)) + + def _named(b: BehaviorSpec, name: str) -> BehaviorSpec: + b.name = name + return b + + # slow 在对向车道(+1)内朝自车驶来,oncoming 在其后方更远处; + # oncoming 变道进入自车车道时,其在 OpenDRIVE 中 s 减小、车头朝 + # 自车。lane_change 的 delta 取 +1:+1 车道的左邻车道即本车道 -1, + # 退回时 -1 的左邻车道又是 +1,两个方向都用 +1。 + return ScenarioSpec( + name="oncoming_intrusion", + description="对向车借道超越慢车,越过中心线迎面驶入本车道," + "自车在 %.0fm 处制动,来车在 %.0fm 处退回原车道" + % (brake_gap, abort_gap), + entities=[_veh("ego", s0, ego_speed, lane, road_id=road_id), + _veh("slow", s0 + slow_gap, slow_speed, LANE_ONCOMING, + road_id=road_id), + _veh("oncoming", s0 + oncoming_gap, oncoming_speed, + LANE_ONCOMING, road_id=road_id)], + behaviors=[ + _named(_speed("ego", ego_speed, DynamicsShape.STEP, 0.0), + "ego_cruise"), + _named(_speed("slow", slow_speed, DynamicsShape.STEP, 0.0), + "slow_cruise"), + _named(_speed("oncoming", oncoming_speed, DynamicsShape.STEP, 0.0), + "oncoming_cruise"), + _named(_lane_change("oncoming", +1, lc_time), + "oncoming_intrude"), + _named(_speed("ego", 0.0, DynamicsShape.LINEAR, brake_time), + "ego_brake"), + _named(_lane_change("oncoming", +1, abort_time), + "oncoming_abort"), + _named(_speed("oncoming", abort_speed, DynamicsShape.LINEAR, 2.0), + "oncoming_slow"), + ], + triggers=[ + TriggerSpec(kind=TriggerKind.TIME, value=0.0, name="start"), + TriggerSpec(kind=TriggerKind.TIME, value=0.0, name="slow_start"), + TriggerSpec(kind=TriggerKind.TIME, value=0.0, name="oncoming_start"), + TriggerSpec(kind=TriggerKind.TIME, value=lc_delay, + name="begin_intrusion"), + TriggerSpec(kind=TriggerKind.RELATIVE_DISTANCE, rule=Rule.LESS_THAN, + value=brake_gap, entity_ref="oncoming", + triggering_entity="ego", name="ego_brake_trigger"), + TriggerSpec(kind=TriggerKind.RELATIVE_DISTANCE, rule=Rule.LESS_THAN, + value=abort_gap, entity_ref="oncoming", + triggering_entity="ego", name="abort_trigger"), + TriggerSpec(kind=TriggerKind.RELATIVE_DISTANCE, rule=Rule.LESS_THAN, + value=abort_gap, entity_ref="oncoming", + triggering_entity="ego", name="abort_slow_trigger"), + ], + duration=duration, template="oncoming_intrusion", + params={"s0": s0, "road_id": road_id, "lane": lane, + "ego_speed": ego_speed, "slow_speed": slow_speed, + "slow_gap": slow_gap, "oncoming_speed": oncoming_speed, + "oncoming_gap": oncoming_gap, "lc_delay": lc_delay, + "brake_gap": brake_gap, "abort_gap": abort_gap, + "abort_time": abort_time}, + tags=["对向借道", "迎面冲突", "城市道路", "制动"], + ) + + # ===================================================================== # 模板 11:紧急避让(轨迹跟随) # 前方本车道有静止障碍,自车按一条 S 形轨迹换道绕行。 @@ -1002,6 +1088,7 @@ def build_signal_phase(p: Dict[str, Any]) -> ScenarioSpec: "close_follow": build_close_follow, "overtake": build_overtake, "overtake_oncoming": build_overtake_oncoming, + "oncoming_intrusion": build_oncoming_intrusion, "evasive_swerve": build_evasive_swerve, "signal_phase": build_signal_phase, } @@ -1208,6 +1295,35 @@ def build_signal_phase(p: Dict[str, Any]) -> ScenarioSpec: "abort_gap": ParamRange(kind="uniform", low=55.0, high=70.0, unit="m"), }), + "oncoming_intrusion": TemplateSpec( + name="oncoming_intrusion", title="对向借道侵入本车道", + description="对向车借道超车越过中心线迎面驶来,自车制动、来车退回" + "(Town03 road 67 城市双向路段)", + carla_function="scenario_wrong_way", tags=["对向借道", "迎面冲突", "城市"], + required_map="Town03", + params={ + # 174 起为 136m 直线段(s<170 处道路开始转弯,自车恒速会偏出 + # 本车道),终点距下一路口仍有余量。 + "s0": ParamRange(kind="fixed", value=174.0, unit="m"), + "road_id": ParamRange(kind="string", value="67"), + "lane": ParamRange(kind="choice", choices=["-1"]), + "ego_speed": ParamRange(kind="uniform", low=11.0, high=13.0, + unit="m/s"), + "slow_speed": ParamRange(kind="uniform", low=5.0, high=8.0, + unit="m/s"), + "slow_gap": ParamRange(kind="uniform", low=40.0, high=55.0, + unit="m"), + "oncoming_speed": ParamRange(kind="uniform", low=11.0, high=13.0, + unit="m/s"), + "oncoming_gap": ParamRange(kind="fixed", value=95.0, unit="m"), + # 退避触发必须晚于侵入完成(lc_delay + lc_time),否则对向车 + # 尚未跨过中心线,此时"退回"会把本车道当作目标再次驶入。 + "lc_delay": ParamRange(kind="fixed", value=0.5, unit="s"), + "brake_gap": ParamRange(kind="uniform", low=65.0, high=75.0, + unit="m"), + "abort_gap": ParamRange(kind="fixed", value=40.0, unit="m"), + "abort_time": ParamRange(kind="fixed", value=1.2, unit="s"), + }), "signal_phase": TemplateSpec( name="signal_phase", title="红绿灯相位", description="红灯等待,信号切绿后起步通过(TrafficSignalCondition + 动作)", diff --git a/llm/src/xosc_player.py b/llm/src/xosc_player.py index af8e386..d073fa8 100644 --- a/llm/src/xosc_player.py +++ b/llm/src/xosc_player.py @@ -1300,15 +1300,45 @@ def keep_lateral(actor, other, target: float, speed: float) -> None: brake=brake)) +def _lane_at(world, road_id, s: float, loc): + """在 (road, s) 处按车道中心与车辆的实际横向距离判定所在车道。 + + CARLA 的部分城市路段上 ``get_waypoint(location)`` 会把车辆识别到相邻 + 车道(实测 Town03 road 67 的 +1 车道中心返回 +2),直接拿它的 + ``get_left_lane`` 会指向本车道,变道动作因此无效。 + """ + cmap = world.get_map() + best, best_d = None, 1e9 + for lid in range(-4, 5): + if lid == 0: + continue + w = cmap.get_waypoint_xodr(int(float(road_id)), lid, s) + if w is None: + continue + d = w.transform.location.distance(loc) + if d < best_d: + best, best_d = w, d + return best + + def _lane_target(actor, delta: int, lookahead: float = 8.0, road_id: Optional[int] = None, - lane_id: Optional[int] = None + lane_id: Optional[int] = None, + look_back: bool = False ) -> Optional[carla.Location]: """取相邻车道上、前方 lookahead 米处的位置。 给定 ``road_id`` / ``lane_id`` 时按**固定的目标车道**取点。变道过程中 车辆当前所在车道会从原车道切到目标车道,如果每帧都重新找"相邻车道", 目标会再往外跳一条,车就会连续跨两条车道(实测横向偏出 5.1 m)。 + + ``look_back`` 表示沿目标车道**反向**取点:对向车借道时其行驶方向与 + 目标车道名义方向相反,必须用 ``previous`` 取点;否则前瞻点落在车后 + (fx < 0),纯追踪转向量恒为 0,车不会并入目标车道。 + + 目标点按 ``(roadId, laneId, 当前站号)`` 解析,而不是用车辆位置反查 + 车道:部分城市路段 ``get_waypoint(location)`` 会误判到相邻车道 + (实测 Town03 road 67 上 +1 车道中心被识别为 +2)。 """ try: world = actor.get_world() @@ -1316,11 +1346,15 @@ def _lane_target(actor, delta: int, lookahead: float = 8.0, if wp is None: return None if road_id is not None and lane_id is not None: - tgt = world.get_map().get_waypoint_xodr( - road_id, lane_id, wp.s + lookahead) - if tgt is None: - tgt = world.get_map().get_waypoint_xodr(road_id, lane_id, wp.s) - return tgt.transform.location if tgt is not None else None + lane = world.get_map().get_waypoint_xodr( + int(float(road_id)), int(float(lane_id)), wp.s) + if lane is None: + return None + ahead = (lane.previous(lookahead) if look_back + else lane.next(lookahead)) + if ahead: + return ahead[0].transform.location + return lane.transform.location if delta > 0: lane = wp.get_left_lane() else: @@ -1616,10 +1650,24 @@ def _on_collision(event, _m=metrics, _c=clock, # 选"相邻车道"导致跨两条车道。 wp0 = world.get_map().get_waypoint( actor.get_transform().location) - lane0 = None + cur = None if wp0 is not None: - lane0 = (wp0.get_left_lane() if delta > 0 - else wp0.get_right_lane()) + cur = _lane_at(world, wp0.road_id, wp0.s, + actor.get_transform().location) + if cur is None: + cur = wp0 + lane0 = None + if cur is not None: + lane0 = (cur.get_left_lane() if delta > 0 + else cur.get_right_lane()) + # 目标点沿车道名义方向还是反向取:负号车道名义朝 + # +s,正号车道名义朝 -s;车若逆行(如对向车借道), + # 前瞻点要沿车道反向取,否则目标落在车后。 + look_back = False + if lane0 is not None: + lf = lane0.transform.get_forward_vector() + af = actor.get_transform().get_forward_vector() + look_back = (lf.x * af.x + lf.y * af.y) < 0 lane_change[ev.entity] = { "delta": delta, "until": t + max(act.dynamics_value, 1.0), @@ -1628,8 +1676,9 @@ def _on_collision(event, _m=metrics, _c=clock, # 借对向车道超车时,目标车道的名义朝向与自车 # 相反;收尾对齐要保留原行驶方向,否则会掉头。 "yaw": actor.get_transform().rotation.yaw, - "road_id": wp0.road_id if wp0 is not None else None, + "road_id": cur.road_id if cur is not None else None, "lane_id": lane0.lane_id if lane0 is not None else None, + "back": look_back, } elif act.kind == "lane_offset": lane_off[ev.entity] = { @@ -1722,7 +1771,8 @@ def _on_collision(event, _m=metrics, _c=clock, lc = lane_change[name] tgt = _lane_target(actor, lc["delta"], road_id=lc["road_id"], - lane_id=lc["lane_id"]) + lane_id=lc["lane_id"], + look_back=lc.get("back", False)) if tgt is None: errors.append("%s 找不到相邻车道" % name) lc["done"] = True @@ -1736,7 +1786,8 @@ def _on_collision(event, _m=metrics, _c=clock, # 若退回恒速,残余偏角会让车斜着漂出车道。 if lc["lane_id"] is not None: lane_hold[name] = {"road_id": lc["road_id"], - "lane_id": lc["lane_id"]} + "lane_id": lc["lane_id"], + "back": lc.get("back", False)} continue if name in lane_off and not lane_off[name]["done"]: lo = lane_off[name] @@ -1787,7 +1838,8 @@ def _on_collision(event, _m=metrics, _c=clock, if name in lane_hold and actor.type_id.startswith("vehicle."): lh = lane_hold[name] tgt = _lane_target(actor, 0, road_id=lh["road_id"], - lane_id=lh["lane_id"]) + lane_id=lh["lane_id"], + look_back=lh.get("back", False)) if tgt is not None: _drive_towards(actor, tgt, target_speed.get(name, 0.0), _speed_of(actor))