fd286e6a9e
- Introduced record_episode.py for capturing ESP32 IMU data and RealSense images. - Added SwitchChangeDetector and SwitchLevelController for managing recording triggers. - Enhanced ESP32Bridge with new methods for reading samples and latest packets. - Updated verify.py and visualise.py to improve serial communication stability. - Modified .gitignore to include dataset, csv, and png directories.
596 lines
16 KiB
Python
596 lines
16 KiB
Python
import argparse
|
|
import ast
|
|
import csv
|
|
import math
|
|
import os
|
|
import re
|
|
import sys
|
|
import time
|
|
from datetime import datetime
|
|
|
|
os.environ.setdefault("MPLCONFIGDIR", "/tmp/matplotlib")
|
|
|
|
import matplotlib.pyplot as plt
|
|
import numpy as np
|
|
import serial
|
|
|
|
from pc_reader import PACKET_SIZE, SYNC, decode_packet
|
|
|
|
|
|
TEXT_LINE_RE = re.compile(
|
|
r"#?\s*(?P<count>\d+)\s+"
|
|
r"(?P<freq>[-+]?\d+(?:\.\d+)?)Hz\s+"
|
|
r"DIN=(?P<din>\[[^\]]+\])\s+"
|
|
r"DOUT=(?P<dout>\[[^\]]+\])\s+"
|
|
r"ACC=(?P<acc>\[[^\]]+\])\s+"
|
|
r"GYRO=(?P<gyro>\[[^\]]+\])\s+"
|
|
r"ANGLE=(?P<angle>\[[^\]]+\])"
|
|
)
|
|
|
|
|
|
def flatten_packet(data, t_s, freq_hz):
|
|
return {
|
|
"t_s": t_s,
|
|
"acc_x_g": data["acc_g"][0],
|
|
"acc_y_g": data["acc_g"][1],
|
|
"acc_z_g": data["acc_g"][2],
|
|
"gyro_x_dps": data["gyro_dps"][0],
|
|
"gyro_y_dps": data["gyro_dps"][1],
|
|
"gyro_z_dps": data["gyro_dps"][2],
|
|
"angle_x_deg": data["angle_deg"][0],
|
|
"angle_y_deg": data["angle_deg"][1],
|
|
"angle_z_deg": data["angle_deg"][2],
|
|
"temp_c": data["temp_c"],
|
|
"freq_hz": freq_hz,
|
|
"din0": data["din"][0],
|
|
"din1": data["din"][1],
|
|
"din2": data["din"][2],
|
|
"din3": data["din"][3],
|
|
"dout0": data["dout"][0],
|
|
"dout1": data["dout"][1],
|
|
}
|
|
|
|
|
|
def read_serial_samples(port, baud, seconds=None, max_samples=None, max_gap_ms=1000):
|
|
samples = []
|
|
buffer = bytearray()
|
|
last_t_ms = None
|
|
elapsed_s = 0.0
|
|
bad_delta_count = 0
|
|
|
|
ser = serial.Serial()
|
|
ser.port = port
|
|
ser.baudrate = baud
|
|
ser.timeout = 0.05
|
|
ser.write_timeout = 0.05
|
|
|
|
# Avoid unwanted ESP32 auto-reset / boot-mode toggling.
|
|
ser.dtr = False
|
|
ser.rts = False
|
|
|
|
ser.open()
|
|
|
|
try:
|
|
# Some USB-UART adapters may still change line states during open().
|
|
ser.dtr = False
|
|
ser.rts = False
|
|
|
|
ser.reset_input_buffer()
|
|
ser.reset_output_buffer()
|
|
|
|
print(f"Reading {port} at {baud} baud...", file=sys.stderr)
|
|
print("Waiting for ESP32 main.py startup...", file=sys.stderr)
|
|
time.sleep(4.0)
|
|
|
|
# Start timing after ESP32 startup wait.
|
|
start = time.monotonic()
|
|
|
|
while True:
|
|
if seconds is not None and time.monotonic() - start >= seconds:
|
|
break
|
|
if max_samples is not None and len(samples) >= max_samples:
|
|
break
|
|
|
|
chunk = ser.read(ser.in_waiting or 1)
|
|
if chunk:
|
|
buffer.extend(chunk)
|
|
|
|
while len(buffer) >= PACKET_SIZE:
|
|
sync_index = buffer.find(SYNC)
|
|
if sync_index < 0:
|
|
del buffer[:-1]
|
|
break
|
|
|
|
if sync_index:
|
|
del buffer[:sync_index]
|
|
|
|
if len(buffer) < PACKET_SIZE:
|
|
break
|
|
|
|
packet = bytes(buffer[:PACKET_SIZE])
|
|
|
|
if (sum(packet[:-1]) & 0xFF) != packet[-1]:
|
|
del buffer[0]
|
|
continue
|
|
|
|
del buffer[:PACKET_SIZE]
|
|
data = decode_packet(packet)
|
|
|
|
if last_t_ms is None:
|
|
freq_hz = 0.0
|
|
else:
|
|
delta = (data["t_ms"] - last_t_ms) & 0xFFFFFFFF
|
|
if 0 < delta <= max_gap_ms:
|
|
elapsed_s += delta / 1000.0
|
|
freq_hz = 1000.0 / delta
|
|
else:
|
|
bad_delta_count += 1
|
|
freq_hz = 0.0
|
|
|
|
last_t_ms = data["t_ms"]
|
|
samples.append(flatten_packet(data, elapsed_s, freq_hz))
|
|
|
|
if max_samples is not None and len(samples) >= max_samples:
|
|
break
|
|
|
|
finally:
|
|
ser.close()
|
|
|
|
if bad_delta_count:
|
|
print(f"Ignored {bad_delta_count} abnormal packet time delta(s).", file=sys.stderr)
|
|
|
|
return samples
|
|
|
|
|
|
def read_text_log(path):
|
|
samples = []
|
|
t_s = 0.0
|
|
|
|
with open(path, "r", encoding="utf-8") as f:
|
|
for line in f:
|
|
match = TEXT_LINE_RE.search(line)
|
|
if not match:
|
|
continue
|
|
|
|
freq_hz = float(match.group("freq"))
|
|
din = ast.literal_eval(match.group("din"))
|
|
dout = ast.literal_eval(match.group("dout"))
|
|
acc = ast.literal_eval(match.group("acc"))
|
|
gyro = ast.literal_eval(match.group("gyro"))
|
|
angle = ast.literal_eval(match.group("angle"))
|
|
|
|
if samples and freq_hz > 0:
|
|
t_s += 1.0 / freq_hz
|
|
|
|
samples.append(
|
|
{
|
|
"t_s": t_s,
|
|
"acc_x_g": acc[0],
|
|
"acc_y_g": acc[1],
|
|
"acc_z_g": acc[2],
|
|
"gyro_x_dps": gyro[0],
|
|
"gyro_y_dps": gyro[1],
|
|
"gyro_z_dps": gyro[2],
|
|
"angle_x_deg": angle[0],
|
|
"angle_y_deg": angle[1],
|
|
"angle_z_deg": angle[2],
|
|
"temp_c": np.nan,
|
|
"freq_hz": freq_hz,
|
|
"din0": din[0],
|
|
"din1": din[1],
|
|
"din2": din[2],
|
|
"din3": din[3],
|
|
"dout0": dout[0],
|
|
"dout1": dout[1],
|
|
}
|
|
)
|
|
|
|
return samples
|
|
|
|
|
|
def verify_gravity_body_g(roll_deg, pitch_deg, gravity_sign=1.0):
|
|
"""
|
|
Provider's roll/pitch gravity projection.
|
|
|
|
Assumption:
|
|
roll = angle_x_deg
|
|
pitch = angle_y_deg
|
|
yaw is ignored.
|
|
|
|
gravity_sign:
|
|
+1.0 means horizontal static gravity is approximately +Z.
|
|
-1.0 means horizontal static gravity is approximately -Z.
|
|
"""
|
|
roll = math.radians(roll_deg)
|
|
pitch = math.radians(pitch_deg)
|
|
|
|
g_x = -math.sin(pitch)
|
|
g_y = math.cos(pitch) * math.sin(roll)
|
|
g_z = math.cos(pitch) * math.cos(roll)
|
|
|
|
return np.array([g_x, g_y, g_z], dtype=float) * gravity_sign
|
|
|
|
|
|
def add_verification(
|
|
samples,
|
|
gravity_sign=1.0,
|
|
bias_seconds=1.0,
|
|
acc_deadband=0.10,
|
|
vel_decay=0.995,
|
|
):
|
|
"""
|
|
Add Provider-method gravity compensation and simple velocity integration.
|
|
|
|
Output columns:
|
|
Provider_gx_g / Provider_gy_g / Provider_gz_g
|
|
Provider_lin_acc_x_g / y / z
|
|
Provider_lin_acc_x_ms2 / y / z
|
|
Provider_vel_x_ms / y / z
|
|
Provider_lin_acc_norm_ms2
|
|
Provider_vel_norm_ms
|
|
"""
|
|
if not samples:
|
|
return samples
|
|
|
|
raw_lin_acc_ms2 = []
|
|
|
|
for sample in samples:
|
|
acc_body_g = np.array(
|
|
[
|
|
sample["acc_x_g"],
|
|
sample["acc_y_g"],
|
|
sample["acc_z_g"],
|
|
],
|
|
dtype=float,
|
|
)
|
|
|
|
gravity_body_g = verify_gravity_body_g(
|
|
sample["angle_x_deg"],
|
|
sample["angle_y_deg"],
|
|
gravity_sign=gravity_sign,
|
|
)
|
|
|
|
linear_acc_body_g = acc_body_g - gravity_body_g
|
|
linear_acc_body_ms2 = linear_acc_body_g * 9.81
|
|
|
|
sample["Provider_gx_g"] = gravity_body_g[0]
|
|
sample["Provider_gy_g"] = gravity_body_g[1]
|
|
sample["Provider_gz_g"] = gravity_body_g[2]
|
|
|
|
sample["Provider_lin_acc_x_g_raw"] = linear_acc_body_g[0]
|
|
sample["Provider_lin_acc_y_g_raw"] = linear_acc_body_g[1]
|
|
sample["Provider_lin_acc_z_g_raw"] = linear_acc_body_g[2]
|
|
|
|
raw_lin_acc_ms2.append(linear_acc_body_ms2)
|
|
|
|
raw_lin_acc_ms2 = np.array(raw_lin_acc_ms2, dtype=float)
|
|
|
|
t0 = samples[0]["t_s"]
|
|
bias_indices = [
|
|
i
|
|
for i, sample in enumerate(samples)
|
|
if sample["t_s"] - t0 <= bias_seconds
|
|
]
|
|
|
|
if bias_indices:
|
|
acc_bias = np.mean(raw_lin_acc_ms2[bias_indices], axis=0)
|
|
else:
|
|
acc_bias = np.zeros(3, dtype=float)
|
|
|
|
print("Provider method")
|
|
print(f" gravity_sign: {gravity_sign}")
|
|
print(f" bias_seconds: {bias_seconds}")
|
|
print(f" estimated acc bias m/s^2: {acc_bias}")
|
|
|
|
v = np.zeros(3, dtype=float)
|
|
last_t = samples[0]["t_s"]
|
|
|
|
for i, sample in enumerate(samples):
|
|
t = sample["t_s"]
|
|
dt = t - last_t
|
|
last_t = t
|
|
|
|
if dt <= 0 or dt > 0.2:
|
|
dt = 0.0
|
|
|
|
linear_acc = raw_lin_acc_ms2[i] - acc_bias
|
|
|
|
if np.linalg.norm(linear_acc) < acc_deadband:
|
|
linear_acc[:] = 0.0
|
|
|
|
v = v * vel_decay + linear_acc * dt
|
|
|
|
sample["Provider_lin_acc_x_ms2"] = linear_acc[0]
|
|
sample["Provider_lin_acc_y_ms2"] = linear_acc[1]
|
|
sample["Provider_lin_acc_z_ms2"] = linear_acc[2]
|
|
sample["Provider_lin_acc_norm_ms2"] = float(np.linalg.norm(linear_acc))
|
|
|
|
sample["Provider_vel_x_ms"] = v[0]
|
|
sample["Provider_vel_y_ms"] = v[1]
|
|
sample["Provider_vel_z_ms"] = v[2]
|
|
sample["Provider_vel_norm_ms"] = float(np.linalg.norm(v))
|
|
|
|
return samples
|
|
|
|
|
|
def unique_path(path):
|
|
if not path:
|
|
return path
|
|
|
|
root, ext = os.path.splitext(os.path.basename(path))
|
|
ext = ext.lower()
|
|
|
|
if ext == ".csv":
|
|
folder = "csv"
|
|
elif ext == ".png":
|
|
folder = "png"
|
|
else:
|
|
folder = "output"
|
|
|
|
os.makedirs(folder, exist_ok=True)
|
|
|
|
new_path = os.path.join(folder, root + ext)
|
|
|
|
if not os.path.exists(new_path):
|
|
return new_path
|
|
|
|
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
|
return os.path.join(folder, f"{root}_{timestamp}{ext}")
|
|
|
|
|
|
def write_csv(samples, path):
|
|
if not samples:
|
|
return
|
|
|
|
columns = list(samples[0].keys())
|
|
|
|
with open(path, "w", newline="", encoding="utf-8") as f:
|
|
writer = csv.DictWriter(f, fieldnames=columns)
|
|
writer.writeheader()
|
|
writer.writerows(samples)
|
|
|
|
|
|
def print_stats(samples):
|
|
if not samples:
|
|
print("No samples parsed.")
|
|
return
|
|
|
|
print(f"Samples: {len(samples)}")
|
|
print(f"Duration: {samples[-1]['t_s'] - samples[0]['t_s']:.3f}s")
|
|
|
|
groups = (
|
|
("RAW ACC g", ("acc_x_g", "acc_y_g", "acc_z_g")),
|
|
("GYRO dps", ("gyro_x_dps", "gyro_y_dps", "gyro_z_dps")),
|
|
("ANGLE deg", ("angle_x_deg", "angle_y_deg", "angle_z_deg")),
|
|
(
|
|
"Provider GRAVITY g",
|
|
("Provider_gx_g", "Provider_gy_g", "Provider_gz_g"),
|
|
),
|
|
(
|
|
"Provider LINEAR ACC m/s^2",
|
|
(
|
|
"Provider_lin_acc_x_ms2",
|
|
"Provider_lin_acc_y_ms2",
|
|
"Provider_lin_acc_z_ms2",
|
|
"Provider_lin_acc_norm_ms2",
|
|
),
|
|
),
|
|
(
|
|
"Provider VELOCITY m/s",
|
|
(
|
|
"Provider_vel_x_ms",
|
|
"Provider_vel_y_ms",
|
|
"Provider_vel_z_ms",
|
|
"Provider_vel_norm_ms",
|
|
),
|
|
),
|
|
)
|
|
|
|
for group_name, fields in groups:
|
|
print(f"\n{group_name}")
|
|
for field in fields:
|
|
values = np.array([sample[field] for sample in samples], dtype=float)
|
|
diffs = np.diff(values)
|
|
|
|
diff_std = float(np.std(diffs)) if len(diffs) else 0.0
|
|
|
|
print(
|
|
" {:24s} mean={: .6f} std={:.6f} ptp={:.6f} final={: .6f} diff_std={:.6f}".format(
|
|
field,
|
|
float(np.mean(values)),
|
|
float(np.std(values)),
|
|
float(np.ptp(values)),
|
|
float(values[-1]),
|
|
diff_std,
|
|
)
|
|
)
|
|
|
|
freq = np.array([sample["freq_hz"] for sample in samples[1:]], dtype=float)
|
|
|
|
if len(freq):
|
|
print(
|
|
"\nFrequency Hz mean={:.3f} std={:.3f} min={:.3f} max={:.3f}".format(
|
|
float(np.mean(freq)),
|
|
float(np.std(freq)),
|
|
float(np.min(freq)),
|
|
float(np.max(freq)),
|
|
)
|
|
)
|
|
|
|
|
|
def plot_group(axis, t, samples, fields, ylabel):
|
|
for field in fields:
|
|
values = np.array([sample[field] for sample in samples], dtype=float)
|
|
axis.plot(t, values, label=field, linewidth=1.0)
|
|
|
|
axis.set_ylabel(ylabel)
|
|
axis.grid(True, alpha=0.3)
|
|
axis.legend(loc="upper right", ncol=3)
|
|
|
|
|
|
def plot_samples(samples, output=None, show=True):
|
|
if not samples:
|
|
raise ValueError("No samples to plot.")
|
|
|
|
t = np.array([sample["t_s"] for sample in samples], dtype=float)
|
|
|
|
fig, axes = plt.subplots(7, 1, sharex=True, figsize=(13, 15))
|
|
|
|
plot_group(
|
|
axes[0],
|
|
t,
|
|
samples,
|
|
("acc_x_g", "acc_y_g", "acc_z_g"),
|
|
"Raw acc (g)",
|
|
)
|
|
|
|
plot_group(
|
|
axes[1],
|
|
t,
|
|
samples,
|
|
("Provider_gx_g", "Provider_gy_g", "Provider_gz_g"),
|
|
"Estimated gravity (g)",
|
|
)
|
|
|
|
plot_group(
|
|
axes[2],
|
|
t,
|
|
samples,
|
|
(
|
|
"Provider_lin_acc_x_ms2",
|
|
"Provider_lin_acc_y_ms2",
|
|
"Provider_lin_acc_z_ms2",
|
|
),
|
|
"Provider linear acc (m/s²)",
|
|
)
|
|
|
|
plot_group(
|
|
axes[3],
|
|
t,
|
|
samples,
|
|
("Provider_lin_acc_norm_ms2",),
|
|
"Linear acc norm (m/s²)",
|
|
)
|
|
|
|
plot_group(
|
|
axes[4],
|
|
t,
|
|
samples,
|
|
("Provider_vel_x_ms", "Provider_vel_y_ms", "Provider_vel_z_ms"),
|
|
"Provider velocity (m/s)",
|
|
)
|
|
|
|
plot_group(
|
|
axes[5],
|
|
t,
|
|
samples,
|
|
("gyro_x_dps", "gyro_y_dps", "gyro_z_dps"),
|
|
"Gyro (deg/s)",
|
|
)
|
|
|
|
plot_group(
|
|
axes[6],
|
|
t,
|
|
samples,
|
|
("angle_x_deg", "angle_y_deg", "angle_z_deg"),
|
|
"Angle (deg)",
|
|
)
|
|
|
|
axes[6].set_xlabel("Time (s)")
|
|
|
|
fig.suptitle("Provider Method IMU Verification")
|
|
fig.tight_layout()
|
|
|
|
if output:
|
|
fig.savefig(output, dpi=160)
|
|
print(f"Saved plot to {output}")
|
|
|
|
if show:
|
|
plt.show()
|
|
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser(
|
|
description="Verify Provider roll/pitch gravity compensation method."
|
|
)
|
|
|
|
source = parser.add_mutually_exclusive_group(required=True)
|
|
source.add_argument("--file", help="pc_reader text log, for example rcd.txt")
|
|
source.add_argument("--port", help="ESP32 serial port, for example /dev/ttyUSB0")
|
|
|
|
parser.add_argument("--baud", type=int, default=115200)
|
|
parser.add_argument("--seconds", type=float, default=10.0)
|
|
parser.add_argument("--samples", type=int)
|
|
parser.add_argument("--max-gap-ms", type=int, default=1000)
|
|
|
|
parser.add_argument(
|
|
"--gravity-sign",
|
|
type=float,
|
|
default=1.0,
|
|
choices=[1.0, -1.0],
|
|
help="Use +1 if horizontal static acc_z is about +1g; use -1 if about -1g.",
|
|
)
|
|
|
|
parser.add_argument(
|
|
"--bias-seconds",
|
|
type=float,
|
|
default=1.0,
|
|
help="Use the first N seconds to estimate residual linear acceleration bias.",
|
|
)
|
|
|
|
parser.add_argument(
|
|
"--acc-deadband",
|
|
type=float,
|
|
default=0.10,
|
|
help="Set linear acceleration to zero if norm is below this value, unit m/s^2.",
|
|
)
|
|
|
|
parser.add_argument(
|
|
"--vel-decay",
|
|
type=float,
|
|
default=0.995,
|
|
help="Velocity decay factor to reduce long-term integration drift.",
|
|
)
|
|
|
|
parser.add_argument("--output", default="verify_l.png")
|
|
parser.add_argument("--csv", default="verify_l.csv")
|
|
parser.add_argument("--no-show", action="store_true")
|
|
|
|
args = parser.parse_args()
|
|
|
|
if args.file:
|
|
samples = read_text_log(args.file)
|
|
else:
|
|
samples = read_serial_samples(
|
|
args.port,
|
|
args.baud,
|
|
args.seconds,
|
|
args.samples,
|
|
args.max_gap_ms,
|
|
)
|
|
|
|
if not samples:
|
|
print("No samples parsed. Check input mode, serial port, baudrate, or log format.")
|
|
return
|
|
|
|
samples = add_verification(
|
|
samples,
|
|
gravity_sign=args.gravity_sign,
|
|
bias_seconds=args.bias_seconds,
|
|
acc_deadband=args.acc_deadband,
|
|
vel_decay=args.vel_decay,
|
|
)
|
|
|
|
print_stats(samples)
|
|
|
|
csv_path = unique_path(args.csv)
|
|
write_csv(samples, csv_path)
|
|
print(f"Saved CSV to {csv_path}")
|
|
|
|
output_path = unique_path(args.output)
|
|
plot_samples(samples, output=output_path, show=not args.no_show)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|
|
# python3 verify.py --port /dev/ttyUSB0 --baud 115200 --seconds 10 --no-show |