增加雷达

README.md

@@ -63,18 +63,58 @@ curl http://{IP}:8765/state
 
 ```json
 {
-  "x": -2.0,
+  "x": -2.0, "y": -2.3, "z": 0.08, "yaw_deg": 0.0,
-  "y": -2.3,
+  "vx": 0.98, "vy": 0.0, "wx": 0.0, "wy": 0.0, "wz": 0.01,
-  "z": 0.08,
+  "speed_target": 1.0, "speed_actual": 0.98, "steer_target": 0.0,
-  "yaw_deg": 0.0,
+  "speed_amp": 1.0, "steer_amp": 0.35
-  "speed_target": 1.0,
-  "speed_actual": 0.98,
-  "steer_target": 0.0,
-  "speed_amp": 1.0,
-  "steer_amp": 0.35
 }
 ```
 
+### GET `/scan`
+
+模拟 LSLIDAR N10 2D 激光雷达（360°, 450 点）。
+
+```bash
+curl http://{IP}:8765/scan
+```
+
+返回示例：
+
+```json
+{
+  "angle_min": -3.14, "angle_max": 3.14, "angle_increment": 0.014,
+  "range_min": 0.15, "range_max": 3.0,
+  "ranges": [0.52, 0.51, 0.53, ...]
+}
+```
+
+### GET `/odom`
+
+里程计数据（匹配 `nav_msgs/Odometry`）。
+
+```bash
+curl http://{IP}:8765/odom
+```
+
+返回示例：
+
+```json
+{
+  "x": -2.0, "y": -2.3, "yaw": 3.14,
+  "vx": 0.98, "vy": 0.0, "wz": 0.01,
+  "orientation": {"w": 0.0, "x": 0.0, "y": 0.0, "z": 1.0}
+}
+```
+
+### GET `/power`
+
+电池电压（模拟 12V）。
+
+```bash
+curl http://{IP}:8765/power
+# → {"voltage": 12.0}
+```
+
 ### GET `/imu`
 
 获取 IMU 数据（含 MPU6050 噪声），JSON。
@@ -107,11 +147,28 @@ curl -X POST http://{IP}:8765/cmd \
   -d '{"speed": 1.0, "steer": 0.3}'
 ```
 
+**Ackermann 模式**：
+
 | 字段 | 类型 | 说明 |
 |------|------|------|
 | `speed` | float | 目标速度 m/s，正=前进，负=后退 |
 | `steer` | float | 目标转向角 rad，正=左转，负=右转 |
 
+**差速模式**（兼容 `cmd_vel/Twist`）：
+
+| 字段 | 类型 | 说明 |
+|------|------|------|
+| `vx` | float | 线速度 m/s |
+| `vy` | float | 横向速度 m/s（忽略） |
+| `wz` | float | 角速度 rad/s，自动转为转向角 |
+
+```bash
+# 差速模式
+curl -X POST http://{IP}:8765/cmd \
+  -H "Content-Type: application/json" \
+  -d '{"vx": 1.0, "wz": 0.5}'
+```
+
 **Python 客户端示例**：
 
 ```python
@@ -135,40 +192,69 @@ time.sleep(2)
 requests.post(f"http://{IP}:8765/cmd", json={"speed": 0.0, "steer": 0.0})
 ```
 
-**ROS2 桥接示例**：
+**ROS2 桥接示例**（仿真机 HTTP → ROS2 局域网）：
 
 ```python
 import rclpy
 from rclpy.node import Node
-from sensor_msgs.msg import Image as RosImage
+from sensor_msgs.msg import Image, Imu, LaserScan
-from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
-from cv_bridge import CvBridge
+from geometry_msgs.msg import Twist, Quaternion
-import requests
+from std_msgs.msg import Float32
-import numpy as np
+import requests, math, numpy as np, cv2
-import cv2
 
 IP = "192.168.1.100"
 
 class OrigincarBridge(Node):
     def __init__(self):
         super().__init__("origincar_bridge")
-        self.bridge = CvBridge()
+        self.pub_scan = self.create_publisher(LaserScan, "/scan", 10)
-        self.pub = self.create_publisher(RosImage, "/image", 10)
+        self.pub_odom = self.create_publisher(Odometry, "/odom", 10)
+        self.pub_imu  = self.create_publisher(Imu, "/imu/data_raw", 10)
+        self.pub_pwr  = self.create_publisher(Float32, "/PowerVoltage", 10)
         self.sub = self.create_subscription(Twist, "/cmd_vel", self.cmd_cb, 10)
-        self.timer = self.create_timer(0.1, self.fetch_and_publish)
+        self.timer = self.create_timer(0.033, self.sync)  # ~30Hz
 
-    def fetch_and_publish(self):
+    def sync(self):
-        r = requests.get(f"http://{IP}:8765/image", timeout=1)
+        # LaserScan
-        if r.status_code == 200:
+        data = requests.get(f"http://{IP}:8765/scan", timeout=1).json()
-            arr = np.frombuffer(r.content, np.uint8)
+        msg = LaserScan()
-            cv_img = cv2.imdecode(arr, cv2.IMREAD_COLOR)
+        msg.header.frame_id = "laser"; msg.header.stamp = self.get_clock().now().to_msg()
-            msg = self.bridge.cv2_to_imgmsg(cv_img, "bgr8")
+        msg.angle_min = data["angle_min"]; msg.angle_max = data["angle_max"]
-            self.pub.publish(msg)
+        msg.angle_increment = data["angle_increment"]
+        msg.range_min = data["range_min"]; msg.range_max = data["range_max"]
+        msg.ranges = data["ranges"]
+        self.pub_scan.publish(msg)
+
+        # Odometry
+        odom = requests.get(f"http://{IP}:8765/odom", timeout=1).json()
+        o = Odometry()
+        o.header.frame_id = "odom"; o.child_frame_id = "base_link"
+        o.pose.pose.position.x = odom["x"]; o.pose.pose.position.y = odom["y"]
+        o.pose.pose.orientation = Quaternion(**odom["orientation"])
+        o.twist.twist.linear.x = odom["vx"]; o.twist.twist.angular.z = odom["wz"]
+        self.pub_odom.publish(o)
+
+        # IMU
+        imu_d = requests.get(f"http://{IP}:8765/imu", timeout=1).json()
+        imu = Imu()
+        imu.header.frame_id = "gyro_link"
+        imu.orientation = Quaternion(**imu_d["orientation"])
+        imu.angular_velocity.x = imu_d["angular_velocity"]["x"]
+        imu.angular_velocity.y = imu_d["angular_velocity"]["y"]
+        imu.angular_velocity.z = imu_d["angular_velocity"]["z"]
+        imu.linear_acceleration.x = imu_d["linear_acceleration"]["x"]
+        imu.linear_acceleration.y = imu_d["linear_acceleration"]["y"]
+        imu.linear_acceleration.z = imu_d["linear_acceleration"]["z"]
+        self.pub_imu.publish(imu)
+
+        # Power
+        p = Float32(data=requests.get(f"http://{IP}:8765/power", timeout=1).json()["voltage"])
+        self.pub_pwr.publish(p)
 
     def cmd_cb(self, msg: Twist):
         requests.post(f"http://{IP}:8765/cmd", json={
-            "speed": msg.linear.x,
+            "vx": msg.linear.x, "wz": msg.angular.z
-            "steer": msg.angular.z
         })
 
 rclpy.init()

main.py

@@ -9,10 +9,11 @@ from collections import deque
 from http.server import BaseHTTPRequestHandler, HTTPServer
 
 import numpy as np
+import qrcode
 from PIL import Image as PILImage
 
 from motrixsim import SceneData, load_model, run, step
-from motrixsim.render import CaptureTask, Layout, RenderApp
+from motrixsim.render import CaptureTask, Color, Layout, RenderApp
 
 # ── Ackermann geometry ──────────────────────────────────────────────
 L = 0.1682  # wheelbase: front-rear axle distance
@@ -20,6 +21,60 @@ T = 0.189   # track width: left-right wheel distance
 
 PORT = 8765
 
+# ── Laser scan parameters ────────────────────────────────────────────
+SCAN_POINTS = 450
+SCAN_ANGLE_MIN = -math.pi       # -180°
+SCAN_ANGLE_MAX = math.pi        # +180°
+SCAN_RANGE_MIN = 0.15           # m
+SCAN_RANGE_MAX = 3.0            # m
+
+
+def raycast_scan(car_x, car_y, car_yaw, obstacles):
+    """Simulate 2D laser scan. obstacles = [(ox, oy, radius), ...]."""
+    N = SCAN_POINTS
+    angles = np.linspace(SCAN_ANGLE_MIN, SCAN_ANGLE_MAX, N)
+    ranges = np.full(N, SCAN_RANGE_MAX)
+    cos_a = np.cos(angles + car_yaw)
+    sin_a = np.sin(angles + car_yaw)
+
+    for ox, oy, r in obstacles:
+        dx = ox - car_x
+        dy = oy - car_y
+        # Project obstacle to ray direction: t = dx*cos + dy*sin
+        t = dx * cos_a + dy * sin_a
+        # Perpendicular distance squared
+        d2 = dx * dx + dy * dy - t * t
+        # Intersection: ray hits circle if d2 < r^2 and t > 0
+        mask = (d2 < r * r) & (t > 0)
+        if not mask.any():
+            continue
+        # Distance along ray to circle intersection
+        hit_dist = t[mask] - np.sqrt(r * r - d2[mask])
+        hit_dist = np.maximum(hit_dist, SCAN_RANGE_MIN)
+        ranges[mask] = np.minimum(ranges[mask], hit_dist)
+
+    # Add Gaussian noise (~1cm)
+    ranges += np.random.normal(0, 0.01, N)
+    ranges = np.clip(ranges, SCAN_RANGE_MIN, SCAN_RANGE_MAX)
+    return angles, ranges
+
+
+def build_obstacle_list(cones, fences=True):
+    """cones: list of {"x":, "y":}; fences: include map boundary walls."""
+    obs = []
+    for c in cones:
+        obs.append((c["x"], c["y"], 0.10))  # cone radius ~10cm
+    if fences:
+        # Fence walls as small circles along the perimeter
+        for x in np.linspace(-2.5, 2.5, 20):
+            obs.append((x, 2.5, 0.02))
+            obs.append((x, -2.5, 0.02))
+        for y in np.linspace(-2.5, 2.5, 20):
+            obs.append((2.5, y, 0.02))
+            obs.append((-2.5, y, 0.02))
+    return obs
+
+
 # ── MPU6050 noise parameters ────────────────────────────────────────
 GYRO_NOISE_STD = 0.01     # rad/s
 ACCEL_NOISE_STD = 0.05    # m/s²
@@ -70,6 +125,9 @@ shared = {
     "cmd_steer": 0.0,         # from POST /cmd
     "imu_json": b"{}",
     "state_json": b"{}",
+    "scan_json": b"{}",
+    "odom_json": b"{}",
+    "power_json": b"{}",
     "key_active": False,      # True when W/A/S/D pressed
 }
 shared_lock = threading.Lock()
@@ -120,6 +178,30 @@ class HTTPHandler(BaseHTTPRequestHandler):
             self.send_header("Content-Length", str(len(body)))
             self.end_headers()
             self.wfile.write(body)
+        elif self.path == "/scan":
+            with shared_lock:
+                body = shared["scan_json"]
+            self.send_response(200)
+            self.send_header("Content-Type", "application/json")
+            self.send_header("Content-Length", str(len(body)))
+            self.end_headers()
+            self.wfile.write(body)
+        elif self.path == "/odom":
+            with shared_lock:
+                body = shared["odom_json"]
+            self.send_response(200)
+            self.send_header("Content-Type", "application/json")
+            self.send_header("Content-Length", str(len(body)))
+            self.end_headers()
+            self.wfile.write(body)
+        elif self.path == "/power":
+            with shared_lock:
+                body = shared["power_json"]
+            self.send_response(200)
+            self.send_header("Content-Type", "application/json")
+            self.send_header("Content-Length", str(len(body)))
+            self.end_headers()
+            self.wfile.write(body)
         else:
             self.send_response(404)
             self.end_headers()
@@ -129,8 +211,20 @@ class HTTPHandler(BaseHTTPRequestHandler):
             length = int(self.headers.get("Content-Length", 0))
             body = json.loads(self.rfile.read(length))
             with shared_lock:
+                # Ackermann mode: {"speed": v, "steer": angle}
+                if "speed" in body or "steer" in body:
                     shared["cmd_speed"] = float(body.get("speed", 0))
                     shared["cmd_steer"] = float(body.get("steer", 0))
+                # Differential mode: {"vx":, "vy":, "wz":} → mapped to speed/steer
+                elif "vx" in body:
+                    vx = float(body.get("vx", 0))
+                    wz = float(body.get("wz", 0))
+                    shared["cmd_speed"] = vx
+                    # Convert angular.z to steer via bike model (wheelbase ≈ 0.17m)
+                    if abs(vx) > 0.01:
+                        shared["cmd_steer"] = math.atan(wz * L / vx)
+                    else:
+                        shared["cmd_steer"] = 0.0
             self.send_response(200)
             self.end_headers()
         else:
@@ -152,9 +246,21 @@ def main():
     with RenderApp() as render:
         render.opt.set_left_panel_vis(True)
 
+        # Generate QR code (random 1~9999)
+        qr_num = random.randint(1, 9999)
+        qr_img = qrcode.make(str(qr_num), border=1).get_image()
+        qr_img = qr_img.resize((200, 200), PILImage.LANCZOS)
+        qr_img.save("qr_code.png")
+        print(f"QR code: {qr_num}")
+
         # Load cone positions and inject into scene
         with open("cones.json") as f:
             cones = json.load(f)
+
+        # Build obstacle list for laser scan simulation
+        obstacles = build_obstacle_list(cones)
+        obstacles.append((2.25, -0.72, 0.12))  # signboard
+
         cone_xml = ""
         for i, c in enumerate(cones):
             cone_xml += (
@@ -205,6 +311,7 @@ def main():
         speed_amp = 1.0
         steer_amp = 0.35
         frame = 0
+        scan_points = []  # cached hit points for gizmo drawing [(x, y, z), ...]
 
         base_link = model.get_link("base_link")
 
@@ -287,7 +394,16 @@ def main():
                 }).encode()
 
         def render_step():
-            nonlocal speed, steer_cmd, capture_index, frame, speed_amp, steer_amp
+            nonlocal speed, steer_cmd, capture_index, frame, speed_amp, steer_amp, scan_points
+
+            # Draw scan rays BEFORE sync (origin at top of car)
+            if scan_points:
+                pose = base_link.get_pose(data)
+                cx, cy, cz = pose[0], pose[1], pose[2] + 0.12
+                green = Color.rgb(0.0, 1.0, 0.0)
+                green.a = 0.5
+                for px, py, pz in scan_points:
+                    render.gizmos.draw_line([cx, cy, cz], [px, py, pz], green)
 
             render.sync(data)
             inp = render.input
@@ -327,6 +443,29 @@ def main():
             with shared_lock:
                 shared["key_active"] = key_active
 
+            # ── Laser scan (~30 Hz) ──
+            if frame % 2 == 0:
+                pose = base_link.get_pose(data)
+                x, y = pose[0], pose[1]
+                qw, qx, qy, qz = pose[3], pose[4], pose[5], pose[6]
+                yaw = math.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz))
+                z_scan = pose[2] + 0.12  # lidar on top of car body
+                angles, ranges = raycast_scan(x, y, yaw, obstacles)
+                scan = {
+                    "angle_min": float(angles[0]), "angle_max": float(angles[-1]),
+                    "angle_increment": float(angles[1] - angles[0]),
+                    "range_min": SCAN_RANGE_MIN, "range_max": SCAN_RANGE_MAX,
+                    "ranges": [round(float(r), 3) for r in ranges],
+                }
+                with shared_lock:
+                    shared["scan_json"] = json.dumps(scan).encode()
+                # Cache hit points for gizmo (downsample 10x)
+                scan_points = []
+                for i in range(0, len(angles), 10):
+                    a = angles[i] + yaw
+                    r = ranges[i]
+                    scan_points.append((x + r * math.cos(a), y + r * math.sin(a), z_scan))
+
             # ── Continuous image capture (~30 Hz) ──
             if frame % 2 == 0:
                 rcam = render.get_camera(0)
@@ -363,15 +502,29 @@ def main():
                 v = base_link.get_linear_velocity(data)
                 actual_spd = math.hypot(v[0], v[1])
                 with shared_lock:
+                    omega = base_link.get_angular_velocity(data)
                     shared["state_json"] = json.dumps({
                         "x": round(float(x), 3), "y": round(float(y), 3), "z": round(float(z), 3),
                         "yaw_deg": round(math.degrees(yaw), 1),
+                        "vx": round(float(v[0]), 3), "vy": round(float(v[1]), 3),
+                        "wx": round(float(omega[0]), 3), "wy": round(float(omega[1]), 3), "wz": round(float(omega[2]), 3),
                         "speed_target": round(speed, 2),
                         "speed_actual": round(float(actual_spd), 2),
                         "steer_target": round(steer_cmd, 3),
                         "speed_amp": round(speed_amp, 1),
                         "steer_amp": round(steer_amp, 3),
                     }).encode()
+                    # Odometry (matches nav_msgs/Odometry)
+                    shared["odom_json"] = json.dumps({
+                        "x": round(float(x), 3), "y": round(float(y), 3),
+                        "yaw": round(float(yaw), 4),
+                        "vx": round(float(v[0]), 3), "vy": round(float(v[1]), 3),
+                        "wz": round(float(omega[2]), 3),
+                        "orientation": {"w": round(float(qw), 4), "x": round(float(qx), 4),
+                                        "y": round(float(qy), 4), "z": round(float(qz), 4)},
+                    }).encode()
+                    # Power (simulated battery ~12V)
+                    shared["power_json"] = json.dumps({"voltage": 12.0}).encode()
                 print(f"[pos] x={x:+.3f} y={y:+.3f} z={z:+.3f} | yaw={math.degrees(yaw):+.1f}°  "
                       f"spd={speed:.1f}/{actual_spd:.2f} steer={steer_cmd:+.2f}", flush=True)
 

pyproject.toml

@@ -7,4 +7,5 @@ requires-python = ">=3.13"
 dependencies = [
     "motrixsim-core",
     "pillow",
+    "qrcode[pil]>=8.2",
 ]

scene.xml

@@ -22,6 +22,9 @@
     <texture name="map_tex" type="2d" file="map_full.png"/>
     <material name="map_mat" texture="map_tex" texrepeat="0.2 0.2" texuniform="true" reflectance="0.0"/>
 
+    <texture name="qr_tex" type="2d" file="qr_code.png"/>
+    <material name="qr_mat" texture="qr_tex" texrepeat="1 1" texuniform="true"/>
+
     <texture type="skybox" builtin="gradient" rgb1="0.4 0.5 0.6" rgb2="0 0 0" width="512" height="512"/>
   </asset>
 
@@ -54,6 +57,12 @@
       material="map_mat" friction="0.6 0.1 0.1" condim="3"/>
 
     <!-- 40cm white fence around the map -->
+    <!-- QR signboard: 25cm(H)×20cm(W)×1cm(D), tilted 15°, facing SW -->
+    <body name="signboard" pos="2.25 -0.72 0.125" euler="-0.262 0 2.242">
+      <geom name="board_white" type="box" size="0.10 0.005 0.125" rgba="1 1 1 1"/>
+      <geom name="board_qr" type="box" size="0.10 0.001 0.10" pos="0 0.006 0.025" material="qr_mat"/>
+    </body>
+
     <!-- ===== Blue cone obstacles (auto-generated from cones.json) ===== -->
     <!-- CONES -->
 
@@ -81,6 +90,11 @@
           rgba="0.965 0.596 0.596 1"/>
       </body>
 
+      <!-- Lidar on top of car -->
+      <body name="lidar" pos="0 0 0.12">
+        <geom name="lidar_geom" type="cylinder" size="0.02 0.015" rgba="0.2 0.2 0.2 1"/>
+      </body>
+
       <!-- ===== Rear wheels (drive only, no steering) ===== -->
       <body name="down_left_Link" pos="-0.0841 0.0945 0.03">
         <joint name="down_left_joint" axis="0 1 0"/>

uv.lock

@@ -11,6 +11,15 @@ wheels = [
     { url = "https://files.pythonhosted.org/packages/a2/ad/e0d3c824784ff121c03cc031f944bc7e139a8f1870ffd2845cc2dd76f6c4/absl_py-2.1.0-py3-none-any.whl", hash = "sha256:526a04eadab8b4ee719ce68f204172ead1027549089702d99b9059f129ff1308", size = 133706, upload-time = "2024-01-16T22:14:24.055Z" },
 ]
 
+[[package]]
+name = "colorama"
+version = "0.4.6"
+source = { registry = "https://pypi.org/simple" }
+sdist = { url = "https://files.pythonhosted.org/packages/d8/53/6f443c9a4a8358a93a6792e2acffb9d9d5cb0a5cfd8802644b7b1c9a02e4/colorama-0.4.6.tar.gz", hash = "sha256:08695f5cb7ed6e0531a20572697297273c47b8cae5a63ffc6d6ed5c201be6e44", size = 27697, upload-time = "2022-10-25T02:36:22.414Z" }
+wheels = [
+    { url = "https://files.pythonhosted.org/packages/d1/d6/3965ed04c63042e047cb6a3e6ed1a63a35087b6a609aa3a15ed8ac56c221/colorama-0.4.6-py2.py3-none-any.whl", hash = "sha256:4f1d9991f5acc0ca119f9d443620b77f9d6b33703e51011c16baf57afb285fc6", size = 25335, upload-time = "2022-10-25T02:36:20.889Z" },
+]
+
 [[package]]
 name = "motrixsim-core"
 version = "0.8.2"
@@ -33,12 +42,14 @@ source = { virtual = "." }
 dependencies = [
     { name = "motrixsim-core" },
     { name = "pillow" },
+    { name = "qrcode", extra = ["pil"] },
 ]
 
 [package.metadata]
 requires-dist = [
     { name = "motrixsim-core" },
     { name = "pillow" },
+    { name = "qrcode", extras = ["pil"], specifier = ">=8.2" },
 ]
 
 [[package]]
@@ -148,3 +159,20 @@ wheels = [
     { url = "https://files.pythonhosted.org/packages/ba/13/306d275efd3a3453f72114b7431c877d10b1154014c1ebbedd067770d629/pillow-12.2.0-cp314-cp314t-win_amd64.whl", hash = "sha256:6562ace0d3fb5f20ed7290f1f929cae41b25ae29528f2af1722966a0a02e2aa1", size = 7225152, upload-time = "2026-04-01T14:45:50.032Z" },
     { url = "https://files.pythonhosted.org/packages/ff/6e/cf826fae916b8658848d7b9f38d88da6396895c676e8086fc0988073aaf8/pillow-12.2.0-cp314-cp314t-win_arm64.whl", hash = "sha256:aa88ccfe4e32d362816319ed727a004423aab09c5cea43c01a4b435643fa34eb", size = 2556579, upload-time = "2026-04-01T14:45:52.529Z" },
 ]
+
+[[package]]
+name = "qrcode"
+version = "8.2"
+source = { registry = "https://pypi.org/simple" }
+dependencies = [
+    { name = "colorama", marker = "sys_platform == 'win32'" },
+]
+sdist = { url = "https://files.pythonhosted.org/packages/8f/b2/7fc2931bfae0af02d5f53b174e9cf701adbb35f39d69c2af63d4a39f81a9/qrcode-8.2.tar.gz", hash = "sha256:35c3f2a4172b33136ab9f6b3ef1c00260dd2f66f858f24d88418a015f446506c", size = 43317, upload-time = "2025-05-01T15:44:24.726Z" }
+wheels = [
+    { url = "https://files.pythonhosted.org/packages/dd/b8/d2d6d731733f51684bbf76bf34dab3b70a9148e8f2cef2bb544fccec681a/qrcode-8.2-py3-none-any.whl", hash = "sha256:16e64e0716c14960108e85d853062c9e8bba5ca8252c0b4d0231b9df4060ff4f", size = 45986, upload-time = "2025-05-01T15:44:22.781Z" },
+]
+
+[package.optional-dependencies]
+pil = [
+    { name = "pillow" },
+]