diff --git a/src/perception3d_pkg/config/3d_fr09.yaml b/src/perception3d_pkg/config/3d_fr09.yaml
index 5c395d7..b03087c 100755
--- a/src/perception3d_pkg/config/3d_fr09.yaml
+++ b/src/perception3d_pkg/config/3d_fr09.yaml
@@ -2,15 +2,12 @@
odometry:
ros__parameters:
- # RewrittenYaml 주입은 키가 존재해야 동작 (DO_NOT YAML 룰) — launch arg 가 실값 주입
- use_sim_time: false
odom_topic: "/vehicle_odom"
odom_frame: "odom"
base_frame: "base_link"
pcd_crop:
ros__parameters:
- use_sim_time: false
input_topic: "/raw/lidar"
output_topic: "/lidar_filtered"
min_z: -5.0
diff --git a/src/perception3d_pkg/config/3d_mkmini.yaml b/src/perception3d_pkg/config/3d_mkmini.yaml
index 27c2f19..fb29cae 100755
--- a/src/perception3d_pkg/config/3d_mkmini.yaml
+++ b/src/perception3d_pkg/config/3d_mkmini.yaml
@@ -2,15 +2,12 @@
odometry:
ros__parameters:
- # RewrittenYaml 주입은 키가 존재해야 동작 (DO_NOT YAML 룰) — launch arg 가 실값 주입
- use_sim_time: false
odom_topic: "/vehicle_odom"
odom_frame: "odom"
base_frame: "base_link"
pcd_crop:
ros__parameters:
- use_sim_time: false
input_topic: "/raw/lidar"
output_topic: "/lidar_filtered"
min_z: -2.5
diff --git a/src/perception_bevplace2_pkg/config/bevplace.yaml b/src/perception_bevplace2_pkg/config/bevplace.yaml
deleted file mode 100755
index 3553809..0000000
--- a/src/perception_bevplace2_pkg/config/bevplace.yaml
+++ /dev/null
@@ -1,21 +0,0 @@
-bevplace2:
- ros__parameters:
- # 맵 DB(map.npz)는 필터된 스캔(=RTABMap .db 매핑 입력 = /lidar_filtered)으로 빌드되므로,
- # 쿼리도 같은 /lidar_filtered 를 써야 분포가 일치해 매칭이 맞는다. /raw/lidar(raw 분포)는
- # 분포가 달라 descriptor 불일치 → 오측위.
- input_topic: "/lidar_filtered"
- model_ckpt_path: ""
- map_db_path: ""
- match_dist_thresh: 1.0
- seed_initialpose_once: true
- # BEV 투영 파라미터 (모델 BEVPlace2 가 기대하는 입력 = KITTI Velodyne BEV 격자 ±40m/0.4m).
- # 센서가 아니라 모델 학습값에 맞춘다. 잘못 주면 점이 엉뚱한 셀에 뭉쳐 descriptor 붕괴.
- # build_map_db(맵 빌드)와 bevplace2_node(query)가 같은 값을 써야 매칭이 맞음
- # (run.sh 가 이 파일로 통일).
- x_min: -40.0
- x_max: 40.0
- y_min: -40.0
- y_max: 40.0
- resolution: 0.4
- max_density: 20.0
- use_sim_time: true
diff --git a/src/perception_bevplace2_pkg/launch/bevplace2.launch.py b/src/perception_bevplace2_pkg/launch/bevplace2.launch.py
deleted file mode 100755
index dc72e93..0000000
--- a/src/perception_bevplace2_pkg/launch/bevplace2.launch.py
+++ /dev/null
@@ -1,149 +0,0 @@
-import glob
-import importlib.util
-import os
-import subprocess
-import sys
-
-import yaml
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
-# 모델/맵 경로 컨벤션 (단독 deploy run.sh 와 동일 — 컨테이너 내부 고정 경로)
-# 가중치 공유 단일 위치(docker/local): /perception_models/bevplace2/{finetuned,pretrained}
-# (호스트 .caches/config/nav2_config/perception/models/bevplace2/*, SWRule config_mounts).
-FINETUNED_CKPT = '/perception_models/bevplace2/finetuned/bevplace2.pth.tar'
-SHARED_PRE_CKPT = '/perception_models/bevplace2/pretrained/bevplace2.pth.tar' # 공유 pretrained (Phase 2 채움)
-PRETRAINED_CKPT = '/opt/bevplace2_pretrained/bevplace2.pth.tar' # 이미지 baked 안전망(Dockerfile COPY)
-MAP_DB = '/perception_data/map_descriptors/map.npz'
-MAP_SRC = MAP_DB + '.src' # 빌드된 map.npz 의 소스(.db+모델) 식별 → 교체 감지
-MAP_SEQ = '/perception_data/pr/sequences/00'
-SELECTED_MAP_DIR = '/selected_map' # 선택 맵 .db (config_mounts 로 마운트)
-
-ARGUMENTS = [
- DeclareLaunchArgument('use_sim_time', default_value='true'),
- # 빈값(기본) = launch 가 model_source 기준으로 자체 resolve.
- # 단독 deploy run.sh 가 MODEL_ARGS 로 명시 전달하면(비어있지 않으면) 그 값이 그대로 이긴다(무회귀).
- DeclareLaunchArgument('model_ckpt_path', default_value=''),
- DeclareLaunchArgument('map_db_path', default_value=''),
- DeclareLaunchArgument('model_source', default_value='finetuned'), # UI 스위치: finetuned|pretrained
-]
-
-
-def _resolve_ckpt(model_source):
- """model_source(07 §6 공통 계약) 기반 ckpt 자동 resolve (run.sh 로직 이식).
- pretrained → finetuned 있어도 건너뛰고 base pretrained 강제. 그 외 → finetuned 우선(조용한 fallback)."""
- if model_source != 'pretrained' and os.path.isfile(FINETUNED_CKPT):
- print(f'[bevplace2] 재학습 모델 사용: {FINETUNED_CKPT}')
- return FINETUNED_CKPT
- if os.path.isfile(SHARED_PRE_CKPT):
- print(f'[bevplace2] 공유 pretrained 사용: {SHARED_PRE_CKPT}')
- return SHARED_PRE_CKPT
- print(f'[bevplace2] base pretrained(baked) 사용(model_source={model_source}): {PRETRAINED_CKPT}')
- return PRETRAINED_CKPT
-
-
-def _script_dir():
- """맵 빌드 스크립트(db_to_mapdb/build_map_db)가 있는 pkg 모듈 경로 (run.sh 의 find src 대체).
- colcon 설치본 site-packages 안에 python 모듈로 함께 설치되므로 import 경로로 찾는다."""
- spec = importlib.util.find_spec('perception_bevplace2_pkg')
- return os.path.dirname(spec.origin) if spec and spec.origin else ''
-
-
-def _proj_args():
- """투영 파라미터를 bevplace.yaml(단일 소스, SWRule override) 에서 읽어 맵 빌드에 적용.
- query 노드(bevplace2_node)도 같은 bevplace.yaml 을 쓰므로 빌드/쿼리 투영이 일치한다(매칭 정합 필수)."""
- try:
- yml = os.path.join(get_package_share_directory('perception_bevplace2_pkg'),
- 'config', 'bevplace.yaml')
- with open(yml) as f:
- p = yaml.safe_load(f)['bevplace2']['ros__parameters']
- return ['--x_min', str(p.get('x_min', -40.0)), '--x_max', str(p.get('x_max', 40.0)),
- '--y_min', str(p.get('y_min', -40.0)), '--y_max', str(p.get('y_max', 40.0)),
- '--resolution', str(p.get('resolution', 0.4)),
- '--max_density', str(p.get('max_density', 20.0))]
- except Exception as e: # 읽기 실패 시 빈 인자 → 스크립트 기본값(모델 학습값 KITTI) 사용
- print(f'[bevplace2] bevplace.yaml 투영 파라미터 읽기 실패(기본값 사용): {e}')
- return []
-
-
-def _mtime(path):
- """run.sh 의 stat -c %Y 대응 (없으면 빈 문자열) — SRC_TAG 포맷 호환 유지."""
- try:
- return str(int(os.stat(path).st_mtime))
- except OSError:
- return ''
-
-
-def _ensure_map_db(ckpt):
- """맵 DB(map.npz) 자동 빌드 (run.sh 로직 이식). 선택 맵 .db 가 마운트돼 있으면 그걸로
- 빌드(맵/모델 바뀌면 재빌드). .db 안의 Admin.opt_poses(최적화 pose)+Data.scan(/lidar_filtered
- 분포)을 직접 디코드 → rtabmap 불필요. 결과 map.npz 가 있으면 그 경로, 없으면 빈 문자열."""
- pydir = _script_dir()
- sel = sorted(glob.glob(os.path.join(SELECTED_MAP_DIR, '*.db'))) # ls -1 | head -1 동일
- selected_db = sel[0] if sel else ''
- proj = _proj_args()
- if selected_db and pydir:
- src_tag = f'{selected_db} {_mtime(selected_db)} {ckpt} {_mtime(ckpt)}'
- old_tag = ''
- if os.path.isfile(MAP_SRC):
- with open(MAP_SRC) as f:
- old_tag = f.read().rstrip('\n')
- if not os.path.isfile(MAP_DB) or old_tag != src_tag:
- print(f'[bevplace2] 맵 DB 자동 빌드 (.db→map.npz): {selected_db} ({" ".join(proj)})')
- r = subprocess.run([sys.executable, os.path.join(pydir, 'db_to_mapdb.py'),
- ckpt, selected_db, MAP_DB] + proj, cwd=pydir)
- if r.returncode == 0:
- with open(MAP_SRC, 'w') as f:
- f.write(src_tag + '\n')
- else:
- print('[bevplace2] 맵 DB 자동 빌드 실패(계속)')
- else:
- print('[bevplace2] map.npz 최신 (선택 맵/모델 변경 없음) — 재빌드 생략')
- # fallback: .db 미마운트 시 수집 시퀀스(00, ride-along)로 빌드
- elif (not os.path.isfile(MAP_DB) and os.path.isdir(os.path.join(MAP_SEQ, 'velodyne'))
- and os.path.isfile(os.path.join(MAP_SEQ, 'poses.txt')) and pydir):
- print(f'[bevplace2] 맵 DB 빌드 (sequences/00→map.npz) ({" ".join(proj)})')
- r = subprocess.run([sys.executable, os.path.join(pydir, 'build_map_db.py'), ckpt,
- os.path.join(MAP_SEQ, 'velodyne'), os.path.join(MAP_SEQ, 'poses.txt'),
- MAP_DB] + proj, cwd=pydir)
- if r.returncode != 0:
- print('[bevplace2] 맵 DB 빌드 실패(계속)')
- return MAP_DB if os.path.isfile(MAP_DB) else ''
-
-
-def _launch_setup(context, *args, **kwargs):
- """OpaqueFunction: 인자가 명시되면(선언 기본값인 빈값과 다르면) 그대로 사용 —
- 단독 deploy run.sh 의 MODEL_ARGS 가 계속 이김(무회귀). 빈값일 때만 자동 resolve."""
- ckpt = LaunchConfiguration('model_ckpt_path').perform(context)
- map_db = LaunchConfiguration('map_db_path').perform(context)
- model_source = LaunchConfiguration('model_source').perform(context) or 'finetuned'
- if not ckpt: # 미명시(기본 빈값) → model_source + finetuned 존재 검사로 자동 resolve
- ckpt = _resolve_ckpt(model_source)
- if not map_db: # 미명시(기본 빈값) → map.npz 자동 빌드(staleness 체크) 후 resolve
- map_db = _ensure_map_db(ckpt)
- pkg = get_package_share_directory('perception_bevplace2_pkg')
- params_file = os.path.join(pkg, 'config', 'bevplace.yaml')
- remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
- load_nodes = GroupAction(actions=[
- Node(
- package='perception_bevplace2_pkg', executable='bevplace2',
- name='bevplace2', output='screen',
- parameters=[params_file, {
- 'use_sim_time': LaunchConfiguration('use_sim_time'),
- 'model_ckpt_path': ckpt,
- 'map_db_path': map_db,
- }],
- remappings=remappings,
- )
- ])
- return [load_nodes]
-
-
-def generate_launch_description():
- ld = LaunchDescription(ARGUMENTS)
- ld.add_action(OpaqueFunction(function=_launch_setup))
- return ld
diff --git a/src/perception_bevplace2_pkg/launch/perception.launch.py b/src/perception_bevplace2_pkg/launch/perception.launch.py
deleted file mode 100755
index 619a576..0000000
--- a/src/perception_bevplace2_pkg/launch/perception.launch.py
+++ /dev/null
@@ -1,32 +0,0 @@
-"""perception_manager 공통 진입점 (통합 perception 모듈).
-
-LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로
-`ros2 launch perception_bevplace2_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..`
-를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(bevplace2.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_bevplace2_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', 'bevplace2.launch.py')),
- launch_arguments={
- 'robot': LaunchConfiguration('robot'),
- 'use_sim_time': LaunchConfiguration('use_sim_time'),
- 'model_source': LaunchConfiguration('model_source'),
- }.items(),
- ),
- ])
diff --git a/src/perception_bevplace2_pkg/package.xml b/src/perception_bevplace2_pkg/package.xml
deleted file mode 100755
index 4cc567d..0000000
--- a/src/perception_bevplace2_pkg/package.xml
+++ /dev/null
@@ -1,25 +0,0 @@
-
-
-
- perception_bevplace2_pkg
- 0.0.0
- LiDAR place recognition (BEVPlace2) perception module
- dgist
- TODO: License declaration
-
- rclpy
- sensor_msgs
- sensor_msgs_py
- geometry_msgs
- std_msgs
- visualization_msgs
-
- ament_copyright
- ament_flake8
- ament_pep257
- python3-pytest
-
-
- ament_python
-
-
diff --git a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/__init__.py b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/__init__.py
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bev_projection.py b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bev_projection.py
deleted file mode 100755
index a93f4f8..0000000
--- a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bev_projection.py
+++ /dev/null
@@ -1,62 +0,0 @@
-#!/usr/bin/env python3
-"""LiDAR point cloud → BEV(bird's-eye-view) image projection (모델 무관, 순수 numpy).
-
-BEVPlace/BEVPlace2 표준 입력. OverlapTransformer 의 range-image 와 달리 **top-down 격자**
-(x-y 평면)에 점 개수(density)를 누적한 1채널 BEV 이미지를 만든다. group conv 백본이 이
-BEV 이미지를 회전-등변(rotation-equivariant) descriptor 로 변환한다.
-
-투영 파라미터(격자 범위/해상도)는 "센서"가 아니라 "모델"이 학습된 입력 레이아웃이다.
-잘못 주면 점이 격자의 엉뚱한 칸에 뭉쳐 descriptor 가 붕괴한다 → 맵 빌드(build_map_db)와
-query(bevplace2_node)가 **반드시 같은 값**을 써야 매칭이 맞는다. 하드코딩 금지 — 호출부
-(node gp / build_map_db CLI / bevplace.yaml SWRule)가 Rule 값으로 덮어쓴다.
-"""
-from dataclasses import dataclass
-
-import numpy as np
-
-
-@dataclass
-class BEVResult:
- bev_image: np.ndarray # (1, H, W) float32 — density(점 개수) BEV, 정규화 전
- grid_x: np.ndarray # (V,) valid 점들의 열 index (x bin)
- grid_y: np.ndarray # (V,) valid 점들의 행 index (y bin)
- valid: np.ndarray # (N,) bool — 격자 범위 안 점 마스크
-
-
-class BEVProjector:
- """점군 (N,>=3) → density BEV 이미지 (1,H,W).
-
- x_range/y_range 는 [min,max] (m), resolution 은 셀 크기(m/px). BEVPlace2 기본은
- KITTI Velodyne 기준 ±40m / 0.4m → 200x200. CARLA Ouster/Vanjee 도 동일 격자를 쓰면
- 모델이 기대하는 분포와 맞는다(범위 밖 점은 drop).
- """
-
- def __init__(self, x_min=-40.0, x_max=40.0, y_min=-40.0, y_max=40.0,
- resolution=0.4, max_density=20.0):
- self.x_min = float(x_min)
- self.x_max = float(x_max)
- self.y_min = float(y_min)
- self.y_max = float(y_max)
- self.res = float(resolution)
- self.max_density = float(max_density)
- self.W = int(round((self.x_max - self.x_min) / self.res))
- self.H = int(round((self.y_max - self.y_min) / self.res))
-
- def project(self, points):
- xyz = points[:, :3]
- x, y = xyz[:, 0], xyz[:, 1]
- valid = (x > self.x_min) & (x < self.x_max) & (y > self.y_min) & (y < self.y_max)
-
- gx = np.clip(np.floor((x - self.x_min) / self.res), 0, self.W - 1).astype(np.int32)
- gy = np.clip(np.floor((y - self.y_min) / self.res), 0, self.H - 1).astype(np.int32)
-
- img = np.zeros((1, self.H, self.W), dtype=np.float32)
- vi = np.where(valid)[0]
- # density: 각 셀에 떨어진 점 개수 누적 (BEVPlace2 입력)
- np.add.at(img[0], (gy[vi], gx[vi]), 1.0)
- # 점 밀집도가 큰 셀이 포화되지 않게 상한 클립 후 [0,1] 정규화
- if self.max_density > 0:
- np.clip(img, 0.0, self.max_density, out=img)
- img /= self.max_density
-
- return BEVResult(bev_image=img, grid_x=gx[valid], grid_y=gy[valid], valid=valid)
diff --git a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bevplace2_node.py b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bevplace2_node.py
deleted file mode 100755
index 0f50eee..0000000
--- a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bevplace2_node.py
+++ /dev/null
@@ -1,132 +0,0 @@
-"""Place Recognition deploy 노드 (BEVPlace2). /lidar_filtered → BEV descriptor → 맵 DB
-최근접 → 현재 pose 추정 → /perception/relocalization(PoseWithCovarianceStamped) +
-/initialpose(측위 씨딩, 1회) + /perception/reloc_markers(★pose) 발행.
-ot_node.py 의 구조 미러 — backend 만 BEVPlaceBackend(BEV+group conv)로 교체."""
-import os
-import sys
-# colcon 설치된 entry point 로 실행되면 같은 패키지의 flat 모듈(bevplace_backend/descriptor_db/
-# bev_projection)이 top-level 로 안 잡혀 ModuleNotFoundError → 자기 디렉토리를 path 에 추가.
-sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
-import numpy as np
-import rclpy
-from rclpy.node import Node
-from sensor_msgs.msg import PointCloud2
-from geometry_msgs.msg import PoseWithCovarianceStamped
-from visualization_msgs.msg import Marker, MarkerArray
-import sensor_msgs_py.point_cloud2 as pc2
-from bevplace_backend import BEVPlaceBackend
-from descriptor_db import DescriptorDB
-
-
-def _is_cuda_error(e):
- s = str(e).lower()
- return 'cuda' in s or 'cudnn' in s or 'out of memory' in s
-
-
-def _pose_from_row(row12):
- T = np.array(row12, dtype=np.float64).reshape(3, 4)
- return T[:, 3], T[:, :3] # translation, R
-
-
-class PlaceRecognition(Node):
- def __init__(self):
- super().__init__('bevplace2')
- gp = lambda n, d: self.declare_parameter(n, d).value
- self.input_topic = gp('input_topic', '/lidar_filtered')
- ckpt = gp('model_ckpt_path', '') or os.environ.get('BEVPLACE_MODEL_CKPT', '')
- db_path = gp('map_db_path', '') or os.environ.get('BEVPLACE_MAP_DB', '')
- self.thresh = float(gp('match_dist_thresh', 1.0)) # L2 desc 거리 임계
- self.seed_once = bool(gp('seed_initialpose_once', True)) # /initialpose 1회만 씨딩
- self._seeded = False
- # BEV 투영 파라미터 — bevplace.yaml(SWRule config_mounts 로 override) 에서 읽는다.
- # 기본값은 모델 학습값(KITTI ±40m/0.4m). 맵 DB(build/db_to_mapdb)와 반드시 같아야 매칭.
- proj = dict(
- x_min=float(gp('x_min', -40.0)), x_max=float(gp('x_max', 40.0)),
- y_min=float(gp('y_min', -40.0)), y_max=float(gp('y_max', 40.0)),
- resolution=float(gp('resolution', 0.4)), max_density=float(gp('max_density', 20.0)),
- )
- device = gp('device', 'auto')
- self._ckpt, self._proj = ckpt, proj # CUDA 폴백 시 ckpt 재로드용 (.to 이동 금지)
- self._cpu_fallback = False # 런타임 1회 폴백 가드
- try:
- self.backend = BEVPlaceBackend(ckpt, device=device, **proj) if ckpt else None
- except Exception as e:
- if device == 'cpu' or not _is_cuda_error(e):
- raise
- self.get_logger().warn(f"[bevplace2] GPU 백엔드 초기화 실패({e}) → CPU 폴백")
- self._cpu_fallback = True
- try:
- self.backend = BEVPlaceBackend(ckpt, device='cpu', **proj)
- except Exception as e2:
- self.get_logger().error(f"[bevplace2] CPU 폴백 초기화 실패: {e2}")
- self.backend = None
- self.db = DescriptorDB(db_path) if db_path and os.path.exists(db_path) else None
- self.pub_reloc = self.create_publisher(PoseWithCovarianceStamped, '/perception/relocalization', 5)
- self.pub_init = self.create_publisher(PoseWithCovarianceStamped, '/initialpose', 1)
- self.pub_mk = self.create_publisher(MarkerArray, '/perception/reloc_markers', 5)
- self.create_subscription(PointCloud2, self.input_topic, self.cb, 5)
- if self.backend is None or self.db is None:
- self.get_logger().warn("model/DB 미설정 — relocalization 비활성(발행 안 함)")
-
- def cb(self, msg):
- if self.backend is None or self.db is None:
- return
- pts = np.array([[p[0], p[1], p[2], 0.0]
- for p in pc2.read_points(msg, ('x', 'y', 'z'), skip_nans=True)],
- dtype=np.float32) # intensity 무관(BEV=density). 필터클라우드는 xyz only
- if len(pts) == 0:
- return
- try:
- desc = self.backend.describe(pts)
- except Exception as e:
- if self._cpu_fallback or not _is_cuda_error(e):
- raise
- self.get_logger().warn("[bevplace2] 추론 중 CUDA 에러 → CPU 백엔드 재구성")
- self._cpu_fallback = True
- try: # 깨진 CUDA 컨텍스트 정리 (실패해도 무해)
- import torch
- del self.backend
- torch.cuda.empty_cache()
- except Exception:
- pass
- self.backend = None
- try: # ckpt 에서 device='cpu' 로 재생성 (.to 이동 금지)
- self.backend = BEVPlaceBackend(self._ckpt, device='cpu', **self._proj)
- desc = self.backend.describe(pts) # 해당 프레임 1회 재시도
- except Exception as e2:
- self.get_logger().error(f"[bevplace2] CPU 폴백 실패 — 프레임 스킵: {e2}")
- return
- idx, dist, topk = self.db.query(desc, topk=5)
- if dist > self.thresh:
- return # 신뢰도 낮으면 발행 안 함
- t, R = _pose_from_row(self.db.poses[idx])
- ps = PoseWithCovarianceStamped()
- ps.header.frame_id = 'map'
- ps.header.stamp = self.get_clock().now().to_msg()
- ps.pose.pose.position.x, ps.pose.pose.position.y, ps.pose.pose.position.z = map(float, t)
- qw = float(np.sqrt(max(0.0, 1.0 + R[0, 0] + R[1, 1] + R[2, 2])) / 2.0) or 1.0
- ps.pose.pose.orientation.w = qw
- self.pub_reloc.publish(ps)
- if not self.seed_once or not self._seeded: # 측위 씨딩 (1회만, 스트림 금지)
- self.pub_init.publish(ps)
- self._seeded = True
- self._publish_markers(t)
-
- def _publish_markers(self, t):
- ma = MarkerArray()
- m = Marker()
- m.header.frame_id = 'map'; m.ns = 'reloc'; m.id = 0
- m.type = Marker.ARROW; m.action = Marker.ADD
- m.pose.position.x, m.pose.position.y = float(t[0]), float(t[1])
- m.scale.x, m.scale.y, m.scale.z = 2.0, 0.3, 0.3
- m.color.g = 1.0; m.color.a = 1.0 # ★ 초록 현재 위치
- ma.markers.append(m)
- self.pub_mk.publish(ma)
-
-
-def main():
- rclpy.init(); rclpy.spin(PlaceRecognition()); rclpy.shutdown()
-
-
-if __name__ == '__main__':
- main()
diff --git a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bevplace_backend.py b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bevplace_backend.py
deleted file mode 100755
index 54815aa..0000000
--- a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bevplace_backend.py
+++ /dev/null
@@ -1,75 +0,0 @@
-"""BEVPlace2 descriptor 추출 backend (torch 격리, lazy import).
-scan (N,>=3) → BEV density image → REIN/group-conv 백본 → global descriptor (D,).
-ot_backend.py 의 구조 미러. 차이: range-image 대신 BEV 입력, REIN 회전-등변 백본.
-
-BEVPlace2 ckpt 는 DataParallel 로 저장될 수 있어 module. prefix 가 붙을 수 있다 →
-state_dict 로드 시 strip 한다(OTBackend 와 다른 점, NOTES 와 동일하게 방어적으로 처리)."""
-import os
-import sys
-
-import numpy as np
-
-# colcon 설치본에서 같은 패키지의 flat 모듈(bev_projection)이 top-level 로 안 잡혀
-# ModuleNotFoundError → 자기 디렉토리를 path 에 추가 (bevplace2_node.py 와 동일 패턴).
-sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
-from bev_projection import BEVProjector
-
-
-def _strip_module_prefix(state):
- """DataParallel 저장본의 'module.' prefix 제거 (없으면 그대로)."""
- if not any(k.startswith('module.') for k in state):
- return state
- return {k[len('module.'):] if k.startswith('module.') else k: v for k, v in state.items()}
-
-
-class BEVPlaceBackend:
- # 투영 기본값은 모델(BEVPlace2)이 학습된 KITTI Velodyne BEV 격자(±40m/0.4m=200x200)에
- # 맞춘다. 이 값은 "센서"가 아니라 "모델"이 기대하는 입력 레이아웃이라, 잘못 주면 점이
- # 격자의 엉뚱한 칸에 뭉쳐 descriptor 가 붕괴한다. 하드코딩 금지 — 호출부(node gp /
- # build_map_db CLI / bevplace.yaml SWRule)가 Rule 값으로 덮어쓴다.
- def __init__(self, ckpt_path, device='auto', x_min=-40.0, x_max=40.0,
- y_min=-40.0, y_max=40.0, resolution=0.4, max_density=20.0):
- import torch
- bev_root = os.environ.get('BEVPLACE_ROOT', '/opt/BEVPlace2')
- # vendored BEVPlace2 의 모델 정의(REIN.py)를 import 경로에 추가. 모듈 로드 시
- # transitively 끌어오는 보조 모듈(utils 등) 때문에 root + 하위도 path 에 넣는다.
- for _p in (bev_root, os.path.join(bev_root, 'models'), os.path.join(bev_root, 'tools')):
- if _p not in sys.path and os.path.isdir(_p):
- sys.path.insert(0, _p)
- from REIN import REIN # vendored BEVPlace2 백본: REM(rotation-equivariant)+NetVLAD, global_feat_dim=8192
-
- self.proj = BEVProjector(x_min=x_min, x_max=x_max, y_min=y_min, y_max=y_max,
- resolution=resolution, max_density=max_density)
- # device='auto' → 런타임 감지: cuda 가용 시 cuda:0, 아니면 cpu (08-gpu-cpu-runtime N4)
- if device == 'auto':
- device = 'cuda:0' if torch.cuda.is_available() else 'cpu'
- # CPU 런타임: vendored REIN.py forward 의 하드코딩 .cuda()(aff=torch.zeros(...).cuda())를
- # torch.Tensor.cuda no-op shim 으로 무력화 (벤더 모델 수정 없이). 이후 .type(x.type())가
- # cpu 로 정렬. GPU 없는 클라이언트 전제라 전역 안전. (08-gpu-cpu-runtime N4, objdet 동일 패턴)
- if device == 'cpu' and not getattr(torch.Tensor, '_cpu_cuda_noop', False):
- torch.Tensor.cuda = lambda self, *a, **k: self
- torch.Tensor._cpu_cuda_noop = True
- self.device = device
- self._t = torch
- model = REIN()
- state = torch.load(ckpt_path, map_location='cpu')
- state = state.get('state_dict', state)
- model.load_state_dict(_strip_module_prefix(state), strict=False)
- self._model = model.to(device).eval()
-
- def describe(self, points):
- """(N,>=3) → (D,) float32 L2-정규화 descriptor."""
- br = self.proj.project(points)
- bev = br.bev_image.copy() # (1,H,W) density BEV, [0,1]
- # REIN(REM=resnet34 백본)은 3채널 입력을 기대한다. 업스트림(kitti_dataset.py)도
- # grayscale BEV 를 repeat(3,0)로 3채널화한다 → 동일하게 1ch density 를 3ch 로 복제.
- bev = np.repeat(bev, 3, axis=0) # (3,H,W)
- with self._t.no_grad():
- inp = self._t.from_numpy(bev).unsqueeze(0).to(self.device) # (1,3,H,W)
- out = self._model(inp)
- # REIN.forward → (out1, local_feats, global_desc). global descriptor 만 사용.
- if isinstance(out, (tuple, list)):
- out = out[-1]
- d = out.cpu().numpy().reshape(-1).astype(np.float32)
- n = np.linalg.norm(d) + 1e-9
- return d / n
diff --git a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/build_map_db.py b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/build_map_db.py
deleted file mode 100755
index 022813f..0000000
--- a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/build_map_db.py
+++ /dev/null
@@ -1,47 +0,0 @@
-#!/usr/bin/env python3
-"""맵 descriptor DB 빌드 one-shot. 맵 시퀀스(velodyne+poses) → map.npz.
-deploy run.sh 가 DB 없을 때 수집 시퀀스로 1회 호출. flat import 라 이 파일이 있는 dir 에서 실행.
-BEV 투영 파라미터는 query 노드(bevplace2_node)와 반드시 같아야 매칭이 맞으므로, run.sh 가
-bevplace.yaml 의 값을 --resolution 등으로 넘긴다(미지정 시 모델 학습값 KITTI 기본). 하드코딩 금지.
-사용: python build_map_db.py [--resolution .. ...]"""
-import argparse
-from bevplace_backend import BEVPlaceBackend
-from descriptor_db import build_db
-
-
-def _is_cuda_error(e):
- s = str(e).lower()
- return 'cuda' in s or 'cudnn' in s or 'out of memory' in s
-
-
-if __name__ == '__main__':
- ap = argparse.ArgumentParser()
- ap.add_argument('ckpt'); ap.add_argument('scan_dir'); ap.add_argument('poses'); ap.add_argument('out')
- ap.add_argument('--x_min', type=float, default=-40.0); ap.add_argument('--x_max', type=float, default=40.0)
- ap.add_argument('--y_min', type=float, default=-40.0); ap.add_argument('--y_max', type=float, default=40.0)
- ap.add_argument('--resolution', type=float, default=0.4); ap.add_argument('--max_density', type=float, default=20.0)
- a = ap.parse_args()
- proj = dict(x_min=a.x_min, x_max=a.x_max, y_min=a.y_min, y_max=a.y_max,
- resolution=a.resolution, max_density=a.max_density)
- try:
- be = BEVPlaceBackend(a.ckpt, **proj)
- except Exception as e:
- if not _is_cuda_error(e):
- raise
- print(f"[bevplace2] GPU 백엔드 초기화 실패({e}) → CPU 폴백")
- be = BEVPlaceBackend(a.ckpt, device='cpu', **proj)
- try:
- shape = build_db(be, a.scan_dir, a.poses, a.out)
- except Exception as e:
- if be.device == 'cpu' or not _is_cuda_error(e):
- raise
- print("[bevplace2] 추론 중 CUDA 에러 → CPU 백엔드 재구성")
- try: # 깨진 CUDA 컨텍스트 정리 (실패해도 무해)
- del be
- import torch
- torch.cuda.empty_cache()
- except Exception:
- pass
- be = BEVPlaceBackend(a.ckpt, device='cpu', **proj) # ckpt 에서 재생성 (.to 이동 금지)
- shape = build_db(be, a.scan_dir, a.poses, a.out)
- print(f"[map_db] built {a.out} {shape} (res={a.resolution} range=[{a.x_min},{a.x_max}])")
diff --git a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/db_to_mapdb.py b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/db_to_mapdb.py
deleted file mode 100755
index 04087fb..0000000
--- a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/db_to_mapdb.py
+++ /dev/null
@@ -1,124 +0,0 @@
-#!/usr/bin/env python3
-"""선택된 3D slam_map(RTABMap .db) → 맵 descriptor DB(map.npz) 직접 빌드 (자동 빌드 경로).
-
-deploy run.sh 가 map.npz 없을 때(또는 선택 맵이 바뀌었을 때) 호출한다. deploy 이미지엔
-rtabmap 바이너리가 없으므로 .db 안의 두 BLOB 을 직접 디코드해 한 방에 만든다:
- - 최적화 pose: Admin.opt_ids + Admin.opt_poses (둘 다 zlib). loop-closure 보정 그래프 pose
- → bevplace2_node 측위 좌표가 nav2 가 쓰는 .db 기하맵과 동일 프레임이 됨.
- - 스캔: Data.scan (zlib, XYZ float32). RTABMap 저장 필터/등록 스캔 = /lidar_filtered
- 분포와 일치 → query 와 같은 분포라 descriptor 가 맞는다.
-node id 로 둘을 매칭 → BEVPlaceBackend.describe → (M,D) descriptors + (M,12) poses → map.npz.
-
-BEV 투영 파라미터는 query 노드와 반드시 같아야 매칭이 맞으므로 run.sh 가 bevplace.yaml 값을
---resolution 등으로 넘긴다(미지정 시 모델 학습값 KITTI 기본). 하드코딩 금지.
-사용: python db_to_mapdb.py [--resolution .. ...]"""
-import argparse
-import os
-import sqlite3
-import sys
-import zlib
-
-import numpy as np
-
-sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) # flat import (colcon entry 와 동일)
-from bevplace_backend import BEVPlaceBackend
-
-
-def _is_cuda_error(e):
- s = str(e).lower()
- return 'cuda' in s or 'cudnn' in s or 'out of memory' in s
-
-
-def load_opt_poses(db_path):
- """Admin.opt_ids(zlib int32) + opt_poses(zlib float32, id당 3x4=12) → {node_id: (12,) float32}."""
- c = sqlite3.connect(db_path)
- try:
- cur = c.cursor()
- cur.execute("SELECT opt_ids, opt_poses FROM Admin ORDER BY time_enter DESC LIMIT 1")
- row = cur.fetchone()
- finally:
- c.close()
- if not row or row[0] is None or row[1] is None:
- raise SystemExit("[db2map] 에러: Admin.opt_poses 없음 — 최적화 안 된 .db (SLAM 미완료?)")
- ids = np.frombuffer(zlib.decompress(row[0]), dtype=np.int32)
- poses = np.frombuffer(zlib.decompress(row[1]), dtype=np.float32).reshape(len(ids), 12)
- return {int(i): poses[k].copy() for k, i in enumerate(ids)}
-
-
-def load_scans(db_path):
- """Data.scan(zlib XYZ float32) → {node_id: (N,4) xyzi}. intensity 0 (BEV=density 써 무관)."""
- c = sqlite3.connect(db_path)
- out = {}
- try:
- cur = c.cursor()
- cur.execute("SELECT id, scan FROM Data WHERE scan IS NOT NULL AND length(scan)>0 ORDER BY id")
- for nid, blob in cur.fetchall():
- raw = zlib.decompress(blob)
- nf = len(raw) // 4
- if nf % 3 == 0:
- pts = np.frombuffer(raw, dtype=np.float32, count=nf).reshape(-1, 3)
- elif nf % 4 == 0:
- pts = np.frombuffer(raw, dtype=np.float32, count=nf).reshape(-1, 4)[:, :3]
- else:
- print(f"[db2map] 경고: node {nid} scan 포맷 불명({nf} floats) — 스킵")
- continue
- xyzi = np.zeros((pts.shape[0], 4), dtype=np.float32)
- xyzi[:, :3] = pts
- out[int(nid)] = xyzi
- finally:
- c.close()
- return out
-
-
-def main():
- ap = argparse.ArgumentParser()
- ap.add_argument('ckpt')
- ap.add_argument('db', help='선택된 RTABMap .db (3D slam_map)')
- ap.add_argument('out', help='출력 map.npz')
- ap.add_argument('--x_min', type=float, default=-40.0)
- ap.add_argument('--x_max', type=float, default=40.0)
- ap.add_argument('--y_min', type=float, default=-40.0)
- ap.add_argument('--y_max', type=float, default=40.0)
- ap.add_argument('--resolution', type=float, default=0.4)
- ap.add_argument('--max_density', type=float, default=20.0)
- a = ap.parse_args()
-
- poses = load_opt_poses(a.db)
- scans = load_scans(a.db)
- ids = sorted(set(poses) & set(scans))
- print(f"[db2map] opt_poses={len(poses)} scans={len(scans)} aligned={len(ids)}")
- if not ids:
- raise SystemExit("[db2map] 에러: pose∩scan 교집합 0 — 손상된 .db")
-
- proj = dict(x_min=a.x_min, x_max=a.x_max, y_min=a.y_min, y_max=a.y_max,
- resolution=a.resolution, max_density=a.max_density)
- try:
- be = BEVPlaceBackend(a.ckpt, **proj)
- except Exception as e:
- if not _is_cuda_error(e):
- raise
- print(f"[bevplace2] GPU 백엔드 초기화 실패({e}) → CPU 폴백")
- be = BEVPlaceBackend(a.ckpt, device='cpu', **proj)
- try:
- descs = np.stack([be.describe(scans[i]) for i in ids]).astype(np.float32)
- except Exception as e:
- if be.device == 'cpu' or not _is_cuda_error(e):
- raise
- print("[bevplace2] 추론 중 CUDA 에러 → CPU 백엔드 재구성")
- try: # 깨진 CUDA 컨텍스트 정리 (실패해도 무해)
- del be
- import torch
- torch.cuda.empty_cache()
- except Exception:
- pass
- be = BEVPlaceBackend(a.ckpt, device='cpu', **proj) # ckpt 에서 재생성 (.to 이동 금지)
- descs = np.stack([be.describe(scans[i]) for i in ids]).astype(np.float32)
- pose_arr = np.stack([poses[i] for i in ids]).astype(np.float32) # (M,12) 3x4 row-major
- os.makedirs(os.path.dirname(a.out), exist_ok=True)
- np.savez(a.out, descriptors=descs, poses=pose_arr)
- print(f"[db2map] built {a.out} desc={descs.shape} poses={pose_arr.shape} "
- f"(res={a.resolution} range=[{a.x_min},{a.x_max}])")
-
-
-if __name__ == '__main__':
- main()
diff --git a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/descriptor_db.py b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/descriptor_db.py
deleted file mode 100755
index 310e296..0000000
--- a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/descriptor_db.py
+++ /dev/null
@@ -1,34 +0,0 @@
-"""맵 descriptor DB 빌드/검색. 맵 시퀀스(velodyne+poses) → descriptor 행렬 + pose 배열.
-검색: 현재 descriptor 와 L2 최근접 → matched 프레임 idx → 그 프레임 pose. faiss-cpu.
-backend-agnostic (BEVPlaceBackend.describe / OTBackend.describe 동일 시그니처 공유)."""
-import os
-import numpy as np
-
-
-def build_db(backend, scan_dir, poses_path, out_npz):
- """scan_dir/*.bin 전부 describe → (M,D) descriptors + (M,12) poses → npz 저장."""
- bins = sorted(f for f in os.listdir(scan_dir) if f.endswith('.bin'))
- descs = []
- for b in bins:
- pts = np.fromfile(os.path.join(scan_dir, b), dtype=np.float32).reshape(-1, 4)
- descs.append(backend.describe(pts))
- descs = np.stack(descs).astype(np.float32)
- poses = np.loadtxt(poses_path).reshape(-1, 12).astype(np.float32)
- os.makedirs(os.path.dirname(out_npz), exist_ok=True)
- np.savez(out_npz, descriptors=descs, poses=poses)
- return descs.shape
-
-
-class DescriptorDB:
- def __init__(self, npz_path):
- import faiss
- z = np.load(npz_path)
- self.descs = z['descriptors'].astype(np.float32)
- self.poses = z['poses']
- self.index = faiss.IndexFlatL2(self.descs.shape[1])
- self.index.add(self.descs)
-
- def query(self, descriptor, topk=5):
- """descriptor (D,) → (best_idx, best_dist, topk_idxs). pose 는 self.poses[idx]."""
- d, i = self.index.search(descriptor.reshape(1, -1), topk)
- return int(i[0, 0]), float(d[0, 0]), i[0].tolist()
diff --git a/src/perception_bevplace2_pkg/resource/perception_bevplace2_pkg b/src/perception_bevplace2_pkg/resource/perception_bevplace2_pkg
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_bevplace2_pkg/setup.cfg b/src/perception_bevplace2_pkg/setup.cfg
deleted file mode 100755
index 6dd6e14..0000000
--- a/src/perception_bevplace2_pkg/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/perception_bevplace2_pkg
-[install]
-install_scripts=$base/lib/perception_bevplace2_pkg
diff --git a/src/perception_bevplace2_pkg/setup.py b/src/perception_bevplace2_pkg/setup.py
deleted file mode 100755
index 3d75aa0..0000000
--- a/src/perception_bevplace2_pkg/setup.py
+++ /dev/null
@@ -1,35 +0,0 @@
-
-import os
-from glob import glob
-from setuptools import find_packages, setup
-
-package_name = 'perception_bevplace2_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 place recognition (BEVPlace2) perception module',
- license='TODO: License declaration',
- extras_require={
- 'test': [
- 'pytest',
- ],
- },
- entry_points={
- 'console_scripts': [
- 'bevplace2 = perception_bevplace2_pkg.bevplace2_node:main'
- ],
- },
-)
diff --git a/src/perception_bevplace2_pkg/test/test_bev_projection.py b/src/perception_bevplace2_pkg/test/test_bev_projection.py
deleted file mode 100755
index 3a0b601..0000000
--- a/src/perception_bevplace2_pkg/test/test_bev_projection.py
+++ /dev/null
@@ -1,33 +0,0 @@
-import numpy as np
-from perception_bevplace2_pkg.bev_projection import BEVProjector
-
-
-def test_bev_shape_and_range():
- # ±40m / 0.4m → 200x200 격자. 범위 밖 점은 drop, 안쪽 점만 valid.
- proj = BEVProjector(x_min=-40.0, x_max=40.0, y_min=-40.0, y_max=40.0,
- resolution=0.4, max_density=20.0)
- assert proj.W == 200 and proj.H == 200
-
- pts = np.array([
- [0.0, 0.0, 0.0, 0.0], # 중앙 (valid)
- [10.0, -10.0, 1.0, 0.0], # valid
- [100.0, 0.0, 0.0, 0.0], # x 범위 밖 (drop)
- [0.0, -100.0, 0.0, 0.0], # y 범위 밖 (drop)
- ], dtype=np.float32)
- out = proj.project(pts)
- assert out.bev_image.shape == (1, 200, 200)
- assert out.valid.tolist() == [True, True, False, False]
- # valid 점 2개가 BEV 에 density 로 찍혔는지
- assert out.bev_image.sum() > 0
- assert out.grid_x.shape[0] == int(out.valid.sum())
-
-
-def test_bev_density_accumulates_and_normalizes():
- # 같은 셀에 여러 점 → density 누적, max_density 로 [0,1] 정규화 (1.0 초과 없음).
- proj = BEVProjector(x_min=-40.0, x_max=40.0, y_min=-40.0, y_max=40.0,
- resolution=0.4, max_density=4.0)
- pts = np.tile(np.array([[1.0, 1.0, 0.0, 0.0]], dtype=np.float32), (10, 1))
- out = proj.project(pts)
- assert out.bev_image.max() <= 1.0 + 1e-6
- # 10점이 한 셀에 → max_density=4 클립 → 정확히 1.0
- assert abs(out.bev_image.max() - 1.0) < 1e-6
diff --git a/src/perception_bevplace2_pkg/test/test_copyright.py b/src/perception_bevplace2_pkg/test/test_copyright.py
deleted file mode 100755
index 97a3919..0000000
--- a/src/perception_bevplace2_pkg/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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'
diff --git a/src/perception_bevplace2_pkg/test/test_flake8.py b/src/perception_bevplace2_pkg/test/test_flake8.py
deleted file mode 100755
index 27ee107..0000000
--- a/src/perception_bevplace2_pkg/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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)
diff --git a/src/perception_bevplace2_pkg/test/test_pep257.py b/src/perception_bevplace2_pkg/test/test_pep257.py
deleted file mode 100755
index b234a38..0000000
--- a/src/perception_bevplace2_pkg/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# 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'
diff --git a/src/perception_centerpoint_pkg/cfgs/centerpoint_custom.yaml b/src/perception_centerpoint_pkg/cfgs/centerpoint_custom.yaml
deleted file mode 100755
index 4bf5a74..0000000
--- a/src/perception_centerpoint_pkg/cfgs/centerpoint_custom.yaml
+++ /dev/null
@@ -1,98 +0,0 @@
-CLASS_NAMES: ['Car', 'Pedestrian', 'Cyclist']
-
-DATA_CONFIG:
- _BASE_CONFIG_: cfgs/dataset_configs/custom_dataset.yaml
-
-MODEL:
- NAME: CenterPoint
-
- # CenterPoint(Pillar variant): PointPillars 와 동일한 pillar + 2D conv 백본을 쓰고
- # 헤드만 anchor → center heatmap 으로 교체 (sparse conv 미사용 → sm_52 GPU 가용).
- VFE:
- NAME: PillarVFE
- WITH_DISTANCE: False
- USE_ABSLOTE_XYZ: True
- USE_NORM: True
- NUM_FILTERS: [64]
-
- MAP_TO_BEV:
- NAME: PointPillarScatter
- NUM_BEV_FEATURES: 64
-
- BACKBONE_2D:
- NAME: BaseBEVBackbone
- LAYER_NUMS: [3, 5, 5]
- LAYER_STRIDES: [2, 2, 2]
- NUM_FILTERS: [64, 128, 256]
- UPSAMPLE_STRIDES: [0.5, 1, 2]
- NUM_UPSAMPLE_FILTERS: [128, 128, 128]
-
- DENSE_HEAD:
- NAME: CenterHead
- CLASS_AGNOSTIC: False
-
- CLASS_NAMES_EACH_HEAD: [
- ['Car', 'Pedestrian', 'Cyclist']
- ]
-
- SHARED_CONV_CHANNEL: 64
- USE_BIAS_BEFORE_NORM: True
- NUM_HM_CONV: 2
- SEPARATE_HEAD_CFG:
- HEAD_ORDER: ['center', 'center_z', 'dim', 'rot']
- HEAD_DICT: {
- 'center': {'out_channels': 2, 'num_conv': 2},
- 'center_z': {'out_channels': 1, 'num_conv': 2},
- 'dim': {'out_channels': 3, 'num_conv': 2},
- 'rot': {'out_channels': 2, 'num_conv': 2},
- }
-
- TARGET_ASSIGNER_CONFIG:
- FEATURE_MAP_STRIDE: 2
- NUM_MAX_OBJS: 500
- GAUSSIAN_OVERLAP: 0.1
- MIN_RADIUS: 2
-
- LOSS_CONFIG:
- LOSS_WEIGHTS: {
- 'cls_weight': 1.0,
- 'loc_weight': 2.0,
- 'code_weights': [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
- }
-
- POST_PROCESSING:
- SCORE_THRESH: 0.1
- POST_CENTER_LIMIT_RANGE: [-61.2, -61.2, -10.0, 61.2, 61.2, 10.0]
- MAX_OBJ_PER_SAMPLE: 500
- NMS_CONFIG:
- NMS_TYPE: nms_gpu
- NMS_THRESH: 0.7
- NMS_PRE_MAXSIZE: 4096
- NMS_POST_MAXSIZE: 500
-
- POST_PROCESSING:
- RECALL_THRESH_LIST: [0.3, 0.5, 0.7]
- SCORE_THRESH: 0.1
- OUTPUT_RAW_SCORE: False
- EVAL_METRIC: kitti
-
-OPTIMIZATION:
- BATCH_SIZE_PER_GPU: 2
- NUM_EPOCHS: 80
-
- OPTIMIZER: adam_onecycle
- LR: 0.003
- WEIGHT_DECAY: 0.01
- MOMENTUM: 0.9
-
- MOMS: [0.95, 0.85]
- PCT_START: 0.4
- DIV_FACTOR: 10
- DECAY_STEP_LIST: [35, 45]
- LR_DECAY: 0.1
- LR_CLIP: 0.0000001
-
- LR_WARMUP: False
- WARMUP_EPOCH: 1
-
- GRAD_NORM_CLIP: 10
diff --git a/src/perception_centerpoint_pkg/cfgs/custom_dataset.yaml b/src/perception_centerpoint_pkg/cfgs/custom_dataset.yaml
deleted file mode 100755
index 06ef0a7..0000000
--- a/src/perception_centerpoint_pkg/cfgs/custom_dataset.yaml
+++ /dev/null
@@ -1,58 +0,0 @@
-DATASET: 'CustomDataset'
-DATA_PATH: '/perception_data/data'
-# 우리 FR09 라이다(360°) → 대칭 range. z[-2,4] 1칸 pillar. x/y 140.8/0.16=880 (16배수)
-POINT_CLOUD_RANGE: [-51.2, -51.2, -2, 51.2, 51.2, 4]
-MAP_CLASS_TO_KITTI: {
- 'Car': 'Car',
- 'Pedestrian': 'Pedestrian',
- 'Cyclist': 'Cyclist',
-}
-DATA_SPLIT: {
- 'train': train,
- 'test': val
-}
-INFO_PATH: {
- 'train': [custom_infos_train.pkl],
- 'test': [custom_infos_val.pkl],
-}
-POINT_FEATURE_ENCODING: {
- encoding_type: absolute_coordinates_encoding,
- used_feature_list: ['x', 'y', 'z', 'intensity'],
- src_feature_list: ['x', 'y', 'z', 'intensity'],
-}
-DATA_AUGMENTOR:
- DISABLE_AUG_LIST: ['gt_sampling'] # 1차 smoke 안정화용. 검증 후 풀어 성능↑
- AUG_CONFIG_LIST:
- - NAME: gt_sampling
- USE_ROAD_PLANE: False
- DB_INFO_PATH:
- - custom_dbinfos_train.pkl
- PREPARE: {
- filter_by_min_points: ['Car:5', 'Pedestrian:5', 'Cyclist:5'],
- }
- SAMPLE_GROUPS: ['Car:15', 'Pedestrian:10', 'Cyclist:10']
- NUM_POINT_FEATURES: 4
- DATABASE_WITH_FAKELIDAR: False
- REMOVE_EXTRA_WIDTH: [0.0, 0.0, 0.0]
- LIMIT_WHOLE_SCENE: True
- - NAME: random_world_flip
- ALONG_AXIS_LIST: ['x', 'y']
- - NAME: random_world_rotation
- WORLD_ROT_ANGLE: [-0.78539816, 0.78539816]
- - NAME: random_world_scaling
- WORLD_SCALE_RANGE: [0.95, 1.05]
-DATA_PROCESSOR:
- - NAME: mask_points_and_boxes_outside_range
- REMOVE_OUTSIDE_BOXES: True
- - NAME: shuffle_points
- SHUFFLE_ENABLED: {
- 'train': True,
- 'test': False
- }
- - NAME: transform_points_to_voxels
- VOXEL_SIZE: [0.16, 0.16, 6]
- MAX_POINTS_PER_VOXEL: 32
- MAX_NUMBER_OF_VOXELS: {
- 'train': 16000,
- 'test': 40000
- }
diff --git a/src/perception_centerpoint_pkg/config/centerpoint.yaml b/src/perception_centerpoint_pkg/config/centerpoint.yaml
deleted file mode 100755
index 94da199..0000000
--- a/src/perception_centerpoint_pkg/config/centerpoint.yaml
+++ /dev/null
@@ -1,21 +0,0 @@
-
-
-centerpoint_centerpoint:
- ros__parameters:
- input_topic: "/raw/lidar"
- detection_topic: "/perception/detections"
- marker_topic: "/perception/detection_markers"
- # 모델 경로는 여기서 지정하거나, 비우면 환경변수
- # (CENTERPOINT_MODEL_CFG / CENTERPOINT_MODEL_CKPT)에서 읽는다.
- model_cfg_path: ""
- model_ckpt_path: ""
- score_threshold: 0.2
- device: "auto" # auto = 런타임 감지(GPU 있으면 cuda, 없으면 cpu). Phase3 dual-runtime
- # CARLA 라이다 보정 (학습분포에 맞춤). 측정값 기반:
- # 지면 z -1.06(CARLA) vs -1.73(KITTI), ~12k 점(KITTI 대비 희박).
- z_offset: 1.0 # 입력 z 보정 (스윕상 max score 최적 ~+1.0)
- max_range: 25.0 # 이 반경(m) 밖 박스 제거 (근~중거리 신뢰)
- # 기하 검증 — 빈 공간/바닥 헛검출 제거
- min_points_in_box: 5 # 박스 안 점 5개 미만 → 버림 (검정영역 헛박스)
- min_box_height: 0.5 # 박스 안 점 높이 0.5m 미만 → 버림 (평평한 도로 오검출)
- use_sim_time: true
diff --git a/src/perception_centerpoint_pkg/launch/centerpoint.launch.py b/src/perception_centerpoint_pkg/launch/centerpoint.launch.py
deleted file mode 100755
index 3c3553b..0000000
--- a/src/perception_centerpoint_pkg/launch/centerpoint.launch.py
+++ /dev/null
@@ -1,127 +0,0 @@
-
-import os
-import shutil
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.actions import GroupAction, OpaqueFunction
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
-
-
-# 선언 기본값(pretrained 베이스라인) — 모델 자동 resolve 에서 "사용자가 명시했는지"
-# 판별 기준으로도 쓰므로 상수로 둔다.
-# 기본값: OpenPCDet nuScenes CenterPoint(Dynamic-Pillar, spconv 미사용 → sm_52 가용)
-# config(이미지 내장) + 이미지에 baked 된 공식 pretrained ckpt.
-# KITTI 엔 CenterPoint pretrained 가 없어 nuScenes(cbgs_dyn_pp_centerpoint) 를 베이스라인으로
-# 사용한다 — 도메인 갭이 있으므로 정식 사용은 CARLA finetuned(/perception_data) 가 우선.
-DEFAULT_MODEL_CFG = '/opt/OpenPCDet/tools/cfgs/nuscenes_models/cbgs_dyn_pp_centerpoint.yaml'
-DEFAULT_MODEL_CKPT = '/opt/centerpoint_pretrained/cbgs_dyn_pp_centerpoint.pth'
-
-# 가중치 공유 단일 위치(docker/local): /perception_models/centerpoint/{finetuned,pretrained}
-# (호스트 .caches/config/nav2_config/perception/models/centerpoint/*, SWRule config_mounts).
-FINETUNED_CKPT = '/perception_models/centerpoint/finetuned/centerpoint_finetuned.pth'
-SHARED_PRE_CKPT = '/perception_models/centerpoint/pretrained/cbgs_dyn_pp_centerpoint.pth' # 공유 pretrained (Phase 2 채움)
-# finetuned 용 custom cfg 는 _BASE_CONFIG_ 상대참조(cfgs/...)가 풀리도록
-# 단독 deploy run.sh 와 동일하게 OpenPCDet tools/cfgs 아래에 복사해 쓴다.
-OPENPCDET_CFGS_DIR = '/opt/OpenPCDet/tools/cfgs'
-RESOLVED_MODEL_CFG = os.path.join(OPENPCDET_CFGS_DIR, 'custom_models', 'centerpoint_custom.yaml')
-
-
-ARGUMENTS = [
- DeclareLaunchArgument('use_sim_time', default_value='true'),
- # 입력 토픽(/raw/lidar)은 sensor_bridge 가 robot 무관하게 정규화하므로
- # robot 별 config 분기는 두지 않는다. robot arg 는 인터페이스 호환용.
- DeclareLaunchArgument('robot', default_value='mkmini', choices=['mkmini', 'fr09']),
- # 모델 소스 스위치 — UI 에서 pretrained 고르면 finetuned 있어도 건너뜀(기본 finetuned).
- # 'pretrained' 외의 값은 전부 finetuned 우선으로 본다(단독 deploy run.sh 와 동일 의미론).
- DeclareLaunchArgument('model_source', default_value='finetuned'),
- # 모델 경로 — Rule(launcher 의 ROS_PARAM)이 덮어쓸 수 있게 launch arg 로 노출.
- DeclareLaunchArgument('model_cfg_path', default_value=DEFAULT_MODEL_CFG),
- DeclareLaunchArgument('model_ckpt_path', default_value=DEFAULT_MODEL_CKPT),
-]
-
-
-def _resolve_model(context, *args, **kwargs):
- """모델 cfg/ckpt 자동 resolve (단독 deploy run.sh 의 finetuned 우선 로직 이식).
-
- - 사용자가 model_cfg_path / model_ckpt_path 를 명시적으로 줬으면(선언 기본값과 다르고
- 빈값이 아니면) 그대로 쓴다 — run.sh 가 MODEL_ARGS 로 명시 전달하는 기존 동작 무회귀.
- - 인자가 기본값/빈값일 때만 model_source(기본 finetuned) 와 finetuned 파일 존재 여부로
- 자동 결정. finetuned 채택 시 pkg 에 편입된 cfgs/ 를 OpenPCDet 경로에 복사한다.
- """
- use_sim_time = LaunchConfiguration('use_sim_time')
-
- model_cfg_path = LaunchConfiguration('model_cfg_path').perform(context)
- model_ckpt_path = LaunchConfiguration('model_ckpt_path').perform(context)
- model_source = LaunchConfiguration('model_source').perform(context) or 'finetuned'
-
- pkg_centerpoint = get_package_share_directory('perception_centerpoint_pkg')
-
- # 명시 전달 여부 — 둘 중 하나라도 기본값과 다른 비어있지 않은 값이면 명시로 본다.
- explicit = ((model_cfg_path and model_cfg_path != DEFAULT_MODEL_CFG)
- or (model_ckpt_path and model_ckpt_path != DEFAULT_MODEL_CKPT))
-
- if not explicit:
- if model_source != 'pretrained' and os.path.isfile(FINETUNED_CKPT):
- # custom cfg 복사 — run.sh 의 cp 순서와 동일 (dataset cfg → model cfg)
- os.makedirs(os.path.join(OPENPCDET_CFGS_DIR, 'dataset_configs'), exist_ok=True)
- shutil.copy(
- os.path.join(pkg_centerpoint, 'cfgs', 'custom_dataset.yaml'),
- os.path.join(OPENPCDET_CFGS_DIR, 'dataset_configs', 'custom_dataset.yaml'))
- os.makedirs(os.path.join(OPENPCDET_CFGS_DIR, 'custom_models'), exist_ok=True)
- shutil.copy(
- os.path.join(pkg_centerpoint, 'cfgs', 'centerpoint_custom.yaml'),
- RESOLVED_MODEL_CFG)
- model_cfg_path = RESOLVED_MODEL_CFG
- model_ckpt_path = FINETUNED_CKPT
- print(f'[centerpoint] 재학습 모델 사용: {FINETUNED_CKPT}')
- elif os.path.isfile(SHARED_PRE_CKPT):
- # 공유 pretrained(nuScenes base) — baked(/opt) 보다 우선. cfg 는 base 와 동일.
- model_cfg_path = DEFAULT_MODEL_CFG
- model_ckpt_path = SHARED_PRE_CKPT
- print(f'[centerpoint] 공유 pretrained 사용: {SHARED_PRE_CKPT}')
- else:
- # pretrained 폴백 — 빈값으로 들어온 경우도 기본값으로 확정
- model_cfg_path = DEFAULT_MODEL_CFG
- model_ckpt_path = DEFAULT_MODEL_CKPT
- print('[centerpoint] 재학습 모델 없음 → 기본 pretrained(baked) 사용')
-
- remappings = [('/tf', 'tf'),
- ('/tf_static', 'tf_static')]
-
- params_file = PathJoinSubstitution([pkg_centerpoint, 'config', 'centerpoint.yaml'])
-
- # nav2_common.RewrittenYaml 대신 yaml + dict override 로 단순화
- # (future-centerpoint 이미지는 ros-base 만 깔려 nav2_common 이 없음).
- load_nodes = GroupAction(
- actions=[
- Node(
- package='perception_centerpoint_pkg',
- executable='centerpoint',
- name='centerpoint_centerpoint',
- output='screen',
- parameters=[
- params_file,
- {
- 'use_sim_time': use_sim_time,
- 'model_cfg_path': model_cfg_path,
- 'model_ckpt_path': model_ckpt_path,
- },
- ],
- remappings=remappings
- )
- ],
- )
-
- return [load_nodes]
-
-
-def generate_launch_description():
- ld = LaunchDescription(ARGUMENTS)
- # 모델 경로는 인자 평가가 필요해 OpaqueFunction 안에서 resolve 후 노드를 만든다
- ld.add_action(OpaqueFunction(function=_resolve_model))
-
- return ld
diff --git a/src/perception_centerpoint_pkg/launch/perception.launch.py b/src/perception_centerpoint_pkg/launch/perception.launch.py
deleted file mode 100755
index 293f46e..0000000
--- a/src/perception_centerpoint_pkg/launch/perception.launch.py
+++ /dev/null
@@ -1,32 +0,0 @@
-"""perception_manager 공통 진입점 (통합 perception 모듈).
-
-LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로
-`ros2 launch perception_centerpoint_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..`
-를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(centerpoint.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_centerpoint_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', 'centerpoint.launch.py')),
- launch_arguments={
- 'robot': LaunchConfiguration('robot'),
- 'use_sim_time': LaunchConfiguration('use_sim_time'),
- 'model_source': LaunchConfiguration('model_source'),
- }.items(),
- ),
- ])
diff --git a/src/perception_centerpoint_pkg/package.xml b/src/perception_centerpoint_pkg/package.xml
deleted file mode 100755
index 0800eae..0000000
--- a/src/perception_centerpoint_pkg/package.xml
+++ /dev/null
@@ -1,26 +0,0 @@
-
-
-
- perception_centerpoint_pkg
- 0.0.0
- LiDAR 3D object detection (CenterPoint / OpenPCDet) perception module
- dgist
- TODO: License declaration
-
- rclpy
- sensor_msgs
- sensor_msgs_py
- vision_msgs
- visualization_msgs
- geometry_msgs
- std_msgs
-
- ament_copyright
- ament_flake8
- ament_pep257
- python3-pytest
-
-
- ament_python
-
-
diff --git a/src/perception_centerpoint_pkg/perception_centerpoint_pkg/__init__.py b/src/perception_centerpoint_pkg/perception_centerpoint_pkg/__init__.py
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_centerpoint_pkg/perception_centerpoint_pkg/centerpoint_backend.py b/src/perception_centerpoint_pkg/perception_centerpoint_pkg/centerpoint_backend.py
deleted file mode 100755
index 742cacd..0000000
--- a/src/perception_centerpoint_pkg/perception_centerpoint_pkg/centerpoint_backend.py
+++ /dev/null
@@ -1,219 +0,0 @@
-#!/usr/bin/env python3
-"""OpenPCDet CenterPoint(Pillar) 추론 래퍼.
-
-ROS 의존이 없는 순수 추론 모듈. OpenPCDet(pcdet) / torch 는 이 클래스를
-인스턴스화할 때만 import 한다 → OpenPCDet 미설치 환경에서도 패키지 build/import 는 통과.
-런타임(도커 이미지)에 OpenPCDet 가 설치돼 있어야 실제 추론이 동작한다.
-
-CenterPoint(Pillar) 는 PointPillars 와 동일한 pillar + 2D conv 백본을 쓰고
-헤드만 anchor → center heatmap 으로 교체한 상위호환이다(sparse conv 미사용 →
-sm_52 GPU 가용). OpenPCDet 의 cfg/checkpoint 인터페이스가 동일하므로 본 백엔드는
-PointPillars 백엔드와 동일한 인터페이스(infer → boxes/scores/labels)를 유지한다.
-다른 검출 모델로 교체 시 이 파일만 갈아끼우면 된다(노드 인터페이스 불변).
-"""
-
-import os
-
-import numpy as np
-
-
-class CenterPointBackend:
- """OpenPCDet config + checkpoint 를 로드해 단일 프레임 포인트클라우드를 추론한다.
-
- infer(points) -> (boxes, scores, labels)
- points : (N, 4) float32 — x, y, z, intensity
- boxes : (M, 7) float32 — x, y, z, dx, dy, dz, heading(yaw, rad)
- scores : (M,) float32
- labels : (M,) int — 1-based class index (class_names[label-1])
- """
-
- def __init__(self, cfg_path, ckpt_path, score_threshold=0.3, device='auto',
- z_offset=0.0, min_points_in_box=0, min_box_height=0.0,
- logger=None):
- # OpenPCDet / torch 는 여기서만 import (미설치 시 패키지 import 자체는 안 깨지게)
- import torch
- from pcdet.config import cfg, cfg_from_yaml_file
- from pcdet.models import build_network, load_data_to_gpu
- from pcdet.datasets import DatasetTemplate
- from pcdet.utils import common_utils
-
- self._torch = torch
- self._load_data_to_gpu = load_data_to_gpu
- self.score_threshold = float(score_threshold)
- # device='auto' → 런타임 감지: cuda 가용 시 cuda:0, 아니면 cpu (08-gpu-cpu-runtime N4)
- if device == 'auto':
- device = 'cuda:0' if torch.cuda.is_available() else 'cpu'
- self.device = device
- self._cpu = (device == 'cpu')
- # CPU 런타임이면 OpenPCDet 의 CUDA 하드코딩(NMS + anchor 생성 .cuda())을 CPU 로 우회
- # (Phase3 dual-runtime). spconv 미사용 모델이라 이 두 패치로 CPU 완결. build_network 전에 적용.
- if self._cpu:
- self._patch_cpu()
- # 보정: 입력 z 에 더하는 오프셋. 라이다 장착 높이/지면 z 가 학습분포와
- # 어긋날 때 맞춘다 (CARLA 지면 ~ -1.06, KITTI ~ -1.73). 0 이면 비활성.
- self.z_offset = float(z_offset)
- # 기하 검증: 박스 안에 실제 점이 충분/입체적으로 있어야 진짜 객체로 인정.
- # min_points_in_box : 박스 내부 최소 점 개수 (빈 공간 헛검출 제거)
- # min_box_height : 박스 내부 점들의 z 스프레드 최소값(m) (평평한 바닥 오검출 제거)
- self.min_points_in_box = int(min_points_in_box)
- self.min_box_height = float(min_box_height)
-
- # OpenPCDet cfg 의 _BASE_CONFIG_ 는 tools/ 기준 상대경로라, 그 디렉토리에서 로드해야
- # dataset base config 가 해석된다. (cfg_path 자체는 절대경로 허용)
- opcdet_tools = os.path.join(
- os.environ.get('OPENPCDET_ROOT', '/opt/OpenPCDet'), 'tools')
- prev_cwd = os.getcwd()
- try:
- if os.path.isdir(opcdet_tools):
- os.chdir(opcdet_tools)
- cfg_from_yaml_file(cfg_path, cfg)
- finally:
- os.chdir(prev_cwd)
- self.class_names = list(cfg.CLASS_NAMES)
-
- log = logger or common_utils.create_logger()
-
- # 모델의 data_processor / point_feature_encoder 만 빌리기 위한 최소 dataset
- self._dataset = DatasetTemplate(
- dataset_cfg=cfg.DATA_CONFIG,
- class_names=self.class_names,
- training=False,
- root_path=None,
- logger=log,
- )
-
- # 모델 cfg 가 기대하는 입력 point feature 수 (custom=4 [x,y,z,intensity],
- # nuScenes pretrained=5 [+timestamp]). infer 입력을 여기에 맞춰 패딩/절단(하드코딩 없음).
- try:
- self._num_src_feat = len(cfg.DATA_CONFIG.POINT_FEATURE_ENCODING.src_feature_list)
- except Exception:
- self._num_src_feat = 4
-
- self._model = build_network(
- model_cfg=cfg.MODEL,
- num_class=len(self.class_names),
- dataset=self._dataset,
- )
- self._model.load_params_from_file(
- filename=ckpt_path, logger=log, to_cpu=(device == 'cpu'))
- self._model.to(device)
- self._model.eval()
-
- def infer(self, points):
- torch = self._torch
- points = np.ascontiguousarray(points, dtype=np.float32)
-
- if self.z_offset:
- points = points.copy()
- points[:, 2] += self.z_offset
-
- # cfg 가 기대하는 feature 수로 정합 (nuScenes pretrained 는 5번째 timestamp 채널 필요)
- if points.shape[1] < self._num_src_feat:
- pad = np.zeros((points.shape[0], self._num_src_feat - points.shape[1]),
- dtype=np.float32)
- points = np.hstack([points, pad])
- elif points.shape[1] > self._num_src_feat:
- points = points[:, :self._num_src_feat]
-
- data_dict = self._dataset.prepare_data(
- data_dict={'points': points, 'frame_id': 0})
- data_dict = self._dataset.collate_batch([data_dict])
- if self._cpu:
- self._load_data_to_cpu(data_dict)
- else:
- self._load_data_to_gpu(data_dict)
-
- with torch.no_grad():
- pred_dicts, _ = self._model.forward(data_dict)
-
- boxes = pred_dicts[0]['pred_boxes'].cpu().numpy()
- scores = pred_dicts[0]['pred_scores'].cpu().numpy()
- labels = pred_dicts[0]['pred_labels'].cpu().numpy()
-
- keep = scores >= self.score_threshold
- boxes, scores, labels = boxes[keep], scores[keep], labels[keep]
-
- # 기하 검증 — 박스 안 실제 점 개수/높이로 헛검출 제거 (points/boxes 동일 좌표계)
- if (self.min_points_in_box > 0 or self.min_box_height > 0.0) and len(boxes):
- valid = np.array([self._box_is_valid(points, b) for b in boxes], dtype=bool)
- boxes, scores, labels = boxes[valid], scores[valid], labels[valid]
-
- # z_offset 만큼 올려서 추론했으므로 출력 박스 z 는 원래 라이다 좌표계로 되돌림
- if self.z_offset and len(boxes):
- boxes = boxes.copy()
- boxes[:, 2] -= self.z_offset
-
- return boxes, scores, labels
-
- def _box_is_valid(self, points, box):
- """박스 내부 점 개수와 z 스프레드로 진짜 객체 여부 판정 (헛검출 컷)."""
- cx, cy, cz, dx, dy, dz, yaw = box[:7]
- rel = points[:, :3] - np.array([cx, cy, cz], dtype=np.float32)
- c, s = np.cos(-yaw), np.sin(-yaw)
- xr = rel[:, 0] * c - rel[:, 1] * s
- yr = rel[:, 0] * s + rel[:, 1] * c
- inside = (np.abs(xr) <= dx / 2) & (np.abs(yr) <= dy / 2) & \
- (np.abs(rel[:, 2]) <= dz / 2)
- n = int(inside.sum())
- if n < self.min_points_in_box:
- return False
- if self.min_box_height > 0.0:
- zin = points[inside, 2]
- if n == 0 or float(zin.max() - zin.min()) < self.min_box_height:
- return False
- return True
-
- def _load_data_to_cpu(self, batch_dict):
- """load_data_to_gpu 의 CPU 판: numpy 배열을 .cuda() 대신 cpu float tensor 로.
- 라이다 전용 파이프라인이라 camera/image 키는 없음 → voxel/point 계열만 변환."""
- torch = self._torch
- skip = ('frame_id', 'metadata', 'calib', 'image_paths', 'ori_shape',
- 'img_process_infos')
- for key, val in batch_dict.items():
- if not isinstance(val, np.ndarray) or key in skip:
- continue
- batch_dict[key] = torch.from_numpy(val).float()
-
- def _patch_cpu(self):
- """CPU 런타임용 OpenPCDet CUDA 하드코딩 2종 우회 (device=='cpu' 일 때만, build 전 1회).
-
- (1) anchor 생성·NMS 모듈이 텐서에 박은 .cuda() → torch.Tensor.cuda 를 no-op(self 반환)로
- shim. forward 경로엔 torch.cuda.*Tensor 직접 alloc 이 없어 이 shim 으로 완결
- (CenterPoint 는 anchor 미사용이라 shim 은 무해, NMS 패치만 실효).
- (2) iou3d_nms_utils.nms_gpu(CUDA-only)를 동일결과 CPU greedy NMS 로 교체. 회전 BEV IoU 는
- OpenPCDet 가 같은 확장(iou3d_nms_cuda)에 함께 컴파일해 둔 boxes_bev_iou_cpu(C++ CPU op)를
- 그대로 호출 → GPU 판과 IoU 계산 동일(결과 동등)."""
- torch = self._torch
-
- # (1) .cuda() no-op shim — GPU 없는 클라이언트 전제(애초에 cuda 못 부름)라 전역 안전
- if not getattr(torch.Tensor, '_cpu_cuda_noop', False):
- torch.Tensor.cuda = lambda self, *a, **k: self
- torch.Tensor._cpu_cuda_noop = True
-
- # (2) CUDA NMS → CPU NMS monkeypatch (모듈 객체에 1회, getattr 로 호출 시점 해석)
- from pcdet.ops.iou3d_nms import iou3d_nms_utils as u
- if getattr(u, '_cpu_nms_patched', False):
- return
-
- def nms_cpu(boxes, scores, thresh, pre_maxsize=None, **kwargs):
- assert boxes.shape[1] == 7
- order = scores.sort(0, descending=True)[1]
- if pre_maxsize is not None:
- order = order[:pre_maxsize]
- boxes = boxes[order].contiguous()
- if boxes.shape[0] == 0:
- return order[:0].contiguous(), None
- iou = u.boxes_bev_iou_cpu(boxes[:, 0:7], boxes[:, 0:7]) # (N,N) CPU
- keep = []
- suppressed = torch.zeros(boxes.shape[0], dtype=torch.bool)
- for i in range(boxes.shape[0]):
- if suppressed[i]:
- continue
- keep.append(i)
- suppressed |= iou[i] > thresh # 자신 포함 — 이미 append 후라 무해
- keep = torch.as_tensor(keep, dtype=torch.long)
- return order[keep].contiguous(), None
-
- u.nms_gpu = nms_cpu
- u.nms_normal_gpu = nms_cpu # objdet 기본 NMS_TYPE=nms_gpu, normal 경로도 crash 방지
- u._cpu_nms_patched = True
diff --git a/src/perception_centerpoint_pkg/perception_centerpoint_pkg/centerpoint_node.py b/src/perception_centerpoint_pkg/perception_centerpoint_pkg/centerpoint_node.py
deleted file mode 100755
index 6df03e6..0000000
--- a/src/perception_centerpoint_pkg/perception_centerpoint_pkg/centerpoint_node.py
+++ /dev/null
@@ -1,309 +0,0 @@
-#!/usr/bin/env python3
-
-import os
-import math
-
-import rclpy
-from rclpy.node import Node
-
-from sensor_msgs.msg import PointCloud2
-import sensor_msgs_py.point_cloud2 as pc2
-
-from vision_msgs.msg import Detection3DArray, Detection3D, ObjectHypothesisWithPose
-from visualization_msgs.msg import Marker, MarkerArray
-from std_msgs.msg import ColorRGBA
-
-import numpy as np
-
-from perception_centerpoint_pkg.centerpoint_backend import CenterPointBackend
-
-
-def _yaw_to_quat(yaw):
- """z축 yaw(rad) -> (x, y, z, w) quaternion."""
- half = yaw * 0.5
- return 0.0, 0.0, math.sin(half), math.cos(half)
-
-
-def _is_cuda_error(e):
- s = str(e).lower()
- return 'cuda' in s or 'cudnn' in s or 'out of memory' in s
-
-
-class ObjectDetection(Node):
- def __init__(self):
- super().__init__('centerpoint_centerpoint')
-
- # 토픽 (구조적 기본값만 — pcd_crop 과 동일 패턴)
- self.declare_parameter('input_topic', '/raw/lidar')
- self.declare_parameter('detection_topic', '/perception/detections')
- self.declare_parameter('marker_topic', '/perception/detection_markers')
-
- # 모델 경로/임계값 — 노드에 박지 않는다. config(yaml) 또는 환경변수에서 주입.
- self.declare_parameter('model_cfg_path', '')
- self.declare_parameter('model_ckpt_path', '')
- self.declare_parameter('score_threshold', 0.3)
- # device: 'auto' → 런타임 감지(cuda 가용 시 cuda:0, 아니면 cpu). Phase3 dual-runtime.
- self.declare_parameter('device', 'auto')
-
- # 보정 파라미터 (Rule 로 조절) — 센서 도메인을 학습분포에 맞춤.
- # z_offset: 입력 z 보정(지면 높이 차). max_range: 이 반경(m) 밖 박스 버림
- # (라이다 밀도 떨어지는 원거리 false positive 제거). 0 이면 비활성.
- self.declare_parameter('z_offset', 0.0)
- self.declare_parameter('max_range', 0.0)
- # 기하 검증 (빈 공간/바닥 헛검출 제거) — 박스 내부 점 개수/높이 최소값. 0 이면 비활성.
- self.declare_parameter('min_points_in_box', 0)
- self.declare_parameter('min_box_height', 0.0)
-
- input_topic = self.get_parameter('input_topic').value
- detection_topic = self.get_parameter('detection_topic').value
- marker_topic = self.get_parameter('marker_topic').value
-
- # config 가 비면 환경변수로 fallback (하드코딩 금지 룰)
- cfg_path = self.get_parameter('model_cfg_path').value or os.environ.get(
- 'CENTERPOINT_MODEL_CFG', '')
- ckpt_path = self.get_parameter('model_ckpt_path').value or os.environ.get(
- 'CENTERPOINT_MODEL_CKPT', '')
- score_threshold = self.get_parameter('score_threshold').value
- device = self.get_parameter('device').value
- z_offset = self.get_parameter('z_offset').value
- self.max_range = float(self.get_parameter('max_range').value)
- min_points_in_box = self.get_parameter('min_points_in_box').value
- min_box_height = self.get_parameter('min_box_height').value
-
- self.pub_det = self.create_publisher(Detection3DArray, detection_topic, 10)
- self.pub_marker = self.create_publisher(MarkerArray, marker_topic, 10)
-
- # 런타임 CUDA 에러 시 CPU 폴백 1회 가드
- self._cpu_fallback_done = False
- self.backend = self._init_backend(
- cfg_path, ckpt_path, score_threshold, device, z_offset,
- min_points_in_box, min_box_height)
-
- self.sub = self.create_subscription(
- PointCloud2, input_topic, self.callback, 10)
-
- self.get_logger().info(
- f"ObjDet(CenterPoint) started: in[{input_topic}] "
- f"det[{detection_topic}] marker[{marker_topic}] "
- f"threshold[{score_threshold}] device[{device}] "
- f"z_offset[{z_offset}] max_range[{self.max_range}]"
- )
-
- def _init_backend(self, cfg_path, ckpt_path, score_threshold, device, z_offset,
- min_points_in_box, min_box_height):
- if not cfg_path or not ckpt_path:
- self.get_logger().error(
- "model_cfg_path / model_ckpt_path 미지정 "
- "(config 또는 CENTERPOINT_MODEL_CFG/CENTERPOINT_MODEL_CKPT 환경변수). "
- "추론 비활성 — 입력 구독만 수행한다."
- )
- return None
- # 런타임 CUDA 폴백 시 ckpt 에서 CPU 백엔드 재생성에 쓰는 공통 인자
- self._backend_kwargs = dict(
- cfg_path=cfg_path, ckpt_path=ckpt_path,
- score_threshold=score_threshold,
- z_offset=z_offset, min_points_in_box=min_points_in_box,
- min_box_height=min_box_height, logger=None,
- )
- try:
- backend = CenterPointBackend(device=device, **self._backend_kwargs)
- self.get_logger().info(
- f"OpenPCDet 로드 완료: classes={backend.class_names}")
- return backend
- except Exception as e: # OpenPCDet 미설치 / 가중치 로드 실패 등
- # GPU 초기화 실패(OOM/cuDNN 등)면 CPU 로 1회 폴백 (08-gpu-cpu-runtime N3)
- if device != 'cpu' and _is_cuda_error(e):
- self.get_logger().warn(
- f"[centerpoint] GPU 백엔드 초기화 실패({e}) → CPU 폴백")
- try:
- backend = CenterPointBackend(
- device='cpu', **self._backend_kwargs)
- self.get_logger().info(
- f"OpenPCDet 로드 완료(CPU 폴백): classes={backend.class_names}")
- return backend
- except Exception as e2:
- self.get_logger().error(f"검출 백엔드 초기화 실패: {e2}")
- return None
- self.get_logger().error(f"검출 백엔드 초기화 실패: {e}")
- return None
-
- def _read_points(self, msg: PointCloud2):
- field_names = {f.name for f in msg.fields}
- if 'intensity' in field_names:
- pts = pc2.read_points(
- msg, field_names=("x", "y", "z", "intensity"), skip_nans=True)
- if pts.shape[0] == 0:
- return None
- return np.stack(
- (pts["x"], pts["y"], pts["z"], pts["intensity"]),
- axis=-1).astype(np.float32, copy=False)
- # intensity 없는 라이다(예: CARLA) — 0 으로 채움
- pts = pc2.read_points(
- msg, field_names=("x", "y", "z"), skip_nans=True)
- if pts.shape[0] == 0:
- return None
- xyz = np.stack((pts["x"], pts["y"], pts["z"]), axis=-1).astype(
- np.float32, copy=False)
- intensity = np.zeros((xyz.shape[0], 1), dtype=np.float32)
- return np.concatenate([xyz, intensity], axis=1)
-
- def callback(self, msg: PointCloud2):
- if self.backend is None:
- return
-
- points = self._read_points(msg)
- if points is None:
- return
-
- try:
- boxes, scores, labels = self.backend.infer(points)
- except Exception as e:
- if self._cpu_fallback_done or not _is_cuda_error(e):
- raise # CUDA 외 예외 / 이미 CPU 폴백 후 → 기존 동작 유지
- self.get_logger().warn(
- "[centerpoint] 추론 중 CUDA 에러 → CPU 백엔드 재구성")
- self._cpu_fallback_done = True
- try: # 깨진 CUDA 컨텍스트 정리 — 실패해도 무해
- torch = self.backend._torch
- del self.backend
- torch.cuda.empty_cache()
- except Exception:
- pass
- # .to('cpu') 이동 금지 — ckpt 에서 device='cpu' 로 재생성해야 _patch_cpu 발동
- try:
- self.backend = CenterPointBackend(
- device='cpu', **self._backend_kwargs)
- except Exception as e2:
- self.get_logger().error(f"CPU 백엔드 재생성 실패: {e2}")
- self.backend = None
- return
- try:
- boxes, scores, labels = self.backend.infer(points)
- except Exception as e2:
- self.get_logger().error(f"CPU 폴백 재시도 실패, 프레임 스킵: {e2}")
- return
-
- # max_range 밖(라이다 밀도 낮은 원거리) 박스 제거 — 박스 중심 수평거리 기준
- if self.max_range > 0.0 and boxes.shape[0] > 0:
- r = np.hypot(boxes[:, 0], boxes[:, 1])
- keep = r <= self.max_range
- boxes, scores, labels = boxes[keep], scores[keep], labels[keep]
-
- self.pub_det.publish(self._to_detection_array(msg.header, boxes, scores, labels))
- self.pub_marker.publish(self._to_marker_array(msg.header, boxes, labels))
-
- def _to_detection_array(self, header, boxes, scores, labels):
- arr = Detection3DArray()
- arr.header = header
- for box, score, label in zip(boxes, scores, labels):
- # nuScenes CenterPoint 은 9-DOF([x,y,z,dx,dy,dz,yaw,vx,vy], 속도 2축 포함),
- # KITTI 계열은 7-DOF. 공통으로 앞 7개(pose+size+yaw)만 쓰고 속도는 무시.
- x, y, z, dx, dy, dz, yaw = [float(v) for v in box[:7]]
- qx, qy, qz, qw = _yaw_to_quat(yaw)
-
- det = Detection3D()
- det.header = header
-
- hyp = ObjectHypothesisWithPose()
- hyp.hypothesis.class_id = str(int(label))
- hyp.hypothesis.score = float(score)
- det.results.append(hyp)
-
- det.bbox.center.position.x = x
- det.bbox.center.position.y = y
- det.bbox.center.position.z = z
- det.bbox.center.orientation.x = qx
- det.bbox.center.orientation.y = qy
- det.bbox.center.orientation.z = qz
- det.bbox.center.orientation.w = qw
- det.bbox.size.x = dx
- det.bbox.size.y = dy
- det.bbox.size.z = dz
-
- arr.detections.append(det)
- return arr
-
- def _to_marker_array(self, header, boxes, labels):
- markers = MarkerArray()
-
- # 이전 프레임 마커 제거
- clear = Marker()
- clear.header = header
- clear.action = Marker.DELETEALL
- markers.markers.append(clear)
-
- for i, (box, label) in enumerate(zip(boxes, labels)):
- # nuScenes 9-DOF / KITTI 7-DOF 공통: 앞 7개만(속도 vx,vy 무시)
- x, y, z, dx, dy, dz, yaw = [float(v) for v in box[:7]]
- qx, qy, qz, qw = _yaw_to_quat(yaw)
-
- m = Marker()
- m.header = header
- m.ns = 'centerpoint'
- m.id = i
- m.type = Marker.CUBE
- m.action = Marker.ADD
- m.pose.position.x = x
- m.pose.position.y = y
- m.pose.position.z = z
- m.pose.orientation.x = qx
- m.pose.orientation.y = qy
- m.pose.orientation.z = qz
- m.pose.orientation.w = qw
- m.scale.x = dx
- m.scale.y = dy
- m.scale.z = dz
- m.color = self._label_color(int(label))
- markers.markers.append(m)
-
- # 박스 위에 클래스명 텍스트 (YOLO 식 라벨)
- t = Marker()
- t.header = header
- t.ns = 'centerpoint_label'
- t.id = i
- t.type = Marker.TEXT_VIEW_FACING
- t.action = Marker.ADD
- t.pose.position.x = x
- t.pose.position.y = y
- t.pose.position.z = z + dz / 2.0 + 0.4
- t.pose.orientation.w = 1.0
- t.scale.z = 0.8
- t.color = self._label_color(int(label))
- t.color.a = 1.0
- t.text = self._label_name(int(label))
- markers.markers.append(t)
-
- return markers
-
- @staticmethod
- def _label_color(label):
- # 클래스별 색 (반투명). 미지정 라벨은 회색.
- palette = {
- 1: (0.0, 1.0, 0.0), # Car
- 2: (1.0, 1.0, 0.0), # Pedestrian
- 3: (0.0, 0.6, 1.0), # Cyclist
- }
- r, g, b = palette.get(label, (0.6, 0.6, 0.6))
- return ColorRGBA(r=r, g=g, b=b, a=0.5)
-
- @staticmethod
- def _label_name(label):
- # 클래스 id → 이름 (_label_color 팔레트와 동일 매핑). 미지정은 cls{id}.
- names = {1: 'Car', 2: 'Pedestrian', 3: 'Cyclist'}
- return names.get(label, f'cls{label}')
-
-
-def main(args=None):
- rclpy.init(args=args)
- node = ObjectDetection()
- try:
- rclpy.spin(node)
- except KeyboardInterrupt:
- pass
- finally:
- node.destroy_node()
- rclpy.shutdown()
-
-
-if __name__ == '__main__':
- main()
diff --git a/src/perception_centerpoint_pkg/resource/perception_centerpoint_pkg b/src/perception_centerpoint_pkg/resource/perception_centerpoint_pkg
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_centerpoint_pkg/setup.cfg b/src/perception_centerpoint_pkg/setup.cfg
deleted file mode 100755
index 618c720..0000000
--- a/src/perception_centerpoint_pkg/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/perception_centerpoint_pkg
-[install]
-install_scripts=$base/lib/perception_centerpoint_pkg
diff --git a/src/perception_centerpoint_pkg/setup.py b/src/perception_centerpoint_pkg/setup.py
deleted file mode 100755
index a03827b..0000000
--- a/src/perception_centerpoint_pkg/setup.py
+++ /dev/null
@@ -1,38 +0,0 @@
-
-import os
-from glob import glob
-from setuptools import find_packages, setup
-
-package_name = 'perception_centerpoint_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')),
- # finetuned 용 OpenPCDet custom cfg — ws 루트 cfgs/ 에서 pkg 로 복제 편입
- # (launch 의 모델 자동 resolve 가 share 경로로 참조).
- (os.path.join('share', package_name, 'cfgs'), glob('cfgs/*.yaml')),
- ],
- install_requires=['setuptools'],
- zip_safe=True,
- maintainer='dgist',
- maintainer_email='jang1future@gmail.com',
- description='LiDAR 3D object detection (CenterPoint / OpenPCDet) perception module',
- license='TODO: License declaration',
- extras_require={
- 'test': [
- 'pytest',
- ],
- },
- entry_points={
- 'console_scripts': [
- 'centerpoint = perception_centerpoint_pkg.centerpoint_node:main'
- ],
- },
-)
diff --git a/src/perception_centerpoint_pkg/test/test_copyright.py b/src/perception_centerpoint_pkg/test/test_copyright.py
deleted file mode 100755
index 97a3919..0000000
--- a/src/perception_centerpoint_pkg/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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'
diff --git a/src/perception_centerpoint_pkg/test/test_flake8.py b/src/perception_centerpoint_pkg/test/test_flake8.py
deleted file mode 100755
index 27ee107..0000000
--- a/src/perception_centerpoint_pkg/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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)
diff --git a/src/perception_centerpoint_pkg/test/test_pep257.py b/src/perception_centerpoint_pkg/test/test_pep257.py
deleted file mode 100755
index b234a38..0000000
--- a/src/perception_centerpoint_pkg/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# 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'
diff --git a/src/perception_lmnet_pkg/launch/lmnet.launch.py b/src/perception_lmnet_pkg/launch/lmnet.launch.py
deleted file mode 100755
index b6fcfe7..0000000
--- a/src/perception_lmnet_pkg/launch/lmnet.launch.py
+++ /dev/null
@@ -1,58 +0,0 @@
-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
diff --git a/src/perception_lmnet_pkg/launch/perception.launch.py b/src/perception_lmnet_pkg/launch/perception.launch.py
deleted file mode 100755
index b18a2b6..0000000
--- a/src/perception_lmnet_pkg/launch/perception.launch.py
+++ /dev/null
@@ -1,32 +0,0 @@
-"""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(),
- ),
- ])
diff --git a/src/perception_lmnet_pkg/package.xml b/src/perception_lmnet_pkg/package.xml
deleted file mode 100755
index 1d11999..0000000
--- a/src/perception_lmnet_pkg/package.xml
+++ /dev/null
@@ -1,24 +0,0 @@
-
-
-
- perception_lmnet_pkg
- 0.0.0
- LiDAR moving object segmentation (LMNet / SalsaNext-MOS) perception module
- dgist
- TODO: License declaration
-
- rclpy
- sensor_msgs
- sensor_msgs_py
- nav_msgs
- std_msgs
-
- ament_copyright
- ament_flake8
- ament_pep257
- python3-pytest
-
-
- ament_python
-
-
diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/__init__.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/__init__.py
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/lmnet_infer.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/lmnet_infer.py
deleted file mode 100755
index 3b2982c..0000000
--- a/src/perception_lmnet_pkg/perception_lmnet_pkg/lmnet_infer.py
+++ /dev/null
@@ -1,97 +0,0 @@
-"""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
diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/lmnet_node.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/lmnet_node.py
deleted file mode 100755
index 6051bb0..0000000
--- a/src/perception_lmnet_pkg/perception_lmnet_pkg/lmnet_node.py
+++ /dev/null
@@ -1,169 +0,0 @@
-"""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()
diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/pose_util.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/pose_util.py
deleted file mode 100755
index 26fb7f4..0000000
--- a/src/perception_lmnet_pkg/perception_lmnet_pkg/pose_util.py
+++ /dev/null
@@ -1,25 +0,0 @@
-"""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
diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/range_projection.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/range_projection.py
deleted file mode 100755
index 3ddda2e..0000000
--- a/src/perception_lmnet_pkg/perception_lmnet_pkg/range_projection.py
+++ /dev/null
@@ -1,68 +0,0 @@
-#!/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
diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/residual_lib.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/residual_lib.py
deleted file mode 100755
index 0b476f8..0000000
--- a/src/perception_lmnet_pkg/perception_lmnet_pkg/residual_lib.py
+++ /dev/null
@@ -1,28 +0,0 @@
-"""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
diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/resolve_model.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/resolve_model.py
deleted file mode 100755
index 699d667..0000000
--- a/src/perception_lmnet_pkg/perception_lmnet_pkg/resolve_model.py
+++ /dev/null
@@ -1,43 +0,0 @@
-"""모델 경로 결정 — 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
diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/scan_buffer.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/scan_buffer.py
deleted file mode 100755
index 883d9a7..0000000
--- a/src/perception_lmnet_pkg/perception_lmnet_pkg/scan_buffer.py
+++ /dev/null
@@ -1,21 +0,0 @@
-"""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]
diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/spawn_npc.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/spawn_npc.py
deleted file mode 100755
index d4e286a..0000000
--- a/src/perception_lmnet_pkg/perception_lmnet_pkg/spawn_npc.py
+++ /dev/null
@@ -1,129 +0,0 @@
-"""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()
diff --git a/src/perception_lmnet_pkg/resource/perception_lmnet_pkg b/src/perception_lmnet_pkg/resource/perception_lmnet_pkg
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_lmnet_pkg/setup.cfg b/src/perception_lmnet_pkg/setup.cfg
deleted file mode 100755
index be6c847..0000000
--- a/src/perception_lmnet_pkg/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/perception_lmnet_pkg
-[install]
-install_scripts=$base/lib/perception_lmnet_pkg
diff --git a/src/perception_lmnet_pkg/setup.py b/src/perception_lmnet_pkg/setup.py
deleted file mode 100755
index b004020..0000000
--- a/src/perception_lmnet_pkg/setup.py
+++ /dev/null
@@ -1,35 +0,0 @@
-
-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'
- ],
- },
-)
diff --git a/src/perception_lmnet_pkg/test/test_copyright.py b/src/perception_lmnet_pkg/test/test_copyright.py
deleted file mode 100755
index 97a3919..0000000
--- a/src/perception_lmnet_pkg/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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'
diff --git a/src/perception_lmnet_pkg/test/test_flake8.py b/src/perception_lmnet_pkg/test/test_flake8.py
deleted file mode 100755
index 27ee107..0000000
--- a/src/perception_lmnet_pkg/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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)
diff --git a/src/perception_lmnet_pkg/test/test_pep257.py b/src/perception_lmnet_pkg/test/test_pep257.py
deleted file mode 100755
index b234a38..0000000
--- a/src/perception_lmnet_pkg/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# 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'
diff --git a/src/perception_lmnet_pkg/test/test_pose_util.py b/src/perception_lmnet_pkg/test/test_pose_util.py
deleted file mode 100755
index bc3d00d..0000000
--- a/src/perception_lmnet_pkg/test/test_pose_util.py
+++ /dev/null
@@ -1,22 +0,0 @@
-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])
diff --git a/src/perception_lmnet_pkg/test/test_residual_lib.py b/src/perception_lmnet_pkg/test/test_residual_lib.py
deleted file mode 100755
index 711c627..0000000
--- a/src/perception_lmnet_pkg/test/test_residual_lib.py
+++ /dev/null
@@ -1,22 +0,0 @@
-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
diff --git a/src/perception_lmnet_pkg/test/test_resolve_model.py b/src/perception_lmnet_pkg/test/test_resolve_model.py
deleted file mode 100755
index 03e7941..0000000
--- a/src/perception_lmnet_pkg/test/test_resolve_model.py
+++ /dev/null
@@ -1,89 +0,0 @@
-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
diff --git a/src/perception_lmnet_pkg/test/test_scan_buffer.py b/src/perception_lmnet_pkg/test/test_scan_buffer.py
deleted file mode 100755
index 88ad31d..0000000
--- a/src/perception_lmnet_pkg/test/test_scan_buffer.py
+++ /dev/null
@@ -1,37 +0,0 @@
-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
diff --git a/src/perception_motionbev_pkg/config/motionbev.yaml b/src/perception_motionbev_pkg/config/motionbev.yaml
deleted file mode 100755
index f54a1e3..0000000
--- a/src/perception_motionbev_pkg/config/motionbev.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-# MotionBEV deploy 파라미터 기본값 (런타임은 launch arg 가 우선).
-# 실제 모델 격자/means/stds 는 finetuned arch_cfg.yaml(또는 baked MotionBEV-semantickitti.yaml)에서
-# 읽는다 — 여기엔 토픽/소스 같은 노드 운영값만 둔다 (하드코딩 격자 금지, 단일소스=arch_cfg).
-motionbev_node:
- ros__parameters:
- use_sim_time: true
- input_topic: /raw/lidar
- odom_topic: /icp_odom
- moving_topic: /perception/moving
- moving_rgb_topic: /perception/moving_rgb
- model_source: finetuned # finetuned | pretrained
diff --git a/src/perception_motionbev_pkg/launch/motionbev.launch.py b/src/perception_motionbev_pkg/launch/motionbev.launch.py
deleted file mode 100755
index a2a6d9f..0000000
--- a/src/perception_motionbev_pkg/launch/motionbev.launch.py
+++ /dev/null
@@ -1,59 +0,0 @@
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
-# 모델 경로 단일소스 = 노드와 같은 pkg 모듈(resolve_model) — launch 에 경로 하드코딩 금지.
-from perception_motionbev_pkg import resolve_model
-
-# 토픽/모델은 launch arg 로 전달(노드는 argparse parse_known_args 로 받음 → --ros-args 무시).
-# odom 기본값 /icp_odom: RTABMap icp_odometry (런타임 확인: /odom 무발행).
-MODEL_SOURCE_DEFAULT = 'finetuned'
-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=MODEL_SOURCE_DEFAULT), # UI 스위치: finetuned|pretrained
-]
-
-
-def _resolve_and_load(context):
- """모델 resolve (OpaqueFunction). MOS 계열은 모델 경로를 노드(resolve_model.resolve)가
- 정하므로 launch 는 model_source 전달만 확정한다 — lmnet 과 동일 원칙.
- - model_source 를 명시(선언 기본값과 다름)하면 그대로 통과 — 단독 deploy run.sh 가
- launch arg 로 명시 전달하는 기존 동작 무회귀.
- - 기본값/빈값이면 finetuned 3파일(MotionBEV_valid_best/arch_cfg.yaml/data_cfg.yaml)
- 존재 검사로 자동 resolve — 노드 fallback 과 같은 결과를 launch 시점에 확정/로그."""
- requested = LaunchConfiguration('model_source').perform(context)
- if requested and requested != MODEL_SOURCE_DEFAULT:
- model_source = requested # 명시 인자가 이긴다 (무회귀)
- else:
- model_source = resolve_model.resolve(model_source=MODEL_SOURCE_DEFAULT)['source']
- print(f"[motionbev.launch] model_source={model_source} (요청='{requested or MODEL_SOURCE_DEFAULT}')",
- flush=True)
-
- remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
- load_nodes = GroupAction(actions=[
- Node(
- package='perception_motionbev_pkg', executable='motionbev',
- name='motionbev_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', model_source,
- ],
- remappings=remappings,
- )
- ])
- return [load_nodes]
-
-
-def generate_launch_description():
- ld = LaunchDescription(ARGUMENTS)
- ld.add_action(OpaqueFunction(function=_resolve_and_load))
- return ld
diff --git a/src/perception_motionbev_pkg/launch/perception.launch.py b/src/perception_motionbev_pkg/launch/perception.launch.py
deleted file mode 100755
index ef7d2a5..0000000
--- a/src/perception_motionbev_pkg/launch/perception.launch.py
+++ /dev/null
@@ -1,32 +0,0 @@
-"""perception_manager 공통 진입점 (통합 perception 모듈).
-
-LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로
-`ros2 launch perception_motionbev_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..`
-를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(motionbev.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_motionbev_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', 'motionbev.launch.py')),
- launch_arguments={
- 'robot': LaunchConfiguration('robot'),
- 'use_sim_time': LaunchConfiguration('use_sim_time'),
- 'model_source': LaunchConfiguration('model_source'),
- }.items(),
- ),
- ])
diff --git a/src/perception_motionbev_pkg/package.xml b/src/perception_motionbev_pkg/package.xml
deleted file mode 100755
index 0d082f7..0000000
--- a/src/perception_motionbev_pkg/package.xml
+++ /dev/null
@@ -1,24 +0,0 @@
-
-
-
- perception_motionbev_pkg
- 0.0.0
- LiDAR moving object segmentation (MotionBEV / Polar-BEV) perception module
- dgist
- TODO: License declaration
-
- rclpy
- sensor_msgs
- sensor_msgs_py
- nav_msgs
- std_msgs
-
- ament_copyright
- ament_flake8
- ament_pep257
- python3-pytest
-
-
- ament_python
-
-
diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/__init__.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/__init__.py
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/motionbev_infer.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/motionbev_infer.py
deleted file mode 100755
index 731b6f7..0000000
--- a/src/perception_motionbev_pkg/perception_motionbev_pkg/motionbev_infer.py
+++ /dev/null
@@ -1,158 +0,0 @@
-"""MotionBEV 실시간 추론. 모델 정의는 baked /opt/MotionBEV 에서 import(재구현 X).
-입력 = polar-BEV base 채널([occupancy,max_z,intensity] 정규화) + polar residual_1..N."""
-import sys
-
-import numpy as np
-import torch
-import yaml
-
-from perception_motionbev_pkg.polar_projection import PolarBEVProjector
-
-# ★ torch 스레드풀 단일화 — 모델 로드 중 다중스레드 futex 데드락 회피(LMNet 2026-06-08 케이스 동일).
-torch.set_num_threads(1)
-try:
- torch.set_num_interop_threads(1)
-except Exception:
- pass
-
-# baked MotionBEV 코드 경로 — Polar-BEV 백본 재사용.
-_MOTIONBEV = "/opt/MotionBEV"
-if _MOTIONBEV not in sys.path:
- sys.path.insert(0, _MOTIONBEV)
-# 업스트림(github.com/xieKKKi/MotionBEV) 실제 모듈 레이아웃:
-# 모델 = network.ptBEVnet.ptBEVnet (point-feature in → polar-BEV voxel logits out),
-# BEV 백본 = network.CA_BEV_Unet.CA_Unet (co-attention) 또는 network.A_BEV_Unet.BEV_Unet.
-# ※ 단일 `MotionBEV` 클래스는 존재하지 않는다 (RA-L'23 코드는 ptBEVnet 래퍼 구조).
-from network.ptBEVnet import ptBEVnet # noqa: E402
-from network.CA_BEV_Unet import CA_Unet # noqa: E402
-from network.A_BEV_Unet import BEV_Unet # noqa: E402
-
-
-class MotionBEVInfer:
- def __init__(self, ckpt_path, arch_cfg_path, data_cfg_path, device="auto"):
- self.arch = yaml.safe_load(open(arch_cfg_path))
- self.data = yaml.safe_load(open(data_cfg_path))
- # device='auto' → 런타임 감지: cuda 가용 시 cuda, 아니면 cpu (08-gpu-cpu-runtime N4)
- if device == "auto":
- device = "cuda" if torch.cuda.is_available() else "cpu"
- self.device = device
- grid = self._resolve_grid()
- self.proj = PolarBEVProjector(grid["n_range"], grid["n_angle"],
- grid["max_range"], grid["min_range"],
- grid.get("z_min", -3.0), grid.get("z_max", 3.0),
- n_height=grid.get("n_height", 32))
- self.R, self.A, self.H = self.proj.R, self.proj.A, self.proj.H
- self.n_residual = self._resolve_n_residual()
- # MOS 2-class: learning_map_inv {train_id: orig}. 업스트림 semantic-kitti-MOS.yaml =
- # {0: 9(static), 1: 251(moving)} → moving train_id=1, nclass=2.
- inv = self.data.get("moving_learning_map_inv", self.data.get("learning_map_inv", {0: 9, 1: 251}))
- self.nclasses = len(inv)
- self.moving_idx = self._moving_class_index(inv)
-
- print("[dbg] before ptBEVnet()", flush=True)
- self.model = self._build_model()
- print("[dbg] after ptBEVnet, 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
- # DataParallel prefix 만 strip — naive replace("module.") 는 attention_module. 같은
- # 서브모듈 이름 속 "module." 까지 깎아 키를 망가뜨린다 (kitti-val-76.54 로드 실패 원인).
- sd = {(k[len("module."):] if k.startswith("module.") else k): 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)
-
- # 업스트림 polar_dataset(spherical_dataset) 의 학습 volume — max=[50.15,π,2], min=[1.85,-π,-4].
- # 배포/finetuned arch_cfg 가 dataset.grid 를 명시하면 그걸 우선한다(하드코딩 회피).
- _UP_RANGE_MAX, _UP_RANGE_MIN = 50.15, 1.85
- _UP_Z_MAX, _UP_Z_MIN = 2.0, -4.0
-
- def _resolve_grid(self):
- """polar BEV 격자 파라미터 결정.
- 1) deploy/finetuned arch_cfg 가 dataset.grid 블록을 주면 그대로 사용.
- 2) 업스트림 schema(model_params.grid_size=[n_range,n_angle,n_height])면 거기서 R/A 추출 +
- 업스트림 polar volume 기본값으로 range/z 보충."""
- if "dataset" in self.arch and "grid" in self.arch["dataset"]:
- return self.arch["dataset"]["grid"]
- gs = list(self.arch["model_params"]["grid_size"]) # [n_range, n_angle, n_height]
- return {"n_range": int(gs[0]), "n_angle": int(gs[1]), "n_height": int(gs[2]),
- "max_range": self._UP_RANGE_MAX, "min_range": self._UP_RANGE_MIN,
- "z_min": self._UP_Z_MIN, "z_max": self._UP_Z_MAX}
-
- def _resolve_n_residual(self):
- """residual(과거 scan) 개수. deploy schema=train.n_input_scans, 업스트림=data_loader.residual."""
- tr = self.arch.get("train", {})
- if "n_input_scans" in tr:
- return int(tr["n_input_scans"])
- return int(self.arch.get("data_loader", {}).get("residual", 8))
-
- def _build_model(self):
- """업스트림 ptBEVnet 구조를 arch_cfg 로부터 구성. 하드코딩 금지 —
- grid_size/fea_dim/init_dim/dropout/co-attention 은 config 에서 읽는다.
- 업스트림 schema(model_params) 와 deploy/finetuned schema 양쪽 허용."""
- mp = self.arch.get("model_params", self.arch) # 업스트림=model_params, deploy=top-level fallback
- n_residual = self.n_residual
- grid_size = list(mp["grid_size"]) # [n_range, n_angle, n_height]
- n_height = int(grid_size[2])
- fea_dim = int(mp["fea_dim"])
- init_dim = int(mp["ppmodel_init_dim"])
- use_norm = bool(mp.get("use_norm", True))
- dropout = float(mp.get("dropout", 0.0))
- use_co_attention = bool(mp.get("use_co_attention", True))
- # circular_padding: 업스트림 infer_SemanticKITTI 는 polar dataset 이면 True (방위각이 순환).
- # pretrained 가중치(double_conv_circular: conv1/conv2)가 이 분기로만 로드된다.
- # 하드코딩 회피 — data_loader.dataset_type 에서 읽고, 기본 polar→True.
- dataset_type = self.arch.get("data_loader", {}).get("dataset_type", "polar")
- circular_padding = (dataset_type == "polar")
- bev_kwargs = dict(n_class=self.nclasses, n_height=n_height,
- residual=n_residual, input_batch_norm=use_norm,
- dropout=dropout, circular_padding=circular_padding)
- bev_model = CA_Unet(**bev_kwargs) if use_co_attention else BEV_Unet(**bev_kwargs)
- return ptBEVnet(bev_model, grid_size=grid_size, fea_dim=fea_dim,
- ppmodel_init_dim=init_dim, kernal_size=1, fea_compre=n_height)
-
- @staticmethod
- def _moving_class_index(inv):
- for train_id, orig in inv.items():
- if int(orig) in (251, 252): # KITTI-MOS moving 원본 id
- return int(train_id)
- return max(int(k) for k in inv) # fallback: 마지막 클래스(moving 관례)
-
- def preprocess(self, points, residuals):
- """points(M,>=3[,4]) + residuals(list of N×(R,A) cell residual)
- → (pt_fea_list, grid_ind_list, PolarPointFeature).
-
- 업스트림 ptBEVnet 은 **점별 피처 리스트**(BEV 이미지 텐서가 아니라)를 받는다:
- forward(pt_fea_list, grid_ind_list, device).
- pt_fea[v] = [appearance 9차원] ++ [residual N차원].
- - appearance = spherical_dataset 폴라 피처(bias·polar·xy·intensity).
- - residual 컬럼 k = compute_residual 가 만든 k번째 (R,A) cell residual 을
- 각 점의 (rho,theta) bin 으로 gather 한 값(업스트림 diff_map[point]←residual[proj] 대응).
- grid_ind[v] = [rho_idx, theta_idx, z_idx] (3D polar voxel index).
- """
- pf = self.proj.point_features(points) # valid 점만 포함
- ri, ai = pf.grid_ind[:, 0], pf.grid_ind[:, 1] # 점별 (rho,theta) bin
- if residuals:
- res_cols = np.stack([r[ri, ai] for r in residuals], axis=1) # (V, N)
- else: # residual 없음(warmup) → 0
- res_cols = np.zeros((pf.pt_fea.shape[0], self.n_residual), np.float32)
- fea = np.concatenate([pf.pt_fea, res_cols.astype(np.float32)], axis=1) # (V, 9+N)
-
- pt_fea_list = [torch.from_numpy(fea).type(torch.FloatTensor).to(self.device)]
- grid_ind_list = [torch.from_numpy(pf.grid_ind).to(self.device)]
- return pt_fea_list, grid_ind_list, pf
-
- @torch.no_grad()
- def infer(self, points, residuals):
- """반환: per-point moving bool (M,). residuals 없으면(warmup) 전부 static."""
- pt_fea_list, grid_ind_list, pf = self.preprocess(points, residuals)
- # ptBEVnet → (voxel_logits, None). voxel_logits: [B, nclass, R, A, H].
- voxel_logits, _ = self.model(pt_fea_list, grid_ind_list, self.device)
- pred = voxel_logits.argmax(dim=1)[0].cpu().numpy() # (R, A, H) train_id
- gi = pf.grid_ind
- pt_labels = pred[gi[:, 0], gi[:, 1], gi[:, 2]] # valid 점별 train_id
-
- moving = np.zeros(pf.valid.shape[0], dtype=bool) # 전체 M 길이
- moving[pf.valid] = (pt_labels == self.moving_idx)
- return moving
diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/motionbev_node.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/motionbev_node.py
deleted file mode 100755
index f0dde5b..0000000
--- a/src/perception_motionbev_pkg/perception_motionbev_pkg/motionbev_node.py
+++ /dev/null
@@ -1,163 +0,0 @@
-"""MotionBEV-MOS deploy 노드. /raw/lidar + odom 구독 → N-스캔 버퍼 → polar-BEV residual →
-MotionBEV 추론 → /perception/moving(label) + /perception/moving_rgb(빨강) 발행.
-상태성 3개(버퍼/odom캐싱/residual) — LMNet deploy 와 동일 패턴(stateful MOS, 01-arch §1.5)."""
-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_motionbev_pkg import resolve_model
-from perception_motionbev_pkg import pose_util
-from perception_motionbev_pkg.scan_buffer import ScanBuffer
-from perception_motionbev_pkg.residual_lib import compute_residual
-from perception_motionbev_pkg.motionbev_infer import MotionBEVInfer
-
-
-def _is_cuda_error(e):
- s = str(e).lower()
- return 'cuda' in s or 'cudnn' in s or 'out of memory' in s
-
-
-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 으로 채움
- 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)
-
-
-class MotionBEVNode(Node):
- def __init__(self, args, infer, model_info):
- super().__init__("motionbev_node")
- # infer 는 rclpy.init() 전에 미리 로드해 받는다 — DDS 스레드와의 데드락 회피.
- self.infer = infer
- self.model_info = model_info # 런타임 CPU 폴백 시 ckpt 재로드용
- self._cpu_fallback_done = (infer.device == "cpu")
- self.N = self.infer.n_residual
- self.proj = self.infer.proj # deploy residual 과 추론 preprocess 가 같은 격자 공유
- 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"[motionbev] 프레임 처리 실패, 스킵: {e}", throttle_duration_sec=5.0)
-
- def _process_lidar(self, msg):
- pts = _read_xyzi(msg)
- if not self.have_odom:
- self.get_logger().warn("[motionbev] 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))
- moving = self._infer_with_fallback(cur_pts, residuals)
- self._publish(msg.header, cur_pts, moving)
-
- def _infer_with_fallback(self, cur_pts, residuals):
- """추론 중 CUDA 에러 시 1회 CPU 폴백 — ckpt 에서 CPU 백엔드 재생성 후 해당 프레임 재시도.
- (.to('cpu') 이동 금지 — 깨진 CUDA 컨텍스트 위험. 08-gpu-cpu-runtime N3 CPU-first 규범)"""
- try:
- return self.infer.infer(cur_pts, residuals)
- except Exception as e:
- if self._cpu_fallback_done or not _is_cuda_error(e):
- raise
- self.get_logger().warn(f"[motionbev] 추론 중 CUDA 에러 → CPU 백엔드 재구성: {e}")
- self._cpu_fallback_done = True
- try: # 기존 GPU 모델 해제 — 실패해도 무해
- del self.infer.model
- torch.cuda.empty_cache()
- except Exception:
- pass
- m = self.model_info
- self.infer = MotionBEVInfer(m["ckpt"], m["arch_cfg"], m["data_cfg"], device="cpu")
- self.proj = self.infer.proj
- return self.infer.infer(cur_pts, residuals)
-
- 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("--model_source", default="finetuned") # UI 스위치: finetuned|pretrained
- ap.add_argument("--device", default="auto") # auto=런타임 감지(GPU 있으면 cuda, 없으면 cpu)
- args, _ = ap.parse_known_args()
- # ★ 모델을 rclpy.init() "전에" 로드 — DDS 수신 스레드와 torch 로드 동시 기동 시 futex 데드락 회피.
- m = resolve_model.resolve(model_source=args.model_source)
- print(f"[motionbev] model source={m['source']} ckpt={m['ckpt']}", flush=True)
- if m["source"] == "pretrained":
- print("[motionbev] finetuned 없음 — KITTI pretrained 동작(CARLA 품질 낮음). 먼저 Retrain 권장", flush=True)
- try:
- infer = MotionBEVInfer(m["ckpt"], m["arch_cfg"], m["data_cfg"], device=args.device)
- except Exception as e:
- if args.device == "cpu" or not _is_cuda_error(e):
- raise
- print(f"[motionbev] GPU 백엔드 초기화 실패({e}) → CPU 폴백", flush=True)
- infer = MotionBEVInfer(m["ckpt"], m["arch_cfg"], m["data_cfg"], device="cpu")
- rclpy.init()
- node = MotionBEVNode(args, infer, m)
- try:
- rclpy.spin(node)
- except KeyboardInterrupt:
- pass
- finally:
- node.destroy_node()
- rclpy.shutdown()
-
-
-if __name__ == "__main__":
- main()
diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/polar_projection.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/polar_projection.py
deleted file mode 100755
index 832a0a8..0000000
--- a/src/perception_motionbev_pkg/perception_motionbev_pkg/polar_projection.py
+++ /dev/null
@@ -1,127 +0,0 @@
-#!/usr/bin/env python3
-"""MotionBEV polar BEV projection (모델 무관, 순수 numpy).
-
-MotionBEV(RA-L'23) 의 입력 표현 = Polar BEV grid. range-image(LMNet) 와 달리 점을
-극좌표 BEV 격자(거리 bin × 방위각 bin)에 투영해 BEV 채널(최대 높이 등)을 만든다.
-시간축 residual(과거-현재 polar BEV 차이)을 만들기 위한 공유 투영기 — 학습(gen_residual)과
-배포(deploy)가 같은 격자 공식을 써야 학습-배포 분포가 일치한다(하드코딩 금지: 격자 파라미터는
-arch_cfg.dataset.grid 에서 읽어 retrain.sh / deploy 가 동일값을 전달).
-
-LMNet 의 range-image 대비 의도된 차이(04-consistency §1.5): MOS 가치를 위한 시간축 입력은
-공통, **표현방식만 Polar-BEV 로 다양화**(교육 가치). 나머지 수집/로그/그래프/split 은 동일.
-"""
-from dataclasses import dataclass
-
-import numpy as np
-
-
-@dataclass
-class PolarBEVResult:
- bev: np.ndarray # (C, R, A): occupancy, max_z, mean_intensity
- rb: np.ndarray # (V,) valid 점들의 거리 bin index
- ab: np.ndarray # (V,) valid 점들의 방위각 bin index
- valid: np.ndarray # (N,) bool — 격자 범위 안 점 마스크
-
-
-@dataclass
-class PolarPointFeature:
- """ptBEVnet 입력용 점별 polar 피처 + 3D polar grid index (업스트림 spherical_dataset 대응).
-
- spherical_dataset.__getitem__ 의 return_fea(폴라) 레이아웃을 그대로 재현한다:
- [bias_rho, bias_theta, bias_z, rho, theta, z, x, y, intensity] (fea_dim=9)
- residual(motion) 컬럼은 infer 단계에서 우측에 concat 한다.
- grid_ind = [rho_idx, theta_idx, z_idx] (업스트림 grid_size=[480,360,32] 순서).
- """
- pt_fea: np.ndarray # (V, 9) valid 점 appearance 피처
- grid_ind: np.ndarray # (V, 3) valid 점 polar grid index
- valid: np.ndarray # (N,) bool — 격자 범위(rho·z) 안 점 마스크
-
-
-class PolarBEVProjector:
- def __init__(self, n_range, n_angle, max_range, min_range, z_min=-3.0, z_max=3.0,
- n_height=32):
- self.R = int(n_range)
- self.A = int(n_angle)
- self.H = int(n_height)
- self.max_range = float(max_range)
- self.min_range = float(min_range)
- self.z_min = float(z_min)
- self.z_max = float(z_max)
-
- def project(self, points):
- """points(N,>=3) → PolarBEVResult. BEV 채널: [occupancy, max_z(정규화), mean_intensity]."""
- xyz = points[:, :3]
- intensity = points[:, 3] if points.shape[1] > 3 else np.zeros(len(points), np.float32)
- x, y, z = xyz[:, 0], xyz[:, 1], xyz[:, 2]
- rho = np.sqrt(x * x + y * y)
- theta = np.arctan2(y, x) # [-pi, pi]
- valid = (rho >= self.min_range) & (rho < self.max_range)
-
- rb = np.clip(((rho - self.min_range) / (self.max_range - self.min_range)
- * self.R).astype(np.int32), 0, self.R - 1)
- ab = np.clip(((theta + np.pi) / (2.0 * np.pi) * self.A).astype(np.int32),
- 0, self.A - 1)
-
- bev = np.zeros((3, self.R, self.A), np.float32)
- vi = np.where(valid)[0]
- # 가까운 점부터(작은 rho) 채워 마지막에 높은 점이 max_z 를 덮어쓰도록 정렬
- order = vi[np.argsort(rho[vi])]
- vr, va, vz, vint = rb[order], ab[order], z[order], intensity[order]
- zn = np.clip((vz - self.z_min) / max(self.z_max - self.z_min, 1e-6), 0.0, 1.0)
- bev[0, vr, va] = 1.0 # occupancy
- np.maximum.at(bev[1], (vr, va), zn) # max height (정규화)
- bev[2, vr, va] = vint # intensity (마지막 점)
- return PolarBEVResult(bev=bev, rb=rb[valid], ab=ab[valid], valid=valid)
-
- def point_features(self, points):
- """points(N,>=3[,4]) → PolarPointFeature (업스트림 spherical_dataset 폴라 피처 재현).
-
- 볼륨/격자 공식은 업스트림과 동일: polar 좌표 [rho, theta, z] 를 고정 볼륨
- ([min_range,-π,z_min]~[max_range,π,z_max]) 에 grid_size-1 등분해 인덱싱한다.
- 하드코딩 금지: R/A/H·range·z 는 모두 생성자(arch_cfg 기반)에서 온다.
- """
- xyz = points[:, :3].astype(np.float32)
- intensity = (points[:, 3] if points.shape[1] > 3
- else np.zeros(len(points), np.float32)).astype(np.float32)
- x, y, z = xyz[:, 0], xyz[:, 1], xyz[:, 2]
- rho = np.sqrt(x * x + y * y)
- theta = np.arctan2(y, x) # [-π, π]
- xyz_pol = np.stack((rho, theta, z), axis=1) # 업스트림 cart2polar 와 동일 순서
-
- min_bound = np.array([self.min_range, -np.pi, self.z_min], np.float32)
- max_bound = np.array([self.max_range, np.pi, self.z_max], np.float32)
- grid_size = np.array([self.R, self.A, self.H], np.int64)
- intervals = (max_bound - min_bound) / (grid_size - 1)
-
- clipped = np.clip(xyz_pol, min_bound, max_bound)
- grid_ind = np.floor((clipped - min_bound) / intervals).astype(np.int64)
- grid_ind = np.clip(grid_ind, 0, grid_size - 1)
-
- # 업스트림: voxel 중심 대비 bias + 절대 polar + (x,y) + intensity = 9 차원
- voxel_centers = (grid_ind.astype(np.float32) + 0.5) * intervals + min_bound
- bias = xyz_pol - voxel_centers
- pt_fea = np.concatenate(
- (bias, xyz_pol, xyz[:, :2], intensity[:, None]), axis=1).astype(np.float32)
-
- # 격자 범위(rho·z) 밖 점은 motion 비교/언프로젝트에서 제외 (방위각은 순환이라 항상 유효)
- valid = ((rho >= self.min_range) & (rho < self.max_range) &
- (z >= self.z_min) & (z < self.z_max))
- return PolarPointFeature(pt_fea=pt_fea[valid], grid_ind=grid_ind[valid], valid=valid)
-
- def unproject(self, cell_labels, proj: PolarBEVResult):
- """cell_labels (R,A) → 점별 라벨 (N,). valid 아닌 점은 0(unlabeled)."""
- n = proj.valid.shape[0]
- labels = np.zeros(n, dtype=np.int64)
- labels[proj.valid] = cell_labels[proj.rb, proj.ab]
- return labels
-
-
-def projector_from_arch(arch_cfg_path):
- """arch_cfg(dataset.grid) 에서 polar BEV 격자 파라미터를 읽어 PolarBEVProjector 생성.
- 하드코딩 금지: residual 격자는 base 채널(학습 parser)·deploy 가 읽는 arch_cfg 와 같아야
- 학습-배포가 일치한다(다르면 모델이 본 적 없는 residual 분포 → 성능 저하)."""
- import yaml
- g = yaml.safe_load(open(arch_cfg_path))['dataset']['grid']
- return PolarBEVProjector(g['n_range'], g['n_angle'],
- g['max_range'], g['min_range'],
- g.get('z_min', -3.0), g.get('z_max', 3.0))
diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/pose_util.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/pose_util.py
deleted file mode 100755
index 26fb7f4..0000000
--- a/src/perception_motionbev_pkg/perception_motionbev_pkg/pose_util.py
+++ /dev/null
@@ -1,25 +0,0 @@
-"""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
diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/residual_lib.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/residual_lib.py
deleted file mode 100755
index 42dc7ff..0000000
--- a/src/perception_motionbev_pkg/perception_motionbev_pkg/residual_lib.py
+++ /dev/null
@@ -1,29 +0,0 @@
-"""MotionBEV polar-BEV residual (deploy). retrain gen_residual.compute_residual 와 동일 로직 복제.
-학습-배포 일관성을 위해 같은 PolarBEVProjector 격자 공식을 쓴다."""
-import numpy as np
-
-from perception_motionbev_pkg.polar_projection import PolarBEVProjector # noqa: F401 (호출측 주입)
-
-
-def _bev_motion_channel(points, projector):
- """polar BEV 의 motion 비교용 채널(R,A) = occupancy(0) + max_z(1) 합성."""
- r = projector.project(points)
- return r.bev[0] + r.bev[1]
-
-
-def compute_residual(curr_points, past_points, T_past_to_curr, projector):
- """현재 프레임 기준 polar-BEV residual(R,A). 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_b = _bev_motion_channel(curr_points, projector)
- past_b = _bev_motion_channel(past_aligned, projector)
- valid = (cur_b > 0) | (past_b > 0)
- res = np.zeros_like(cur_b)
- res[valid] = np.abs(cur_b[valid] - past_b[valid]) / np.maximum(cur_b[valid], 1e-6)
- return res
diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/resolve_model.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/resolve_model.py
deleted file mode 100755
index a6c15d9..0000000
--- a/src/perception_motionbev_pkg/perception_motionbev_pkg/resolve_model.py
+++ /dev/null
@@ -1,43 +0,0 @@
-"""모델 경로 결정 — finetuned(retrain 산출물) 우선, 없거나 불완전하면 baked pretrained fallback.
-※ 작은 함수 하나로 분리 — model_source=auto|finetuned|pretrained 명시 선택을 한 분기로 끼우기 위함."""
-import os
-
-# 가중치 공유 단일 위치(docker/local): 호스트 .caches/config/nav2_config/perception/models/motionbev/*
-# → 컨테이너 /perception_models/motionbev/{finetuned,pretrained} (SWRule config_mounts).
-DEFAULT_FINETUNED = "/perception_models/motionbev/finetuned"
-SHARED_PRE_DIR = "/perception_models/motionbev/pretrained" # 공유 pretrained (Phase 2 채움)
-DEFAULT_PRE_CKPT = "/opt/motionbev_pretrained/MotionBEV" # 이미지 baked 안전망
-DEFAULT_PRE_ARCH = "/opt/MotionBEV/config/MotionBEV-semantickitti.yaml"
-# 업스트림 실제 경로(infer_SemanticKITTI.py 의 data_config_path 와 동일).
-# 옛 "config/labels/semantic-kitti-mos.yaml" 는 존재하지 않음(생성기 전용 경로와 혼동).
-DEFAULT_PRE_DATA = "/opt/MotionBEV/config/semantic-kitti-MOS.yaml"
-
-
-def _shared_pretrained():
- # 공유 pretrained(.../pretrained: ckpt+arch+data)가 채워져 있으면 baked(/opt) 보다 우선.
- ckpt = os.path.join(SHARED_PRE_DIR, "MotionBEV")
- 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, "MotionBEV_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
diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/scan_buffer.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/scan_buffer.py
deleted file mode 100755
index 883d9a7..0000000
--- a/src/perception_motionbev_pkg/perception_motionbev_pkg/scan_buffer.py
+++ /dev/null
@@ -1,21 +0,0 @@
-"""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]
diff --git a/src/perception_motionbev_pkg/resource/perception_motionbev_pkg b/src/perception_motionbev_pkg/resource/perception_motionbev_pkg
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_motionbev_pkg/setup.cfg b/src/perception_motionbev_pkg/setup.cfg
deleted file mode 100755
index dac6b29..0000000
--- a/src/perception_motionbev_pkg/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/perception_motionbev_pkg
-[install]
-install_scripts=$base/lib/perception_motionbev_pkg
diff --git a/src/perception_motionbev_pkg/setup.py b/src/perception_motionbev_pkg/setup.py
deleted file mode 100755
index b33ee64..0000000
--- a/src/perception_motionbev_pkg/setup.py
+++ /dev/null
@@ -1,35 +0,0 @@
-
-import os
-from glob import glob
-from setuptools import find_packages, setup
-
-package_name = 'perception_motionbev_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 (MotionBEV / Polar-BEV) perception module',
- license='TODO: License declaration',
- extras_require={
- 'test': [
- 'pytest',
- ],
- },
- entry_points={
- 'console_scripts': [
- 'motionbev = perception_motionbev_pkg.motionbev_node:main'
- ],
- },
-)
diff --git a/src/perception_motionbev_pkg/test/test_copyright.py b/src/perception_motionbev_pkg/test/test_copyright.py
deleted file mode 100755
index 97a3919..0000000
--- a/src/perception_motionbev_pkg/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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'
diff --git a/src/perception_motionbev_pkg/test/test_flake8.py b/src/perception_motionbev_pkg/test/test_flake8.py
deleted file mode 100755
index 27ee107..0000000
--- a/src/perception_motionbev_pkg/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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)
diff --git a/src/perception_motionbev_pkg/test/test_pep257.py b/src/perception_motionbev_pkg/test/test_pep257.py
deleted file mode 100755
index b234a38..0000000
--- a/src/perception_motionbev_pkg/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# 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'
diff --git a/src/perception_motionbev_pkg/test/test_polar_projection.py b/src/perception_motionbev_pkg/test/test_polar_projection.py
deleted file mode 100755
index 6a875f9..0000000
--- a/src/perception_motionbev_pkg/test/test_polar_projection.py
+++ /dev/null
@@ -1,31 +0,0 @@
-import numpy as np
-from perception_motionbev_pkg.polar_projection import PolarBEVProjector
-
-
-def test_shape_and_occupancy():
- proj = PolarBEVProjector(n_range=32, n_angle=90, max_range=50.0, min_range=2.0)
- pts = np.array([[10.0, 0.0, 0.5, 0.7]], np.float32)
- r = proj.project(pts)
- assert r.bev.shape == (3, 32, 90)
- assert r.bev[0].sum() == 1.0 # 점 1개 → occupancy 1칸
- assert r.valid.sum() == 1
-
-
-def test_out_of_range_excluded():
- proj = PolarBEVProjector(n_range=32, n_angle=90, max_range=50.0, min_range=2.0)
- pts = np.array([[0.5, 0.0, 0.0, 1.0], # rhomax → 제외
- np.float32)
- r = proj.project(pts)
- assert r.valid.sum() == 0
-
-
-def test_unproject_round_trip():
- proj = PolarBEVProjector(n_range=16, n_angle=60, max_range=50.0, min_range=2.0)
- pts = np.array([[10.0, 0.0, 0.0, 1.0], [20.0, 5.0, 0.0, 1.0]], np.float32)
- r = proj.project(pts)
- cells = np.zeros((16, 60), np.int64)
- cells[r.rb, r.ab] = 2 # 두 점이 떨어진 칸을 moving(2)으로
- labels = proj.unproject(cells, r)
- assert labels.shape == (2,)
- assert (labels == 2).all()
diff --git a/src/perception_motionbev_pkg/test/test_pose_util.py b/src/perception_motionbev_pkg/test/test_pose_util.py
deleted file mode 100755
index 498e208..0000000
--- a/src/perception_motionbev_pkg/test/test_pose_util.py
+++ /dev/null
@@ -1,22 +0,0 @@
-import numpy as np
-from perception_motionbev_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])
diff --git a/src/perception_motionbev_pkg/test/test_residual_lib.py b/src/perception_motionbev_pkg/test/test_residual_lib.py
deleted file mode 100755
index ac646b9..0000000
--- a/src/perception_motionbev_pkg/test/test_residual_lib.py
+++ /dev/null
@@ -1,21 +0,0 @@
-import numpy as np
-from perception_motionbev_pkg import residual_lib
-from perception_motionbev_pkg.polar_projection import PolarBEVProjector
-
-
-def _proj():
- return PolarBEVProjector(n_range=48, n_angle=180, max_range=50.0, min_range=2.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 == (48, 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] = 8.0
- res = residual_lib.compute_residual(pts, pts, T, _proj())
- assert np.nanmax(np.abs(res)) > 0.0
diff --git a/src/perception_motionbev_pkg/test/test_resolve_model.py b/src/perception_motionbev_pkg/test/test_resolve_model.py
deleted file mode 100755
index 19075d8..0000000
--- a/src/perception_motionbev_pkg/test/test_resolve_model.py
+++ /dev/null
@@ -1,72 +0,0 @@
-from perception_motionbev_pkg import resolve_model
-
-PRE_CKPT = "/opt/motionbev_pretrained/MotionBEV"
-PRE_ARCH = "/opt/x/MotionBEV-semantickitti.yaml"
-PRE_DATA = "/opt/x/semantic-kitti-mos.yaml"
-
-
-def _make_finetuned(tmp_path):
- ft = tmp_path / "motionbev_finetuned"
- ft.mkdir()
- (ft / "MotionBEV_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_finetuned_when_present(tmp_path):
- ft = _make_finetuned(tmp_path)
- r = resolve_model.resolve(finetuned_dir=str(ft), pretrained_ckpt=PRE_CKPT,
- pretrained_arch=PRE_ARCH, pretrained_data=PRE_DATA)
- assert r["source"] == "finetuned"
- assert r["ckpt"] == str(ft / "MotionBEV_valid_best")
- assert r["arch_cfg"] == str(ft / "arch_cfg.yaml")
-
-
-def test_pretrained_fallback_when_absent(tmp_path):
- ft = tmp_path / "motionbev_finetuned"
- r = resolve_model.resolve(finetuned_dir=str(ft), pretrained_ckpt=PRE_CKPT,
- pretrained_arch=PRE_ARCH, pretrained_data=PRE_DATA)
- assert r["source"] == "pretrained"
- assert r["ckpt"] == PRE_CKPT
- assert r["arch_cfg"] == PRE_ARCH
-
-
-def test_partial_finetuned_falls_back(tmp_path):
- ft = tmp_path / "motionbev_finetuned"; ft.mkdir()
- (ft / "MotionBEV_valid_best").write_bytes(b"x")
- r = resolve_model.resolve(finetuned_dir=str(ft), pretrained_ckpt=PRE_CKPT,
- pretrained_arch=PRE_ARCH, pretrained_data=PRE_DATA)
- assert r["source"] == "pretrained"
-
-
-def test_model_source_pretrained_forces_baked_even_if_finetuned_present(tmp_path):
- ft = _make_finetuned(tmp_path)
- r = resolve_model.resolve(model_source="pretrained", finetuned_dir=str(ft),
- pretrained_ckpt=PRE_CKPT, pretrained_arch=PRE_ARCH,
- pretrained_data=PRE_DATA)
- assert r["source"] == "pretrained"
- assert r["ckpt"] == PRE_CKPT
-
-
-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=PRE_CKPT, pretrained_arch=PRE_ARCH,
- pretrained_data=PRE_DATA)
- assert r["source"] == "finetuned"
- assert r["ckpt"] == str(ft / "MotionBEV_valid_best")
-
-
-def test_model_source_finetuned_silent_fallback_when_absent(tmp_path):
- ft = tmp_path / "motionbev_finetuned"
- r = resolve_model.resolve(model_source="finetuned", finetuned_dir=str(ft),
- pretrained_ckpt=PRE_CKPT, pretrained_arch=PRE_ARCH,
- pretrained_data=PRE_DATA)
- assert r["source"] == "pretrained"
-
-
-def test_resolve_no_args_backward_compat():
- r = resolve_model.resolve()
- assert r["source"] == "pretrained"
- assert r["ckpt"] == resolve_model.DEFAULT_PRE_CKPT
diff --git a/src/perception_motionbev_pkg/test/test_scan_buffer.py b/src/perception_motionbev_pkg/test/test_scan_buffer.py
deleted file mode 100755
index ee26837..0000000
--- a/src/perception_motionbev_pkg/test/test_scan_buffer.py
+++ /dev/null
@@ -1,37 +0,0 @@
-import numpy as np
-from perception_motionbev_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
diff --git a/src/perception_overlaptransformer_pkg/config/ot.yaml b/src/perception_overlaptransformer_pkg/config/ot.yaml
deleted file mode 100755
index 34771b5..0000000
--- a/src/perception_overlaptransformer_pkg/config/ot.yaml
+++ /dev/null
@@ -1,19 +0,0 @@
-overlaptransformer:
- ros__parameters:
- # 맵 DB(map.npz)는 필터된 스캔(=RTABMap .db 매핑 입력 = /lidar_filtered)으로 빌드되므로,
- # 쿼리도 같은 /lidar_filtered 를 써야 분포가 일치해 매칭이 맞는다. /raw/lidar(z 6.6m·88m raw)는
- # 분포가 달라 descriptor 불일치 → 오측위. (FR09 실측: raw 11622pts/88m vs filtered 9572pts/41m)
- input_topic: "/lidar_filtered"
- model_ckpt_path: ""
- map_db_path: ""
- match_dist_thresh: 1.0
- # range 투영 파라미터 (모델 OverlapTransformer 가 기대하는 입력 = KITTI Velodyne FOV).
- # 센서가 아니라 모델 학습값에 맞춘다. 잘못 주면 descriptor 붕괴(FR09 Ouster 16% 사례).
- # build_map_db(맵 빌드)와 ot_node(query)가 같은 값을 써야 매칭이 맞음 (run.sh 가 이 파일로 통일).
- proj_H: 64
- proj_W: 900
- fov_up: 3.0
- fov_down: -25.0
- max_range: 50.0
- min_range: 1.0
- use_sim_time: true
diff --git a/src/perception_overlaptransformer_pkg/launch/ot.launch.py b/src/perception_overlaptransformer_pkg/launch/ot.launch.py
deleted file mode 100755
index 3eebe2f..0000000
--- a/src/perception_overlaptransformer_pkg/launch/ot.launch.py
+++ /dev/null
@@ -1,161 +0,0 @@
-# File: ot.launch.py
-# Description:
-# Place Recognition (OverlapTransformer) launch — 단독 deploy run.sh 의 모델/맵 DB
-# 해석 로직을 OpaqueFunction 으로 이식 (통합 perception ws 편입, 단일 출처).
-#
-# resolve 의미론(무회귀 원칙):
-# - model_ckpt_path/map_db_path 가 명시되면(선언 기본값 '' 과 다르면) 그대로 사용
-# → 단독 deploy run.sh 가 model_ckpt_path:=$CKPT map_db_path:=$MAPDB 로
-# 명시 전달하는 기존 동작이 그대로 이긴다.
-# - 미명시(기본값/빈값)일 때만 model_source(기본 finetuned, 07-model-source-switch 계약)
-# + finetuned 파일 존재 검사로 자동 resolve. map.npz 는 /selected_map/*.db(RTABMap)
-# → sequences/00 순으로 자동 빌드(staleness 태그로 재빌드 판단).
-import glob
-import os
-import subprocess
-import sys
-
-import yaml
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
-# 컨테이너 내부 컨벤션 경로 (perception 공통: /perception_data 마운트·/opt baked·/selected_map 마운트)
-# 가중치 공유 단일 위치(docker/local): /perception_models/overlaptransformer/{finetuned,pretrained}
-# (호스트 .caches/config/nav2_config/perception/models/overlaptransformer/*, SWRule config_mounts).
-FINETUNED_CKPT = '/perception_models/overlaptransformer/finetuned/overlaptransformer.pth.tar'
-SHARED_PRE_CKPT = '/perception_models/overlaptransformer/pretrained/overlaptransformer.pth.tar' # 공유 pretrained (Phase 2 채움)
-PRETRAINED_CKPT = '/opt/ot_pretrained/overlaptransformer.pth.tar' # 이미지 baked 안전망(Dockerfile COPY)
-MAP_DB = '/perception_data/map_descriptors/map.npz'
-MAP_SRC = MAP_DB + '.src' # 빌드된 map.npz 의 소스(.db+모델) 식별 → 교체 감지
-MAP_SEQ = '/perception_data/pr/sequences/00' # fallback: 수집 시퀀스(2D/ride-along)
-SELECTED_MAP_DIR = '/selected_map' # 선택 맵 .db (config_mounts 로 마운트)
-
-ARGUMENTS = [
- DeclareLaunchArgument('use_sim_time', default_value='true'),
- DeclareLaunchArgument('model_ckpt_path', default_value=''),
- DeclareLaunchArgument('map_db_path', default_value=''),
- # UI 스위치(07 계약): pretrained 면 finetuned 있어도 baked 강제, 그 외 finetuned 우선
- DeclareLaunchArgument('model_source', default_value='finetuned'),
-]
-
-
-def _resolve_ckpt(model_source):
- """finetuned(재학습 산출물) → 공유 pretrained → baked pretrained(/opt 안전망) 3단 fallback.
- model_source=='pretrained' 면 finetuned 건너뛰고 pretrained(공유 우선, 없으면 baked) 강제."""
- if model_source != 'pretrained' and os.path.isfile(FINETUNED_CKPT):
- print(f'[overlaptransformer] 재학습 모델 사용: {FINETUNED_CKPT}')
- return FINETUNED_CKPT
- if os.path.isfile(SHARED_PRE_CKPT):
- print(f'[overlaptransformer] 공유 pretrained 사용: {SHARED_PRE_CKPT}')
- return SHARED_PRE_CKPT
- print(f'[overlaptransformer] base pretrained(baked) 사용: {PRETRAINED_CKPT}')
- return PRETRAINED_CKPT
-
-
-def _proj_args(params_file):
- """투영 파라미터를 ot.yaml(단일 소스, SWRule override) 에서 읽어 맵 빌드에 적용.
- query 노드(ot_node)도 같은 ot.yaml 을 쓰므로 빌드/쿼리 투영이 일치한다(매칭 정합 필수)."""
- with open(params_file) as f:
- p = yaml.safe_load(f)['overlaptransformer']['ros__parameters']
- return ['--proj_H', str(p.get('proj_H', 64)), '--proj_W', str(p.get('proj_W', 900)),
- '--fov_up', str(p.get('fov_up', 3.0)), '--fov_down', str(p.get('fov_down', -25.0)),
- '--max_range', str(p.get('max_range', 50.0)), '--min_range', str(p.get('min_range', 1.0))]
-
-
-def _script_dir():
- """db_to_mapdb/build_map_db 가 든 pkg python 모듈 디렉토리 (run.sh 의 find src 대체)."""
- import perception_overlaptransformer_pkg as _m
- return os.path.dirname(os.path.abspath(_m.__file__))
-
-
-def _mtime(path):
- """stat -c %Y 동치 (없으면 0) — staleness 태그용."""
- try:
- return int(os.path.getmtime(path))
- except OSError:
- return 0
-
-
-def _resolve_map_db(ckpt, params_file):
- """map.npz 자동 빌드 (run.sh 로직 이식). 선택 맵 .db 우선(맵/모델 바뀌면 재빌드),
- 없으면 수집 시퀀스(00) fallback. 끝까지 없으면 '' 반환(노드가 reloc 비활성 경고)."""
- try:
- pydir = _script_dir()
- except ImportError:
- print('[overlaptransformer] 에러: pkg 모듈(db_to_mapdb/build_map_db) 미발견 — map.npz 자동 빌드 생략')
- return MAP_DB if os.path.isfile(MAP_DB) else ''
- proj = _proj_args(params_file)
- dbs = sorted(glob.glob(os.path.join(SELECTED_MAP_DIR, '*.db')))
- selected_db = dbs[0] if dbs else ''
- # 맵 DB(map.npz) 자동 빌드: 선택 맵 .db 가 마운트돼 있으면 그걸로 빌드(맵/모델 바뀌면 재빌드).
- # .db 안의 Admin.opt_poses(최적화 pose)+Data.scan(/lidar_filtered 분포)을 직접 디코드 → rtabmap 불필요.
- if selected_db:
- # staleness 태그 = ".db 경로+mtime + ckpt 경로+mtime" — run.sh 가 쓴 map.npz.src 와 포맷 호환
- src_tag = '%s %d %s %d' % (selected_db, _mtime(selected_db), ckpt, _mtime(ckpt))
- old_tag = ''
- if os.path.isfile(MAP_SRC):
- with open(MAP_SRC) as f:
- old_tag = f.read().rstrip('\n')
- if not os.path.isfile(MAP_DB) or old_tag != src_tag:
- print(f"[overlaptransformer] 맵 DB 자동 빌드 (.db→map.npz): {selected_db} ({' '.join(proj)})")
- r = subprocess.run([sys.executable, os.path.join(pydir, 'db_to_mapdb.py'),
- ckpt, selected_db, MAP_DB] + proj, cwd=pydir)
- if r.returncode == 0:
- os.makedirs(os.path.dirname(MAP_SRC), exist_ok=True)
- with open(MAP_SRC, 'w') as f:
- f.write(src_tag + '\n')
- else:
- print('[overlaptransformer] 맵 DB 자동 빌드 실패(계속)')
- else:
- print('[overlaptransformer] map.npz 최신 (선택 맵/모델 변경 없음) — 재빌드 생략')
- # fallback: .db 미마운트 시 수집 시퀀스(00, 2D/ride-along)로 빌드
- elif (not os.path.isfile(MAP_DB) and os.path.isdir(os.path.join(MAP_SEQ, 'velodyne'))
- and os.path.isfile(os.path.join(MAP_SEQ, 'poses.txt'))):
- print(f"[overlaptransformer] 맵 DB 빌드 (sequences/00→map.npz) ({' '.join(proj)})")
- r = subprocess.run([sys.executable, os.path.join(pydir, 'build_map_db.py'), ckpt,
- os.path.join(MAP_SEQ, 'velodyne'), os.path.join(MAP_SEQ, 'poses.txt'),
- MAP_DB] + proj, cwd=pydir)
- if r.returncode != 0:
- print('[overlaptransformer] 맵 DB 빌드 실패(계속)')
- if os.path.isfile(MAP_DB):
- return MAP_DB
- # run.sh 동작 보존: 맵 소스가 없어도 launch 는 계속 — 노드가 "model/DB 미설정" 경고 후 비활성
- print('[overlaptransformer] 에러: 맵 소스 없음 (/selected_map/*.db·sequences/00 모두 부재) — relocalization 비활성')
- return ''
-
-
-def launch_setup(context, *args, **kwargs):
- pkg = get_package_share_directory('perception_overlaptransformer_pkg')
- params_file = os.path.join(pkg, 'config', 'ot.yaml')
- ckpt = LaunchConfiguration('model_ckpt_path').perform(context)
- map_db = LaunchConfiguration('map_db_path').perform(context)
- model_source = LaunchConfiguration('model_source').perform(context)
- # 명시 인자(기본값 '' 아님)는 그대로 — 단독 deploy run.sh 의 명시 전달 무회귀
- if not ckpt:
- ckpt = _resolve_ckpt(model_source)
- if not map_db:
- map_db = _resolve_map_db(ckpt, params_file)
- remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
- load_nodes = GroupAction(actions=[
- Node(
- package='perception_overlaptransformer_pkg', executable='overlaptransformer',
- name='overlaptransformer', output='screen',
- parameters=[params_file, {
- 'use_sim_time': LaunchConfiguration('use_sim_time'),
- 'model_ckpt_path': ckpt,
- 'map_db_path': map_db,
- }],
- remappings=remappings,
- )
- ])
- return [load_nodes]
-
-
-def generate_launch_description():
- ld = LaunchDescription(ARGUMENTS)
- ld.add_action(OpaqueFunction(function=launch_setup))
- return ld
diff --git a/src/perception_overlaptransformer_pkg/launch/perception.launch.py b/src/perception_overlaptransformer_pkg/launch/perception.launch.py
deleted file mode 100755
index 7fec305..0000000
--- a/src/perception_overlaptransformer_pkg/launch/perception.launch.py
+++ /dev/null
@@ -1,32 +0,0 @@
-"""perception_manager 공통 진입점 (통합 perception 모듈).
-
-LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로
-`ros2 launch perception_overlaptransformer_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..`
-를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(ot.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_overlaptransformer_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', 'ot.launch.py')),
- launch_arguments={
- 'robot': LaunchConfiguration('robot'),
- 'use_sim_time': LaunchConfiguration('use_sim_time'),
- 'model_source': LaunchConfiguration('model_source'),
- }.items(),
- ),
- ])
diff --git a/src/perception_overlaptransformer_pkg/package.xml b/src/perception_overlaptransformer_pkg/package.xml
deleted file mode 100755
index 6cf8767..0000000
--- a/src/perception_overlaptransformer_pkg/package.xml
+++ /dev/null
@@ -1,25 +0,0 @@
-
-
-
- perception_overlaptransformer_pkg
- 0.0.0
- LiDAR place recognition (OverlapTransformer) perception module
- dgist
- TODO: License declaration
-
- rclpy
- sensor_msgs
- sensor_msgs_py
- geometry_msgs
- std_msgs
- visualization_msgs
-
- ament_copyright
- ament_flake8
- ament_pep257
- python3-pytest
-
-
- ament_python
-
-
diff --git a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/__init__.py b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/__init__.py
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/build_map_db.py b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/build_map_db.py
deleted file mode 100755
index b7bb91c..0000000
--- a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/build_map_db.py
+++ /dev/null
@@ -1,35 +0,0 @@
-#!/usr/bin/env python3
-"""맵 descriptor DB 빌드 one-shot (spec §7). 맵 시퀀스(velodyne+poses) → map.npz.
-deploy run.sh 가 DB 없을 때 수집 시퀀스로 1회 호출. flat import 라 이 파일이 있는 dir 에서 실행.
-투영 파라미터는 query 노드(ot_node)와 반드시 같아야 매칭이 맞으므로, run.sh 가 ot.yaml 의 값을
---fov_up 등으로 넘긴다(미지정 시 모델 학습값 KITTI 기본). 하드코딩 금지.
-사용: python build_map_db.py [--fov_up .. --fov_down .. ...]"""
-import argparse
-from ot_backend import OTBackend
-from descriptor_db import build_db
-
-
-def _is_cuda_error(e):
- s = str(e).lower()
- return 'cuda' in s or 'cudnn' in s or 'out of memory' in s
-
-
-if __name__ == '__main__':
- ap = argparse.ArgumentParser()
- ap.add_argument('ckpt'); ap.add_argument('scan_dir'); ap.add_argument('poses'); ap.add_argument('out')
- ap.add_argument('--proj_H', type=int, default=64); ap.add_argument('--proj_W', type=int, default=900)
- ap.add_argument('--fov_up', type=float, default=3.0); ap.add_argument('--fov_down', type=float, default=-25.0)
- ap.add_argument('--max_range', type=float, default=50.0); ap.add_argument('--min_range', type=float, default=1.0)
- ap.add_argument('--device', default='auto') # auto: cuda 가용 시 GPU, 아니면 CPU (08-gpu-cpu-runtime N3/N4)
- a = ap.parse_args()
- try:
- be = OTBackend(a.ckpt, device=a.device, proj_H=a.proj_H, proj_W=a.proj_W, fov_up=a.fov_up,
- fov_down=a.fov_down, max_range=a.max_range, min_range=a.min_range)
- except Exception as e:
- if a.device == 'cpu' or not _is_cuda_error(e):
- raise
- print(f"[map_db] 경고: GPU 백엔드 초기화 실패({e}) → CPU 폴백")
- be = OTBackend(a.ckpt, device='cpu', proj_H=a.proj_H, proj_W=a.proj_W, fov_up=a.fov_up,
- fov_down=a.fov_down, max_range=a.max_range, min_range=a.min_range)
- shape = build_db(be, a.scan_dir, a.poses, a.out)
- print(f"[map_db] built {a.out} {shape} (fov_up={a.fov_up} fov_down={a.fov_down})")
diff --git a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/db_to_mapdb.py b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/db_to_mapdb.py
deleted file mode 100755
index 967ad19..0000000
--- a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/db_to_mapdb.py
+++ /dev/null
@@ -1,113 +0,0 @@
-#!/usr/bin/env python3
-"""선택된 3D slam_map(RTABMap .db) → 맵 descriptor DB(map.npz) 직접 빌드 (자동 빌드 경로).
-
-deploy run.sh 가 map.npz 없을 때(또는 선택 맵이 바뀌었을 때) 호출한다. 기존 흐름
-(rtabmap-export → g2o → sequences/00 → build_map_db.py)은 rtabmap 바이너리가 필요했으나,
-deploy 이미지엔 rtabmap 이 없다. 대신 .db 안의 두 BLOB 을 직접 디코드해 한 방에 만든다:
- - 최적화 pose: Admin.opt_ids + Admin.opt_poses (둘 다 zlib). loop-closure 보정된 그래프 pose.
- → ot_node 가 발행하는 측위 좌표가 nav2 가 쓰는 .db 기하맵과 동일 프레임이 됨.
- - 스캔: Data.scan (zlib, XYZ float32). RTABMap 이 저장한 필터/등록된 스캔 = /lidar_filtered
- 분포와 일치 → query 와 같은 분포라 descriptor 가 맞는다.
-node id 로 둘을 매칭 → OTBackend.describe → (M,D) descriptors + (M,12) poses → map.npz.
-
-투영 파라미터는 query 노드(ot_node)와 반드시 같아야 매칭이 맞으므로 run.sh 가 ot.yaml 값을
---fov_up 등으로 넘긴다(미지정 시 모델 학습값 KITTI 기본). 하드코딩 금지.
-사용: python db_to_mapdb.py [--fov_up .. --fov_down .. ...]"""
-import argparse
-import os
-import sqlite3
-import sys
-import zlib
-
-import numpy as np
-
-sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) # flat import (colcon entry 와 동일)
-from ot_backend import OTBackend
-
-
-def _is_cuda_error(e):
- s = str(e).lower()
- return 'cuda' in s or 'cudnn' in s or 'out of memory' in s
-
-
-def load_opt_poses(db_path):
- """Admin.opt_ids(zlib int32) + opt_poses(zlib float32, id당 3x4=12) → {node_id: (12,) float32}."""
- c = sqlite3.connect(db_path)
- try:
- cur = c.cursor()
- cur.execute("SELECT opt_ids, opt_poses FROM Admin ORDER BY time_enter DESC LIMIT 1")
- row = cur.fetchone()
- finally:
- c.close()
- if not row or row[0] is None or row[1] is None:
- raise SystemExit("[db2map] 에러: Admin.opt_poses 없음 — 최적화 안 된 .db (SLAM 미완료?)")
- ids = np.frombuffer(zlib.decompress(row[0]), dtype=np.int32)
- poses = np.frombuffer(zlib.decompress(row[1]), dtype=np.float32).reshape(len(ids), 12)
- return {int(i): poses[k].copy() for k, i in enumerate(ids)}
-
-
-def load_scans(db_path):
- """Data.scan(zlib XYZ float32) → {node_id: (N,4) xyzi}. intensity 0 (OT=range만 써 무관)."""
- c = sqlite3.connect(db_path)
- out = {}
- try:
- cur = c.cursor()
- cur.execute("SELECT id, scan FROM Data WHERE scan IS NOT NULL AND length(scan)>0 ORDER BY id")
- for nid, blob in cur.fetchall():
- raw = zlib.decompress(blob)
- nf = len(raw) // 4
- if nf % 3 == 0:
- pts = np.frombuffer(raw, dtype=np.float32, count=nf).reshape(-1, 3)
- elif nf % 4 == 0:
- pts = np.frombuffer(raw, dtype=np.float32, count=nf).reshape(-1, 4)[:, :3]
- else:
- print(f"[db2map] 경고: node {nid} scan 포맷 불명({nf} floats) — 스킵")
- continue
- xyzi = np.zeros((pts.shape[0], 4), dtype=np.float32)
- xyzi[:, :3] = pts
- out[int(nid)] = xyzi
- finally:
- c.close()
- return out
-
-
-def main():
- ap = argparse.ArgumentParser()
- ap.add_argument('ckpt')
- ap.add_argument('db', help='선택된 RTABMap .db (3D slam_map)')
- ap.add_argument('out', help='출력 map.npz')
- ap.add_argument('--proj_H', type=int, default=64)
- ap.add_argument('--proj_W', type=int, default=900)
- ap.add_argument('--fov_up', type=float, default=3.0)
- ap.add_argument('--fov_down', type=float, default=-25.0)
- ap.add_argument('--max_range', type=float, default=50.0)
- ap.add_argument('--min_range', type=float, default=1.0)
- ap.add_argument('--device', default='auto') # auto: cuda 가용 시 GPU, 아니면 CPU (08-gpu-cpu-runtime N3/N4)
- a = ap.parse_args()
-
- poses = load_opt_poses(a.db)
- scans = load_scans(a.db)
- ids = sorted(set(poses) & set(scans))
- print(f"[db2map] opt_poses={len(poses)} scans={len(scans)} aligned={len(ids)}")
- if not ids:
- raise SystemExit("[db2map] 에러: pose∩scan 교집합 0 — 손상된 .db")
-
- try:
- be = OTBackend(a.ckpt, device=a.device, proj_H=a.proj_H, proj_W=a.proj_W, fov_up=a.fov_up,
- fov_down=a.fov_down, max_range=a.max_range, min_range=a.min_range)
- except Exception as e:
- if a.device == 'cpu' or not _is_cuda_error(e):
- raise
- print(f"[db2map] 경고: GPU 백엔드 초기화 실패({e}) → CPU 폴백")
- be = OTBackend(a.ckpt, device='cpu', proj_H=a.proj_H, proj_W=a.proj_W, fov_up=a.fov_up,
- fov_down=a.fov_down, max_range=a.max_range, min_range=a.min_range)
- descs = np.stack([be.describe(scans[i]) for i in ids]).astype(np.float32)
- pose_arr = np.stack([poses[i] for i in ids]).astype(np.float32) # (M,12) 3x4 row-major
- os.makedirs(os.path.dirname(a.out), exist_ok=True)
- np.savez(a.out, descriptors=descs, poses=pose_arr)
- print(f"[db2map] built {a.out} desc={descs.shape} poses={pose_arr.shape} "
- f"(fov_up={a.fov_up} fov_down={a.fov_down})")
-
-
-if __name__ == '__main__':
- main()
diff --git a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/descriptor_db.py b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/descriptor_db.py
deleted file mode 100755
index 7a6f0f9..0000000
--- a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/descriptor_db.py
+++ /dev/null
@@ -1,33 +0,0 @@
-"""맵 descriptor DB 빌드/검색 (spec §7). 맵 시퀀스(velodyne+poses) → descriptor 행렬 + pose 배열.
-검색: 현재 descriptor 와 L2 최근접 → matched 프레임 idx → 그 프레임 pose. faiss-cpu."""
-import os
-import numpy as np
-
-
-def build_db(backend, scan_dir, poses_path, out_npz):
- """scan_dir/*.bin 전부 describe → (M,D) descriptors + (M,12) poses → npz 저장."""
- bins = sorted(f for f in os.listdir(scan_dir) if f.endswith('.bin'))
- descs = []
- for b in bins:
- pts = np.fromfile(os.path.join(scan_dir, b), dtype=np.float32).reshape(-1, 4)
- descs.append(backend.describe(pts))
- descs = np.stack(descs).astype(np.float32)
- poses = np.loadtxt(poses_path).reshape(-1, 12).astype(np.float32)
- os.makedirs(os.path.dirname(out_npz), exist_ok=True)
- np.savez(out_npz, descriptors=descs, poses=poses)
- return descs.shape
-
-
-class DescriptorDB:
- def __init__(self, npz_path):
- import faiss
- z = np.load(npz_path)
- self.descs = z['descriptors'].astype(np.float32)
- self.poses = z['poses']
- self.index = faiss.IndexFlatL2(self.descs.shape[1])
- self.index.add(self.descs)
-
- def query(self, descriptor, topk=5):
- """descriptor (D,) → (best_idx, best_dist, topk_idxs). pose 는 self.poses[idx]."""
- d, i = self.index.search(descriptor.reshape(1, -1), topk)
- return int(i[0, 0]), float(d[0, 0]), i[0].tolist()
diff --git a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/ot_backend.py b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/ot_backend.py
deleted file mode 100755
index a12d262..0000000
--- a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/ot_backend.py
+++ /dev/null
@@ -1,43 +0,0 @@
-"""OverlapTransformer descriptor 추출 backend (torch 격리, lazy import).
-scan (N,4)[x,y,z,i] → range image → featureExtracter → global descriptor (D,).
-salsanext_backend.py 의 구조 미러. ckpt 는 DataParallel 아님(NOTES §f) → module. strip 불필요."""
-import os
-import sys
-import numpy as np
-from range_projection import RangeProjector
-
-
-class OTBackend:
- # 투영 기본값은 모델(OverlapTransformer)이 학습된 KITTI Velodyne 의 수직 FOV(≈+2°/−24.8°)에
- # 맞춘다. 이 값은 "센서"가 아니라 "모델"이 기대하는 입력 레이아웃이라, 잘못 주면(예: ±21.2°)
- # 스캔이 range image 의 엉뚱한 행대에 뭉쳐 descriptor 가 붕괴한다(FR09 Ouster 16% 사례).
- # 하드코딩 금지 — 호출부(ot_node gp / build_map_db CLI / ot.yaml SWRule)가 Rule 값으로 덮어쓴다.
- def __init__(self, ckpt_path, device='cuda:0', proj_H=64, proj_W=900,
- fov_up=3.0, fov_down=-25.0, max_range=50.0, min_range=1.0):
- import torch
- if device == 'auto':
- device = 'cuda:0' if torch.cuda.is_available() else 'cpu'
- ot_root = os.environ.get('OT_ROOT', '/opt/OverlapTransformer')
- # overlap_transformer.py 가 모듈 로드 시 tools.read_samples → utils.utils 를 끌어와
- # ot_root 와 ot_root/tools 도 path 에 있어야 한다 (실 GPU 검증서 확인). modules 만으론 ImportError.
- for _p in (ot_root, os.path.join(ot_root, 'tools'), os.path.join(ot_root, 'modules')):
- if _p not in sys.path:
- sys.path.insert(0, _p)
- from overlap_transformer import featureExtracter # NOTES §a 모델 (descriptor dim=256)
- self.proj = RangeProjector(proj_H, proj_W, fov_up, fov_down, max_range, min_range)
- self.device = device
- self._t = torch
- model = featureExtracter(channels=1, use_transformer=True)
- state = torch.load(ckpt_path, map_location='cpu')
- model.load_state_dict(state.get('state_dict', state), strict=True) # module. 없음
- self._model = model.to(device).eval()
-
- def describe(self, points):
- """(N,4) → (D,) float32 L2-정규화 descriptor."""
- pr = self.proj.project(points)
- depth = pr.range_image[0:1].copy() # (1,H,W) range 채널만 (OT 입력=depth 1ch)
- with self._t.no_grad():
- inp = self._t.from_numpy(depth).unsqueeze(0).to(self.device) # (1,1,H,W)
- d = self._model(inp).cpu().numpy().reshape(-1).astype(np.float32)
- n = np.linalg.norm(d) + 1e-9
- return d / n
diff --git a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/ot_node.py b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/ot_node.py
deleted file mode 100755
index 79e0e0b..0000000
--- a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/ot_node.py
+++ /dev/null
@@ -1,135 +0,0 @@
-"""Place Recognition deploy 노드. /raw/lidar → descriptor → 맵 DB 최근접 → 현재 pose 추정 →
-/perception/relocalization(PoseWithCovarianceStamped) + /initialpose(AMCL 씨딩, 접근 A) +
-/perception/reloc_markers(★pose+top-k) 발행. (place_match_image 는 §8, best-effort 후속.)"""
-import os
-import sys
-# colcon 설치된 entry point 로 실행되면 같은 패키지의 flat 모듈(ot_backend/descriptor_db/range_projection)이
-# top-level 로 안 잡혀 ModuleNotFoundError → 자기 디렉토리를 path 에 추가해 flat import 를 살린다.
-sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
-import numpy as np
-import rclpy
-from rclpy.node import Node
-from sensor_msgs.msg import PointCloud2
-from geometry_msgs.msg import PoseWithCovarianceStamped
-from visualization_msgs.msg import Marker, MarkerArray
-import sensor_msgs_py.point_cloud2 as pc2
-from ot_backend import OTBackend
-from descriptor_db import DescriptorDB
-
-
-def _is_cuda_error(e):
- s = str(e).lower()
- return 'cuda' in s or 'cudnn' in s or 'out of memory' in s
-
-
-def _pose_from_row(row12):
- T = np.array(row12, dtype=np.float64).reshape(3, 4)
- return T[:, 3], T[:, :3] # translation, R
-
-
-class PlaceRecognition(Node):
- def __init__(self):
- super().__init__('overlaptransformer')
- gp = lambda n, d: self.declare_parameter(n, d).value
- self.input_topic = gp('input_topic', '/raw/lidar')
- ckpt = gp('model_ckpt_path', '') or os.environ.get('OT_MODEL_CKPT', '')
- db_path = gp('map_db_path', '') or os.environ.get('OT_MAP_DB', '')
- self.thresh = float(gp('match_dist_thresh', 1.0)) # L2 desc 거리 임계
- self.seed_once = bool(gp('seed_initialpose_once', True)) # /initialpose 1회만 씨딩(스트림 금지)
- self._seeded = False
- # range 투영 파라미터 — ot.yaml(SWRule config_mounts 로 override) 에서 읽는다. 기본값은
- # 모델 학습값(KITTI). 맵 DB(build_map_db)와 반드시 같은 값이어야 매칭이 맞음.
- proj = dict(
- proj_H=int(gp('proj_H', 64)), proj_W=int(gp('proj_W', 900)),
- fov_up=float(gp('fov_up', 3.0)), fov_down=float(gp('fov_down', -25.0)),
- max_range=float(gp('max_range', 50.0)), min_range=float(gp('min_range', 1.0)),
- )
- # device: 'auto' → 런타임 감지(cuda 가용 시 cuda:0, 아니면 cpu). CPU-first dual-runtime(08 N3/N4).
- # device 는 노드 파라미터(yaml/launch)로만 흐른다 — execute options/SWRule 로 노출 금지.
- device = gp('device', 'auto')
- self._ckpt, self._proj = ckpt, proj # 런타임 CPU 폴백 시 재생성용
- self._cpu_fallback = False # 런타임 폴백 1회 가드
- self.backend = None
- if ckpt:
- try:
- self.backend = OTBackend(ckpt, device=device, **proj)
- except Exception as e:
- if device != 'cpu' and _is_cuda_error(e):
- self.get_logger().warn(f"[overlaptransformer] GPU 백엔드 초기화 실패({e}) → CPU 폴백")
- try:
- self.backend = OTBackend(ckpt, device='cpu', **proj)
- self._cpu_fallback = True
- except Exception as e2:
- self.get_logger().error(f"[overlaptransformer] CPU 폴백 초기화도 실패: {e2}")
- else:
- self.get_logger().error(f"[overlaptransformer] 백엔드 초기화 실패: {e}")
- self.db = DescriptorDB(db_path) if db_path and os.path.exists(db_path) else None
- self.pub_reloc = self.create_publisher(PoseWithCovarianceStamped, '/perception/relocalization', 5)
- self.pub_init = self.create_publisher(PoseWithCovarianceStamped, '/initialpose', 1)
- self.pub_mk = self.create_publisher(MarkerArray, '/perception/reloc_markers', 5)
- self.create_subscription(PointCloud2, self.input_topic, self.cb, 5)
- if self.backend is None or self.db is None:
- self.get_logger().warn("model/DB 미설정 — relocalization 비활성(발행 안 함)")
-
- def cb(self, msg):
- if self.backend is None or self.db is None:
- return
- pts = np.array([[p[0], p[1], p[2], 0.0]
- for p in pc2.read_points(msg, ('x', 'y', 'z'), skip_nans=True)],
- dtype=np.float32) # intensity 무관(OT=range만). 필터클라우드는 xyz only
- if len(pts) == 0:
- return
- try:
- desc = self.backend.describe(pts)
- except Exception as e:
- if self._cpu_fallback or not _is_cuda_error(e):
- raise # CUDA 외 예외/폴백 후엔 기존 동작 유지
- self.get_logger().warn("[overlaptransformer] 추론 중 CUDA 에러 → CPU 백엔드 재구성")
- self._cpu_fallback = True
- try: # 깨진 CUDA 컨텍스트의 GPU 모델 해제
- bk, self.backend = self.backend, None
- del bk
- import torch
- torch.cuda.empty_cache()
- except Exception:
- pass
- try: # ckpt 에서 CPU 로 재생성 (.to 이동 금지)
- self.backend = OTBackend(self._ckpt, device='cpu', **self._proj)
- desc = self.backend.describe(pts) # 해당 프레임 1회 재시도
- except Exception as e2:
- self.get_logger().error(f"[overlaptransformer] CPU 폴백 재시도 실패 — 프레임 스킵: {e2}")
- return
- idx, dist, topk = self.db.query(desc, topk=5)
- if dist > self.thresh:
- return # 신뢰도 낮으면 발행 안 함
- t, R = _pose_from_row(self.db.poses[idx])
- ps = PoseWithCovarianceStamped()
- ps.header.frame_id = 'map'
- ps.header.stamp = self.get_clock().now().to_msg()
- ps.pose.pose.position.x, ps.pose.pose.position.y, ps.pose.pose.position.z = map(float, t)
- qw = float(np.sqrt(max(0.0, 1.0 + R[0, 0] + R[1, 1] + R[2, 2])) / 2.0) or 1.0
- ps.pose.pose.orientation.w = qw
- self.pub_reloc.publish(ps)
- if not self.seed_once or not self._seeded: # 접근 A: 측위 씨딩 (1회만, 스트림 금지)
- self.pub_init.publish(ps)
- self._seeded = True
- self._publish_markers(t, topk)
-
- def _publish_markers(self, t, topk):
- ma = MarkerArray()
- m = Marker()
- m.header.frame_id = 'map'; m.ns = 'reloc'; m.id = 0
- m.type = Marker.ARROW; m.action = Marker.ADD
- m.pose.position.x, m.pose.position.y = float(t[0]), float(t[1])
- m.scale.x, m.scale.y, m.scale.z = 2.0, 0.3, 0.3
- m.color.g = 1.0; m.color.a = 1.0 # ★ 초록 현재 위치
- ma.markers.append(m)
- self.pub_mk.publish(ma)
-
-
-def main():
- rclpy.init(); rclpy.spin(PlaceRecognition()); rclpy.shutdown()
-
-
-if __name__ == '__main__':
- main()
diff --git a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/range_projection.py b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/range_projection.py
deleted file mode 100755
index 3ddda2e..0000000
--- a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/range_projection.py
+++ /dev/null
@@ -1,68 +0,0 @@
-#!/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
diff --git a/src/perception_overlaptransformer_pkg/resource/perception_overlaptransformer_pkg b/src/perception_overlaptransformer_pkg/resource/perception_overlaptransformer_pkg
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_overlaptransformer_pkg/setup.cfg b/src/perception_overlaptransformer_pkg/setup.cfg
deleted file mode 100755
index 5820b7a..0000000
--- a/src/perception_overlaptransformer_pkg/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/perception_overlaptransformer_pkg
-[install]
-install_scripts=$base/lib/perception_overlaptransformer_pkg
diff --git a/src/perception_overlaptransformer_pkg/setup.py b/src/perception_overlaptransformer_pkg/setup.py
deleted file mode 100755
index 1657da6..0000000
--- a/src/perception_overlaptransformer_pkg/setup.py
+++ /dev/null
@@ -1,35 +0,0 @@
-
-import os
-from glob import glob
-from setuptools import find_packages, setup
-
-package_name = 'perception_overlaptransformer_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 place recognition (OverlapTransformer) perception module',
- license='TODO: License declaration',
- extras_require={
- 'test': [
- 'pytest',
- ],
- },
- entry_points={
- 'console_scripts': [
- 'overlaptransformer = perception_overlaptransformer_pkg.ot_node:main'
- ],
- },
-)
diff --git a/src/perception_overlaptransformer_pkg/test/test_copyright.py b/src/perception_overlaptransformer_pkg/test/test_copyright.py
deleted file mode 100755
index 97a3919..0000000
--- a/src/perception_overlaptransformer_pkg/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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'
diff --git a/src/perception_overlaptransformer_pkg/test/test_flake8.py b/src/perception_overlaptransformer_pkg/test/test_flake8.py
deleted file mode 100755
index 27ee107..0000000
--- a/src/perception_overlaptransformer_pkg/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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)
diff --git a/src/perception_overlaptransformer_pkg/test/test_pep257.py b/src/perception_overlaptransformer_pkg/test/test_pep257.py
deleted file mode 100755
index b234a38..0000000
--- a/src/perception_overlaptransformer_pkg/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# 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'
diff --git a/src/perception_overlaptransformer_pkg/test/test_range_projection.py b/src/perception_overlaptransformer_pkg/test/test_range_projection.py
deleted file mode 100755
index ce0f62c..0000000
--- a/src/perception_overlaptransformer_pkg/test/test_range_projection.py
+++ /dev/null
@@ -1,29 +0,0 @@
-import numpy as np
-from perception_overlaptransformer_pkg.range_projection import RangeProjector
-
-
-def test_unproject_recovers_per_point_labels():
- # 정면 부채꼴에 흩뿌린 점들 — projection 후 픽셀 라벨을 도로(0)/객체(1)로 칠하고
- # un-projection 이 점별 라벨을 되돌리는지 (충돌 없는 sparse 케이스).
- rng = np.random.default_rng(0)
- n = 500
- xyz = rng.uniform(-20, 20, size=(n, 3)).astype(np.float32)
- xyz[:, 2] = rng.uniform(-2, 1, size=n) # 적당한 높이
- pts = np.concatenate([xyz, np.zeros((n, 1), np.float32)], axis=1)
-
- proj = RangeProjector(proj_H=64, proj_W=1024, fov_up=3.0, fov_down=-25.0,
- max_range=50.0, min_range=1.0)
- out = proj.project(pts)
- assert out.range_image.shape == (5, 64, 1024)
- assert out.px.shape[0] == out.valid.sum()
-
- # 픽셀 라벨 = (depth>10 ? 1 : 0) 으로 합성 → 점별로 같은 규칙이 복원돼야 함
- depth_img = out.range_image[0]
- pixel_labels = (depth_img > 10.0).astype(np.int64)
- labels = proj.unproject(pixel_labels, out)
- assert labels.shape == (n,)
- # valid 점들에 대해 점 depth 기준 기대 라벨과 일치
- vp = pts[out.valid][:, :3]
- vdepth = np.linalg.norm(vp, axis=1)
- expected = (vdepth > 10.0).astype(np.int64)
- assert (labels[out.valid] == expected).mean() > 0.98
diff --git a/src/perception_pkg/test/test_launch_spec.py b/src/perception_pkg/test/test_launch_spec.py
index 109d30b..5755123 100755
--- a/src/perception_pkg/test/test_launch_spec.py
+++ b/src/perception_pkg/test/test_launch_spec.py
@@ -12,18 +12,12 @@ def test_level_none_skipped():
def test_perception_entries_appended():
- # LocalLauncher buildPerceptionArg 실제 컨벤션: perception__pkg +
- # perception.launch.py 진입점 + robot/use_sim_time/model_source args
cmds = build_perception_commands({
"level": "None", "robot": "mkmini",
- "perception": [{"category": "object_detection",
- "package": "perception_pointpillars_pkg",
- "launch": "perception.launch.py",
- "args": {"robot": "mkmini", "use_sim_time": "true",
- "model_source": "finetuned"}}],
+ "perception": [{"category": "object_detection", "package": "pointpillars_pkg",
+ "launch": "perception.launch.py", "args": {"robot": "mkmini"}}],
})
- assert ["ros2", "launch", "perception_pointpillars_pkg", "perception.launch.py",
- "robot:=mkmini", "use_sim_time:=true", "model_source:=finetuned"] in cmds
+ assert ["ros2", "launch", "pointpillars_pkg", "perception.launch.py", "robot:=mkmini"] in cmds
assert len(cmds) == 1
@@ -34,6 +28,6 @@ def test_entry_without_package_skipped():
def test_level_and_perception_combined():
cmds = build_perception_commands({
"level": "3d", "robot": "mkmini",
- "perception": [{"package": "perception_pointpillars_pkg", "args": {}}],
+ "perception": [{"package": "pointpillars_pkg", "args": {}}],
})
assert len(cmds) == 2
diff --git a/src/perception_pointpillars_pkg/cfgs/custom_dataset.yaml b/src/perception_pointpillars_pkg/cfgs/custom_dataset.yaml
deleted file mode 100755
index 06ef0a7..0000000
--- a/src/perception_pointpillars_pkg/cfgs/custom_dataset.yaml
+++ /dev/null
@@ -1,58 +0,0 @@
-DATASET: 'CustomDataset'
-DATA_PATH: '/perception_data/data'
-# 우리 FR09 라이다(360°) → 대칭 range. z[-2,4] 1칸 pillar. x/y 140.8/0.16=880 (16배수)
-POINT_CLOUD_RANGE: [-51.2, -51.2, -2, 51.2, 51.2, 4]
-MAP_CLASS_TO_KITTI: {
- 'Car': 'Car',
- 'Pedestrian': 'Pedestrian',
- 'Cyclist': 'Cyclist',
-}
-DATA_SPLIT: {
- 'train': train,
- 'test': val
-}
-INFO_PATH: {
- 'train': [custom_infos_train.pkl],
- 'test': [custom_infos_val.pkl],
-}
-POINT_FEATURE_ENCODING: {
- encoding_type: absolute_coordinates_encoding,
- used_feature_list: ['x', 'y', 'z', 'intensity'],
- src_feature_list: ['x', 'y', 'z', 'intensity'],
-}
-DATA_AUGMENTOR:
- DISABLE_AUG_LIST: ['gt_sampling'] # 1차 smoke 안정화용. 검증 후 풀어 성능↑
- AUG_CONFIG_LIST:
- - NAME: gt_sampling
- USE_ROAD_PLANE: False
- DB_INFO_PATH:
- - custom_dbinfos_train.pkl
- PREPARE: {
- filter_by_min_points: ['Car:5', 'Pedestrian:5', 'Cyclist:5'],
- }
- SAMPLE_GROUPS: ['Car:15', 'Pedestrian:10', 'Cyclist:10']
- NUM_POINT_FEATURES: 4
- DATABASE_WITH_FAKELIDAR: False
- REMOVE_EXTRA_WIDTH: [0.0, 0.0, 0.0]
- LIMIT_WHOLE_SCENE: True
- - NAME: random_world_flip
- ALONG_AXIS_LIST: ['x', 'y']
- - NAME: random_world_rotation
- WORLD_ROT_ANGLE: [-0.78539816, 0.78539816]
- - NAME: random_world_scaling
- WORLD_SCALE_RANGE: [0.95, 1.05]
-DATA_PROCESSOR:
- - NAME: mask_points_and_boxes_outside_range
- REMOVE_OUTSIDE_BOXES: True
- - NAME: shuffle_points
- SHUFFLE_ENABLED: {
- 'train': True,
- 'test': False
- }
- - NAME: transform_points_to_voxels
- VOXEL_SIZE: [0.16, 0.16, 6]
- MAX_POINTS_PER_VOXEL: 32
- MAX_NUMBER_OF_VOXELS: {
- 'train': 16000,
- 'test': 40000
- }
diff --git a/src/perception_pointpillars_pkg/cfgs/pointpillar_custom.yaml b/src/perception_pointpillars_pkg/cfgs/pointpillar_custom.yaml
deleted file mode 100755
index cb093b4..0000000
--- a/src/perception_pointpillars_pkg/cfgs/pointpillar_custom.yaml
+++ /dev/null
@@ -1,119 +0,0 @@
-CLASS_NAMES: ['Car', 'Pedestrian', 'Cyclist']
-
-DATA_CONFIG:
- _BASE_CONFIG_: cfgs/dataset_configs/custom_dataset.yaml
-
-MODEL:
- NAME: PointPillar
-
- VFE:
- NAME: PillarVFE
- WITH_DISTANCE: False
- USE_ABSLOTE_XYZ: True
- USE_NORM: True
- NUM_FILTERS: [64]
-
- MAP_TO_BEV:
- NAME: PointPillarScatter
- NUM_BEV_FEATURES: 64
-
- BACKBONE_2D:
- NAME: BaseBEVBackbone
- LAYER_NUMS: [3, 5, 5]
- LAYER_STRIDES: [2, 2, 2]
- NUM_FILTERS: [64, 128, 256]
- UPSAMPLE_STRIDES: [1, 2, 4]
- NUM_UPSAMPLE_FILTERS: [128, 128, 128]
-
- DENSE_HEAD:
- NAME: AnchorHeadSingle
- CLASS_AGNOSTIC: False
-
- USE_DIRECTION_CLASSIFIER: True
- DIR_OFFSET: 0.78539
- DIR_LIMIT_OFFSET: 0.0
- NUM_DIR_BINS: 2
-
- # anchor_sizes 는 KITTI 와 동일(pretrained 전이 최대). bottom_heights 는 우리 라이다
- # 지면(z≈-1.1) 기준으로 조정. fine-tune 이 미세조정함.
- ANCHOR_GENERATOR_CONFIG: [
- {
- 'class_name': 'Car',
- 'anchor_sizes': [[3.9, 1.6, 1.56]],
- 'anchor_rotations': [0, 1.57],
- 'anchor_bottom_heights': [-1.0],
- 'align_center': False,
- 'feature_map_stride': 2,
- 'matched_threshold': 0.6,
- 'unmatched_threshold': 0.45
- },
- {
- 'class_name': 'Pedestrian',
- 'anchor_sizes': [[0.8, 0.6, 1.73]],
- 'anchor_rotations': [0, 1.57],
- 'anchor_bottom_heights': [-1.0],
- 'align_center': False,
- 'feature_map_stride': 2,
- 'matched_threshold': 0.5,
- 'unmatched_threshold': 0.35
- },
- {
- 'class_name': 'Cyclist',
- 'anchor_sizes': [[1.76, 0.6, 1.73]],
- 'anchor_rotations': [0, 1.57],
- 'anchor_bottom_heights': [-1.0],
- 'align_center': False,
- 'feature_map_stride': 2,
- 'matched_threshold': 0.5,
- 'unmatched_threshold': 0.35
- }
- ]
-
- TARGET_ASSIGNER_CONFIG:
- NAME: AxisAlignedTargetAssigner
- POS_FRACTION: -1.0
- SAMPLE_SIZE: 512
- NORM_BY_NUM_EXAMPLES: False
- MATCH_HEIGHT: False
- BOX_CODER: ResidualCoder
-
- LOSS_CONFIG:
- LOSS_WEIGHTS: {
- 'cls_weight': 1.0,
- 'loc_weight': 2.0,
- 'dir_weight': 0.2,
- 'code_weights': [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
- }
-
- POST_PROCESSING:
- RECALL_THRESH_LIST: [0.3, 0.5, 0.7]
- SCORE_THRESH: 0.1
- OUTPUT_RAW_SCORE: False
- EVAL_METRIC: kitti
- NMS_CONFIG:
- MULTI_CLASSES_NMS: False
- NMS_TYPE: nms_gpu
- NMS_THRESH: 0.01
- NMS_PRE_MAXSIZE: 4096
- NMS_POST_MAXSIZE: 500
-
-OPTIMIZATION:
- BATCH_SIZE_PER_GPU: 2
- NUM_EPOCHS: 80
-
- OPTIMIZER: adam_onecycle
- LR: 0.003
- WEIGHT_DECAY: 0.01
- MOMENTUM: 0.9
-
- MOMS: [0.95, 0.85]
- PCT_START: 0.4
- DIV_FACTOR: 10
- DECAY_STEP_LIST: [35, 45]
- LR_DECAY: 0.1
- LR_CLIP: 0.0000001
-
- LR_WARMUP: False
- WARMUP_EPOCH: 1
-
- GRAD_NORM_CLIP: 10
diff --git a/src/perception_pointpillars_pkg/config/pointpillars.yaml b/src/perception_pointpillars_pkg/config/pointpillars.yaml
deleted file mode 100755
index ffdae27..0000000
--- a/src/perception_pointpillars_pkg/config/pointpillars.yaml
+++ /dev/null
@@ -1,21 +0,0 @@
-
-
-pointpillars_pointpillars:
- ros__parameters:
- input_topic: "/raw/lidar"
- detection_topic: "/perception/detections"
- marker_topic: "/perception/detection_markers"
- # 모델 경로는 여기서 지정하거나, 비우면 환경변수
- # (POINTPILLARS_MODEL_CFG / POINTPILLARS_MODEL_CKPT)에서 읽는다.
- model_cfg_path: ""
- model_ckpt_path: ""
- score_threshold: 0.2
- device: "auto" # auto = 런타임 감지(GPU 있으면 cuda, 없으면 cpu). Phase3 dual-runtime
- # CARLA 라이다 보정 (KITTI 학습분포에 맞춤). 측정값 기반:
- # 지면 z -1.06(CARLA) vs -1.73(KITTI), ~12k 점(KITTI 대비 희박).
- z_offset: 1.0 # 입력 z 보정 (스윕상 max score 최적 ~+1.0)
- max_range: 25.0 # 이 반경(m) 밖 박스 제거 (근~중거리 신뢰)
- # 기하 검증 — 빈 공간/바닥 헛검출 제거 (당신 증상 직격)
- min_points_in_box: 5 # 박스 안 점 5개 미만 → 버림 (검정영역 헛박스)
- min_box_height: 0.5 # 박스 안 점 높이 0.5m 미만 → 버림 (평평한 도로 오검출)
- use_sim_time: true
diff --git a/src/perception_pointpillars_pkg/launch/__pycache__/pointpillars.launch.cpython-310.pyc b/src/perception_pointpillars_pkg/launch/__pycache__/pointpillars.launch.cpython-310.pyc
deleted file mode 100644
index 22406c0..0000000
Binary files a/src/perception_pointpillars_pkg/launch/__pycache__/pointpillars.launch.cpython-310.pyc and /dev/null differ
diff --git a/src/perception_pointpillars_pkg/launch/perception.launch.py b/src/perception_pointpillars_pkg/launch/perception.launch.py
deleted file mode 100755
index 1329523..0000000
--- a/src/perception_pointpillars_pkg/launch/perception.launch.py
+++ /dev/null
@@ -1,32 +0,0 @@
-"""perception_manager 공통 진입점 (통합 perception 모듈).
-
-LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로
-`ros2 launch perception_pointpillars_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..`
-를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(pointpillars.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_pointpillars_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', 'pointpillars.launch.py')),
- launch_arguments={
- 'robot': LaunchConfiguration('robot'),
- 'use_sim_time': LaunchConfiguration('use_sim_time'),
- 'model_source': LaunchConfiguration('model_source'),
- }.items(),
- ),
- ])
diff --git a/src/perception_pointpillars_pkg/launch/pointpillars.launch.py b/src/perception_pointpillars_pkg/launch/pointpillars.launch.py
deleted file mode 100755
index 966c9f5..0000000
--- a/src/perception_pointpillars_pkg/launch/pointpillars.launch.py
+++ /dev/null
@@ -1,131 +0,0 @@
-
-import os
-import shutil
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.actions import GroupAction
-from launch.actions import LogInfo
-from launch.actions import OpaqueFunction
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
-
-
-# 모델 경로 기본값 — 자동 resolve 의 "명시 인자" 판별 기준이라 상수로 둔다.
-# cfg/ckpt(pretrained) 둘 다 perception 통합 이미지에 내장됨
-# (/models 마운트는 공급자가 없어 폐기, /opt/pointpillar_pretrained 로 대체).
-DEFAULT_MODEL_CFG = '/opt/OpenPCDet/tools/cfgs/kitti_models/pointpillar.yaml'
-DEFAULT_MODEL_CKPT = '/opt/pointpillar_pretrained/pointpillar_7728.pth'
-# 가중치 공유 단일 위치(docker/local): /perception_models/pointpillars/{finetuned,pretrained}
-# (호스트 .caches/config/nav2_config/perception/models/pointpillars/*, SWRule config_mounts).
-FINETUNED_CKPT = '/perception_models/pointpillars/finetuned/pointpillar_finetuned.pth'
-SHARED_PRE_CKPT = '/perception_models/pointpillars/pretrained/pointpillar_7728.pth' # 공유 pretrained (Phase 2 채움)
-# finetuned cfg 가 OpenPCDet 트리에 놓일 위치 (_BASE_CONFIG_ 상대참조 때문에 고정).
-CUSTOM_DATASET_DST = '/opt/OpenPCDet/tools/cfgs/dataset_configs/custom_dataset.yaml'
-CUSTOM_MODEL_DST = '/opt/OpenPCDet/tools/cfgs/custom_models/pointpillar_custom.yaml'
-
-
-ARGUMENTS = [
- DeclareLaunchArgument('use_sim_time', default_value='true'),
- # 입력 토픽(/raw/lidar)은 sensor_bridge 가 robot 무관하게 정규화하므로
- # robot 별 config 분기는 두지 않는다. robot arg 는 인터페이스 호환용.
- DeclareLaunchArgument('robot', default_value='turtlebot3', choices=['turtlebot3', 'mkmini', 'fr09']),
- # 모델 소스 스위치 — pretrained 고르면 finetuned 있어도 건너뜀(기본 finetuned).
- DeclareLaunchArgument(
- 'model_source',
- default_value='finetuned', choices=['finetuned', 'pretrained']),
- # 모델 경로 — Rule(launcher 의 ROS_PARAM)이 덮어쓸 수 있게 launch arg 로 노출.
- # 기본값/빈값이면 아래 launch_setup 에서 model_source 기반 자동 resolve.
- DeclareLaunchArgument('model_cfg_path', default_value=DEFAULT_MODEL_CFG),
- DeclareLaunchArgument('model_ckpt_path', default_value=DEFAULT_MODEL_CKPT),
-]
-
-
-def launch_setup(context, *args, **kwargs):
- """모델 cfg/ckpt resolve 후 노드 생성 (단독 deploy run.sh 의 finetuned 우선 로직 이식).
-
- - 사용자가 model_cfg_path/model_ckpt_path 를 명시적으로 줬으면(선언 기본값과
- 다르면) 그대로 쓴다 — 단독 deploy run.sh 가 MODEL_ARGS 로 명시 전달하는
- 기존 동작이 그대로 이긴다(무회귀).
- - 인자가 기본값/빈값일 때만 model_source(기본 finetuned) 와 finetuned 파일
- 존재 여부로 자동 결정한다.
- """
- use_sim_time = LaunchConfiguration('use_sim_time')
- model_source = LaunchConfiguration('model_source').perform(context)
- model_cfg_path = LaunchConfiguration('model_cfg_path').perform(context)
- model_ckpt_path = LaunchConfiguration('model_ckpt_path').perform(context)
-
- pkg_pointpillars = get_package_share_directory('perception_pointpillars_pkg')
- params_file = PathJoinSubstitution([pkg_pointpillars, 'config', 'pointpillars.yaml'])
-
- cfg_explicit = model_cfg_path not in ('', DEFAULT_MODEL_CFG)
- ckpt_explicit = model_ckpt_path not in ('', DEFAULT_MODEL_CKPT)
-
- log_actions = []
- if cfg_explicit or ckpt_explicit:
- # 명시 인자 우선 — resolve 건너뜀.
- log_actions.append(LogInfo(
- msg=f'[pointpillars] 명시 모델 인자 사용: cfg[{model_cfg_path}] '
- f'ckpt[{model_ckpt_path}]'))
- elif model_source != 'pretrained' and os.path.isfile(FINETUNED_CKPT):
- # 편입된 cfgs/ 를 OpenPCDet 트리로 복사 (pointpillar_custom.yaml 의
- # _BASE_CONFIG_ 가 cfgs/dataset_configs/ 상대경로를 참조하므로 필수).
- shutil.copyfile(
- os.path.join(pkg_pointpillars, 'cfgs', 'custom_dataset.yaml'),
- CUSTOM_DATASET_DST)
- os.makedirs(os.path.dirname(CUSTOM_MODEL_DST), exist_ok=True)
- shutil.copyfile(
- os.path.join(pkg_pointpillars, 'cfgs', 'pointpillar_custom.yaml'),
- CUSTOM_MODEL_DST)
- model_cfg_path = CUSTOM_MODEL_DST
- model_ckpt_path = FINETUNED_CKPT
- log_actions.append(LogInfo(
- msg=f'[pointpillars] 재학습 모델 사용: {FINETUNED_CKPT}'))
- elif os.path.isfile(SHARED_PRE_CKPT):
- # 공유 pretrained(KITTI base) — baked(/opt) 보다 우선. cfg 는 base 와 동일.
- model_cfg_path = model_cfg_path or DEFAULT_MODEL_CFG
- model_ckpt_path = SHARED_PRE_CKPT
- log_actions.append(LogInfo(
- msg=f'[pointpillars] 공유 pretrained 사용: {SHARED_PRE_CKPT}'))
- else:
- # 빈값으로 들어온 경우도 기본 pretrained 로 보정.
- model_cfg_path = model_cfg_path or DEFAULT_MODEL_CFG
- model_ckpt_path = model_ckpt_path or DEFAULT_MODEL_CKPT
- log_actions.append(LogInfo(
- msg='[pointpillars] 재학습 모델 없음/미선택 → 기본 pretrained(baked) 사용'))
-
- remappings = [('/tf', 'tf'),
- ('/tf_static', 'tf_static')]
-
- # nav2_common.RewrittenYaml 대신 yaml + dict override 로 단순화
- # (future-pointpillars 이미지는 ros-base 만 깔려 nav2_common 이 없음).
- load_nodes = GroupAction(
- actions=[
- Node(
- package='perception_pointpillars_pkg',
- executable='pointpillars',
- name='pointpillars_pointpillars',
- output='screen',
- parameters=[
- params_file,
- {
- 'use_sim_time': use_sim_time,
- 'model_cfg_path': model_cfg_path,
- 'model_ckpt_path': model_ckpt_path,
- },
- ],
- remappings=remappings
- )
- ],
- )
-
- return log_actions + [load_nodes]
-
-
-def generate_launch_description():
- ld = LaunchDescription(ARGUMENTS)
- ld.add_action(OpaqueFunction(function=launch_setup))
-
- return ld
diff --git a/src/perception_pointpillars_pkg/package.xml b/src/perception_pointpillars_pkg/package.xml
deleted file mode 100755
index 2528b0f..0000000
--- a/src/perception_pointpillars_pkg/package.xml
+++ /dev/null
@@ -1,26 +0,0 @@
-
-
-
- perception_pointpillars_pkg
- 0.0.0
- LiDAR 3D object detection (PointPillars / OpenPCDet) perception module
- dgist
- TODO: License declaration
-
- rclpy
- sensor_msgs
- sensor_msgs_py
- vision_msgs
- visualization_msgs
- geometry_msgs
- std_msgs
-
- ament_copyright
- ament_flake8
- ament_pep257
- python3-pytest
-
-
- ament_python
-
-
diff --git a/src/perception_pointpillars_pkg/perception_pointpillars_pkg/__init__.py b/src/perception_pointpillars_pkg/perception_pointpillars_pkg/__init__.py
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_pointpillars_pkg/perception_pointpillars_pkg/pointpillars_backend.py b/src/perception_pointpillars_pkg/perception_pointpillars_pkg/pointpillars_backend.py
deleted file mode 100755
index 179946d..0000000
--- a/src/perception_pointpillars_pkg/perception_pointpillars_pkg/pointpillars_backend.py
+++ /dev/null
@@ -1,201 +0,0 @@
-#!/usr/bin/env python3
-"""OpenPCDet PointPillars 추론 래퍼.
-
-ROS 의존이 없는 순수 추론 모듈. OpenPCDet(pcdet) / torch 는 이 클래스를
-인스턴스화할 때만 import 한다 → OpenPCDet 미설치 환경에서도 패키지 build/import 는 통과.
-런타임(도커 이미지)에 OpenPCDet 가 설치돼 있어야 실제 추론이 동작한다.
-
-추론 백엔드를 노드에서 떼어내 격리했으므로, 다른 검출 모델로 교체 시
-이 파일만 갈아끼우면 된다(노드 인터페이스 불변).
-"""
-
-import os
-
-import numpy as np
-
-
-class PointPillarsBackend:
- """OpenPCDet config + checkpoint 를 로드해 단일 프레임 포인트클라우드를 추론한다.
-
- infer(points) -> (boxes, scores, labels)
- points : (N, 4) float32 — x, y, z, intensity
- boxes : (M, 7) float32 — x, y, z, dx, dy, dz, heading(yaw, rad)
- scores : (M,) float32
- labels : (M,) int — 1-based class index (class_names[label-1])
- """
-
- def __init__(self, cfg_path, ckpt_path, score_threshold=0.3, device='auto',
- z_offset=0.0, min_points_in_box=0, min_box_height=0.0,
- logger=None):
- # OpenPCDet / torch 는 여기서만 import (미설치 시 패키지 import 자체는 안 깨지게)
- import torch
- from pcdet.config import cfg, cfg_from_yaml_file
- from pcdet.models import build_network, load_data_to_gpu
- from pcdet.datasets import DatasetTemplate
- from pcdet.utils import common_utils
-
- self._torch = torch
- self._load_data_to_gpu = load_data_to_gpu
- self.score_threshold = float(score_threshold)
- # device='auto' → 런타임 감지: cuda 가용 시 cuda:0, 아니면 cpu (08-gpu-cpu-runtime N4)
- if device == 'auto':
- device = 'cuda:0' if torch.cuda.is_available() else 'cpu'
- self.device = device
- self._cpu = (device == 'cpu')
- # CPU 런타임이면 OpenPCDet 의 CUDA 하드코딩(NMS + anchor 생성 .cuda())을 CPU 로 우회
- # (Phase3 dual-runtime). spconv 미사용 모델이라 이 두 패치로 CPU 완결. build_network 전에 적용.
- if self._cpu:
- self._patch_cpu()
- # 보정: 입력 z 에 더하는 오프셋. 라이다 장착 높이/지면 z 가 KITTI 학습분포와
- # 어긋날 때 맞춘다 (CARLA 지면 ~ -1.06, KITTI ~ -1.73). 0 이면 비활성.
- self.z_offset = float(z_offset)
- # 기하 검증: 박스 안에 실제 점이 충분/입체적으로 있어야 진짜 객체로 인정.
- # min_points_in_box : 박스 내부 최소 점 개수 (빈 공간 헛검출 제거)
- # min_box_height : 박스 내부 점들의 z 스프레드 최소값(m) (평평한 바닥 오검출 제거)
- self.min_points_in_box = int(min_points_in_box)
- self.min_box_height = float(min_box_height)
-
- # OpenPCDet cfg 의 _BASE_CONFIG_ 는 tools/ 기준 상대경로라, 그 디렉토리에서 로드해야
- # dataset base config 가 해석된다. (cfg_path 자체는 절대경로 허용)
- opcdet_tools = os.path.join(
- os.environ.get('OPENPCDET_ROOT', '/opt/OpenPCDet'), 'tools')
- prev_cwd = os.getcwd()
- try:
- if os.path.isdir(opcdet_tools):
- os.chdir(opcdet_tools)
- cfg_from_yaml_file(cfg_path, cfg)
- finally:
- os.chdir(prev_cwd)
- self.class_names = list(cfg.CLASS_NAMES)
-
- log = logger or common_utils.create_logger()
-
- # 모델의 data_processor / point_feature_encoder 만 빌리기 위한 최소 dataset
- self._dataset = DatasetTemplate(
- dataset_cfg=cfg.DATA_CONFIG,
- class_names=self.class_names,
- training=False,
- root_path=None,
- logger=log,
- )
-
- self._model = build_network(
- model_cfg=cfg.MODEL,
- num_class=len(self.class_names),
- dataset=self._dataset,
- )
- self._model.load_params_from_file(
- filename=ckpt_path, logger=log, to_cpu=(device == 'cpu'))
- self._model.to(device)
- self._model.eval()
-
- def infer(self, points):
- torch = self._torch
- points = np.ascontiguousarray(points, dtype=np.float32)
-
- if self.z_offset:
- points = points.copy()
- points[:, 2] += self.z_offset
-
- data_dict = self._dataset.prepare_data(
- data_dict={'points': points, 'frame_id': 0})
- data_dict = self._dataset.collate_batch([data_dict])
- if self._cpu:
- self._load_data_to_cpu(data_dict)
- else:
- self._load_data_to_gpu(data_dict)
-
- with torch.no_grad():
- pred_dicts, _ = self._model.forward(data_dict)
-
- boxes = pred_dicts[0]['pred_boxes'].cpu().numpy()
- scores = pred_dicts[0]['pred_scores'].cpu().numpy()
- labels = pred_dicts[0]['pred_labels'].cpu().numpy()
-
- keep = scores >= self.score_threshold
- boxes, scores, labels = boxes[keep], scores[keep], labels[keep]
-
- # 기하 검증 — 박스 안 실제 점 개수/높이로 헛검출 제거 (points/boxes 동일 좌표계)
- if (self.min_points_in_box > 0 or self.min_box_height > 0.0) and len(boxes):
- valid = np.array([self._box_is_valid(points, b) for b in boxes], dtype=bool)
- boxes, scores, labels = boxes[valid], scores[valid], labels[valid]
-
- # z_offset 만큼 올려서 추론했으므로 출력 박스 z 는 원래 라이다 좌표계로 되돌림
- if self.z_offset and len(boxes):
- boxes = boxes.copy()
- boxes[:, 2] -= self.z_offset
-
- return boxes, scores, labels
-
- def _box_is_valid(self, points, box):
- """박스 내부 점 개수와 z 스프레드로 진짜 객체 여부 판정 (헛검출 컷)."""
- cx, cy, cz, dx, dy, dz, yaw = box[:7]
- rel = points[:, :3] - np.array([cx, cy, cz], dtype=np.float32)
- c, s = np.cos(-yaw), np.sin(-yaw)
- xr = rel[:, 0] * c - rel[:, 1] * s
- yr = rel[:, 0] * s + rel[:, 1] * c
- inside = (np.abs(xr) <= dx / 2) & (np.abs(yr) <= dy / 2) & \
- (np.abs(rel[:, 2]) <= dz / 2)
- n = int(inside.sum())
- if n < self.min_points_in_box:
- return False
- if self.min_box_height > 0.0:
- zin = points[inside, 2]
- if n == 0 or float(zin.max() - zin.min()) < self.min_box_height:
- return False
- return True
-
- def _load_data_to_cpu(self, batch_dict):
- """load_data_to_gpu 의 CPU 판: numpy 배열을 .cuda() 대신 cpu float tensor 로.
- 라이다 전용 파이프라인이라 camera/image 키는 없음 → voxel/point 계열만 변환."""
- torch = self._torch
- skip = ('frame_id', 'metadata', 'calib', 'image_paths', 'ori_shape',
- 'img_process_infos')
- for key, val in batch_dict.items():
- if not isinstance(val, np.ndarray) or key in skip:
- continue
- batch_dict[key] = torch.from_numpy(val).float()
-
- def _patch_cpu(self):
- """CPU 런타임용 OpenPCDet CUDA 하드코딩 2종 우회 (device=='cpu' 일 때만, build 전 1회).
-
- (1) anchor 생성·NMS 모듈이 텐서에 박은 .cuda() → torch.Tensor.cuda 를 no-op(self 반환)로
- shim. forward 경로엔 torch.cuda.*Tensor 직접 alloc 이 없어 이 shim 으로 완결
- (anchor_generator/anchor_head_template 의 build 타임 .cuda() 3곳 커버).
- (2) iou3d_nms_utils.nms_gpu(CUDA-only)를 동일결과 CPU greedy NMS 로 교체. 회전 BEV IoU 는
- OpenPCDet 가 같은 확장(iou3d_nms_cuda)에 함께 컴파일해 둔 boxes_bev_iou_cpu(C++ CPU op)를
- 그대로 호출 → GPU 판과 IoU 계산 동일(결과 동등)."""
- torch = self._torch
-
- # (1) .cuda() no-op shim — GPU 없는 클라이언트 전제(애초에 cuda 못 부름)라 전역 안전
- if not getattr(torch.Tensor, '_cpu_cuda_noop', False):
- torch.Tensor.cuda = lambda self, *a, **k: self
- torch.Tensor._cpu_cuda_noop = True
-
- # (2) CUDA NMS → CPU NMS monkeypatch (모듈 객체에 1회, getattr 로 호출 시점 해석)
- from pcdet.ops.iou3d_nms import iou3d_nms_utils as u
- if getattr(u, '_cpu_nms_patched', False):
- return
-
- def nms_cpu(boxes, scores, thresh, pre_maxsize=None, **kwargs):
- assert boxes.shape[1] == 7
- order = scores.sort(0, descending=True)[1]
- if pre_maxsize is not None:
- order = order[:pre_maxsize]
- boxes = boxes[order].contiguous()
- if boxes.shape[0] == 0:
- return order[:0].contiguous(), None
- iou = u.boxes_bev_iou_cpu(boxes[:, 0:7], boxes[:, 0:7]) # (N,N) CPU
- keep = []
- suppressed = torch.zeros(boxes.shape[0], dtype=torch.bool)
- for i in range(boxes.shape[0]):
- if suppressed[i]:
- continue
- keep.append(i)
- suppressed |= iou[i] > thresh # 자신 포함 — 이미 append 후라 무해
- keep = torch.as_tensor(keep, dtype=torch.long)
- return order[keep].contiguous(), None
-
- u.nms_gpu = nms_cpu
- u.nms_normal_gpu = nms_cpu # objdet 기본 NMS_TYPE=nms_gpu, normal 경로도 crash 방지
- u._cpu_nms_patched = True
diff --git a/src/perception_pointpillars_pkg/perception_pointpillars_pkg/pointpillars_node.py b/src/perception_pointpillars_pkg/perception_pointpillars_pkg/pointpillars_node.py
deleted file mode 100755
index 8abd8e4..0000000
--- a/src/perception_pointpillars_pkg/perception_pointpillars_pkg/pointpillars_node.py
+++ /dev/null
@@ -1,313 +0,0 @@
-#!/usr/bin/env python3
-
-import os
-import math
-
-import rclpy
-from rclpy.node import Node
-
-from sensor_msgs.msg import PointCloud2
-import sensor_msgs_py.point_cloud2 as pc2
-
-from vision_msgs.msg import Detection3DArray, Detection3D, ObjectHypothesisWithPose
-from visualization_msgs.msg import Marker, MarkerArray
-from std_msgs.msg import ColorRGBA
-
-import numpy as np
-
-from perception_pointpillars_pkg.pointpillars_backend import PointPillarsBackend
-
-
-def _yaw_to_quat(yaw):
- """z축 yaw(rad) -> (x, y, z, w) quaternion."""
- half = yaw * 0.5
- return 0.0, 0.0, math.sin(half), math.cos(half)
-
-
-def _is_cuda_error(e):
- s = str(e).lower()
- return 'cuda' in s or 'cudnn' in s or 'out of memory' in s
-
-
-class ObjectDetection(Node):
- def __init__(self):
- super().__init__('pointpillars_pointpillars')
-
- # 토픽 (구조적 기본값만 — pcd_crop 과 동일 패턴)
- self.declare_parameter('input_topic', '/raw/lidar')
- self.declare_parameter('detection_topic', '/perception/detections')
- self.declare_parameter('marker_topic', '/perception/detection_markers')
-
- # 모델 경로/임계값 — 노드에 박지 않는다. config(yaml) 또는 환경변수에서 주입.
- self.declare_parameter('model_cfg_path', '')
- self.declare_parameter('model_ckpt_path', '')
- self.declare_parameter('score_threshold', 0.3)
- # device: 'auto' → 런타임 감지(cuda 가용 시 cuda:0, 아니면 cpu). Phase3 dual-runtime.
- self.declare_parameter('device', 'auto')
-
- # 보정 파라미터 (Rule 로 조절) — 센서 도메인을 KITTI 학습분포에 맞춤.
- # z_offset: 입력 z 보정(지면 높이 차). max_range: 이 반경(m) 밖 박스 버림
- # (라이다 밀도 떨어지는 원거리 false positive 제거). 0 이면 비활성.
- self.declare_parameter('z_offset', 0.0)
- self.declare_parameter('max_range', 0.0)
- # 기하 검증 (빈 공간/바닥 헛검출 제거) — 박스 내부 점 개수/높이 최소값. 0 이면 비활성.
- self.declare_parameter('min_points_in_box', 0)
- self.declare_parameter('min_box_height', 0.0)
-
- input_topic = self.get_parameter('input_topic').value
- detection_topic = self.get_parameter('detection_topic').value
- marker_topic = self.get_parameter('marker_topic').value
-
- # config 가 비면 환경변수로 fallback (하드코딩 금지 룰)
- cfg_path = self.get_parameter('model_cfg_path').value or os.environ.get(
- 'POINTPILLARS_MODEL_CFG', '')
- ckpt_path = self.get_parameter('model_ckpt_path').value or os.environ.get(
- 'POINTPILLARS_MODEL_CKPT', '')
- score_threshold = self.get_parameter('score_threshold').value
- device = self.get_parameter('device').value
- z_offset = self.get_parameter('z_offset').value
- self.max_range = float(self.get_parameter('max_range').value)
- min_points_in_box = self.get_parameter('min_points_in_box').value
- min_box_height = self.get_parameter('min_box_height').value
-
- self.pub_det = self.create_publisher(Detection3DArray, detection_topic, 10)
- self.pub_marker = self.create_publisher(MarkerArray, marker_topic, 10)
-
- # 런타임 CUDA 폴백 가드 + ckpt 에서 CPU 백엔드 재생성에 필요한 인자 보관
- self._cpu_fallback_done = False
- self._backend_args = (cfg_path, ckpt_path, score_threshold, z_offset,
- min_points_in_box, min_box_height)
-
- self.backend = self._init_backend(
- cfg_path, ckpt_path, score_threshold, device, z_offset,
- min_points_in_box, min_box_height)
-
- self.sub = self.create_subscription(
- PointCloud2, input_topic, self.callback, 10)
-
- self.get_logger().info(
- f"ObjDet(PointPillars) started: in[{input_topic}] "
- f"det[{detection_topic}] marker[{marker_topic}] "
- f"threshold[{score_threshold}] device[{device}] "
- f"z_offset[{z_offset}] max_range[{self.max_range}]"
- )
-
- def _init_backend(self, cfg_path, ckpt_path, score_threshold, device, z_offset,
- min_points_in_box, min_box_height):
- if not cfg_path or not ckpt_path:
- self.get_logger().error(
- "model_cfg_path / model_ckpt_path 미지정 "
- "(config 또는 POINTPILLARS_MODEL_CFG/POINTPILLARS_MODEL_CKPT 환경변수). "
- "추론 비활성 — 입력 구독만 수행한다."
- )
- return None
- try:
- backend = PointPillarsBackend(
- cfg_path=cfg_path, ckpt_path=ckpt_path,
- score_threshold=score_threshold, device=device,
- z_offset=z_offset, min_points_in_box=min_points_in_box,
- min_box_height=min_box_height, logger=None,
- )
- self.get_logger().info(
- f"OpenPCDet 로드 완료: classes={backend.class_names}")
- return backend
- except Exception as e: # OpenPCDet 미설치 / 가중치 로드 실패 등
- # GPU 계열 실패(OOM/cuDNN 등)면 CPU 로 1회 재생성 (08-gpu-cpu-runtime N3)
- if device != 'cpu' and _is_cuda_error(e):
- self.get_logger().warn(
- f"[pointpillars] GPU 백엔드 초기화 실패({e}) → CPU 폴백")
- try:
- backend = PointPillarsBackend(
- cfg_path=cfg_path, ckpt_path=ckpt_path,
- score_threshold=score_threshold, device='cpu',
- z_offset=z_offset, min_points_in_box=min_points_in_box,
- min_box_height=min_box_height, logger=None,
- )
- self.get_logger().info(
- f"OpenPCDet 로드 완료(CPU 폴백): classes={backend.class_names}")
- return backend
- except Exception as e2:
- self.get_logger().error(f"검출 백엔드 초기화 실패: {e2}")
- return None
- self.get_logger().error(f"검출 백엔드 초기화 실패: {e}")
- return None
-
- def _read_points(self, msg: PointCloud2):
- field_names = {f.name for f in msg.fields}
- if 'intensity' in field_names:
- pts = pc2.read_points(
- msg, field_names=("x", "y", "z", "intensity"), skip_nans=True)
- if pts.shape[0] == 0:
- return None
- return np.stack(
- (pts["x"], pts["y"], pts["z"], pts["intensity"]),
- axis=-1).astype(np.float32, copy=False)
- # intensity 없는 라이다(예: CARLA) — 0 으로 채움
- pts = pc2.read_points(
- msg, field_names=("x", "y", "z"), skip_nans=True)
- if pts.shape[0] == 0:
- return None
- xyz = np.stack((pts["x"], pts["y"], pts["z"]), axis=-1).astype(
- np.float32, copy=False)
- intensity = np.zeros((xyz.shape[0], 1), dtype=np.float32)
- return np.concatenate([xyz, intensity], axis=1)
-
- def callback(self, msg: PointCloud2):
- if self.backend is None:
- return
-
- points = self._read_points(msg)
- if points is None:
- return
-
- try:
- boxes, scores, labels = self.backend.infer(points)
- except Exception as e:
- if self._cpu_fallback_done or not _is_cuda_error(e):
- raise
- self.get_logger().warn(
- f"[pointpillars] 추론 중 CUDA 에러 → CPU 백엔드 재구성: {e}")
- self._cpu_fallback_done = True
- # 깨진 CUDA 컨텍스트의 GPU 모델 정리 — .to('cpu') 이동 금지, ckpt 재생성
- try:
- torch = self.backend._torch
- del self.backend._model
- self.backend = None
- torch.cuda.empty_cache()
- except Exception:
- self.backend = None
- cfg_path, ckpt_path, score_threshold, z_offset, \
- min_points_in_box, min_box_height = self._backend_args
- self.backend = self._init_backend(
- cfg_path, ckpt_path, score_threshold, 'cpu', z_offset,
- min_points_in_box, min_box_height)
- if self.backend is None:
- return
- try:
- boxes, scores, labels = self.backend.infer(points)
- except Exception as e2:
- self.get_logger().error(
- f"CPU 폴백 재시도 실패 — 해당 프레임 스킵: {e2}")
- return
-
- # max_range 밖(라이다 밀도 낮은 원거리) 박스 제거 — 박스 중심 수평거리 기준
- if self.max_range > 0.0 and boxes.shape[0] > 0:
- r = np.hypot(boxes[:, 0], boxes[:, 1])
- keep = r <= self.max_range
- boxes, scores, labels = boxes[keep], scores[keep], labels[keep]
-
- self.pub_det.publish(self._to_detection_array(msg.header, boxes, scores, labels))
- self.pub_marker.publish(self._to_marker_array(msg.header, boxes, labels))
-
- def _to_detection_array(self, header, boxes, scores, labels):
- arr = Detection3DArray()
- arr.header = header
- for box, score, label in zip(boxes, scores, labels):
- x, y, z, dx, dy, dz, yaw = [float(v) for v in box]
- qx, qy, qz, qw = _yaw_to_quat(yaw)
-
- det = Detection3D()
- det.header = header
-
- hyp = ObjectHypothesisWithPose()
- hyp.hypothesis.class_id = str(int(label))
- hyp.hypothesis.score = float(score)
- det.results.append(hyp)
-
- det.bbox.center.position.x = x
- det.bbox.center.position.y = y
- det.bbox.center.position.z = z
- det.bbox.center.orientation.x = qx
- det.bbox.center.orientation.y = qy
- det.bbox.center.orientation.z = qz
- det.bbox.center.orientation.w = qw
- det.bbox.size.x = dx
- det.bbox.size.y = dy
- det.bbox.size.z = dz
-
- arr.detections.append(det)
- return arr
-
- def _to_marker_array(self, header, boxes, labels):
- markers = MarkerArray()
-
- # 이전 프레임 마커 제거
- clear = Marker()
- clear.header = header
- clear.action = Marker.DELETEALL
- markers.markers.append(clear)
-
- for i, (box, label) in enumerate(zip(boxes, labels)):
- x, y, z, dx, dy, dz, yaw = [float(v) for v in box]
- qx, qy, qz, qw = _yaw_to_quat(yaw)
-
- m = Marker()
- m.header = header
- m.ns = 'pointpillars'
- m.id = i
- m.type = Marker.CUBE
- m.action = Marker.ADD
- m.pose.position.x = x
- m.pose.position.y = y
- m.pose.position.z = z
- m.pose.orientation.x = qx
- m.pose.orientation.y = qy
- m.pose.orientation.z = qz
- m.pose.orientation.w = qw
- m.scale.x = dx
- m.scale.y = dy
- m.scale.z = dz
- m.color = self._label_color(int(label))
- markers.markers.append(m)
-
- # 박스 위에 클래스명 텍스트 (YOLO 식 라벨)
- t = Marker()
- t.header = header
- t.ns = 'pointpillars_label'
- t.id = i
- t.type = Marker.TEXT_VIEW_FACING
- t.action = Marker.ADD
- t.pose.position.x = x
- t.pose.position.y = y
- t.pose.position.z = z + dz / 2.0 + 0.4
- t.pose.orientation.w = 1.0
- t.scale.z = 0.8
- t.color = self._label_color(int(label))
- t.color.a = 1.0
- t.text = self._label_name(int(label))
- markers.markers.append(t)
-
- return markers
-
- @staticmethod
- def _label_color(label):
- # 클래스별 색 (반투명). 미지정 라벨은 회색.
- palette = {
- 1: (0.0, 1.0, 0.0), # Car
- 2: (1.0, 1.0, 0.0), # Pedestrian
- 3: (0.0, 0.6, 1.0), # Cyclist
- }
- r, g, b = palette.get(label, (0.6, 0.6, 0.6))
- return ColorRGBA(r=r, g=g, b=b, a=0.5)
-
- @staticmethod
- def _label_name(label):
- # 클래스 id → 이름 (_label_color 팔레트와 동일 매핑). 미지정은 cls{id}.
- names = {1: 'Car', 2: 'Pedestrian', 3: 'Cyclist'}
- return names.get(label, f'cls{label}')
-
-
-def main(args=None):
- rclpy.init(args=args)
- node = ObjectDetection()
- try:
- rclpy.spin(node)
- except KeyboardInterrupt:
- pass
- finally:
- node.destroy_node()
- rclpy.shutdown()
-
-
-if __name__ == '__main__':
- main()
diff --git a/src/perception_pointpillars_pkg/resource/perception_pointpillars_pkg b/src/perception_pointpillars_pkg/resource/perception_pointpillars_pkg
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_pointpillars_pkg/setup.cfg b/src/perception_pointpillars_pkg/setup.cfg
deleted file mode 100755
index 40f23a0..0000000
--- a/src/perception_pointpillars_pkg/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/perception_pointpillars_pkg
-[install]
-install_scripts=$base/lib/perception_pointpillars_pkg
diff --git a/src/perception_pointpillars_pkg/setup.py b/src/perception_pointpillars_pkg/setup.py
deleted file mode 100755
index cff02ea..0000000
--- a/src/perception_pointpillars_pkg/setup.py
+++ /dev/null
@@ -1,37 +0,0 @@
-
-import os
-from glob import glob
-from setuptools import find_packages, setup
-
-package_name = 'perception_pointpillars_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')),
- # finetuned 용 OpenPCDet cfg — ws 루트 cfgs/ 의 편입본 (launch 가 share 경로로 참조)
- (os.path.join('share', package_name, 'cfgs'), glob('cfgs/*.yaml')),
- ],
- install_requires=['setuptools'],
- zip_safe=True,
- maintainer='dgist',
- maintainer_email='jang1future@gmail.com',
- description='LiDAR 3D object detection (PointPillars / OpenPCDet) perception module',
- license='TODO: License declaration',
- extras_require={
- 'test': [
- 'pytest',
- ],
- },
- entry_points={
- 'console_scripts': [
- 'pointpillars = perception_pointpillars_pkg.pointpillars_node:main'
- ],
- },
-)
diff --git a/src/perception_pointpillars_pkg/test/test_copyright.py b/src/perception_pointpillars_pkg/test/test_copyright.py
deleted file mode 100755
index 97a3919..0000000
--- a/src/perception_pointpillars_pkg/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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'
diff --git a/src/perception_pointpillars_pkg/test/test_flake8.py b/src/perception_pointpillars_pkg/test/test_flake8.py
deleted file mode 100755
index 27ee107..0000000
--- a/src/perception_pointpillars_pkg/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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)
diff --git a/src/perception_pointpillars_pkg/test/test_pep257.py b/src/perception_pointpillars_pkg/test/test_pep257.py
deleted file mode 100755
index b234a38..0000000
--- a/src/perception_pointpillars_pkg/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# 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'
diff --git a/src/perception_rangenetpp_pkg/config/rangenetpp.yaml b/src/perception_rangenetpp_pkg/config/rangenetpp.yaml
deleted file mode 100755
index a1dfd62..0000000
--- a/src/perception_rangenetpp_pkg/config/rangenetpp.yaml
+++ /dev/null
@@ -1,23 +0,0 @@
-rangenetpp:
- ros__parameters:
- input_topic: "/raw/lidar"
- segmented_topic: "/perception/segmented"
- segmented_rgb_topic: "/perception/segmented_rgb"
- model_dir: "" # 비우면 env RANGENETPP_MODEL_DIR, launch 기본 /opt/rangenetpp_pretrained
- model_ckpt_path: "" # RangeNet++ 는 model_dir 안 backbone/decoder/head 로드 → 미사용(인터페이스 일관성)
- device: "auto" # auto = 런타임 감지(GPU 있으면 cuda, 없으면 cpu). dual-runtime
- # range projection — 실 센서 FOV 에 맞춘다 (CARLA fr09 ouster: 32ch, +21.2/-21.2).
- # proj_H 는 네트워크 학습 높이(64) 유지하고 32 실채널을 KNN 보간으로 채움.
- # FOV 가 KITTI(+3/-25)면 ±21.2 점들이 엉뚱한 행에 박혀 추론이 한 클래스로 붕괴됨.
- proj_H: 64
- proj_W: 2048
- fov_up: 21.2
- fov_down: -21.2
- max_range: 50.0
- min_range: 1.0
- use_knn: true
- # CARLA 도메인갭 보정 (objdet z_offset 측정값 참고: CARLA 지면 -1.06 vs KITTI -1.73)
- z_offset: 0.0
- # CARLA intensity(~0.96) 를 KITTI 평균(0.21)에 맞춰 중화 — 안 하면 +4.7σ 로 saturate.
- intensity_scale: 0.22
- use_sim_time: true
diff --git a/src/perception_rangenetpp_pkg/config/semantic_kitti_labels.yaml b/src/perception_rangenetpp_pkg/config/semantic_kitti_labels.yaml
deleted file mode 100755
index 4cdc533..0000000
--- a/src/perception_rangenetpp_pkg/config/semantic_kitti_labels.yaml
+++ /dev/null
@@ -1,22 +0,0 @@
-# SemanticKITTI class id → BGR (RViz 색칠용). 표준 color_map 일부.
-color_map:
- 0: [0, 0, 0] # unlabeled
- 10: [245, 150, 100] # car
- 11: [245, 230, 100] # bicycle
- 15: [150, 60, 30] # motorcycle
- 18: [180, 30, 80] # truck
- 20: [255, 0, 0] # other-vehicle
- 30: [30, 30, 255] # person
- 31: [200, 40, 255] # bicyclist
- 32: [90, 30, 150] # motorcyclist
- 40: [255, 0, 255] # road
- 44: [255, 150, 255] # parking
- 48: [75, 0, 75] # sidewalk
- 49: [75, 0, 175] # other-ground
- 50: [0, 200, 255] # building
- 51: [50, 120, 255] # fence
- 70: [0, 175, 0] # vegetation
- 71: [0, 60, 135] # trunk
- 72: [80, 240, 150] # terrain
- 80: [150, 240, 255] # pole
- 81: [0, 0, 255] # traffic-sign
diff --git a/src/perception_rangenetpp_pkg/launch/perception.launch.py b/src/perception_rangenetpp_pkg/launch/perception.launch.py
deleted file mode 100755
index 9807859..0000000
--- a/src/perception_rangenetpp_pkg/launch/perception.launch.py
+++ /dev/null
@@ -1,32 +0,0 @@
-"""perception_manager 공통 진입점 (통합 perception 모듈).
-
-LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로
-`ros2 launch perception_rangenetpp_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..`
-를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(rangenetpp.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_rangenetpp_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', 'rangenetpp.launch.py')),
- launch_arguments={
- 'robot': LaunchConfiguration('robot'),
- 'use_sim_time': LaunchConfiguration('use_sim_time'),
- 'model_source': LaunchConfiguration('model_source'),
- }.items(),
- ),
- ])
diff --git a/src/perception_rangenetpp_pkg/launch/rangenetpp.launch.py b/src/perception_rangenetpp_pkg/launch/rangenetpp.launch.py
deleted file mode 100755
index c928ba6..0000000
--- a/src/perception_rangenetpp_pkg/launch/rangenetpp.launch.py
+++ /dev/null
@@ -1,86 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
-
-# 선언 기본값 — 명시 인자 판별(자동 resolve 게이트)에도 쓰므로 상수로 공유.
-DEFAULT_MODEL_DIR = '/opt/rangenetpp_pretrained'
-DEFAULT_INTENSITY_SCALE = '0.22'
-# 가중치 공유 단일 위치(docker/local): /perception_models/rangenetpp/{finetuned,pretrained}
-# (호스트 .caches/config/nav2_config/perception/models/rangenetpp/*, SWRule config_mounts).
-FINETUNED_DIR = '/perception_models/rangenetpp/finetuned'
-SHARED_PRE_DIR = '/perception_models/rangenetpp/pretrained' # 공유 pretrained (Phase 2 채움)
-
-ARGUMENTS = [
- DeclareLaunchArgument('use_sim_time', default_value='true'),
- DeclareLaunchArgument('robot', default_value='mkmini', choices=['mkmini', 'fr09']),
- DeclareLaunchArgument('model_dir', default_value=DEFAULT_MODEL_DIR),
- DeclareLaunchArgument('model_ckpt_path', default_value=''),
- # base(KITTI) pretrained 는 0.22(CARLA intensity 중화), finetuned 는 1.0(raw 로 학습됨).
- DeclareLaunchArgument('intensity_scale', default_value=DEFAULT_INTENSITY_SCALE),
- # 모델 자동 resolve 스위치: pretrained 선택 시 finetuned 있어도 건너뛰고 base pretrained 사용.
- DeclareLaunchArgument('model_source', default_value='finetuned',
- choices=['finetuned', 'pretrained']),
-]
-
-
-def _resolve_model(context):
- """모델 resolve — 단독 deploy run.sh 의 MODEL_ARGS 로직을 launch 안으로 이식.
-
- - model_dir/intensity_scale 이 명시되면(선언 기본값과 다르면) 그대로 사용
- → 단독 run.sh 가 MODEL_ARGS 로 명시 전달하는 기존 동작이 그대로 이긴다(무회귀).
- - 기본값/빈값일 때만 model_source(기본 finetuned) + finetuned 존재 검사로 자동 resolve.
- RangeNet++ finetuned 는 단일 ckpt 가 아니라 model_dir 안 backbone 파일 존재로 게이트.
- """
- model_dir = LaunchConfiguration('model_dir').perform(context)
- intensity_scale = LaunchConfiguration('intensity_scale').perform(context)
- model_source = LaunchConfiguration('model_source').perform(context)
-
- explicit = (model_dir not in ('', DEFAULT_MODEL_DIR)
- or intensity_scale not in ('', DEFAULT_INTENSITY_SCALE))
- if not explicit:
- if model_source != 'pretrained' and os.path.isfile(
- os.path.join(FINETUNED_DIR, 'backbone')):
- # finetuned 는 raw CARLA intensity(~0.95)로 학습됨 → 추론도 raw(intensity_scale=1.0).
- # base(0.22 중화)와 달라야 train/infer 정규화가 일치(안 그러면 재학습 효과 깎임).
- model_dir = FINETUNED_DIR
- intensity_scale = '1.0'
- print(f'[rangenetpp] 재학습 모델 사용: {FINETUNED_DIR} (intensity_scale=1.0)')
- elif os.path.isfile(os.path.join(SHARED_PRE_DIR, 'backbone')):
- # 공유 pretrained(KITTI base) — baked(/opt) 보다 우선. intensity 는 base 0.22.
- model_dir = SHARED_PRE_DIR
- intensity_scale = DEFAULT_INTENSITY_SCALE
- print(f'[rangenetpp] 공유 pretrained 사용: {SHARED_PRE_DIR}')
- else:
- model_dir = model_dir or DEFAULT_MODEL_DIR
- intensity_scale = intensity_scale or DEFAULT_INTENSITY_SCALE
- print('[rangenetpp] 재학습 모델 없음 → base pretrained(baked) 사용')
-
- pkg = get_package_share_directory('perception_rangenetpp_pkg')
- params_file = PathJoinSubstitution([pkg, 'config', 'rangenetpp.yaml'])
- color_map = PathJoinSubstitution([pkg, 'config', 'semantic_kitti_labels.yaml'])
- remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
- load_nodes = GroupAction(actions=[
- Node(
- package='perception_rangenetpp_pkg', executable='rangenetpp',
- name='rangenetpp', output='screen',
- parameters=[params_file, {
- 'use_sim_time': LaunchConfiguration('use_sim_time'),
- 'model_dir': model_dir,
- 'model_ckpt_path': LaunchConfiguration('model_ckpt_path'),
- 'intensity_scale': float(intensity_scale),
- 'color_map_path': color_map,
- }],
- remappings=remappings,
- )
- ])
- return [load_nodes]
-
-
-def generate_launch_description():
- ld = LaunchDescription(ARGUMENTS)
- ld.add_action(OpaqueFunction(function=_resolve_model))
- return ld
diff --git a/src/perception_rangenetpp_pkg/package.xml b/src/perception_rangenetpp_pkg/package.xml
deleted file mode 100755
index 0b80c7f..0000000
--- a/src/perception_rangenetpp_pkg/package.xml
+++ /dev/null
@@ -1,24 +0,0 @@
-
-
-
- perception_rangenetpp_pkg
- 0.0.0
- LiDAR semantic segmentation (RangeNet++) perception module
- dgist
- TODO: License declaration
-
- rclpy
- sensor_msgs
- sensor_msgs_py
- geometry_msgs
- std_msgs
-
- ament_copyright
- ament_flake8
- ament_pep257
- python3-pytest
-
-
- ament_python
-
-
diff --git a/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/__init__.py b/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/__init__.py
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/range_projection.py b/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/range_projection.py
deleted file mode 100755
index 269a070..0000000
--- a/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/range_projection.py
+++ /dev/null
@@ -1,68 +0,0 @@
-#!/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 (RangeNet 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
diff --git a/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/rangenetpp_backend.py b/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/rangenetpp_backend.py
deleted file mode 100755
index ec2fde8..0000000
--- a/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/rangenetpp_backend.py
+++ /dev/null
@@ -1,125 +0,0 @@
-#!/usr/bin/env python3
-"""RangeNet++ semantic segmentation 추론 래퍼 (ROS 무관).
-
-torch / lidar-bonnetal(RangeNet++) 모델 코드는 인스턴스화할 때만 import → 미설치
-환경에서도 패키지 import/build 통과(salsanext_backend 와 동일 격리).
-
-infer(points) -> labels (N,) uint32 (SemanticKITTI class id)
-
-모델 산출물(arch_cfg/data_cfg + backbone/segmentation_decoder/segmentation_head,
-color map)은 컨테이너 /models 에 config_mounts 로 bind-mount 된다.
-projection 파라미터는 노드(config/Rule)가 주입.
-
-⚠️ RangeNet++ 가 SalsaNext 와 다른 점(범위 표현·KNN·정규화는 동일):
- - 모델 = lidar-bonnetal `Segmentator`(darknet 백본+디코더+head). 단일 .pth 가 아니라
- model_dir 안 `backbone`/`segmentation_decoder`/`segmentation_head` 3개 weight 파일을
- Segmentator 가 내부적으로 로드(strict). 따라서 backend 는 ckpt_path 를 직접 안 읽고
- model_dir 만 넘긴다(파일이 없으면 random weight 로 fallback — 그건 잘못된 추론이므로
- 노드가 model_dir 유효성을 보장해야 한다).
-"""
-import os
-import sys
-import types
-
-import numpy as np
-
-from perception_rangenetpp_pkg.range_projection import RangeProjector
-
-
-class RangeNetppBackend:
- def __init__(self, model_dir, ckpt_path, device,
- proj_H, proj_W, fov_up, fov_down, max_range, min_range,
- z_offset=0.0, intensity_scale=1.0, use_knn=True, logger=None):
- import torch
- import yaml
-
- # lidar-bonnetal repo(train/) 를 path 에 추가 — 이미지에 clone 됨(Dockerfile).
- rnet_root = os.environ.get('RANGENETPP_ROOT', '/opt/lidar-bonnetal/train')
- sys.path.append(rnet_root)
- # ⚠️ lidar-bonnetal 함정: segmentator.py 가 `import __init__ as booger` 로 잡는 모듈의
- # TRAIN_PATH(상대경로 "../../")를 imp.load_source 백본/디코더 파일 경로에 쓴다 → cwd 의존
- # (deploy 는 cwd=/nav2_ws 라 backbones/*.py 를 못 찾음). segmentator import 전에
- # '__init__' 모듈을 절대경로 TRAIN_PATH 로 sys.modules 에 선등록 — `import __init__` 이
- # 캐시에서 이 stub 을 그대로 집어가므로 어떤 cwd 에서도 안전. (사후 패치는 불가:
- # '__init__' 은 segmentator import 시점에야 sys.modules 에 생긴다.)
- booger = sys.modules.get('__init__')
- if booger is None:
- booger = types.ModuleType('__init__')
- sys.modules['__init__'] = booger
- booger.TRAIN_PATH = rnet_root
- booger.DEPLOY_PATH = rnet_root
- from tasks.semantic.modules.segmentator import Segmentator
- from tasks.semantic.postproc.KNN import KNN
-
- self._torch = torch
- # device='auto' → 런타임 감지: cuda 가용 시 cuda:0, 아니면 cpu (08-gpu-cpu-runtime N4)
- if device == 'auto':
- device = 'cuda:0' if torch.cuda.is_available() else 'cpu'
- self.device = device
- self.z_offset = float(z_offset)
- self.intensity_scale = float(intensity_scale)
-
- # 모델 산출물 cfg (mount 된 /models 안). nclasses / 정규화 means·stds / color map.
- arch = yaml.safe_load(open(os.path.join(model_dir, 'arch_cfg.yaml')))
- data = yaml.safe_load(open(os.path.join(model_dir, 'data_cfg.yaml')))
- self.nclasses = len(data['learning_map_inv'])
- sensor = arch['dataset']['sensor']
- self.img_means = np.array(sensor['img_means'], dtype=np.float32)
- self.img_stds = np.array(sensor['img_stds'], dtype=np.float32)
- # 추론 결과는 train id → 원래 SemanticKITTI id 로 역매핑
- self.learning_map_inv = data['learning_map_inv']
-
- self.proj = RangeProjector(proj_H, proj_W, fov_up, fov_down, max_range, min_range)
-
- # Segmentator(ARCH, nclasses, path=model_dir) → model_dir 안 backbone/
- # segmentation_decoder/segmentation_head 를 strict 로 로드(없으면 random).
- with torch.no_grad():
- model = Segmentator(arch, self.nclasses, model_dir)
- self._model = model.to(device).eval()
-
- self._knn = None
- if use_knn:
- self._knn = KNN(arch['post']['KNN']['params'], self.nclasses)
- if logger:
- logger(f"RangeNet++ loaded: nclasses={self.nclasses} device={device}")
-
- def infer(self, points):
- torch = self._torch
- points = np.ascontiguousarray(points, dtype=np.float32)
- if self.z_offset:
- points = points.copy()
- points[:, 2] += self.z_offset
- if self.intensity_scale != 1.0 and points.shape[1] > 3:
- points = points.copy()
- points[:, 3] *= self.intensity_scale
-
- pr = self.proj.project(points)
- img = pr.range_image.copy()
- # 정규화 (학습 분포) — valid 픽셀만 (range=채널0 이 채워진 픽셀)
- mask = (img[0] > 0).astype(np.float32)
- for c in range(5):
- img[c] = (img[c] - self.img_means[c]) / self.img_stds[c]
- img[c] *= mask
-
- with torch.no_grad():
- inp = torch.from_numpy(img).unsqueeze(0).to(self.device)
- logits = self._model(inp) # (1, C, H, W)
- pixel_argmax = logits.argmax(dim=1)[0] # (H, W)
-
- if self._knn is not None:
- depth_img = torch.from_numpy(pr.range_image[0]).to(self.device)
- py = torch.from_numpy(pr.py.astype(np.int64)).to(self.device)
- px = torch.from_numpy(pr.px.astype(np.int64)).to(self.device)
- pt_depth = torch.from_numpy(
- np.linalg.norm(points[pr.valid, :3], axis=1)).to(self.device)
- knn_labels = self._knn(depth_img, pt_depth, pixel_argmax, px, py)
- train_ids = np.zeros(len(points), dtype=np.int64)
- train_ids[pr.valid] = knn_labels.cpu().numpy()
- else:
- train_ids = self.proj.unproject(pixel_argmax.cpu().numpy(), pr)
-
- # train id → SemanticKITTI id
- out = np.zeros(len(points), dtype=np.uint32)
- for tid, kid in self.learning_map_inv.items():
- out[train_ids == int(tid)] = int(kid)
- return out
diff --git a/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/rangenetpp_node.py b/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/rangenetpp_node.py
deleted file mode 100755
index 97bc57c..0000000
--- a/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/rangenetpp_node.py
+++ /dev/null
@@ -1,149 +0,0 @@
-#!/usr/bin/env python3
-import os
-import struct
-
-import numpy as np
-import rclpy
-from rclpy.node import Node
-from sensor_msgs.msg import PointCloud2, PointField
-import sensor_msgs_py.point_cloud2 as pc2
-import yaml
-
-from perception_rangenetpp_pkg.rangenetpp_backend import RangeNetppBackend
-
-
-class SemanticSegmentation(Node):
- def __init__(self):
- super().__init__('rangenetpp')
-
- self.declare_parameter('input_topic', '/raw/lidar')
- self.declare_parameter('segmented_topic', '/perception/segmented')
- self.declare_parameter('segmented_rgb_topic', '/perception/segmented_rgb')
- # 모델 산출물 디렉토리(/models) — RangeNet++ 는 dir 안 backbone/decoder/head 로드.
- # ckpt_path 는 인터페이스 일관성(salsanext)용 — RangeNet++ 는 model_dir 만 쓴다.
- self.declare_parameter('model_dir', '')
- self.declare_parameter('model_ckpt_path', '')
- self.declare_parameter('color_map_path', '')
- # device: 'auto' → 런타임 감지(cuda 가용 시 cuda:0, 아니면 cpu). dual-runtime
- self.declare_parameter('device', 'auto')
- # range projection (모델 입력 규격, Rule 로 조절)
- self.declare_parameter('proj_H', 64)
- self.declare_parameter('proj_W', 2048)
- self.declare_parameter('fov_up', 3.0)
- self.declare_parameter('fov_down', -25.0)
- self.declare_parameter('max_range', 50.0)
- self.declare_parameter('min_range', 1.0)
- self.declare_parameter('use_knn', True)
- # 도메인갭 보정
- self.declare_parameter('z_offset', 0.0)
- self.declare_parameter('intensity_scale', 1.0)
-
- gp = self.get_parameter
- input_topic = gp('input_topic').value
- seg_topic = gp('segmented_topic').value
- seg_rgb_topic = gp('segmented_rgb_topic').value
- model_dir = gp('model_dir').value or os.environ.get('RANGENETPP_MODEL_DIR', '')
- ckpt = gp('model_ckpt_path').value or os.environ.get('RANGENETPP_MODEL_CKPT', '')
- color_map_path = gp('color_map_path').value or os.environ.get('RANGENETPP_COLOR_MAP', '')
-
- self.color_map = self._load_color_map(color_map_path)
- self.pub_seg = self.create_publisher(PointCloud2, seg_topic, 5)
- self.pub_rgb = self.create_publisher(PointCloud2, seg_rgb_topic, 5)
-
- self.backend = self._init_backend(model_dir, ckpt, gp)
- self.sub = self.create_subscription(PointCloud2, input_topic, self.callback, 5)
- self.get_logger().info(
- f"RangeNet++ started: in[{input_topic}] "
- f"seg[{seg_topic}] rgb[{seg_rgb_topic}] device[{gp('device').value}]")
-
- def _init_backend(self, model_dir, ckpt, gp):
- # RangeNet++ 는 model_dir 안 backbone/segmentation_decoder/segmentation_head 를 로드.
- # 단일 ckpt 파일이 아니라 dir 유효성으로 게이트(arch_cfg 존재로 간이 검증).
- if not model_dir or not os.path.isfile(os.path.join(model_dir, 'arch_cfg.yaml')):
- self.get_logger().error(
- "model_dir 미지정/유효하지 않음 (config 또는 RANGENETPP_MODEL_DIR). "
- "arch_cfg.yaml + backbone/segmentation_decoder/segmentation_head 필요. "
- "추론 비활성 — 구독만 수행.")
- return None
- try:
- return RangeNetppBackend(
- model_dir=model_dir, ckpt_path=ckpt, device=gp('device').value,
- proj_H=gp('proj_H').value, proj_W=gp('proj_W').value,
- fov_up=gp('fov_up').value, fov_down=gp('fov_down').value,
- max_range=gp('max_range').value, min_range=gp('min_range').value,
- z_offset=gp('z_offset').value, intensity_scale=gp('intensity_scale').value,
- use_knn=gp('use_knn').value, logger=self.get_logger().info)
- except Exception as e:
- self.get_logger().error(f"분할 백엔드 초기화 실패: {e}")
- return None
-
- def _load_color_map(self, path):
- if path and os.path.exists(path):
- data = yaml.safe_load(open(path))
- return {int(k): tuple(v) for k, v in data.get('color_map', {}).items()}
- return {}
-
- def _read_points(self, msg):
- names = {f.name for f in msg.fields}
- if 'intensity' in names:
- pts = pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True)
- if pts.shape[0] == 0:
- return None
- return np.stack((pts["x"], pts["y"], pts["z"], pts["intensity"]), axis=-1).astype(np.float32)
- pts = pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True)
- if pts.shape[0] == 0:
- return None
- xyz = np.stack((pts["x"], pts["y"], pts["z"]), axis=-1).astype(np.float32)
- return np.concatenate([xyz, np.zeros((xyz.shape[0], 1), np.float32)], axis=1)
-
- def callback(self, msg):
- if self.backend is None:
- return
- points = self._read_points(msg)
- if points is None:
- return
- labels = self.backend.infer(points) # (N,) uint32 SemanticKITTI id
- self.pub_seg.publish(self._to_label_cloud(msg.header, points, labels))
- if self.color_map:
- self.pub_rgb.publish(self._to_rgb_cloud(msg.header, points, labels))
-
- def _to_label_cloud(self, header, points, labels):
- fields = [
- PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
- PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
- PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
- PointField(name='label', offset=12, datatype=PointField.UINT32, count=1),
- ]
- data = [(float(p[0]), float(p[1]), float(p[2]), int(l))
- for p, l in zip(points, labels)]
- return pc2.create_cloud(header, fields, data)
-
- def _to_rgb_cloud(self, header, points, labels):
- fields = [
- PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
- PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
- PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
- PointField(name='rgb', offset=12, datatype=PointField.FLOAT32, count=1),
- ]
- data = []
- for p, l in zip(points, labels):
- b, g, r = self.color_map.get(int(l), (0, 0, 0)) # SemanticKITTI BGR
- rgb = struct.unpack('f', struct.pack('I', (int(r) << 16) | (int(g) << 8) | int(b)))[0]
- data.append((float(p[0]), float(p[1]), float(p[2]), rgb))
- return pc2.create_cloud(header, fields, data)
-
-
-def main(args=None):
- rclpy.init(args=args)
- node = SemanticSegmentation()
- try:
- rclpy.spin(node)
- except KeyboardInterrupt:
- pass
- finally:
- node.destroy_node()
- rclpy.shutdown()
-
-
-if __name__ == '__main__':
- main()
diff --git a/src/perception_rangenetpp_pkg/resource/perception_rangenetpp_pkg b/src/perception_rangenetpp_pkg/resource/perception_rangenetpp_pkg
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_rangenetpp_pkg/setup.cfg b/src/perception_rangenetpp_pkg/setup.cfg
deleted file mode 100755
index d7495c5..0000000
--- a/src/perception_rangenetpp_pkg/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/perception_rangenetpp_pkg
-[install]
-install_scripts=$base/lib/perception_rangenetpp_pkg
diff --git a/src/perception_rangenetpp_pkg/setup.py b/src/perception_rangenetpp_pkg/setup.py
deleted file mode 100755
index de31ad3..0000000
--- a/src/perception_rangenetpp_pkg/setup.py
+++ /dev/null
@@ -1,35 +0,0 @@
-
-import os
-from glob import glob
-from setuptools import find_packages, setup
-
-package_name = 'perception_rangenetpp_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 semantic segmentation (RangeNet++) perception module',
- license='TODO: License declaration',
- extras_require={
- 'test': [
- 'pytest',
- ],
- },
- entry_points={
- 'console_scripts': [
- 'rangenetpp = perception_rangenetpp_pkg.rangenetpp_node:main'
- ],
- },
-)
diff --git a/src/perception_rangenetpp_pkg/test/test_copyright.py b/src/perception_rangenetpp_pkg/test/test_copyright.py
deleted file mode 100755
index 97a3919..0000000
--- a/src/perception_rangenetpp_pkg/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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'
diff --git a/src/perception_rangenetpp_pkg/test/test_flake8.py b/src/perception_rangenetpp_pkg/test/test_flake8.py
deleted file mode 100755
index 27ee107..0000000
--- a/src/perception_rangenetpp_pkg/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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)
diff --git a/src/perception_rangenetpp_pkg/test/test_pep257.py b/src/perception_rangenetpp_pkg/test/test_pep257.py
deleted file mode 100755
index b234a38..0000000
--- a/src/perception_rangenetpp_pkg/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# 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'
diff --git a/src/perception_rangenetpp_pkg/test/test_range_projection.py b/src/perception_rangenetpp_pkg/test/test_range_projection.py
deleted file mode 100755
index 56e848b..0000000
--- a/src/perception_rangenetpp_pkg/test/test_range_projection.py
+++ /dev/null
@@ -1,29 +0,0 @@
-import numpy as np
-from perception_rangenetpp_pkg.range_projection import RangeProjector
-
-
-def test_unproject_recovers_per_point_labels():
- # 정면 부채꼴에 흩뿌린 점들 — projection 후 픽셀 라벨을 도로(0)/객체(1)로 칠하고
- # un-projection 이 점별 라벨을 되돌리는지 (충돌 없는 sparse 케이스).
- rng = np.random.default_rng(0)
- n = 500
- xyz = rng.uniform(-20, 20, size=(n, 3)).astype(np.float32)
- xyz[:, 2] = rng.uniform(-2, 1, size=n) # 적당한 높이
- pts = np.concatenate([xyz, np.zeros((n, 1), np.float32)], axis=1)
-
- proj = RangeProjector(proj_H=64, proj_W=1024, fov_up=3.0, fov_down=-25.0,
- max_range=50.0, min_range=1.0)
- out = proj.project(pts)
- assert out.range_image.shape == (5, 64, 1024)
- assert out.px.shape[0] == out.valid.sum()
-
- # 픽셀 라벨 = (depth>10 ? 1 : 0) 으로 합성 → 점별로 같은 규칙이 복원돼야 함
- depth_img = out.range_image[3]
- pixel_labels = (depth_img > 10.0).astype(np.int64)
- labels = proj.unproject(pixel_labels, out)
- assert labels.shape == (n,)
- # valid 점들에 대해 점 depth 기준 기대 라벨과 일치
- vp = pts[out.valid][:, :3]
- vdepth = np.linalg.norm(vp, axis=1)
- expected = (vdepth > 10.0).astype(np.int64)
- assert (labels[out.valid] == expected).mean() > 0.98
diff --git a/src/perception_salsanext_pkg/config/salsanext.yaml b/src/perception_salsanext_pkg/config/salsanext.yaml
deleted file mode 100755
index 2205033..0000000
--- a/src/perception_salsanext_pkg/config/salsanext.yaml
+++ /dev/null
@@ -1,23 +0,0 @@
-salsanext:
- ros__parameters:
- input_topic: "/raw/lidar"
- segmented_topic: "/perception/segmented"
- segmented_rgb_topic: "/perception/segmented_rgb"
- model_dir: "" # 비우면 env SALSANEXT_MODEL_DIR, launch 기본 /models
- model_ckpt_path: "" # 비우면 env SALSANEXT_MODEL_CKPT, launch 기본 /models/SalsaNext
- device: "auto" # auto: cuda 가용 시 GPU, 아니면 CPU (08-gpu-cpu-runtime N3/N4)
- # range projection — 실 센서 FOV 에 맞춘다 (CARLA fr09 ouster: 32ch, +21.2/-21.2).
- # proj_H 는 네트워크 학습 높이(64) 유지하고 32 실채널을 KNN 보간으로 채움.
- # FOV 가 KITTI(+3/-25)면 ±21.2 점들이 엉뚱한 행에 박혀 추론이 한 클래스로 붕괴됨.
- proj_H: 64
- proj_W: 2048
- fov_up: 21.2
- fov_down: -21.2
- max_range: 50.0
- min_range: 1.0
- use_knn: true
- # CARLA 도메인갭 보정 (objdet z_offset 측정값 참고: CARLA 지면 -1.06 vs KITTI -1.73)
- z_offset: 0.0
- # CARLA intensity(~0.96) 를 KITTI 평균(0.21)에 맞춰 중화 — 안 하면 +4.7σ 로 saturate.
- intensity_scale: 0.22
- use_sim_time: true
diff --git a/src/perception_salsanext_pkg/config/semantic_kitti_labels.yaml b/src/perception_salsanext_pkg/config/semantic_kitti_labels.yaml
deleted file mode 100755
index 4cdc533..0000000
--- a/src/perception_salsanext_pkg/config/semantic_kitti_labels.yaml
+++ /dev/null
@@ -1,22 +0,0 @@
-# SemanticKITTI class id → BGR (RViz 색칠용). 표준 color_map 일부.
-color_map:
- 0: [0, 0, 0] # unlabeled
- 10: [245, 150, 100] # car
- 11: [245, 230, 100] # bicycle
- 15: [150, 60, 30] # motorcycle
- 18: [180, 30, 80] # truck
- 20: [255, 0, 0] # other-vehicle
- 30: [30, 30, 255] # person
- 31: [200, 40, 255] # bicyclist
- 32: [90, 30, 150] # motorcyclist
- 40: [255, 0, 255] # road
- 44: [255, 150, 255] # parking
- 48: [75, 0, 75] # sidewalk
- 49: [75, 0, 175] # other-ground
- 50: [0, 200, 255] # building
- 51: [50, 120, 255] # fence
- 70: [0, 175, 0] # vegetation
- 71: [0, 60, 135] # trunk
- 72: [80, 240, 150] # terrain
- 80: [150, 240, 255] # pole
- 81: [0, 0, 255] # traffic-sign
diff --git a/src/perception_salsanext_pkg/launch/perception.launch.py b/src/perception_salsanext_pkg/launch/perception.launch.py
deleted file mode 100755
index 333df31..0000000
--- a/src/perception_salsanext_pkg/launch/perception.launch.py
+++ /dev/null
@@ -1,32 +0,0 @@
-"""perception_manager 공통 진입점 (통합 perception 모듈).
-
-LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로
-`ros2 launch perception_salsanext_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..`
-를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(salsanext.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_salsanext_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', 'salsanext.launch.py')),
- launch_arguments={
- 'robot': LaunchConfiguration('robot'),
- 'use_sim_time': LaunchConfiguration('use_sim_time'),
- 'model_source': LaunchConfiguration('model_source'),
- }.items(),
- ),
- ])
diff --git a/src/perception_salsanext_pkg/launch/salsanext.launch.py b/src/perception_salsanext_pkg/launch/salsanext.launch.py
deleted file mode 100755
index d3e84c7..0000000
--- a/src/perception_salsanext_pkg/launch/salsanext.launch.py
+++ /dev/null
@@ -1,80 +0,0 @@
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
-
-# resolve 분기/경로 상수는 pkg 모듈 resolve_model 이 단일 출처 (launch 는 게이트만 담당).
-from perception_salsanext_pkg import resolve_model
-
-# 선언 기본값 — 명시 인자 판별(자동 resolve 게이트)에도 쓰므로 resolve_model 상수 공유.
-DEFAULT_MODEL_DIR = resolve_model.DEFAULT_PRE_DIR
-DEFAULT_MODEL_CKPT = resolve_model.DEFAULT_PRE_CKPT
-DEFAULT_INTENSITY_SCALE = resolve_model.PRETRAINED_INTENSITY_SCALE
-
-ARGUMENTS = [
- DeclareLaunchArgument('use_sim_time', default_value='true'),
- DeclareLaunchArgument('robot', default_value='mkmini', choices=['mkmini', 'fr09']),
- DeclareLaunchArgument('model_dir', default_value=DEFAULT_MODEL_DIR),
- DeclareLaunchArgument('model_ckpt_path', default_value=DEFAULT_MODEL_CKPT),
- # base(KITTI) pretrained 는 0.22(CARLA intensity 중화), finetuned 는 1.0(raw 로 학습됨).
- DeclareLaunchArgument('intensity_scale', default_value=DEFAULT_INTENSITY_SCALE),
- # 모델 자동 resolve 스위치: pretrained 선택 시 finetuned 있어도 건너뛰고 base pretrained 사용.
- DeclareLaunchArgument('model_source', default_value='finetuned',
- choices=['finetuned', 'pretrained']),
-]
-
-
-def _resolve_model(context):
- """모델 resolve — 단독 deploy run.sh 의 MODEL_ARGS 로직을 launch 안으로 이식.
-
- - model_dir/model_ckpt_path/intensity_scale 이 명시되면(선언 기본값과 다르면) 그대로 사용
- → 단독 run.sh 가 MODEL_ARGS 로 명시 전달하는 기존 동작이 그대로 이긴다(무회귀).
- - 기본값/빈값일 때만 model_source(기본 finetuned) + finetuned ckpt 존재 검사로 자동 resolve.
- 분기 자체는 pkg 모듈 resolve_model.resolve() 사용 — intensity_scale 까지 함께 결정
- (finetuned=raw CARLA 1.0 / pretrained=0.22, train/infer 정규화 일치 목적).
- """
- model_dir = LaunchConfiguration('model_dir').perform(context)
- model_ckpt_path = LaunchConfiguration('model_ckpt_path').perform(context)
- intensity_scale = LaunchConfiguration('intensity_scale').perform(context)
- model_source = LaunchConfiguration('model_source').perform(context)
-
- explicit = (model_dir not in ('', DEFAULT_MODEL_DIR)
- or model_ckpt_path not in ('', DEFAULT_MODEL_CKPT)
- or intensity_scale not in ('', DEFAULT_INTENSITY_SCALE))
- if not explicit:
- resolved = resolve_model.resolve(model_source)
- model_dir = resolved['model_dir']
- model_ckpt_path = resolved['ckpt']
- intensity_scale = resolved['intensity_scale']
- if resolved['source'] == 'finetuned':
- print(f'[salsanext] 재학습 모델 사용: {model_dir} '
- f'(intensity_scale={intensity_scale})')
- else:
- print('[salsanext] 재학습 모델 없음 → base pretrained 사용')
-
- pkg = get_package_share_directory('perception_salsanext_pkg')
- params_file = PathJoinSubstitution([pkg, 'config', 'salsanext.yaml'])
- color_map = PathJoinSubstitution([pkg, 'config', 'semantic_kitti_labels.yaml'])
- remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
- load_nodes = GroupAction(actions=[
- Node(
- package='perception_salsanext_pkg', executable='salsanext',
- name='salsanext', output='screen',
- parameters=[params_file, {
- 'use_sim_time': LaunchConfiguration('use_sim_time'),
- 'model_dir': model_dir,
- 'model_ckpt_path': model_ckpt_path,
- 'intensity_scale': float(intensity_scale),
- 'color_map_path': color_map,
- }],
- remappings=remappings,
- )
- ])
- return [load_nodes]
-
-
-def generate_launch_description():
- ld = LaunchDescription(ARGUMENTS)
- ld.add_action(OpaqueFunction(function=_resolve_model))
- return ld
diff --git a/src/perception_salsanext_pkg/package.xml b/src/perception_salsanext_pkg/package.xml
deleted file mode 100755
index 5bf0a38..0000000
--- a/src/perception_salsanext_pkg/package.xml
+++ /dev/null
@@ -1,24 +0,0 @@
-
-
-
- perception_salsanext_pkg
- 0.0.0
- LiDAR semantic segmentation (SalsaNext) perception module
- dgist
- TODO: License declaration
-
- rclpy
- sensor_msgs
- sensor_msgs_py
- geometry_msgs
- std_msgs
-
- ament_copyright
- ament_flake8
- ament_pep257
- python3-pytest
-
-
- ament_python
-
-
diff --git a/src/perception_salsanext_pkg/perception_salsanext_pkg/__init__.py b/src/perception_salsanext_pkg/perception_salsanext_pkg/__init__.py
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_salsanext_pkg/perception_salsanext_pkg/range_projection.py b/src/perception_salsanext_pkg/perception_salsanext_pkg/range_projection.py
deleted file mode 100755
index 3ddda2e..0000000
--- a/src/perception_salsanext_pkg/perception_salsanext_pkg/range_projection.py
+++ /dev/null
@@ -1,68 +0,0 @@
-#!/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
diff --git a/src/perception_salsanext_pkg/perception_salsanext_pkg/resolve_model.py b/src/perception_salsanext_pkg/perception_salsanext_pkg/resolve_model.py
deleted file mode 100755
index 7dec380..0000000
--- a/src/perception_salsanext_pkg/perception_salsanext_pkg/resolve_model.py
+++ /dev/null
@@ -1,44 +0,0 @@
-"""모델 경로 결정 — finetuned(retrain 산출물) 우선 → 공유 pretrained → baked pretrained.
-※ 작은 함수 하나로 분리 — model_source=auto|finetuned|pretrained 명시 선택을 한 분기로 끼우기 위함.
-salsanext 는 intensity 정규화가 모델별로 달라 intensity_scale 까지 함께 resolve 한다."""
-import os
-
-# 가중치 공유 단일 위치(docker/local 공유):
-# 호스트 .caches/config/nav2_config/perception/models/salsanext/{finetuned,pretrained}
-# → 컨테이너 /perception_models/salsanext/* (SWRule config_mounts).
-DEFAULT_FINETUNED = "/perception_models/salsanext/finetuned"
-SHARED_PRE_DIR = "/perception_models/salsanext/pretrained" # 공유 pretrained (Phase 2 채움)
-DEFAULT_PRE_DIR = "/opt/salsanext_pretrained" # 이미지 baked 안전망(오프라인/Jetson)
-DEFAULT_PRE_CKPT = "/opt/salsanext_pretrained/SalsaNext"
-# base(KITTI) pretrained 는 0.22(CARLA intensity ~0.96 을 KITTI 평균 0.21 로 중화),
-# finetuned 는 raw CARLA intensity(~0.95)로 학습됨 → 추론도 raw(1.0).
-# 달라야 train/infer 정규화가 일치(안 그러면 재학습 효과 깎임).
-PRETRAINED_INTENSITY_SCALE = "0.22"
-FINETUNED_INTENSITY_SCALE = "1.0"
-
-
-def _shared_pretrained():
- # 공유 pretrained(.../pretrained)가 채워져 있으면 baked(/opt) 보다 우선 — 공유 단일 출처.
- ckpt = os.path.join(SHARED_PRE_DIR, "SalsaNext")
- if os.path.isfile(ckpt):
- return {"source": "pretrained", "model_dir": SHARED_PRE_DIR, "ckpt": ckpt,
- "intensity_scale": PRETRAINED_INTENSITY_SCALE}
- return None
-
-
-def resolve(model_source="finetuned",
- finetuned_dir=DEFAULT_FINETUNED,
- pretrained_dir=DEFAULT_PRE_DIR,
- pretrained_ckpt=DEFAULT_PRE_CKPT):
- # 3단 fallback: finetuned → 공유 pretrained → baked pretrained(/opt 안전망).
- # model_source=="pretrained" 면 finetuned 있어도 pretrained 강제(공유 우선, 없으면 baked).
- _baked = {"source": "pretrained", "model_dir": pretrained_dir, "ckpt": pretrained_ckpt,
- "intensity_scale": PRETRAINED_INTENSITY_SCALE}
- _pretrained = _shared_pretrained() or _baked
- if model_source == "pretrained":
- return _pretrained
- ckpt = os.path.join(finetuned_dir, "SalsaNext_valid_best")
- if os.path.isfile(ckpt):
- return {"source": "finetuned", "model_dir": finetuned_dir, "ckpt": ckpt,
- "intensity_scale": FINETUNED_INTENSITY_SCALE}
- return _pretrained
diff --git a/src/perception_salsanext_pkg/perception_salsanext_pkg/salsanext_backend.py b/src/perception_salsanext_pkg/perception_salsanext_pkg/salsanext_backend.py
deleted file mode 100755
index 0a34990..0000000
--- a/src/perception_salsanext_pkg/perception_salsanext_pkg/salsanext_backend.py
+++ /dev/null
@@ -1,106 +0,0 @@
-#!/usr/bin/env python3
-"""SalsaNext semantic segmentation 추론 래퍼 (ROS 무관).
-
-torch / SalsaNext 모델 코드는 인스턴스화할 때만 import → 미설치 환경에서도
-패키지 import/build 통과(objdet 의 pointpillars_backend 와 동일 격리).
-
-infer(points) -> labels (N,) int (SemanticKITTI class id)
-
-모델 산출물(arch_cfg/data_cfg/checkpoint, color map)은 컨테이너 /models 에
-config_mounts 로 bind-mount 된다. projection 파라미터는 노드(config/Rule)가 주입.
-"""
-import os
-import sys
-
-import numpy as np
-
-from perception_salsanext_pkg.range_projection import RangeProjector
-
-
-class SalsaNextBackend:
- def __init__(self, model_dir, ckpt_path, device,
- proj_H, proj_W, fov_up, fov_down, max_range, min_range,
- z_offset=0.0, intensity_scale=1.0, use_knn=True, logger=None):
- import torch
- import yaml
-
- # SalsaNext repo(train/) 를 path 에 추가 — 이미지에 clone 됨(Dockerfile).
- salsa_root = os.environ.get('SALSANEXT_ROOT', '/opt/SalsaNext')
- sys.path.append(os.path.join(salsa_root, 'train'))
- from tasks.semantic.modules.SalsaNext import SalsaNext
- from tasks.semantic.postproc.KNN import KNN
-
- # device='auto' → 런타임 감지: cuda 가용 시 cuda:0, 아니면 cpu (08-gpu-cpu-runtime N4)
- if device == 'auto':
- device = 'cuda:0' if torch.cuda.is_available() else 'cpu'
-
- self._torch = torch
- self.device = device
- self.z_offset = float(z_offset)
- self.intensity_scale = float(intensity_scale)
-
- # 모델 산출물 cfg (mount 된 /models 안). nclasses / 정규화 means·stds / color map.
- arch = yaml.safe_load(open(os.path.join(model_dir, 'arch_cfg.yaml')))
- data = yaml.safe_load(open(os.path.join(model_dir, 'data_cfg.yaml')))
- self.nclasses = len(data['learning_map_inv'])
- sensor = arch['dataset']['sensor']
- self.img_means = np.array(sensor['img_means'], dtype=np.float32)
- self.img_stds = np.array(sensor['img_stds'], dtype=np.float32)
- # 추론 결과는 train id → 원래 SemanticKITTI id 로 역매핑
- self.learning_map_inv = data['learning_map_inv']
-
- self.proj = RangeProjector(proj_H, proj_W, fov_up, fov_down, max_range, min_range)
-
- model = SalsaNext(self.nclasses)
- state = torch.load(ckpt_path, map_location='cpu')
- sd = state.get('state_dict', state)
- sd = {k.replace('module.', ''): v for k, v in sd.items()}
- model.load_state_dict(sd, strict=True)
- self._model = model.to(device).eval()
-
- self._knn = None
- if use_knn:
- self._knn = KNN(arch['post']['KNN']['params'], self.nclasses)
- if logger:
- logger(f"SalsaNext loaded: nclasses={self.nclasses} device={device}")
-
- def infer(self, points):
- torch = self._torch
- points = np.ascontiguousarray(points, dtype=np.float32)
- if self.z_offset:
- points = points.copy()
- points[:, 2] += self.z_offset
- if self.intensity_scale != 1.0 and points.shape[1] > 3:
- points = points.copy()
- points[:, 3] *= self.intensity_scale
-
- pr = self.proj.project(points)
- img = pr.range_image.copy()
- # 정규화 (학습 분포) — valid 픽셀만 (range=채널0 이 채워진 픽셀)
- mask = (img[0] > 0).astype(np.float32)
- for c in range(5):
- img[c] = (img[c] - self.img_means[c]) / self.img_stds[c]
- img[c] *= mask
-
- with torch.no_grad():
- inp = torch.from_numpy(img).unsqueeze(0).to(self.device)
- logits = self._model(inp) # (1, C, H, W)
- pixel_argmax = logits.argmax(dim=1)[0] # (H, W)
-
- if self._knn is not None:
- depth_img = torch.from_numpy(pr.range_image[0]).to(self.device)
- py = torch.from_numpy(pr.py.astype(np.int64)).to(self.device)
- px = torch.from_numpy(pr.px.astype(np.int64)).to(self.device)
- pt_depth = torch.from_numpy(
- np.linalg.norm(points[pr.valid, :3], axis=1)).to(self.device)
- knn_labels = self._knn(depth_img, pt_depth, pixel_argmax, px, py)
- train_ids = np.zeros(len(points), dtype=np.int64)
- train_ids[pr.valid] = knn_labels.cpu().numpy()
- else:
- train_ids = self.proj.unproject(pixel_argmax.cpu().numpy(), pr)
-
- # train id → SemanticKITTI id
- out = np.zeros(len(points), dtype=np.uint32)
- for tid, kid in self.learning_map_inv.items():
- out[train_ids == int(tid)] = int(kid)
- return out
diff --git a/src/perception_salsanext_pkg/perception_salsanext_pkg/salsanext_node.py b/src/perception_salsanext_pkg/perception_salsanext_pkg/salsanext_node.py
deleted file mode 100755
index 3403ea9..0000000
--- a/src/perception_salsanext_pkg/perception_salsanext_pkg/salsanext_node.py
+++ /dev/null
@@ -1,183 +0,0 @@
-#!/usr/bin/env python3
-import os
-import struct
-
-import numpy as np
-import rclpy
-from rclpy.node import Node
-from sensor_msgs.msg import PointCloud2, PointField
-import sensor_msgs_py.point_cloud2 as pc2
-import yaml
-
-from perception_salsanext_pkg.salsanext_backend import SalsaNextBackend
-
-
-def _is_cuda_error(e):
- s = str(e).lower()
- return 'cuda' in s or 'cudnn' in s or 'out of memory' in s
-
-
-class SemanticSegmentation(Node):
- def __init__(self):
- super().__init__('salsanext')
-
- self.declare_parameter('input_topic', '/raw/lidar')
- self.declare_parameter('segmented_topic', '/perception/segmented')
- self.declare_parameter('segmented_rgb_topic', '/perception/segmented_rgb')
- # 모델 산출물 디렉토리(/models) + checkpoint — 하드코딩 금지, env fallback
- self.declare_parameter('model_dir', '')
- self.declare_parameter('model_ckpt_path', '')
- self.declare_parameter('color_map_path', '')
- # device: 'auto' → 런타임 감지(cuda 가용 시 cuda:0, 아니면 cpu).
- # 'cuda:0'/'cpu' 등 명시값은 그대로 존중(override). (08-gpu-cpu-runtime N3/N4)
- self.declare_parameter('device', 'auto')
- # range projection (모델 입력 규격, Rule 로 조절)
- self.declare_parameter('proj_H', 64)
- self.declare_parameter('proj_W', 2048)
- self.declare_parameter('fov_up', 3.0)
- self.declare_parameter('fov_down', -25.0)
- self.declare_parameter('max_range', 50.0)
- self.declare_parameter('min_range', 1.0)
- self.declare_parameter('use_knn', True)
- # 도메인갭 보정
- self.declare_parameter('z_offset', 0.0)
- self.declare_parameter('intensity_scale', 1.0)
-
- gp = self.get_parameter
- input_topic = gp('input_topic').value
- seg_topic = gp('segmented_topic').value
- seg_rgb_topic = gp('segmented_rgb_topic').value
- model_dir = gp('model_dir').value or os.environ.get('SALSANEXT_MODEL_DIR', '')
- ckpt = gp('model_ckpt_path').value or os.environ.get('SALSANEXT_MODEL_CKPT', '')
- color_map_path = gp('color_map_path').value or os.environ.get('SALSANEXT_COLOR_MAP', '')
-
- self.color_map = self._load_color_map(color_map_path)
- self.pub_seg = self.create_publisher(PointCloud2, seg_topic, 5)
- self.pub_rgb = self.create_publisher(PointCloud2, seg_rgb_topic, 5)
-
- self._model_dir = model_dir
- self._ckpt = ckpt
- self._cpu_fallback_done = False
- self.backend = self._init_backend(model_dir, ckpt, gp)
- self.sub = self.create_subscription(PointCloud2, input_topic, self.callback, 5)
- self.get_logger().info(
- f"SalsaNext started: in[{input_topic}] "
- f"seg[{seg_topic}] rgb[{seg_rgb_topic}] device[{gp('device').value}]")
-
- def _init_backend(self, model_dir, ckpt, gp):
- if not model_dir or not ckpt:
- self.get_logger().error(
- "model_dir / model_ckpt_path 미지정 (config 또는 "
- "SALSANEXT_MODEL_DIR/SALSANEXT_MODEL_CKPT). 추론 비활성 — 구독만 수행.")
- return None
- try:
- return self._create_backend(model_dir, ckpt, gp('device').value)
- except Exception as e:
- if gp('device').value != 'cpu' and _is_cuda_error(e):
- self.get_logger().warn(f"[salsanext] GPU 백엔드 초기화 실패({e}) → CPU 폴백")
- try:
- return self._create_backend(model_dir, ckpt, 'cpu')
- except Exception as e:
- self.get_logger().error(f"분할 백엔드 초기화 실패: {e}")
- return None
- self.get_logger().error(f"분할 백엔드 초기화 실패: {e}")
- return None
-
- def _create_backend(self, model_dir, ckpt, device):
- gp = self.get_parameter
- return SalsaNextBackend(
- model_dir=model_dir, ckpt_path=ckpt, device=device,
- proj_H=gp('proj_H').value, proj_W=gp('proj_W').value,
- fov_up=gp('fov_up').value, fov_down=gp('fov_down').value,
- max_range=gp('max_range').value, min_range=gp('min_range').value,
- z_offset=gp('z_offset').value, intensity_scale=gp('intensity_scale').value,
- use_knn=gp('use_knn').value, logger=self.get_logger().info)
-
- def _load_color_map(self, path):
- if path and os.path.exists(path):
- data = yaml.safe_load(open(path))
- return {int(k): tuple(v) for k, v in data.get('color_map', {}).items()}
- return {}
-
- def _read_points(self, msg):
- names = {f.name for f in msg.fields}
- if 'intensity' in names:
- pts = pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True)
- if pts.shape[0] == 0:
- return None
- return np.stack((pts["x"], pts["y"], pts["z"], pts["intensity"]), axis=-1).astype(np.float32)
- pts = pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True)
- if pts.shape[0] == 0:
- return None
- xyz = np.stack((pts["x"], pts["y"], pts["z"]), axis=-1).astype(np.float32)
- return np.concatenate([xyz, np.zeros((xyz.shape[0], 1), np.float32)], axis=1)
-
- def callback(self, msg):
- if self.backend is None:
- return
- points = self._read_points(msg)
- if points is None:
- return
- try:
- labels = self.backend.infer(points) # (N,) uint32 SemanticKITTI id
- except Exception as e:
- if self._cpu_fallback_done or not _is_cuda_error(e):
- raise
- self.get_logger().warn("[salsanext] 추론 중 CUDA 에러 → CPU 백엔드 재구성")
- self._cpu_fallback_done = True
- try:
- del self.backend._model
- self.backend._torch.cuda.empty_cache()
- except Exception:
- pass
- try:
- self.backend = self._create_backend(self._model_dir, self._ckpt, 'cpu')
- labels = self.backend.infer(points)
- except Exception as e2:
- self.get_logger().error(f"분할 백엔드 CPU 폴백 실패: {e2}")
- self.backend = None
- return
- self.pub_seg.publish(self._to_label_cloud(msg.header, points, labels))
- if self.color_map:
- self.pub_rgb.publish(self._to_rgb_cloud(msg.header, points, labels))
-
- def _to_label_cloud(self, header, points, labels):
- fields = [
- PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
- PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
- PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
- PointField(name='label', offset=12, datatype=PointField.UINT32, count=1),
- ]
- data = [(float(p[0]), float(p[1]), float(p[2]), int(l))
- for p, l in zip(points, labels)]
- return pc2.create_cloud(header, fields, data)
-
- def _to_rgb_cloud(self, header, points, labels):
- fields = [
- PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
- PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
- PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
- PointField(name='rgb', offset=12, datatype=PointField.FLOAT32, count=1),
- ]
- data = []
- for p, l in zip(points, labels):
- b, g, r = self.color_map.get(int(l), (0, 0, 0)) # SemanticKITTI BGR
- rgb = struct.unpack('f', struct.pack('I', (int(r) << 16) | (int(g) << 8) | int(b)))[0]
- data.append((float(p[0]), float(p[1]), float(p[2]), rgb))
- return pc2.create_cloud(header, fields, data)
-
-
-def main(args=None):
- rclpy.init(args=args)
- node = SemanticSegmentation()
- try:
- rclpy.spin(node)
- except KeyboardInterrupt:
- pass
- finally:
- node.destroy_node()
- rclpy.shutdown()
-
-
-if __name__ == '__main__':
- main()
diff --git a/src/perception_salsanext_pkg/resource/perception_salsanext_pkg b/src/perception_salsanext_pkg/resource/perception_salsanext_pkg
deleted file mode 100755
index e69de29..0000000
diff --git a/src/perception_salsanext_pkg/setup.cfg b/src/perception_salsanext_pkg/setup.cfg
deleted file mode 100755
index 2451944..0000000
--- a/src/perception_salsanext_pkg/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/perception_salsanext_pkg
-[install]
-install_scripts=$base/lib/perception_salsanext_pkg
diff --git a/src/perception_salsanext_pkg/setup.py b/src/perception_salsanext_pkg/setup.py
deleted file mode 100755
index afd16df..0000000
--- a/src/perception_salsanext_pkg/setup.py
+++ /dev/null
@@ -1,35 +0,0 @@
-
-import os
-from glob import glob
-from setuptools import find_packages, setup
-
-package_name = 'perception_salsanext_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 semantic segmentation (SalsaNext) perception module',
- license='TODO: License declaration',
- extras_require={
- 'test': [
- 'pytest',
- ],
- },
- entry_points={
- 'console_scripts': [
- 'salsanext = perception_salsanext_pkg.salsanext_node:main'
- ],
- },
-)
diff --git a/src/perception_salsanext_pkg/test/test_copyright.py b/src/perception_salsanext_pkg/test/test_copyright.py
deleted file mode 100755
index 97a3919..0000000
--- a/src/perception_salsanext_pkg/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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'
diff --git a/src/perception_salsanext_pkg/test/test_flake8.py b/src/perception_salsanext_pkg/test/test_flake8.py
deleted file mode 100755
index 27ee107..0000000
--- a/src/perception_salsanext_pkg/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# 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)
diff --git a/src/perception_salsanext_pkg/test/test_pep257.py b/src/perception_salsanext_pkg/test/test_pep257.py
deleted file mode 100755
index b234a38..0000000
--- a/src/perception_salsanext_pkg/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# 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'
diff --git a/src/perception_salsanext_pkg/test/test_range_projection.py b/src/perception_salsanext_pkg/test/test_range_projection.py
deleted file mode 100755
index 739bbce..0000000
--- a/src/perception_salsanext_pkg/test/test_range_projection.py
+++ /dev/null
@@ -1,29 +0,0 @@
-import numpy as np
-from perception_salsanext_pkg.range_projection import RangeProjector
-
-
-def test_unproject_recovers_per_point_labels():
- # 정면 부채꼴에 흩뿌린 점들 — projection 후 픽셀 라벨을 도로(0)/객체(1)로 칠하고
- # un-projection 이 점별 라벨을 되돌리는지 (충돌 없는 sparse 케이스).
- rng = np.random.default_rng(0)
- n = 500
- xyz = rng.uniform(-20, 20, size=(n, 3)).astype(np.float32)
- xyz[:, 2] = rng.uniform(-2, 1, size=n) # 적당한 높이
- pts = np.concatenate([xyz, np.zeros((n, 1), np.float32)], axis=1)
-
- proj = RangeProjector(proj_H=64, proj_W=1024, fov_up=3.0, fov_down=-25.0,
- max_range=50.0, min_range=1.0)
- out = proj.project(pts)
- assert out.range_image.shape == (5, 64, 1024)
- assert out.px.shape[0] == out.valid.sum()
-
- # 픽셀 라벨 = (depth>10 ? 1 : 0) 으로 합성 → 점별로 같은 규칙이 복원돼야 함
- depth_img = out.range_image[3]
- pixel_labels = (depth_img > 10.0).astype(np.int64)
- labels = proj.unproject(pixel_labels, out)
- assert labels.shape == (n,)
- # valid 점들에 대해 점 depth 기준 기대 라벨과 일치
- vp = pts[out.valid][:, :3]
- vdepth = np.linalg.norm(vp, axis=1)
- expected = (vdepth > 10.0).astype(np.int64)
- assert (labels[out.valid] == expected).mean() > 0.98