diff --git a/README.md b/README.md
index 466b7c9..21ac648 100644
--- a/README.md
+++ b/README.md
@@ -63,18 +63,58 @@ curl http://{IP}:8765/state
```json
{
- "x": -2.0,
- "y": -2.3,
- "z": 0.08,
- "yaw_deg": 0.0,
- "speed_target": 1.0,
- "speed_actual": 0.98,
- "steer_target": 0.0,
- "speed_amp": 1.0,
- "steer_amp": 0.35
+ "x": -2.0, "y": -2.3, "z": 0.08, "yaw_deg": 0.0,
+ "vx": 0.98, "vy": 0.0, "wx": 0.0, "wy": 0.0, "wz": 0.01,
+ "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 geometry_msgs.msg import Twist
-from cv_bridge import CvBridge
-import requests
-import numpy as np
-import cv2
+from sensor_msgs.msg import Image, Imu, LaserScan
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import Twist, Quaternion
+from std_msgs.msg import Float32
+import requests, math, numpy as np, cv2
IP = "192.168.1.100"
class OrigincarBridge(Node):
def __init__(self):
super().__init__("origincar_bridge")
- self.bridge = CvBridge()
- self.pub = self.create_publisher(RosImage, "/image", 10)
+ self.pub_scan = self.create_publisher(LaserScan, "/scan", 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):
- r = requests.get(f"http://{IP}:8765/image", timeout=1)
- if r.status_code == 200:
- arr = np.frombuffer(r.content, np.uint8)
- cv_img = cv2.imdecode(arr, cv2.IMREAD_COLOR)
- msg = self.bridge.cv2_to_imgmsg(cv_img, "bgr8")
- self.pub.publish(msg)
+ def sync(self):
+ # LaserScan
+ data = requests.get(f"http://{IP}:8765/scan", timeout=1).json()
+ msg = LaserScan()
+ msg.header.frame_id = "laser"; msg.header.stamp = self.get_clock().now().to_msg()
+ msg.angle_min = data["angle_min"]; msg.angle_max = data["angle_max"]
+ 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,
- "steer": msg.angular.z
+ "vx": msg.linear.x, "wz": msg.angular.z
})
rclpy.init()
diff --git a/main.py b/main.py
index 54eef59..592119c 100644
--- a/main.py
+++ b/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:
- shared["cmd_speed"] = float(body.get("speed", 0))
- shared["cmd_steer"] = float(body.get("steer", 0))
+ # 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)
diff --git a/pyproject.toml b/pyproject.toml
index da56330..cdcd9d9 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -7,4 +7,5 @@ requires-python = ">=3.13"
dependencies = [
"motrixsim-core",
"pillow",
+ "qrcode[pil]>=8.2",
]
diff --git a/qr_code.png b/qr_code.png
new file mode 100644
index 0000000..e4c424f
Binary files /dev/null and b/qr_code.png differ
diff --git a/scene.xml b/scene.xml
index 1b79334..14ea378 100644
--- a/scene.xml
+++ b/scene.xml
@@ -22,6 +22,9 @@
+
+
+
@@ -54,6 +57,12 @@
material="map_mat" friction="0.6 0.1 0.1" condim="3"/>
+
+
+
+
+
+
@@ -81,6 +90,11 @@
rgba="0.965 0.596 0.596 1"/>
+
+