27 lines
1.0 KiB
Python
27 lines
1.0 KiB
Python
import time
|
|
import numpy as np
|
|
from pathlib import Path
|
|
from tools.math_utils import get_body_velocity
|
|
|
|
class SimpleLogger:
|
|
def __init__(self, log_dir="."):
|
|
self.log_file = Path(log_dir) / f"sim2sim_log_{int(time.time())}.txt"
|
|
self.data = []
|
|
print(f"[Logger] Will write diagnostic data to: {self.log_file}")
|
|
|
|
def update(self, sim_time, m, d, command):
|
|
true_vel_body = get_body_velocity(m, d)
|
|
# 记录 仿真时间、控制指令、真实的机体线速度
|
|
self.data.append(
|
|
f"{sim_time:.4f}, {command[0]:.4f}, {command[1]:.4f}, {command[2]:.4f}, "
|
|
f"{true_vel_body[0]:.4f}, {true_vel_body[1]:.4f}, {true_vel_body[2]:.4f}"
|
|
)
|
|
|
|
def save(self):
|
|
print(f"\n[Logger] Saving {len(self.data)} records to {self.log_file}...")
|
|
with open(self.log_file, "w") as f:
|
|
f.write("time, cmd_vx, cmd_vy, cmd_yaw, true_vx, true_vy, true_vz\n")
|
|
for row in self.data:
|
|
f.write(row + "\n")
|
|
print("[Logger] Save complete.")
|