Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 15 additions & 0 deletions llm/src/scenario_intents.py
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,14 @@
"examples": ["借对向车道超车时对面有车驶来,只好放弃超车",
"超车到一半发现对向来车,赶紧回到原车道"],
},
"oncoming_intrusion": {
"aliases": ["对向车借道", "对向借道侵入", "对向车侵入", "对向车占道",
"对面车占道", "迎面占道", "越过中心线", "越线迎面",
"对向车跑到我车道", "对向车迎面驶来", "迎面对撞风险",
"占了我的车道", "占我车道", "对面来车占道"],
"examples": ["对向车借道超车,越过中心线朝我迎面开过来",
"对面来车占了我的车道,只能紧急刹车"],
},
"signal_phase": {
"aliases": ["红绿灯", "信号配时", "绿灯起步", "黄灯", "红灯停车",
"信号灯变化"],
Expand Down Expand Up @@ -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),
Expand All @@ -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,
Expand Down
116 changes: 116 additions & 0 deletions llm/src/scenario_templates.py
Original file line number Diff line number Diff line change
Expand Up @@ -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 形轨迹换道绕行。
Expand Down Expand Up @@ -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,
}
Expand Down Expand Up @@ -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 + 动作)",
Expand Down
78 changes: 65 additions & 13 deletions llm/src/xosc_player.py
Original file line number Diff line number Diff line change
Expand Up @@ -1300,27 +1300,61 @@ 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()
wp = world.get_map().get_waypoint(actor.get_transform().location)
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:
Expand Down Expand Up @@ -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),
Expand All @@ -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] = {
Expand Down Expand Up @@ -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
Expand All @@ -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]
Expand Down Expand Up @@ -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))
Expand Down
Loading