commit ba24d3fc1aee0a972d782e029e66df649d119055 Author: li-shihao-code <2469171725@qq.com> Date: Wed Feb 25 20:30:54 2026 +0800 feat:完善了自动标定车间的环境 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..9af65bd --- /dev/null +++ b/.gitignore @@ -0,0 +1,23 @@ +# Python 缓存 +__pycache__/ +*.py[cod] +*$py.class + +# 虚拟环境 (如果在这个目录下) +venv/ +env/ +miniconda3/ + +# Isaac Sim & 3D 缓存文件 +*.usd.cache +*.usda.cache +*.crash +core.* + +# ROS 2 日志与编译文件 +log/ +build/ +install/ +*.db3 +*.bag +.vscode/ diff --git a/README.md b/README.md new file mode 100644 index 0000000..0e82c72 --- /dev/null +++ b/README.md @@ -0,0 +1,202 @@ +# 🛠️ AutoCalib-Workshop: 传感器自动标定仿真车间 + +本项目基于 NVIDIA Isaac Sim 和 ROS 2，搭建了一个用于多传感器联合标定（Camera + LiDAR）的仿真车间环境。场景内置了 4 个全局机械式旋转激光雷达和标定相机，用于输出高精度的合成数据，以供标定算法和 SLAM 算法的测试与验证。 + +## 📑 目录 +1. [环境依赖](#1-环境依赖) +2. [快速开始](#2-快速开始) +3. [输出的 ROS 2 话题](#3-输出的-ros-2-话题) +4. [团队协作与 Git 配置指南 (必读)](#4-团队协作与-git-配置指南-必读) +5. [开发与分支规范](#5-开发与分支规范) + +--- + +## 1. 环境依赖 + +本项目不依赖 Omniverse Launcher，而是采用 **纯 Python (pip) 安装的 Isaac Sim**，便于团队在不同机器（或云端服务器）上快速复现环境。 + +- **操作系统:** Ubuntu 22.04 +- **ROS 2:** Humble Hawksbill +- **Python 环境:** Conda (Python 3.10) +- **仿真引擎:** Isaac Sim 4.2+ (`isaacsim` pip package) + +--- + +## 2. 快速开始 + +### 2.1 克隆代码 +> ⚠️ **注意**：如果您是本团队的开发者，请优先完成 [第 4 节的 Git 配置指南](#4-团队协作与-git-配置指南-必读)，然后再克隆代码。 + +使用 HTTPS 方式克隆（无需提前配置 SSH）： +```bash +git clone https://github.com/li-shihao-code/AutoCalib-Workshop.git +cd AutoCalib-Workshop +``` + +### 2.2 配置虚拟环境与依赖 +请确保机器上已安装 Miniconda 或 Anaconda。 + +```bash +# 创建并激活 Conda 环境 +conda create -n AutoCalib_Workshop python=3.10 -y +conda activate AutoCalib_Workshop + +# 安装 Isaac Sim 核心包 (基于 NVIDIA 官方源) +pip install isaacsim==4.2.0.2 isaacsim-extscache-physics==4.2.0.2 isaacsim-extscache-kit==4.2.0.2 isaacsim-extscache-kit-sdk==4.2.0.2 --extra-index-url https://pypi.nvidia.com + +# 安装 boto3 botocore s3transfer 功能包，将下列脚本复制到终端执行 +python -c """ +import sys +import os +import glob +import subprocess + +# 1. 动态获取当前的 Python 环境根目录 (例如 /home/nvidia/.../AutoCalib_Workshop) +env_base = sys.prefix + +# 2. 使用通配符动态搜索 isaacsim 的 pip_prebundle 目录，无视具体版本号和系统用户名 +search_pattern = os.path.join(env_base, 'lib', 'python*', 'site-packages', 'isaacsim', 'extscache', 'omni.kit.pip_archive*', 'pip_prebundle') +matches = glob.glob(search_pattern) + +if not matches: + print('❌ 错误: 在当前环境中未找到 Isaac Sim 的 pip_prebundle 缓存目录。请确认 Isaac Sim 已正确安装。') + sys.exit(1) + +prebundle_path = matches[0] +sys.path.insert(0, prebundle_path) + +try: + import botocore + ver = botocore.__version__ + print('=========================================') + print(f'[*] 成功动态定位到底层 botocore 路径: {prebundle_path}') + print(f'[*] Isaac Sim 内部的 botocore 版本为: {ver}') + print(f'[*] 正在为你安装完美匹配的 boto3=={ver} ...') + print('=========================================') + + # 3. 强制安装匹配的 boto3 + subprocess.check_call([sys.executable, '-m', 'pip', 'install', f'boto3=={ver}']) + print('[*] 🎉 修复完成！现在运行仿真脚本不会再报版本冲突错误了。') +except ImportError: + print('❌ 错误: 找到了目录，但里面没有 botocore 模块。') +""" +``` + +### 2.3 运行仿真车间 +每次运行前，请务必先 source ROS 2 环境，否则底层的 ROS 2 Bridge 插件将无法加载并发布话题！ + +```bash +# 1. 激活 ROS 2 环境 (请根据实际安装路径调整) +source /opt/ros/humble/setup.bash + +# 2. 激活 Python 虚拟环境 +conda activate AutoCalib_Workshop + +# 3. 启动仿真脚本 +python build_calibration_room.py +``` + +--- + +## 3. 输出的 ROS 2 话题 + +脚本成功运行后，将在局域网内广播以下 ROS 2 话题。可直接使用 RViz2 订阅查看。 + +💡 **RViz2 可视化提示：** 必须将 RViz2 的 Fixed Frame 设置为 `World`，方可查看到静止、规整的 3D 车间点云。 + +| 数据类型 | 话题名称 (Topic) | 说明 | +|------------------| -------------------------------- | -------------------------------------------------- | +| TF 树 s | `/tf` | 包含所有雷达与相机的全局坐标关系 (World -> Lidar_xx) | +| 图像 |`/workshop/camera/image_raw` | 标定板对齐相机 (720p, 20Hz) | +| 点云 (前左) | `/workshop/lidar/fl/pointcloud` | 机械旋转雷达 FL (fullScan 模式聚合的 360° 全帧点云) | +| 点云 (前右) | `/workshop/lidar/fr/pointcloud` | 机械旋转雷达 FR (fullScan 模式聚合的 360° 全帧点云) | +| 点云 (后左) | `/workshop/lidar/bl/pointcloud` | 机械旋转雷达 BL (fullScan 模式聚合的 360° 全帧点云) | +| 点云 (后右) | `/workshop/lidar/br/pointcloud` | 机械旋转雷达 BR (fullScan 模式聚合的 360° 全帧点云) | + +--- + +## 4. 团队协作与 Git 配置指南 (必读) + +新加入的团队成员，请严格按照以下步骤配置本地 Git 环境，以便顺畅地与组织私有仓库进行交互（使用 HTTPS 方式）。 + +### 4.1 声明你的 Git 身份 +在首次提交代码前，必须全局设置你的身份信息： + +```bash +git config --global user.name "你的名字或GitHub用户名" +git config --global user.email "你注册GitHub的邮箱@example.com" +``` + +### 4.2 配置 HTTPS 访问 (使用 Personal Access Token) +由于 GitHub 已废弃密码验证，HTTPS 方式需要使用 **Personal Access Token (PAT)** 作为密码。请按以下步骤生成并配置 token： + +1. **生成 Token：** + - 登录 GitHub，点击右上角头像 → **Settings** → **Developer settings** → **Personal access tokens** → **Tokens (classic)**。 + - 点击 **Generate new token (classic)**。 + - 在 **Note** 中输入用途说明（例如 “Isaac Sim 开发”）。 + - 设置过期时间（建议选 90 天或自定义）。 + - 在 **Select scopes** 中勾选 **`repo`**（完全控制私有仓库）以及必要的 **`workflow`** 权限（如需操作 GitHub Actions）。 + - 点击 **Generate token**，**立即复制并保存生成的 token**（页面刷新后将无法再次查看）。 + +2. **配置 Git 凭据存储（避免每次输入密码）：** + ```bash + # 启用 Git 凭据缓存（默认缓存 15 分钟） + git config --global credential.helper cache + + # 或者使用更持久的存储（凭据将明文保存在磁盘，请确保系统安全） + git config --global credential.helper store + ``` + +3. **首次推送时输入凭据：** + - 执行 `git push` 等需要鉴权的操作时，Git 会提示输入用户名和密码。 + - **用户名**：输入你的 GitHub 用户名。 + - **密码**：输入刚才生成的 Personal Access Token（不是你的登录密码）。 + - 如果配置了 credential helper，后续操作将不再重复提示。 + +### 4.3 关键：切勿忽略 .gitignore +Isaac Sim 和 ROS 2 运行期间会产生海量的缓存文件（如 `*.usd.cache`、`__pycache__`、`log/` 等）。 + +**严禁将这些大文件推送到远程仓库！** +本仓库根目录已配置好 `.gitignore` 文件，请在执行 `git add .` 时确保不要使用 `-f` 强行添加被忽略的文件。 + +--- + +## 5. 开发与分支规范 + +为了保证代码库的稳定，请团队成员遵守以下 Git 协作工作流。 + +### 5.1 分支命名规范 +在创建新分支时，请使用 **前缀/功能描述** 的格式（例如：`feature/lidar-sync`）。 + +| 前缀 | 适用场景 | 示例 | +| ---------- | -------------------------------------- | ----------------------------- | +| `feature/` | **新功能**：新增传感器、模型或标定算法 | `feature/add-calibration-node` | +| `bugfix/` | **修复错误**：解决代码逻辑、话题发布等 Bug | `bugfix/fix-tf-error` | +| `docs/` | **文档更新**：修改 README 或注释 | `docs/update-readme` | +| `refactor/`| **代码重构**：优化代码结构而不改变功能 | `refactor/clean-lidar-logic` | +| `test/` | **测试**：增加单元测试或仿真验证脚本 | `test/calibration-verify` | + +### 5.2 保护主分支 +绝对不要直接在 `main` 分支上开发和提交代码。`main` 分支仅用于存放稳定、经过测试的代码。 + +### 5.3 切分支干活 +开始新功能开发（如编写新的标定算法节点）时，请从最新的 `main` 切出一个独立分支： +```bash +git checkout main +git pull origin main +git checkout -b feature/your_feature_name +``` + +### 5.4 提交与推送 +```bash +git add . +git commit -m "feat: 添加了xxx功能" +git push origin feature/your_feature_name +``` + +### 5.5 代码合并 (Code Review) +开发完成后，请在 GitHub 页面上针对您的分支发起 **Pull Request (PR)**。由其他团队成员进行 Code Review，确认无误后再 Merge 合并入 `main` 分支。 + +--- + +以上即为调整后的完整 README 文档，已针对 HTTPS 访问组织私有仓库进行了配置说明，格式规范统一，内容清晰易读。 \ No newline at end of file diff --git a/build_calibration_room.py b/build_calibration_room.py new file mode 100644 index 0000000..5e5a106 --- /dev/null +++ b/build_calibration_room.py @@ -0,0 +1,276 @@ +import os + +os.environ["OMNI_KIT_ACCEPT_EULA"] = "YES" +from isaacsim import SimulationApp + +simulation_app = SimulationApp({"headless": False}) + +from omni.isaac.core.utils.extensions import enable_extension + +enable_extension("omni.isaac.ros2_bridge") +simulation_app.update() + +import numpy as np +from PIL import Image +import omni.kit.commands +from pathlib import Path + +# 【🔥核心防坑：只依赖底层基石 USD API】 +import omni.usd +from pxr import Gf, Sdf, UsdShade, UsdGeom, Vt + +from omni.isaac.core import World +from omni.isaac.core.objects import FixedCuboid +from omni.isaac.core.utils.prims import create_prim +from omni.isaac.core.utils.viewports import set_camera_view +from omni.isaac.core.utils.rotations import euler_angles_to_quat + +from omni.isaac.sensor import Camera +import omni.replicator.core as rep +import omni.graph.core as og + + +def create_checkerboard_image(filepath="checkerboard.png", rows=6, cols=9, square_size_px=100): + width = cols * square_size_px + height = rows * square_size_px + img = np.ones((height, width, 3), dtype=np.uint8) * 255 + + for r in range(rows): + for c in range(cols): + if (r + c) % 2 == 1: + img[r * square_size_px:(r + 1) * square_size_px, c * square_size_px:(c + 1) * square_size_px] = 0 + + border = square_size_px + img_with_border = np.pad(img, pad_width=((border, border), (border, border), (0, 0)), mode='constant', + constant_values=255) + + pil_img = Image.fromarray(img_with_border) + abs_filepath = Path(filepath).resolve() + pil_img.save(abs_filepath) + usd_filepath = str(abs_filepath).replace("\\", "/") + print(f"[*] 棋盘格纹理已自动生成: {usd_filepath}") + return usd_filepath + + +def add_corner_rotary_lidars(room_length=10.0, room_width=6.0, height=3.5, + lidar_config="Example_Rotary", + topic_prefix="/workshop/lidar"): + offset = 0.3 + x_pos = (room_length / 2.0) - offset + y_pos = (room_width / 2.0) - offset + + lidar_configs = [ + {"name": "FL", "pos": [x_pos, y_pos, height], "yaw": np.degrees(np.arctan2(-y_pos, -x_pos))}, + {"name": "FR", "pos": [x_pos, -y_pos, height], "yaw": np.degrees(np.arctan2(y_pos, -x_pos))}, + {"name": "BL", "pos": [-x_pos, y_pos, height], "yaw": np.degrees(np.arctan2(-y_pos, x_pos))}, + {"name": "BR", "pos": [-x_pos, -y_pos, height], "yaw": np.degrees(np.arctan2(y_pos, x_pos))} + ] + + keys = og.Controller.Keys + graph_path = "/World/ROS2_Lidar_Graph" + + nodes = [ + ("OnTick", "omni.graph.action.OnTick"), + ("ReadSimTime", "omni.isaac.core_nodes.IsaacReadSimulationTime"), + ("PublishTF", "omni.isaac.ros2_bridge.ROS2PublishTransformTree") + ] + + connections = [ + ("OnTick.outputs:tick", "PublishTF.inputs:execIn"), + ("ReadSimTime.outputs:simulationTime", "PublishTF.inputs:timeStamp") + ] + + set_values = [] + lidar_paths = [] + + for cfg in lidar_configs: + lidar_path = f"/World/Sensors/Lidar_{cfg['name']}" + lidar_paths.append(lidar_path) + + pitch_angle = 15.0 + quat = euler_angles_to_quat(np.array([0, pitch_angle, cfg['yaw']]), degrees=True) + orientation = Gf.Quatd(quat[0], quat[1], quat[2], quat[3]) + + omni.kit.commands.execute( + "IsaacSensorCreateRtxLidar", path=lidar_path, parent=None, + config=lidar_config, translation=Gf.Vec3d(*cfg["pos"]), orientation=orientation + ) + + render_product = rep.create.render_product(lidar_path, [1, 1]) + helper_name = f"ROS2LidarHelper_{cfg['name']}" + nodes.append((helper_name, "omni.isaac.ros2_bridge.ROS2RtxLidarHelper")) + connections.append(("OnTick.outputs:tick", f"{helper_name}.inputs:execIn")) + + set_values.extend([ + (f"{helper_name}.inputs:renderProductPath", str(render_product.path)), + (f"{helper_name}.inputs:topicName", f"{topic_prefix}/{cfg['name'].lower()}/pointcloud"), + (f"{helper_name}.inputs:frameId", f"Lidar_{cfg['name']}"), + (f"{helper_name}.inputs:type", "point_cloud"), + (f"{helper_name}.inputs:fullScan", True) + ]) + + set_values.append(("PublishTF.inputs:targetPrims", lidar_paths)) + + og.Controller.edit({"graph_path": graph_path, "evaluator_name": "execution"}, + {keys.CREATE_NODES: nodes, keys.CONNECT: connections, keys.SET_VALUES: set_values}) + + +# ================= 【🔥纯血底层 API：手工构造材质与带 UV 的网格】 ================= +def create_raw_usd_material(stage, mat_path, tex_path): + material = UsdShade.Material.Define(stage, mat_path) + + pbr_shader = UsdShade.Shader.Define(stage, f"{mat_path}/PBRShader") + pbr_shader.CreateIdAttr("UsdPreviewSurface") + pbr_shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(1.0) # 纯哑光去反光 + pbr_shader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(0.0) # 非金属 + + tex_sampler = UsdShade.Shader.Define(stage, f"{mat_path}/diffuseTexture") + tex_sampler.CreateIdAttr("UsdUVTexture") + tex_sampler.CreateInput("file", Sdf.ValueTypeNames.Asset).Set(Sdf.AssetPath(tex_path)) + + # 🔥🔥🔥 核心修改 1：强制关闭 GPU 的双线性平滑插值，使用“最近邻(Nearest)”采样！🔥🔥🔥 + # 这一步能让黑白方块的交界处像刀切一样锐利，彻底消除模糊过渡带！ + tex_sampler.CreateInput("magFilter", Sdf.ValueTypeNames.Token).Set("nearest") + tex_sampler.CreateInput("minFilter", Sdf.ValueTypeNames.Token).Set("nearest") + + st_reader = UsdShade.Shader.Define(stage, f"{mat_path}/stReader") + st_reader.CreateIdAttr("UsdPrimvarReader_float2") + st_reader.CreateInput("varname", Sdf.ValueTypeNames.Token).Set("st") + + tex_sampler.CreateInput("st", Sdf.ValueTypeNames.Float2).ConnectToSource(st_reader.ConnectableAPI(), "result") + pbr_shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).ConnectToSource(tex_sampler.ConnectableAPI(), + "rgb") + material.CreateSurfaceOutput().ConnectToSource(pbr_shader.ConnectableAPI(), "surface") + + return material + + +def create_textured_board(stage, prim_path, width, height, center, euler_rot_deg, usd_material): + mesh = UsdGeom.Mesh.Define(stage, prim_path) + w, h = width / 2.0, height / 2.0 + + points = Vt.Vec3fArray([Gf.Vec3f(-w, -h, 0), Gf.Vec3f(w, -h, 0), Gf.Vec3f(w, h, 0), Gf.Vec3f(-w, h, 0)]) + mesh.GetPointsAttr().Set(points) + mesh.GetFaceVertexCountsAttr().Set([4]) + mesh.GetFaceVertexIndicesAttr().Set([0, 1, 2, 3]) + + mesh.GetNormalsAttr().Set([Gf.Vec3f(0, 0, 1)] * 4) + mesh.SetNormalsInterpolation(UsdGeom.Tokens.vertex) + + primvars_api = UsdGeom.PrimvarsAPI(mesh) + st_primvar = primvars_api.CreatePrimvar("st", Sdf.ValueTypeNames.TexCoord2fArray, UsdGeom.Tokens.vertex) + st_primvar.Set([Gf.Vec2f(0, 0), Gf.Vec2f(1, 0), Gf.Vec2f(1, 1), Gf.Vec2f(0, 1)]) + + mesh.GetExtentAttr().Set([Gf.Vec3f(-w, -h, -0.01), Gf.Vec3f(w, h, 0.01)]) + + xform = UsdGeom.Xformable(mesh) + xform.AddTranslateOp().Set(Gf.Vec3d(*center)) + xform.AddRotateXYZOp().Set(Gf.Vec3f(*euler_rot_deg)) + + UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim()).Bind(usd_material) + return mesh + + +# ============================================================================== + + +def build_workshop(): + world = World(stage_units_in_meters=1.0) + L, W, H, T = 10.0, 6.0, 3.5, 0.2 + floor_color = np.array([0.2, 0.2, 0.2]) + wall_color = np.array([0.8, 0.8, 0.8]) + + world.scene.add(FixedCuboid(prim_path="/World/Workshop/Floor", name="floor", position=np.array([0, 0, -T / 2]), + scale=np.array([L + 2 * T, W + 2 * T, T]), color=floor_color)) + world.scene.add( + FixedCuboid(prim_path="/World/Workshop/Ceiling", name="ceiling", position=np.array([0, 0, H + T / 2]), + scale=np.array([L + 2 * T, W + 2 * T, T]), color=wall_color)) + world.scene.add(FixedCuboid(prim_path="/World/Workshop/Wall_Front", name="wall_front", + position=np.array([L / 2 + T / 2, 0, H / 2]), scale=np.array([T, W, H]), + color=wall_color)) + world.scene.add(FixedCuboid(prim_path="/World/Workshop/Wall_Back", name="wall_back", + position=np.array([-L / 2 - T / 2, 0, H / 2]), scale=np.array([T, W, H]), + color=wall_color)) + world.scene.add(FixedCuboid(prim_path="/World/Workshop/Wall_Left", name="wall_left", + position=np.array([0, W / 2 + T / 2, H / 2]), scale=np.array([L + 2 * T, T, H]), + color=wall_color)) + world.scene.add(FixedCuboid(prim_path="/World/Workshop/Wall_Right", name="wall_right", + position=np.array([0, -W / 2 - T / 2, H / 2]), scale=np.array([L + 2 * T, T, H]), + color=wall_color)) + + light_positions = [(L / 4, W / 4, H - 0.5), (L / 4, -W / 4, H - 0.5), (-L / 4, W / 4, H - 0.5), + (-L / 4, -W / 4, H - 0.5)] + for i, pos in enumerate(light_positions): + create_prim(prim_path=f"/World/Workshop/Lights/Light_{i}", prim_type="SphereLight", position=np.array(pos), + attributes={"inputs:radius": 0.3, "inputs:intensity": 30000.0, "inputs:color": (1.0, 1.0, 0.95)}) + + cb_rows, cb_cols = 6, 9 + cb_square_size = 0.20 + + # 🔥🔥🔥 核心修改 2：适当提升分辨率 🔥🔥🔥 + # 将 square_size_px 从 100 提高到 500！ + tex_path = create_checkerboard_image("checkerboard.png", rows=cb_rows, cols=cb_cols, square_size_px=500) + + stage = omni.usd.get_context().get_stage() + + mat_path = "/World/Workshop/Materials/CheckerboardMat" + usd_material = create_raw_usd_material(stage, mat_path, tex_path) + + board_w = (cb_cols + 2) * cb_square_size + board_h = (cb_rows + 2) * cb_square_size + z_height = H / 2.0 + offset = 0.05 + + board_configs = [ + ("/World/Workshop/CalibrationBoards/Front", [L / 2 - offset, 0, z_height], [90, 0, 90]), + ("/World/Workshop/CalibrationBoards/Back", [-L / 2 + offset, 0, z_height], [90, 0, -90]), + ("/World/Workshop/CalibrationBoards/Left", [0, W / 2 - offset, z_height], [90, 0, 0]), + ("/World/Workshop/CalibrationBoards/Right", [0, -W / 2 + offset, z_height], [90, 0, 180]) + ] + + for path, pos, euler_rot in board_configs: + create_textured_board(stage, path, board_w, board_h, pos, euler_rot, usd_material) + + camera = Camera(prim_path="/World/Workshop/CalibrationCamera", position=np.array([-4.0, 0.0, 3.5]), frequency=20, + resolution=(1280, 720)) + camera.set_world_pose(orientation=np.array([0.7071, 0.0, 0.7071, 0.0])) + camera.initialize() + + keys = og.Controller.Keys + og.Controller.edit({"graph_path": "/World/ROS2_Camera_Graph", "evaluator_name": "execution"}, + {keys.CREATE_NODES: [("OnTick", "omni.graph.action.OnTick"), + ("ROS2Camera", "omni.isaac.ros2_bridge.ROS2CameraHelper")], + keys.CONNECT: [("OnTick.outputs:tick", "ROS2Camera.inputs:execIn")], + keys.SET_VALUES: [("ROS2Camera.inputs:renderProductPath", camera.get_render_product_path()), + ("ROS2Camera.inputs:topicName", "/AutoCalib_Workshop/camera/image_raw"), + ("ROS2Camera.inputs:type", "rgb")]}) + + add_corner_rotary_lidars(room_length=L, room_width=W, height=H - 0.2, lidar_config="Example_Rotary", + topic_prefix="/AutoCalib_Workshop/lidar") + + return world + + +def main(): + world = build_workshop() + + world.reset() + + set_camera_view(eye=np.array([-4.0, 0.0, 2.0]), target=np.array([5.0, 0.0, 3.5])) + + print("======================================================") + print(" 🎯 标定车间完美运行！纯锐利边缘棋盘格已加载完毕！") + print(" ---------------------------------------------------") + print(" 💡 标定算法所需的关键真值参数 (Ground Truth)：") + print(" - 内部角点维度 (Pattern Size) : 8 x 5") + print(" - 绝对物理边长 (Square Size) : 0.20 米 (20cm)") + print("======================================================") + + while simulation_app.is_running(): + world.step(render=True) + + simulation_app.close() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/calibration_gazebo/build.sh b/calibration_gazebo/build.sh new file mode 100755 index 0000000..88ed324 --- /dev/null +++ b/calibration_gazebo/build.sh @@ -0,0 +1,37 @@ +#!/usr/bin/env bash +# --------------------------------------------------------- +# Autoware 编译脚本 +# --------------------------------------------------------- +set -euo pipefail + +readonly RED='\033[0;31m' +readonly GREEN='\033[0;32m' +readonly NC='\033[0m' + +log() { echo -e "${GREEN}[INFO]${NC} $*"; } +warn() { echo -e "${RED}[WARN]${NC} $*" >&2; } + +BUILD_ARGS=( + colcon build + --symlink-install + --cmake-args -DCMAKE_BUILD_TYPE=Release +) + +# 如需部分包编译，把 xxx 换成包名后取消下一行注释 +# BUILD_ARGS+=(--packages-select autoware_pose_initializer) + +log "Starting colcon build..." +if AUTOWARE_COMPILE_WITH_CUDA=1 "${BUILD_ARGS[@]}"; then + log "Build succeeded. Sourcing workspace..." + + # 临时关闭 -u，避免 COLCON_TRACE 未定义报错 + set +u + source install/setup.bash + set -u + + log "Done." +else + warn "Build failed, skip sourcing." +fi + + diff --git a/calibration_gazebo/src/calibration_sim/CMakeLists.txt b/calibration_gazebo/src/calibration_sim/CMakeLists.txt new file mode 100644 index 0000000..6f37ca2 --- /dev/null +++ b/calibration_gazebo/src/calibration_sim/CMakeLists.txt @@ -0,0 +1,91 @@ +cmake_minimum_required(VERSION 3.8) +project(calibration_sim) + +# 1. 基础编译设置 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# 2. 寻找依赖库 +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) # 用于 PointCloud2 +find_package(geometry_msgs REQUIRED) # 用于坐标变换 +find_package(tf2_ros REQUIRED) # 坐标变换工具 +find_package(tf2_geometry_msgs REQUIRED) +find_package(pcl_conversions REQUIRED)# ROS <-> PCL 转换桥梁 + +# 寻找系统级安装的 PCL 库 (非 ROS 包) +find_package(PCL REQUIRED COMPONENTS common io visualization filters registration segmentation) + +# 3. 定义可执行文件 (你的核心 C++ 节点) +# 假设你将来会写一个 lidar_fusion_node.cpp +#add_executable(lidar_fusion_node src/lidar_fusion_node.cpp) + +# 4. 配置头文件路径 +#target_include_directories(lidar_fusion_node PUBLIC +# $ + # $ + # ${PCL_INCLUDE_DIRS} # 必须包含 PCL 的头文件 +#) + +# 5. 链接库文件 +# 链接 ROS 相关的依赖 +#ament_target_dependencies(lidar_fusion_node + # rclcpp + # std_msgs + #sensor_msgs + #geometry_msgs + # tf2_ros + # tf2_geometry_msgs + # pcl_conversions +#) + +# 链接 PCL 相关的依赖 (PCL 不是 ament 包，需要单独链接) +#target_link_libraries(lidar_fusion_node + # ${PCL_LIBRARIES} +#) + +# 6. 安装规则 (至关重要!) + +# 安装可执行文件 (C++ 节点) +#install(TARGETS + # lidar_fusion_node +# DESTINATION lib/${PROJECT_NAME} +#) + +# 安装脚本文件 (如果你有 Python 节点) +# install(PROGRAMS +# scripts/my_python_script.py +# DESTINATION lib/${PROJECT_NAME} +# ) + +# 安装资源目录 (Launch, URDF, RViz, Worlds) +# 如果没有这步，你的 ros2 launch 命令会找不到文件 +install(DIRECTORY + launch + urdf + rviz + worlds + config + DESTINATION share/${PROJECT_NAME} +) + +# 7. 导出依赖与生成包 +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips the linter which checks for eccentric indentation + # comment the line when it conforms to defaults + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/calibration_gazebo/src/calibration_sim/launch/calibration_system.launch.py b/calibration_gazebo/src/calibration_sim/launch/calibration_system.launch.py new file mode 100644 index 0000000..9f17a6c --- /dev/null +++ b/calibration_gazebo/src/calibration_sim/launch/calibration_system.launch.py @@ -0,0 +1,73 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, ExecuteProcess +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +import xacro + +def generate_launch_description(): + pkg_name = 'calibration_sim' + + # 1. 解析 URDF + infra_xacro = os.path.join(get_package_share_directory(pkg_name), 'urdf', 'workshop_sensors.xacro') + infra_doc = xacro.process_file(infra_xacro) + infra_xml = infra_doc.toxml() + + agv_xacro = os.path.join(get_package_share_directory(pkg_name), 'urdf', 'agv.xacro') + agv_doc = xacro.process_file(agv_xacro) + agv_xml = agv_doc.toxml() + + world_path = os.path.join( + get_package_share_directory('calibration_sim'), + 'worlds', + 'calibration_room.world' + ) + + # 2. 启动 Gazebo + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]), + launch_arguments={'world': world_path}.items(), # 【关键】指定 world 参数 + ) + + # 3. 生成基础设施 (4个雷达) + spawn_infra = Node( + package='gazebo_ros', executable='spawn_entity.py', + arguments=['-topic', 'robot_description_infra', '-entity', 'workshop_sensors'], + output='screen' + ) + + # 发布基础设施的 State Publisher (为了TF树) + infra_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='infra_state_publisher', + parameters=[{'robot_description': infra_xml}], + remappings=[('robot_description', 'robot_description_infra')] + ) + + # 4. 生成 AGV + spawn_agv = Node( + package='gazebo_ros', executable='spawn_entity.py', + arguments=['-topic', 'robot_description_agv', '-entity', 'my_agv', '-z', '0.1'], + output='screen' + ) + + # 发布 AGV 的 State Publisher + agv_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='agv_state_publisher', + parameters=[{'robot_description': agv_xml}], + remappings=[('robot_description', 'robot_description_agv')] + ) + + return LaunchDescription([ + gazebo, + infra_state_publisher, + spawn_infra, + agv_state_publisher, + spawn_agv, + # 这里还可以加上 RViz 的节点 + ]) diff --git a/calibration_gazebo/src/calibration_sim/package.xml b/calibration_gazebo/src/calibration_sim/package.xml new file mode 100644 index 0000000..4b96452 --- /dev/null +++ b/calibration_gazebo/src/calibration_sim/package.xml @@ -0,0 +1,26 @@ + + + + calibration_sim + 0.0.0 + Simulated environment for workshop calibration project + User + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + sensor_msgs + geometry_msgs + tf2_ros + tf2_geometry_msgs + pcl_conversions + + gazebo_ros ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/calibration_gazebo/src/calibration_sim/urdf/agv.xacro b/calibration_gazebo/src/calibration_sim/urdf/agv.xacro new file mode 100644 index 0000000..1d1032e --- /dev/null +++ b/calibration_gazebo/src/calibration_sim/urdf/agv.xacro @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + / + + left_wheel_joint + right_wheel_joint + 0.4 + 0.2 + true + true + odom + base_footprint + + + + diff --git a/calibration_gazebo/src/calibration_sim/urdf/calibration_board.xacro b/calibration_gazebo/src/calibration_sim/urdf/calibration_board.xacro new file mode 100644 index 0000000..f3f337c --- /dev/null +++ b/calibration_gazebo/src/calibration_sim/urdf/calibration_board.xacro @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Checkerboard + + + + true + + + diff --git a/calibration_gazebo/src/calibration_sim/urdf/workshop_sensors.xacro b/calibration_gazebo/src/calibration_sim/urdf/workshop_sensors.xacro new file mode 100644 index 0000000..b5f887a --- /dev/null +++ b/calibration_gazebo/src/calibration_sim/urdf/workshop_sensors.xacro @@ -0,0 +1,87 @@ + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/FlatBlack + + + 0 0 0 0 0 0 + false + 10 + + + 600 + 1 + -0.7854 + 0.7854 + + + + 128 + 1 + -0.2181 + 0.2251 + + + + + 0.5 210.0 + + + + gaussian + 0.0 + 0.03 + + + + + + /infrastructure + ~/out:=${name}/points + + sensor_msgs/PointCloud2 + ${name}_link + + + + + + + + + + + diff --git a/calibration_gazebo/src/calibration_sim/worlds/calibration_room.world b/calibration_gazebo/src/calibration_sim/worlds/calibration_room.world new file mode 100644 index 0000000..15ce2aa --- /dev/null +++ b/calibration_gazebo/src/calibration_sim/worlds/calibration_room.world @@ -0,0 +1,101 @@ + + + + + + 0.6 0.6 0.6 1.0 + 0.7 0.7 0.7 1.0 + true + + + model://ground_plane + + + true + 0 0 0 0 0 0 + 0 5 1.25 0 0 010.2 0.2 2.510.2 0.2 2.5 + 0 -5 1.25 0 0 010.2 0.2 2.510.2 0.2 2.5 + 5 0 1.25 0 0 00.2 10.0 2.50.2 10.0 2.5 + -5 0 1.25 0 0 00.2 10.0 2.50.2 10.0 2.5 + + + model://checkerboard_planeboard_n1-3.75 4.89 1.25 1.57 0 00.3125 0.3125 1 + model://checkerboard_planeboard_n2-1.25 4.89 1.25 1.57 0 00.3125 0.3125 1 + model://checkerboard_planeboard_n3 1.25 4.89 1.25 1.57 0 00.3125 0.3125 1 + model://checkerboard_planeboard_n4 3.75 4.89 1.25 1.57 0 00.3125 0.3125 1 + + model://checkerboard_planeboard_s1-3.75 -4.89 1.25 1.57 0 3.141590.3125 0.3125 1 + model://checkerboard_planeboard_s2-1.25 -4.89 1.25 1.57 0 3.141590.3125 0.3125 1 + model://checkerboard_planeboard_s3 1.25 -4.89 1.25 1.57 0 3.141590.3125 0.3125 1 + model://checkerboard_planeboard_s4 3.75 -4.89 1.25 1.57 0 3.141590.3125 0.3125 1 + + model://checkerboard_planeboard_e14.89 -3.75 1.25 1.57 0 -1.570.3125 0.3125 1 + model://checkerboard_planeboard_e24.89 -1.25 1.25 1.57 0 -1.570.3125 0.3125 1 + model://checkerboard_planeboard_e34.89 1.25 1.25 1.57 0 -1.570.3125 0.3125 1 + model://checkerboard_planeboard_e44.89 3.75 1.25 1.57 0 -1.570.3125 0.3125 1 + + model://checkerboard_planeboard_w1-4.89 -3.75 1.25 1.57 0 1.570.3125 0.3125 1 + model://checkerboard_planeboard_w2-4.89 -1.25 1.25 1.57 0 1.570.3125 0.3125 1 + model://checkerboard_planeboard_w3-4.89 1.25 1.25 1.57 0 1.570.3125 0.3125 1 + model://checkerboard_planeboard_w4-4.89 3.75 1.25 1.57 0 1.570.3125 0.3125 1 + + + + true + 0 0 2.5 0 0 0 + + 0-4.5 -4.5 0 0 0 00.10.050.5 0.5 0.5 1 + 0-1.5 -4.5 0 0 0 00.10.050.5 0.5 0.5 1 + 0 1.5 -4.5 0 0 0 00.10.050.5 0.5 0.5 1 + 0 4.5 -4.5 0 0 0 00.10.050.5 0.5 0.5 1 + 0-4.5 -1.5 0 0 0 00.10.050.5 0.5 0.5 1 + 0-1.5 -1.5 0 0 0 00.10.050.5 0.5 0.5 1 + 0 1.5 -1.5 0 0 0 00.10.050.5 0.5 0.5 1 + 0 4.5 -1.5 0 0 0 00.10.050.5 0.5 0.5 1 + 0-4.5 1.5 0 0 0 00.10.050.5 0.5 0.5 1 + 0-1.5 1.5 0 0 0 00.10.050.5 0.5 0.5 1 + 0 1.5 1.5 0 0 0 00.10.050.5 0.5 0.5 1 + 0 4.5 1.5 0 0 0 00.10.050.5 0.5 0.5 1 + 0-4.5 4.5 0 0 0 00.10.050.5 0.5 0.5 1 + 0-1.5 4.5 0 0 0 00.10.050.5 0.5 0.5 1 + 0 1.5 4.5 0 0 0 00.10.050.5 0.5 0.5 1 + 0 4.5 4.5 0 0 0 00.10.050.5 0.5 0.5 1 + + + + -4.5 -4.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + -1.5 -4.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + 1.5 -4.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + 4.5 -4.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + -4.5 -1.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + -1.5 -1.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + 1.5 -1.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + 4.5 -1.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + -4.5 1.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + -1.5 1.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + 1.5 1.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + 4.5 1.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + -4.5 4.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + -1.5 4.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + 1.5 4.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + 4.5 4.5 2.45 0 0 00.3 0.3 0.3 10.1 0.1 0.1 160.20.050.10 + + + model://checkerboard_planefloor_n1-3.75 3.75 0.01 0 0 00.3125 0.3125 1 + model://checkerboard_planefloor_n2-1.25 3.75 0.01 0 0 00.3125 0.3125 1 + model://checkerboard_planefloor_n3 1.25 3.75 0.01 0 0 00.3125 0.3125 1 + model://checkerboard_planefloor_n4 3.75 3.75 0.01 0 0 00.3125 0.3125 1 + + model://checkerboard_planefloor_s1-3.75 -3.75 0.01 0 0 00.3125 0.3125 1 + model://checkerboard_planefloor_s2-1.25 -3.75 0.01 0 0 00.3125 0.3125 1 + model://checkerboard_planefloor_s3 1.25 -3.75 0.01 0 0 00.3125 0.3125 1 + model://checkerboard_planefloor_s4 3.75 -3.75 0.01 0 0 00.3125 0.3125 1 + + model://checkerboard_planefloor_e13.75 -1.25 0.01 0 0 00.3125 0.3125 1 + model://checkerboard_planefloor_e23.75 1.25 0.01 0 0 00.3125 0.3125 1 + + model://checkerboard_planefloor_w1-3.75 -1.25 0.01 0 0 00.3125 0.3125 1 + model://checkerboard_planefloor_w2-3.75 1.25 0.01 0 0 00.3125 0.3125 1 + + + diff --git a/checkerboard.png b/checkerboard.png new file mode 100644 index 0000000..26b3c06 Binary files /dev/null and b/checkerboard.png differ diff --git a/msg/agv_calib_chassis.proto b/msg/agv_calib_chassis.proto new file mode 100644 index 0000000..e69de29 diff --git a/msg/agv_calib_control.proto b/msg/agv_calib_control.proto new file mode 100644 index 0000000..e69de29 diff --git a/msg/agv_calib_sensor.proto b/msg/agv_calib_sensor.proto new file mode 100644 index 0000000..2295180 --- /dev/null +++ b/msg/agv_calib_sensor.proto @@ -0,0 +1,104 @@ +syntax = "proto3"; + +// 规范包名，防止与其他业务（如底盘运控调优）的接口冲突 +package agv.calibration.sensor; + +// ========================================================= +// 核心服务：传感器自动化标定代理服务 +// 部署端：Windows车端 (Server) | 调用端：Linux服务器 (Client) +// ========================================================= +service SensorCalibrationService { + // 1. 走位调度：指挥车辆开到特定的观测点并【绝对静止】 + rpc MoveToObservationPose (PoseRequest) returns (StandardResponse); + + // 2. 同步锁存：命令车辆瞬间冻结指定传感器的当前画面/点云到内存 + rpc TriggerSyncCapture (CaptureRequest) returns (CaptureResponse); + + // 3. 大文件下载：通过凭证流式拉取图片和点云（注意：使用 stream 防爆内存） + rpc DownloadImage (DataFetchRequest) returns (stream FileChunk); + rpc DownloadPointCloud (DataFetchRequest) returns (stream FileChunk); + + // 4. 标定闭环：Linux算完矩阵后，下发给车端持久化保存（覆写配置文件） + rpc CommitCalibrationResults (CalibrationPayload) returns (StandardResponse); +} + +// ========================================================= +// 基础响应 +// ========================================================= +message StandardResponse { + bool success = 1; + string message = 2; // 成功提示或具体的报错原因（如：碰撞急停） +} + +// ========================================================= +// 1. 物理走位请求 (走) +// ========================================================= +message PoseRequest { + double target_x_m = 1; // 目标 X 坐标 (米) + double target_y_m = 2; // 目标 Y 坐标 (米) + double target_yaw_deg = 3; // 目标偏航角 (度) + bool is_relative = 4; // true: 相对当前位置移动; false: 绝对世界坐标 +} + +// ========================================================= +// 2. 触发同步抓拍请求与响应 (停与拍) +// ========================================================= +message CaptureRequest { + // 告诉车端这次要同时拍哪些传感器，例如 ["cam_front", "lidar_top"] + repeated string sensor_ids = 1; +} + +message CaptureResponse { + bool success = 1; + // 极度关键：车端打上的高精度硬件时间戳(微秒)。 + // 这是提取数据的“取件码”，保证多传感器在物理时间上的绝对对齐！ + int64 capture_timestamp_us = 2; + string error_message = 3; +} + +// ========================================================= +// 3. 大文件下载请求与文件流块 (传) +// ========================================================= +message DataFetchRequest { + int64 capture_timestamp_us = 1; // 阶段2拿到的取件码 + string sensor_id = 2; // 具体要下载哪个传感器，例如 "cam_front" +} + +// 流式文件块 (规避 gRPC 单条消息默认 4MB 的内存限制) +message FileChunk { + bytes chunk_data = 1; // 文件的二进制分块（建议每次发 512KB - 1MB） + bool is_last_chunk = 2; // 是否为最后一块 + string format_ext = 3; // 格式标注，如 "png", "bmp", "pcd" +} + +// ========================================================= +// 4. 标定结果载荷（支持内参、外参灵活组合组合发回车端） (写) +// ========================================================= +message CameraIntrinsics { + string camera_id = 1; + double fx = 2; double fy = 3; + double cx = 4; double cy = 5; + repeated double dist_coeffs = 6; // 畸变系数阵列 [k1, k2, p1, p2, k3] +} + +message SensorExtrinsics { + string source_frame = 1; // 源坐标系，如 "lidar_top" 或 "cam_left" + string target_frame = 2; // 目标坐标系，如 "cam_front" 或 "base_link" + + // 平移向量 (强制规定单位为毫米 mm) + double trans_x_mm = 3; + double trans_y_mm = 4; + double trans_z_mm = 5; + + // 旋转姿态 (强制规定单位为度 degrees) + double roll_deg = 6; + double pitch_deg = 7; + double yaw_deg = 8; +} + +message CalibrationPayload { + string task_id = 1; // 标定任务流水号，用于 MES 系统追溯 + // 采用 repeated 数组：Linux 可以一次性下发多个相机的内参和多个外参 + repeated CameraIntrinsics updated_intrinsics = 2; + repeated SensorExtrinsics updated_extrinsics = 3; +} \ No newline at end of file diff --git a/test.py b/test.py new file mode 100644 index 0000000..2e1972b --- /dev/null +++ b/test.py @@ -0,0 +1,167 @@ +import numpy as np +import matplotlib.pyplot as plt +from matplotlib import cm +import matplotlib.patheffects as path_effects +from mpl_toolkits.mplot3d import Axes3D + + +# ========================================== +# 核心理论模块：通信迟滞容忍的动力学风险场发生器 +# (Latency-Tolerant Kinematic Risk Field Generator) +# ========================================== +def generate_kinematic_risk_field(X, Y, obj): + """ + 根据车辆的物理状态和通信迟滞，生成高斯风险势能场 + """ + x0, y0 = obj['pos'] + vx, vy = obj['vel'] + L, W = obj['size'] + dt = obj['latency'] # V2X 通信延迟 (秒) + + # 1. 运动学位置补偿 (由于延迟，目标在真实物理世界已经往前移动了) + x_real = x0 + vx * dt + y_real = y0 + vy * dt + + # 2. 时空耦合：协方差膨胀 (速度越快、延迟越高，沿运动方向的不确定性风险拖尾越长) + speed = np.hypot(vx, vy) + # 基础物理边界 (方差，代表车辆本身的尺寸) + sigma_x = L / 2.0 + sigma_y = W / 2.0 + + if speed > 0.1: + # ⚠️ 顶刊核心公式：沿着运动方向剧烈拉伸协方差！ + stretch_factor = 1.0 + 0.8 * speed * dt + sigma_x_dilated = sigma_x * stretch_factor + theta = np.arctan2(vy, vx) + else: + sigma_x_dilated = sigma_x + theta = 0.0 + + # 3. 旋转协方差矩阵，对齐到运动方向 + cos_t, sin_t = np.cos(theta), np.sin(theta) + R = np.array([[cos_t, -sin_t], + [sin_t, cos_t]]) + S = np.array([[sigma_x_dilated ** 2, 0], + [0, sigma_y ** 2]]) + Cov = R @ S @ R.T # 膨胀后的 2D 协方差矩阵 + Cov_inv = np.linalg.inv(Cov) + + # 4. 计算二维高斯势能曲面 (Mahalanobis Distance) + dx = X - x_real + dy = Y - y_real + # 矢量化二次型计算 + E = np.exp(-0.5 * (Cov_inv[0, 0] * dx ** 2 + 2 * Cov_inv[0, 1] * dx * dy + Cov_inv[1, 1] * dy ** 2)) + return E * obj['risk_weight'] + + +# ========================================== +# 场景构建：BEV 上帝视角物理空间初始化 +# ========================================== +# 设定一个 50米 x 50米 的路口物理网格 (高分辨率) +x_grid = np.linspace(0, 50, 300) +y_grid = np.linspace(0, 50, 300) +X, Y = np.meshgrid(x_grid, y_grid) + +# 定义场景中的交通参与者 (完美复现你的任务书痛点) +objects = [ + { + 'name': '高速来车 (带极端V2X延迟)', + 'pos': (10, 25), 'vel': (18, 0), # 速度 18m/s (约65km/h)向右 + 'size': (4.8, 2.0), 'latency': 0.4, # 恐怖的 400ms 网络延迟！ + 'risk_weight': 1.0 + }, + { + 'name': '非标事件: 散落物/掉落轮胎', + 'pos': (35, 12), 'vel': (0, 0), # 静止 + 'size': (1.5, 1.5), 'latency': 0.0, + 'risk_weight': 0.8 + }, + { + 'name': '鬼探头行人 (突然窜出)', + 'pos': (28, 42), 'vel': (0, -4), # 速度 4m/s 向下横穿 + 'size': (0.8, 0.8), 'latency': 0.1, + 'risk_weight': 0.9 + } +] + +# ========================================== +# 计算全局连续风险势能场 (Superposition of Risk Fields) +# ========================================== +Total_Risk_Field = np.zeros_like(X) +for obj in objects: + E_obj = generate_kinematic_risk_field(X, Y, obj) + Total_Risk_Field = np.maximum(Total_Risk_Field, E_obj) # 多风险源叠加取极值 + +# ========================================== +# 惊艳大招：计算势能梯度（多车协同调速排斥力场） F = -∇E +# ========================================== +dEy, dEx = np.gradient(Total_Risk_Field) +Force_X = -dEx +Force_Y = -dEy + +# ========================================== +# 顶刊级数据可视化渲染 (Matplotlib 画图) +# ========================================== +fig = plt.figure(figsize=(16, 7), facecolor='#111111') # 暗黑极客底色 +fig.suptitle("End-to-End Continuous Risk Potential Field for V2X Cooperative Driving", + fontsize=18, fontweight='bold', color='white', y=0.98) + +# --- 子图 1: 3D 连续风险势能面 --- +ax1 = fig.add_subplot(1, 2, 1, projection='3d') +ax1.set_facecolor('#111111') +surf = ax1.plot_surface(X, Y, Total_Risk_Field, cmap=cm.inferno, alpha=0.9, rstride=3, cstride=3, linewidth=0, + antialiased=True) + +ax1.set_title("(a) 3D Spatiotemporal Risk Surface\n【任务2】时空演化连续风险曲面", fontsize=14, fontweight='bold', + color='white', pad=15) +ax1.set_xlabel("BEV X-Coordinate (m)", color='white') +ax1.set_ylabel("BEV Y-Coordinate (m)", color='white') +ax1.set_zlabel("Risk Potential Energy ($E_{risk}$)", color='white') +ax1.tick_params(colors='white') +ax1.xaxis.pane.fill = False +ax1.yaxis.pane.fill = False +ax1.zaxis.pane.fill = False +ax1.view_init(elev=40, azim=-45) # 绝佳的观察视角 + +# --- 子图 2: 2D 梯度力场与规控闭环 --- +ax2 = fig.add_subplot(1, 2, 2) +ax2.set_facecolor('#111111') +# 画势能等高线 +contour = ax2.contourf(X, Y, Total_Risk_Field, levels=30, cmap=cm.inferno, alpha=0.8) + +# 画排斥力场 (Quiver 矢量箭头) +step = 10 # 箭头采样稀疏度 +Q = ax2.quiver(X[::step, ::step], Y[::step, ::step], Force_X[::step, ::step], Force_Y[::step, ::step], + color='cyan', scale=1.5, width=0.003, alpha=0.9) + + +# 标注解释 +def add_label(x, y, text): + ax2.text(x, y, text, color='white', ha='center', fontsize=10, + path_effects=[path_effects.withStroke(linewidth=2, foreground='k')]) + + +add_label(35, 15, "Static Debris\n(Isotropic Field)") +add_label(20, 25, "Latency-Dilated Comet Tail\n($\Delta t = 400ms$)") +add_label(28, 38, "Crossing Pedestrian") + +ax2.set_title("(b) Gradient-Driven Repulsive Force Field ($-\\nabla E$)\n【任务3】风险梯度驱动的多车协同调速诱导力场", + fontsize=14, fontweight='bold', color='white', pad=15) +ax2.set_xlabel("BEV X-Coordinate (m)", color='white') +ax2.set_ylabel("BEV Y-Coordinate (m)", color='white') +ax2.tick_params(colors='white') +ax2.set_xlim(0, 50) +ax2.set_ylim(0, 50) +ax2.set_aspect('equal') +ax2.grid(color='white', linestyle='--', linewidth=0.3, alpha=0.2) + +# 添加 Colorbar +cbar = fig.colorbar(surf, ax=[ax1, ax2], shrink=0.5, aspect=15, pad=0.05) +cbar.set_label('Collision Probability / Risk Intensity', color='white') +cbar.ax.yaxis.set_tick_params(color='white') +plt.setp(plt.getp(cbar.ax.axes, 'yticklabels'), color='white') + +plt.tight_layout(pad=3.0) +plt.show() +# 如果你想保存高清原图，取消下一行的注释 +# plt.savefig("v2x_risk_field.png", dpi=300, facecolor=fig.get_facecolor(), edgecolor='none') \ No newline at end of file