308 lines
8.0 KiB
Markdown
308 lines
8.0 KiB
Markdown
# Origincar Simulation
|
||
|
||
基于 MotrixSim 的阿克曼小车仿真，支持键盘控制和 HTTP API 局域网共享。
|
||
|
||
|
||
## 依赖
|
||
|
||
| 依赖 | 说明 | 安装 |
|
||
|------|------|------|
|
||
| Python ≥3.13 | 运行环境 | `brew install python` 或 [python.org](https://python.org) |
|
||
| [uv](https://docs.astral.sh/uv/) | 包管理器 | `curl -LsSf https://astral.sh/uv/install.sh \| sh` |
|
||
| `motrixsim-core` | 物理仿真引擎 | `uv sync` 自动安装 |
|
||
| `pillow` | 摄像头图像转 JPEG | `uv sync` 自动安装 |
|
||
|
||
```bash
|
||
# 克隆后一键安装
|
||
uv sync
|
||
```
|
||
|
||
> `mxpython` 是 motrixsim-core 自带的启动器，**macOS 上必须使用**（主线程需保留给渲染），Linux / Windows 可直接用 `python`。
|
||
|
||
## 启动
|
||
|
||
```bash
|
||
# macOS
|
||
uv run mxpython main.py
|
||
|
||
# Linux / Windows
|
||
uv run python main.py
|
||
```
|
||
|
||
## HTTP API（端口 8765）
|
||
|
||
仿真启动后，局域网内其他设备可通过 HTTP 获取摄像头图像、IMU 数据和发送控制指令。
|
||
|
||
### GET `/image`
|
||
|
||
获取前置摄像头最新 JPEG 图像。浏览器可直接打开。
|
||
|
||
```bash
|
||
# 浏览器
|
||
http://<仿真机IP>:8765/image
|
||
|
||
# curl
|
||
curl http://1{IP}:8765/image -o frame.jpg
|
||
|
||
# Python
|
||
import requests
|
||
r = requests.get("http://{IP}:8765/image")
|
||
with open("frame.jpg", "wb") as f:
|
||
f.write(r.content)
|
||
```
|
||
|
||
### GET `/state`
|
||
|
||
获取小车当前状态（JSON）。
|
||
|
||
```bash
|
||
curl http://{IP}:8765/state
|
||
```
|
||
|
||
返回示例：
|
||
|
||
```json
|
||
{
|
||
"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。
|
||
|
||
```bash
|
||
curl http://{IP}:8765/imu
|
||
```
|
||
|
||
返回示例：
|
||
|
||
```json
|
||
{
|
||
"orientation": {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0},
|
||
"angular_velocity": {"x": 0.001, "y": -0.003, "z": 0.008},
|
||
"linear_acceleration": {"x": 0.02, "y": -0.01, "z": 9.81}
|
||
}
|
||
```
|
||
|
||
噪声参数（MPU6050 级别）：
|
||
- 陀螺仪白噪声 σ = 0.01 rad/s，bias 随机游走
|
||
- 加速度计白噪声 σ = 0.05 m/s²，bias 随机游走
|
||
|
||
### POST `/cmd`
|
||
|
||
发送控制指令（JSON）。
|
||
|
||
```bash
|
||
curl -X POST http://{IP}:8765/cmd \
|
||
-H "Content-Type: application/json" \
|
||
-d '{"speed": 1.0, "steer": 0.3}'
|
||
```
|
||
|
||
匹配串口协议（11 字节帧），对接 `cmd_vel/Twist`：
|
||
|
||
| 字段 | 类型 | 说明 |
|
||
|------|------|------|
|
||
| `vx` | float | 线速度 m/s（对应 `X_speed`） |
|
||
| `wz` | float | 角速度 rad/s（对应 `Z_speed`，固件内用 bicycle model 转阿克曼转向角） |
|
||
|
||
```bash
|
||
curl -X POST http://{IP}:8765/cmd \
|
||
-H "Content-Type: application/json" \
|
||
-d '{"vx": 1.0, "wz": 0.5}'
|
||
```
|
||
|
||
**Python 客户端示例**：
|
||
|
||
```python
|
||
import time
|
||
import requests
|
||
|
||
IP = "192.168.1.100"
|
||
|
||
# 获取图像
|
||
r = requests.get(f"http://{IP}:8765/image")
|
||
with open("frame.jpg", "wb") as f:
|
||
f.write(r.content)
|
||
|
||
# 获取状态
|
||
state = requests.get(f"http://{IP}:8765/state").json()
|
||
print(f"位置: ({state['x']}, {state['y']}), 速度: {state['speed_actual']} m/s")
|
||
|
||
# 发送控制指令（串口协议格式：vx + wz）
|
||
requests.post(f"http://{IP}:8765/cmd", json={"vx": 1.0, "wz": 0.5})
|
||
time.sleep(2)
|
||
requests.post(f"http://{IP}:8765/cmd", json={"vx": 0.0, "wz": 0.0})
|
||
```
|
||
|
||
**ROS2 桥接示例**（仿真机 HTTP → ROS2 局域网）：
|
||
|
||
```python
|
||
import rclpy
|
||
from rclpy.node import Node
|
||
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.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.033, self.sync) # ~30Hz
|
||
|
||
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={
|
||
"vx": msg.linear.x, "wz": msg.angular.z
|
||
})
|
||
|
||
rclpy.init()
|
||
node = OrigincarBridge()
|
||
rclpy.spin(node)
|
||
```
|
||
|
||
## 键盘控制
|
||
|
||
| 按键 | 方式 | 作用 |
|
||
|------|------|------|
|
||
| W/S | 按住 | 前进/后退 |
|
||
| A/D | 按住 | 左转/右转 |
|
||
| ↑/↓ | 点按 | 速度幅度 ±0.2 m/s（0~2.0） |
|
||
| ←/→ | 点按 | 转向幅度 ±0.05 rad（0~0.5） |
|
||
| Space | 点按 | 截图保存到 `captures/` |
|
||
|
||
键盘与 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 — 场景描述（地图 + 围栏 + 小车，锥桶动态注入）
|
||
main.py — 仿真控制 + HTTP API 服务
|
||
cone_editor.py — 锥桶可视化编辑器
|
||
cones.json — 锥桶位置配置
|
||
cone.obj — 锥体网格（底20cm→顶5cm，高27cm）
|
||
cone_white.obj — 白色反光条网格
|
||
origincar.urdf / origincar.xacro — 小车模型（参考）
|
||
```
|