feat: add lms-nav2-gazebo-perception module

2026-07-28 10:35:46 +09:00
commit 29472a4c3e
198 changed files with 9123 additions and 0 deletions
@@ -0,0 +1,58 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+# 토픽/모델은 launch arg 로 전달(노드는 argparse parse_known_args 로 받음 → --ros-args 무시).
+# odom 기본값 /icp_odom: RTABMap icp_odometry (런타임 확인: /odom 무발행).
+#
+# 모델 resolve 의미론(통합 perception ws 편입):
+#   - model_source 를 명시 전달하면(finetuned|pretrained) 그대로 노드에 흐른다 —
+#     단독 deploy run.sh 가 launch 인자를 명시 전달하는 기존 동작이 그대로 이김(무회귀).
+#   - 기본값(finetuned)이면 노드 내부 resolve_model.resolve() 가 finetuned 3파일
+#     (ckpt+arch_cfg+data_cfg, /perception_models/lmnet/finetuned) 존재 검사 후
+#     없으면 baked pretrained(/opt/lmnet_pretrained + /opt/LMNet config) 로 자동 폴백.
+#   resolve 구현은 perception_lmnet_pkg/resolve_model.py 단일 출처 — 노드가 rclpy.init()
+#   "전에" resolve+모델로드까지 책임지므로(DDS 데드락 회피 순서) launch 에 중복 구현하지 않는다.
+ARGUMENTS = [
+    DeclareLaunchArgument('use_sim_time', default_value='true'),
+    DeclareLaunchArgument('input_topic', default_value='/raw/lidar'),
+    DeclareLaunchArgument('odom_topic', default_value='/icp_odom'),
+    DeclareLaunchArgument('moving_topic', default_value='/perception/moving'),
+    DeclareLaunchArgument('moving_rgb_topic', default_value='/perception/moving_rgb'),
+    DeclareLaunchArgument('model_source', default_value='finetuned'),  # UI 스위치: finetuned|pretrained
+]
+
+
+def generate_launch_description():
+    remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
+    # ★ 스레드풀 단일화 — 단독 deploy run.sh 의 export 와 동일한 보호를 launch 에도 둔다.
+    #   통합 perception ws 에선 perception_manager 가 본 launch 를 직접 스폰(run.sh 미경유)하므로
+    #   여기서 안 잡으면 SalsaNext 로드 중 PyTorch 멀티스레드(OpenMP/MKL/OpenBLAS) futex
+    #   데드락 방어가 빠진다. lmnet_infer.py 의 torch.set_num_threads(1) 와 이중 방어
+    #   (2026-06-08 C5 런타임서 발견).
+    thread_env = [
+        SetEnvironmentVariable('OMP_NUM_THREADS', '1'),
+        SetEnvironmentVariable('MKL_NUM_THREADS', '1'),
+        SetEnvironmentVariable('OPENBLAS_NUM_THREADS', '1'),
+    ]
+    load_nodes = GroupAction(actions=[
+        Node(
+            package='perception_lmnet_pkg', executable='lmnet',
+            name='lmnet_node', output='screen',
+            parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
+            arguments=[
+                '--input_topic', LaunchConfiguration('input_topic'),
+                '--odom_topic', LaunchConfiguration('odom_topic'),
+                '--moving_topic', LaunchConfiguration('moving_topic'),
+                '--moving_rgb_topic', LaunchConfiguration('moving_rgb_topic'),
+                '--model_source', LaunchConfiguration('model_source'),
+            ],
+            remappings=remappings,
+        )
+    ])
+    ld = LaunchDescription(ARGUMENTS)
+    for env in thread_env:
+        ld.add_action(env)
+    ld.add_action(load_nodes)
+    return ld
@@ -0,0 +1,32 @@
+"""perception_manager 공통 진입점 (통합 perception 모듈).
+
+LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로
+`ros2 launch perception_lmnet_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..`
+를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(lmnet.launch.py)로 위임한다.
+robot 은 공통 계약상 선언만 하고, 모델 launch 가 선언한 경우에만 소비된다.
+"""
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    share = get_package_share_directory('perception_lmnet_pkg')
+    return LaunchDescription([
+        DeclareLaunchArgument('robot', default_value='fr09'),
+        DeclareLaunchArgument('use_sim_time', default_value='true'),
+        DeclareLaunchArgument('model_source', default_value='finetuned'),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(share, 'launch', 'lmnet.launch.py')),
+            launch_arguments={
+                'robot': LaunchConfiguration('robot'),
+                'use_sim_time': LaunchConfiguration('use_sim_time'),
+                'model_source': LaunchConfiguration('model_source'),
+            }.items(),
+        ),
+    ])
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>perception_lmnet_pkg</name>
+  <version>0.0.0</version>
+  <description>LiDAR moving object segmentation (LMNet / SalsaNext-MOS) perception module</description>
+  <maintainer email="jang1future@gmail.com">dgist</maintainer>
+  <license>TODO: License declaration</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>sensor_msgs_py</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
@@ -0,0 +1,97 @@
+"""SalsaNext-MOS 실시간 추론. 모델 정의는 baked /opt/LMNet 에서 import(재구현 X).
+preprocess 는 학습 parser 와 동일 레이아웃: [range,x,y,z,remission](정규화) + residual_1..N."""
+import os
+import sys
+
+import numpy as np
+import torch
+import yaml
+
+# ★ torch 스레드풀 단일화 — 모델 로드 중 다중스레드 futex 데드락 회피.
+#   GPU/CPU 고경합 환경(다른 학습이 GPU 점유 중 등)에서 .to(cuda)/load 가 여러 스레드를 띄우다
+#   intermittent 하게 데드락(2026-06-08 C5 런타임서 발견). 단일스레드면 경합 자체가 없어 안정.
+torch.set_num_threads(1)
+try:
+    torch.set_num_interop_threads(1)
+except Exception:
+    pass
+
+# baked LMNet 코드 경로 — SalsaNext 네트워크 + laserscan 투영 재사용.
+# common/ 은 train/ 아래라 train 도 path 에 넣어야 'from common.laserscan' 가 해석됨
+# (런타임 CWD=/nav2_ws 라 upstream __init__.py 의 상대경로 트릭에 의존 못 함).
+_LMNET = "/opt/LMNet/mos_SalsaNext/train/tasks/semantic"
+_TRAIN = "/opt/LMNet/mos_SalsaNext/train"
+for _p in (_LMNET, _TRAIN):
+    if _p not in sys.path:
+        sys.path.insert(0, _p)
+from modules.SalsaNext import SalsaNext           # noqa: E402
+from common.laserscan import LaserScan            # noqa: E402
+
+
+class LMNetInfer:
+    def __init__(self, ckpt_path, arch_cfg_path, data_cfg_path, device="auto"):
+        # device='auto' → 런타임 감지(cuda 가용 시 cuda:0, 아니면 cpu). CPU-first dual-runtime(08 N3/N4).
+        if device == 'auto':
+            device = 'cuda:0' if torch.cuda.is_available() else 'cpu'
+        self.arch = yaml.safe_load(open(arch_cfg_path))
+        self.data = yaml.safe_load(open(data_cfg_path))
+        self.device = device
+        sensor = self.arch["dataset"]["sensor"]
+        self.H = sensor["img_prop"]["height"]
+        self.W = sensor["img_prop"]["width"]
+        self.fov_up = sensor["fov_up"]
+        self.fov_down = sensor["fov_down"]
+        self.means = np.array(sensor["img_means"], np.float32)  # 길이 5 (base: range,x,y,z,remission)
+        self.stds = np.array(sensor["img_stds"], np.float32)    # residual 채널은 정규화 안 함(raw)
+        self.n_residual = int(self.arch["train"]["n_input_scans"])
+        self.nclasses = len(self.data["learning_map_inv"])
+        # moving class index — learning_map_inv 에서 moving(원본 251)에 매핑되는 학습 클래스
+        self.moving_idx = self._moving_class_index()
+
+        print("[dbg] before SalsaNext()", flush=True)
+        self.model = SalsaNext(self.nclasses, self.arch)
+        print("[dbg] after SalsaNext, before torch.load", flush=True)
+        w = torch.load(ckpt_path, map_location="cpu")
+        print("[dbg] after torch.load, before load_state_dict", flush=True)
+        sd = w["state_dict"] if isinstance(w, dict) and "state_dict" in w else w
+        sd = {k.replace("module.", ""): v for k, v in sd.items()}
+        self.model.load_state_dict(sd, strict=True)
+        print("[dbg] after load_state_dict, before .to(cuda)", flush=True)
+        self.model.to(device).eval()
+        print("[dbg] after .to(cuda) — DONE", flush=True)
+
+    def _moving_class_index(self):
+        inv = self.data["learning_map_inv"]      # {train_id: orig_id}
+        for train_id, orig in inv.items():
+            if orig in (251, 252):               # KITTI-MOS moving 원본 id
+                return int(train_id)
+        return self.nclasses - 1                  # fallback: 마지막 클래스
+
+    def _project(self, points):
+        scan = LaserScan(project=True, H=self.H, W=self.W,
+                         fov_up=self.fov_up, fov_down=self.fov_down)
+        scan.set_points(points[:, :3], points[:, 3] if points.shape[1] > 3 else None)
+        return scan  # scan.proj_range/proj_xyz/proj_remission/proj_mask/proj_x/proj_y
+
+    def preprocess(self, points, residuals):
+        """points(M,4) + residuals(list of (H,W)) → (proj_in[1,5+N,H,W], p_x, p_y, mask).
+        학습 parser 와 동일: base 5채널만 means/stds 정규화 → residual 은 raw concat → 마지막에 mask."""
+        scan = self._project(points)
+        base = np.concatenate([
+            scan.proj_range[None],                      # (1,H,W)
+            scan.proj_xyz.transpose(2, 0, 1),           # (3,H,W)
+            scan.proj_remission[None],                  # (1,H,W)
+        ], axis=0)                                      # (5,H,W)
+        base = (base - self.means[:, None, None]) / self.stds[:, None, None]
+        proj = np.concatenate([base] + [r[None] for r in residuals], axis=0)  # (5+N,H,W) residual raw
+        proj = proj * scan.proj_mask.astype(np.float32)[None]
+        proj_in = torch.from_numpy(proj.astype(np.float32))[None].to(self.device)
+        return proj_in, scan.proj_x, scan.proj_y, scan.proj_mask
+
+    @torch.no_grad()
+    def infer(self, points, residuals):
+        """반환: per-point moving bool (M,). residuals 없으면(빈) 전부 static."""
+        proj_in, p_x, p_y, _ = self.preprocess(points, residuals)
+        proj_argmax = self.model(proj_in)[0].argmax(dim=0)   # (H,W)
+        labels = proj_argmax[p_y, p_x].cpu().numpy()         # per-point train_id
+        return labels == self.moving_idx
@@ -0,0 +1,169 @@
+"""LMNet-MOS deploy 노드. /raw/lidar + odom 구독 → N-스캔 버퍼 → residual → SalsaNext 추론 →
+/perception/moving(label) + /perception/moving_rgb(빨강) 발행. 상태성 3개(버퍼/odom캐싱/residual)."""
+import argparse
+import struct
+
+import numpy as np
+import torch
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2, PointField
+from nav_msgs.msg import Odometry
+import sensor_msgs_py.point_cloud2 as pc2
+
+from perception_lmnet_pkg import resolve_model
+from perception_lmnet_pkg import pose_util
+from perception_lmnet_pkg.scan_buffer import ScanBuffer
+from perception_lmnet_pkg.range_projection import RangeProjector
+from perception_lmnet_pkg.residual_lib import compute_residual
+from perception_lmnet_pkg.lmnet_infer import LMNetInfer
+
+
+def _read_xyzi(msg):
+    xyz = pc2.read_points_numpy(msg, field_names=("x", "y", "z"),
+                                reshape_organized_cloud=False).astype(np.float32)
+    try:  # intensity 없는 cloud 면 0 으로 채움 (read_points 는 없는 필드에 raise)
+        inten = pc2.read_points_numpy(msg, field_names=("intensity",),
+                                      reshape_organized_cloud=False).astype(np.float32).reshape(-1, 1)
+    except Exception:
+        inten = np.zeros((len(xyz), 1), np.float32)
+    return np.concatenate([xyz, inten], axis=1)
+
+
+def _is_cuda_error(e):
+    s = str(e).lower()
+    return 'cuda' in s or 'cudnn' in s or 'out of memory' in s
+
+
+def _build_infer(m, device):
+    return LMNetInfer(m["ckpt"], m["arch_cfg"], m["data_cfg"], device=device)
+
+
+class LMNetNode(Node):
+    def __init__(self, args, infer, m):
+        super().__init__("lmnet_node")
+        # infer 는 rclpy.init() 전에 미리 로드해 받는다(아래 main 주석 참고) — DDS 스레드와의 데드락 회피.
+        self.infer = infer
+        self.m = m                 # 런타임 CPU 폴백 시 백엔드 재생성용 모델 경로
+        self.cpu_fallback = False  # 런타임 CPU 폴백 1회 가드
+        self.N = self.infer.n_residual
+        self.proj = RangeProjector(self.infer.H, self.infer.W, self.infer.fov_up,
+                                   self.infer.fov_down, args.max_range, args.min_range)
+        self.buf = ScanBuffer(self.N)
+        self.last_pose = np.eye(4)
+        self.have_odom = False
+
+        self.create_subscription(Odometry, args.odom_topic, self._on_odom, 10)
+        self.create_subscription(PointCloud2, args.input_topic, self._on_lidar, 1)
+        self.pub_lbl = self.create_publisher(PointCloud2, args.moving_topic, 1)
+        self.pub_rgb = self.create_publisher(PointCloud2, args.moving_rgb_topic, 1)
+
+    def _on_odom(self, msg):
+        p, q = msg.pose.pose.position, msg.pose.pose.orientation
+        self.last_pose = pose_util.pose_to_matrix(p.x, p.y, p.z, q.x, q.y, q.z, q.w)
+        self.have_odom = True
+
+    def _on_lidar(self, msg):
+        try:
+            self._process_lidar(msg)
+        except Exception as e:
+            self.get_logger().warn(f"[lmnet] 프레임 처리 실패, 스킵: {e}", throttle_duration_sec=5.0)
+
+    def _process_lidar(self, msg):
+        pts = _read_xyzi(msg)
+        if not self.have_odom:
+            self.get_logger().warn("[lmnet] odom 미수신 — 직전 pose 사용", throttle_duration_sec=5.0)
+        self.buf.push(pts, self.last_pose.copy())
+        if not self.buf.ready():
+            self._publish(msg.header, pts, np.zeros(len(pts), bool))  # warmup: all-static
+            return
+        cur_pts, cur_pose = self.buf.current()
+        residuals = []
+        for k in range(1, self.N + 1):
+            past_pts, past_pose = self.buf.past(k)
+            T = pose_util.relative_pose(cur_pose, past_pose)
+            residuals.append(compute_residual(cur_pts, past_pts, T, self.proj))
+        try:
+            moving = self.infer.infer(cur_pts, residuals)
+        except Exception as e:
+            if self.cpu_fallback or not _is_cuda_error(e):
+                raise  # CUDA 외 예외/폴백 이후엔 기존 스킵 경로(_on_lidar) 유지
+            self.get_logger().warn("[lmnet] 추론 중 CUDA 에러 → CPU 백엔드 재구성")
+            self.cpu_fallback = True
+            try:  # 깨진 CUDA 컨텍스트의 GPU 모델 해제 — 실패해도 무해
+                del self.infer.model
+                torch.cuda.empty_cache()
+            except Exception:
+                pass
+            # ckpt 에서 device='cpu' 로 재생성(.to('cpu') 이동 금지 — 깨진 CUDA 컨텍스트 위험)
+            self.infer = _build_infer(self.m, "cpu")
+            moving = self.infer.infer(cur_pts, residuals)  # 해당 프레임 1회 재시도
+        self._publish(msg.header, cur_pts, moving)
+
+    def _publish(self, header, pts, moving):
+        # label cloud: x,y,z,label(0/1)
+        lbl = moving.astype(np.float32)
+        lbl_pts = np.column_stack([pts[:, :3], lbl]).astype(np.float32)
+        self.pub_lbl.publish(self._cloud(header, lbl_pts, "label"))
+        # rgb cloud: moving=빨강(255,0,0), static=회색(120,120,120)
+        rgb = np.where(moving[:, None], np.array([255, 0, 0], np.uint8),
+                       np.array([120, 120, 120], np.uint8))
+        self.pub_rgb.publish(self._rgb_cloud(header, pts[:, :3], rgb))
+
+    def _cloud(self, header, pts, fourth):
+        fields = [PointField(name=n, offset=i * 4, datatype=PointField.FLOAT32, count=1)
+                  for i, n in enumerate(["x", "y", "z", fourth])]
+        return pc2.create_cloud(header, fields, pts)
+
+    def _rgb_cloud(self, header, xyz, rgb):
+        fields = [PointField(name=n, offset=i * 4, datatype=PointField.FLOAT32, count=1)
+                  for i, n in enumerate(["x", "y", "z"])]
+        fields.append(PointField(name="rgb", offset=12, datatype=PointField.FLOAT32, count=1))
+        data = []
+        for (x, y, z), (r, g, b) in zip(xyz, rgb):
+            packed = struct.unpack("f", struct.pack("I", (int(r) << 16) | (int(g) << 8) | int(b)))[0]
+            data.append([float(x), float(y), float(z), packed])
+        return pc2.create_cloud(header, fields, data)
+
+
+def main():
+    ap = argparse.ArgumentParser()
+    ap.add_argument("--input_topic", default="/raw/lidar")
+    ap.add_argument("--odom_topic", default="/odom")
+    ap.add_argument("--moving_topic", default="/perception/moving")
+    ap.add_argument("--moving_rgb_topic", default="/perception/moving_rgb")
+    ap.add_argument("--max_range", type=float, default=90.0)
+    ap.add_argument("--min_range", type=float, default=1.0)
+    ap.add_argument("--model_source", default="finetuned")  # UI 스위치: finetuned|pretrained
+    # device: 'auto' → 런타임 감지(cuda 가용 시 cuda:0, 아니면 cpu). CPU-first dual-runtime(08 N3/N4).
+    # ★ device 는 노드 파라미터(yaml/launch)로만 흐른다 — execute options/SWRule 로 노출 금지.
+    ap.add_argument("--device", default="auto")
+    args, _ = ap.parse_known_args()
+    # ★ 모델을 rclpy.init() "전에" 로드한다. rclpy.init()/Node 가 DDS(rmw) 수신 스레드를 띄우는데,
+    #   그 스레드들과 torch 모델로드(CUDA/스레드풀)가 동시에 뜨면 futex 데드락에 걸린다
+    #   (라이브 파이프라인의 토픽이 많아 DDS 가 바쁠수록 intermittent 하게 멈춤). 모델을 먼저
+    #   완전히 올려 CUDA/스레드를 안정화한 뒤 rclpy 를 띄우면 충돌이 사라진다.
+    m = resolve_model.resolve(model_source=args.model_source)
+    print(f"[lmnet] model source={m['source']} ckpt={m['ckpt']}", flush=True)
+    if m["source"] == "pretrained":
+        print("[lmnet] finetuned 없음 — KITTI pretrained 동작(CARLA 품질 낮음). 먼저 Retrain 권장", flush=True)
+    try:
+        infer = _build_infer(m, args.device)
+    except Exception as e:
+        if args.device == "cpu" or not _is_cuda_error(e):
+            raise  # CUDA 외 실패는 기존 실패 경로(예외 전파) 유지
+        print(f"[lmnet] GPU 백엔드 초기화 실패({e}) → CPU 폴백", flush=True)
+        infer = _build_infer(m, "cpu")
+    rclpy.init()
+    node = LMNetNode(args, infer, m)
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
@@ -0,0 +1,25 @@
+"""ego-pose 유틸 — nav_msgs/Odometry 의 (position, orientation) → 4x4, 상대 pose."""
+import numpy as np
+
+
+def pose_to_matrix(x, y, z, qx, qy, qz, qw):
+    """위치 + 쿼터니언 → 4x4 homogeneous."""
+    n = qx * qx + qy * qy + qz * qz + qw * qw
+    s = 0.0 if n == 0 else 2.0 / n
+    xx, yy, zz = qx * qx * s, qy * qy * s, qz * qz * s
+    xy, xz, yz = qx * qy * s, qx * qz * s, qy * qz * s
+    wx, wy, wz = qw * qx * s, qw * qy * s, qw * qz * s
+    R = np.array([
+        [1 - (yy + zz), xy - wz,        xz + wy],
+        [xy + wz,       1 - (xx + zz),  yz - wx],
+        [xz - wy,       yz + wx,        1 - (xx + yy)],
+    ], dtype=np.float64)
+    T = np.eye(4)
+    T[:3, :3] = R
+    T[:3, 3] = [x, y, z]
+    return T
+
+
+def relative_pose(curr, past):
+    """과거점을 현재 프레임으로 옮기는 변환 inv(curr) @ past."""
+    return np.linalg.inv(curr) @ past
@@ -0,0 +1,68 @@
+#!/usr/bin/env python3
+"""LiDAR spherical range-image projection (모델 무관, 순수 numpy).
+
+RangeNet++/SalsaNext 계열 표준 투영. 점별 (proj_x, proj_y) 를 보존해
+네트워크 픽셀 출력 → 점별 라벨 복원(un-projection)을 가능케 한다.
+"""
+from dataclasses import dataclass
+
+import numpy as np
+
+
+@dataclass
+class ProjectionResult:
+    range_image: np.ndarray  # (5, H, W): range(depth), x, y, z, intensity  (RangeNet/SalsaNext 순서)
+    px: np.ndarray           # (V,) valid 점들의 열 index
+    py: np.ndarray           # (V,) valid 점들의 행 index
+    valid: np.ndarray        # (N,) bool — range 범위 안 점 마스크
+
+
+class RangeProjector:
+    def __init__(self, proj_H, proj_W, fov_up, fov_down, max_range, min_range):
+        self.H = int(proj_H)
+        self.W = int(proj_W)
+        self.fov_up = float(fov_up) / 180.0 * np.pi
+        self.fov_down = float(fov_down) / 180.0 * np.pi
+        self.fov = abs(self.fov_up) + abs(self.fov_down)
+        self.max_range = float(max_range)
+        self.min_range = float(min_range)
+
+    def project(self, points):
+        xyz = points[:, :3]
+        intensity = points[:, 3] if points.shape[1] > 3 else np.zeros(len(points), np.float32)
+        depth = np.linalg.norm(xyz, axis=1)
+        valid = (depth > self.min_range) & (depth < self.max_range)
+
+        x, y, z = xyz[:, 0], xyz[:, 1], xyz[:, 2]
+        yaw = -np.arctan2(y, x)
+        pitch = np.arcsin(np.clip(z / np.maximum(depth, 1e-6), -1.0, 1.0))
+
+        proj_x = 0.5 * (yaw / np.pi + 1.0)              # [0,1]
+        proj_y = 1.0 - (pitch + abs(self.fov_down)) / self.fov
+        proj_x = np.clip(np.floor(proj_x * self.W), 0, self.W - 1).astype(np.int32)
+        proj_y = np.clip(np.floor(proj_y * self.H), 0, self.H - 1).astype(np.int32)
+
+        img = np.zeros((5, self.H, self.W), dtype=np.float32)
+        # 가까운 점 우선(겹치면 작은 depth 가 픽셀 차지) — depth 내림차순으로 채움
+        order = np.argsort(depth)[::-1]
+        vi = np.where(valid)[0]
+        # depth 내림차순 중 valid 만 채워 마지막에 가까운 점이 덮어쓰게
+        fill = order[np.isin(order, vi)]
+        py, px = proj_y[fill], proj_x[fill]
+        img[0, py, px] = depth[fill]      # range (SalsaNext img_means[0]=12.12)
+        img[1, py, px] = x[fill]
+        img[2, py, px] = y[fill]
+        img[3, py, px] = z[fill]
+        img[4, py, px] = intensity[fill]
+
+        return ProjectionResult(
+            range_image=img,
+            px=proj_x[valid], py=proj_y[valid], valid=valid,
+        )
+
+    def unproject(self, pixel_labels, proj: ProjectionResult):
+        """pixel_labels (H,W) → 점별 라벨 (N,). valid 아닌 점은 0(unlabeled)."""
+        n = proj.valid.shape[0]
+        labels = np.zeros(n, dtype=np.int64)
+        labels[proj.valid] = pixel_labels[proj.py, proj.px]
+        return labels
@@ -0,0 +1,28 @@
+"""LMNet residual range-image (deploy). retrain gen_residual.compute_residual 와 동일 로직 복제.
+학습-배포 일관성을 위해 같은 RangeProjector 공식을 쓴다."""
+import numpy as np
+
+from perception_lmnet_pkg.range_projection import RangeProjector  # noqa: F401  (호출측이 projector 주입)
+
+
+def _range_image(points, projector):
+    """투영 결과의 range 채널(H,W). range_image[0] = depth."""
+    return projector.project(points).range_image[0]
+
+
+def compute_residual(curr_points, past_points, T_past_to_curr, projector):
+    """현재 프레임 기준 residual(H,W). T_past_to_curr: 과거점을 현재 프레임으로 옮기는 4x4."""
+    past_xyz = past_points[:, :3]
+    hom = np.concatenate([past_xyz, np.ones((len(past_xyz), 1), np.float32)], axis=1)
+    past_in_curr = (T_past_to_curr @ hom.T).T[:, :3]
+    if past_points.shape[1] > 3:
+        past_aligned = np.concatenate(
+            [past_in_curr.astype(np.float32), past_points[:, 3:4]], axis=1)
+    else:
+        past_aligned = past_in_curr.astype(np.float32)
+    cur_r = _range_image(curr_points, projector)
+    past_r = _range_image(past_aligned, projector)
+    valid = (cur_r > 0) & (past_r > 0)
+    res = np.zeros_like(cur_r)
+    res[valid] = np.abs(cur_r[valid] - past_r[valid]) / np.maximum(cur_r[valid], 1e-6)
+    return res
@@ -0,0 +1,43 @@
+"""모델 경로 결정 — finetuned(retrain 산출물) 우선, 없거나 불완전하면 baked pretrained fallback.
+※ 작은 함수 하나로 분리 — 나중에 명시적 선택(Rule model_source=auto|finetuned|pretrained)을
+   한 분기로 끼우기 위함."""
+import os
+
+# 가중치 공유 단일 위치(docker/local): 호스트 .caches/config/nav2_config/perception/models/lmnet/*
+#   → 컨테이너 /perception_models/lmnet/{finetuned,pretrained} (SWRule config_mounts).
+DEFAULT_FINETUNED = "/perception_models/lmnet/finetuned"
+SHARED_PRE_DIR = "/perception_models/lmnet/pretrained"   # 공유 pretrained (Phase 2 채움)
+DEFAULT_PRE_CKPT = "/opt/lmnet_pretrained/SalsaNext"     # 이미지 baked 안전망
+DEFAULT_PRE_ARCH = "/opt/LMNet/mos_SalsaNext/salsanext_mos.yml"
+DEFAULT_PRE_DATA = ("/opt/LMNet/mos_SalsaNext/train/tasks/semantic/"
+                    "config/labels/semantic-kitti-mos.yaml")
+
+
+def _shared_pretrained():
+    # 공유 pretrained(.../pretrained: ckpt+arch+data)가 채워져 있으면 baked(/opt) 보다 우선.
+    ckpt = os.path.join(SHARED_PRE_DIR, "SalsaNext")
+    arch = os.path.join(SHARED_PRE_DIR, "arch_cfg.yaml")
+    data = os.path.join(SHARED_PRE_DIR, "data_cfg.yaml")
+    if os.path.isfile(ckpt) and os.path.isfile(arch) and os.path.isfile(data):
+        return {"source": "pretrained", "ckpt": ckpt, "arch_cfg": arch, "data_cfg": data}
+    return None
+
+
+def resolve(model_source="finetuned",
+            finetuned_dir=DEFAULT_FINETUNED,
+            pretrained_ckpt=DEFAULT_PRE_CKPT,
+            pretrained_arch=DEFAULT_PRE_ARCH,
+            pretrained_data=DEFAULT_PRE_DATA):
+    # 3단 fallback: finetuned → 공유 pretrained → baked pretrained(/opt 안전망).
+    # model_source=="pretrained" 면 finetuned 있어도 pretrained 강제(공유 우선, 없으면 baked).
+    _baked = {"source": "pretrained", "ckpt": pretrained_ckpt,
+              "arch_cfg": pretrained_arch, "data_cfg": pretrained_data}
+    _pretrained = _shared_pretrained() or _baked
+    if model_source == "pretrained":
+        return _pretrained
+    ckpt = os.path.join(finetuned_dir, "SalsaNext_valid_best")
+    arch = os.path.join(finetuned_dir, "arch_cfg.yaml")
+    data = os.path.join(finetuned_dir, "data_cfg.yaml")
+    if os.path.isfile(ckpt) and os.path.isfile(arch) and os.path.isfile(data):
+        return {"source": "finetuned", "ckpt": ckpt, "arch_cfg": arch, "data_cfg": data}
+    return _pretrained
@@ -0,0 +1,21 @@
+"""N-스캔 링버퍼 — (points, pose_4x4). residual 생성에 현재+과거 N 프레임 필요."""
+from collections import deque
+
+
+class ScanBuffer:
+    def __init__(self, num_residual):
+        self.n = num_residual
+        self._buf = deque(maxlen=num_residual + 1)  # 현재 + 과거 N
+
+    def push(self, points, pose):
+        self._buf.append((points, pose))
+
+    def ready(self):
+        return len(self._buf) >= self.n + 1
+
+    def current(self):
+        return self._buf[-1]
+
+    def past(self, k):
+        """k 프레임 전 (1=직전). ready() 일 때만 호출."""
+        return self._buf[-1 - k]
@@ -0,0 +1,129 @@
+"""deploy 검증용 — 이미 떠 있는 CARLA 에 connect(load_world ❌) 후 ego 근처에 차+보행자 스폰.
+MOS 가 빨갛게 칠하는지 RViz 비교용. Ctrl-C 시 스폰 actor 전부 cleanup.
+
+★ sync 모드 대응: 파이프라인(브리지)이 world 를 tick 한다. 우리가 만든 TM 은 tick 되지 않아
+  autopilot 차가 안 움직인다. 그래서 autopilot/AI-controller 대신 enable_constant_velocity(차)
+  + WalkerControl(보행자)를 쓴다 — 둘 다 브리지의 world tick 으로 전진하므로 TM 불필요.
+★ ego 근처 스폰: 라이다 범위 안에 있어야 MOS 가 본다. 맵 전역 랜덤 spawn 은 대부분 사거리 밖.
+"""
+import argparse
+import math
+import random
+import time
+
+import carla
+
+
+def _find_ego(world, tries=20):
+    """hero/ego(=vehicle.custom.*) 위치 찾기. sync 보조클라 snapshot 이 불안정해 재시도.
+    ★ wait_for_tick 으로 매 시도마다 브리지 tick 을 받아 신선한 스냅샷 확보 — sleep 만 하면
+      passive 클라가 빈 스냅샷을 반복 수신해 ego 를 못 찾고 맵 랜덤 위치에 스폰됨(범위 밖)."""
+    for _ in range(tries):
+        try:
+            world.wait_for_tick(2.0)
+        except Exception:
+            time.sleep(0.3)
+        for a in world.get_actors().filter("vehicle.*"):
+            if a.attributes.get("role_name") in ("hero", "ego") or a.type_id.startswith("vehicle.custom."):
+                return a
+    return None
+
+
+def _near(loc, ref, radius):
+    return math.hypot(loc.x - ref.x, loc.y - ref.y) <= radius
+
+
+def _orbit_dir(loc, center):
+    """ego 중심 접선 방향(반시계) — 보행자가 ego 주위를 돌며 라이다 범위에 머물게."""
+    rx, ry = loc.x - center.x, loc.y - center.y
+    n = math.hypot(rx, ry) or 1.0
+    return carla.Vector3D(x=-ry / n, y=rx / n, z=0.0)
+
+
+def main():
+    ap = argparse.ArgumentParser()
+    ap.add_argument("--host", default="localhost")
+    ap.add_argument("--port", type=int, default=2000)
+    ap.add_argument("--num-vehicles", type=int, default=15)
+    ap.add_argument("--num-walkers", type=int, default=10)
+    ap.add_argument("--radius", type=float, default=45.0, help="ego 중심 스폰 반경(m, 라이다 범위 내)")
+    ap.add_argument("--speed", type=float, default=5.0, help="차량 등속(m/s)")
+    args = ap.parse_args()
+
+    client = carla.Client(args.host, args.port)
+    client.set_timeout(10.0)
+    world = client.get_world()             # ❌ load_world 안 함 — 실행 중 맵 유지
+    bp = world.get_blueprint_library()
+    spawned = []
+
+    ego = _find_ego(world)
+    if ego is None:
+        print("[spawn_npc] 경고: ego 를 못 찾음 — 맵 전역에 스폰(라이다 범위 밖일 수 있음)")
+        center = carla.Location(0, 0, 0)
+    else:
+        center = ego.get_location()
+        print(f"[spawn_npc] ego={ego.type_id} @ ({center.x:.1f},{center.y:.1f}) 반경 {args.radius}m 내 스폰")
+
+    try:
+        # ── 차량: ego 근처 spawn point 에 스폰 후 enable_constant_velocity (브리지 tick 으로 전진) ──
+        vbps = [b for b in bp.filter("vehicle.*") if not b.id.startswith("vehicle.custom.")]
+        sps = [sp for sp in world.get_map().get_spawn_points()
+               if ego is None or _near(sp.location, center, args.radius)]
+        random.shuffle(sps)
+        nveh = 0
+        for sp in sps:
+            if nveh >= args.num_vehicles:
+                break
+            v = world.try_spawn_actor(random.choice(vbps), sp)
+            if v:
+                # 차량 local +x = 전방. 등속으로 전진 → static 세계 대비 moving.
+                v.enable_constant_velocity(carla.Vector3D(x=args.speed, y=0.0, z=0.0))
+                spawned.append(v)
+                nveh += 1
+
+        # ── 보행자: ego 주변 링 좌표에 직접 스폰 (nav mesh 불필요 — WalkerControl 로 직접 보행) ──
+        #   get_random_location_from_navigation 은 nav mesh 없으면 None 만 → 못 씀.
+        wbps = bp.filter("walker.pedestrian.*")
+        ring = []
+        for r in (6, 9, 12, 16, 20):
+            for deg in range(0, 360, 30):
+                a = math.radians(deg)
+                ring.append(carla.Location(center.x + r * math.cos(a),
+                                           center.y + r * math.sin(a),
+                                           center.z + 1.0))   # +1m: 지면 위로(낙하 안착)
+        random.shuffle(ring)
+        nwlk = 0
+        for loc in ring:
+            if nwlk >= args.num_walkers:
+                break
+            w = world.try_spawn_actor(random.choice(wbps), carla.Transform(loc))
+            if not w:
+                continue
+            w.apply_control(carla.WalkerControl(direction=_orbit_dir(loc, center), speed=1.3))
+            spawned.append(w)
+            nwlk += 1
+
+        print(f"[spawn_npc] 차 {nveh} + 보행자 {nwlk} 스폰(ego 주위 orbit). Ctrl-C 로 종료/정리")
+        # 매 주기 현재 위치 기준 접선방향 재지정 → ego 주위를 계속 돌게(라이다 범위 유지)
+        while True:
+            for a in spawned:
+                if a.type_id.startswith("walker.pedestrian"):
+                    try:
+                        a.apply_control(carla.WalkerControl(
+                            direction=_orbit_dir(a.get_location(), center), speed=1.3))
+                    except Exception:
+                        pass
+            time.sleep(1.5)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        for a in spawned:
+            try:
+                a.destroy()
+            except Exception:
+                pass
+        print("[spawn_npc] cleanup 완료")
+
+
+if __name__ == "__main__":
+    main()
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/perception_lmnet_pkg
+[install]
+install_scripts=$base/lib/perception_lmnet_pkg
@@ -0,0 +1,35 @@
+
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'perception_lmnet_pkg'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='dgist',
+    maintainer_email='jang1future@gmail.com',
+    description='LiDAR moving object segmentation (LMNet / SalsaNext-MOS) perception module',
+    license='TODO: License declaration',
+    extras_require={
+        'test': [
+            'pytest',
+        ],
+    },
+    entry_points={
+        'console_scripts': [
+            'lmnet = perception_lmnet_pkg.lmnet_node:main'
+        ],
+    },
+)
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'
@@ -0,0 +1,22 @@
+import numpy as np
+from perception_lmnet_pkg import pose_util
+
+
+def test_identity_quaternion_pure_translation():
+    T = pose_util.pose_to_matrix(x=1.0, y=2.0, z=3.0, qx=0, qy=0, qz=0, qw=1)
+    assert np.allclose(T[:3, 3], [1, 2, 3])
+    assert np.allclose(T[:3, :3], np.eye(3))
+
+
+def test_90deg_yaw():
+    s = np.sqrt(0.5)
+    T = pose_util.pose_to_matrix(0, 0, 0, 0, 0, s, s)
+    assert np.allclose(T[:3, :3] @ np.array([1.0, 0, 0]), [0, 1, 0], atol=1e-6)
+
+
+def test_relative_pose_past_to_curr():
+    A = pose_util.pose_to_matrix(0, 0, 0, 0, 0, 0, 1)
+    B = pose_util.pose_to_matrix(5, 0, 0, 0, 0, 0, 1)
+    T = pose_util.relative_pose(curr=B, past=A)
+    p = T @ np.array([0.0, 0, 0, 1])
+    assert np.allclose(p[:3], [-5, 0, 0])
@@ -0,0 +1,22 @@
+import numpy as np
+from perception_lmnet_pkg import residual_lib
+from perception_lmnet_pkg.range_projection import RangeProjector
+
+
+def _proj():
+    return RangeProjector(proj_H=32, proj_W=180, fov_up=21.2, fov_down=-21.2,
+                          max_range=90.0, min_range=1.0)
+
+
+def test_identical_scan_zero_motion_zero_residual():
+    pts = np.random.RandomState(0).uniform(-20, 20, (500, 4)).astype(np.float32)
+    res = residual_lib.compute_residual(pts, pts, np.eye(4), _proj())
+    assert res.shape == (32, 180)
+    assert np.nanmax(np.abs(res)) < 1e-3
+
+
+def test_moved_point_makes_nonzero_residual():
+    pts = np.array([[10.0, 0, 0, 1]] * 50, np.float32)
+    T = np.eye(4); T[0, 3] = 5.0
+    res = residual_lib.compute_residual(pts, pts, T, _proj())
+    assert np.nanmax(np.abs(res)) > 0.0
@@ -0,0 +1,89 @@
+import os
+from perception_lmnet_pkg import resolve_model
+
+
+def test_finetuned_when_present(tmp_path):
+    ft = tmp_path / "lmnet_finetuned"
+    ft.mkdir()
+    (ft / "SalsaNext_valid_best").write_bytes(b"x")
+    (ft / "arch_cfg.yaml").write_text("a: 1")
+    (ft / "data_cfg.yaml").write_text("b: 1")
+    r = resolve_model.resolve(finetuned_dir=str(ft),
+                              pretrained_ckpt="/opt/lmnet_pretrained/SalsaNext",
+                              pretrained_arch="/opt/x/salsanext_mos.yml",
+                              pretrained_data="/opt/x/semantic-kitti-mos.yaml")
+    assert r["source"] == "finetuned"
+    assert r["ckpt"] == str(ft / "SalsaNext_valid_best")
+    assert r["arch_cfg"] == str(ft / "arch_cfg.yaml")
+
+
+def test_pretrained_fallback_when_absent(tmp_path):
+    ft = tmp_path / "lmnet_finetuned"
+    r = resolve_model.resolve(finetuned_dir=str(ft),
+                              pretrained_ckpt="/opt/lmnet_pretrained/SalsaNext",
+                              pretrained_arch="/opt/x/salsanext_mos.yml",
+                              pretrained_data="/opt/x/semantic-kitti-mos.yaml")
+    assert r["source"] == "pretrained"
+    assert r["ckpt"] == "/opt/lmnet_pretrained/SalsaNext"
+    assert r["arch_cfg"] == "/opt/x/salsanext_mos.yml"
+
+
+def test_partial_finetuned_falls_back(tmp_path):
+    ft = tmp_path / "lmnet_finetuned"; ft.mkdir()
+    (ft / "SalsaNext_valid_best").write_bytes(b"x")
+    r = resolve_model.resolve(finetuned_dir=str(ft),
+                              pretrained_ckpt="/opt/lmnet_pretrained/SalsaNext",
+                              pretrained_arch="/opt/x/salsanext_mos.yml",
+                              pretrained_data="/opt/x/semantic-kitti-mos.yaml")
+    assert r["source"] == "pretrained"
+
+
+def _make_finetuned(tmp_path):
+    ft = tmp_path / "lmnet_finetuned"
+    ft.mkdir()
+    (ft / "SalsaNext_valid_best").write_bytes(b"x")
+    (ft / "arch_cfg.yaml").write_text("a: 1")
+    (ft / "data_cfg.yaml").write_text("b: 1")
+    return ft
+
+
+def test_model_source_pretrained_forces_baked_even_if_finetuned_present(tmp_path):
+    # 스위치=pretrained 면 finetuned 3파일 다 있어도 무시하고 baked pretrained 강제
+    ft = _make_finetuned(tmp_path)
+    r = resolve_model.resolve(model_source="pretrained",
+                              finetuned_dir=str(ft),
+                              pretrained_ckpt="/opt/lmnet_pretrained/SalsaNext",
+                              pretrained_arch="/opt/x/salsanext_mos.yml",
+                              pretrained_data="/opt/x/semantic-kitti-mos.yaml")
+    assert r["source"] == "pretrained"
+    assert r["ckpt"] == "/opt/lmnet_pretrained/SalsaNext"
+
+
+def test_model_source_finetuned_uses_finetuned_when_present(tmp_path):
+    ft = _make_finetuned(tmp_path)
+    r = resolve_model.resolve(model_source="finetuned",
+                              finetuned_dir=str(ft),
+                              pretrained_ckpt="/opt/lmnet_pretrained/SalsaNext",
+                              pretrained_arch="/opt/x/salsanext_mos.yml",
+                              pretrained_data="/opt/x/semantic-kitti-mos.yaml")
+    assert r["source"] == "finetuned"
+    assert r["ckpt"] == str(ft / "SalsaNext_valid_best")
+
+
+def test_model_source_finetuned_silent_fallback_when_absent(tmp_path):
+    # 스위치=finetuned 인데 학습본 없으면 조용히 pretrained fallback
+    ft = tmp_path / "lmnet_finetuned"
+    r = resolve_model.resolve(model_source="finetuned",
+                              finetuned_dir=str(ft),
+                              pretrained_ckpt="/opt/lmnet_pretrained/SalsaNext",
+                              pretrained_arch="/opt/x/salsanext_mos.yml",
+                              pretrained_data="/opt/x/semantic-kitti-mos.yaml")
+    assert r["source"] == "pretrained"
+
+
+def test_resolve_no_args_backward_compat(tmp_path):
+    # 인자 없이 resolve() — model_source default 로 기존 동작 유지(하위호환).
+    # default finetuned 경로는 실 환경에 없으므로 pretrained 로 떨어진다.
+    r = resolve_model.resolve()
+    assert r["source"] == "pretrained"
+    assert r["ckpt"] == resolve_model.DEFAULT_PRE_CKPT
@@ -0,0 +1,37 @@
+import numpy as np
+from perception_lmnet_pkg.scan_buffer import ScanBuffer
+
+
+def _pts(v):
+    return np.full((3, 4), float(v), np.float32)
+
+
+def test_not_ready_until_n_plus_one():
+    buf = ScanBuffer(num_residual=2)
+    buf.push(_pts(0), np.eye(4))
+    assert not buf.ready()
+    buf.push(_pts(1), np.eye(4))
+    assert not buf.ready()
+    buf.push(_pts(2), np.eye(4))
+    assert buf.ready()
+
+
+def test_past_k_returns_kth_back():
+    buf = ScanBuffer(num_residual=2)
+    for i in range(3):
+        buf.push(_pts(i), np.eye(4))
+    cur_pts, cur_pose = buf.current()
+    assert cur_pts[0, 0] == 2.0
+    p1_pts, _ = buf.past(1)
+    p2_pts, _ = buf.past(2)
+    assert p1_pts[0, 0] == 1.0
+    assert p2_pts[0, 0] == 0.0
+
+
+def test_maxlen_evicts_old():
+    buf = ScanBuffer(num_residual=1)
+    for i in range(5):
+        buf.push(_pts(i), np.eye(4))
+    cur, _ = buf.current()
+    prev, _ = buf.past(1)
+    assert cur[0, 0] == 4.0 and prev[0, 0] == 3.0