在抓取到gz sim的数据流后 ,就可以不需要这个工具了 完全可以用python来写一个服务替代gz的模拟数据
/////////////////////////////////////////////////////
#!/usr/bin/env python3
"""
Gazebo JSON 仿真服务器（应答模式）
-
绑定 UDP 9002 端口
-
每收到一个来自 SITL 的数据包，就发送一帧 JSON 数据作为响应
-
时间戳每次增加固定值（如 0.001 秒），运动积分使用实际接收间隔
"""
import socket
import json
import time
import sys
import random
import signal
BASE_PAYLOAD = {
"timestamp": 0.0,
"imu": {
"gyro": -1.3821477057480989e-15, -5.405425456198043e-16, 1.6857277913934e-18,
"accel_body": -1.372462947196204e-8, 2.101031229379505e-10, -9.8
},
"position": 2.573308782284005e-10, -3.938280660138266e-12, -0.19499942739960947,
"quaternion": 1.0, -1.0098482109629705e-11, -6.598405615460205e-10, 1.110223024564242e-15,
"velocity": 1.5902986085520575e-10, -2.4317334849982724e-12, -5.75753623985299e-06,
"no_time_sync": True,
"no_lockstep": False
}
def build_packet(payload):
return (json.dumps(payload, separators=(',', ':')) + '\n').encode('utf-8')
def update_payload(payload, dt_physical, dt_timestamp):
"""物理积分用 dt_physical，时间戳增量用 dt_timestamp"""
payload"timestamp" += dt_timestamp
payload"position"0 += payload"velocity"0 * dt_physical
payload"position"1 += payload"velocity"1 * dt_physical
payload"position"2 += payload"velocity"2 * dt_physical
添加噪声
payload"imu""gyro"0 += random.uniform(-1e-5, 1e-5)
payload"imu""gyro"1 += random.uniform(-1e-5, 1e-5)
payload"imu""gyro"2 += random.uniform(-1e-5, 1e-5)
payload"imu""accel_body"0 += random.uniform(-1e-3, 1e-3)
payload"imu""accel_body"1 += random.uniform(-1e-3, 1e-3)
payload"imu""accel_body"2 = -9.8 + random.uniform(-1e-3, 1e-3)
return payload
class JSONSimServer:
def init(self, bind_port=9002, timestamp_delta=0.001):
self.bind_port = bind_port
self.timestamp_delta = timestamp_delta
self.sock = None
self.client_addr = None
self.running = False
self.payload = {
"timestamp": BASE_PAYLOAD"timestamp",
"imu": {
"gyro": BASE_PAYLOAD"imu""gyro":,
"accel_body": BASE_PAYLOAD"imu""accel_body":
},
"position": BASE_PAYLOAD"position":,
"quaternion": BASE_PAYLOAD"quaternion":,
"velocity": BASE_PAYLOAD"velocity":,
"no_time_sync": BASE_PAYLOAD"no_time_sync",
"no_lockstep": BASE_PAYLOAD"no_lockstep"
}
def start(self):
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.sock.bind(('0.0.0.0', self.bind_port))
print(f"服务器 绑定 UDP {self.bind_port}，等待 SITL 数据包...")
self.running = True
last_recv_time = None
packet_count = 0
try:
while self.running:
data, addr = self.sock.recvfrom(4096)
if self.client_addr is None:
self.client_addr = addr
print(f"服务器 收到来自 {addr} 的连接，进入应答模式")
elif addr != self.client_addr:
忽略其他地址
continue
now = time.perf_counter()
if last_recv_time is None:
dt_physical = 0.01 # 默认步长
else:
dt_physical = now - last_recv_time
last_recv_time = now
更新仿真状态：物理步长用实际间隔，时间戳增量用固定值
self.payload = update_payload(self.payload, dt_physical, self.timestamp_delta)
packet = build_packet(self.payload)
self.sock.sendto(packet, self.client_addr)
packet_count += 1
if packet_count % 100 == 0:
print(f"服务器 已发送 {packet_count} 包, 时间戳={self.payload'timestamp':.3f}s")
except KeyboardInterrupt:
print("\n服务器 用户中断")
except Exception as e:
print(f"服务器 错误: {e}")
finally:
self.stop()
def stop(self):
self.running = False
if self.sock:
self.sock.close()
print("服务器 已停止")
def main():
port = 9002
timestamp_delta = 0.001 # 每收到一次，时间戳增加 0.001 秒
if len(sys.argv) > 1:
try:
timestamp_delta = float(sys.argv1)
except ValueError:
pass
if len(sys.argv) > 2:
try:
port = int(sys.argv2)
except ValueError:
pass
server = JSONSimServer(bind_port=port, timestamp_delta=timestamp_delta)
signal.signal(signal.SIGINT, lambda s, f: server.stop())
signal.signal(signal.SIGTERM, lambda s, f: server.stop())
server.start()
if name == "main":
main()
/////////////
根据抓包的数据每10m收到一个包 会包里面的timestamp 5.892 每次加 0.001
////////////////
如果需要longitude 变化 需要将 "velocity": 1.5902986085520575e-10, -2.4317334849982724e-12, -5.75753623985299e-06,
改成 "velocity": 1.5902986085520575e-10, 5.0, -5.75753623985299e-06,
///////////////////////////
./waf distclean
配置为 SITL 仿真平台 + 开启 debug 调试符号
./waf configure --debug --board=sitl
编译四轴飞控固件
./waf copter
//////启动sitl
/home/charlie/opt/ardu/ardupilot/build/sitl/bin/arducopter --model JSON --speedup 1 --slave 0 --sim-address=127.0.0.1 -I0
mavproxy.py --retries 5 --out 127.0.0.1:14550 --master tcp:127.0.0.1:5760
//////////////////////////////////快速解锁起飞
rc 3 1000
arm throttle
rc 3 1600
void JSON::output_servos(const struct sitl_input &input)
{
添加pwm给到电机的数值
static uint32_t ccc = 0;
if (ccc++ % 100 == 0) {
printf("JSON servo out (16ch): frame=%u rate=%u PWM=[", pkt.frame_count, pkt.frame_rate);
for (uint8_t i=0; i<8 && i<16; i++) {
printf("%s%u", i?",":"", pkt.pwmi);
}
printf("...]\n");
}
/////输出示例
PWM=1000,1000,1000,1000,0,0,0,0 //对应X型多旋翼的4个电机从右上角逆时针排序
JSON servo out (16ch): frame=20900 rate=1200 PWM=1000,1000,1000,1000,0,0,0,0
JSON servo out (16ch): frame=21000 rate=1200 PWM=1006,1006,1006,1006,0,0,0,0
JSON servo out (16ch): frame=21100 rate=1200 PWM=1036,1036,1036,1036,0,0,0,0
JSON servo out (16ch): frame=21200 rate=1200 PWM=1066,1066,1066,1066,0,0,0,0
JSON servo out (16ch): frame=21300 rate=1200 PWM=1096,1096,1096,1096,0,0,0,0
JSON servo out (16ch): frame=21400 rate=1200 PWM=1126,1126,1126,1126,0,0,0,0
JSON servo out (16ch): frame=21500 rate=1200 PWM=1150,1150,1150,1150,0,0,0,0
JSON servo out (16ch): frame=21600 rate=1200 PWM=1439,1439,1439,1439,0,0,0,0
JSON servo out (16ch): frame=21700 rate=1200 PWM=1577,1573,1573,1577,0,0,0,0
//////////////////////////
更新position