633 lines
36 KiB
Python
633 lines
36 KiB
Python
"""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()
|