From cf94fc566b7fb7efe1e2faaa7d34d0479281956c Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Tue, 16 Jun 2026 22:45:15 +0800 Subject: [PATCH] =?UTF-8?q?=E9=94=A5=E6=A1=B6=E8=87=AA=E5=AE=9A=E4=B9=89?= =?UTF-8?q?=E6=9B=B4=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .DS_Store | Bin 6148 -> 6148 bytes README.md | 37 +++++++++++++++- cone_editor.py | 113 +++++++++++++++++++++++++++++++++++++++++++++++++ cones.json | 30 +++++++++++++ main.py | 22 +++++++++- scene.xml | 25 +---------- 6 files changed, 201 insertions(+), 26 deletions(-) create mode 100644 cone_editor.py create mode 100644 cones.json diff --git a/.DS_Store b/.DS_Store index 9a76a8961cc37c2c28e1afc83d998cbccaa8a43b..2f81ddbd1814e5992ce84c6983829e494a4525b9 100644 GIT binary patch delta 48 zcmZoMXfc@JFUrfnz`)4BAi%&-%233R%24c?lb^g6w$CoRqU!kYhQsFi47>A)XAYEGKG0FJI7ys0C{B-rT_o{ diff --git a/README.md b/README.md index b90687c..466b7c9 100644 --- a/README.md +++ b/README.md @@ -188,10 +188,43 @@ rclpy.spin(node) 键盘与 HTTP `/cmd` 可同时使用：按住 W/A/S/D 时键盘优先，松开后自动切回 HTTP 控制。 +## 锥桶编辑器 + +可视化在地图上放置锥桶，保存到 `cones.json`，仿真启动时自动加载。 + +```bash +uv run python cone_editor.py +``` + +| 操作 | 说明 | +|------|------| +| 左键点击地图 | 放置锥桶 | +| 右键点击地图 | 删除最近锥桶 | +| 鼠标移动 | 查看世界坐标 | +| 保存按钮 | 写入 `cones.json` | + +地图为 5m×5m，左下角 (-2.5, -2.5)，右上角 (2.5, 2.5)。 + +`cones.json` 格式： + +```json +[ + {"x": -1.45, "y": -2.29}, + {"x": -0.06, "y": -1.56}, + {"x": 1.82, "y": -0.91} +] +``` + +也可直接编辑此文件增删锥桶。 + ## 场景结构 ``` -scene.xml — 5m×5m 地图 + 40cm 白色围栏 + 阿克曼小车 -main.py — 仿真控制 + HTTP API 服务 +scene.xml — 场景描述（地图 + 围栏 + 小车，锥桶动态注入） +main.py — 仿真控制 + HTTP API 服务 +cone_editor.py — 锥桶可视化编辑器 +cones.json — 锥桶位置配置 +cone.obj — 锥体网格（底20cm→顶5cm，高27cm） +cone_white.obj — 白色反光条网格 origincar.urdf / origincar.xacro — 小车模型（参考） ``` diff --git a/cone_editor.py b/cone_editor.py new file mode 100644 index 0000000..b8d91ed --- /dev/null +++ b/cone_editor.py @@ -0,0 +1,113 @@ +"""Visual cone placement editor. Click on the 5m×5m map to place cones.""" +import json +import os +import tkinter as tk + +from PIL import Image, ImageTk + +MAP_FILE = "智能车地图源文件.avif" +OUTPUT = "cones.json" +MAP_SIZE_M = 5.0 # meters +CANVAS_W = 800 +CANVAS_H = 800 + + +class ConeEditor: + def __init__(self, root): + self.root = root + root.title("锥桶编辑器 — 左键放置 / 右键删除 / 滚轮保存") + self.cones = [] # list of {"x": world_x, "y": world_y} + + # Load map image + img = Image.open(MAP_FILE) + self.img_w, self.img_h = img.size + img = img.resize((CANVAS_W, CANVAS_H), Image.LANCZOS) + self.photo = ImageTk.PhotoImage(img) + + # Canvas + self.canvas = tk.Canvas(root, width=CANVAS_W, height=CANVAS_H, cursor="crosshair") + self.canvas.pack() + self.canvas.create_image(0, 0, anchor=tk.NW, image=self.photo) + + # Info label + self.info = tk.Label(root, text="左键放置 | 右键删除最近 | 鼠标查看坐标") + self.info.pack() + + # Buttons + btn_frame = tk.Frame(root) + btn_frame.pack(pady=5) + tk.Button(btn_frame, text="保存 (cones.json)", command=self.save).pack(side=tk.LEFT, padx=5) + tk.Button(btn_frame, text="清除全部", command=self.clear).pack(side=tk.LEFT, padx=5) + tk.Button(btn_frame, text="加载已有", command=self.load_existing).pack(side=tk.LEFT, padx=5) + tk.Label(btn_frame, text=f"地图: {MAP_SIZE_M}m×{MAP_SIZE_M}m | (-2.5,-2.5) → (2.5,2.5)").pack(side=tk.LEFT, padx=10) + + # Bindings + self.canvas.bind("", self.on_click) + self.canvas.bind("", self.on_right_click) + self.canvas.bind("", self.on_move) + + # Load existing if available + self.load_existing() + + def px_to_world(self, px, py): + x = px / CANVAS_W * MAP_SIZE_M - MAP_SIZE_M / 2 + y = MAP_SIZE_M / 2 - py / CANVAS_H * MAP_SIZE_M + return x, y + + def world_to_px(self, wx, wy): + px = (wx + MAP_SIZE_M / 2) / MAP_SIZE_M * CANVAS_W + py = (MAP_SIZE_M / 2 - wy) / MAP_SIZE_M * CANVAS_H + return px, py + + def on_move(self, event): + wx, wy = self.px_to_world(event.x, event.y) + self.info.config(text=f"坐标: ({wx:.3f}, {wy:.3f}) | 锥桶数量: {len(self.cones)}") + + def on_click(self, event): + wx, wy = self.px_to_world(event.x, event.y) + # Clamp to map bounds + half = MAP_SIZE_M / 2 + wx = max(-half, min(half, wx)) + wy = max(-half, min(half, wy)) + self.cones.append({"x": round(wx, 3), "y": round(wy, 3)}) + self.redraw() + + def on_right_click(self, event): + if not self.cones: + return + wx, wy = self.px_to_world(event.x, event.y) + # Remove nearest cone + idx = min(range(len(self.cones)), key=lambda i: (self.cones[i]["x"] - wx) ** 2 + (self.cones[i]["y"] - wy) ** 2) + self.cones.pop(idx) + self.redraw() + + def redraw(self): + self.canvas.delete("cone") + r = 5 + for c in self.cones: + px, py = self.world_to_px(c["x"], c["y"]) + self.canvas.create_oval(px - r, py - r, px + r, py + r, fill="blue", outline="white", width=2, tags="cone") + + def save(self): + with open(OUTPUT, "w") as f: + json.dump(self.cones, f, indent=2) + print(f"已保存 {len(self.cones)} 个锥桶到 {OUTPUT}") + self.info.config(text=f"已保存 {len(self.cones)} 个锥桶") + + def clear(self): + self.cones.clear() + self.redraw() + print("已清除所有锥桶") + + def load_existing(self): + if os.path.exists(OUTPUT): + with open(OUTPUT) as f: + self.cones = json.load(f) + self.redraw() + print(f"已加载 {len(self.cones)} 个锥桶") + + +if __name__ == "__main__": + root = tk.Tk() + app = ConeEditor(root) + root.mainloop() diff --git a/cones.json b/cones.json new file mode 100644 index 0000000..d624dfa --- /dev/null +++ b/cones.json @@ -0,0 +1,30 @@ +[ + { + "x": -1.45, + "y": -2.294 + }, + { + "x": -0.062, + "y": -1.562 + }, + { + "x": 1.819, + "y": -0.906 + }, + { + "x": 0.913, + "y": -1.931 + }, + { + "x": -1.144, + "y": -1.262 + }, + { + "x": -0.362, + "y": -0.656 + }, + { + "x": -2.281, + "y": -1.944 + } +] \ No newline at end of file diff --git a/main.py b/main.py index 08430d9..54eef59 100644 --- a/main.py +++ b/main.py @@ -3,6 +3,7 @@ import json import math import os import random +import tempfile import threading from collections import deque from http.server import BaseHTTPRequestHandler, HTTPServer @@ -151,7 +152,26 @@ def main(): with RenderApp() as render: render.opt.set_left_panel_vis(True) - model = load_model("scene.xml") + # Load cone positions and inject into scene + with open("cones.json") as f: + cones = json.load(f) + cone_xml = "" + for i, c in enumerate(cones): + cone_xml += ( + f'\n \n' + f' \n' + f' \n' + f' \n' + f' ' + ) + with open("scene.xml") as f: + xml = f.read() + xml = xml.replace("", cone_xml) + tmp = tempfile.NamedTemporaryFile(suffix=".xml", delete=False, dir=".", mode="w") + tmp.write(xml) + tmp.close() + model = load_model(tmp.name) + os.unlink(tmp.name) cameras = model.cameras front_cam = cameras[0] diff --git a/scene.xml b/scene.xml index 2c6fafa..1b79334 100644 --- a/scene.xml +++ b/scene.xml @@ -54,29 +54,8 @@ material="map_mat" friction="0.6 0.1 0.1" condim="3"/> - - - - - - - - - - - - - - - - - - - - - - - + +