SelfRising/RDK/calibration/imu_server.py

633 lines
36 KiB
Python
Raw Permalink Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

"""Loopback X5 calibration and explicitly requested, leased SG90 attempts."""
import argparse
import base64
import copy
from datetime import datetime, timezone
import hashlib
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
import json
from pathlib import Path
import secrets
import shlex
import subprocess
import threading
import time
import uuid
import numpy as np
from .desk_camera import DeskCamera
from .bridge import Bridge
from .server import atomic_json
from .motion_bridge import MotionBridge
from .speed_bench import SpeedBench
ROOT = Path(__file__).resolve().parent.parent
REMOTE = '/home/sunrise/x5_imu_calibration_20261112'
DATA = ROOT / 'calibration/imu_data_20261112'
POSES = ('flat', 'x_up', 'y_up')
MOTION_SCRIPT = 'v2/motion_cli.py'
def digest(value):
return hashlib.sha256(json.dumps(value, sort_keys=True, separators=(',', ':'),
allow_nan=False).encode()).hexdigest()
def stationary(capture):
if (capture.get('status') != 'sampled' or capture.get('sensor_config') !=
{'odr_hz': 224.2, 'gyro_dps': 1024, 'accel_g': 8}):
raise ValueError('采样状态或传感器配置与本次模型不一致')
if capture.get('configuration_restored') is not True or capture.get('pwm_written') is not False:
raise ValueError('采样未确认恢复可读配置及无 PWM 写入')
samples = capture.get('samples', [])
if len(samples) < 100:
raise ValueError('静止数据不足100帧,请重新采样')
accel = np.asarray([s['accel_g'] for s in samples], dtype=float)
gyro = np.asarray([s['gyro_dps'] for s in samples], dtype=float)
times = np.asarray([s['elapsed_s'] for s in samples], dtype=float)
if accel.shape != (len(samples), 3) or gyro.shape != accel.shape or not all(
np.isfinite(a).all() for a in (accel, gyro, times)):
raise ValueError('六轴数据必须为有限三维向量')
dt = np.diff(times)
norms = np.linalg.norm(accel, axis=1)
if (times[-1] - times[0] < 1 or np.any(dt <= 0) or np.max(dt) > .15
or np.min(norms) < .85 or np.max(norms) > 1.15
or np.max(np.std(accel, axis=0)) >= .02
or np.max(np.std(gyro, axis=0)) >= .5
or np.max(np.abs(np.mean(gyro, axis=0))) >= 10):
raise ValueError('未通过静止筛查:保持该姿态不动,避免手抖/桌面振动后重采')
return np.mean(accel, axis=0), np.mean(gyro, axis=0)
def candidate(captures, model_candidate_sha256=None):
if any(p not in captures for p in POSES):
return None
means = {p: stationary(captures[p]['result']) for p in POSES}
unit = {p: a / np.linalg.norm(a) for p, (a, _) in means.items()}
measured = np.column_stack([unit['x_up'], unit['y_up'], unit['flat']])
dots = [float(np.dot(measured[:, a], measured[:, b])) for a, b in ((0, 1), (0, 2), (1, 2))]
if np.linalg.det(measured) <= 0 or max(abs(d) for d in dots) > np.sin(np.deg2rad(15)):
raise ValueError('三个姿态方向不正交或正负号反了;确认 +X/+Y 端朝上,重新采样')
u, _, vt = np.linalg.svd(measured)
rotation = (u @ vt).T
errors = [float(np.rad2deg(np.arccos(np.clip(np.dot(rotation @ measured[:, i],
np.eye(3)[:, i]), -1, 1)))) for i in range(3)]
if max(errors) > 15 or any(np.max(np.abs(g - means['flat'][1])) > 1 for _, g in means.values()):
raise ValueError('姿态拟合误差或静止零偏变化过大,请重采')
value = {'schema': 'qmi8658c_installation_v1',
'gyro_bias_dps': means['flat'][1].tolist(),
'sensor_to_body': rotation.tolist(),
'reference_accel_g': means['flat'][0].tolist(),
'reference_samples': captures['flat']['result']['samples'],
'source_capture_ids': {p: captures[p]['id'] for p in POSES},
'source_capture_sha256': {p: captures[p]['result']['output_sha256'] for p in POSES},
'sensor_config': {'odr_hz': 224.2, 'gyro_dps': 1024, 'accel_g': 8},
'pose_fit_error_deg': dict(zip(('x_up', 'y_up', 'flat'), errors)),
'accelerometer_bias_calibrated': False,
'model_candidate_sha256': model_candidate_sha256,
'created_at': datetime.now(timezone.utc).isoformat()}
value['calibration_candidate_sha256'] = digest(value)
return value
class App:
def __init__(self, transport, data=DATA, continuous=None):
self.transport, self.data = transport, Path(data)
self.continuous=copy.deepcopy(continuous)
self.autonomous = {}
self.data.mkdir(parents=True, exist_ok=True)
self.token = secrets.token_urlsafe(32)
self.lock = threading.RLock()
self.device = {'connected': False, 'error': '正在读取 X5 状态', 'read_at': None}
self.captures, self.proposal, self.saved, self.job = {}, None, None, None
self.candidate_error = None
self.frames, self.observations, self.preflight = [], [], None
self.motion_status = {}
session = self.data / 'session.json'
if session.is_file():
saved = json.loads(session.read_text())
self.captures = saved.get('captures', {})
self.proposal = saved.get('candidate')
self.saved = saved.get('saved')
self.job = saved.get('job')
self.frames = saved.get('frames', [])
self.observations = saved.get('observations', [])
self.preflight = saved.get('preflight')
if self.job and self.job['status'] == 'running':
self.job.update(status='failed', error='服务重启,旧任务结果未知;不会自动重放')
self.motion = (SpeedBench(transport, self.data/'speed-motion', continuous,
ROOT/'deployment/x5_continuous_speed_20261112/bench_receipts')
if continuous else MotionBridge(transport, self.data/'motion'))
def persist(self):
with self.lock:
atomic_json(self.data / 'session.json', self.export())
def export(self):
with self.lock:
return copy.deepcopy({'captures': self.captures, 'candidate': self.proposal,
'saved': self.saved, 'job': self.job, 'frames': self.frames,
'observations': self.observations, 'preflight': self.preflight,
'motion': self.motion.export()})
def command(self, args, payload=None, timeout=15, script='board_tool.py', allow_failure=False):
cmd = 'sudo -n python3 ' + REMOTE + '/' + script + ' ' + shlex.join(args)
run = subprocess.run(self.transport._ssh(cmd), input=None if payload is None else json.dumps(payload),
text=True, capture_output=True, timeout=timeout, check=False)
try:
value = json.loads(run.stdout)
except ValueError:
raise RuntimeError((run.stderr or run.stdout or '板端未返回JSON结果')[-2000:])
if not isinstance(value, dict):
raise RuntimeError('板端结果必须为JSON对象')
if run.returncode and not allow_failure:
raise RuntimeError(str(value.get('error') or value.get('errors') or run.stderr or '板端命令失败')[-2000:])
return value
def refresh(self):
with self.lock:
if self.job and self.job['status'] == 'running' or self.motion._worker_active:
return
try:
value = self.command(['status'], timeout=10)
if value.get('identity') != 'D-Robotics RDK X5 V1.0':
raise ValueError('连接的设备不是已核验的 RDK X5 V1.0')
value.update(connected=True, read_at=time.time(), error=None)
if self.continuous:
result = subprocess.run(self.transport._ssh("systemctl is-active x5-selfright.service; cat /run/x5-selfright.json 2>/dev/null || true"),capture_output=True,text=True,timeout=5,check=True)
line, _, data = result.stdout.partition('\n')
self.autonomous = dict(json.loads(data) if data.strip() else {}, active=line.strip() in ('active','activating','deactivating'))
except Exception as exc:
value = {'connected': False, 'read_at': time.time(), 'error': str(exc)}
with self.lock:
self.device = value
try:
motion = self.command(['status'], script=MOTION_SCRIPT, timeout=10)
except Exception as exc:
motion = {'installed': False, 'error': str(exc)}
with self.lock:
self.motion_status = motion
self.motion.device = copy.deepcopy(self.device)
self.motion.prepared = motion.get('installed') is True
remote_profile = motion.get('installation_profile')
candidate_matches = (not self.proposal or remote_profile and
remote_profile.get('calibration_candidate_sha256') == self.proposal.get('calibration_candidate_sha256'))
if not self.saved and candidate_matches and remote_profile and remote_profile.get('sha256'):
self.saved = dict(saved=True, sha256=remote_profile['sha256'],
profile_path=REMOTE+'/calibration_profile.json', restored_from_board=True)
def state(self):
with self.lock:
return copy.deepcopy({'device': self.device, 'job': self.job,
'captures': {p: {k: v for k, v in record.items() if k != 'result'} for p, record in self.captures.items()},
'candidate': None if self.proposal is None else {k: v for k, v in self.proposal.items() if k != 'reference_samples'},
'candidate_error': self.candidate_error, 'saved': self.saved,
'motion': self.motion.status(), 'motion_status': self.motion_status,
'frames': self.frames, 'observations': self.observations, 'preflight': self.preflight,
'csrf_token': self.token, 'server_time': time.time(),
'continuous_deployment':self.continuous, 'autonomous':self.autonomous,
'last_speed_trial': self.motion.last_trial() if self.continuous else None,
'speed_axes': self.motion.axes if self.continuous else {},
'speed_replay': (self.motion.job['id'] if self.continuous and self.motion.job and
(self.motion.output/(self.motion.job['id']+'.mp4')).is_file() else None),
'mode': 'continuous_speed_web' if self.continuous else 'guarded_sg90_bench',
'motion_enabled': self.motion.prepared and (not self.continuous or bool(self.continuous.get('passed') and not self.continuous.get('supply_hold')))})
def start(self, payload):
kind, pose = payload.get('type'), payload.get('pose')
if self.continuous and kind in ('shadow','preflight'):
raise ValueError('旧定位策略的验证已停用;新360速度策略的板端验收见当前部署结果')
if kind not in ('capture', 'shadow', 'preflight') or payload.get('pose_confirmed') is not True:
raise ValueError('请先确认按图摆放并保持不动')
if kind == 'preflight' and payload.get('operator_observing') is not True:
raise ValueError('请确认操作者在现场观察并保持静止')
if kind == 'capture' and pose not in POSES:
raise ValueError('未知姿态')
with self.lock:
if self.job and self.job['status'] == 'running' or self.motion._worker_active:
raise ValueError('已有采样/推理任务,请等待结束')
if not self.device.get('connected') or time.time() - self.device.get('read_at', 0) > 20:
raise ValueError('设备未连接或状态已过期,请先刷新状态')
if self.device.get('pwm'):
raise ValueError('检测到已导出的 PWM,请先检查既有任务')
if kind in ('shadow', 'preflight') and not self.saved:
raise ValueError('请先完成三个姿态并明确保存本次安装校准')
ident = str(uuid.uuid4())
self.job = {'id': ident, 'type': kind, 'pose': pose, 'status': 'running',
'started_at': time.time(), 'latest_sample': None, 'error': None}
target = self.execute_preflight if kind == 'preflight' else self.execute
self.persist()
threading.Thread(target=target, args=(copy.deepcopy(self.job),), daemon=True).start()
return self.state()
def execute_preflight(self, job):
try:
result = self.command(['arm-check'], payload={'request_id':job['id'], 'stationary_confirmed':True, 'operator_observing':True}, script=MOTION_SCRIPT, timeout=25, allow_failure=True)
atomic_json(self.data / (job['id']+'.json'), result)
summary = {k:v for k,v in result.items() if k not in ('samples','controls')}
summary.update(sample_count=len(result.get('samples', [])),
control_count=len(result.get('controls', [])), local_evidence_file=job['id']+'.json')
with self.lock:
self.preflight = summary
passed = (result.get('passed') is True and result.get('status') == 'passed'
and result.get('cleanup_verified') is True and result.get('pwm_written') is False)
self.job.update(status='completed' if passed else 'failed', result=summary,
error=None if passed else (result.get('error') or '前置检查或清理未通过,请查看具体阻断项'),
completed_at=time.time())
except Exception as exc:
with self.lock:
self.job.update(status='failed', error=str(exc), completed_at=time.time())
finally:
self.persist()
self.refresh()
def execute(self, job):
args = [job['type'], '--seconds', '8', '--output', job['id'] + '.json', '--stream']
if job['type'] == 'shadow':
args += ['--calibration', REMOTE + '/calibration_profile.json']
command = 'sudo -n python3 -u ' + REMOTE + '/board_tool.py ' + shlex.join(args)
proc, errors, terminal = None, [], None
try:
proc = subprocess.Popen(self.transport._ssh(command), stdout=subprocess.PIPE,
stderr=subprocess.PIPE, text=True, bufsize=1)
timer = threading.Timer(25, proc.kill)
timer.start()
threading.Thread(target=lambda: errors.append(proc.stderr.read(16000)), daemon=True).start()
try:
for line in proc.stdout:
event = json.loads(line)
if event.get('event') == 'sample':
with self.lock:
self.job['latest_sample'] = event['sample']
self.job['sample_received_at'] = time.time()
elif event.get('event') == 'control':
with self.lock:
self.job['latest_control'] = {k: v for k, v in event['control'].items() if k != 'observation'}
elif event.get('event') == 'result':
terminal = event['result']
code = proc.wait(timeout=2)
finally:
timer.cancel()
if terminal is None:
raise RuntimeError('板端任务未正常完成:' + ''.join(errors)[-1000:])
atomic_json(self.data / (job['id'] + '.json'), terminal)
summary = {k: v for k, v in terminal.items() if k not in ('samples', 'controls')}
summary['control_count'] = len(terminal.get('controls', []))
with self.lock:
self.job['result'] = summary
if code != 0 or terminal.get('status') == 'failed':
raise RuntimeError('板端任务未通过:' + terminal.get('error', ''.join(errors)[-1000:]))
if terminal.get('pwm_written') is not False or terminal.get('configuration_restored') is not True:
raise RuntimeError('板端未确认可读配置恢复/无 PWM 写入,结果不可用于校准')
with self.lock:
if job['type'] == 'capture':
stationary(terminal)
self.captures[job['pose']] = {'id': job['id'], 'sample_count': len(terminal['samples']),
'captured_at': time.time(), 'result': terminal, 'sha256': digest(terminal)}
self.proposal, self.saved, self.candidate_error = None, None, None
try:
self.proposal = candidate(self.captures, self.device.get('model', {}).get('candidate_sha256'))
except ValueError as exc:
self.candidate_error = str(exc)
if self.proposal:
atomic_json(self.data / 'candidate.json', self.proposal)
self.job.update(status='completed', result=summary,
completed_at=time.time())
except Exception as exc:
with self.lock:
self.job.update(status='failed', error=str(exc), completed_at=time.time())
finally:
if proc and proc.poll() is None:
proc.kill()
self.refresh()
self.persist()
def save(self, payload):
with self.lock:
if (not self.proposal or payload.get('calibration_candidate_sha256') != self.proposal['calibration_candidate_sha256']
or payload.get('installation_confirmed') is not True):
raise ValueError('请确认安装固定,并保存当前候选;候选已变化时必须重新检查')
if self.job and self.job['status'] == 'running' or self.motion._worker_active:
raise ValueError('任务运行时不能修改校准')
value = self.command(['save-profile'], self.proposal)
if value.get('pwm_written') is not False:
raise RuntimeError('板端保存回执无效')
self.saved = value
atomic_json(self.data / 'saved-profile-receipt.json', value)
self.persist()
return value
def frame(self, payload):
role = payload.get('role')
if role not in ('before', 'after', 'phase') or payload.get('source') not in ('camera', 'uploaded_current_photo'):
raise ValueError('需要实际相机或用户上传的当前前后照片')
encoded = payload.get('image', '')
if not isinstance(encoded, str) or not encoded.startswith('data:image/jpeg;base64,'):
raise ValueError('需要 JPEG 图像')
raw = base64.b64decode(encoded.split(',', 1)[1], validate=True)
if len(raw) > 2_000_000 or not raw.startswith(b'\xff\xd8\xff') or not raw.endswith(b'\xff\xd9'):
raise ValueError('JPEG 文件无效或超过2MB')
with self.lock:
job_id = payload.get('job_id')
if role == 'before' and job_id:
raise ValueError('前图不能预先绑定到任务')
if role == 'after' and not any(j['id'] == job_id and j['status'] not in ('queued','running','stopping')
for j in self.motion.jobs):
raise ValueError('后图必须关联已结束的动作任务')
if role == 'phase':
job = self.motion.status().get('job')
if (not job or job['id'] != job_id or job['status'] != 'running'
or job.get('phase') != 'increment' or payload.get('phase') != 'increment'):
raise ValueError('目标保持阶段已变化,不能将照片绑定到旧阶段')
event = next((e for e in reversed(job.get('events', []))
if e.get('event') == 'phase' and e.get('phase') == 'increment'), None)
if not event or time.time()-event['received_at'] < .5:
raise ValueError('目标尚未保持足够时间')
ident = str(uuid.uuid4())
value = dict(id=ident, role=role, job_id=job_id, source=payload['source'],
captured_at=time.time(), timestamp_kind='server_receive_time_not_camera_exposure_time',
sha256=hashlib.sha256(raw).hexdigest(), url='/frames/'+ident+'.jpg')
if role == 'phase':
value.update(phase='increment', hardware_synchronized=False)
(self.data/(ident+'.jpg')).write_bytes(raw)
self.frames.append(value)
self.persist()
return value
def start_motion(self, payload):
if self.continuous:
raise ValueError('当前是360连续旋转舵机,旧角度点测/爬起接口已停用;先完成速度模式停止点与方向验证')
with self.lock:
if self.job and self.job['status'] == 'running':
raise ValueError('请先等待当前传感器任务结束')
frame = next((f for f in self.frames if f['id'] == payload.get('before_frame_id')), None)
if (not frame or frame['role'] != 'before' or frame.get('job_id')
or time.time()-frame['captured_at'] > 15):
raise ValueError('需要15秒内当前前图;旧前图或另一任务前图不能复用')
if payload.get('observation_mode') == 'photo_operator' and frame['source'] != 'uploaded_current_photo':
raise ValueError('请主动上传当前照片并确认现场持续观察')
if payload.get('observation_mode') == 'camera' and frame['source'] != 'camera':
raise ValueError('实时相机模式需要实际相机前图')
job = self.motion.start(payload)
frame['job_id'] = job['id']
self.persist()
return job
def autonomous_command(self, action):
if action not in ('start','stop') or not self.continuous:
raise ValueError('Unknown autonomous command')
if action == 'start' and (self.motion._worker_active or (self.job or {}).get('status')=='running'):
raise ValueError('请先等待当前实验结束')
cmd = 'sudo -n systemctl ' + ('enable --now' if action=='start' else 'disable --now') + ' x5-selfright.service'
proc = subprocess.run(self.transport._ssh(cmd), capture_output=True,text=True,timeout=25)
if proc.returncode:
raise RuntimeError(proc.stderr[-500:])
self.autonomous = dict(active=action=='start',phase='starting' if action=='start' else 'stopped')
return self.autonomous
def stop_motion(self):
legacy=self.motion.stop()
if not self.continuous:return legacy
self.autonomous_command('stop')
command='sudo -n python3 /home/sunrise/x5_continuous_speed_20261112/bench_pulse.py stop'
run=subprocess.run(self.transport._ssh(command),capture_output=True,text=True,timeout=8)
if run.returncode:raise RuntimeError('速度模式停止请求未确认:'+run.stderr[-500:])
return dict(legacy=legacy,continuous=json.loads(run.stdout))
def save_servo(self, payload):
if self.continuous:raise ValueError('连续旋转舵机不能保存为旧角度映射')
with self.lock:
if self.motion._worker_active or self.job and self.job['status'] == 'running':
raise ValueError('任务执行期间不能修改舵机校准')
result = self.command(['save-servo-profile'], payload, script=MOTION_SCRIPT)
atomic_json(self.data/'servo-profile-receipt.json', result)
self.refresh()
self.persist()
return result
def calibration_point(self, payload):
if self.continuous:raise ValueError('连续旋转模式不使用1500微秒等于0度的定位标定')
with self.lock:
if self.motion._worker_active:
raise ValueError('先等待动作完成及 PWM 清理回读,再保存量角记录')
job = next((j for j in self.motion.jobs if j['id'] == payload.get('request_id')), None)
if not job or job['type'] != 'jog' or job['status'] != 'completed' or job.get('cleanup_verified') is not True:
raise ValueError('需要本次已完成且清理确认的真实单轴任务')
frame = next((f for f in self.frames if f['id'] == payload.get('frame_id')
and f.get('job_id') == job['id']), None)
if not frame:
raise ValueError('量角记录必须引用此任务的真实照片')
if payload.get('operator_measured') is not True or payload.get('zero_mounted_confirmed') is not True:
raise ValueError('请按 CAD 对齐零位并使用量角器测量,不能用未测默认值')
result = self.command(['save-calibration-point'], payload, script=MOTION_SCRIPT)
self.refresh()
self.persist()
return result
def observe_motion(self, payload):
with self.lock:
job = next((j for j in self.motion.jobs if j['id'] == payload.get('job_id')), None)
if not job or job['status'] in ('queued','running','stopping'):
raise ValueError('请选择已结束的真实动作任务')
if payload.get('standing_confirmed') is not True and payload.get('physical_result') not in ('moved','no_motion','uncertain'):
raise ValueError('请明确记录现场看到的结果')
if payload.get('standing_confirmed') is True and job['type'] != 'recover':
raise ValueError('站稳观察只能关联本次爬起任务')
record = {k: payload.get(k) for k in ('job_id','standing_confirmed','physical_result','note')}
if not isinstance(record.get('note') or '', str) or len(record.get('note') or '') > 2000:
raise ValueError('记录说明过长')
record.update(recorded_at=time.time(), source='user_observation_not_automatic_success')
self.observations.append(record)
self.persist()
return record
def handler_for(app, port):
hosts = {f'127.0.0.1:{port}', f'localhost:{port}'}
class Handler(BaseHTTPRequestHandler):
def log_message(self, fmt, *args):
print(self.command, self.path, fmt % args, flush=True)
def reply(self, status, value, kind='application/json; charset=utf-8', headers=()):
body = value if isinstance(value, bytes) else json.dumps(value, ensure_ascii=False, allow_nan=False).encode()
self.send_response(status)
for name, val in [('Content-Type', kind), ('Content-Length', str(len(body))),
('Cache-Control', 'no-store'), ('X-Content-Type-Options', 'nosniff'),
('X-Frame-Options', 'DENY'), *headers]:
self.send_header(name, val)
self.end_headers()
try:
self.wfile.write(body)
except (BrokenPipeError, ConnectionResetError):
pass
def trusted(self):
if (self.headers.get('Host') not in hosts or
self.headers.get('Origin') not in {None, *('http://' + h for h in hosts)} or
self.command == 'POST' and (self.headers.get('Sec-Fetch-Site') == 'cross-site' or
not secrets.compare_digest(self.headers.get('X-Cal-Token', ''), app.token))):
self.reply(403, {'error': '仅允许从本机当前校准页面操作'})
return False
return True
def do_GET(self):
if not self.trusted():
return
if self.path in ('/', '/imu', '/imu/'):
return self.reply(200, (ROOT / 'calibration/imu.html').read_bytes(), 'text/html; charset=utf-8')
if self.path == '/api/imu/state':
return self.reply(200, app.state())
if self.path == '/api/imu/export':
return self.reply(200, app.export())
if self.path == '/speed-bench.js':
return self.reply(200, (ROOT/'calibration/speed_bench.js').read_bytes(), 'text/javascript; charset=utf-8')
if self.path == '/desk-camera.js':
return self.reply(200, (ROOT/'calibration/desk_camera.js').read_bytes(), 'text/javascript; charset=utf-8')
if self.path == '/api/imu/camera.jpg':
if not secrets.compare_digest(self.headers.get('X-Cal-Token', ''), app.token) or self.headers.get('Sec-Fetch-Site') == 'cross-site':
return self.reply(403, {'error': '请在本机实机页面打开相机'})
try:
frame = self.server.camera.frame()
except OSError:
frame = None
if frame is None:
return self.reply(503, {'error': '等待桌面相机的新画面'})
image, captured_at = frame
return self.reply(200, image, 'image/jpeg', [('X-Frame-Time', str(captured_at))])
if self.path == '/imu-motion.js':
return self.reply(200, (ROOT/'calibration/imu_motion.js').read_bytes(), 'text/javascript; charset=utf-8')
if self.path.startswith('/frames/'):
name = self.path.removeprefix('/frames/')
with app.lock:
allowed = {f['id']+'.jpg' for f in app.frames}
if name in allowed:
return self.reply(200, (app.data/name).read_bytes(), 'image/jpeg')
return self.reply(404, {'error': '未知照片'})
if self.path.startswith('/speed-recordings/') and app.continuous:
name = self.path.removeprefix('/speed-recordings/')
if not any(name == j['id']+'.mp4' for j in app.motion.jobs):
return self.reply(404, {'error': '没有这次实验录像'})
clip = app.motion.output / name
if clip.is_file():
return self.reply(200, clip.read_bytes(), 'video/mp4')
return self.reply(404, {'error': '录像还未生成'})
recordings = {
'/autonomous-other-side.mp4':'auto-29a7c0b1-6e62-4aee-ab1b-676441f19c75.mp4',
'/autonomous-support.mp4':'auto-9098872b-1678-48b0-997b-5e0a4de90b57.mp4',
'/support-start.jpg':'support-start.jpg',
'/upper-support-steps.mp4': 'upper-support-steps.mp4',
'/upper-loaded-1550.mp4': '1ecac18c-86ee-4059-83e9-c0a4a6d5211c-load.mp4',
'/upper-loaded-1550.jpg': '1ecac18c-86ee-4059-83e9-c0a4a6d5211c-measurement.jpg',
'/lower-probe.mp4': 'a6f56223-835e-4cab-bc13-adbda6b60f1f-camera.mp4',
'/corrected-trial.mp4': '9acd1a04-a96b-4fea-a9ca-35a67937baaa-camera.mp4',
'/lower-flat-latest.mp4': '6748b741-2cdc-46c0-ae84-a71ff79b9401-camera.mp4',
'/lower90-comparison.mp4': 'lower90-comparison.mp4',
'/lower45-clockwise.mp4': 'lower45-clockwise.mp4',
'/lower45-comparison.jpg': 'lower45-comparison.jpg',
}
if self.path in recordings:
clip = ROOT/'deployment/x5_continuous_speed_20261112/bench_receipts'/recordings[self.path]
if clip.is_file():
return self.reply(200, clip.read_bytes(), 'image/jpeg' if clip.suffix == '.jpg' else 'video/mp4')
return self.reply(404, {'error': '实拍文件不可用'})
if self.path == '/imu-placement.png':
return self.reply(200, (ROOT.parent / '3Dmodel/imu_placement.png').read_bytes(), 'image/png')
if self.path == '/installation.md':
return self.reply(200, (ROOT / 'WIRING.md').read_bytes(), 'text/markdown; charset=utf-8')
if self.path == '/bench-test.md':
return self.reply(200, (ROOT / 'deployment/x5_imu_calibration_20261112/v2/BENCH_TEST.md').read_bytes(), 'text/markdown; charset=utf-8')
self.reply(404, {'error': '未知路径'})
def do_POST(self):
if not self.trusted():
return
try:
size = int(self.headers.get('Content-Length', 0))
if self.headers.get('Transfer-Encoding') or not 0 < size <= 3_000_000:
return self.reply(413, {'error': '请求体大小无效'})
if self.headers.get_content_type() != 'application/json':
raise ValueError('需要 JSON 请求')
payload = json.loads(self.rfile.read(size), parse_constant=lambda _: (_ for _ in ()).throw(ValueError('非有限数字')))
if not isinstance(payload, dict):
raise ValueError('需要 JSON 对象')
if self.path == '/api/imu/start':
return self.reply(200, app.start(payload))
if self.path == '/api/imu/save':
return self.reply(200, app.save(payload))
if self.path=='/api/imu/autonomous/start' and payload=={}:
with app.lock:
return self.reply(200,app.autonomous_command('start'))
if self.path in ('/api/imu/speed/start', '/api/imu/speed/observe'):
if app.autonomous.get('active'):
raise ValueError('X5 自主控制中;单次调试前请先停止自主运行')
if not app.continuous or app.continuous.get('supply_hold') or not app.continuous.get('passed'):
raise ValueError('360° 实验台未就绪')
with app.lock:
if (app.job or {}).get('status') == 'running':
raise ValueError('请先等 IMU 采样结束')
if self.path.endswith('/observe'):
return self.reply(200, app.motion.observe(payload))
if self.server.camera.frame() is None:
raise ValueError('实时相机还没有新画面')
if not app.saved or app.saved.get('sha256') != app.continuous.get('installation_sha256'):
raise ValueError('IMU 安装校准与此模型不一致')
return self.reply(200, app.motion.start(payload))
methods = {'/api/imu/frame': app.frame, '/api/imu/motion/start': app.start_motion,
'/api/imu/motion/heartbeat': lambda p: app.motion.heartbeat(p.get('id')),
'/api/imu/motion/stop': lambda p: app.stop_motion(),
'/api/imu/motion/calibration-point': app.calibration_point,
'/api/imu/motion/save-servo': app.save_servo,
'/api/imu/motion/observation': app.observe_motion}
if self.path in methods:
return self.reply(200, methods[self.path](payload))
if self.path == '/api/imu/refresh' and payload == {}:
app.refresh()
return self.reply(200, app.state())
self.reply(404, {'error': '未知接口;本页不提供舵机动作'})
except (ValueError, KeyError, TypeError) as exc:
self.reply(400, {'error': str(exc)})
except Exception as exc:
self.reply(409, {'error': str(exc)})
return Handler
def maintain(app):
while True:
app.refresh()
time.sleep(5)
class CalibrationHTTPServer(ThreadingHTTPServer):
def service_actions(self):
self.camera.release_if_idle()
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--port', type=int, default=8767)
args = parser.parse_args()
# Only reuse SSH transport: the old bridge's prepare/start are never invoked.
marker=ROOT/'deployment/x5_continuous_speed_20261112/DEPLOYED.json'
app = App(Bridge(output=DATA / 'transport', debug_only=True),
continuous=json.loads(marker.read_text()) if marker.exists() else None)
http = CalibrationHTTPServer(('127.0.0.1', args.port), handler_for(app, args.port))
http.camera = DeskCamera()
if app.continuous:
app.motion.camera = http.camera
http.daemon_threads = True
threading.Thread(target=maintain, args=(app,), daemon=True).start()
print(f'IMU-only calibration: http://127.0.0.1:{args.port}/', flush=True)
try:
http.serve_forever()
finally:
http.camera.close()
app.motion.stop()
app.persist()
http.server_close()
if __name__ == '__main__':
main()