锥桶自定义更新

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 白色围栏 + 阿克曼小车
+scene.xml      — 场景描述（地图 + 围栏 + 小车，锥桶动态注入）
 main.py        — 仿真控制 + HTTP API 服务
+cone_editor.py — 锥桶可视化编辑器
+cones.json     — 锥桶位置配置
+cone.obj       — 锥体网格（底20cm→顶5cm，高27cm）
+cone_white.obj — 白色反光条网格
 origincar.urdf / origincar.xacro — 小车模型（参考）
 ```

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("<Button-1>", self.on_click)
+        self.canvas.bind("<Button-3>", self.on_right_click)
+        self.canvas.bind("<Motion>", 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()

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
+  }
+]

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    <body name="cone_{i}" pos="{c["x"]} {c["y"]} 0.0">\n'
+                f'      <geom type="box" size="0.10 0.10 0.015" pos="0 0 0.015" class="cone_blue"/>\n'
+                f'      <geom type="mesh" mesh="cone_mesh" pos="0 0 0.03" class="cone_blue"/>\n'
+                f'      <geom type="mesh" mesh="cone_white_mesh" pos="0 0 0.15" rgba="1 1 1 1"/>\n'
+                f'    </body>'
+            )
+        with open("scene.xml") as f:
+            xml = f.read()
+        xml = xml.replace("<!-- CONES -->", 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]

scene.xml

@@ -54,29 +54,8 @@
       material="map_mat" friction="0.6 0.1 0.1" condim="3"/>
 
     <!-- 40cm white fence around the map -->
-    <!-- ===== Blue cone obstacles ===== -->
+    <!-- ===== Blue cone obstacles (auto-generated from cones.json) ===== -->
-    <!-- Square base 20x20x3cm + cone taper 20cm→2cm over 27cm, total H=30cm -->
+    <!-- CONES -->
-    <!-- White reflective strip at 2/3 height (z=20cm) -->
-    <body name="cone_1" pos=" 1.0  1.0 0.0">
-      <geom type="box" size="0.10 0.10 0.015" pos="0 0 0.015" class="cone_blue"/>
-      <geom type="mesh" mesh="cone_mesh" pos="0 0 0.03" class="cone_blue"/>
-      <geom type="mesh" mesh="cone_white_mesh" pos="0 0 0.15" rgba="1 1 1 1"/>
-    </body>
-    <body name="cone_2" pos="-1.0  0.5 0.0">
-      <geom type="box" size="0.10 0.10 0.015" pos="0 0 0.015" class="cone_blue"/>
-      <geom type="mesh" mesh="cone_mesh" pos="0 0 0.03" class="cone_blue"/>
-      <geom type="mesh" mesh="cone_white_mesh" pos="0 0 0.15" rgba="1 1 1 1"/>
-    </body>
-    <body name="cone_3" pos=" 0.0 -1.5 0.0">
-      <geom type="box" size="0.10 0.10 0.015" pos="0 0 0.015" class="cone_blue"/>
-      <geom type="mesh" mesh="cone_mesh" pos="0 0 0.03" class="cone_blue"/>
-      <geom type="mesh" mesh="cone_white_mesh" pos="0 0 0.15" rgba="1 1 1 1"/>
-    </body>
-    <body name="cone_4" pos=" 1.5 -1.0 0.0">
-      <geom type="box" size="0.10 0.10 0.015" pos="0 0 0.015" class="cone_blue"/>
-      <geom type="mesh" mesh="cone_mesh" pos="0 0 0.03" class="cone_blue"/>
-      <geom type="mesh" mesh="cone_white_mesh" pos="0 0 0.15" rgba="1 1 1 1"/>
-    </body>
 
     <geom name="fence_n" type="box" size="2.5 0.02 0.2" pos="0  2.5 0.2" rgba="1 1 1 1"/>
     <geom name="fence_s" type="box" size="2.5 0.02 0.2" pos="0 -2.5 0.2" rgba="1 1 1 1"/>