fix: use perception 1.0 module sources
This commit is contained in:
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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(),
|
||||
),
|
||||
])
|
||||
@@ -1,25 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>perception_bevplace2_pkg</name>
|
||||
<version>0.0.0</version>
|
||||
<description>LiDAR place recognition (BEVPlace2) perception module</description>
|
||||
<maintainer email="jang1future@gmail.com">dgist</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs_py</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>visualization_msgs</exec_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -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)
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -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 <ckpt> <scan_dir> <poses.txt> <out.npz> [--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}])")
|
||||
@@ -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 <ckpt> <rtabmap.db> <out.npz> [--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()
|
||||
@@ -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()
|
||||
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/perception_bevplace2_pkg
|
||||
[install]
|
||||
install_scripts=$base/lib/perception_bevplace2_pkg
|
||||
@@ -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'
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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
|
||||
@@ -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'
|
||||
@@ -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)
|
||||
@@ -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'
|
||||
@@ -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
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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(),
|
||||
),
|
||||
])
|
||||
@@ -1,26 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>perception_centerpoint_pkg</name>
|
||||
<version>0.0.0</version>
|
||||
<description>LiDAR 3D object detection (CenterPoint / OpenPCDet) perception module</description>
|
||||
<maintainer email="jang1future@gmail.com">dgist</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs_py</exec_depend>
|
||||
<exec_depend>vision_msgs</exec_depend>
|
||||
<exec_depend>visualization_msgs</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/perception_centerpoint_pkg
|
||||
[install]
|
||||
install_scripts=$base/lib/perception_centerpoint_pkg
|
||||
@@ -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'
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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'
|
||||
@@ -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)
|
||||
@@ -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'
|
||||
@@ -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
|
||||
@@ -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(),
|
||||
),
|
||||
])
|
||||
@@ -1,24 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>perception_lmnet_pkg</name>
|
||||
<version>0.0.0</version>
|
||||
<description>LiDAR moving object segmentation (LMNet / SalsaNext-MOS) perception module</description>
|
||||
<maintainer email="jang1future@gmail.com">dgist</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs_py</exec_depend>
|
||||
<exec_depend>nav_msgs</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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]
|
||||
@@ -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()
|
||||
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/perception_lmnet_pkg
|
||||
[install]
|
||||
install_scripts=$base/lib/perception_lmnet_pkg
|
||||
@@ -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'
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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'
|
||||
@@ -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)
|
||||
@@ -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'
|
||||
@@ -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])
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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(),
|
||||
),
|
||||
])
|
||||
@@ -1,24 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>perception_motionbev_pkg</name>
|
||||
<version>0.0.0</version>
|
||||
<description>LiDAR moving object segmentation (MotionBEV / Polar-BEV) perception module</description>
|
||||
<maintainer email="jang1future@gmail.com">dgist</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs_py</exec_depend>
|
||||
<exec_depend>nav_msgs</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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))
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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]
|
||||
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/perception_motionbev_pkg
|
||||
[install]
|
||||
install_scripts=$base/lib/perception_motionbev_pkg
|
||||
@@ -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'
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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'
|
||||
@@ -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)
|
||||
@@ -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'
|
||||
@@ -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], # rho<min → 제외
|
||||
[100.0, 0.0, 0.0, 1.0]], # rho>max → 제외
|
||||
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()
|
||||
@@ -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])
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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(),
|
||||
),
|
||||
])
|
||||
@@ -1,25 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>perception_overlaptransformer_pkg</name>
|
||||
<version>0.0.0</version>
|
||||
<description>LiDAR place recognition (OverlapTransformer) perception module</description>
|
||||
<maintainer email="jang1future@gmail.com">dgist</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs_py</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>visualization_msgs</exec_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
-35
@@ -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 <ckpt> <scan_dir> <poses.txt> <out.npz> [--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})")
|
||||
-113
@@ -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 <ckpt> <rtabmap.db> <out.npz> [--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()
|
||||
-33
@@ -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()
|
||||
@@ -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
|
||||
@@ -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()
|
||||
-68
@@ -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
|
||||
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/perception_overlaptransformer_pkg
|
||||
[install]
|
||||
install_scripts=$base/lib/perception_overlaptransformer_pkg
|
||||
@@ -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'
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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'
|
||||
@@ -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)
|
||||
@@ -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'
|
||||
@@ -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
|
||||
@@ -12,18 +12,12 @@ def test_level_none_skipped():
|
||||
|
||||
|
||||
def test_perception_entries_appended():
|
||||
# LocalLauncher buildPerceptionArg 실제 컨벤션: perception_<model_key>_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
|
||||
|
||||
@@ -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
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user