측정 시각과 처리 시각

독립적으로 실행하는 PC 계산입니다. 동일한 세계의 정지점 1.20m와 비교합니다.

import math

def finite(value):
    return type(value) in (int, float) and math.isfinite(value)

class History:
    def __init__(self, samples):
        if not samples: raise ValueError('EMPTY_HISTORY')
        self.samples = list(samples)
        for t, p in self.samples:
            if not finite(t) or len(p) != 3 or not all(finite(v) for v in p):
                raise ValueError('BAD_POSE')
        if any(a[0] >= b[0] for a,b in zip(self.samples,self.samples[1:])):
            raise ValueError('ORDER')
    def at(self, stamp):
        if not finite(stamp): raise ValueError('BAD_STAMP')
        if not self.samples[0][0] <= stamp <= self.samples[-1][0]:
            raise ValueError('OUTSIDE_HISTORY')
        for t,p in self.samples:
            if t == stamp: return tuple(p)
        for (ta,a),(tb,b) in zip(self.samples,self.samples[1:]):
            if ta < stamp < tb:
                ratio = (stamp-ta)/(tb-ta)
                angle = math.atan2(math.sin(b[2]-a[2]), math.cos(b[2]-a[2]))
                return (a[0]+ratio*(b[0]-a[0]), a[1]+ratio*(b[1]-a[1]),
                        math.atan2(math.sin(a[2]+ratio*angle),math.cos(a[2]+ratio*angle)))
        raise ValueError('OUTSIDE_HISTORY')

def project(history, stamp, point, mount=(0.10,0.0)):
    if len(point)!=2 or not all(finite(v) for v in point): raise ValueError('BAD_POINT')
    x,y,yaw = history.at(stamp)
    px,py = point[0]+mount[0],point[1]+mount[1]
    return (x+math.cos(yaw)*px-math.sin(yaw)*py,
            y+math.sin(yaw)*px+math.cos(yaw)*py)

class RangeGate:
    def __init__(self, max_age=0.20):
        if not finite(max_age) or max_age<0: raise ValueError('BAD_AGE')
        self.max_age=max_age;self.last_stamp=None
    def accept(self, sample, now, history):
        if not isinstance(sample,dict): raise ValueError('MISSING')
        if not finite(now): raise ValueError('BAD_NOW')
        stamp=sample.get('stamp_s');received=sample.get('received_s');r=sample.get('range_m')
        if not all(finite(v) for v in (stamp,received,r)) or not 0.05 <= r <= 4.0:
            raise ValueError('BAD_RANGE')
        if not stamp <= received <= now: raise ValueError('CLOCK')
        if self.last_stamp is not None and stamp <= self.last_stamp: raise ValueError('ORDER')
        if now-stamp > self.max_age+1e-12: raise ValueError('STALE')
        point=project(history,stamp,(r,0.0))
        if r <= 0.20: raise ValueError('NEAR')
        self.last_stamp=stamp
        return point

h=History([(0,(0,0,0)),(1,(.2,0,0))])
for name,t in [('naive',.65),('aligned',.50)]:
    p=project(h,t,(1,0))
    print(f'{name}: map_x={p[0]:.6f} error_m={abs(p[0]-1.20):.6f}')

중간 yaw는 약 180도입니다. 범위 밖 요청을 최신값으로 바꾸지 않습니다.

import math

def finite(value):
    return type(value) in (int, float) and math.isfinite(value)

class History:
    def __init__(self, samples):
        if not samples: raise ValueError('EMPTY_HISTORY')
        self.samples = list(samples)
        for t, p in self.samples:
            if not finite(t) or len(p) != 3 or not all(finite(v) for v in p):
                raise ValueError('BAD_POSE')
        if any(a[0] >= b[0] for a,b in zip(self.samples,self.samples[1:])):
            raise ValueError('ORDER')
    def at(self, stamp):
        if not finite(stamp): raise ValueError('BAD_STAMP')
        if not self.samples[0][0] <= stamp <= self.samples[-1][0]:
            raise ValueError('OUTSIDE_HISTORY')
        for t,p in self.samples:
            if t == stamp: return tuple(p)
        for (ta,a),(tb,b) in zip(self.samples,self.samples[1:]):
            if ta < stamp < tb:
                ratio = (stamp-ta)/(tb-ta)
                angle = math.atan2(math.sin(b[2]-a[2]), math.cos(b[2]-a[2]))
                return (a[0]+ratio*(b[0]-a[0]), a[1]+ratio*(b[1]-a[1]),
                        math.atan2(math.sin(a[2]+ratio*angle),math.cos(a[2]+ratio*angle)))
        raise ValueError('OUTSIDE_HISTORY')

def project(history, stamp, point, mount=(0.10,0.0)):
    if len(point)!=2 or not all(finite(v) for v in point): raise ValueError('BAD_POINT')
    x,y,yaw = history.at(stamp)
    px,py = point[0]+mount[0],point[1]+mount[1]
    return (x+math.cos(yaw)*px-math.sin(yaw)*py,
            y+math.sin(yaw)*px+math.cos(yaw)*py)

class RangeGate:
    def __init__(self, max_age=0.20):
        if not finite(max_age) or max_age<0: raise ValueError('BAD_AGE')
        self.max_age=max_age;self.last_stamp=None
    def accept(self, sample, now, history):
        if not isinstance(sample,dict): raise ValueError('MISSING')
        if not finite(now): raise ValueError('BAD_NOW')
        stamp=sample.get('stamp_s');received=sample.get('received_s');r=sample.get('range_m')
        if not all(finite(v) for v in (stamp,received,r)) or not 0.05 <= r <= 4.0:
            raise ValueError('BAD_RANGE')
        if not stamp <= received <= now: raise ValueError('CLOCK')
        if self.last_stamp is not None and stamp <= self.last_stamp: raise ValueError('ORDER')
        if now-stamp > self.max_age+1e-12: raise ValueError('STALE')
        point=project(history,stamp,(r,0.0))
        if r <= 0.20: raise ValueError('NEAR')
        self.last_stamp=stamp
        return point

h=History([(0,(0,0,math.radians(179))),(1,(0,0,math.radians(-179)))])
print(f'middle_deg={math.degrees(h.at(.5)[2]):.1f}')
try: h.at(1.1)
except ValueError as e: print(e)

0.20초 경계와 초과·누락을 각각 확인합니다. 실제 모터 정지는 로컬 미션에서 검사합니다.

import math

def finite(value):
    return type(value) in (int, float) and math.isfinite(value)

class History:
    def __init__(self, samples):
        if not samples: raise ValueError('EMPTY_HISTORY')
        self.samples = list(samples)
        for t, p in self.samples:
            if not finite(t) or len(p) != 3 or not all(finite(v) for v in p):
                raise ValueError('BAD_POSE')
        if any(a[0] >= b[0] for a,b in zip(self.samples,self.samples[1:])):
            raise ValueError('ORDER')
    def at(self, stamp):
        if not finite(stamp): raise ValueError('BAD_STAMP')
        if not self.samples[0][0] <= stamp <= self.samples[-1][0]:
            raise ValueError('OUTSIDE_HISTORY')
        for t,p in self.samples:
            if t == stamp: return tuple(p)
        for (ta,a),(tb,b) in zip(self.samples,self.samples[1:]):
            if ta < stamp < tb:
                ratio = (stamp-ta)/(tb-ta)
                angle = math.atan2(math.sin(b[2]-a[2]), math.cos(b[2]-a[2]))
                return (a[0]+ratio*(b[0]-a[0]), a[1]+ratio*(b[1]-a[1]),
                        math.atan2(math.sin(a[2]+ratio*angle),math.cos(a[2]+ratio*angle)))
        raise ValueError('OUTSIDE_HISTORY')

def project(history, stamp, point, mount=(0.10,0.0)):
    if len(point)!=2 or not all(finite(v) for v in point): raise ValueError('BAD_POINT')
    x,y,yaw = history.at(stamp)
    px,py = point[0]+mount[0],point[1]+mount[1]
    return (x+math.cos(yaw)*px-math.sin(yaw)*py,
            y+math.sin(yaw)*px+math.cos(yaw)*py)

class RangeGate:
    def __init__(self, max_age=0.20):
        if not finite(max_age) or max_age<0: raise ValueError('BAD_AGE')
        self.max_age=max_age;self.last_stamp=None
    def accept(self, sample, now, history):
        if not isinstance(sample,dict): raise ValueError('MISSING')
        if not finite(now): raise ValueError('BAD_NOW')
        stamp=sample.get('stamp_s');received=sample.get('received_s');r=sample.get('range_m')
        if not all(finite(v) for v in (stamp,received,r)) or not 0.05 <= r <= 4.0:
            raise ValueError('BAD_RANGE')
        if not stamp <= received <= now: raise ValueError('CLOCK')
        if self.last_stamp is not None and stamp <= self.last_stamp: raise ValueError('ORDER')
        if now-stamp > self.max_age+1e-12: raise ValueError('STALE')
        point=project(history,stamp,(r,0.0))
        if r <= 0.20: raise ValueError('NEAR')
        self.last_stamp=stamp
        return point

h=History([(0,(0,0,0)),(1,(.2,0,0))])
s=dict(stamp_s=.5,received_s=.65,range_m=1)
for sample,now in [(s,.70),(s,.701),(None,.65)]:
    try:
        RangeGate().accept(sample,now,h)
        print('ACCEPTED')
    except ValueError as e: print(e)

starter.zip을 별도 폴더에 풀고 sensor_time.py의 History.at에서 ratio TODO를 완성합니다. bash check.sh 한 줄로 Python 테스트 12개와 비교 demo를 실행합니다. 보간·끝점·각도 경계·장착 위치·이력 밖·0.20초 경계·누락·역순·수신 후 대기를 검사합니다. starter는 실제 assertion 실패, solution은 전체 통과입니다. 미션 zip에는 앞 모듈 코드와 C HAL 회귀가 함께 들어 있으며 gcc가 필요합니다. 미션에서는 같은 보간 구현을 경로 액션의 시간 검사와 엔코더 추정 평가에 연결하고 세 조건 JSON과 정지 근거를 제출합니다.