chassis-sim 0.2.0__py3-none-any.whl

chassis_sim/__init__.py

@@ -0,0 +1,12 @@
+"""chassis-sim：三种底盘的 Gymnasium 环境。
+
+import 本包即注册三个 id（ChassisSim-Ackermann-v0 / ChassisSim-DiffDrive-v0 /
+ChassisSim-Mecanum-v0）。注册逻辑在 gym_registration.py，只依赖 gymnasium：
+环境本体按 entry_point 字符串延迟 import，不拉起 numba / matplotlib / torch。
+"""
+
+from . import gym_registration as _gym_registration  # noqa: F401 - 注册副作用
+
+__version__ = "0.2.0"
+
+__all__ = ["__version__"]

chassis_sim/bridges/__init__.py

@@ -0,0 +1,34 @@
+"""纯 numpy 的传感器桥（`SensorProvider` 实现）。
+
+对外暴露：抽象接口 `SensorProvider`、实现 `SimpleBridge`、把归一化动作接到吃
+物理命令的外部模拟器上的适配器 `PhysicalCommandBridge`，以及扫描通路的两个
+入口 `get_scan` / `scan_to_pointcloud`（后者原本就在 base.py 的模块层）。
+`raycast` 里的两个下划线函数是实现细节，不在此转发（避免把私有符号变成公共 API）。
+
+`get_scan` 在 `SensorProvider` 上是**方法**（基类实现直接抛 NotImplementedError，
+由 `SimpleBridge` 覆盖），这里转成一个模块级入口，这样
+`from chassis_sim.bridges import get_scan, scan_to_pointcloud` 取到的是一对函数，
+不必先拿到具体桥的类。转发而非别名：别名会绑到基类那个抛异常的实现上。
+"""
+
+from __future__ import annotations
+
+from typing import Any
+
+from .adapters import PhysicalCommandBridge
+from .base import SensorProvider, scan_to_pointcloud
+from .simple_bridge import SimpleBridge
+
+
+def get_scan(bridge: SensorProvider, side: str = "front") -> dict[str, Any]:
+    """转调 `bridge.get_scan(side)`（模块级入口；语义与实现完全由桥决定）。"""
+    return bridge.get_scan(side)
+
+
+__all__ = [
+    "PhysicalCommandBridge",
+    "SensorProvider",
+    "SimpleBridge",
+    "get_scan",
+    "scan_to_pointcloud",
+]

chassis_sim/bridges/adapters.py

@@ -0,0 +1,200 @@
+"""把"吃物理命令"的模拟器接到归一化 action 的 env 契约上。
+
+env 的分工是"env 下发归一化动作、桥把它变成物理量再推演"：
+`envs/chassis_env.py:1294` 只把 `act` 交给 `bridge.step(act)`，映射由桥负责。
+本仓自带的 `SimpleBridge` 一步做完；但外部模拟器（ir-sim、ROS gazebo、真车
+接口，以及主仓 `ros_bridge/numpy_bridge.py` 的 `NumpySimBridge`）只吃物理量。
+直接挂上去，归一化动作会被当成物理量吃掉。
+
+`PhysicalCommandBridge` 补上缺的那一段：
+
+    归一化 action --chassis.physical_action()--> 物理向量
+
+三条约定：
+
+* **映射走 `Chassis.physical_action()`**（纯函数，不读也不写执行器状态）。
+  `AckermannChassis.step()` 带一拍转向执行器滞后（`_prev_delta`），而**内层
+  物理模拟器自己还有一次**；适配器再用 `step()` 会让滞后施加两次，整车响应
+  慢一拍。
+* **向量顺序由车型决定**（`chassis/base.py` 的 `physical_action`）：
+  ackermann `[delta(rad), v(m/s)]`、diff_drive `[v(m/s), omega(rad/s)]`、
+  mecanum `[v(m/s), vy(m/s), omega(rad/s)]`。阿克曼的 delta 由底盘从
+  `_map_action()` 直接给出，**不**再做 `arctan(omega*L/v)` 反解：反解在
+  |v| < 1e-6 时无定义（只能沿用上一拍，成了跨步状态），200 例随机动作里还有
+  64 例差 1 ulp。
+* **执行器滞后只由内层模拟器施加恰好一次**：适配器不推进底盘状态、不调
+  `chassis.step()`、也不调 `chassis.reset_state()`。内层不带滞后时，它与
+  `SimpleBridge` 的位姿等价性会从 1.5e-15 崩到 9.76e-01。
+"""
+
+from __future__ import annotations
+
+from typing import Any
+
+import numpy as np
+
+from chassis_sim.chassis import Chassis
+
+from .base import SensorProvider
+
+
+class PhysicalCommandBridge(SensorProvider):
+    """把"吃物理命令的模拟器"接到归一化 action 的 env 契约上。
+
+    归一化 action --chassis.physical_action()--> 物理向量 --inner.step()--> 物理桥
+
+    物理向量按车型（`Chassis.physical_action()`）：
+    ackermann `[delta, v]`、diff_drive `[v, omega]`、mecanum `[v, vy, omega]`。
+
+    除了 `step` / `reset` / `set_dynamic_obstacles` / `get_sensor_reading`，
+    其余成员都是**逐字透传**（同一个返回对象，不拷贝、不重建）：适配器只改动作
+    通路，不改传感器通路。`get_sensor_reading` 多一层可选能力判定：内层没有
+    该方法时返回 `None`（详见该方法），因为"没有传感器"是合法状态，而不是错误。
+
+    Attributes:
+        inner: 内层物理桥；每次 `step` 收到一个 float64 ndarray，顺序见上。
+        chassis: 归一化动作的映射器。env 注入桥时要求
+            `bridge.chassis is env.chassis`（`envs/chassis_env.py:438`），所以
+            这里保存调用方给的**同一个实例**，不自己 new 一个。
+    """
+
+    def __init__(self, inner: SensorProvider, chassis: Chassis) -> None:
+        """绑定内层物理桥与动作映射器。
+
+        Args:
+            inner: 吃物理命令的桥（ir-sim / gazebo / NumpySimBridge 之类）。
+                只要求它实现 `SensorProvider` 的契约，不要求是本包类型；它
+                收到的向量顺序必须与 `chassis.physical_action()` 一致。
+            chassis: 与 env 共享的底盘实例，只用来做归一化 -> 物理的映射。
+        """
+        self.inner = inner
+        self.chassis = chassis
+
+    # ── 动作通路：归一化 action -> 物理向量 ──────────────────────────────
+    def to_physical(self, action: np.ndarray) -> np.ndarray:
+        """归一化动作 -> 内层桥要的物理向量（float64，长度 = action_dim）。
+
+        顺序由车型决定：ackermann `[delta, v]`、diff_drive `[v, omega]`、
+        mecanum `[v, vy, omega]`。**纯函数**：同一 action 永远得到逐位相同的
+        向量，与底盘执行器状态（阿克曼的 `_prev_delta`）无关。单独暴露是为了
+        让使用方与测试能直接看一条动作对应的物理命令，而不必先推进仿真。
+        """
+        return np.asarray(self.chassis.physical_action(action), dtype=np.float64)
+
+    def step(self, action: np.ndarray) -> None:
+        """把归一化动作映射成物理向量，转交内层桥（积分在内层发生）。"""
+        self.inner.step(self.to_physical(action))
+
+    def reset(
+        self,
+        obstacles: list | None = None,
+        path_waypoints: np.ndarray | None = None,
+        initial_heading: float | None = None,
+    ) -> None:
+        """复位内层桥（适配器自身无状态，只转发）。
+
+        刻意**不**调用 `chassis.reset_state()`：适配器从不调用 `chassis.step()`，
+        底盘的执行器状态（`_prev_delta`）对纯函数映射没有影响，而不存在的那次
+        状态推进也就不需要复位。位姿与执行器状态由 env / 内层桥各自负责。
+
+        Args:
+            obstacles: 原样转发给内层桥。
+            path_waypoints: 原样转发给内层桥。
+            initial_heading: 原样转发给内层桥。
+        """
+        self.inner.reset(
+            obstacles=obstacles,
+            path_waypoints=path_waypoints,
+            initial_heading=initial_heading,
+        )
+
+    # ── 传感器与判定：逐字透传 ──────────────────────────────────────────
+    def get_lidar_front(self) -> np.ndarray:
+        """透传内层桥的前向扫描。"""
+        return self.inner.get_lidar_front()
+
+    def get_lidar_rear(self) -> np.ndarray:
+        """透传内层桥的后向扫描。"""
+        return self.inner.get_lidar_rear()
+
+    def get_odometry(self) -> np.ndarray:
+        """透传内层桥的里程计 `[x, y, theta, v, omega]`。"""
+        return self.inner.get_odometry()
+
+    def get_path(self) -> np.ndarray:
+        """透传内层桥的车体系路径 `(N, 3)`。"""
+        return self.inner.get_path()
+
+    def get_path_global(self) -> np.ndarray:
+        """透传内层桥的世界系路径 `(N, 3)`。"""
+        return self.inner.get_path_global()
+
+    def check_collision(self) -> bool:
+        """透传内层桥的碰撞判定。"""
+        return self.inner.check_collision()
+
+    def check_goal_reached(self, threshold: float = 0.3) -> bool:
+        """透传内层桥的到点判定。
+
+        Args:
+            threshold: 到点距离阈值（米），原样转发。
+        """
+        return self.inner.check_goal_reached(threshold)
+
+    def close(self) -> None:
+        """透传内层桥的资源释放。"""
+        self.inner.close()
+
+    @property
+    def goal_heading(self) -> float:
+        """透传内层桥的目标期望朝向。"""
+        return self.inner.goal_heading
+
+    def get_scan(self, side: str = "front") -> dict[str, Any]:
+        """透传内层桥的扫描字典（`side` 原样转发）。
+
+        Args:
+            side: 传感器侧别，取值由内层桥决定。
+        """
+        return self.inner.get_scan(side)
+
+    def get_sensor_reading(self) -> dict[str, Any] | None:
+        """转发内层桥的可选传感器读数；内层不支持该能力时返回 `None`。
+
+        传感器是**可选能力**（与 `set_dynamic_obstacles` 的"必须响亮报错"
+        正相反）：内层桥压根没有这个方法（主仓 NumpySimBridge / ir-sim
+        SimBridge 就是这样），或只继承了 `SensorProvider` 的非抽象默认实现，
+        都视为"未启用可选传感器"，返回 `None` 而不是抛 `AttributeError` ——
+        否则 env 侧 `getattr` 兜底（`envs/chassis_env.py:1735`）会被本类的
+        具体方法架空，第一步就崩。
+
+        Returns:
+            内层读数（`None` 也照原样返回），或内层不支持时的 `None`。
+        """
+        impl = getattr(self.inner, "get_sensor_reading", None)
+        if impl is None or (
+            getattr(impl, "__func__", None) is SensorProvider.get_sensor_reading
+        ):
+            return None
+        return impl()
+
+    def set_dynamic_obstacles(self, specs: list | None = None) -> None:
+        """转发给内层桥；内层不支持时**响亮地**报错，不静默跳过。
+
+        Args:
+            specs: 动态障碍规格列表（`None` / `[]` = 清空），原样转发。
+
+        Raises:
+            NotImplementedError: 内层桥只实现了静态场景（没有覆盖
+                `SensorProvider.set_dynamic_obstacles`）。
+        """
+        impl = getattr(self.inner, "set_dynamic_obstacles", None)
+        if impl is None or (
+            getattr(impl, "__func__", None) is SensorProvider.set_dynamic_obstacles
+        ):
+            raise NotImplementedError(
+                f"内层桥 {type(self.inner).__name__} 不支持动态障碍"
+                "（dynamic_obstacles）：适配器只做转发，不自己维护障碍物；"
+                "请改用支持动态障碍的内层桥"
+            )
+        impl(specs)

chassis_sim/bridges/base.py

@@ -0,0 +1,217 @@
+"""SensorProvider 抽象接口 —— 与主仓 ros_bridge/sensor_provider.py 逐字同构。
+
+签名（9 个抽象方法 + 1 个抽象属性）按字节照抄
+`/home/zhao/drl-sim/ros_bridge/sensor_provider.py`，只把英文 docstring 译成中文：
+消费方（envs / 控制器 / 回放脚本）按位置或关键字调用这些方法，任何签名漂移都会让
+旧桥与新桥不可互换。
+
+实现者：
+    SimpleBridge —— 本包纯 numpy 的射线投射实现（bridges/simple_bridge.py）
+    旧仓 SimBridge / ROS2Bridge / MockBridge —— 同签名，可直接替换
+
+在上面的**抽象**接口之外，本模块还提供两项**附加（非抽象）**能力，供需要 ROS
+式扫描元数据的消费方使用；不实现它们也不影响已有桥的可用性：
+
+    SensorProvider.get_scan() —— 单侧扫描字典（字段见 SCAN_FIELDS，对齐 ROS LaserScan）
+    scan_to_pointcloud()      —— 扫描字典 -> 世界系 (N, 2) float64 点云
+"""
+
+from __future__ import annotations
+
+from abc import ABC, abstractmethod
+from collections.abc import Mapping
+from typing import Any
+
+import numpy as np
+
+# ROS sensor_msgs/LaserScan 的十个字段，**顺序即字典键序**（消费方按 ROS 顺序读）。
+SCAN_FIELDS: tuple[str, ...] = (
+    "angle_min",
+    "angle_max",
+    "angle_increment",
+    "time_increment",
+    "scan_time",
+    "range_min",
+    "range_max",
+    "ranges",
+    "intensities",
+    "frame_id",
+)
+
+
+class SensorProvider(ABC):
+    """传感器数据与动作下发的抽象接口。"""
+
+    @abstractmethod
+    def reset(
+        self,
+        obstacles: list | None = None,
+        path_waypoints: np.ndarray | None = None,
+        initial_heading: float | None = None,
+    ) -> None:
+        """复位仿真状态。
+
+        Args:
+            obstacles: 可选的障碍物 kwargs 列表（动态创建用）。
+            path_waypoints: 可选的 (N, 2) 全局路径航点。
+            initial_heading: 可选的起始车头朝向（弧度）。
+        """
+        ...
+
+    @abstractmethod
+    def step(self, action: np.ndarray) -> None:
+        """用给定动作推进一个时间步。
+
+        Args:
+            action: [delta, v] 转向角与速度
+        """
+        ...
+
+    @abstractmethod
+    def get_lidar_front(self) -> np.ndarray:
+        """返回前向 LiDAR 扫描：[180] 个距离（米）。"""
+        ...
+
+    @abstractmethod
+    def get_lidar_rear(self) -> np.ndarray:
+        """返回后向 LiDAR 扫描：[180] 个距离（米）。"""
+        ...
+
+    @abstractmethod
+    def get_odometry(self) -> np.ndarray:
+        """返回里程计：[x, y, theta, v, omega]。"""
+        ...
+
+    @abstractmethod
+    def get_path(self) -> np.ndarray:
+        """返回车体系下的全局路径航点：[N, 3] (dx, dy, dtheta)。"""
+        ...
+
+    @abstractmethod
+    def get_path_global(self) -> np.ndarray:
+        """返回世界系下**全部**路径航点，形状 (N, 3) [x, y, θ]。"""
+        ...
+
+    @abstractmethod
+    def check_collision(self) -> bool:
+        """机器人与任何障碍物重叠时返回 True。"""
+        ...
+
+    @abstractmethod
+    def check_goal_reached(self, threshold: float = 0.3) -> bool:
+        """机器人进入末航点 threshold 以内且朝向匹配时返回 True。"""
+        ...
+
+    @abstractmethod
+    def close(self) -> None:
+        """释放资源。"""
+        ...
+
+    @property
+    @abstractmethod
+    def goal_heading(self) -> float:
+        """目标点的期望朝向（用于终端奖励中的朝向对齐计算）。"""
+        ...
+
+    # ── 附加（非抽象）API：动态障碍 ────────────────────────────────────
+    def set_dynamic_obstacles(self, specs: list | None = None) -> None:
+        """换一组动态障碍（`None` / `[]` = 清空）。**非抽象**：不实现即视为不支持。
+
+        Raises:
+            NotImplementedError: 该桥不支持动态障碍（构造它的模拟器只实现了静态
+                场景）。这里**故意响亮地报错**而不是静默跳过 —— 一个被静默忽略的
+                动态障碍会让"碰撞没发生"看起来像策略变好了。
+        """
+        raise NotImplementedError(
+            f"{type(self).__name__} 不支持动态障碍（dynamic_obstacles）："
+            "该桥只实现了静态场景；请改用支持动态障碍的桥（如 SimpleBridge）"
+        )
+
+    # ── 附加（非抽象）API：ROS 式扫描元数据 ────────────────────────────
+    def get_scan(self, side: str = "front") -> dict[str, Any]:
+        """返回带元数据的单侧扫描字典（**非抽象**：不实现即视为不支持）。
+
+        Returns:
+            含 SCAN_FIELDS 十个键的字典，键序与 ROS sensor_msgs/LaserScan 一致：
+            angle_min, angle_max, angle_increment, time_increment, scan_time,
+            range_min, range_max, ranges, intensities, frame_id。
+
+        角度约定（实现方必须遵守，scan_to_pointcloud 依赖它）：
+            第 i 条射线的方位角 = angle_min + i * angle_increment，单位为弧度、
+            以**车头**为 0；且
+            angle_increment == (angle_max - angle_min) / (len(ranges) - 1)。
+            即 angle_max 是**最后一条射线**的方位角（不是"末尾再加一个步长"），
+            与 np.linspace(angle_min, angle_max, len(ranges)) 端点包含的语义一致。
+
+        数值约定：ranges 必须是 float32 一维数组；**无返回/超出量程的射线用
+        +inf** 表示，不得用 0 或 range_max 冒充有效测距（scan_to_pointcloud 依赖
+        np.isfinite 丢弃这些射线）。intensities 允许为 None（不建模回波强度）。
+
+        Args:
+            side: 传感器侧别，实现方各自的合法取值。
+
+        Raises:
+            NotImplementedError: 本实现不提供扫描元数据。
+            ValueError: side 不是合法取值（消息里必须列出合法值）。
+        """
+        raise NotImplementedError(
+            f"{type(self).__name__} 未实现 get_scan()；只有提供扫描元数据的桥才支持"
+        )
+
+    # ── 附加（非抽象）API：可选传感器读数 ──────────────────────────────
+    def get_sensor_reading(self) -> dict[str, Any] | None:
+        """返回当前步的可选传感器读数（**非抽象**：不实现即视为不启用）。
+
+        读数**默认只进 info**：obs 的维度由 ObsSpec 决定（0.2.0 起默认是
+        odom+path+goal+act 的 128 维），与是否启用传感器无关；只有 obs_spec 里
+        用 lidar 段显式引用了某个传感器名，它的 scan 才会进观测。
+
+        Returns: dict[传感器名, 读数]，每个读数本身是 dict，实现方只放已启用的项：
+
+        * Lidar:     {"scan": (n_rays,) float32 或 None（延迟预热期 / 丢帧）,
+                      "scan_meta": {name, mount, n_rays, fov, angle_min, ...}}
+        * Imu:       {ax, ay, yaw_rate, yaw, t}
+        * WheelOdom: {v_left, v_right, v, t}
+
+        Returns:
+            dict[str, Any]，或 None（未启用任何可选传感器；此时 env 的 info 不新增
+            任何键，信息路径与加入该接口前逐位相同）。
+        """
+        return None
+
+
+def scan_to_pointcloud(
+    scan: Mapping[str, Any],
+    origin: tuple[float, float] = (0.0, 0.0),
+    heading: float = 0.0,
+) -> np.ndarray:
+    """把 get_scan() 的字典转换为世界系点云 (N, 2) float64。
+
+    第 i 条射线的方位角按 angle_min + i * angle_increment 还原，它是**相对车头**
+    的 bearing；世界系方向 = heading + bearing。
+
+    Args:
+        scan: 至少含 angle_min / angle_increment / ranges 的扫描字典。
+        origin: 传感器在**世界系**的 (x, y) 位置（米）；注意不是车体后轴中心，
+            前/后雷达各有安装偏移，需要由调用方叠加。
+        heading: 传感器在**世界系**的朝向（弧度），即车头朝向。
+
+    Returns:
+        (N, 2) float64 世界系坐标。ranges 里的非有限值（inf/nan，即"无返回"）
+        对应的射线整条丢弃，因此返回行数 <= len(ranges)；全部无返回时为 (0, 2)。
+    """
+    ranges = np.asarray(scan["ranges"], dtype=np.float64).reshape(-1)
+    angle_min = float(scan["angle_min"])
+    angle_increment = float(scan["angle_increment"])
+    angles = angle_min + np.arange(ranges.size, dtype=np.float64) * angle_increment
+    angles = angles + float(heading)
+
+    finite = np.isfinite(ranges)
+    if not np.any(finite):
+        return np.zeros((0, 2), dtype=np.float64)
+
+    r = ranges[finite]
+    a = angles[finite]
+    ox = float(origin[0])
+    oy = float(origin[1])
+    return np.column_stack([ox + r * np.cos(a), oy + r * np.sin(a)])

chassis_sim/bridges/raycast.py

@@ -0,0 +1,124 @@
+"""模块级 ray-casting 辅助函数 —— 从 numpy_bridge.py:30-132 原样移植。
+
+两个函数都是"射线 × 障碍物"维度上的全向量化实现（没有单条射线的 Python 循环），
+公式一字未改：圆的解析二次方程、矩形的"世界→局部"变换 + slab test。
+
+⚠ 保留的两条旧坑（注释即代码的一部分，别删）：
+
+* `_ray_rects_intersection` 里的世界→局部变换用 **R(−θ)**
+  （`l = (c·dx + s·dy, −s·dx + c·dy)`）。旧实现曾写成 cos(−θ)/sin(−θ) 搭配
+  转置形式，两个取负相互抵消等于 R(+θ)，把矩形镜像成 −θ 朝向 —— 非轴对齐时
+  约 90% 射线量错。
+* slab test 的退化分支必须保留 `parallel_x/parallel_y` 与 `out_x/out_y`：
+  方向分量为 0 时 `t = ±inf` 会污染 `t_min`/`t_max` 的比较。
+"""
+
+from __future__ import annotations
+
+import numpy as np
+
+
+def _ray_circles_intersection(
+    origins: np.ndarray,  # (R, 2)
+    directions: np.ndarray,  # (R, 2)  单位向量
+    centers: np.ndarray,  # (C, 2)
+    radii: np.ndarray,  # (C,)
+    max_range: float,
+) -> np.ndarray:  # (R,)
+    """每条射线到最近圆形交点的距离。
+
+    对每一对 (射线, 圆) 使用 broadcasting 求解 |origin + t·direction − center|² = radius²，
+    然后取每条射线的最小 *t*。
+    """
+    # to_center: (R, 1, 2) − (1, C, 2) → (R, C, 2)
+    to_center = origins[:, None, :] - centers[None, :, :]
+
+    # b = 2·(S−C)·D   → (R, C)
+    b = 2.0 * np.sum(to_center * directions[:, None, :], axis=-1)
+
+    # c = |S−C|² − r²   → (R, C)
+    c = np.sum(to_center * to_center, axis=-1) - radii[None, :] ** 2
+
+    disc = b * b - 4.0 * c  # (R, C)
+    sqrt_disc = np.sqrt(np.maximum(disc, 0.0))
+
+    t1 = (-b - sqrt_disc) * 0.5
+    t2 = (-b + sqrt_disc) * 0.5
+
+    # 每对 (射线, 圆) 的最小正 t
+    t = np.where((t1 > 0.0) & (disc >= 0.0), t1, np.inf)
+    t = np.where((t2 > 0.0) & (disc >= 0.0) & (t2 < t), t2, t)
+
+    t_min = np.min(t, axis=1)  # (R,)
+    t_min = np.where(np.isfinite(t_min), t_min, max_range)
+    return np.clip(t_min, 0.0, max_range)
+
+
+def _ray_rects_intersection(
+    origins: np.ndarray,  # (R, 2)
+    directions: np.ndarray,  # (R, 2)  单位向量
+    centers: np.ndarray,  # (M, 2)
+    half_dims: np.ndarray,  # (M, 2)  [半长, 半宽]
+    thetas: np.ndarray,  # (M,)    矩形在世界坐标系中的朝向
+    max_range: float,
+) -> np.ndarray:  # (R,)
+    """每条射线到最近矩形交点的距离。
+
+    将每条射线转换到每个矩形的局部坐标系（AABB），然后应用标准的 slab test。
+    在射线 × 矩形维度上完全向量化。
+    """
+    # ── 将射线起点转换到每个矩形的局部坐标系 ──────────────
+    # 世界→局部 需要 R(−θ)：l = (c·dx + s·dy, −s·dx + c·dy)，c=cosθ, s=sinθ。
+    # ⚠ 修复 (2026-08-17): 旧代码写成 cos(−θ)/sin(−θ) 搭配 R 的转置形式，
+    # 两个取负相互抵消等于 R(+θ) —— 即把矩形镜像成 −θ 朝向，
+    # 导致 LiDAR 看到的矩形与 env SAT 碰撞/真实几何不一致（非轴对齐时 ~90% 射线错误）。
+    cos_t = np.cos(thetas)  # (M,)
+    sin_t = np.sin(thetas)
+
+    local_o = origins[:, None, :] - centers[None, :, :]  # (R, M, 2)
+    local_ox = cos_t[None, :] * local_o[:, :, 0] + sin_t[None, :] * local_o[:, :, 1]
+    local_oy = -sin_t[None, :] * local_o[:, :, 0] + cos_t[None, :] * local_o[:, :, 1]
+
+    # ── 转换射线方向 ──────────────────────────────────────
+    local_dx = (
+        cos_t[None, :] * directions[:, None, 0]
+        + sin_t[None, :] * directions[:, None, 1]
+    )
+    local_dy = (
+        -sin_t[None, :] * directions[:, None, 0]
+        + cos_t[None, :] * directions[:, None, 1]
+    )
+
+    hl = half_dims[None, :, 0]  # (1, M)
+    hw = half_dims[None, :, 1]  # (1, M)
+
+    with np.errstate(divide="ignore", invalid="ignore"):
+        # ── slab test: x 轴 ─────────────────────────────────────
+        tx1 = (-hl - local_ox) / local_dx
+        tx2 = (hl - local_ox) / local_dx
+        tx_min = np.minimum(tx1, tx2)
+        tx_max = np.maximum(tx1, tx2)
+
+        # ── slab test: y 轴 ─────────────────────────────────────
+        ty1 = (-hw - local_oy) / local_dy
+        ty2 = (hw - local_oy) / local_dy
+        ty_min = np.minimum(ty1, ty2)
+        ty_max = np.maximum(ty1, ty2)
+
+    t_min = np.maximum(tx_min, ty_min)
+    t_max = np.minimum(tx_max, ty_max)
+
+    # ── 退化情况 ──────────────────────────────────────────
+    eps = 1e-10
+    parallel_x = np.abs(local_dx) < eps
+    parallel_y = np.abs(local_dy) < eps
+    out_x = parallel_x & ((local_ox < -hl) | (local_ox > hl))
+    out_y = parallel_y & ((local_oy < -hw) | (local_oy > hw))
+
+    t = np.where(t_min > 0.0, t_min, t_max)
+    valid = (t_min <= t_max) & (t_max > 0.0) & ~out_x & ~out_y
+    t = np.where(valid, t, np.inf)
+
+    t_min_ray = np.min(t, axis=1)
+    t_min_ray = np.where(np.isfinite(t_min_ray), t_min_ray, max_range)
+    return np.clip(t_min_ray, 0.0, max_range)