From 31d1d09917cd4feb8ee36ea2c14f0d9e15cef3c6 Mon Sep 17 00:00:00 2001 From: chyoo Date: Tue, 28 Jul 2026 16:55:59 +0900 Subject: [PATCH] fix: use perception 1.0 module sources --- src/perception3d_pkg/config/3d_fr09.yaml | 3 - src/perception3d_pkg/config/3d_mkmini.yaml | 3 - .../config/bevplace.yaml | 21 -- .../launch/bevplace2.launch.py | 149 --------- .../launch/perception.launch.py | 32 -- src/perception_bevplace2_pkg/package.xml | 25 -- .../perception_bevplace2_pkg/__init__.py | 0 .../bev_projection.py | 62 ---- .../bevplace2_node.py | 132 -------- .../bevplace_backend.py | 75 ----- .../perception_bevplace2_pkg/build_map_db.py | 47 --- .../perception_bevplace2_pkg/db_to_mapdb.py | 124 ------- .../perception_bevplace2_pkg/descriptor_db.py | 34 -- .../resource/perception_bevplace2_pkg | 0 src/perception_bevplace2_pkg/setup.cfg | 4 - src/perception_bevplace2_pkg/setup.py | 35 -- .../test/test_bev_projection.py | 33 -- .../test/test_copyright.py | 25 -- .../test/test_flake8.py | 25 -- .../test/test_pep257.py | 23 -- .../cfgs/centerpoint_custom.yaml | 98 ------ .../cfgs/custom_dataset.yaml | 58 ---- .../config/centerpoint.yaml | 21 -- .../launch/centerpoint.launch.py | 127 ------- .../launch/perception.launch.py | 32 -- src/perception_centerpoint_pkg/package.xml | 26 -- .../perception_centerpoint_pkg/__init__.py | 0 .../centerpoint_backend.py | 219 ------------ .../centerpoint_node.py | 309 ----------------- .../resource/perception_centerpoint_pkg | 0 src/perception_centerpoint_pkg/setup.cfg | 4 - src/perception_centerpoint_pkg/setup.py | 38 --- .../test/test_copyright.py | 25 -- .../test/test_flake8.py | 25 -- .../test/test_pep257.py | 23 -- .../launch/lmnet.launch.py | 58 ---- .../launch/perception.launch.py | 32 -- src/perception_lmnet_pkg/package.xml | 24 -- .../perception_lmnet_pkg/__init__.py | 0 .../perception_lmnet_pkg/lmnet_infer.py | 97 ------ .../perception_lmnet_pkg/lmnet_node.py | 169 ---------- .../perception_lmnet_pkg/pose_util.py | 25 -- .../perception_lmnet_pkg/range_projection.py | 68 ---- .../perception_lmnet_pkg/residual_lib.py | 28 -- .../perception_lmnet_pkg/resolve_model.py | 43 --- .../perception_lmnet_pkg/scan_buffer.py | 21 -- .../perception_lmnet_pkg/spawn_npc.py | 129 -------- .../resource/perception_lmnet_pkg | 0 src/perception_lmnet_pkg/setup.cfg | 4 - src/perception_lmnet_pkg/setup.py | 35 -- .../test/test_copyright.py | 25 -- src/perception_lmnet_pkg/test/test_flake8.py | 25 -- src/perception_lmnet_pkg/test/test_pep257.py | 23 -- .../test/test_pose_util.py | 22 -- .../test/test_residual_lib.py | 22 -- .../test/test_resolve_model.py | 89 ----- .../test/test_scan_buffer.py | 37 --- .../config/motionbev.yaml | 11 - .../launch/motionbev.launch.py | 59 ---- .../launch/perception.launch.py | 32 -- src/perception_motionbev_pkg/package.xml | 24 -- .../perception_motionbev_pkg/__init__.py | 0 .../motionbev_infer.py | 158 --------- .../motionbev_node.py | 163 --------- .../polar_projection.py | 127 ------- .../perception_motionbev_pkg/pose_util.py | 25 -- .../perception_motionbev_pkg/residual_lib.py | 29 -- .../perception_motionbev_pkg/resolve_model.py | 43 --- .../perception_motionbev_pkg/scan_buffer.py | 21 -- .../resource/perception_motionbev_pkg | 0 src/perception_motionbev_pkg/setup.cfg | 4 - src/perception_motionbev_pkg/setup.py | 35 -- .../test/test_copyright.py | 25 -- .../test/test_flake8.py | 25 -- .../test/test_pep257.py | 23 -- .../test/test_polar_projection.py | 31 -- .../test/test_pose_util.py | 22 -- .../test/test_residual_lib.py | 21 -- .../test/test_resolve_model.py | 72 ---- .../test/test_scan_buffer.py | 37 --- .../config/ot.yaml | 19 -- .../launch/ot.launch.py | 161 --------- .../launch/perception.launch.py | 32 -- .../package.xml | 25 -- .../__init__.py | 0 .../build_map_db.py | 35 -- .../db_to_mapdb.py | 113 ------- .../descriptor_db.py | 33 -- .../ot_backend.py | 43 --- .../ot_node.py | 135 -------- .../range_projection.py | 68 ---- .../perception_overlaptransformer_pkg | 0 .../setup.cfg | 4 - .../setup.py | 35 -- .../test/test_copyright.py | 25 -- .../test/test_flake8.py | 25 -- .../test/test_pep257.py | 23 -- .../test/test_range_projection.py | 29 -- src/perception_pkg/test/test_launch_spec.py | 14 +- .../cfgs/custom_dataset.yaml | 58 ---- .../cfgs/pointpillar_custom.yaml | 119 ------- .../config/pointpillars.yaml | 21 -- .../pointpillars.launch.cpython-310.pyc | Bin 3640 -> 0 bytes .../launch/perception.launch.py | 32 -- .../launch/pointpillars.launch.py | 131 -------- src/perception_pointpillars_pkg/package.xml | 26 -- .../perception_pointpillars_pkg/__init__.py | 0 .../pointpillars_backend.py | 201 ----------- .../pointpillars_node.py | 313 ------------------ .../resource/perception_pointpillars_pkg | 0 src/perception_pointpillars_pkg/setup.cfg | 4 - src/perception_pointpillars_pkg/setup.py | 37 --- .../test/test_copyright.py | 25 -- .../test/test_flake8.py | 25 -- .../test/test_pep257.py | 23 -- .../config/rangenetpp.yaml | 23 -- .../config/semantic_kitti_labels.yaml | 22 -- .../launch/perception.launch.py | 32 -- .../launch/rangenetpp.launch.py | 86 ----- src/perception_rangenetpp_pkg/package.xml | 24 -- .../perception_rangenetpp_pkg/__init__.py | 0 .../range_projection.py | 68 ---- .../rangenetpp_backend.py | 125 ------- .../rangenetpp_node.py | 149 --------- .../resource/perception_rangenetpp_pkg | 0 src/perception_rangenetpp_pkg/setup.cfg | 4 - src/perception_rangenetpp_pkg/setup.py | 35 -- .../test/test_copyright.py | 25 -- .../test/test_flake8.py | 25 -- .../test/test_pep257.py | 23 -- .../test/test_range_projection.py | 29 -- .../config/salsanext.yaml | 23 -- .../config/semantic_kitti_labels.yaml | 22 -- .../launch/perception.launch.py | 32 -- .../launch/salsanext.launch.py | 80 ----- src/perception_salsanext_pkg/package.xml | 24 -- .../perception_salsanext_pkg/__init__.py | 0 .../range_projection.py | 68 ---- .../perception_salsanext_pkg/resolve_model.py | 44 --- .../salsanext_backend.py | 106 ------ .../salsanext_node.py | 183 ---------- .../resource/perception_salsanext_pkg | 0 src/perception_salsanext_pkg/setup.cfg | 4 - src/perception_salsanext_pkg/setup.py | 35 -- .../test/test_copyright.py | 25 -- .../test/test_flake8.py | 25 -- .../test/test_pep257.py | 23 -- .../test/test_range_projection.py | 29 -- 148 files changed, 4 insertions(+), 7043 deletions(-) delete mode 100755 src/perception_bevplace2_pkg/config/bevplace.yaml delete mode 100755 src/perception_bevplace2_pkg/launch/bevplace2.launch.py delete mode 100755 src/perception_bevplace2_pkg/launch/perception.launch.py delete mode 100755 src/perception_bevplace2_pkg/package.xml delete mode 100755 src/perception_bevplace2_pkg/perception_bevplace2_pkg/__init__.py delete mode 100755 src/perception_bevplace2_pkg/perception_bevplace2_pkg/bev_projection.py delete mode 100755 src/perception_bevplace2_pkg/perception_bevplace2_pkg/bevplace2_node.py delete mode 100755 src/perception_bevplace2_pkg/perception_bevplace2_pkg/bevplace_backend.py delete mode 100755 src/perception_bevplace2_pkg/perception_bevplace2_pkg/build_map_db.py delete mode 100755 src/perception_bevplace2_pkg/perception_bevplace2_pkg/db_to_mapdb.py delete mode 100755 src/perception_bevplace2_pkg/perception_bevplace2_pkg/descriptor_db.py delete mode 100755 src/perception_bevplace2_pkg/resource/perception_bevplace2_pkg delete mode 100755 src/perception_bevplace2_pkg/setup.cfg delete mode 100755 src/perception_bevplace2_pkg/setup.py delete mode 100755 src/perception_bevplace2_pkg/test/test_bev_projection.py delete mode 100755 src/perception_bevplace2_pkg/test/test_copyright.py delete mode 100755 src/perception_bevplace2_pkg/test/test_flake8.py delete mode 100755 src/perception_bevplace2_pkg/test/test_pep257.py delete mode 100755 src/perception_centerpoint_pkg/cfgs/centerpoint_custom.yaml delete mode 100755 src/perception_centerpoint_pkg/cfgs/custom_dataset.yaml delete mode 100755 src/perception_centerpoint_pkg/config/centerpoint.yaml delete mode 100755 src/perception_centerpoint_pkg/launch/centerpoint.launch.py delete mode 100755 src/perception_centerpoint_pkg/launch/perception.launch.py delete mode 100755 src/perception_centerpoint_pkg/package.xml delete mode 100755 src/perception_centerpoint_pkg/perception_centerpoint_pkg/__init__.py delete mode 100755 src/perception_centerpoint_pkg/perception_centerpoint_pkg/centerpoint_backend.py delete mode 100755 src/perception_centerpoint_pkg/perception_centerpoint_pkg/centerpoint_node.py delete mode 100755 src/perception_centerpoint_pkg/resource/perception_centerpoint_pkg delete mode 100755 src/perception_centerpoint_pkg/setup.cfg delete mode 100755 src/perception_centerpoint_pkg/setup.py delete mode 100755 src/perception_centerpoint_pkg/test/test_copyright.py delete mode 100755 src/perception_centerpoint_pkg/test/test_flake8.py delete mode 100755 src/perception_centerpoint_pkg/test/test_pep257.py delete mode 100755 src/perception_lmnet_pkg/launch/lmnet.launch.py delete mode 100755 src/perception_lmnet_pkg/launch/perception.launch.py delete mode 100755 src/perception_lmnet_pkg/package.xml delete mode 100755 src/perception_lmnet_pkg/perception_lmnet_pkg/__init__.py delete mode 100755 src/perception_lmnet_pkg/perception_lmnet_pkg/lmnet_infer.py delete mode 100755 src/perception_lmnet_pkg/perception_lmnet_pkg/lmnet_node.py delete mode 100755 src/perception_lmnet_pkg/perception_lmnet_pkg/pose_util.py delete mode 100755 src/perception_lmnet_pkg/perception_lmnet_pkg/range_projection.py delete mode 100755 src/perception_lmnet_pkg/perception_lmnet_pkg/residual_lib.py delete mode 100755 src/perception_lmnet_pkg/perception_lmnet_pkg/resolve_model.py delete mode 100755 src/perception_lmnet_pkg/perception_lmnet_pkg/scan_buffer.py delete mode 100755 src/perception_lmnet_pkg/perception_lmnet_pkg/spawn_npc.py delete mode 100755 src/perception_lmnet_pkg/resource/perception_lmnet_pkg delete mode 100755 src/perception_lmnet_pkg/setup.cfg delete mode 100755 src/perception_lmnet_pkg/setup.py delete mode 100755 src/perception_lmnet_pkg/test/test_copyright.py delete mode 100755 src/perception_lmnet_pkg/test/test_flake8.py delete mode 100755 src/perception_lmnet_pkg/test/test_pep257.py delete mode 100755 src/perception_lmnet_pkg/test/test_pose_util.py delete mode 100755 src/perception_lmnet_pkg/test/test_residual_lib.py delete mode 100755 src/perception_lmnet_pkg/test/test_resolve_model.py delete mode 100755 src/perception_lmnet_pkg/test/test_scan_buffer.py delete mode 100755 src/perception_motionbev_pkg/config/motionbev.yaml delete mode 100755 src/perception_motionbev_pkg/launch/motionbev.launch.py delete mode 100755 src/perception_motionbev_pkg/launch/perception.launch.py delete mode 100755 src/perception_motionbev_pkg/package.xml delete mode 100755 src/perception_motionbev_pkg/perception_motionbev_pkg/__init__.py delete mode 100755 src/perception_motionbev_pkg/perception_motionbev_pkg/motionbev_infer.py delete mode 100755 src/perception_motionbev_pkg/perception_motionbev_pkg/motionbev_node.py delete mode 100755 src/perception_motionbev_pkg/perception_motionbev_pkg/polar_projection.py delete mode 100755 src/perception_motionbev_pkg/perception_motionbev_pkg/pose_util.py delete mode 100755 src/perception_motionbev_pkg/perception_motionbev_pkg/residual_lib.py delete mode 100755 src/perception_motionbev_pkg/perception_motionbev_pkg/resolve_model.py delete mode 100755 src/perception_motionbev_pkg/perception_motionbev_pkg/scan_buffer.py delete mode 100755 src/perception_motionbev_pkg/resource/perception_motionbev_pkg delete mode 100755 src/perception_motionbev_pkg/setup.cfg delete mode 100755 src/perception_motionbev_pkg/setup.py delete mode 100755 src/perception_motionbev_pkg/test/test_copyright.py delete mode 100755 src/perception_motionbev_pkg/test/test_flake8.py delete mode 100755 src/perception_motionbev_pkg/test/test_pep257.py delete mode 100755 src/perception_motionbev_pkg/test/test_polar_projection.py delete mode 100755 src/perception_motionbev_pkg/test/test_pose_util.py delete mode 100755 src/perception_motionbev_pkg/test/test_residual_lib.py delete mode 100755 src/perception_motionbev_pkg/test/test_resolve_model.py delete mode 100755 src/perception_motionbev_pkg/test/test_scan_buffer.py delete mode 100755 src/perception_overlaptransformer_pkg/config/ot.yaml delete mode 100755 src/perception_overlaptransformer_pkg/launch/ot.launch.py delete mode 100755 src/perception_overlaptransformer_pkg/launch/perception.launch.py delete mode 100755 src/perception_overlaptransformer_pkg/package.xml delete mode 100755 src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/__init__.py delete mode 100755 src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/build_map_db.py delete mode 100755 src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/db_to_mapdb.py delete mode 100755 src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/descriptor_db.py delete mode 100755 src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/ot_backend.py delete mode 100755 src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/ot_node.py delete mode 100755 src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/range_projection.py delete mode 100755 src/perception_overlaptransformer_pkg/resource/perception_overlaptransformer_pkg delete mode 100755 src/perception_overlaptransformer_pkg/setup.cfg delete mode 100755 src/perception_overlaptransformer_pkg/setup.py delete mode 100755 src/perception_overlaptransformer_pkg/test/test_copyright.py delete mode 100755 src/perception_overlaptransformer_pkg/test/test_flake8.py delete mode 100755 src/perception_overlaptransformer_pkg/test/test_pep257.py delete mode 100755 src/perception_overlaptransformer_pkg/test/test_range_projection.py delete mode 100755 src/perception_pointpillars_pkg/cfgs/custom_dataset.yaml delete mode 100755 src/perception_pointpillars_pkg/cfgs/pointpillar_custom.yaml delete mode 100755 src/perception_pointpillars_pkg/config/pointpillars.yaml delete mode 100644 src/perception_pointpillars_pkg/launch/__pycache__/pointpillars.launch.cpython-310.pyc delete mode 100755 src/perception_pointpillars_pkg/launch/perception.launch.py delete mode 100755 src/perception_pointpillars_pkg/launch/pointpillars.launch.py delete mode 100755 src/perception_pointpillars_pkg/package.xml delete mode 100755 src/perception_pointpillars_pkg/perception_pointpillars_pkg/__init__.py delete mode 100755 src/perception_pointpillars_pkg/perception_pointpillars_pkg/pointpillars_backend.py delete mode 100755 src/perception_pointpillars_pkg/perception_pointpillars_pkg/pointpillars_node.py delete mode 100755 src/perception_pointpillars_pkg/resource/perception_pointpillars_pkg delete mode 100755 src/perception_pointpillars_pkg/setup.cfg delete mode 100755 src/perception_pointpillars_pkg/setup.py delete mode 100755 src/perception_pointpillars_pkg/test/test_copyright.py delete mode 100755 src/perception_pointpillars_pkg/test/test_flake8.py delete mode 100755 src/perception_pointpillars_pkg/test/test_pep257.py delete mode 100755 src/perception_rangenetpp_pkg/config/rangenetpp.yaml delete mode 100755 src/perception_rangenetpp_pkg/config/semantic_kitti_labels.yaml delete mode 100755 src/perception_rangenetpp_pkg/launch/perception.launch.py delete mode 100755 src/perception_rangenetpp_pkg/launch/rangenetpp.launch.py delete mode 100755 src/perception_rangenetpp_pkg/package.xml delete mode 100755 src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/__init__.py delete mode 100755 src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/range_projection.py delete mode 100755 src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/rangenetpp_backend.py delete mode 100755 src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/rangenetpp_node.py delete mode 100755 src/perception_rangenetpp_pkg/resource/perception_rangenetpp_pkg delete mode 100755 src/perception_rangenetpp_pkg/setup.cfg delete mode 100755 src/perception_rangenetpp_pkg/setup.py delete mode 100755 src/perception_rangenetpp_pkg/test/test_copyright.py delete mode 100755 src/perception_rangenetpp_pkg/test/test_flake8.py delete mode 100755 src/perception_rangenetpp_pkg/test/test_pep257.py delete mode 100755 src/perception_rangenetpp_pkg/test/test_range_projection.py delete mode 100755 src/perception_salsanext_pkg/config/salsanext.yaml delete mode 100755 src/perception_salsanext_pkg/config/semantic_kitti_labels.yaml delete mode 100755 src/perception_salsanext_pkg/launch/perception.launch.py delete mode 100755 src/perception_salsanext_pkg/launch/salsanext.launch.py delete mode 100755 src/perception_salsanext_pkg/package.xml delete mode 100755 src/perception_salsanext_pkg/perception_salsanext_pkg/__init__.py delete mode 100755 src/perception_salsanext_pkg/perception_salsanext_pkg/range_projection.py delete mode 100755 src/perception_salsanext_pkg/perception_salsanext_pkg/resolve_model.py delete mode 100755 src/perception_salsanext_pkg/perception_salsanext_pkg/salsanext_backend.py delete mode 100755 src/perception_salsanext_pkg/perception_salsanext_pkg/salsanext_node.py delete mode 100755 src/perception_salsanext_pkg/resource/perception_salsanext_pkg delete mode 100755 src/perception_salsanext_pkg/setup.cfg delete mode 100755 src/perception_salsanext_pkg/setup.py delete mode 100755 src/perception_salsanext_pkg/test/test_copyright.py delete mode 100755 src/perception_salsanext_pkg/test/test_flake8.py delete mode 100755 src/perception_salsanext_pkg/test/test_pep257.py delete mode 100755 src/perception_salsanext_pkg/test/test_range_projection.py 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 22406c00fd4eedb83c4b738a1b5321e49ac004db..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3640 zcmahLYj51f@yO%xcu$=kw&U1wSfB-TXnp6nZQKTJQ$Y_mtzWg|FEk8-zTLZ{#Ut6} z(y@dS>eO~n#1i5raT42-Aq5E%2LkHWN}~e)7yZVM?x*UgFMWI|(5SOZ>U4etA;ICy z?(EF$Ja)$^6tW3;@ZZi~=-Zn}{GAcKj{?Nc;qx8=KnWyR2`%~yngFzDiLIoT6xdkO zl3FP(#c|1!TWKxL@f6^Srf^&aJfmeeo(4RtWjU@`xmI4ww+dQ;(;2JS>eKoF&uS%< z)A~_f8$bnZ5EZo{)TiyjX;eb}XkbAEY!D4K5@^>)0@}Th1bPp84&J>DF{Yo#rI>Rc z=4%=$Z8zGFUbvdj_MijkMfg95gvmr%{RiwbaiS~)`x=<)u2Gvd8d&$52EjTq39eCx z%)oU2F~hfO%`xoNi0M+(v0-r67_M2s8O@E5hTp7VzgN&Q%obizS+_DI!nY4xOw z?Ng&;m{zIdSYEYOZ+O*dlTuS}0T+z8Ab`521vXY@jFvTfh%f9J)?I=rF-#kyUT*!! zk;6Z#xU@MtT6Hn0VZP&Mi1n&)34t!GN|D}=xW@5;052K zPD_u6qiw(X^~+=SryZt8Gh9HB_dQ4ov!zqBg#(EaA44n)2-gkYqWVR{^0Ag8&UuFh zS?UvNVL-oMPLd*|mnOHSTc&MlQk@+7aakk<7!1<2reoHy7v#Be-E(|!w3g|*F39e< zB`8Kr5MxME+6?+)dfH`KOPe);C35^Qr8)Rc!sp$9848trMVL#%U%VnL3+=>;Fed?( z>{2P9q%I{>5v5>7nkL(6B%^c#IOmjh3Mo{oix-8tOj`y##qn$-F_&v+yqDWqGSSX0 z%QTHL?bIEp=C%wlx0FKp1z|-H5;ks2s4$%*SEzC!vz(>56#?-4a)HxD&{jl!D@oe7 zA_@r@EAjDuI-b8>9KAkE!LEQwvs!?DIe(0Heva_6Z8 z{DWQoAv*XNe+hW*GM={VeneAUzFpfA(QdS-A;yv3x0QCX4J**T#biTl z=h6NJxgnqz7KFJ%JJ&8Wggps3cMkafgs^lz|E#mHu0l|&H3+0iu;*A8vATKZs@l1> z-g$RX4X-SPSO26AcdmcddHb4*uxmLpD)H@#*Hpu$57lnEsNwDV;niiev$7a|`UcFc zh1ZwLm26gp=b#$?ZZ*7pFT8tW+F@#T7F{xVv*-B9vGHU2$mye#%=}ol8ZKY$TwmS1`C;d+_hJp8G+bFzJ8$0#@BRU{*sEu0 zZDXCO8t$yFZQh>W`0_IBKHmJ&dc5gN1BERC%Klft{JYWTrw=P%Ko@7@3t)r~*j50`Il-dqH$B2x$}OHyKBfPp~( z+PyV+FSC-UAZ@v>}X^+g| z^Tsqr5KdkXWpOrHrFBiDbwIO#>K=tFxTa-x(0tE^J#B92qw#;4Al7$4I|IlI?{&ch0nn&ZxJ4(S5)6v4pgnaQb>$Mvz1sgcR?DSd2m zDo|R?u_hRw2FQ0-irFHY7*pTmFGfy-^i!wDxdxJAJ77I&xd=C+V^e&I29iMCVnG%%7Y@5?^vHvu0AQz|wt(%vnxDk3CBEgK8~nIR?^gXqvox z3Bro%KX(9Bn@$T?Yt0$QsX_&Zx7XlsfXBM+ThyCKtnjNAZ#a9qGOrSiRos~%AE7$5KE7K4W4&1b zN~S2VR}qUsUd#)Us0cEAq9AW&w}z5ZvIw~J)$XJOTnv){lLbY{ZskQq+{P6N*1^28 zB_@GuOH7HP2sD2IWq;{^;#Z;~{Y%^e*+b+osIS4-eON=nqB{zo$J|U4lrBY6;rxLKImKuGF{N;uJ~3iXa15!kOdaC#EK~tbtG*HIjxouCOd?DZmcGvz}VDMP>D?$m}Lw*Qoc`x}E{K$6zX#1c9ZK|6#!k2V+hBh6$ z64$QB>rIm*soX-X7>8s#pCl%TCpA;TE!Bo%9w&k#rAO{$D zk%4bB@I40DAxpl?zz-N;Zs(imaRN(V!abq - - - 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