增加雷达
This commit is contained in:
cyy_mac
161
main.py
161
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)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user