cyy/car_simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加雷达

Browse Source
This commit is contained in:
cyy_mac
2026-06-20 21:46:03 +08:00
parent cf94fc566b
commit 228efaa361
6 changed files with 314 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

142
README.md
View File

@@ -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()

161
main.py
View File

@@ -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)

1
pyproject.toml
View File

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

BIN
qr_code.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 387 B

14
scene.xml
View File

@@ -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"/>

28
uv.lock generated
View File

@@ -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" },
]