commit 29472a4c3ece6a78b688d73d7988369422eda5ec Author: 유창훈 Date: Tue Jul 28 10:35:46 2026 +0900 feat: add lms-nav2-gazebo-perception module diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..76b3365 --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +build/ +install/ +log/ +.colcon/ +*.log diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..0c1c5bc --- /dev/null +++ b/build.sh @@ -0,0 +1,6 @@ +#!/usr/bin/env bash +set -euo pipefail + +MODULE_PATH="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +RUNTIME_ROOT="${AUTONOMY_STUDIO_RUNTIME_ROOT:-$HOME/autonomy-studio/runtime/nav2}" +exec bash "$RUNTIME_ROOT/docker/docker_build_nav2.sh" "$(dirname "$MODULE_PATH")" "$(basename "$MODULE_PATH")" "${NAV2_DOCKER_IMAGE:-future-nav2-gazebo:humble}" "${ROS_DOMAIN_ID:-0}" diff --git a/run.sh b/run.sh new file mode 100755 index 0000000..543a742 --- /dev/null +++ b/run.sh @@ -0,0 +1,8 @@ +#!/usr/bin/env bash +set -euo pipefail + +MODULE_PATH="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +RUNTIME_ROOT="${AUTONOMY_STUDIO_RUNTIME_ROOT:-$HOME/autonomy-studio/runtime/nav2}" +FOLDER_NAME="$(basename "$MODULE_PATH")" +CONTAINER_NAME="${FOLDER_NAME#\[}"; CONTAINER_NAME="${CONTAINER_NAME/\]/-}" +exec bash "$RUNTIME_ROOT/docker/docker_run_nav2.sh" "$CONTAINER_NAME" "$(dirname "$MODULE_PATH")" "$FOLDER_NAME" "${NAV2_DOCKER_IMAGE:-future-nav2-gazebo:humble}" "${ROS_DOMAIN_ID:-0}" sim_mode:=true "$@" diff --git a/src/perception2d_pkg/config/perception_fr09.yaml b/src/perception2d_pkg/config/perception_fr09.yaml new file mode 100755 index 0000000..9e5d61a --- /dev/null +++ b/src/perception2d_pkg/config/perception_fr09.yaml @@ -0,0 +1,17 @@ +# File: fr09/perception.yaml +# Description: Perception parameters for FR-09 + +pointcloud_to_laserscan: + ros__parameters: + use_inf: true + target_frame: os_lidar + transform_tolerance: 2.0 + min_height: -0.98 + max_height: 0.00 + angle_min: -3.14159 + angle_max: 3.14159 + angle_increment: 0.0087 + range_min: 0.19 + range_max: 12.0 + qos_overrides./scan.subscriber.reliability: best_effort + qos_overrides./scan.publisher.reliability: reliable diff --git a/src/perception2d_pkg/config/perception_mkmini.yaml b/src/perception2d_pkg/config/perception_mkmini.yaml new file mode 100755 index 0000000..fd00cb1 --- /dev/null +++ b/src/perception2d_pkg/config/perception_mkmini.yaml @@ -0,0 +1,18 @@ +# File: mkmini/perception.yaml +# Description: Perception parameters for MK-Mini + +pointcloud_to_laserscan: + ros__parameters: + use_inf: true + target_frame: os_lidar + transform_tolerance: 2.0 + min_height: -0.46 + max_height: 0.10 + angle_min: -3.14159 + angle_max: 3.14159 + angle_increment: 0.0087 + # range_min: 0.2 + range_min: 2.2 + range_max: 12.0 + qos_overrides./scan.subscriber.reliability: best_effort + qos_overrides./scan.publisher.reliability: reliable diff --git a/src/perception2d_pkg/launch/perception.launch.py b/src/perception2d_pkg/launch/perception.launch.py new file mode 100755 index 0000000..fd5b301 --- /dev/null +++ b/src/perception2d_pkg/launch/perception.launch.py @@ -0,0 +1,54 @@ +# File: perception.launch.py +# Description: +# 공통 perception 파이프라인 — odometry, static TF, pointcloud→laserscan +# Inputs : /raw/lidar, /raw/imu, /raw/speed +# Outputs: /odom, /scan +# TF : odom→base_link, base_link→os_lidar, base_link→os_imu +# +# Note: +# - sensor_bridge가 먼저 실행되어 /raw/* 토픽을 발행 중이어야 함 +# - 차량 모델 추가 시 ARGUMENTS의 choices 목록과 config/{robot}/ 디렉토리 추가 + +import os +import yaml + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +ARGUMENTS = [ + DeclareLaunchArgument('robot', default_value='fr09', choices=['fr09', 'mkmini']) +] + + +def launch_setup(context, *args, **kwargs): + robot = LaunchConfiguration('robot').perform(context) + + pkg_share = get_package_share_directory('perception2d_pkg') + config_dir = os.path.join(pkg_share, 'config', f'perception_{robot}.yaml') + + return [ + Node( + package='pointcloud_to_laserscan', + executable='pointcloud_to_laserscan_node', + name='pointcloud_to_laserscan', + output='screen', + parameters=[ + config_dir, + {'use_sim_time': True}, + ], + remappings=[ + ('cloud_in', '/raw/lidar'), # 추후에 config에서 바꿀 수 있도록 변경하는게 좋을 것 같음. + ('scan', '/scan'), + ], + ), + ] + + +def generate_launch_description(): + ld = LaunchDescription(ARGUMENTS) + ld.add_action(OpaqueFunction(function=launch_setup)) + return ld diff --git a/src/perception2d_pkg/package.xml b/src/perception2d_pkg/package.xml new file mode 100755 index 0000000..286734c --- /dev/null +++ b/src/perception2d_pkg/package.xml @@ -0,0 +1,18 @@ + + + + perception2d_pkg + 0.0.0 + TODO: Package description + dgist + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception2d_pkg/perception2d_pkg/__init__.py b/src/perception2d_pkg/perception2d_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/perception2d_pkg/perception2d_pkg/odom_tf_from_map_pose.py b/src/perception2d_pkg/perception2d_pkg/odom_tf_from_map_pose.py new file mode 100755 index 0000000..4376d72 --- /dev/null +++ b/src/perception2d_pkg/perception2d_pkg/odom_tf_from_map_pose.py @@ -0,0 +1,209 @@ +#!/usr/bin/env python3 + +import math +from typing import Tuple + +import rclpy +from geometry_msgs.msg import TransformStamped +from nav_msgs.msg import Odometry +from rclpy.node import Node +from tf2_ros import TransformBroadcaster + + +def _normalize_quat(q: Tuple[float, float, float, float]) -> Tuple[float, float, float, float]: + x, y, z, w = q + n = math.sqrt(x * x + y * y + z * z + w * w) + if n < 1e-12: + return (0.0, 0.0, 0.0, 1.0) + return (x / n, y / n, z / n, w / n) + + +def _quat_inverse(q: Tuple[float, float, float, float]) -> Tuple[float, float, float, float]: + x, y, z, w = _normalize_quat(q) + return (-x, -y, -z, w) + + +def _quat_multiply( + q1: Tuple[float, float, float, float], + q2: Tuple[float, float, float, float], +) -> Tuple[float, float, float, float]: + x1, y1, z1, w1 = q1 + x2, y2, z2, w2 = q2 + return ( + w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2, + w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2, + w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2, + w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2, + ) + + +def _rotate_vec_by_quat_inv( + q: Tuple[float, float, float, float], + v: Tuple[float, float, float], +) -> Tuple[float, float, float]: + # v' = q^-1 * [v, 0] * q + qi = _quat_inverse(q) + vq = (v[0], v[1], v[2], 0.0) + r = _quat_multiply(_quat_multiply(qi, vq), q) + return (r[0], r[1], r[2]) + + +def _yaw_from_quat(q: Tuple[float, float, float, float]) -> float: + x, y, z, w = _normalize_quat(q) + # Standard ZYX yaw extraction. + return math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)) + + +def _quat_from_yaw(yaw: float) -> Tuple[float, float, float, float]: + h = yaw * 0.5 + return (0.0, 0.0, math.sin(h), math.cos(h)) + + +def _normalize_angle(a: float) -> float: + return math.atan2(math.sin(a), math.cos(a)) + + +class OdomTfFromMapPose(Node): + def __init__(self) -> None: + super().__init__('odom_tf_from_map_pose') + + self.declare_parameter('input_topic', '/carla/hero/odometry') + self.declare_parameter('output_odom_topic', '/odom_gt') + self.declare_parameter('odom_frame', 'odom_gt') + self.declare_parameter('base_frame', 'base_link') + self.declare_parameter('publish_rate', 30.0) + self.declare_parameter('use_msg_stamp', False) + + self._input_topic = self.get_parameter('input_topic').get_parameter_value().string_value + self._output_odom_topic = self.get_parameter('output_odom_topic').get_parameter_value().string_value + self._odom_frame = self.get_parameter('odom_frame').get_parameter_value().string_value + self._base_frame = self.get_parameter('base_frame').get_parameter_value().string_value + self._publish_rate = self.get_parameter('publish_rate').get_parameter_value().double_value + self._use_msg_stamp = self.get_parameter('use_msg_stamp').get_parameter_value().bool_value + + self._tf_broadcaster = TransformBroadcaster(self) + self._odom_publisher = self.create_publisher(Odometry, self._output_odom_topic, 10) + self._sub = self.create_subscription(Odometry, self._input_topic, self._on_odom, 10) + self._timer = None + if not self._use_msg_stamp: + self._timer = self.create_timer(1.0 / max(self._publish_rate, 1.0), self._on_timer) + + self._have_t0 = False + self._have_latest = False + self._p0_xy = (0.0, 0.0) + self._yaw0 = 0.0 + self._latest_translation_xy = (0.0, 0.0) + self._latest_q_rel = (0.0, 0.0, 0.0, 1.0) + self._latest_msg_stamp = None + self._latest_linear_xy = (0.0, 0.0) + self._latest_angular_z = 0.0 + + self.get_logger().info( + f'Listening {self._input_topic} and publishing TF {self._odom_frame} -> {self._base_frame} ' + f'and Odometry on {self._output_odom_topic} ' + f'(publish_rate={self._publish_rate}Hz, use_msg_stamp={self._use_msg_stamp}, ' + f'timer_enabled={self._timer is not None})' + ) + + def _on_odom(self, msg: Odometry) -> None: + p = ( + msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z, + ) + q = ( + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w, + ) + q = _normalize_quat(q) + + yaw = _yaw_from_quat(q) + + if not self._have_t0: + self._p0_xy = (p[0], p[1]) + self._yaw0 = yaw + self._have_t0 = True + self.get_logger().info('Captured initial pose T0 from odometry') + + # 2D relative transform from initial pose: + # - Use only x, y, yaw + # - Force z, roll, pitch to zero for 2D SLAM consistency + dx = p[0] - self._p0_xy[0] + dy = p[1] - self._p0_xy[1] + + c0 = math.cos(self._yaw0) + s0 = math.sin(self._yaw0) + x_rel = c0 * dx + s0 * dy + y_rel = -s0 * dx + c0 * dy + yaw_rel = _normalize_angle(yaw - self._yaw0) + q_rel = _quat_from_yaw(yaw_rel) + self._latest_translation_xy = (x_rel, y_rel) + self._latest_q_rel = q_rel + self._latest_msg_stamp = msg.header.stamp + self._latest_linear_xy = ( + float(msg.twist.twist.linear.x), + float(msg.twist.twist.linear.y), + ) + self._latest_angular_z = float(msg.twist.twist.angular.z) + self._have_latest = True + self._publish_transform() + + def _on_timer(self) -> None: + self._publish_transform() + + def _publish_transform(self) -> None: + if not self._have_latest: + return + + t = TransformStamped() + if self._use_msg_stamp and self._latest_msg_stamp is not None: + t.header.stamp = self._latest_msg_stamp + else: + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = self._odom_frame + t.child_frame_id = self._base_frame + + t.transform.translation.x = self._latest_translation_xy[0] + t.transform.translation.y = self._latest_translation_xy[1] + t.transform.translation.z = 0.0 + + t.transform.rotation.x = self._latest_q_rel[0] + t.transform.rotation.y = self._latest_q_rel[1] + t.transform.rotation.z = self._latest_q_rel[2] + t.transform.rotation.w = self._latest_q_rel[3] + + self._tf_broadcaster.sendTransform(t) + + odom = Odometry() + odom.header.stamp = t.header.stamp + odom.header.frame_id = self._odom_frame + odom.child_frame_id = self._base_frame + odom.pose.pose.position.x = self._latest_translation_xy[0] + odom.pose.pose.position.y = self._latest_translation_xy[1] + odom.pose.pose.position.z = 0.0 + odom.pose.pose.orientation.x = self._latest_q_rel[0] + odom.pose.pose.orientation.y = self._latest_q_rel[1] + odom.pose.pose.orientation.z = self._latest_q_rel[2] + odom.pose.pose.orientation.w = self._latest_q_rel[3] + odom.twist.twist.linear.x = self._latest_linear_xy[0] + odom.twist.twist.linear.y = self._latest_linear_xy[1] + odom.twist.twist.angular.z = self._latest_angular_z + self._odom_publisher.publish(odom) + + +def main(args=None) -> None: + rclpy.init(args=args) + node = OdomTfFromMapPose() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception2d_pkg/perception2d_pkg/odometry.py b/src/perception2d_pkg/perception2d_pkg/odometry.py new file mode 100755 index 0000000..a253c37 --- /dev/null +++ b/src/perception2d_pkg/perception2d_pkg/odometry.py @@ -0,0 +1,287 @@ +#!/usr/bin/env python3 +# File: odometry.py +# Description: +# speedometer(Float32) + IMU를 융합하여 /odom(Odometry)과 +# TF(odom → base_link)를 발행하는 dead reckoning 노드. +# 파라미터로 토픽명과 프레임명을 주입할 수 있으며, +# 실차 naming(/raw/imu, /odom)에 맞춰 launch에서 remap해 사용한다. + +import math +from typing import Optional + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float32 +from sensor_msgs.msg import Imu +from rosgraph_msgs.msg import Clock +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Quaternion, TransformStamped +from tf2_ros import TransformBroadcaster + + +# 좌표 변환 유틸 + +def quat_to_yaw(x: float, y: float, z: float, w: float) -> float: + """쿼터니언 → yaw(rad) 변환""" + siny_cosp = 2.0 * (w * z + x * y) + cosy_cosp = 1.0 - 2.0 * (y * y + z * z) + return math.atan2(siny_cosp, cosy_cosp) + + +def yaw_to_quaternion(yaw: float) -> Quaternion: + """yaw(rad) → 쿼터니언 변환 (2D 평면 운동 가정, roll/pitch = 0)""" + q = Quaternion() + q.x = 0.0 + q.y = 0.0 + q.z = math.sin(yaw * 0.5) + q.w = math.cos(yaw * 0.5) + return q + + +def normalize_angle(angle: float) -> float: + """각도를 [-π, π] 범위로 정규화""" + while angle > math.pi: + angle -= 2.0 * math.pi + while angle < -math.pi: + angle += 2.0 * math.pi + return angle + + +# 메인 노드 + +class SpeedImuOdomNode(Node): + def __init__(self): + super().__init__('speed_imu_odom_node') + + # 파라미터 선언 및 로드 + # perception_params.yaml의 speed_imu_odom_node 섹션에서 읽어옴 + self.declare_parameter('sim_mode', True) + self.declare_parameter('speed_topic', '/hero/speedometer') + self.declare_parameter('imu_topic', '/hero/imu') + self.declare_parameter('clock_topic', '/clock') + self.declare_parameter('odom_topic', '/hero/wheel_odom') + self.declare_parameter('odom_frame', 'odom') + self.declare_parameter('base_frame', 'base_link') + self.declare_parameter('publish_tf', False) + self.declare_parameter('use_imu_angular_velocity', True) + self.declare_parameter('invert_speed', False) + + self.sim_mode = self.get_parameter('sim_mode').value + self.speed_topic = self.get_parameter('speed_topic').value + self.imu_topic = self.get_parameter('imu_topic').value + self.clock_topic = self.get_parameter('clock_topic').value + self.odom_topic = self.get_parameter('odom_topic').value + self.odom_frame = self.get_parameter('odom_frame').value + self.base_frame = self.get_parameter('base_frame').value + self.publish_tf = self.get_parameter('publish_tf').value + self.use_imu_angular_velocity = self.get_parameter('use_imu_angular_velocity').value + self.invert_speed = self.get_parameter('invert_speed').value + + # 내부 상태 변수 + self.speed_mps: float = 0.0 # 현재 속도 (m/s) + self.current_yaw: Optional[float] = None + self.initial_yaw: Optional[float] = None + self.last_yaw: Optional[float] = None + self.last_time: Optional[float] = None + self.last_clock_msg = None + + self.x = 0.0 # odom 프레임 기준 x 위치 (m) + self.y = 0.0 # odom 프레임 기준 y 위치 (m) + + self.has_speed = False + self.has_imu = False + + self.last_imu_msg: Optional[Imu] = None + + # Publisher / Subscriber + self.odom_pub = self.create_publisher(Odometry, self.odom_topic, 10) + self.tf_broadcaster = TransformBroadcaster(self) + + self.create_subscription(Float32, self.speed_topic, self.speed_cb, 10) + self.create_subscription(Imu, self.imu_topic, self.imu_cb, 50) + if self.sim_mode: + self.create_subscription(Clock, self.clock_topic, self.clock_cb, 100) + + self.get_logger().info('SpeedImuOdomNode started') + self.get_logger().info(f'speed_topic: {self.speed_topic}') + self.get_logger().info(f'imu_topic: {self.imu_topic}') + self.get_logger().info(f'clock_topic: {self.clock_topic}') + self.get_logger().info(f'odom_topic: {self.odom_topic}') + self.get_logger().info(f'publish_tf: {self.publish_tf}') + + # 콜백 + + def speed_cb(self, msg: Float32): + """speedometer 수신 → 속도 저장 (invert_speed=True면 부호 반전)""" + speed = float(msg.data) + if self.invert_speed: + speed = -speed + self.speed_mps = speed + self.has_speed = True + + def imu_cb(self, msg: Imu): + """ + IMU 수신 → yaw 갱신 + - use_imu_angular_velocity=False: IMU orientation에서 직접 yaw 추출 (CARLA 시뮬용) + - use_imu_angular_velocity=True : angular_velocity.z를 적분하여 yaw 계산 (실차용) + """ + self.last_imu_msg = msg + + if not self.use_imu_angular_velocity: + # 절대 yaw 모드: CARLA가 제공하는 ground truth orientation 사용 + q = msg.orientation + yaw = quat_to_yaw(q.x, q.y, q.z, q.w) + if self.initial_yaw is None: + self.initial_yaw = yaw + self.current_yaw = normalize_angle(yaw - self.initial_yaw) + else: + # 상대 yaw 모드: IMU angular_velocity.z를 시간 적분 -> 실차에 적합 + stamp = float(msg.header.stamp.sec) + float(msg.header.stamp.nanosec) * 1e-9 + if self.current_yaw is None: + self.current_yaw = 0.0 + self._last_imu_stamp = stamp + self.has_imu = True + return + + dt = stamp - self._last_imu_stamp + if 0.0 < dt < 0.5: + self.current_yaw = normalize_angle( + self.current_yaw + msg.angular_velocity.z * dt + ) + self._last_imu_stamp = stamp + self.has_imu = True + + # 실차 모드: IMU 콜백이 odom 적분 트리거 + if not self.sim_mode and self.has_speed and 0.0 < dt < 0.5: + yaw = self.current_yaw + v = self.speed_mps + self.x += v * math.cos(yaw) * dt + self.y += v * math.sin(yaw) * dt + omega = float(msg.angular_velocity.z) + self.publish_odom(msg.header.stamp, yaw, v, omega) + return + + self.has_imu = True + + def clock_cb(self, msg: Clock): + """ + /clock 수신마다 위치 적분 및 odometry 발행 + - dt가 0.5s 초과 시 (일시정지 후 재개 등) 적분 skip + - speed와 imu 데이터가 모두 준비된 경우에만 적분 실행 + """ + sim_time = float(msg.clock.sec) + float(msg.clock.nanosec) * 1e-9 + self.last_clock_msg = msg + + if self.last_time is None: + self.last_time = sim_time + if self.current_yaw is not None: + self.last_yaw = self.current_yaw + return + + dt = sim_time - self.last_time + if dt <= 0.0: + return + + # 너무 긴 공백 구간은 적분 오차 방지를 위해 skip + if dt > 0.5: + self.last_time = sim_time + if self.current_yaw is not None: + self.last_yaw = self.current_yaw + return + + self.last_time = sim_time + + if not self.has_speed or not self.has_imu or self.current_yaw is None: + return + + yaw = self.current_yaw + v = self.speed_mps + + # 속도와 yaw로 x, y 위치 적분 (오일러 적분) + self.x += v * math.cos(yaw) * dt + self.y += v * math.sin(yaw) * dt + + # 각속도 계산: IMU angular_velocity 직접 사용 또는 yaw 차분으로 추정 + omega = 0.0 + if self.use_imu_angular_velocity and self.last_imu_msg is not None: + omega = float(self.last_imu_msg.angular_velocity.z) + elif self.last_yaw is not None: + dyaw = normalize_angle(yaw - self.last_yaw) + omega = dyaw / dt + + self.last_yaw = yaw + + self.publish_odom(msg.clock, yaw, v, omega) + + def publish_odom(self, stamp, yaw: float, v: float, omega: float): + """ + Odometry 메시지 발행 및 TF(odom → base_link) broadcast + - covariance: x/y/yaw에만 유효한 값, 나머지(z, roll, pitch)는 999로 무효 처리 + """ + odom = Odometry() + odom.header.stamp = stamp + odom.header.frame_id = self.odom_frame + odom.child_frame_id = self.base_frame + + odom.pose.pose.position.x = self.x + odom.pose.pose.position.y = self.y + odom.pose.pose.position.z = 0.0 + odom.pose.pose.orientation = yaw_to_quaternion(yaw) + + odom.twist.twist.linear.x = v + odom.twist.twist.linear.y = 0.0 + odom.twist.twist.linear.z = 0.0 + odom.twist.twist.angular.x = 0.0 + odom.twist.twist.angular.y = 0.0 + odom.twist.twist.angular.z = omega + + # 6x6 covariance (x, y, z, roll, pitch, yaw 순서) + # z/roll/pitch는 2D 주행에서 의미 없으므로 999(매우 큰 불확실도)로 설정 + odom.pose.covariance = [ + 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 999.0,0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 999.0,0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 999.0,0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.02 + ] + + odom.twist.covariance = [ + 0.02, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 999.0,0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 999.0,0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 999.0,0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.05 + ] + + self.odom_pub.publish(odom) + + # publish_tf=True일 때 odom → base_link TF도 함께 발행 + if self.publish_tf: + tf_msg = TransformStamped() + tf_msg.header.stamp = stamp + tf_msg.header.frame_id = self.odom_frame + tf_msg.child_frame_id = self.base_frame + tf_msg.transform.translation.x = self.x + tf_msg.transform.translation.y = self.y + tf_msg.transform.translation.z = 0.0 + tf_msg.transform.rotation = yaw_to_quaternion(yaw) + self.tf_broadcaster.sendTransform(tf_msg) + + +def main(args=None): + rclpy.init(args=args) + node = SpeedImuOdomNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception2d_pkg/perception2d_pkg/test.py b/src/perception2d_pkg/perception2d_pkg/test.py new file mode 100755 index 0000000..307d937 --- /dev/null +++ b/src/perception2d_pkg/perception2d_pkg/test.py @@ -0,0 +1,6 @@ +def main(): + print('Hi from perception2d_pkg.') + + +if __name__ == '__main__': + main() diff --git a/src/perception2d_pkg/resource/perception2d_pkg b/src/perception2d_pkg/resource/perception2d_pkg new file mode 100755 index 0000000..e69de29 diff --git a/src/perception2d_pkg/setup.cfg b/src/perception2d_pkg/setup.cfg new file mode 100755 index 0000000..b2bd225 --- /dev/null +++ b/src/perception2d_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/perception2d_pkg +[install] +install_scripts=$base/lib/perception2d_pkg diff --git a/src/perception2d_pkg/setup.py b/src/perception2d_pkg/setup.py new file mode 100755 index 0000000..441d061 --- /dev/null +++ b/src/perception2d_pkg/setup.py @@ -0,0 +1,35 @@ + +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'perception2d_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='TODO: Package description', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'test = perception2d_pkg.test:main' + ], + }, +) diff --git a/src/perception2d_pkg/test/test_copyright.py b/src/perception2d_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/src/perception2d_pkg/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/perception2d_pkg/test/test_flake8.py b/src/perception2d_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/src/perception2d_pkg/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/perception2d_pkg/test/test_pep257.py b/src/perception2d_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/src/perception2d_pkg/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/perception3d_pkg/config/3d_fr09.yaml b/src/perception3d_pkg/config/3d_fr09.yaml new file mode 100755 index 0000000..5c395d7 --- /dev/null +++ b/src/perception3d_pkg/config/3d_fr09.yaml @@ -0,0 +1,23 @@ + + +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 + max_z: 0.5 + # min_z: -10.0 + # max_z: 10.0 + min_radius: 1.0 + max_radius: 50.0 + # min_angle: -1.570796 + # max_angle: 2.356194 diff --git a/src/perception3d_pkg/config/3d_mkmini.yaml b/src/perception3d_pkg/config/3d_mkmini.yaml new file mode 100755 index 0000000..27c2f19 --- /dev/null +++ b/src/perception3d_pkg/config/3d_mkmini.yaml @@ -0,0 +1,19 @@ + + +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 + max_z: 1.5 + min_radius: 2.0 + max_radius: 30.0 diff --git a/src/perception3d_pkg/config/tf_fr09-carla.yaml b/src/perception3d_pkg/config/tf_fr09-carla.yaml new file mode 100755 index 0000000..e8b6219 --- /dev/null +++ b/src/perception3d_pkg/config/tf_fr09-carla.yaml @@ -0,0 +1,31 @@ + +lidar: + x: 0.456 + y: 0.0 + z: 1.135 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + frame_id: base_link + child_frame_id: os_lidar + +imu: + x: 0.0 + y: 0.0 + z: 0.5 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + frame_id: base_link + child_frame_id: os_imu + +rgb_front: + x: 0.56331 + y: 0.56343 + z: 0.80703 + roll: -0.025 + pitch: 0.315 + yaw: 1.035 + frame_id: base_link + child_frame_id: hero/rgb_front + diff --git a/src/perception3d_pkg/config/tf_fr09-real.yaml b/src/perception3d_pkg/config/tf_fr09-real.yaml new file mode 100755 index 0000000..3e83097 --- /dev/null +++ b/src/perception3d_pkg/config/tf_fr09-real.yaml @@ -0,0 +1,21 @@ + +lidar: + x: 0.0 + y: 0.0 + z: 0.03618 + roll: 0.0 + pitch: 0.0 + yaw: 2.356194 + frame_id: base_link + child_frame_id: os_lidar + +imu: + x: 0.006253 + y: -0.011775 + z: 0.007645 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + frame_id: base_link + child_frame_id: os_imu + diff --git a/src/perception3d_pkg/config/tf_fr09.yaml b/src/perception3d_pkg/config/tf_fr09.yaml new file mode 100755 index 0000000..e8b6219 --- /dev/null +++ b/src/perception3d_pkg/config/tf_fr09.yaml @@ -0,0 +1,31 @@ + +lidar: + x: 0.456 + y: 0.0 + z: 1.135 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + frame_id: base_link + child_frame_id: os_lidar + +imu: + x: 0.0 + y: 0.0 + z: 0.5 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + frame_id: base_link + child_frame_id: os_imu + +rgb_front: + x: 0.56331 + y: 0.56343 + z: 0.80703 + roll: -0.025 + pitch: 0.315 + yaw: 1.035 + frame_id: base_link + child_frame_id: hero/rgb_front + diff --git a/src/perception3d_pkg/config/tf_mkmini.yaml b/src/perception3d_pkg/config/tf_mkmini.yaml new file mode 100755 index 0000000..1452a9c --- /dev/null +++ b/src/perception3d_pkg/config/tf_mkmini.yaml @@ -0,0 +1,31 @@ + +lidar: + x: 0.1 + y: 0.0 + z: 0.6 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + frame_id: base_link + child_frame_id: os_lidar + +imu: + x: 0.0 + y: 0.0 + z: 0.5 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + frame_id: base_link + child_frame_id: os_imu + +rgb_front: + x: 0.10731 + y: 0.56343 + z: -0.27697 + roll: -0.025 + pitch: 0.315 + yaw: 1.035 + frame_id: base_link + child_frame_id: hero/rgb_front + diff --git a/src/perception3d_pkg/launch/perception.launch.py b/src/perception3d_pkg/launch/perception.launch.py new file mode 100755 index 0000000..132c28b --- /dev/null +++ b/src/perception3d_pkg/launch/perception.launch.py @@ -0,0 +1,67 @@ + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.descriptions import ParameterFile +from nav2_common.launch import RewrittenYaml +from launch.actions import OpaqueFunction +from launch.substitutions import PythonExpression +from launch.conditions import IfCondition + +import os +import yaml + + +ARGUMENTS = [ + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('sim_mode', default_value='true'), + DeclareLaunchArgument('robot', default_value='fr09', choices=['mkmini', 'fr09']), +] + +def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time') + robot = LaunchConfiguration('robot') + + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Directories + pkg_perception = get_package_share_directory( + 'perception3d_pkg') + + params_file = PathJoinSubstitution([ + pkg_perception, + 'config', + PythonExpression(["'3d_' + '", robot, "' + '.yaml'"])]) + param_substitutions = { + 'use_sim_time': use_sim_time, + } + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) + + load_nodes = GroupAction( + actions=[ + Node( + package='perception3d_pkg', + executable='pcd_crop', + name='pcd_crop', + output='screen', + parameters=[configured_params], + remappings=remappings + ) + ], + ) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(load_nodes) + + return ld diff --git a/src/perception3d_pkg/package.xml b/src/perception3d_pkg/package.xml new file mode 100755 index 0000000..a5f9977 --- /dev/null +++ b/src/perception3d_pkg/package.xml @@ -0,0 +1,18 @@ + + + + perception3d_pkg + 0.0.0 + TODO: Package description + dgist + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception3d_pkg/perception3d_pkg/__init__.py b/src/perception3d_pkg/perception3d_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/perception3d_pkg/perception3d_pkg/odometry.py b/src/perception3d_pkg/perception3d_pkg/odometry.py new file mode 100755 index 0000000..2f3b235 --- /dev/null +++ b/src/perception3d_pkg/perception3d_pkg/odometry.py @@ -0,0 +1,212 @@ + +#!/usr/bin/env python3 +import math +from typing import Optional + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float32 +from sensor_msgs.msg import Imu +from rosgraph_msgs.msg import Clock +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Quaternion, TransformStamped +from tf2_ros import TransformBroadcaster + + +def quat_to_yaw(x: float, y: float, z: float, w: float) -> float: + siny_cosp = 2.0 * (w * z + x * y) + cosy_cosp = 1.0 - 2.0 * (y * y + z * z) + return math.atan2(siny_cosp, cosy_cosp) + + +def yaw_to_quaternion(yaw: float) -> Quaternion: + q = Quaternion() + q.x = 0.0 + q.y = 0.0 + q.z = math.sin(yaw * 0.5) + q.w = math.cos(yaw * 0.5) + return q + + +def normalize_angle(angle: float) -> float: + while angle > math.pi: + angle -= 2.0 * math.pi + while angle < -math.pi: + angle += 2.0 * math.pi + return angle + + +def stamp_to_sec(stamp) -> float: + return float(stamp.sec) + float(stamp.nanosec) * 1e-9 + + +class SpeedImuOdomNode(Node): + def __init__(self): + super().__init__('speed_imu_odom_node') + + # Parameters + self.declare_parameter('odom_topic', '/vehicle_odom') + self.declare_parameter('odom_frame', 'odom') + self.declare_parameter('base_frame', 'base_link') + + self.odom_topic = self.get_parameter('odom_topic').value + self.odom_frame = self.get_parameter('odom_frame').value + self.base_frame = self.get_parameter('base_frame').value + + self.declare_parameter('pose_cov_x', 0.5) + self.declare_parameter('pose_cov_y', 0.5) + self.declare_parameter('pose_cov_yaw', 0.2) + self.declare_parameter('twist_cov_vx', 0.2) + self.declare_parameter('twist_cov_vy', 0.2) + self.declare_parameter('twist_cov_wz', 0.1) + + self.pose_cov_x = self.get_parameter('pose_cov_x').get_parameter_value().double_value + self.pose_cov_y = self.get_parameter('pose_cov_y').get_parameter_value().double_value + self.pose_cov_yaw = self.get_parameter('pose_cov_yaw').get_parameter_value().double_value + self.twist_cov_vx = self.get_parameter('twist_cov_vx').get_parameter_value().double_value + self.twist_cov_vy = self.get_parameter('twist_cov_vy').get_parameter_value().double_value + self.twist_cov_wz = self.get_parameter('twist_cov_wz').get_parameter_value().double_value + + self.x = 0.0 + self.y = 0.0 + self.yaw = 0.0 + + self.max_dt = 0.1 + + # State + self.speed_mps: float = 0.0 + self.current_yaw: Optional[float] = None + self.last_yaw: Optional[float] = None + self.last_pub_time: Optional[float] = None + + self.x = 0.0 + self.y = 0.0 + + self.has_speed = False + self.has_imu = False + + self.ground_truth_odom: Optional[Odometry] = None + + # Pub/Sub + self.odom_pub = self.create_publisher(Odometry, self.odom_topic, 50) + self.tf_broadcaster = TransformBroadcaster(self) + + self.create_subscription(Float32, '/raw/speed', self.speed_cb, 50) + self.create_subscription(Imu, '/raw/imu', self.imu_cb, 50) + + self.get_logger().info('SpeedImuOdomNode started') + self.get_logger().info(f'odom_topic: {self.odom_topic}') + + def speed_cb(self, msg: Float32): + self.speed_mps = float(msg.data) + self.has_speed = True + + def imu_cb(self, msg: Imu): + self.has_imu = True + + # speed가 아직 없으면 선택적으로 skip + if not self.has_speed: + return + + # IMU timestamp 우선 사용 + stamp = msg.header.stamp + now_sec = stamp_to_sec(stamp) + + omega = float(msg.angular_velocity.z) + + if self.last_pub_time is None: + self.last_pub_time = now_sec + self.current_yaw = self.yaw + self.last_yaw = self.yaw + # 첫 샘플은 dt가 없으므로 현재 yaw로 publish + self.publish_odom(stamp, self.yaw, self.speed_mps, omega) + return + + dt = now_sec - self.last_pub_time + if dt <= 0.0: + # 시간 역전/동일시간이면 적분하지 않고 현재 yaw만 publish + self.publish_odom(stamp, self.yaw, self.speed_mps, omega) + return + + # 긴 공백은 과적분 방지 + if dt > self.max_dt: + dt = self.max_dt + + # angular velocity integration + self.yaw += omega * dt + self.yaw = normalize_angle(self.yaw) + + # position integration + self.x += self.speed_mps * math.cos(self.yaw) * dt + self.y += self.speed_mps * math.sin(self.yaw) * dt + + self.current_yaw = self.yaw + self.last_yaw = self.yaw + self.last_pub_time = now_sec + + self.publish_odom(stamp, self.yaw, self.speed_mps, omega) + + def publish_odom(self, stamp, yaw: float, v: float, omega: float): + odom = Odometry() + odom.header.stamp = stamp + odom.header.frame_id = self.odom_frame + odom.child_frame_id = self.base_frame + + odom.pose.pose.position.x = self.x + odom.pose.pose.position.y = self.y + odom.pose.pose.position.z = 0.0 + odom.pose.pose.orientation = yaw_to_quaternion(yaw) + + odom.twist.twist.linear.x = v + odom.twist.twist.linear.y = 0.0 + odom.twist.twist.linear.z = 0.0 + odom.twist.twist.angular.x = 0.0 + odom.twist.twist.angular.y = 0.0 + odom.twist.twist.angular.z = omega + + odom.pose.covariance = [ + self.pose_cov_x, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, self.pose_cov_y, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 999.0,0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 999.0,0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 999.0,0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, self.pose_cov_yaw + ] + + odom.twist.covariance = [ + self.twist_cov_vx, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, self.twist_cov_vy, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 999.0,0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 999.0,0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 999.0,0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, self.twist_cov_wz + ] + + self.odom_pub.publish(odom) + + tf_msg = TransformStamped() + tf_msg.header.stamp = stamp + tf_msg.header.frame_id = self.odom_frame + tf_msg.child_frame_id = self.base_frame + tf_msg.transform.translation.x = self.x + tf_msg.transform.translation.y = self.y + tf_msg.transform.translation.z = 0.0 + tf_msg.transform.rotation = yaw_to_quaternion(yaw) + self.tf_broadcaster.sendTransform(tf_msg) + + +def main(args=None): + rclpy.init(args=args) + node = SpeedImuOdomNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/perception3d_pkg/perception3d_pkg/pcd_crop.py b/src/perception3d_pkg/perception3d_pkg/pcd_crop.py new file mode 100755 index 0000000..ef91a87 --- /dev/null +++ b/src/perception3d_pkg/perception3d_pkg/pcd_crop.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import PointCloud2 +import sensor_msgs_py.point_cloud2 as pc2 + +import numpy as np + +class PointCloudCrop(Node): + def __init__(self): + super().__init__('pcd_crop') + + self.declare_parameter('min_z', -2.5) + self.declare_parameter('max_z', -0.5) + self.declare_parameter('min_radius', 5.0) # 너무 가까운 점 제거 + self.declare_parameter('max_radius', 30.0) # 너무 먼 점 제거 + self.declare_parameter('input_topic', '/raw/lidar') + self.declare_parameter('output_topic', '/lidar_filtered') + + self.min_z = self.get_parameter('min_z').value + self.max_z = self.get_parameter('max_z').value + self.min_radius = self.get_parameter('min_radius').value + self.max_radius = self.get_parameter('max_radius').value + + input_topic = self.get_parameter('input_topic').value + output_topic = self.get_parameter('output_topic').value + + self.declare_parameter('min_angle', (-np.pi)) # rad + self.declare_parameter('max_angle', (np.pi)) # rad + + self.min_angle = self.get_parameter('min_angle').value + self.max_angle = self.get_parameter('max_angle').value + + self.sub = self.create_subscription( + PointCloud2, + input_topic, + self.callback, + 10 + ) + + self.pub = self.create_publisher( + PointCloud2, + output_topic, + 10 + ) + + self.get_logger().info( + f"Crop started: Z[{self.min_z}, {self.max_z}] " + f"Radius[{self.min_radius}, {self.max_radius}]" + f"Angle[{self.min_angle}, {self.max_angle}]" + ) + + def callback(self, msg: PointCloud2): + pts = pc2.read_points( + msg, + field_names=("x", "y", "z"), + skip_nans=True + ) + + if pts.shape[0] == 0: + return + + # structured array -> 일반 (N,3) float32 배열 + points = np.stack( + (pts["x"], pts["y"], pts["z"]), axis=-1).astype(np.float32, copy=False) + + x = points[:, 0] + y = points[:, 1] + z = points[:, 2] + + min_r2 = self.min_radius * self.min_radius + max_r2 = self.max_radius * self.max_radius + r2 = x * x + y * y + + angles = np.arctan2(-y, -x) + + if self.min_angle < self.max_angle: + angle_mask = (angles >= self.min_angle) & (angles <= self.max_angle) + else: + angle_mask = (angles >= self.min_angle) | (angles <= self.max_angle) + + mask = ( + (z >= self.min_z) & + (z <= self.max_z) & + (r2 >= min_r2) & + (r2 <= max_r2) & + angle_mask + ) + + filtered = points[mask] + + if filtered.shape[0] == 0: + return + + filtered_msg = pc2.create_cloud_xyz32(msg.header, filtered.tolist()) + self.pub.publish(filtered_msg) + + +def main(args=None): + rclpy.init(args=args) + node = PointCloudCrop() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() + \ No newline at end of file diff --git a/src/perception3d_pkg/perception3d_pkg/test.py b/src/perception3d_pkg/perception3d_pkg/test.py new file mode 100755 index 0000000..e2e3661 --- /dev/null +++ b/src/perception3d_pkg/perception3d_pkg/test.py @@ -0,0 +1,6 @@ +def main(): + print('Hi from perception3d_pkg.') + + +if __name__ == '__main__': + main() diff --git a/src/perception3d_pkg/resource/perception3d_pkg b/src/perception3d_pkg/resource/perception3d_pkg new file mode 100755 index 0000000..e69de29 diff --git a/src/perception3d_pkg/setup.cfg b/src/perception3d_pkg/setup.cfg new file mode 100755 index 0000000..4dd6cb1 --- /dev/null +++ b/src/perception3d_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/perception3d_pkg +[install] +install_scripts=$base/lib/perception3d_pkg diff --git a/src/perception3d_pkg/setup.py b/src/perception3d_pkg/setup.py new file mode 100755 index 0000000..02e8e8e --- /dev/null +++ b/src/perception3d_pkg/setup.py @@ -0,0 +1,36 @@ + +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'perception3d_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='TODO: Package description', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'test = perception3d_pkg.test:main', + 'pcd_crop = perception3d_pkg.pcd_crop:main' + ], + }, +) diff --git a/src/perception3d_pkg/test/test_copyright.py b/src/perception3d_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/src/perception3d_pkg/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/perception3d_pkg/test/test_flake8.py b/src/perception3d_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/src/perception3d_pkg/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/perception3d_pkg/test/test_pep257.py b/src/perception3d_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/src/perception3d_pkg/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/perception_bevplace2_pkg/config/bevplace.yaml b/src/perception_bevplace2_pkg/config/bevplace.yaml new file mode 100755 index 0000000..3553809 --- /dev/null +++ b/src/perception_bevplace2_pkg/config/bevplace.yaml @@ -0,0 +1,21 @@ +bevplace2: + ros__parameters: + # 맵 DB(map.npz)는 필터된 스캔(=RTABMap .db 매핑 입력 = /lidar_filtered)으로 빌드되므로, + # 쿼리도 같은 /lidar_filtered 를 써야 분포가 일치해 매칭이 맞는다. /raw/lidar(raw 분포)는 + # 분포가 달라 descriptor 불일치 → 오측위. + input_topic: "/lidar_filtered" + model_ckpt_path: "" + map_db_path: "" + match_dist_thresh: 1.0 + seed_initialpose_once: true + # BEV 투영 파라미터 (모델 BEVPlace2 가 기대하는 입력 = KITTI Velodyne BEV 격자 ±40m/0.4m). + # 센서가 아니라 모델 학습값에 맞춘다. 잘못 주면 점이 엉뚱한 셀에 뭉쳐 descriptor 붕괴. + # build_map_db(맵 빌드)와 bevplace2_node(query)가 같은 값을 써야 매칭이 맞음 + # (run.sh 가 이 파일로 통일). + x_min: -40.0 + x_max: 40.0 + y_min: -40.0 + y_max: 40.0 + resolution: 0.4 + max_density: 20.0 + use_sim_time: true diff --git a/src/perception_bevplace2_pkg/launch/bevplace2.launch.py b/src/perception_bevplace2_pkg/launch/bevplace2.launch.py new file mode 100755 index 0000000..dc72e93 --- /dev/null +++ b/src/perception_bevplace2_pkg/launch/bevplace2.launch.py @@ -0,0 +1,149 @@ +import glob +import importlib.util +import os +import subprocess +import sys + +import yaml + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +# 모델/맵 경로 컨벤션 (단독 deploy run.sh 와 동일 — 컨테이너 내부 고정 경로) +# 가중치 공유 단일 위치(docker/local): /perception_models/bevplace2/{finetuned,pretrained} +# (호스트 .caches/config/nav2_config/perception/models/bevplace2/*, SWRule config_mounts). +FINETUNED_CKPT = '/perception_models/bevplace2/finetuned/bevplace2.pth.tar' +SHARED_PRE_CKPT = '/perception_models/bevplace2/pretrained/bevplace2.pth.tar' # 공유 pretrained (Phase 2 채움) +PRETRAINED_CKPT = '/opt/bevplace2_pretrained/bevplace2.pth.tar' # 이미지 baked 안전망(Dockerfile COPY) +MAP_DB = '/perception_data/map_descriptors/map.npz' +MAP_SRC = MAP_DB + '.src' # 빌드된 map.npz 의 소스(.db+모델) 식별 → 교체 감지 +MAP_SEQ = '/perception_data/pr/sequences/00' +SELECTED_MAP_DIR = '/selected_map' # 선택 맵 .db (config_mounts 로 마운트) + +ARGUMENTS = [ + DeclareLaunchArgument('use_sim_time', default_value='true'), + # 빈값(기본) = launch 가 model_source 기준으로 자체 resolve. + # 단독 deploy run.sh 가 MODEL_ARGS 로 명시 전달하면(비어있지 않으면) 그 값이 그대로 이긴다(무회귀). + DeclareLaunchArgument('model_ckpt_path', default_value=''), + DeclareLaunchArgument('map_db_path', default_value=''), + DeclareLaunchArgument('model_source', default_value='finetuned'), # UI 스위치: finetuned|pretrained +] + + +def _resolve_ckpt(model_source): + """model_source(07 §6 공통 계약) 기반 ckpt 자동 resolve (run.sh 로직 이식). + pretrained → finetuned 있어도 건너뛰고 base pretrained 강제. 그 외 → finetuned 우선(조용한 fallback).""" + if model_source != 'pretrained' and os.path.isfile(FINETUNED_CKPT): + print(f'[bevplace2] 재학습 모델 사용: {FINETUNED_CKPT}') + return FINETUNED_CKPT + if os.path.isfile(SHARED_PRE_CKPT): + print(f'[bevplace2] 공유 pretrained 사용: {SHARED_PRE_CKPT}') + return SHARED_PRE_CKPT + print(f'[bevplace2] base pretrained(baked) 사용(model_source={model_source}): {PRETRAINED_CKPT}') + return PRETRAINED_CKPT + + +def _script_dir(): + """맵 빌드 스크립트(db_to_mapdb/build_map_db)가 있는 pkg 모듈 경로 (run.sh 의 find src 대체). + colcon 설치본 site-packages 안에 python 모듈로 함께 설치되므로 import 경로로 찾는다.""" + spec = importlib.util.find_spec('perception_bevplace2_pkg') + return os.path.dirname(spec.origin) if spec and spec.origin else '' + + +def _proj_args(): + """투영 파라미터를 bevplace.yaml(단일 소스, SWRule override) 에서 읽어 맵 빌드에 적용. + query 노드(bevplace2_node)도 같은 bevplace.yaml 을 쓰므로 빌드/쿼리 투영이 일치한다(매칭 정합 필수).""" + try: + yml = os.path.join(get_package_share_directory('perception_bevplace2_pkg'), + 'config', 'bevplace.yaml') + with open(yml) as f: + p = yaml.safe_load(f)['bevplace2']['ros__parameters'] + return ['--x_min', str(p.get('x_min', -40.0)), '--x_max', str(p.get('x_max', 40.0)), + '--y_min', str(p.get('y_min', -40.0)), '--y_max', str(p.get('y_max', 40.0)), + '--resolution', str(p.get('resolution', 0.4)), + '--max_density', str(p.get('max_density', 20.0))] + except Exception as e: # 읽기 실패 시 빈 인자 → 스크립트 기본값(모델 학습값 KITTI) 사용 + print(f'[bevplace2] bevplace.yaml 투영 파라미터 읽기 실패(기본값 사용): {e}') + return [] + + +def _mtime(path): + """run.sh 의 stat -c %Y 대응 (없으면 빈 문자열) — SRC_TAG 포맷 호환 유지.""" + try: + return str(int(os.stat(path).st_mtime)) + except OSError: + return '' + + +def _ensure_map_db(ckpt): + """맵 DB(map.npz) 자동 빌드 (run.sh 로직 이식). 선택 맵 .db 가 마운트돼 있으면 그걸로 + 빌드(맵/모델 바뀌면 재빌드). .db 안의 Admin.opt_poses(최적화 pose)+Data.scan(/lidar_filtered + 분포)을 직접 디코드 → rtabmap 불필요. 결과 map.npz 가 있으면 그 경로, 없으면 빈 문자열.""" + pydir = _script_dir() + sel = sorted(glob.glob(os.path.join(SELECTED_MAP_DIR, '*.db'))) # ls -1 | head -1 동일 + selected_db = sel[0] if sel else '' + proj = _proj_args() + if selected_db and pydir: + src_tag = f'{selected_db} {_mtime(selected_db)} {ckpt} {_mtime(ckpt)}' + old_tag = '' + if os.path.isfile(MAP_SRC): + with open(MAP_SRC) as f: + old_tag = f.read().rstrip('\n') + if not os.path.isfile(MAP_DB) or old_tag != src_tag: + print(f'[bevplace2] 맵 DB 자동 빌드 (.db→map.npz): {selected_db} ({" ".join(proj)})') + r = subprocess.run([sys.executable, os.path.join(pydir, 'db_to_mapdb.py'), + ckpt, selected_db, MAP_DB] + proj, cwd=pydir) + if r.returncode == 0: + with open(MAP_SRC, 'w') as f: + f.write(src_tag + '\n') + else: + print('[bevplace2] 맵 DB 자동 빌드 실패(계속)') + else: + print('[bevplace2] map.npz 최신 (선택 맵/모델 변경 없음) — 재빌드 생략') + # fallback: .db 미마운트 시 수집 시퀀스(00, ride-along)로 빌드 + elif (not os.path.isfile(MAP_DB) and os.path.isdir(os.path.join(MAP_SEQ, 'velodyne')) + and os.path.isfile(os.path.join(MAP_SEQ, 'poses.txt')) and pydir): + print(f'[bevplace2] 맵 DB 빌드 (sequences/00→map.npz) ({" ".join(proj)})') + r = subprocess.run([sys.executable, os.path.join(pydir, 'build_map_db.py'), ckpt, + os.path.join(MAP_SEQ, 'velodyne'), os.path.join(MAP_SEQ, 'poses.txt'), + MAP_DB] + proj, cwd=pydir) + if r.returncode != 0: + print('[bevplace2] 맵 DB 빌드 실패(계속)') + return MAP_DB if os.path.isfile(MAP_DB) else '' + + +def _launch_setup(context, *args, **kwargs): + """OpaqueFunction: 인자가 명시되면(선언 기본값인 빈값과 다르면) 그대로 사용 — + 단독 deploy run.sh 의 MODEL_ARGS 가 계속 이김(무회귀). 빈값일 때만 자동 resolve.""" + ckpt = LaunchConfiguration('model_ckpt_path').perform(context) + map_db = LaunchConfiguration('map_db_path').perform(context) + model_source = LaunchConfiguration('model_source').perform(context) or 'finetuned' + if not ckpt: # 미명시(기본 빈값) → model_source + finetuned 존재 검사로 자동 resolve + ckpt = _resolve_ckpt(model_source) + if not map_db: # 미명시(기본 빈값) → map.npz 자동 빌드(staleness 체크) 후 resolve + map_db = _ensure_map_db(ckpt) + pkg = get_package_share_directory('perception_bevplace2_pkg') + params_file = os.path.join(pkg, 'config', 'bevplace.yaml') + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + load_nodes = GroupAction(actions=[ + Node( + package='perception_bevplace2_pkg', executable='bevplace2', + name='bevplace2', output='screen', + parameters=[params_file, { + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'model_ckpt_path': ckpt, + 'map_db_path': map_db, + }], + remappings=remappings, + ) + ]) + return [load_nodes] + + +def generate_launch_description(): + ld = LaunchDescription(ARGUMENTS) + ld.add_action(OpaqueFunction(function=_launch_setup)) + return ld diff --git a/src/perception_bevplace2_pkg/launch/perception.launch.py b/src/perception_bevplace2_pkg/launch/perception.launch.py new file mode 100755 index 0000000..619a576 --- /dev/null +++ b/src/perception_bevplace2_pkg/launch/perception.launch.py @@ -0,0 +1,32 @@ +"""perception_manager 공통 진입점 (통합 perception 모듈). + +LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로 +`ros2 launch perception_bevplace2_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..` +를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(bevplace2.launch.py)로 위임한다. +robot 은 공통 계약상 선언만 하고, 모델 launch 가 선언한 경우에만 소비된다. +""" +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + share = get_package_share_directory('perception_bevplace2_pkg') + return LaunchDescription([ + DeclareLaunchArgument('robot', default_value='fr09'), + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('model_source', default_value='finetuned'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(share, 'launch', 'bevplace2.launch.py')), + launch_arguments={ + 'robot': LaunchConfiguration('robot'), + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'model_source': LaunchConfiguration('model_source'), + }.items(), + ), + ]) diff --git a/src/perception_bevplace2_pkg/package.xml b/src/perception_bevplace2_pkg/package.xml new file mode 100755 index 0000000..4cc567d --- /dev/null +++ b/src/perception_bevplace2_pkg/package.xml @@ -0,0 +1,25 @@ + + + + perception_bevplace2_pkg + 0.0.0 + LiDAR place recognition (BEVPlace2) perception module + dgist + TODO: License declaration + + rclpy + sensor_msgs + sensor_msgs_py + geometry_msgs + std_msgs + visualization_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/__init__.py b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bev_projection.py b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bev_projection.py new file mode 100755 index 0000000..a93f4f8 --- /dev/null +++ b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bev_projection.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +"""LiDAR point cloud → BEV(bird's-eye-view) image projection (모델 무관, 순수 numpy). + +BEVPlace/BEVPlace2 표준 입력. OverlapTransformer 의 range-image 와 달리 **top-down 격자** +(x-y 평면)에 점 개수(density)를 누적한 1채널 BEV 이미지를 만든다. group conv 백본이 이 +BEV 이미지를 회전-등변(rotation-equivariant) descriptor 로 변환한다. + +투영 파라미터(격자 범위/해상도)는 "센서"가 아니라 "모델"이 학습된 입력 레이아웃이다. +잘못 주면 점이 격자의 엉뚱한 칸에 뭉쳐 descriptor 가 붕괴한다 → 맵 빌드(build_map_db)와 +query(bevplace2_node)가 **반드시 같은 값**을 써야 매칭이 맞는다. 하드코딩 금지 — 호출부 +(node gp / build_map_db CLI / bevplace.yaml SWRule)가 Rule 값으로 덮어쓴다. +""" +from dataclasses import dataclass + +import numpy as np + + +@dataclass +class BEVResult: + bev_image: np.ndarray # (1, H, W) float32 — density(점 개수) BEV, 정규화 전 + grid_x: np.ndarray # (V,) valid 점들의 열 index (x bin) + grid_y: np.ndarray # (V,) valid 점들의 행 index (y bin) + valid: np.ndarray # (N,) bool — 격자 범위 안 점 마스크 + + +class BEVProjector: + """점군 (N,>=3) → density BEV 이미지 (1,H,W). + + x_range/y_range 는 [min,max] (m), resolution 은 셀 크기(m/px). BEVPlace2 기본은 + KITTI Velodyne 기준 ±40m / 0.4m → 200x200. CARLA Ouster/Vanjee 도 동일 격자를 쓰면 + 모델이 기대하는 분포와 맞는다(범위 밖 점은 drop). + """ + + def __init__(self, x_min=-40.0, x_max=40.0, y_min=-40.0, y_max=40.0, + resolution=0.4, max_density=20.0): + self.x_min = float(x_min) + self.x_max = float(x_max) + self.y_min = float(y_min) + self.y_max = float(y_max) + self.res = float(resolution) + self.max_density = float(max_density) + self.W = int(round((self.x_max - self.x_min) / self.res)) + self.H = int(round((self.y_max - self.y_min) / self.res)) + + def project(self, points): + xyz = points[:, :3] + x, y = xyz[:, 0], xyz[:, 1] + valid = (x > self.x_min) & (x < self.x_max) & (y > self.y_min) & (y < self.y_max) + + gx = np.clip(np.floor((x - self.x_min) / self.res), 0, self.W - 1).astype(np.int32) + gy = np.clip(np.floor((y - self.y_min) / self.res), 0, self.H - 1).astype(np.int32) + + img = np.zeros((1, self.H, self.W), dtype=np.float32) + vi = np.where(valid)[0] + # density: 각 셀에 떨어진 점 개수 누적 (BEVPlace2 입력) + np.add.at(img[0], (gy[vi], gx[vi]), 1.0) + # 점 밀집도가 큰 셀이 포화되지 않게 상한 클립 후 [0,1] 정규화 + if self.max_density > 0: + np.clip(img, 0.0, self.max_density, out=img) + img /= self.max_density + + return BEVResult(bev_image=img, grid_x=gx[valid], grid_y=gy[valid], valid=valid) diff --git a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bevplace2_node.py b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bevplace2_node.py new file mode 100755 index 0000000..0f50eee --- /dev/null +++ b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bevplace2_node.py @@ -0,0 +1,132 @@ +"""Place Recognition deploy 노드 (BEVPlace2). /lidar_filtered → BEV descriptor → 맵 DB +최근접 → 현재 pose 추정 → /perception/relocalization(PoseWithCovarianceStamped) + +/initialpose(측위 씨딩, 1회) + /perception/reloc_markers(★pose) 발행. +ot_node.py 의 구조 미러 — backend 만 BEVPlaceBackend(BEV+group conv)로 교체.""" +import os +import sys +# colcon 설치된 entry point 로 실행되면 같은 패키지의 flat 모듈(bevplace_backend/descriptor_db/ +# bev_projection)이 top-level 로 안 잡혀 ModuleNotFoundError → 자기 디렉토리를 path 에 추가. +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) +import numpy as np +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2 +from geometry_msgs.msg import PoseWithCovarianceStamped +from visualization_msgs.msg import Marker, MarkerArray +import sensor_msgs_py.point_cloud2 as pc2 +from bevplace_backend import BEVPlaceBackend +from descriptor_db import DescriptorDB + + +def _is_cuda_error(e): + s = str(e).lower() + return 'cuda' in s or 'cudnn' in s or 'out of memory' in s + + +def _pose_from_row(row12): + T = np.array(row12, dtype=np.float64).reshape(3, 4) + return T[:, 3], T[:, :3] # translation, R + + +class PlaceRecognition(Node): + def __init__(self): + super().__init__('bevplace2') + gp = lambda n, d: self.declare_parameter(n, d).value + self.input_topic = gp('input_topic', '/lidar_filtered') + ckpt = gp('model_ckpt_path', '') or os.environ.get('BEVPLACE_MODEL_CKPT', '') + db_path = gp('map_db_path', '') or os.environ.get('BEVPLACE_MAP_DB', '') + self.thresh = float(gp('match_dist_thresh', 1.0)) # L2 desc 거리 임계 + self.seed_once = bool(gp('seed_initialpose_once', True)) # /initialpose 1회만 씨딩 + self._seeded = False + # BEV 투영 파라미터 — bevplace.yaml(SWRule config_mounts 로 override) 에서 읽는다. + # 기본값은 모델 학습값(KITTI ±40m/0.4m). 맵 DB(build/db_to_mapdb)와 반드시 같아야 매칭. + proj = dict( + x_min=float(gp('x_min', -40.0)), x_max=float(gp('x_max', 40.0)), + y_min=float(gp('y_min', -40.0)), y_max=float(gp('y_max', 40.0)), + resolution=float(gp('resolution', 0.4)), max_density=float(gp('max_density', 20.0)), + ) + device = gp('device', 'auto') + self._ckpt, self._proj = ckpt, proj # CUDA 폴백 시 ckpt 재로드용 (.to 이동 금지) + self._cpu_fallback = False # 런타임 1회 폴백 가드 + try: + self.backend = BEVPlaceBackend(ckpt, device=device, **proj) if ckpt else None + except Exception as e: + if device == 'cpu' or not _is_cuda_error(e): + raise + self.get_logger().warn(f"[bevplace2] GPU 백엔드 초기화 실패({e}) → CPU 폴백") + self._cpu_fallback = True + try: + self.backend = BEVPlaceBackend(ckpt, device='cpu', **proj) + except Exception as e2: + self.get_logger().error(f"[bevplace2] CPU 폴백 초기화 실패: {e2}") + self.backend = None + self.db = DescriptorDB(db_path) if db_path and os.path.exists(db_path) else None + self.pub_reloc = self.create_publisher(PoseWithCovarianceStamped, '/perception/relocalization', 5) + self.pub_init = self.create_publisher(PoseWithCovarianceStamped, '/initialpose', 1) + self.pub_mk = self.create_publisher(MarkerArray, '/perception/reloc_markers', 5) + self.create_subscription(PointCloud2, self.input_topic, self.cb, 5) + if self.backend is None or self.db is None: + self.get_logger().warn("model/DB 미설정 — relocalization 비활성(발행 안 함)") + + def cb(self, msg): + if self.backend is None or self.db is None: + return + pts = np.array([[p[0], p[1], p[2], 0.0] + for p in pc2.read_points(msg, ('x', 'y', 'z'), skip_nans=True)], + dtype=np.float32) # intensity 무관(BEV=density). 필터클라우드는 xyz only + if len(pts) == 0: + return + try: + desc = self.backend.describe(pts) + except Exception as e: + if self._cpu_fallback or not _is_cuda_error(e): + raise + self.get_logger().warn("[bevplace2] 추론 중 CUDA 에러 → CPU 백엔드 재구성") + self._cpu_fallback = True + try: # 깨진 CUDA 컨텍스트 정리 (실패해도 무해) + import torch + del self.backend + torch.cuda.empty_cache() + except Exception: + pass + self.backend = None + try: # ckpt 에서 device='cpu' 로 재생성 (.to 이동 금지) + self.backend = BEVPlaceBackend(self._ckpt, device='cpu', **self._proj) + desc = self.backend.describe(pts) # 해당 프레임 1회 재시도 + except Exception as e2: + self.get_logger().error(f"[bevplace2] CPU 폴백 실패 — 프레임 스킵: {e2}") + return + idx, dist, topk = self.db.query(desc, topk=5) + if dist > self.thresh: + return # 신뢰도 낮으면 발행 안 함 + t, R = _pose_from_row(self.db.poses[idx]) + ps = PoseWithCovarianceStamped() + ps.header.frame_id = 'map' + ps.header.stamp = self.get_clock().now().to_msg() + ps.pose.pose.position.x, ps.pose.pose.position.y, ps.pose.pose.position.z = map(float, t) + qw = float(np.sqrt(max(0.0, 1.0 + R[0, 0] + R[1, 1] + R[2, 2])) / 2.0) or 1.0 + ps.pose.pose.orientation.w = qw + self.pub_reloc.publish(ps) + if not self.seed_once or not self._seeded: # 측위 씨딩 (1회만, 스트림 금지) + self.pub_init.publish(ps) + self._seeded = True + self._publish_markers(t) + + def _publish_markers(self, t): + ma = MarkerArray() + m = Marker() + m.header.frame_id = 'map'; m.ns = 'reloc'; m.id = 0 + m.type = Marker.ARROW; m.action = Marker.ADD + m.pose.position.x, m.pose.position.y = float(t[0]), float(t[1]) + m.scale.x, m.scale.y, m.scale.z = 2.0, 0.3, 0.3 + m.color.g = 1.0; m.color.a = 1.0 # ★ 초록 현재 위치 + ma.markers.append(m) + self.pub_mk.publish(ma) + + +def main(): + rclpy.init(); rclpy.spin(PlaceRecognition()); rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bevplace_backend.py b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bevplace_backend.py new file mode 100755 index 0000000..54815aa --- /dev/null +++ b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/bevplace_backend.py @@ -0,0 +1,75 @@ +"""BEVPlace2 descriptor 추출 backend (torch 격리, lazy import). +scan (N,>=3) → BEV density image → REIN/group-conv 백본 → global descriptor (D,). +ot_backend.py 의 구조 미러. 차이: range-image 대신 BEV 입력, REIN 회전-등변 백본. + +BEVPlace2 ckpt 는 DataParallel 로 저장될 수 있어 module. prefix 가 붙을 수 있다 → +state_dict 로드 시 strip 한다(OTBackend 와 다른 점, NOTES 와 동일하게 방어적으로 처리).""" +import os +import sys + +import numpy as np + +# colcon 설치본에서 같은 패키지의 flat 모듈(bev_projection)이 top-level 로 안 잡혀 +# ModuleNotFoundError → 자기 디렉토리를 path 에 추가 (bevplace2_node.py 와 동일 패턴). +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) +from bev_projection import BEVProjector + + +def _strip_module_prefix(state): + """DataParallel 저장본의 'module.' prefix 제거 (없으면 그대로).""" + if not any(k.startswith('module.') for k in state): + return state + return {k[len('module.'):] if k.startswith('module.') else k: v for k, v in state.items()} + + +class BEVPlaceBackend: + # 투영 기본값은 모델(BEVPlace2)이 학습된 KITTI Velodyne BEV 격자(±40m/0.4m=200x200)에 + # 맞춘다. 이 값은 "센서"가 아니라 "모델"이 기대하는 입력 레이아웃이라, 잘못 주면 점이 + # 격자의 엉뚱한 칸에 뭉쳐 descriptor 가 붕괴한다. 하드코딩 금지 — 호출부(node gp / + # build_map_db CLI / bevplace.yaml SWRule)가 Rule 값으로 덮어쓴다. + def __init__(self, ckpt_path, device='auto', x_min=-40.0, x_max=40.0, + y_min=-40.0, y_max=40.0, resolution=0.4, max_density=20.0): + import torch + bev_root = os.environ.get('BEVPLACE_ROOT', '/opt/BEVPlace2') + # vendored BEVPlace2 의 모델 정의(REIN.py)를 import 경로에 추가. 모듈 로드 시 + # transitively 끌어오는 보조 모듈(utils 등) 때문에 root + 하위도 path 에 넣는다. + for _p in (bev_root, os.path.join(bev_root, 'models'), os.path.join(bev_root, 'tools')): + if _p not in sys.path and os.path.isdir(_p): + sys.path.insert(0, _p) + from REIN import REIN # vendored BEVPlace2 백본: REM(rotation-equivariant)+NetVLAD, global_feat_dim=8192 + + self.proj = BEVProjector(x_min=x_min, x_max=x_max, y_min=y_min, y_max=y_max, + resolution=resolution, max_density=max_density) + # device='auto' → 런타임 감지: cuda 가용 시 cuda:0, 아니면 cpu (08-gpu-cpu-runtime N4) + if device == 'auto': + device = 'cuda:0' if torch.cuda.is_available() else 'cpu' + # CPU 런타임: vendored REIN.py forward 의 하드코딩 .cuda()(aff=torch.zeros(...).cuda())를 + # torch.Tensor.cuda no-op shim 으로 무력화 (벤더 모델 수정 없이). 이후 .type(x.type())가 + # cpu 로 정렬. GPU 없는 클라이언트 전제라 전역 안전. (08-gpu-cpu-runtime N4, objdet 동일 패턴) + if device == 'cpu' and not getattr(torch.Tensor, '_cpu_cuda_noop', False): + torch.Tensor.cuda = lambda self, *a, **k: self + torch.Tensor._cpu_cuda_noop = True + self.device = device + self._t = torch + model = REIN() + state = torch.load(ckpt_path, map_location='cpu') + state = state.get('state_dict', state) + model.load_state_dict(_strip_module_prefix(state), strict=False) + self._model = model.to(device).eval() + + def describe(self, points): + """(N,>=3) → (D,) float32 L2-정규화 descriptor.""" + br = self.proj.project(points) + bev = br.bev_image.copy() # (1,H,W) density BEV, [0,1] + # REIN(REM=resnet34 백본)은 3채널 입력을 기대한다. 업스트림(kitti_dataset.py)도 + # grayscale BEV 를 repeat(3,0)로 3채널화한다 → 동일하게 1ch density 를 3ch 로 복제. + bev = np.repeat(bev, 3, axis=0) # (3,H,W) + with self._t.no_grad(): + inp = self._t.from_numpy(bev).unsqueeze(0).to(self.device) # (1,3,H,W) + out = self._model(inp) + # REIN.forward → (out1, local_feats, global_desc). global descriptor 만 사용. + if isinstance(out, (tuple, list)): + out = out[-1] + d = out.cpu().numpy().reshape(-1).astype(np.float32) + n = np.linalg.norm(d) + 1e-9 + return d / n diff --git a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/build_map_db.py b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/build_map_db.py new file mode 100755 index 0000000..022813f --- /dev/null +++ b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/build_map_db.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 +"""맵 descriptor DB 빌드 one-shot. 맵 시퀀스(velodyne+poses) → map.npz. +deploy run.sh 가 DB 없을 때 수집 시퀀스로 1회 호출. flat import 라 이 파일이 있는 dir 에서 실행. +BEV 투영 파라미터는 query 노드(bevplace2_node)와 반드시 같아야 매칭이 맞으므로, run.sh 가 +bevplace.yaml 의 값을 --resolution 등으로 넘긴다(미지정 시 모델 학습값 KITTI 기본). 하드코딩 금지. +사용: python build_map_db.py [--resolution .. ...]""" +import argparse +from bevplace_backend import BEVPlaceBackend +from descriptor_db import build_db + + +def _is_cuda_error(e): + s = str(e).lower() + return 'cuda' in s or 'cudnn' in s or 'out of memory' in s + + +if __name__ == '__main__': + ap = argparse.ArgumentParser() + ap.add_argument('ckpt'); ap.add_argument('scan_dir'); ap.add_argument('poses'); ap.add_argument('out') + ap.add_argument('--x_min', type=float, default=-40.0); ap.add_argument('--x_max', type=float, default=40.0) + ap.add_argument('--y_min', type=float, default=-40.0); ap.add_argument('--y_max', type=float, default=40.0) + ap.add_argument('--resolution', type=float, default=0.4); ap.add_argument('--max_density', type=float, default=20.0) + a = ap.parse_args() + proj = dict(x_min=a.x_min, x_max=a.x_max, y_min=a.y_min, y_max=a.y_max, + resolution=a.resolution, max_density=a.max_density) + try: + be = BEVPlaceBackend(a.ckpt, **proj) + except Exception as e: + if not _is_cuda_error(e): + raise + print(f"[bevplace2] GPU 백엔드 초기화 실패({e}) → CPU 폴백") + be = BEVPlaceBackend(a.ckpt, device='cpu', **proj) + try: + shape = build_db(be, a.scan_dir, a.poses, a.out) + except Exception as e: + if be.device == 'cpu' or not _is_cuda_error(e): + raise + print("[bevplace2] 추론 중 CUDA 에러 → CPU 백엔드 재구성") + try: # 깨진 CUDA 컨텍스트 정리 (실패해도 무해) + del be + import torch + torch.cuda.empty_cache() + except Exception: + pass + be = BEVPlaceBackend(a.ckpt, device='cpu', **proj) # ckpt 에서 재생성 (.to 이동 금지) + shape = build_db(be, a.scan_dir, a.poses, a.out) + print(f"[map_db] built {a.out} {shape} (res={a.resolution} range=[{a.x_min},{a.x_max}])") diff --git a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/db_to_mapdb.py b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/db_to_mapdb.py new file mode 100755 index 0000000..04087fb --- /dev/null +++ b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/db_to_mapdb.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python3 +"""선택된 3D slam_map(RTABMap .db) → 맵 descriptor DB(map.npz) 직접 빌드 (자동 빌드 경로). + +deploy run.sh 가 map.npz 없을 때(또는 선택 맵이 바뀌었을 때) 호출한다. deploy 이미지엔 +rtabmap 바이너리가 없으므로 .db 안의 두 BLOB 을 직접 디코드해 한 방에 만든다: + - 최적화 pose: Admin.opt_ids + Admin.opt_poses (둘 다 zlib). loop-closure 보정 그래프 pose + → bevplace2_node 측위 좌표가 nav2 가 쓰는 .db 기하맵과 동일 프레임이 됨. + - 스캔: Data.scan (zlib, XYZ float32). RTABMap 저장 필터/등록 스캔 = /lidar_filtered + 분포와 일치 → query 와 같은 분포라 descriptor 가 맞는다. +node id 로 둘을 매칭 → BEVPlaceBackend.describe → (M,D) descriptors + (M,12) poses → map.npz. + +BEV 투영 파라미터는 query 노드와 반드시 같아야 매칭이 맞으므로 run.sh 가 bevplace.yaml 값을 +--resolution 등으로 넘긴다(미지정 시 모델 학습값 KITTI 기본). 하드코딩 금지. +사용: python db_to_mapdb.py [--resolution .. ...]""" +import argparse +import os +import sqlite3 +import sys +import zlib + +import numpy as np + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) # flat import (colcon entry 와 동일) +from bevplace_backend import BEVPlaceBackend + + +def _is_cuda_error(e): + s = str(e).lower() + return 'cuda' in s or 'cudnn' in s or 'out of memory' in s + + +def load_opt_poses(db_path): + """Admin.opt_ids(zlib int32) + opt_poses(zlib float32, id당 3x4=12) → {node_id: (12,) float32}.""" + c = sqlite3.connect(db_path) + try: + cur = c.cursor() + cur.execute("SELECT opt_ids, opt_poses FROM Admin ORDER BY time_enter DESC LIMIT 1") + row = cur.fetchone() + finally: + c.close() + if not row or row[0] is None or row[1] is None: + raise SystemExit("[db2map] 에러: Admin.opt_poses 없음 — 최적화 안 된 .db (SLAM 미완료?)") + ids = np.frombuffer(zlib.decompress(row[0]), dtype=np.int32) + poses = np.frombuffer(zlib.decompress(row[1]), dtype=np.float32).reshape(len(ids), 12) + return {int(i): poses[k].copy() for k, i in enumerate(ids)} + + +def load_scans(db_path): + """Data.scan(zlib XYZ float32) → {node_id: (N,4) xyzi}. intensity 0 (BEV=density 써 무관).""" + c = sqlite3.connect(db_path) + out = {} + try: + cur = c.cursor() + cur.execute("SELECT id, scan FROM Data WHERE scan IS NOT NULL AND length(scan)>0 ORDER BY id") + for nid, blob in cur.fetchall(): + raw = zlib.decompress(blob) + nf = len(raw) // 4 + if nf % 3 == 0: + pts = np.frombuffer(raw, dtype=np.float32, count=nf).reshape(-1, 3) + elif nf % 4 == 0: + pts = np.frombuffer(raw, dtype=np.float32, count=nf).reshape(-1, 4)[:, :3] + else: + print(f"[db2map] 경고: node {nid} scan 포맷 불명({nf} floats) — 스킵") + continue + xyzi = np.zeros((pts.shape[0], 4), dtype=np.float32) + xyzi[:, :3] = pts + out[int(nid)] = xyzi + finally: + c.close() + return out + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument('ckpt') + ap.add_argument('db', help='선택된 RTABMap .db (3D slam_map)') + ap.add_argument('out', help='출력 map.npz') + ap.add_argument('--x_min', type=float, default=-40.0) + ap.add_argument('--x_max', type=float, default=40.0) + ap.add_argument('--y_min', type=float, default=-40.0) + ap.add_argument('--y_max', type=float, default=40.0) + ap.add_argument('--resolution', type=float, default=0.4) + ap.add_argument('--max_density', type=float, default=20.0) + a = ap.parse_args() + + poses = load_opt_poses(a.db) + scans = load_scans(a.db) + ids = sorted(set(poses) & set(scans)) + print(f"[db2map] opt_poses={len(poses)} scans={len(scans)} aligned={len(ids)}") + if not ids: + raise SystemExit("[db2map] 에러: pose∩scan 교집합 0 — 손상된 .db") + + proj = dict(x_min=a.x_min, x_max=a.x_max, y_min=a.y_min, y_max=a.y_max, + resolution=a.resolution, max_density=a.max_density) + try: + be = BEVPlaceBackend(a.ckpt, **proj) + except Exception as e: + if not _is_cuda_error(e): + raise + print(f"[bevplace2] GPU 백엔드 초기화 실패({e}) → CPU 폴백") + be = BEVPlaceBackend(a.ckpt, device='cpu', **proj) + try: + descs = np.stack([be.describe(scans[i]) for i in ids]).astype(np.float32) + except Exception as e: + if be.device == 'cpu' or not _is_cuda_error(e): + raise + print("[bevplace2] 추론 중 CUDA 에러 → CPU 백엔드 재구성") + try: # 깨진 CUDA 컨텍스트 정리 (실패해도 무해) + del be + import torch + torch.cuda.empty_cache() + except Exception: + pass + be = BEVPlaceBackend(a.ckpt, device='cpu', **proj) # ckpt 에서 재생성 (.to 이동 금지) + descs = np.stack([be.describe(scans[i]) for i in ids]).astype(np.float32) + pose_arr = np.stack([poses[i] for i in ids]).astype(np.float32) # (M,12) 3x4 row-major + os.makedirs(os.path.dirname(a.out), exist_ok=True) + np.savez(a.out, descriptors=descs, poses=pose_arr) + print(f"[db2map] built {a.out} desc={descs.shape} poses={pose_arr.shape} " + f"(res={a.resolution} range=[{a.x_min},{a.x_max}])") + + +if __name__ == '__main__': + main() diff --git a/src/perception_bevplace2_pkg/perception_bevplace2_pkg/descriptor_db.py b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/descriptor_db.py new file mode 100755 index 0000000..310e296 --- /dev/null +++ b/src/perception_bevplace2_pkg/perception_bevplace2_pkg/descriptor_db.py @@ -0,0 +1,34 @@ +"""맵 descriptor DB 빌드/검색. 맵 시퀀스(velodyne+poses) → descriptor 행렬 + pose 배열. +검색: 현재 descriptor 와 L2 최근접 → matched 프레임 idx → 그 프레임 pose. faiss-cpu. +backend-agnostic (BEVPlaceBackend.describe / OTBackend.describe 동일 시그니처 공유).""" +import os +import numpy as np + + +def build_db(backend, scan_dir, poses_path, out_npz): + """scan_dir/*.bin 전부 describe → (M,D) descriptors + (M,12) poses → npz 저장.""" + bins = sorted(f for f in os.listdir(scan_dir) if f.endswith('.bin')) + descs = [] + for b in bins: + pts = np.fromfile(os.path.join(scan_dir, b), dtype=np.float32).reshape(-1, 4) + descs.append(backend.describe(pts)) + descs = np.stack(descs).astype(np.float32) + poses = np.loadtxt(poses_path).reshape(-1, 12).astype(np.float32) + os.makedirs(os.path.dirname(out_npz), exist_ok=True) + np.savez(out_npz, descriptors=descs, poses=poses) + return descs.shape + + +class DescriptorDB: + def __init__(self, npz_path): + import faiss + z = np.load(npz_path) + self.descs = z['descriptors'].astype(np.float32) + self.poses = z['poses'] + self.index = faiss.IndexFlatL2(self.descs.shape[1]) + self.index.add(self.descs) + + def query(self, descriptor, topk=5): + """descriptor (D,) → (best_idx, best_dist, topk_idxs). pose 는 self.poses[idx].""" + d, i = self.index.search(descriptor.reshape(1, -1), topk) + return int(i[0, 0]), float(d[0, 0]), i[0].tolist() diff --git a/src/perception_bevplace2_pkg/resource/perception_bevplace2_pkg b/src/perception_bevplace2_pkg/resource/perception_bevplace2_pkg new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_bevplace2_pkg/setup.cfg b/src/perception_bevplace2_pkg/setup.cfg new file mode 100755 index 0000000..6dd6e14 --- /dev/null +++ b/src/perception_bevplace2_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/perception_bevplace2_pkg +[install] +install_scripts=$base/lib/perception_bevplace2_pkg diff --git a/src/perception_bevplace2_pkg/setup.py b/src/perception_bevplace2_pkg/setup.py new file mode 100755 index 0000000..3d75aa0 --- /dev/null +++ b/src/perception_bevplace2_pkg/setup.py @@ -0,0 +1,35 @@ + +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'perception_bevplace2_pkg' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='dgist', + maintainer_email='jang1future@gmail.com', + description='LiDAR place recognition (BEVPlace2) perception module', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'bevplace2 = perception_bevplace2_pkg.bevplace2_node:main' + ], + }, +) diff --git a/src/perception_bevplace2_pkg/test/test_bev_projection.py b/src/perception_bevplace2_pkg/test/test_bev_projection.py new file mode 100755 index 0000000..3a0b601 --- /dev/null +++ b/src/perception_bevplace2_pkg/test/test_bev_projection.py @@ -0,0 +1,33 @@ +import numpy as np +from perception_bevplace2_pkg.bev_projection import BEVProjector + + +def test_bev_shape_and_range(): + # ±40m / 0.4m → 200x200 격자. 범위 밖 점은 drop, 안쪽 점만 valid. + proj = BEVProjector(x_min=-40.0, x_max=40.0, y_min=-40.0, y_max=40.0, + resolution=0.4, max_density=20.0) + assert proj.W == 200 and proj.H == 200 + + pts = np.array([ + [0.0, 0.0, 0.0, 0.0], # 중앙 (valid) + [10.0, -10.0, 1.0, 0.0], # valid + [100.0, 0.0, 0.0, 0.0], # x 범위 밖 (drop) + [0.0, -100.0, 0.0, 0.0], # y 범위 밖 (drop) + ], dtype=np.float32) + out = proj.project(pts) + assert out.bev_image.shape == (1, 200, 200) + assert out.valid.tolist() == [True, True, False, False] + # valid 점 2개가 BEV 에 density 로 찍혔는지 + assert out.bev_image.sum() > 0 + assert out.grid_x.shape[0] == int(out.valid.sum()) + + +def test_bev_density_accumulates_and_normalizes(): + # 같은 셀에 여러 점 → density 누적, max_density 로 [0,1] 정규화 (1.0 초과 없음). + proj = BEVProjector(x_min=-40.0, x_max=40.0, y_min=-40.0, y_max=40.0, + resolution=0.4, max_density=4.0) + pts = np.tile(np.array([[1.0, 1.0, 0.0, 0.0]], dtype=np.float32), (10, 1)) + out = proj.project(pts) + assert out.bev_image.max() <= 1.0 + 1e-6 + # 10점이 한 셀에 → max_density=4 클립 → 정확히 1.0 + assert abs(out.bev_image.max() - 1.0) < 1e-6 diff --git a/src/perception_bevplace2_pkg/test/test_copyright.py b/src/perception_bevplace2_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/src/perception_bevplace2_pkg/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/perception_bevplace2_pkg/test/test_flake8.py b/src/perception_bevplace2_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/src/perception_bevplace2_pkg/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/perception_bevplace2_pkg/test/test_pep257.py b/src/perception_bevplace2_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/src/perception_bevplace2_pkg/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/perception_centerpoint_pkg/cfgs/centerpoint_custom.yaml b/src/perception_centerpoint_pkg/cfgs/centerpoint_custom.yaml new file mode 100755 index 0000000..4bf5a74 --- /dev/null +++ b/src/perception_centerpoint_pkg/cfgs/centerpoint_custom.yaml @@ -0,0 +1,98 @@ +CLASS_NAMES: ['Car', 'Pedestrian', 'Cyclist'] + +DATA_CONFIG: + _BASE_CONFIG_: cfgs/dataset_configs/custom_dataset.yaml + +MODEL: + NAME: CenterPoint + + # CenterPoint(Pillar variant): PointPillars 와 동일한 pillar + 2D conv 백본을 쓰고 + # 헤드만 anchor → center heatmap 으로 교체 (sparse conv 미사용 → sm_52 GPU 가용). + VFE: + NAME: PillarVFE + WITH_DISTANCE: False + USE_ABSLOTE_XYZ: True + USE_NORM: True + NUM_FILTERS: [64] + + MAP_TO_BEV: + NAME: PointPillarScatter + NUM_BEV_FEATURES: 64 + + BACKBONE_2D: + NAME: BaseBEVBackbone + LAYER_NUMS: [3, 5, 5] + LAYER_STRIDES: [2, 2, 2] + NUM_FILTERS: [64, 128, 256] + UPSAMPLE_STRIDES: [0.5, 1, 2] + NUM_UPSAMPLE_FILTERS: [128, 128, 128] + + DENSE_HEAD: + NAME: CenterHead + CLASS_AGNOSTIC: False + + CLASS_NAMES_EACH_HEAD: [ + ['Car', 'Pedestrian', 'Cyclist'] + ] + + SHARED_CONV_CHANNEL: 64 + USE_BIAS_BEFORE_NORM: True + NUM_HM_CONV: 2 + SEPARATE_HEAD_CFG: + HEAD_ORDER: ['center', 'center_z', 'dim', 'rot'] + HEAD_DICT: { + 'center': {'out_channels': 2, 'num_conv': 2}, + 'center_z': {'out_channels': 1, 'num_conv': 2}, + 'dim': {'out_channels': 3, 'num_conv': 2}, + 'rot': {'out_channels': 2, 'num_conv': 2}, + } + + TARGET_ASSIGNER_CONFIG: + FEATURE_MAP_STRIDE: 2 + NUM_MAX_OBJS: 500 + GAUSSIAN_OVERLAP: 0.1 + MIN_RADIUS: 2 + + LOSS_CONFIG: + LOSS_WEIGHTS: { + 'cls_weight': 1.0, + 'loc_weight': 2.0, + 'code_weights': [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + } + + POST_PROCESSING: + SCORE_THRESH: 0.1 + POST_CENTER_LIMIT_RANGE: [-61.2, -61.2, -10.0, 61.2, 61.2, 10.0] + MAX_OBJ_PER_SAMPLE: 500 + NMS_CONFIG: + NMS_TYPE: nms_gpu + NMS_THRESH: 0.7 + NMS_PRE_MAXSIZE: 4096 + NMS_POST_MAXSIZE: 500 + + POST_PROCESSING: + RECALL_THRESH_LIST: [0.3, 0.5, 0.7] + SCORE_THRESH: 0.1 + OUTPUT_RAW_SCORE: False + EVAL_METRIC: kitti + +OPTIMIZATION: + BATCH_SIZE_PER_GPU: 2 + NUM_EPOCHS: 80 + + OPTIMIZER: adam_onecycle + LR: 0.003 + WEIGHT_DECAY: 0.01 + MOMENTUM: 0.9 + + MOMS: [0.95, 0.85] + PCT_START: 0.4 + DIV_FACTOR: 10 + DECAY_STEP_LIST: [35, 45] + LR_DECAY: 0.1 + LR_CLIP: 0.0000001 + + LR_WARMUP: False + WARMUP_EPOCH: 1 + + GRAD_NORM_CLIP: 10 diff --git a/src/perception_centerpoint_pkg/cfgs/custom_dataset.yaml b/src/perception_centerpoint_pkg/cfgs/custom_dataset.yaml new file mode 100755 index 0000000..06ef0a7 --- /dev/null +++ b/src/perception_centerpoint_pkg/cfgs/custom_dataset.yaml @@ -0,0 +1,58 @@ +DATASET: 'CustomDataset' +DATA_PATH: '/perception_data/data' +# 우리 FR09 라이다(360°) → 대칭 range. z[-2,4] 1칸 pillar. x/y 140.8/0.16=880 (16배수) +POINT_CLOUD_RANGE: [-51.2, -51.2, -2, 51.2, 51.2, 4] +MAP_CLASS_TO_KITTI: { + 'Car': 'Car', + 'Pedestrian': 'Pedestrian', + 'Cyclist': 'Cyclist', +} +DATA_SPLIT: { + 'train': train, + 'test': val +} +INFO_PATH: { + 'train': [custom_infos_train.pkl], + 'test': [custom_infos_val.pkl], +} +POINT_FEATURE_ENCODING: { + encoding_type: absolute_coordinates_encoding, + used_feature_list: ['x', 'y', 'z', 'intensity'], + src_feature_list: ['x', 'y', 'z', 'intensity'], +} +DATA_AUGMENTOR: + DISABLE_AUG_LIST: ['gt_sampling'] # 1차 smoke 안정화용. 검증 후 풀어 성능↑ + AUG_CONFIG_LIST: + - NAME: gt_sampling + USE_ROAD_PLANE: False + DB_INFO_PATH: + - custom_dbinfos_train.pkl + PREPARE: { + filter_by_min_points: ['Car:5', 'Pedestrian:5', 'Cyclist:5'], + } + SAMPLE_GROUPS: ['Car:15', 'Pedestrian:10', 'Cyclist:10'] + NUM_POINT_FEATURES: 4 + DATABASE_WITH_FAKELIDAR: False + REMOVE_EXTRA_WIDTH: [0.0, 0.0, 0.0] + LIMIT_WHOLE_SCENE: True + - NAME: random_world_flip + ALONG_AXIS_LIST: ['x', 'y'] + - NAME: random_world_rotation + WORLD_ROT_ANGLE: [-0.78539816, 0.78539816] + - NAME: random_world_scaling + WORLD_SCALE_RANGE: [0.95, 1.05] +DATA_PROCESSOR: + - NAME: mask_points_and_boxes_outside_range + REMOVE_OUTSIDE_BOXES: True + - NAME: shuffle_points + SHUFFLE_ENABLED: { + 'train': True, + 'test': False + } + - NAME: transform_points_to_voxels + VOXEL_SIZE: [0.16, 0.16, 6] + MAX_POINTS_PER_VOXEL: 32 + MAX_NUMBER_OF_VOXELS: { + 'train': 16000, + 'test': 40000 + } diff --git a/src/perception_centerpoint_pkg/config/centerpoint.yaml b/src/perception_centerpoint_pkg/config/centerpoint.yaml new file mode 100755 index 0000000..94da199 --- /dev/null +++ b/src/perception_centerpoint_pkg/config/centerpoint.yaml @@ -0,0 +1,21 @@ + + +centerpoint_centerpoint: + ros__parameters: + input_topic: "/raw/lidar" + detection_topic: "/perception/detections" + marker_topic: "/perception/detection_markers" + # 모델 경로는 여기서 지정하거나, 비우면 환경변수 + # (CENTERPOINT_MODEL_CFG / CENTERPOINT_MODEL_CKPT)에서 읽는다. + model_cfg_path: "" + model_ckpt_path: "" + score_threshold: 0.2 + device: "auto" # auto = 런타임 감지(GPU 있으면 cuda, 없으면 cpu). Phase3 dual-runtime + # CARLA 라이다 보정 (학습분포에 맞춤). 측정값 기반: + # 지면 z -1.06(CARLA) vs -1.73(KITTI), ~12k 점(KITTI 대비 희박). + z_offset: 1.0 # 입력 z 보정 (스윕상 max score 최적 ~+1.0) + max_range: 25.0 # 이 반경(m) 밖 박스 제거 (근~중거리 신뢰) + # 기하 검증 — 빈 공간/바닥 헛검출 제거 + min_points_in_box: 5 # 박스 안 점 5개 미만 → 버림 (검정영역 헛박스) + min_box_height: 0.5 # 박스 안 점 높이 0.5m 미만 → 버림 (평평한 도로 오검출) + use_sim_time: true diff --git a/src/perception_centerpoint_pkg/launch/centerpoint.launch.py b/src/perception_centerpoint_pkg/launch/centerpoint.launch.py new file mode 100755 index 0000000..3c3553b --- /dev/null +++ b/src/perception_centerpoint_pkg/launch/centerpoint.launch.py @@ -0,0 +1,127 @@ + +import os +import shutil + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node + + +# 선언 기본값(pretrained 베이스라인) — 모델 자동 resolve 에서 "사용자가 명시했는지" +# 판별 기준으로도 쓰므로 상수로 둔다. +# 기본값: OpenPCDet nuScenes CenterPoint(Dynamic-Pillar, spconv 미사용 → sm_52 가용) +# config(이미지 내장) + 이미지에 baked 된 공식 pretrained ckpt. +# KITTI 엔 CenterPoint pretrained 가 없어 nuScenes(cbgs_dyn_pp_centerpoint) 를 베이스라인으로 +# 사용한다 — 도메인 갭이 있으므로 정식 사용은 CARLA finetuned(/perception_data) 가 우선. +DEFAULT_MODEL_CFG = '/opt/OpenPCDet/tools/cfgs/nuscenes_models/cbgs_dyn_pp_centerpoint.yaml' +DEFAULT_MODEL_CKPT = '/opt/centerpoint_pretrained/cbgs_dyn_pp_centerpoint.pth' + +# 가중치 공유 단일 위치(docker/local): /perception_models/centerpoint/{finetuned,pretrained} +# (호스트 .caches/config/nav2_config/perception/models/centerpoint/*, SWRule config_mounts). +FINETUNED_CKPT = '/perception_models/centerpoint/finetuned/centerpoint_finetuned.pth' +SHARED_PRE_CKPT = '/perception_models/centerpoint/pretrained/cbgs_dyn_pp_centerpoint.pth' # 공유 pretrained (Phase 2 채움) +# finetuned 용 custom cfg 는 _BASE_CONFIG_ 상대참조(cfgs/...)가 풀리도록 +# 단독 deploy run.sh 와 동일하게 OpenPCDet tools/cfgs 아래에 복사해 쓴다. +OPENPCDET_CFGS_DIR = '/opt/OpenPCDet/tools/cfgs' +RESOLVED_MODEL_CFG = os.path.join(OPENPCDET_CFGS_DIR, 'custom_models', 'centerpoint_custom.yaml') + + +ARGUMENTS = [ + DeclareLaunchArgument('use_sim_time', default_value='true'), + # 입력 토픽(/raw/lidar)은 sensor_bridge 가 robot 무관하게 정규화하므로 + # robot 별 config 분기는 두지 않는다. robot arg 는 인터페이스 호환용. + DeclareLaunchArgument('robot', default_value='mkmini', choices=['mkmini', 'fr09']), + # 모델 소스 스위치 — UI 에서 pretrained 고르면 finetuned 있어도 건너뜀(기본 finetuned). + # 'pretrained' 외의 값은 전부 finetuned 우선으로 본다(단독 deploy run.sh 와 동일 의미론). + DeclareLaunchArgument('model_source', default_value='finetuned'), + # 모델 경로 — Rule(launcher 의 ROS_PARAM)이 덮어쓸 수 있게 launch arg 로 노출. + DeclareLaunchArgument('model_cfg_path', default_value=DEFAULT_MODEL_CFG), + DeclareLaunchArgument('model_ckpt_path', default_value=DEFAULT_MODEL_CKPT), +] + + +def _resolve_model(context, *args, **kwargs): + """모델 cfg/ckpt 자동 resolve (단독 deploy run.sh 의 finetuned 우선 로직 이식). + + - 사용자가 model_cfg_path / model_ckpt_path 를 명시적으로 줬으면(선언 기본값과 다르고 + 빈값이 아니면) 그대로 쓴다 — run.sh 가 MODEL_ARGS 로 명시 전달하는 기존 동작 무회귀. + - 인자가 기본값/빈값일 때만 model_source(기본 finetuned) 와 finetuned 파일 존재 여부로 + 자동 결정. finetuned 채택 시 pkg 에 편입된 cfgs/ 를 OpenPCDet 경로에 복사한다. + """ + use_sim_time = LaunchConfiguration('use_sim_time') + + model_cfg_path = LaunchConfiguration('model_cfg_path').perform(context) + model_ckpt_path = LaunchConfiguration('model_ckpt_path').perform(context) + model_source = LaunchConfiguration('model_source').perform(context) or 'finetuned' + + pkg_centerpoint = get_package_share_directory('perception_centerpoint_pkg') + + # 명시 전달 여부 — 둘 중 하나라도 기본값과 다른 비어있지 않은 값이면 명시로 본다. + explicit = ((model_cfg_path and model_cfg_path != DEFAULT_MODEL_CFG) + or (model_ckpt_path and model_ckpt_path != DEFAULT_MODEL_CKPT)) + + if not explicit: + if model_source != 'pretrained' and os.path.isfile(FINETUNED_CKPT): + # custom cfg 복사 — run.sh 의 cp 순서와 동일 (dataset cfg → model cfg) + os.makedirs(os.path.join(OPENPCDET_CFGS_DIR, 'dataset_configs'), exist_ok=True) + shutil.copy( + os.path.join(pkg_centerpoint, 'cfgs', 'custom_dataset.yaml'), + os.path.join(OPENPCDET_CFGS_DIR, 'dataset_configs', 'custom_dataset.yaml')) + os.makedirs(os.path.join(OPENPCDET_CFGS_DIR, 'custom_models'), exist_ok=True) + shutil.copy( + os.path.join(pkg_centerpoint, 'cfgs', 'centerpoint_custom.yaml'), + RESOLVED_MODEL_CFG) + model_cfg_path = RESOLVED_MODEL_CFG + model_ckpt_path = FINETUNED_CKPT + print(f'[centerpoint] 재학습 모델 사용: {FINETUNED_CKPT}') + elif os.path.isfile(SHARED_PRE_CKPT): + # 공유 pretrained(nuScenes base) — baked(/opt) 보다 우선. cfg 는 base 와 동일. + model_cfg_path = DEFAULT_MODEL_CFG + model_ckpt_path = SHARED_PRE_CKPT + print(f'[centerpoint] 공유 pretrained 사용: {SHARED_PRE_CKPT}') + else: + # pretrained 폴백 — 빈값으로 들어온 경우도 기본값으로 확정 + model_cfg_path = DEFAULT_MODEL_CFG + model_ckpt_path = DEFAULT_MODEL_CKPT + print('[centerpoint] 재학습 모델 없음 → 기본 pretrained(baked) 사용') + + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + params_file = PathJoinSubstitution([pkg_centerpoint, 'config', 'centerpoint.yaml']) + + # nav2_common.RewrittenYaml 대신 yaml + dict override 로 단순화 + # (future-centerpoint 이미지는 ros-base 만 깔려 nav2_common 이 없음). + load_nodes = GroupAction( + actions=[ + Node( + package='perception_centerpoint_pkg', + executable='centerpoint', + name='centerpoint_centerpoint', + output='screen', + parameters=[ + params_file, + { + 'use_sim_time': use_sim_time, + 'model_cfg_path': model_cfg_path, + 'model_ckpt_path': model_ckpt_path, + }, + ], + remappings=remappings + ) + ], + ) + + return [load_nodes] + + +def generate_launch_description(): + ld = LaunchDescription(ARGUMENTS) + # 모델 경로는 인자 평가가 필요해 OpaqueFunction 안에서 resolve 후 노드를 만든다 + ld.add_action(OpaqueFunction(function=_resolve_model)) + + return ld diff --git a/src/perception_centerpoint_pkg/launch/perception.launch.py b/src/perception_centerpoint_pkg/launch/perception.launch.py new file mode 100755 index 0000000..293f46e --- /dev/null +++ b/src/perception_centerpoint_pkg/launch/perception.launch.py @@ -0,0 +1,32 @@ +"""perception_manager 공통 진입점 (통합 perception 모듈). + +LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로 +`ros2 launch perception_centerpoint_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..` +를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(centerpoint.launch.py)로 위임한다. +robot 은 공통 계약상 선언만 하고, 모델 launch 가 선언한 경우에만 소비된다. +""" +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + share = get_package_share_directory('perception_centerpoint_pkg') + return LaunchDescription([ + DeclareLaunchArgument('robot', default_value='fr09'), + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('model_source', default_value='finetuned'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(share, 'launch', 'centerpoint.launch.py')), + launch_arguments={ + 'robot': LaunchConfiguration('robot'), + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'model_source': LaunchConfiguration('model_source'), + }.items(), + ), + ]) diff --git a/src/perception_centerpoint_pkg/package.xml b/src/perception_centerpoint_pkg/package.xml new file mode 100755 index 0000000..0800eae --- /dev/null +++ b/src/perception_centerpoint_pkg/package.xml @@ -0,0 +1,26 @@ + + + + perception_centerpoint_pkg + 0.0.0 + LiDAR 3D object detection (CenterPoint / OpenPCDet) perception module + dgist + TODO: License declaration + + rclpy + sensor_msgs + sensor_msgs_py + vision_msgs + visualization_msgs + geometry_msgs + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception_centerpoint_pkg/perception_centerpoint_pkg/__init__.py b/src/perception_centerpoint_pkg/perception_centerpoint_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_centerpoint_pkg/perception_centerpoint_pkg/centerpoint_backend.py b/src/perception_centerpoint_pkg/perception_centerpoint_pkg/centerpoint_backend.py new file mode 100755 index 0000000..742cacd --- /dev/null +++ b/src/perception_centerpoint_pkg/perception_centerpoint_pkg/centerpoint_backend.py @@ -0,0 +1,219 @@ +#!/usr/bin/env python3 +"""OpenPCDet CenterPoint(Pillar) 추론 래퍼. + +ROS 의존이 없는 순수 추론 모듈. OpenPCDet(pcdet) / torch 는 이 클래스를 +인스턴스화할 때만 import 한다 → OpenPCDet 미설치 환경에서도 패키지 build/import 는 통과. +런타임(도커 이미지)에 OpenPCDet 가 설치돼 있어야 실제 추론이 동작한다. + +CenterPoint(Pillar) 는 PointPillars 와 동일한 pillar + 2D conv 백본을 쓰고 +헤드만 anchor → center heatmap 으로 교체한 상위호환이다(sparse conv 미사용 → +sm_52 GPU 가용). OpenPCDet 의 cfg/checkpoint 인터페이스가 동일하므로 본 백엔드는 +PointPillars 백엔드와 동일한 인터페이스(infer → boxes/scores/labels)를 유지한다. +다른 검출 모델로 교체 시 이 파일만 갈아끼우면 된다(노드 인터페이스 불변). +""" + +import os + +import numpy as np + + +class CenterPointBackend: + """OpenPCDet config + checkpoint 를 로드해 단일 프레임 포인트클라우드를 추론한다. + + infer(points) -> (boxes, scores, labels) + points : (N, 4) float32 — x, y, z, intensity + boxes : (M, 7) float32 — x, y, z, dx, dy, dz, heading(yaw, rad) + scores : (M,) float32 + labels : (M,) int — 1-based class index (class_names[label-1]) + """ + + def __init__(self, cfg_path, ckpt_path, score_threshold=0.3, device='auto', + z_offset=0.0, min_points_in_box=0, min_box_height=0.0, + logger=None): + # OpenPCDet / torch 는 여기서만 import (미설치 시 패키지 import 자체는 안 깨지게) + import torch + from pcdet.config import cfg, cfg_from_yaml_file + from pcdet.models import build_network, load_data_to_gpu + from pcdet.datasets import DatasetTemplate + from pcdet.utils import common_utils + + self._torch = torch + self._load_data_to_gpu = load_data_to_gpu + self.score_threshold = float(score_threshold) + # device='auto' → 런타임 감지: cuda 가용 시 cuda:0, 아니면 cpu (08-gpu-cpu-runtime N4) + if device == 'auto': + device = 'cuda:0' if torch.cuda.is_available() else 'cpu' + self.device = device + self._cpu = (device == 'cpu') + # CPU 런타임이면 OpenPCDet 의 CUDA 하드코딩(NMS + anchor 생성 .cuda())을 CPU 로 우회 + # (Phase3 dual-runtime). spconv 미사용 모델이라 이 두 패치로 CPU 완결. build_network 전에 적용. + if self._cpu: + self._patch_cpu() + # 보정: 입력 z 에 더하는 오프셋. 라이다 장착 높이/지면 z 가 학습분포와 + # 어긋날 때 맞춘다 (CARLA 지면 ~ -1.06, KITTI ~ -1.73). 0 이면 비활성. + self.z_offset = float(z_offset) + # 기하 검증: 박스 안에 실제 점이 충분/입체적으로 있어야 진짜 객체로 인정. + # min_points_in_box : 박스 내부 최소 점 개수 (빈 공간 헛검출 제거) + # min_box_height : 박스 내부 점들의 z 스프레드 최소값(m) (평평한 바닥 오검출 제거) + self.min_points_in_box = int(min_points_in_box) + self.min_box_height = float(min_box_height) + + # OpenPCDet cfg 의 _BASE_CONFIG_ 는 tools/ 기준 상대경로라, 그 디렉토리에서 로드해야 + # dataset base config 가 해석된다. (cfg_path 자체는 절대경로 허용) + opcdet_tools = os.path.join( + os.environ.get('OPENPCDET_ROOT', '/opt/OpenPCDet'), 'tools') + prev_cwd = os.getcwd() + try: + if os.path.isdir(opcdet_tools): + os.chdir(opcdet_tools) + cfg_from_yaml_file(cfg_path, cfg) + finally: + os.chdir(prev_cwd) + self.class_names = list(cfg.CLASS_NAMES) + + log = logger or common_utils.create_logger() + + # 모델의 data_processor / point_feature_encoder 만 빌리기 위한 최소 dataset + self._dataset = DatasetTemplate( + dataset_cfg=cfg.DATA_CONFIG, + class_names=self.class_names, + training=False, + root_path=None, + logger=log, + ) + + # 모델 cfg 가 기대하는 입력 point feature 수 (custom=4 [x,y,z,intensity], + # nuScenes pretrained=5 [+timestamp]). infer 입력을 여기에 맞춰 패딩/절단(하드코딩 없음). + try: + self._num_src_feat = len(cfg.DATA_CONFIG.POINT_FEATURE_ENCODING.src_feature_list) + except Exception: + self._num_src_feat = 4 + + self._model = build_network( + model_cfg=cfg.MODEL, + num_class=len(self.class_names), + dataset=self._dataset, + ) + self._model.load_params_from_file( + filename=ckpt_path, logger=log, to_cpu=(device == 'cpu')) + self._model.to(device) + self._model.eval() + + def infer(self, points): + torch = self._torch + points = np.ascontiguousarray(points, dtype=np.float32) + + if self.z_offset: + points = points.copy() + points[:, 2] += self.z_offset + + # cfg 가 기대하는 feature 수로 정합 (nuScenes pretrained 는 5번째 timestamp 채널 필요) + if points.shape[1] < self._num_src_feat: + pad = np.zeros((points.shape[0], self._num_src_feat - points.shape[1]), + dtype=np.float32) + points = np.hstack([points, pad]) + elif points.shape[1] > self._num_src_feat: + points = points[:, :self._num_src_feat] + + data_dict = self._dataset.prepare_data( + data_dict={'points': points, 'frame_id': 0}) + data_dict = self._dataset.collate_batch([data_dict]) + if self._cpu: + self._load_data_to_cpu(data_dict) + else: + self._load_data_to_gpu(data_dict) + + with torch.no_grad(): + pred_dicts, _ = self._model.forward(data_dict) + + boxes = pred_dicts[0]['pred_boxes'].cpu().numpy() + scores = pred_dicts[0]['pred_scores'].cpu().numpy() + labels = pred_dicts[0]['pred_labels'].cpu().numpy() + + keep = scores >= self.score_threshold + boxes, scores, labels = boxes[keep], scores[keep], labels[keep] + + # 기하 검증 — 박스 안 실제 점 개수/높이로 헛검출 제거 (points/boxes 동일 좌표계) + if (self.min_points_in_box > 0 or self.min_box_height > 0.0) and len(boxes): + valid = np.array([self._box_is_valid(points, b) for b in boxes], dtype=bool) + boxes, scores, labels = boxes[valid], scores[valid], labels[valid] + + # z_offset 만큼 올려서 추론했으므로 출력 박스 z 는 원래 라이다 좌표계로 되돌림 + if self.z_offset and len(boxes): + boxes = boxes.copy() + boxes[:, 2] -= self.z_offset + + return boxes, scores, labels + + def _box_is_valid(self, points, box): + """박스 내부 점 개수와 z 스프레드로 진짜 객체 여부 판정 (헛검출 컷).""" + cx, cy, cz, dx, dy, dz, yaw = box[:7] + rel = points[:, :3] - np.array([cx, cy, cz], dtype=np.float32) + c, s = np.cos(-yaw), np.sin(-yaw) + xr = rel[:, 0] * c - rel[:, 1] * s + yr = rel[:, 0] * s + rel[:, 1] * c + inside = (np.abs(xr) <= dx / 2) & (np.abs(yr) <= dy / 2) & \ + (np.abs(rel[:, 2]) <= dz / 2) + n = int(inside.sum()) + if n < self.min_points_in_box: + return False + if self.min_box_height > 0.0: + zin = points[inside, 2] + if n == 0 or float(zin.max() - zin.min()) < self.min_box_height: + return False + return True + + def _load_data_to_cpu(self, batch_dict): + """load_data_to_gpu 의 CPU 판: numpy 배열을 .cuda() 대신 cpu float tensor 로. + 라이다 전용 파이프라인이라 camera/image 키는 없음 → voxel/point 계열만 변환.""" + torch = self._torch + skip = ('frame_id', 'metadata', 'calib', 'image_paths', 'ori_shape', + 'img_process_infos') + for key, val in batch_dict.items(): + if not isinstance(val, np.ndarray) or key in skip: + continue + batch_dict[key] = torch.from_numpy(val).float() + + def _patch_cpu(self): + """CPU 런타임용 OpenPCDet CUDA 하드코딩 2종 우회 (device=='cpu' 일 때만, build 전 1회). + + (1) anchor 생성·NMS 모듈이 텐서에 박은 .cuda() → torch.Tensor.cuda 를 no-op(self 반환)로 + shim. forward 경로엔 torch.cuda.*Tensor 직접 alloc 이 없어 이 shim 으로 완결 + (CenterPoint 는 anchor 미사용이라 shim 은 무해, NMS 패치만 실효). + (2) iou3d_nms_utils.nms_gpu(CUDA-only)를 동일결과 CPU greedy NMS 로 교체. 회전 BEV IoU 는 + OpenPCDet 가 같은 확장(iou3d_nms_cuda)에 함께 컴파일해 둔 boxes_bev_iou_cpu(C++ CPU op)를 + 그대로 호출 → GPU 판과 IoU 계산 동일(결과 동등).""" + torch = self._torch + + # (1) .cuda() no-op shim — GPU 없는 클라이언트 전제(애초에 cuda 못 부름)라 전역 안전 + if not getattr(torch.Tensor, '_cpu_cuda_noop', False): + torch.Tensor.cuda = lambda self, *a, **k: self + torch.Tensor._cpu_cuda_noop = True + + # (2) CUDA NMS → CPU NMS monkeypatch (모듈 객체에 1회, getattr 로 호출 시점 해석) + from pcdet.ops.iou3d_nms import iou3d_nms_utils as u + if getattr(u, '_cpu_nms_patched', False): + return + + def nms_cpu(boxes, scores, thresh, pre_maxsize=None, **kwargs): + assert boxes.shape[1] == 7 + order = scores.sort(0, descending=True)[1] + if pre_maxsize is not None: + order = order[:pre_maxsize] + boxes = boxes[order].contiguous() + if boxes.shape[0] == 0: + return order[:0].contiguous(), None + iou = u.boxes_bev_iou_cpu(boxes[:, 0:7], boxes[:, 0:7]) # (N,N) CPU + keep = [] + suppressed = torch.zeros(boxes.shape[0], dtype=torch.bool) + for i in range(boxes.shape[0]): + if suppressed[i]: + continue + keep.append(i) + suppressed |= iou[i] > thresh # 자신 포함 — 이미 append 후라 무해 + keep = torch.as_tensor(keep, dtype=torch.long) + return order[keep].contiguous(), None + + u.nms_gpu = nms_cpu + u.nms_normal_gpu = nms_cpu # objdet 기본 NMS_TYPE=nms_gpu, normal 경로도 crash 방지 + u._cpu_nms_patched = True diff --git a/src/perception_centerpoint_pkg/perception_centerpoint_pkg/centerpoint_node.py b/src/perception_centerpoint_pkg/perception_centerpoint_pkg/centerpoint_node.py new file mode 100755 index 0000000..6df03e6 --- /dev/null +++ b/src/perception_centerpoint_pkg/perception_centerpoint_pkg/centerpoint_node.py @@ -0,0 +1,309 @@ +#!/usr/bin/env python3 + +import os +import math + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import PointCloud2 +import sensor_msgs_py.point_cloud2 as pc2 + +from vision_msgs.msg import Detection3DArray, Detection3D, ObjectHypothesisWithPose +from visualization_msgs.msg import Marker, MarkerArray +from std_msgs.msg import ColorRGBA + +import numpy as np + +from perception_centerpoint_pkg.centerpoint_backend import CenterPointBackend + + +def _yaw_to_quat(yaw): + """z축 yaw(rad) -> (x, y, z, w) quaternion.""" + half = yaw * 0.5 + return 0.0, 0.0, math.sin(half), math.cos(half) + + +def _is_cuda_error(e): + s = str(e).lower() + return 'cuda' in s or 'cudnn' in s or 'out of memory' in s + + +class ObjectDetection(Node): + def __init__(self): + super().__init__('centerpoint_centerpoint') + + # 토픽 (구조적 기본값만 — pcd_crop 과 동일 패턴) + self.declare_parameter('input_topic', '/raw/lidar') + self.declare_parameter('detection_topic', '/perception/detections') + self.declare_parameter('marker_topic', '/perception/detection_markers') + + # 모델 경로/임계값 — 노드에 박지 않는다. config(yaml) 또는 환경변수에서 주입. + self.declare_parameter('model_cfg_path', '') + self.declare_parameter('model_ckpt_path', '') + self.declare_parameter('score_threshold', 0.3) + # device: 'auto' → 런타임 감지(cuda 가용 시 cuda:0, 아니면 cpu). Phase3 dual-runtime. + self.declare_parameter('device', 'auto') + + # 보정 파라미터 (Rule 로 조절) — 센서 도메인을 학습분포에 맞춤. + # z_offset: 입력 z 보정(지면 높이 차). max_range: 이 반경(m) 밖 박스 버림 + # (라이다 밀도 떨어지는 원거리 false positive 제거). 0 이면 비활성. + self.declare_parameter('z_offset', 0.0) + self.declare_parameter('max_range', 0.0) + # 기하 검증 (빈 공간/바닥 헛검출 제거) — 박스 내부 점 개수/높이 최소값. 0 이면 비활성. + self.declare_parameter('min_points_in_box', 0) + self.declare_parameter('min_box_height', 0.0) + + input_topic = self.get_parameter('input_topic').value + detection_topic = self.get_parameter('detection_topic').value + marker_topic = self.get_parameter('marker_topic').value + + # config 가 비면 환경변수로 fallback (하드코딩 금지 룰) + cfg_path = self.get_parameter('model_cfg_path').value or os.environ.get( + 'CENTERPOINT_MODEL_CFG', '') + ckpt_path = self.get_parameter('model_ckpt_path').value or os.environ.get( + 'CENTERPOINT_MODEL_CKPT', '') + score_threshold = self.get_parameter('score_threshold').value + device = self.get_parameter('device').value + z_offset = self.get_parameter('z_offset').value + self.max_range = float(self.get_parameter('max_range').value) + min_points_in_box = self.get_parameter('min_points_in_box').value + min_box_height = self.get_parameter('min_box_height').value + + self.pub_det = self.create_publisher(Detection3DArray, detection_topic, 10) + self.pub_marker = self.create_publisher(MarkerArray, marker_topic, 10) + + # 런타임 CUDA 에러 시 CPU 폴백 1회 가드 + self._cpu_fallback_done = False + self.backend = self._init_backend( + cfg_path, ckpt_path, score_threshold, device, z_offset, + min_points_in_box, min_box_height) + + self.sub = self.create_subscription( + PointCloud2, input_topic, self.callback, 10) + + self.get_logger().info( + f"ObjDet(CenterPoint) started: in[{input_topic}] " + f"det[{detection_topic}] marker[{marker_topic}] " + f"threshold[{score_threshold}] device[{device}] " + f"z_offset[{z_offset}] max_range[{self.max_range}]" + ) + + def _init_backend(self, cfg_path, ckpt_path, score_threshold, device, z_offset, + min_points_in_box, min_box_height): + if not cfg_path or not ckpt_path: + self.get_logger().error( + "model_cfg_path / model_ckpt_path 미지정 " + "(config 또는 CENTERPOINT_MODEL_CFG/CENTERPOINT_MODEL_CKPT 환경변수). " + "추론 비활성 — 입력 구독만 수행한다." + ) + return None + # 런타임 CUDA 폴백 시 ckpt 에서 CPU 백엔드 재생성에 쓰는 공통 인자 + self._backend_kwargs = dict( + cfg_path=cfg_path, ckpt_path=ckpt_path, + score_threshold=score_threshold, + z_offset=z_offset, min_points_in_box=min_points_in_box, + min_box_height=min_box_height, logger=None, + ) + try: + backend = CenterPointBackend(device=device, **self._backend_kwargs) + self.get_logger().info( + f"OpenPCDet 로드 완료: classes={backend.class_names}") + return backend + except Exception as e: # OpenPCDet 미설치 / 가중치 로드 실패 등 + # GPU 초기화 실패(OOM/cuDNN 등)면 CPU 로 1회 폴백 (08-gpu-cpu-runtime N3) + if device != 'cpu' and _is_cuda_error(e): + self.get_logger().warn( + f"[centerpoint] GPU 백엔드 초기화 실패({e}) → CPU 폴백") + try: + backend = CenterPointBackend( + device='cpu', **self._backend_kwargs) + self.get_logger().info( + f"OpenPCDet 로드 완료(CPU 폴백): classes={backend.class_names}") + return backend + except Exception as e2: + self.get_logger().error(f"검출 백엔드 초기화 실패: {e2}") + return None + self.get_logger().error(f"검출 백엔드 초기화 실패: {e}") + return None + + def _read_points(self, msg: PointCloud2): + field_names = {f.name for f in msg.fields} + if 'intensity' in field_names: + pts = pc2.read_points( + msg, field_names=("x", "y", "z", "intensity"), skip_nans=True) + if pts.shape[0] == 0: + return None + return np.stack( + (pts["x"], pts["y"], pts["z"], pts["intensity"]), + axis=-1).astype(np.float32, copy=False) + # intensity 없는 라이다(예: CARLA) — 0 으로 채움 + pts = pc2.read_points( + msg, field_names=("x", "y", "z"), skip_nans=True) + if pts.shape[0] == 0: + return None + xyz = np.stack((pts["x"], pts["y"], pts["z"]), axis=-1).astype( + np.float32, copy=False) + intensity = np.zeros((xyz.shape[0], 1), dtype=np.float32) + return np.concatenate([xyz, intensity], axis=1) + + def callback(self, msg: PointCloud2): + if self.backend is None: + return + + points = self._read_points(msg) + if points is None: + return + + try: + boxes, scores, labels = self.backend.infer(points) + except Exception as e: + if self._cpu_fallback_done or not _is_cuda_error(e): + raise # CUDA 외 예외 / 이미 CPU 폴백 후 → 기존 동작 유지 + self.get_logger().warn( + "[centerpoint] 추론 중 CUDA 에러 → CPU 백엔드 재구성") + self._cpu_fallback_done = True + try: # 깨진 CUDA 컨텍스트 정리 — 실패해도 무해 + torch = self.backend._torch + del self.backend + torch.cuda.empty_cache() + except Exception: + pass + # .to('cpu') 이동 금지 — ckpt 에서 device='cpu' 로 재생성해야 _patch_cpu 발동 + try: + self.backend = CenterPointBackend( + device='cpu', **self._backend_kwargs) + except Exception as e2: + self.get_logger().error(f"CPU 백엔드 재생성 실패: {e2}") + self.backend = None + return + try: + boxes, scores, labels = self.backend.infer(points) + except Exception as e2: + self.get_logger().error(f"CPU 폴백 재시도 실패, 프레임 스킵: {e2}") + return + + # max_range 밖(라이다 밀도 낮은 원거리) 박스 제거 — 박스 중심 수평거리 기준 + if self.max_range > 0.0 and boxes.shape[0] > 0: + r = np.hypot(boxes[:, 0], boxes[:, 1]) + keep = r <= self.max_range + boxes, scores, labels = boxes[keep], scores[keep], labels[keep] + + self.pub_det.publish(self._to_detection_array(msg.header, boxes, scores, labels)) + self.pub_marker.publish(self._to_marker_array(msg.header, boxes, labels)) + + def _to_detection_array(self, header, boxes, scores, labels): + arr = Detection3DArray() + arr.header = header + for box, score, label in zip(boxes, scores, labels): + # nuScenes CenterPoint 은 9-DOF([x,y,z,dx,dy,dz,yaw,vx,vy], 속도 2축 포함), + # KITTI 계열은 7-DOF. 공통으로 앞 7개(pose+size+yaw)만 쓰고 속도는 무시. + x, y, z, dx, dy, dz, yaw = [float(v) for v in box[:7]] + qx, qy, qz, qw = _yaw_to_quat(yaw) + + det = Detection3D() + det.header = header + + hyp = ObjectHypothesisWithPose() + hyp.hypothesis.class_id = str(int(label)) + hyp.hypothesis.score = float(score) + det.results.append(hyp) + + det.bbox.center.position.x = x + det.bbox.center.position.y = y + det.bbox.center.position.z = z + det.bbox.center.orientation.x = qx + det.bbox.center.orientation.y = qy + det.bbox.center.orientation.z = qz + det.bbox.center.orientation.w = qw + det.bbox.size.x = dx + det.bbox.size.y = dy + det.bbox.size.z = dz + + arr.detections.append(det) + return arr + + def _to_marker_array(self, header, boxes, labels): + markers = MarkerArray() + + # 이전 프레임 마커 제거 + clear = Marker() + clear.header = header + clear.action = Marker.DELETEALL + markers.markers.append(clear) + + for i, (box, label) in enumerate(zip(boxes, labels)): + # nuScenes 9-DOF / KITTI 7-DOF 공통: 앞 7개만(속도 vx,vy 무시) + x, y, z, dx, dy, dz, yaw = [float(v) for v in box[:7]] + qx, qy, qz, qw = _yaw_to_quat(yaw) + + m = Marker() + m.header = header + m.ns = 'centerpoint' + m.id = i + m.type = Marker.CUBE + m.action = Marker.ADD + m.pose.position.x = x + m.pose.position.y = y + m.pose.position.z = z + m.pose.orientation.x = qx + m.pose.orientation.y = qy + m.pose.orientation.z = qz + m.pose.orientation.w = qw + m.scale.x = dx + m.scale.y = dy + m.scale.z = dz + m.color = self._label_color(int(label)) + markers.markers.append(m) + + # 박스 위에 클래스명 텍스트 (YOLO 식 라벨) + t = Marker() + t.header = header + t.ns = 'centerpoint_label' + t.id = i + t.type = Marker.TEXT_VIEW_FACING + t.action = Marker.ADD + t.pose.position.x = x + t.pose.position.y = y + t.pose.position.z = z + dz / 2.0 + 0.4 + t.pose.orientation.w = 1.0 + t.scale.z = 0.8 + t.color = self._label_color(int(label)) + t.color.a = 1.0 + t.text = self._label_name(int(label)) + markers.markers.append(t) + + return markers + + @staticmethod + def _label_color(label): + # 클래스별 색 (반투명). 미지정 라벨은 회색. + palette = { + 1: (0.0, 1.0, 0.0), # Car + 2: (1.0, 1.0, 0.0), # Pedestrian + 3: (0.0, 0.6, 1.0), # Cyclist + } + r, g, b = palette.get(label, (0.6, 0.6, 0.6)) + return ColorRGBA(r=r, g=g, b=b, a=0.5) + + @staticmethod + def _label_name(label): + # 클래스 id → 이름 (_label_color 팔레트와 동일 매핑). 미지정은 cls{id}. + names = {1: 'Car', 2: 'Pedestrian', 3: 'Cyclist'} + return names.get(label, f'cls{label}') + + +def main(args=None): + rclpy.init(args=args) + node = ObjectDetection() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception_centerpoint_pkg/resource/perception_centerpoint_pkg b/src/perception_centerpoint_pkg/resource/perception_centerpoint_pkg new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_centerpoint_pkg/setup.cfg b/src/perception_centerpoint_pkg/setup.cfg new file mode 100755 index 0000000..618c720 --- /dev/null +++ b/src/perception_centerpoint_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/perception_centerpoint_pkg +[install] +install_scripts=$base/lib/perception_centerpoint_pkg diff --git a/src/perception_centerpoint_pkg/setup.py b/src/perception_centerpoint_pkg/setup.py new file mode 100755 index 0000000..a03827b --- /dev/null +++ b/src/perception_centerpoint_pkg/setup.py @@ -0,0 +1,38 @@ + +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'perception_centerpoint_pkg' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + # finetuned 용 OpenPCDet custom cfg — ws 루트 cfgs/ 에서 pkg 로 복제 편입 + # (launch 의 모델 자동 resolve 가 share 경로로 참조). + (os.path.join('share', package_name, 'cfgs'), glob('cfgs/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='dgist', + maintainer_email='jang1future@gmail.com', + description='LiDAR 3D object detection (CenterPoint / OpenPCDet) perception module', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'centerpoint = perception_centerpoint_pkg.centerpoint_node:main' + ], + }, +) diff --git a/src/perception_centerpoint_pkg/test/test_copyright.py b/src/perception_centerpoint_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/src/perception_centerpoint_pkg/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/perception_centerpoint_pkg/test/test_flake8.py b/src/perception_centerpoint_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/src/perception_centerpoint_pkg/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/perception_centerpoint_pkg/test/test_pep257.py b/src/perception_centerpoint_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/src/perception_centerpoint_pkg/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/perception_lmnet_pkg/launch/lmnet.launch.py b/src/perception_lmnet_pkg/launch/lmnet.launch.py new file mode 100755 index 0000000..b6fcfe7 --- /dev/null +++ b/src/perception_lmnet_pkg/launch/lmnet.launch.py @@ -0,0 +1,58 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +# 토픽/모델은 launch arg 로 전달(노드는 argparse parse_known_args 로 받음 → --ros-args 무시). +# odom 기본값 /icp_odom: RTABMap icp_odometry (런타임 확인: /odom 무발행). +# +# 모델 resolve 의미론(통합 perception ws 편입): +# - model_source 를 명시 전달하면(finetuned|pretrained) 그대로 노드에 흐른다 — +# 단독 deploy run.sh 가 launch 인자를 명시 전달하는 기존 동작이 그대로 이김(무회귀). +# - 기본값(finetuned)이면 노드 내부 resolve_model.resolve() 가 finetuned 3파일 +# (ckpt+arch_cfg+data_cfg, /perception_models/lmnet/finetuned) 존재 검사 후 +# 없으면 baked pretrained(/opt/lmnet_pretrained + /opt/LMNet config) 로 자동 폴백. +# resolve 구현은 perception_lmnet_pkg/resolve_model.py 단일 출처 — 노드가 rclpy.init() +# "전에" resolve+모델로드까지 책임지므로(DDS 데드락 회피 순서) launch 에 중복 구현하지 않는다. +ARGUMENTS = [ + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('input_topic', default_value='/raw/lidar'), + DeclareLaunchArgument('odom_topic', default_value='/icp_odom'), + DeclareLaunchArgument('moving_topic', default_value='/perception/moving'), + DeclareLaunchArgument('moving_rgb_topic', default_value='/perception/moving_rgb'), + DeclareLaunchArgument('model_source', default_value='finetuned'), # UI 스위치: finetuned|pretrained +] + + +def generate_launch_description(): + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + # ★ 스레드풀 단일화 — 단독 deploy run.sh 의 export 와 동일한 보호를 launch 에도 둔다. + # 통합 perception ws 에선 perception_manager 가 본 launch 를 직접 스폰(run.sh 미경유)하므로 + # 여기서 안 잡으면 SalsaNext 로드 중 PyTorch 멀티스레드(OpenMP/MKL/OpenBLAS) futex + # 데드락 방어가 빠진다. lmnet_infer.py 의 torch.set_num_threads(1) 와 이중 방어 + # (2026-06-08 C5 런타임서 발견). + thread_env = [ + SetEnvironmentVariable('OMP_NUM_THREADS', '1'), + SetEnvironmentVariable('MKL_NUM_THREADS', '1'), + SetEnvironmentVariable('OPENBLAS_NUM_THREADS', '1'), + ] + load_nodes = GroupAction(actions=[ + Node( + package='perception_lmnet_pkg', executable='lmnet', + name='lmnet_node', output='screen', + parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}], + arguments=[ + '--input_topic', LaunchConfiguration('input_topic'), + '--odom_topic', LaunchConfiguration('odom_topic'), + '--moving_topic', LaunchConfiguration('moving_topic'), + '--moving_rgb_topic', LaunchConfiguration('moving_rgb_topic'), + '--model_source', LaunchConfiguration('model_source'), + ], + remappings=remappings, + ) + ]) + ld = LaunchDescription(ARGUMENTS) + for env in thread_env: + ld.add_action(env) + ld.add_action(load_nodes) + return ld diff --git a/src/perception_lmnet_pkg/launch/perception.launch.py b/src/perception_lmnet_pkg/launch/perception.launch.py new file mode 100755 index 0000000..b18a2b6 --- /dev/null +++ b/src/perception_lmnet_pkg/launch/perception.launch.py @@ -0,0 +1,32 @@ +"""perception_manager 공통 진입점 (통합 perception 모듈). + +LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로 +`ros2 launch perception_lmnet_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..` +를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(lmnet.launch.py)로 위임한다. +robot 은 공통 계약상 선언만 하고, 모델 launch 가 선언한 경우에만 소비된다. +""" +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + share = get_package_share_directory('perception_lmnet_pkg') + return LaunchDescription([ + DeclareLaunchArgument('robot', default_value='fr09'), + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('model_source', default_value='finetuned'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(share, 'launch', 'lmnet.launch.py')), + launch_arguments={ + 'robot': LaunchConfiguration('robot'), + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'model_source': LaunchConfiguration('model_source'), + }.items(), + ), + ]) diff --git a/src/perception_lmnet_pkg/package.xml b/src/perception_lmnet_pkg/package.xml new file mode 100755 index 0000000..1d11999 --- /dev/null +++ b/src/perception_lmnet_pkg/package.xml @@ -0,0 +1,24 @@ + + + + perception_lmnet_pkg + 0.0.0 + LiDAR moving object segmentation (LMNet / SalsaNext-MOS) perception module + dgist + TODO: License declaration + + rclpy + sensor_msgs + sensor_msgs_py + nav_msgs + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/__init__.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/lmnet_infer.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/lmnet_infer.py new file mode 100755 index 0000000..3b2982c --- /dev/null +++ b/src/perception_lmnet_pkg/perception_lmnet_pkg/lmnet_infer.py @@ -0,0 +1,97 @@ +"""SalsaNext-MOS 실시간 추론. 모델 정의는 baked /opt/LMNet 에서 import(재구현 X). +preprocess 는 학습 parser 와 동일 레이아웃: [range,x,y,z,remission](정규화) + residual_1..N.""" +import os +import sys + +import numpy as np +import torch +import yaml + +# ★ torch 스레드풀 단일화 — 모델 로드 중 다중스레드 futex 데드락 회피. +# GPU/CPU 고경합 환경(다른 학습이 GPU 점유 중 등)에서 .to(cuda)/load 가 여러 스레드를 띄우다 +# intermittent 하게 데드락(2026-06-08 C5 런타임서 발견). 단일스레드면 경합 자체가 없어 안정. +torch.set_num_threads(1) +try: + torch.set_num_interop_threads(1) +except Exception: + pass + +# baked LMNet 코드 경로 — SalsaNext 네트워크 + laserscan 투영 재사용. +# common/ 은 train/ 아래라 train 도 path 에 넣어야 'from common.laserscan' 가 해석됨 +# (런타임 CWD=/nav2_ws 라 upstream __init__.py 의 상대경로 트릭에 의존 못 함). +_LMNET = "/opt/LMNet/mos_SalsaNext/train/tasks/semantic" +_TRAIN = "/opt/LMNet/mos_SalsaNext/train" +for _p in (_LMNET, _TRAIN): + if _p not in sys.path: + sys.path.insert(0, _p) +from modules.SalsaNext import SalsaNext # noqa: E402 +from common.laserscan import LaserScan # noqa: E402 + + +class LMNetInfer: + def __init__(self, ckpt_path, arch_cfg_path, data_cfg_path, device="auto"): + # device='auto' → 런타임 감지(cuda 가용 시 cuda:0, 아니면 cpu). CPU-first dual-runtime(08 N3/N4). + if device == 'auto': + device = 'cuda:0' if torch.cuda.is_available() else 'cpu' + self.arch = yaml.safe_load(open(arch_cfg_path)) + self.data = yaml.safe_load(open(data_cfg_path)) + self.device = device + sensor = self.arch["dataset"]["sensor"] + self.H = sensor["img_prop"]["height"] + self.W = sensor["img_prop"]["width"] + self.fov_up = sensor["fov_up"] + self.fov_down = sensor["fov_down"] + self.means = np.array(sensor["img_means"], np.float32) # 길이 5 (base: range,x,y,z,remission) + self.stds = np.array(sensor["img_stds"], np.float32) # residual 채널은 정규화 안 함(raw) + self.n_residual = int(self.arch["train"]["n_input_scans"]) + self.nclasses = len(self.data["learning_map_inv"]) + # moving class index — learning_map_inv 에서 moving(원본 251)에 매핑되는 학습 클래스 + self.moving_idx = self._moving_class_index() + + print("[dbg] before SalsaNext()", flush=True) + self.model = SalsaNext(self.nclasses, self.arch) + print("[dbg] after SalsaNext, before torch.load", flush=True) + w = torch.load(ckpt_path, map_location="cpu") + print("[dbg] after torch.load, before load_state_dict", flush=True) + sd = w["state_dict"] if isinstance(w, dict) and "state_dict" in w else w + sd = {k.replace("module.", ""): v for k, v in sd.items()} + self.model.load_state_dict(sd, strict=True) + print("[dbg] after load_state_dict, before .to(cuda)", flush=True) + self.model.to(device).eval() + print("[dbg] after .to(cuda) — DONE", flush=True) + + def _moving_class_index(self): + inv = self.data["learning_map_inv"] # {train_id: orig_id} + for train_id, orig in inv.items(): + if orig in (251, 252): # KITTI-MOS moving 원본 id + return int(train_id) + return self.nclasses - 1 # fallback: 마지막 클래스 + + def _project(self, points): + scan = LaserScan(project=True, H=self.H, W=self.W, + fov_up=self.fov_up, fov_down=self.fov_down) + scan.set_points(points[:, :3], points[:, 3] if points.shape[1] > 3 else None) + return scan # scan.proj_range/proj_xyz/proj_remission/proj_mask/proj_x/proj_y + + def preprocess(self, points, residuals): + """points(M,4) + residuals(list of (H,W)) → (proj_in[1,5+N,H,W], p_x, p_y, mask). + 학습 parser 와 동일: base 5채널만 means/stds 정규화 → residual 은 raw concat → 마지막에 mask.""" + scan = self._project(points) + base = np.concatenate([ + scan.proj_range[None], # (1,H,W) + scan.proj_xyz.transpose(2, 0, 1), # (3,H,W) + scan.proj_remission[None], # (1,H,W) + ], axis=0) # (5,H,W) + base = (base - self.means[:, None, None]) / self.stds[:, None, None] + proj = np.concatenate([base] + [r[None] for r in residuals], axis=0) # (5+N,H,W) residual raw + proj = proj * scan.proj_mask.astype(np.float32)[None] + proj_in = torch.from_numpy(proj.astype(np.float32))[None].to(self.device) + return proj_in, scan.proj_x, scan.proj_y, scan.proj_mask + + @torch.no_grad() + def infer(self, points, residuals): + """반환: per-point moving bool (M,). residuals 없으면(빈) 전부 static.""" + proj_in, p_x, p_y, _ = self.preprocess(points, residuals) + proj_argmax = self.model(proj_in)[0].argmax(dim=0) # (H,W) + labels = proj_argmax[p_y, p_x].cpu().numpy() # per-point train_id + return labels == self.moving_idx diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/lmnet_node.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/lmnet_node.py new file mode 100755 index 0000000..6051bb0 --- /dev/null +++ b/src/perception_lmnet_pkg/perception_lmnet_pkg/lmnet_node.py @@ -0,0 +1,169 @@ +"""LMNet-MOS deploy 노드. /raw/lidar + odom 구독 → N-스캔 버퍼 → residual → SalsaNext 추론 → +/perception/moving(label) + /perception/moving_rgb(빨강) 발행. 상태성 3개(버퍼/odom캐싱/residual).""" +import argparse +import struct + +import numpy as np +import torch +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2, PointField +from nav_msgs.msg import Odometry +import sensor_msgs_py.point_cloud2 as pc2 + +from perception_lmnet_pkg import resolve_model +from perception_lmnet_pkg import pose_util +from perception_lmnet_pkg.scan_buffer import ScanBuffer +from perception_lmnet_pkg.range_projection import RangeProjector +from perception_lmnet_pkg.residual_lib import compute_residual +from perception_lmnet_pkg.lmnet_infer import LMNetInfer + + +def _read_xyzi(msg): + xyz = pc2.read_points_numpy(msg, field_names=("x", "y", "z"), + reshape_organized_cloud=False).astype(np.float32) + try: # intensity 없는 cloud 면 0 으로 채움 (read_points 는 없는 필드에 raise) + inten = pc2.read_points_numpy(msg, field_names=("intensity",), + reshape_organized_cloud=False).astype(np.float32).reshape(-1, 1) + except Exception: + inten = np.zeros((len(xyz), 1), np.float32) + return np.concatenate([xyz, inten], axis=1) + + +def _is_cuda_error(e): + s = str(e).lower() + return 'cuda' in s or 'cudnn' in s or 'out of memory' in s + + +def _build_infer(m, device): + return LMNetInfer(m["ckpt"], m["arch_cfg"], m["data_cfg"], device=device) + + +class LMNetNode(Node): + def __init__(self, args, infer, m): + super().__init__("lmnet_node") + # infer 는 rclpy.init() 전에 미리 로드해 받는다(아래 main 주석 참고) — DDS 스레드와의 데드락 회피. + self.infer = infer + self.m = m # 런타임 CPU 폴백 시 백엔드 재생성용 모델 경로 + self.cpu_fallback = False # 런타임 CPU 폴백 1회 가드 + self.N = self.infer.n_residual + self.proj = RangeProjector(self.infer.H, self.infer.W, self.infer.fov_up, + self.infer.fov_down, args.max_range, args.min_range) + self.buf = ScanBuffer(self.N) + self.last_pose = np.eye(4) + self.have_odom = False + + self.create_subscription(Odometry, args.odom_topic, self._on_odom, 10) + self.create_subscription(PointCloud2, args.input_topic, self._on_lidar, 1) + self.pub_lbl = self.create_publisher(PointCloud2, args.moving_topic, 1) + self.pub_rgb = self.create_publisher(PointCloud2, args.moving_rgb_topic, 1) + + def _on_odom(self, msg): + p, q = msg.pose.pose.position, msg.pose.pose.orientation + self.last_pose = pose_util.pose_to_matrix(p.x, p.y, p.z, q.x, q.y, q.z, q.w) + self.have_odom = True + + def _on_lidar(self, msg): + try: + self._process_lidar(msg) + except Exception as e: + self.get_logger().warn(f"[lmnet] 프레임 처리 실패, 스킵: {e}", throttle_duration_sec=5.0) + + def _process_lidar(self, msg): + pts = _read_xyzi(msg) + if not self.have_odom: + self.get_logger().warn("[lmnet] odom 미수신 — 직전 pose 사용", throttle_duration_sec=5.0) + self.buf.push(pts, self.last_pose.copy()) + if not self.buf.ready(): + self._publish(msg.header, pts, np.zeros(len(pts), bool)) # warmup: all-static + return + cur_pts, cur_pose = self.buf.current() + residuals = [] + for k in range(1, self.N + 1): + past_pts, past_pose = self.buf.past(k) + T = pose_util.relative_pose(cur_pose, past_pose) + residuals.append(compute_residual(cur_pts, past_pts, T, self.proj)) + try: + moving = self.infer.infer(cur_pts, residuals) + except Exception as e: + if self.cpu_fallback or not _is_cuda_error(e): + raise # CUDA 외 예외/폴백 이후엔 기존 스킵 경로(_on_lidar) 유지 + self.get_logger().warn("[lmnet] 추론 중 CUDA 에러 → CPU 백엔드 재구성") + self.cpu_fallback = True + try: # 깨진 CUDA 컨텍스트의 GPU 모델 해제 — 실패해도 무해 + del self.infer.model + torch.cuda.empty_cache() + except Exception: + pass + # ckpt 에서 device='cpu' 로 재생성(.to('cpu') 이동 금지 — 깨진 CUDA 컨텍스트 위험) + self.infer = _build_infer(self.m, "cpu") + moving = self.infer.infer(cur_pts, residuals) # 해당 프레임 1회 재시도 + self._publish(msg.header, cur_pts, moving) + + def _publish(self, header, pts, moving): + # label cloud: x,y,z,label(0/1) + lbl = moving.astype(np.float32) + lbl_pts = np.column_stack([pts[:, :3], lbl]).astype(np.float32) + self.pub_lbl.publish(self._cloud(header, lbl_pts, "label")) + # rgb cloud: moving=빨강(255,0,0), static=회색(120,120,120) + rgb = np.where(moving[:, None], np.array([255, 0, 0], np.uint8), + np.array([120, 120, 120], np.uint8)) + self.pub_rgb.publish(self._rgb_cloud(header, pts[:, :3], rgb)) + + def _cloud(self, header, pts, fourth): + fields = [PointField(name=n, offset=i * 4, datatype=PointField.FLOAT32, count=1) + for i, n in enumerate(["x", "y", "z", fourth])] + return pc2.create_cloud(header, fields, pts) + + def _rgb_cloud(self, header, xyz, rgb): + fields = [PointField(name=n, offset=i * 4, datatype=PointField.FLOAT32, count=1) + for i, n in enumerate(["x", "y", "z"])] + fields.append(PointField(name="rgb", offset=12, datatype=PointField.FLOAT32, count=1)) + data = [] + for (x, y, z), (r, g, b) in zip(xyz, rgb): + packed = struct.unpack("f", struct.pack("I", (int(r) << 16) | (int(g) << 8) | int(b)))[0] + data.append([float(x), float(y), float(z), packed]) + return pc2.create_cloud(header, fields, data) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--input_topic", default="/raw/lidar") + ap.add_argument("--odom_topic", default="/odom") + ap.add_argument("--moving_topic", default="/perception/moving") + ap.add_argument("--moving_rgb_topic", default="/perception/moving_rgb") + ap.add_argument("--max_range", type=float, default=90.0) + ap.add_argument("--min_range", type=float, default=1.0) + ap.add_argument("--model_source", default="finetuned") # UI 스위치: finetuned|pretrained + # device: 'auto' → 런타임 감지(cuda 가용 시 cuda:0, 아니면 cpu). CPU-first dual-runtime(08 N3/N4). + # ★ device 는 노드 파라미터(yaml/launch)로만 흐른다 — execute options/SWRule 로 노출 금지. + ap.add_argument("--device", default="auto") + args, _ = ap.parse_known_args() + # ★ 모델을 rclpy.init() "전에" 로드한다. rclpy.init()/Node 가 DDS(rmw) 수신 스레드를 띄우는데, + # 그 스레드들과 torch 모델로드(CUDA/스레드풀)가 동시에 뜨면 futex 데드락에 걸린다 + # (라이브 파이프라인의 토픽이 많아 DDS 가 바쁠수록 intermittent 하게 멈춤). 모델을 먼저 + # 완전히 올려 CUDA/스레드를 안정화한 뒤 rclpy 를 띄우면 충돌이 사라진다. + m = resolve_model.resolve(model_source=args.model_source) + print(f"[lmnet] model source={m['source']} ckpt={m['ckpt']}", flush=True) + if m["source"] == "pretrained": + print("[lmnet] finetuned 없음 — KITTI pretrained 동작(CARLA 품질 낮음). 먼저 Retrain 권장", flush=True) + try: + infer = _build_infer(m, args.device) + except Exception as e: + if args.device == "cpu" or not _is_cuda_error(e): + raise # CUDA 외 실패는 기존 실패 경로(예외 전파) 유지 + print(f"[lmnet] GPU 백엔드 초기화 실패({e}) → CPU 폴백", flush=True) + infer = _build_infer(m, "cpu") + rclpy.init() + node = LMNetNode(args, infer, m) + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/pose_util.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/pose_util.py new file mode 100755 index 0000000..26fb7f4 --- /dev/null +++ b/src/perception_lmnet_pkg/perception_lmnet_pkg/pose_util.py @@ -0,0 +1,25 @@ +"""ego-pose 유틸 — nav_msgs/Odometry 의 (position, orientation) → 4x4, 상대 pose.""" +import numpy as np + + +def pose_to_matrix(x, y, z, qx, qy, qz, qw): + """위치 + 쿼터니언 → 4x4 homogeneous.""" + n = qx * qx + qy * qy + qz * qz + qw * qw + s = 0.0 if n == 0 else 2.0 / n + xx, yy, zz = qx * qx * s, qy * qy * s, qz * qz * s + xy, xz, yz = qx * qy * s, qx * qz * s, qy * qz * s + wx, wy, wz = qw * qx * s, qw * qy * s, qw * qz * s + R = np.array([ + [1 - (yy + zz), xy - wz, xz + wy], + [xy + wz, 1 - (xx + zz), yz - wx], + [xz - wy, yz + wx, 1 - (xx + yy)], + ], dtype=np.float64) + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = [x, y, z] + return T + + +def relative_pose(curr, past): + """과거점을 현재 프레임으로 옮기는 변환 inv(curr) @ past.""" + return np.linalg.inv(curr) @ past diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/range_projection.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/range_projection.py new file mode 100755 index 0000000..3ddda2e --- /dev/null +++ b/src/perception_lmnet_pkg/perception_lmnet_pkg/range_projection.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 +"""LiDAR spherical range-image projection (모델 무관, 순수 numpy). + +RangeNet++/SalsaNext 계열 표준 투영. 점별 (proj_x, proj_y) 를 보존해 +네트워크 픽셀 출력 → 점별 라벨 복원(un-projection)을 가능케 한다. +""" +from dataclasses import dataclass + +import numpy as np + + +@dataclass +class ProjectionResult: + range_image: np.ndarray # (5, H, W): range(depth), x, y, z, intensity (RangeNet/SalsaNext 순서) + px: np.ndarray # (V,) valid 점들의 열 index + py: np.ndarray # (V,) valid 점들의 행 index + valid: np.ndarray # (N,) bool — range 범위 안 점 마스크 + + +class RangeProjector: + def __init__(self, proj_H, proj_W, fov_up, fov_down, max_range, min_range): + self.H = int(proj_H) + self.W = int(proj_W) + self.fov_up = float(fov_up) / 180.0 * np.pi + self.fov_down = float(fov_down) / 180.0 * np.pi + self.fov = abs(self.fov_up) + abs(self.fov_down) + self.max_range = float(max_range) + self.min_range = float(min_range) + + def project(self, points): + xyz = points[:, :3] + intensity = points[:, 3] if points.shape[1] > 3 else np.zeros(len(points), np.float32) + depth = np.linalg.norm(xyz, axis=1) + valid = (depth > self.min_range) & (depth < self.max_range) + + x, y, z = xyz[:, 0], xyz[:, 1], xyz[:, 2] + yaw = -np.arctan2(y, x) + pitch = np.arcsin(np.clip(z / np.maximum(depth, 1e-6), -1.0, 1.0)) + + proj_x = 0.5 * (yaw / np.pi + 1.0) # [0,1] + proj_y = 1.0 - (pitch + abs(self.fov_down)) / self.fov + proj_x = np.clip(np.floor(proj_x * self.W), 0, self.W - 1).astype(np.int32) + proj_y = np.clip(np.floor(proj_y * self.H), 0, self.H - 1).astype(np.int32) + + img = np.zeros((5, self.H, self.W), dtype=np.float32) + # 가까운 점 우선(겹치면 작은 depth 가 픽셀 차지) — depth 내림차순으로 채움 + order = np.argsort(depth)[::-1] + vi = np.where(valid)[0] + # depth 내림차순 중 valid 만 채워 마지막에 가까운 점이 덮어쓰게 + fill = order[np.isin(order, vi)] + py, px = proj_y[fill], proj_x[fill] + img[0, py, px] = depth[fill] # range (SalsaNext img_means[0]=12.12) + img[1, py, px] = x[fill] + img[2, py, px] = y[fill] + img[3, py, px] = z[fill] + img[4, py, px] = intensity[fill] + + return ProjectionResult( + range_image=img, + px=proj_x[valid], py=proj_y[valid], valid=valid, + ) + + def unproject(self, pixel_labels, proj: ProjectionResult): + """pixel_labels (H,W) → 점별 라벨 (N,). valid 아닌 점은 0(unlabeled).""" + n = proj.valid.shape[0] + labels = np.zeros(n, dtype=np.int64) + labels[proj.valid] = pixel_labels[proj.py, proj.px] + return labels diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/residual_lib.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/residual_lib.py new file mode 100755 index 0000000..0b476f8 --- /dev/null +++ b/src/perception_lmnet_pkg/perception_lmnet_pkg/residual_lib.py @@ -0,0 +1,28 @@ +"""LMNet residual range-image (deploy). retrain gen_residual.compute_residual 와 동일 로직 복제. +학습-배포 일관성을 위해 같은 RangeProjector 공식을 쓴다.""" +import numpy as np + +from perception_lmnet_pkg.range_projection import RangeProjector # noqa: F401 (호출측이 projector 주입) + + +def _range_image(points, projector): + """투영 결과의 range 채널(H,W). range_image[0] = depth.""" + return projector.project(points).range_image[0] + + +def compute_residual(curr_points, past_points, T_past_to_curr, projector): + """현재 프레임 기준 residual(H,W). T_past_to_curr: 과거점을 현재 프레임으로 옮기는 4x4.""" + past_xyz = past_points[:, :3] + hom = np.concatenate([past_xyz, np.ones((len(past_xyz), 1), np.float32)], axis=1) + past_in_curr = (T_past_to_curr @ hom.T).T[:, :3] + if past_points.shape[1] > 3: + past_aligned = np.concatenate( + [past_in_curr.astype(np.float32), past_points[:, 3:4]], axis=1) + else: + past_aligned = past_in_curr.astype(np.float32) + cur_r = _range_image(curr_points, projector) + past_r = _range_image(past_aligned, projector) + valid = (cur_r > 0) & (past_r > 0) + res = np.zeros_like(cur_r) + res[valid] = np.abs(cur_r[valid] - past_r[valid]) / np.maximum(cur_r[valid], 1e-6) + return res diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/resolve_model.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/resolve_model.py new file mode 100755 index 0000000..699d667 --- /dev/null +++ b/src/perception_lmnet_pkg/perception_lmnet_pkg/resolve_model.py @@ -0,0 +1,43 @@ +"""모델 경로 결정 — finetuned(retrain 산출물) 우선, 없거나 불완전하면 baked pretrained fallback. +※ 작은 함수 하나로 분리 — 나중에 명시적 선택(Rule model_source=auto|finetuned|pretrained)을 + 한 분기로 끼우기 위함.""" +import os + +# 가중치 공유 단일 위치(docker/local): 호스트 .caches/config/nav2_config/perception/models/lmnet/* +# → 컨테이너 /perception_models/lmnet/{finetuned,pretrained} (SWRule config_mounts). +DEFAULT_FINETUNED = "/perception_models/lmnet/finetuned" +SHARED_PRE_DIR = "/perception_models/lmnet/pretrained" # 공유 pretrained (Phase 2 채움) +DEFAULT_PRE_CKPT = "/opt/lmnet_pretrained/SalsaNext" # 이미지 baked 안전망 +DEFAULT_PRE_ARCH = "/opt/LMNet/mos_SalsaNext/salsanext_mos.yml" +DEFAULT_PRE_DATA = ("/opt/LMNet/mos_SalsaNext/train/tasks/semantic/" + "config/labels/semantic-kitti-mos.yaml") + + +def _shared_pretrained(): + # 공유 pretrained(.../pretrained: ckpt+arch+data)가 채워져 있으면 baked(/opt) 보다 우선. + ckpt = os.path.join(SHARED_PRE_DIR, "SalsaNext") + arch = os.path.join(SHARED_PRE_DIR, "arch_cfg.yaml") + data = os.path.join(SHARED_PRE_DIR, "data_cfg.yaml") + if os.path.isfile(ckpt) and os.path.isfile(arch) and os.path.isfile(data): + return {"source": "pretrained", "ckpt": ckpt, "arch_cfg": arch, "data_cfg": data} + return None + + +def resolve(model_source="finetuned", + finetuned_dir=DEFAULT_FINETUNED, + pretrained_ckpt=DEFAULT_PRE_CKPT, + pretrained_arch=DEFAULT_PRE_ARCH, + pretrained_data=DEFAULT_PRE_DATA): + # 3단 fallback: finetuned → 공유 pretrained → baked pretrained(/opt 안전망). + # model_source=="pretrained" 면 finetuned 있어도 pretrained 강제(공유 우선, 없으면 baked). + _baked = {"source": "pretrained", "ckpt": pretrained_ckpt, + "arch_cfg": pretrained_arch, "data_cfg": pretrained_data} + _pretrained = _shared_pretrained() or _baked + if model_source == "pretrained": + return _pretrained + ckpt = os.path.join(finetuned_dir, "SalsaNext_valid_best") + arch = os.path.join(finetuned_dir, "arch_cfg.yaml") + data = os.path.join(finetuned_dir, "data_cfg.yaml") + if os.path.isfile(ckpt) and os.path.isfile(arch) and os.path.isfile(data): + return {"source": "finetuned", "ckpt": ckpt, "arch_cfg": arch, "data_cfg": data} + return _pretrained diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/scan_buffer.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/scan_buffer.py new file mode 100755 index 0000000..883d9a7 --- /dev/null +++ b/src/perception_lmnet_pkg/perception_lmnet_pkg/scan_buffer.py @@ -0,0 +1,21 @@ +"""N-스캔 링버퍼 — (points, pose_4x4). residual 생성에 현재+과거 N 프레임 필요.""" +from collections import deque + + +class ScanBuffer: + def __init__(self, num_residual): + self.n = num_residual + self._buf = deque(maxlen=num_residual + 1) # 현재 + 과거 N + + def push(self, points, pose): + self._buf.append((points, pose)) + + def ready(self): + return len(self._buf) >= self.n + 1 + + def current(self): + return self._buf[-1] + + def past(self, k): + """k 프레임 전 (1=직전). ready() 일 때만 호출.""" + return self._buf[-1 - k] diff --git a/src/perception_lmnet_pkg/perception_lmnet_pkg/spawn_npc.py b/src/perception_lmnet_pkg/perception_lmnet_pkg/spawn_npc.py new file mode 100755 index 0000000..d4e286a --- /dev/null +++ b/src/perception_lmnet_pkg/perception_lmnet_pkg/spawn_npc.py @@ -0,0 +1,129 @@ +"""deploy 검증용 — 이미 떠 있는 CARLA 에 connect(load_world ❌) 후 ego 근처에 차+보행자 스폰. +MOS 가 빨갛게 칠하는지 RViz 비교용. Ctrl-C 시 스폰 actor 전부 cleanup. + +★ sync 모드 대응: 파이프라인(브리지)이 world 를 tick 한다. 우리가 만든 TM 은 tick 되지 않아 + autopilot 차가 안 움직인다. 그래서 autopilot/AI-controller 대신 enable_constant_velocity(차) + + WalkerControl(보행자)를 쓴다 — 둘 다 브리지의 world tick 으로 전진하므로 TM 불필요. +★ ego 근처 스폰: 라이다 범위 안에 있어야 MOS 가 본다. 맵 전역 랜덤 spawn 은 대부분 사거리 밖. +""" +import argparse +import math +import random +import time + +import carla + + +def _find_ego(world, tries=20): + """hero/ego(=vehicle.custom.*) 위치 찾기. sync 보조클라 snapshot 이 불안정해 재시도. + ★ wait_for_tick 으로 매 시도마다 브리지 tick 을 받아 신선한 스냅샷 확보 — sleep 만 하면 + passive 클라가 빈 스냅샷을 반복 수신해 ego 를 못 찾고 맵 랜덤 위치에 스폰됨(범위 밖).""" + for _ in range(tries): + try: + world.wait_for_tick(2.0) + except Exception: + time.sleep(0.3) + for a in world.get_actors().filter("vehicle.*"): + if a.attributes.get("role_name") in ("hero", "ego") or a.type_id.startswith("vehicle.custom."): + return a + return None + + +def _near(loc, ref, radius): + return math.hypot(loc.x - ref.x, loc.y - ref.y) <= radius + + +def _orbit_dir(loc, center): + """ego 중심 접선 방향(반시계) — 보행자가 ego 주위를 돌며 라이다 범위에 머물게.""" + rx, ry = loc.x - center.x, loc.y - center.y + n = math.hypot(rx, ry) or 1.0 + return carla.Vector3D(x=-ry / n, y=rx / n, z=0.0) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--host", default="localhost") + ap.add_argument("--port", type=int, default=2000) + ap.add_argument("--num-vehicles", type=int, default=15) + ap.add_argument("--num-walkers", type=int, default=10) + ap.add_argument("--radius", type=float, default=45.0, help="ego 중심 스폰 반경(m, 라이다 범위 내)") + ap.add_argument("--speed", type=float, default=5.0, help="차량 등속(m/s)") + args = ap.parse_args() + + client = carla.Client(args.host, args.port) + client.set_timeout(10.0) + world = client.get_world() # ❌ load_world 안 함 — 실행 중 맵 유지 + bp = world.get_blueprint_library() + spawned = [] + + ego = _find_ego(world) + if ego is None: + print("[spawn_npc] 경고: ego 를 못 찾음 — 맵 전역에 스폰(라이다 범위 밖일 수 있음)") + center = carla.Location(0, 0, 0) + else: + center = ego.get_location() + print(f"[spawn_npc] ego={ego.type_id} @ ({center.x:.1f},{center.y:.1f}) 반경 {args.radius}m 내 스폰") + + try: + # ── 차량: ego 근처 spawn point 에 스폰 후 enable_constant_velocity (브리지 tick 으로 전진) ── + vbps = [b for b in bp.filter("vehicle.*") if not b.id.startswith("vehicle.custom.")] + sps = [sp for sp in world.get_map().get_spawn_points() + if ego is None or _near(sp.location, center, args.radius)] + random.shuffle(sps) + nveh = 0 + for sp in sps: + if nveh >= args.num_vehicles: + break + v = world.try_spawn_actor(random.choice(vbps), sp) + if v: + # 차량 local +x = 전방. 등속으로 전진 → static 세계 대비 moving. + v.enable_constant_velocity(carla.Vector3D(x=args.speed, y=0.0, z=0.0)) + spawned.append(v) + nveh += 1 + + # ── 보행자: ego 주변 링 좌표에 직접 스폰 (nav mesh 불필요 — WalkerControl 로 직접 보행) ── + # get_random_location_from_navigation 은 nav mesh 없으면 None 만 → 못 씀. + wbps = bp.filter("walker.pedestrian.*") + ring = [] + for r in (6, 9, 12, 16, 20): + for deg in range(0, 360, 30): + a = math.radians(deg) + ring.append(carla.Location(center.x + r * math.cos(a), + center.y + r * math.sin(a), + center.z + 1.0)) # +1m: 지면 위로(낙하 안착) + random.shuffle(ring) + nwlk = 0 + for loc in ring: + if nwlk >= args.num_walkers: + break + w = world.try_spawn_actor(random.choice(wbps), carla.Transform(loc)) + if not w: + continue + w.apply_control(carla.WalkerControl(direction=_orbit_dir(loc, center), speed=1.3)) + spawned.append(w) + nwlk += 1 + + print(f"[spawn_npc] 차 {nveh} + 보행자 {nwlk} 스폰(ego 주위 orbit). Ctrl-C 로 종료/정리") + # 매 주기 현재 위치 기준 접선방향 재지정 → ego 주위를 계속 돌게(라이다 범위 유지) + while True: + for a in spawned: + if a.type_id.startswith("walker.pedestrian"): + try: + a.apply_control(carla.WalkerControl( + direction=_orbit_dir(a.get_location(), center), speed=1.3)) + except Exception: + pass + time.sleep(1.5) + except KeyboardInterrupt: + pass + finally: + for a in spawned: + try: + a.destroy() + except Exception: + pass + print("[spawn_npc] cleanup 완료") + + +if __name__ == "__main__": + main() diff --git a/src/perception_lmnet_pkg/resource/perception_lmnet_pkg b/src/perception_lmnet_pkg/resource/perception_lmnet_pkg new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_lmnet_pkg/setup.cfg b/src/perception_lmnet_pkg/setup.cfg new file mode 100755 index 0000000..be6c847 --- /dev/null +++ b/src/perception_lmnet_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/perception_lmnet_pkg +[install] +install_scripts=$base/lib/perception_lmnet_pkg diff --git a/src/perception_lmnet_pkg/setup.py b/src/perception_lmnet_pkg/setup.py new file mode 100755 index 0000000..b004020 --- /dev/null +++ b/src/perception_lmnet_pkg/setup.py @@ -0,0 +1,35 @@ + +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'perception_lmnet_pkg' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='dgist', + maintainer_email='jang1future@gmail.com', + description='LiDAR moving object segmentation (LMNet / SalsaNext-MOS) perception module', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'lmnet = perception_lmnet_pkg.lmnet_node:main' + ], + }, +) diff --git a/src/perception_lmnet_pkg/test/test_copyright.py b/src/perception_lmnet_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/src/perception_lmnet_pkg/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/perception_lmnet_pkg/test/test_flake8.py b/src/perception_lmnet_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/src/perception_lmnet_pkg/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/perception_lmnet_pkg/test/test_pep257.py b/src/perception_lmnet_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/src/perception_lmnet_pkg/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/perception_lmnet_pkg/test/test_pose_util.py b/src/perception_lmnet_pkg/test/test_pose_util.py new file mode 100755 index 0000000..bc3d00d --- /dev/null +++ b/src/perception_lmnet_pkg/test/test_pose_util.py @@ -0,0 +1,22 @@ +import numpy as np +from perception_lmnet_pkg import pose_util + + +def test_identity_quaternion_pure_translation(): + T = pose_util.pose_to_matrix(x=1.0, y=2.0, z=3.0, qx=0, qy=0, qz=0, qw=1) + assert np.allclose(T[:3, 3], [1, 2, 3]) + assert np.allclose(T[:3, :3], np.eye(3)) + + +def test_90deg_yaw(): + s = np.sqrt(0.5) + T = pose_util.pose_to_matrix(0, 0, 0, 0, 0, s, s) + assert np.allclose(T[:3, :3] @ np.array([1.0, 0, 0]), [0, 1, 0], atol=1e-6) + + +def test_relative_pose_past_to_curr(): + A = pose_util.pose_to_matrix(0, 0, 0, 0, 0, 0, 1) + B = pose_util.pose_to_matrix(5, 0, 0, 0, 0, 0, 1) + T = pose_util.relative_pose(curr=B, past=A) + p = T @ np.array([0.0, 0, 0, 1]) + assert np.allclose(p[:3], [-5, 0, 0]) diff --git a/src/perception_lmnet_pkg/test/test_residual_lib.py b/src/perception_lmnet_pkg/test/test_residual_lib.py new file mode 100755 index 0000000..711c627 --- /dev/null +++ b/src/perception_lmnet_pkg/test/test_residual_lib.py @@ -0,0 +1,22 @@ +import numpy as np +from perception_lmnet_pkg import residual_lib +from perception_lmnet_pkg.range_projection import RangeProjector + + +def _proj(): + return RangeProjector(proj_H=32, proj_W=180, fov_up=21.2, fov_down=-21.2, + max_range=90.0, min_range=1.0) + + +def test_identical_scan_zero_motion_zero_residual(): + pts = np.random.RandomState(0).uniform(-20, 20, (500, 4)).astype(np.float32) + res = residual_lib.compute_residual(pts, pts, np.eye(4), _proj()) + assert res.shape == (32, 180) + assert np.nanmax(np.abs(res)) < 1e-3 + + +def test_moved_point_makes_nonzero_residual(): + pts = np.array([[10.0, 0, 0, 1]] * 50, np.float32) + T = np.eye(4); T[0, 3] = 5.0 + res = residual_lib.compute_residual(pts, pts, T, _proj()) + assert np.nanmax(np.abs(res)) > 0.0 diff --git a/src/perception_lmnet_pkg/test/test_resolve_model.py b/src/perception_lmnet_pkg/test/test_resolve_model.py new file mode 100755 index 0000000..03e7941 --- /dev/null +++ b/src/perception_lmnet_pkg/test/test_resolve_model.py @@ -0,0 +1,89 @@ +import os +from perception_lmnet_pkg import resolve_model + + +def test_finetuned_when_present(tmp_path): + ft = tmp_path / "lmnet_finetuned" + ft.mkdir() + (ft / "SalsaNext_valid_best").write_bytes(b"x") + (ft / "arch_cfg.yaml").write_text("a: 1") + (ft / "data_cfg.yaml").write_text("b: 1") + r = resolve_model.resolve(finetuned_dir=str(ft), + pretrained_ckpt="/opt/lmnet_pretrained/SalsaNext", + pretrained_arch="/opt/x/salsanext_mos.yml", + pretrained_data="/opt/x/semantic-kitti-mos.yaml") + assert r["source"] == "finetuned" + assert r["ckpt"] == str(ft / "SalsaNext_valid_best") + assert r["arch_cfg"] == str(ft / "arch_cfg.yaml") + + +def test_pretrained_fallback_when_absent(tmp_path): + ft = tmp_path / "lmnet_finetuned" + r = resolve_model.resolve(finetuned_dir=str(ft), + pretrained_ckpt="/opt/lmnet_pretrained/SalsaNext", + pretrained_arch="/opt/x/salsanext_mos.yml", + pretrained_data="/opt/x/semantic-kitti-mos.yaml") + assert r["source"] == "pretrained" + assert r["ckpt"] == "/opt/lmnet_pretrained/SalsaNext" + assert r["arch_cfg"] == "/opt/x/salsanext_mos.yml" + + +def test_partial_finetuned_falls_back(tmp_path): + ft = tmp_path / "lmnet_finetuned"; ft.mkdir() + (ft / "SalsaNext_valid_best").write_bytes(b"x") + r = resolve_model.resolve(finetuned_dir=str(ft), + pretrained_ckpt="/opt/lmnet_pretrained/SalsaNext", + pretrained_arch="/opt/x/salsanext_mos.yml", + pretrained_data="/opt/x/semantic-kitti-mos.yaml") + assert r["source"] == "pretrained" + + +def _make_finetuned(tmp_path): + ft = tmp_path / "lmnet_finetuned" + ft.mkdir() + (ft / "SalsaNext_valid_best").write_bytes(b"x") + (ft / "arch_cfg.yaml").write_text("a: 1") + (ft / "data_cfg.yaml").write_text("b: 1") + return ft + + +def test_model_source_pretrained_forces_baked_even_if_finetuned_present(tmp_path): + # 스위치=pretrained 면 finetuned 3파일 다 있어도 무시하고 baked pretrained 강제 + ft = _make_finetuned(tmp_path) + r = resolve_model.resolve(model_source="pretrained", + finetuned_dir=str(ft), + pretrained_ckpt="/opt/lmnet_pretrained/SalsaNext", + pretrained_arch="/opt/x/salsanext_mos.yml", + pretrained_data="/opt/x/semantic-kitti-mos.yaml") + assert r["source"] == "pretrained" + assert r["ckpt"] == "/opt/lmnet_pretrained/SalsaNext" + + +def test_model_source_finetuned_uses_finetuned_when_present(tmp_path): + ft = _make_finetuned(tmp_path) + r = resolve_model.resolve(model_source="finetuned", + finetuned_dir=str(ft), + pretrained_ckpt="/opt/lmnet_pretrained/SalsaNext", + pretrained_arch="/opt/x/salsanext_mos.yml", + pretrained_data="/opt/x/semantic-kitti-mos.yaml") + assert r["source"] == "finetuned" + assert r["ckpt"] == str(ft / "SalsaNext_valid_best") + + +def test_model_source_finetuned_silent_fallback_when_absent(tmp_path): + # 스위치=finetuned 인데 학습본 없으면 조용히 pretrained fallback + ft = tmp_path / "lmnet_finetuned" + r = resolve_model.resolve(model_source="finetuned", + finetuned_dir=str(ft), + pretrained_ckpt="/opt/lmnet_pretrained/SalsaNext", + pretrained_arch="/opt/x/salsanext_mos.yml", + pretrained_data="/opt/x/semantic-kitti-mos.yaml") + assert r["source"] == "pretrained" + + +def test_resolve_no_args_backward_compat(tmp_path): + # 인자 없이 resolve() — model_source default 로 기존 동작 유지(하위호환). + # default finetuned 경로는 실 환경에 없으므로 pretrained 로 떨어진다. + r = resolve_model.resolve() + assert r["source"] == "pretrained" + assert r["ckpt"] == resolve_model.DEFAULT_PRE_CKPT diff --git a/src/perception_lmnet_pkg/test/test_scan_buffer.py b/src/perception_lmnet_pkg/test/test_scan_buffer.py new file mode 100755 index 0000000..88ad31d --- /dev/null +++ b/src/perception_lmnet_pkg/test/test_scan_buffer.py @@ -0,0 +1,37 @@ +import numpy as np +from perception_lmnet_pkg.scan_buffer import ScanBuffer + + +def _pts(v): + return np.full((3, 4), float(v), np.float32) + + +def test_not_ready_until_n_plus_one(): + buf = ScanBuffer(num_residual=2) + buf.push(_pts(0), np.eye(4)) + assert not buf.ready() + buf.push(_pts(1), np.eye(4)) + assert not buf.ready() + buf.push(_pts(2), np.eye(4)) + assert buf.ready() + + +def test_past_k_returns_kth_back(): + buf = ScanBuffer(num_residual=2) + for i in range(3): + buf.push(_pts(i), np.eye(4)) + cur_pts, cur_pose = buf.current() + assert cur_pts[0, 0] == 2.0 + p1_pts, _ = buf.past(1) + p2_pts, _ = buf.past(2) + assert p1_pts[0, 0] == 1.0 + assert p2_pts[0, 0] == 0.0 + + +def test_maxlen_evicts_old(): + buf = ScanBuffer(num_residual=1) + for i in range(5): + buf.push(_pts(i), np.eye(4)) + cur, _ = buf.current() + prev, _ = buf.past(1) + assert cur[0, 0] == 4.0 and prev[0, 0] == 3.0 diff --git a/src/perception_motionbev_pkg/config/motionbev.yaml b/src/perception_motionbev_pkg/config/motionbev.yaml new file mode 100755 index 0000000..f54a1e3 --- /dev/null +++ b/src/perception_motionbev_pkg/config/motionbev.yaml @@ -0,0 +1,11 @@ +# MotionBEV deploy 파라미터 기본값 (런타임은 launch arg 가 우선). +# 실제 모델 격자/means/stds 는 finetuned arch_cfg.yaml(또는 baked MotionBEV-semantickitti.yaml)에서 +# 읽는다 — 여기엔 토픽/소스 같은 노드 운영값만 둔다 (하드코딩 격자 금지, 단일소스=arch_cfg). +motionbev_node: + ros__parameters: + use_sim_time: true + input_topic: /raw/lidar + odom_topic: /icp_odom + moving_topic: /perception/moving + moving_rgb_topic: /perception/moving_rgb + model_source: finetuned # finetuned | pretrained diff --git a/src/perception_motionbev_pkg/launch/motionbev.launch.py b/src/perception_motionbev_pkg/launch/motionbev.launch.py new file mode 100755 index 0000000..a2a6d9f --- /dev/null +++ b/src/perception_motionbev_pkg/launch/motionbev.launch.py @@ -0,0 +1,59 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +# 모델 경로 단일소스 = 노드와 같은 pkg 모듈(resolve_model) — launch 에 경로 하드코딩 금지. +from perception_motionbev_pkg import resolve_model + +# 토픽/모델은 launch arg 로 전달(노드는 argparse parse_known_args 로 받음 → --ros-args 무시). +# odom 기본값 /icp_odom: RTABMap icp_odometry (런타임 확인: /odom 무발행). +MODEL_SOURCE_DEFAULT = 'finetuned' +ARGUMENTS = [ + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('input_topic', default_value='/raw/lidar'), + DeclareLaunchArgument('odom_topic', default_value='/icp_odom'), + DeclareLaunchArgument('moving_topic', default_value='/perception/moving'), + DeclareLaunchArgument('moving_rgb_topic', default_value='/perception/moving_rgb'), + DeclareLaunchArgument('model_source', default_value=MODEL_SOURCE_DEFAULT), # UI 스위치: finetuned|pretrained +] + + +def _resolve_and_load(context): + """모델 resolve (OpaqueFunction). MOS 계열은 모델 경로를 노드(resolve_model.resolve)가 + 정하므로 launch 는 model_source 전달만 확정한다 — lmnet 과 동일 원칙. + - model_source 를 명시(선언 기본값과 다름)하면 그대로 통과 — 단독 deploy run.sh 가 + launch arg 로 명시 전달하는 기존 동작 무회귀. + - 기본값/빈값이면 finetuned 3파일(MotionBEV_valid_best/arch_cfg.yaml/data_cfg.yaml) + 존재 검사로 자동 resolve — 노드 fallback 과 같은 결과를 launch 시점에 확정/로그.""" + requested = LaunchConfiguration('model_source').perform(context) + if requested and requested != MODEL_SOURCE_DEFAULT: + model_source = requested # 명시 인자가 이긴다 (무회귀) + else: + model_source = resolve_model.resolve(model_source=MODEL_SOURCE_DEFAULT)['source'] + print(f"[motionbev.launch] model_source={model_source} (요청='{requested or MODEL_SOURCE_DEFAULT}')", + flush=True) + + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + load_nodes = GroupAction(actions=[ + Node( + package='perception_motionbev_pkg', executable='motionbev', + name='motionbev_node', output='screen', + parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}], + arguments=[ + '--input_topic', LaunchConfiguration('input_topic'), + '--odom_topic', LaunchConfiguration('odom_topic'), + '--moving_topic', LaunchConfiguration('moving_topic'), + '--moving_rgb_topic', LaunchConfiguration('moving_rgb_topic'), + '--model_source', model_source, + ], + remappings=remappings, + ) + ]) + return [load_nodes] + + +def generate_launch_description(): + ld = LaunchDescription(ARGUMENTS) + ld.add_action(OpaqueFunction(function=_resolve_and_load)) + return ld diff --git a/src/perception_motionbev_pkg/launch/perception.launch.py b/src/perception_motionbev_pkg/launch/perception.launch.py new file mode 100755 index 0000000..ef7d2a5 --- /dev/null +++ b/src/perception_motionbev_pkg/launch/perception.launch.py @@ -0,0 +1,32 @@ +"""perception_manager 공통 진입점 (통합 perception 모듈). + +LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로 +`ros2 launch perception_motionbev_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..` +를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(motionbev.launch.py)로 위임한다. +robot 은 공통 계약상 선언만 하고, 모델 launch 가 선언한 경우에만 소비된다. +""" +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + share = get_package_share_directory('perception_motionbev_pkg') + return LaunchDescription([ + DeclareLaunchArgument('robot', default_value='fr09'), + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('model_source', default_value='finetuned'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(share, 'launch', 'motionbev.launch.py')), + launch_arguments={ + 'robot': LaunchConfiguration('robot'), + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'model_source': LaunchConfiguration('model_source'), + }.items(), + ), + ]) diff --git a/src/perception_motionbev_pkg/package.xml b/src/perception_motionbev_pkg/package.xml new file mode 100755 index 0000000..0d082f7 --- /dev/null +++ b/src/perception_motionbev_pkg/package.xml @@ -0,0 +1,24 @@ + + + + perception_motionbev_pkg + 0.0.0 + LiDAR moving object segmentation (MotionBEV / Polar-BEV) perception module + dgist + TODO: License declaration + + rclpy + sensor_msgs + sensor_msgs_py + nav_msgs + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/__init__.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/motionbev_infer.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/motionbev_infer.py new file mode 100755 index 0000000..731b6f7 --- /dev/null +++ b/src/perception_motionbev_pkg/perception_motionbev_pkg/motionbev_infer.py @@ -0,0 +1,158 @@ +"""MotionBEV 실시간 추론. 모델 정의는 baked /opt/MotionBEV 에서 import(재구현 X). +입력 = polar-BEV base 채널([occupancy,max_z,intensity] 정규화) + polar residual_1..N.""" +import sys + +import numpy as np +import torch +import yaml + +from perception_motionbev_pkg.polar_projection import PolarBEVProjector + +# ★ torch 스레드풀 단일화 — 모델 로드 중 다중스레드 futex 데드락 회피(LMNet 2026-06-08 케이스 동일). +torch.set_num_threads(1) +try: + torch.set_num_interop_threads(1) +except Exception: + pass + +# baked MotionBEV 코드 경로 — Polar-BEV 백본 재사용. +_MOTIONBEV = "/opt/MotionBEV" +if _MOTIONBEV not in sys.path: + sys.path.insert(0, _MOTIONBEV) +# 업스트림(github.com/xieKKKi/MotionBEV) 실제 모듈 레이아웃: +# 모델 = network.ptBEVnet.ptBEVnet (point-feature in → polar-BEV voxel logits out), +# BEV 백본 = network.CA_BEV_Unet.CA_Unet (co-attention) 또는 network.A_BEV_Unet.BEV_Unet. +# ※ 단일 `MotionBEV` 클래스는 존재하지 않는다 (RA-L'23 코드는 ptBEVnet 래퍼 구조). +from network.ptBEVnet import ptBEVnet # noqa: E402 +from network.CA_BEV_Unet import CA_Unet # noqa: E402 +from network.A_BEV_Unet import BEV_Unet # noqa: E402 + + +class MotionBEVInfer: + def __init__(self, ckpt_path, arch_cfg_path, data_cfg_path, device="auto"): + self.arch = yaml.safe_load(open(arch_cfg_path)) + self.data = yaml.safe_load(open(data_cfg_path)) + # device='auto' → 런타임 감지: cuda 가용 시 cuda, 아니면 cpu (08-gpu-cpu-runtime N4) + if device == "auto": + device = "cuda" if torch.cuda.is_available() else "cpu" + self.device = device + grid = self._resolve_grid() + self.proj = PolarBEVProjector(grid["n_range"], grid["n_angle"], + grid["max_range"], grid["min_range"], + grid.get("z_min", -3.0), grid.get("z_max", 3.0), + n_height=grid.get("n_height", 32)) + self.R, self.A, self.H = self.proj.R, self.proj.A, self.proj.H + self.n_residual = self._resolve_n_residual() + # MOS 2-class: learning_map_inv {train_id: orig}. 업스트림 semantic-kitti-MOS.yaml = + # {0: 9(static), 1: 251(moving)} → moving train_id=1, nclass=2. + inv = self.data.get("moving_learning_map_inv", self.data.get("learning_map_inv", {0: 9, 1: 251})) + self.nclasses = len(inv) + self.moving_idx = self._moving_class_index(inv) + + print("[dbg] before ptBEVnet()", flush=True) + self.model = self._build_model() + print("[dbg] after ptBEVnet, before torch.load", flush=True) + w = torch.load(ckpt_path, map_location="cpu") + print("[dbg] after torch.load, before load_state_dict", flush=True) + sd = w["state_dict"] if isinstance(w, dict) and "state_dict" in w else w + # DataParallel prefix 만 strip — naive replace("module.") 는 attention_module. 같은 + # 서브모듈 이름 속 "module." 까지 깎아 키를 망가뜨린다 (kitti-val-76.54 로드 실패 원인). + sd = {(k[len("module."):] if k.startswith("module.") else k): v for k, v in sd.items()} + self.model.load_state_dict(sd, strict=True) + print("[dbg] after load_state_dict, before .to(cuda)", flush=True) + self.model.to(device).eval() + print("[dbg] after .to(cuda) — DONE", flush=True) + + # 업스트림 polar_dataset(spherical_dataset) 의 학습 volume — max=[50.15,π,2], min=[1.85,-π,-4]. + # 배포/finetuned arch_cfg 가 dataset.grid 를 명시하면 그걸 우선한다(하드코딩 회피). + _UP_RANGE_MAX, _UP_RANGE_MIN = 50.15, 1.85 + _UP_Z_MAX, _UP_Z_MIN = 2.0, -4.0 + + def _resolve_grid(self): + """polar BEV 격자 파라미터 결정. + 1) deploy/finetuned arch_cfg 가 dataset.grid 블록을 주면 그대로 사용. + 2) 업스트림 schema(model_params.grid_size=[n_range,n_angle,n_height])면 거기서 R/A 추출 + + 업스트림 polar volume 기본값으로 range/z 보충.""" + if "dataset" in self.arch and "grid" in self.arch["dataset"]: + return self.arch["dataset"]["grid"] + gs = list(self.arch["model_params"]["grid_size"]) # [n_range, n_angle, n_height] + return {"n_range": int(gs[0]), "n_angle": int(gs[1]), "n_height": int(gs[2]), + "max_range": self._UP_RANGE_MAX, "min_range": self._UP_RANGE_MIN, + "z_min": self._UP_Z_MIN, "z_max": self._UP_Z_MAX} + + def _resolve_n_residual(self): + """residual(과거 scan) 개수. deploy schema=train.n_input_scans, 업스트림=data_loader.residual.""" + tr = self.arch.get("train", {}) + if "n_input_scans" in tr: + return int(tr["n_input_scans"]) + return int(self.arch.get("data_loader", {}).get("residual", 8)) + + def _build_model(self): + """업스트림 ptBEVnet 구조를 arch_cfg 로부터 구성. 하드코딩 금지 — + grid_size/fea_dim/init_dim/dropout/co-attention 은 config 에서 읽는다. + 업스트림 schema(model_params) 와 deploy/finetuned schema 양쪽 허용.""" + mp = self.arch.get("model_params", self.arch) # 업스트림=model_params, deploy=top-level fallback + n_residual = self.n_residual + grid_size = list(mp["grid_size"]) # [n_range, n_angle, n_height] + n_height = int(grid_size[2]) + fea_dim = int(mp["fea_dim"]) + init_dim = int(mp["ppmodel_init_dim"]) + use_norm = bool(mp.get("use_norm", True)) + dropout = float(mp.get("dropout", 0.0)) + use_co_attention = bool(mp.get("use_co_attention", True)) + # circular_padding: 업스트림 infer_SemanticKITTI 는 polar dataset 이면 True (방위각이 순환). + # pretrained 가중치(double_conv_circular: conv1/conv2)가 이 분기로만 로드된다. + # 하드코딩 회피 — data_loader.dataset_type 에서 읽고, 기본 polar→True. + dataset_type = self.arch.get("data_loader", {}).get("dataset_type", "polar") + circular_padding = (dataset_type == "polar") + bev_kwargs = dict(n_class=self.nclasses, n_height=n_height, + residual=n_residual, input_batch_norm=use_norm, + dropout=dropout, circular_padding=circular_padding) + bev_model = CA_Unet(**bev_kwargs) if use_co_attention else BEV_Unet(**bev_kwargs) + return ptBEVnet(bev_model, grid_size=grid_size, fea_dim=fea_dim, + ppmodel_init_dim=init_dim, kernal_size=1, fea_compre=n_height) + + @staticmethod + def _moving_class_index(inv): + for train_id, orig in inv.items(): + if int(orig) in (251, 252): # KITTI-MOS moving 원본 id + return int(train_id) + return max(int(k) for k in inv) # fallback: 마지막 클래스(moving 관례) + + def preprocess(self, points, residuals): + """points(M,>=3[,4]) + residuals(list of N×(R,A) cell residual) + → (pt_fea_list, grid_ind_list, PolarPointFeature). + + 업스트림 ptBEVnet 은 **점별 피처 리스트**(BEV 이미지 텐서가 아니라)를 받는다: + forward(pt_fea_list, grid_ind_list, device). + pt_fea[v] = [appearance 9차원] ++ [residual N차원]. + - appearance = spherical_dataset 폴라 피처(bias·polar·xy·intensity). + - residual 컬럼 k = compute_residual 가 만든 k번째 (R,A) cell residual 을 + 각 점의 (rho,theta) bin 으로 gather 한 값(업스트림 diff_map[point]←residual[proj] 대응). + grid_ind[v] = [rho_idx, theta_idx, z_idx] (3D polar voxel index). + """ + pf = self.proj.point_features(points) # valid 점만 포함 + ri, ai = pf.grid_ind[:, 0], pf.grid_ind[:, 1] # 점별 (rho,theta) bin + if residuals: + res_cols = np.stack([r[ri, ai] for r in residuals], axis=1) # (V, N) + else: # residual 없음(warmup) → 0 + res_cols = np.zeros((pf.pt_fea.shape[0], self.n_residual), np.float32) + fea = np.concatenate([pf.pt_fea, res_cols.astype(np.float32)], axis=1) # (V, 9+N) + + pt_fea_list = [torch.from_numpy(fea).type(torch.FloatTensor).to(self.device)] + grid_ind_list = [torch.from_numpy(pf.grid_ind).to(self.device)] + return pt_fea_list, grid_ind_list, pf + + @torch.no_grad() + def infer(self, points, residuals): + """반환: per-point moving bool (M,). residuals 없으면(warmup) 전부 static.""" + pt_fea_list, grid_ind_list, pf = self.preprocess(points, residuals) + # ptBEVnet → (voxel_logits, None). voxel_logits: [B, nclass, R, A, H]. + voxel_logits, _ = self.model(pt_fea_list, grid_ind_list, self.device) + pred = voxel_logits.argmax(dim=1)[0].cpu().numpy() # (R, A, H) train_id + gi = pf.grid_ind + pt_labels = pred[gi[:, 0], gi[:, 1], gi[:, 2]] # valid 점별 train_id + + moving = np.zeros(pf.valid.shape[0], dtype=bool) # 전체 M 길이 + moving[pf.valid] = (pt_labels == self.moving_idx) + return moving diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/motionbev_node.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/motionbev_node.py new file mode 100755 index 0000000..f0dde5b --- /dev/null +++ b/src/perception_motionbev_pkg/perception_motionbev_pkg/motionbev_node.py @@ -0,0 +1,163 @@ +"""MotionBEV-MOS deploy 노드. /raw/lidar + odom 구독 → N-스캔 버퍼 → polar-BEV residual → +MotionBEV 추론 → /perception/moving(label) + /perception/moving_rgb(빨강) 발행. +상태성 3개(버퍼/odom캐싱/residual) — LMNet deploy 와 동일 패턴(stateful MOS, 01-arch §1.5).""" +import argparse +import struct + +import numpy as np +import torch +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2, PointField +from nav_msgs.msg import Odometry +import sensor_msgs_py.point_cloud2 as pc2 + +from perception_motionbev_pkg import resolve_model +from perception_motionbev_pkg import pose_util +from perception_motionbev_pkg.scan_buffer import ScanBuffer +from perception_motionbev_pkg.residual_lib import compute_residual +from perception_motionbev_pkg.motionbev_infer import MotionBEVInfer + + +def _is_cuda_error(e): + s = str(e).lower() + return 'cuda' in s or 'cudnn' in s or 'out of memory' in s + + +def _read_xyzi(msg): + xyz = pc2.read_points_numpy(msg, field_names=("x", "y", "z"), + reshape_organized_cloud=False).astype(np.float32) + try: # intensity 없는 cloud 면 0 으로 채움 + inten = pc2.read_points_numpy(msg, field_names=("intensity",), + reshape_organized_cloud=False).astype(np.float32).reshape(-1, 1) + except Exception: + inten = np.zeros((len(xyz), 1), np.float32) + return np.concatenate([xyz, inten], axis=1) + + +class MotionBEVNode(Node): + def __init__(self, args, infer, model_info): + super().__init__("motionbev_node") + # infer 는 rclpy.init() 전에 미리 로드해 받는다 — DDS 스레드와의 데드락 회피. + self.infer = infer + self.model_info = model_info # 런타임 CPU 폴백 시 ckpt 재로드용 + self._cpu_fallback_done = (infer.device == "cpu") + self.N = self.infer.n_residual + self.proj = self.infer.proj # deploy residual 과 추론 preprocess 가 같은 격자 공유 + self.buf = ScanBuffer(self.N) + self.last_pose = np.eye(4) + self.have_odom = False + + self.create_subscription(Odometry, args.odom_topic, self._on_odom, 10) + self.create_subscription(PointCloud2, args.input_topic, self._on_lidar, 1) + self.pub_lbl = self.create_publisher(PointCloud2, args.moving_topic, 1) + self.pub_rgb = self.create_publisher(PointCloud2, args.moving_rgb_topic, 1) + + def _on_odom(self, msg): + p, q = msg.pose.pose.position, msg.pose.pose.orientation + self.last_pose = pose_util.pose_to_matrix(p.x, p.y, p.z, q.x, q.y, q.z, q.w) + self.have_odom = True + + def _on_lidar(self, msg): + try: + self._process_lidar(msg) + except Exception as e: + self.get_logger().warn(f"[motionbev] 프레임 처리 실패, 스킵: {e}", throttle_duration_sec=5.0) + + def _process_lidar(self, msg): + pts = _read_xyzi(msg) + if not self.have_odom: + self.get_logger().warn("[motionbev] odom 미수신 — 직전 pose 사용", throttle_duration_sec=5.0) + self.buf.push(pts, self.last_pose.copy()) + if not self.buf.ready(): + self._publish(msg.header, pts, np.zeros(len(pts), bool)) # warmup: all-static + return + cur_pts, cur_pose = self.buf.current() + residuals = [] + for k in range(1, self.N + 1): + past_pts, past_pose = self.buf.past(k) + T = pose_util.relative_pose(cur_pose, past_pose) + residuals.append(compute_residual(cur_pts, past_pts, T, self.proj)) + moving = self._infer_with_fallback(cur_pts, residuals) + self._publish(msg.header, cur_pts, moving) + + def _infer_with_fallback(self, cur_pts, residuals): + """추론 중 CUDA 에러 시 1회 CPU 폴백 — ckpt 에서 CPU 백엔드 재생성 후 해당 프레임 재시도. + (.to('cpu') 이동 금지 — 깨진 CUDA 컨텍스트 위험. 08-gpu-cpu-runtime N3 CPU-first 규범)""" + try: + return self.infer.infer(cur_pts, residuals) + except Exception as e: + if self._cpu_fallback_done or not _is_cuda_error(e): + raise + self.get_logger().warn(f"[motionbev] 추론 중 CUDA 에러 → CPU 백엔드 재구성: {e}") + self._cpu_fallback_done = True + try: # 기존 GPU 모델 해제 — 실패해도 무해 + del self.infer.model + torch.cuda.empty_cache() + except Exception: + pass + m = self.model_info + self.infer = MotionBEVInfer(m["ckpt"], m["arch_cfg"], m["data_cfg"], device="cpu") + self.proj = self.infer.proj + return self.infer.infer(cur_pts, residuals) + + def _publish(self, header, pts, moving): + # label cloud: x,y,z,label(0/1) + lbl = moving.astype(np.float32) + lbl_pts = np.column_stack([pts[:, :3], lbl]).astype(np.float32) + self.pub_lbl.publish(self._cloud(header, lbl_pts, "label")) + # rgb cloud: moving=빨강(255,0,0), static=회색(120,120,120) + rgb = np.where(moving[:, None], np.array([255, 0, 0], np.uint8), + np.array([120, 120, 120], np.uint8)) + self.pub_rgb.publish(self._rgb_cloud(header, pts[:, :3], rgb)) + + def _cloud(self, header, pts, fourth): + fields = [PointField(name=n, offset=i * 4, datatype=PointField.FLOAT32, count=1) + for i, n in enumerate(["x", "y", "z", fourth])] + return pc2.create_cloud(header, fields, pts) + + def _rgb_cloud(self, header, xyz, rgb): + fields = [PointField(name=n, offset=i * 4, datatype=PointField.FLOAT32, count=1) + for i, n in enumerate(["x", "y", "z"])] + fields.append(PointField(name="rgb", offset=12, datatype=PointField.FLOAT32, count=1)) + data = [] + for (x, y, z), (r, g, b) in zip(xyz, rgb): + packed = struct.unpack("f", struct.pack("I", (int(r) << 16) | (int(g) << 8) | int(b)))[0] + data.append([float(x), float(y), float(z), packed]) + return pc2.create_cloud(header, fields, data) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--input_topic", default="/raw/lidar") + ap.add_argument("--odom_topic", default="/odom") + ap.add_argument("--moving_topic", default="/perception/moving") + ap.add_argument("--moving_rgb_topic", default="/perception/moving_rgb") + ap.add_argument("--model_source", default="finetuned") # UI 스위치: finetuned|pretrained + ap.add_argument("--device", default="auto") # auto=런타임 감지(GPU 있으면 cuda, 없으면 cpu) + args, _ = ap.parse_known_args() + # ★ 모델을 rclpy.init() "전에" 로드 — DDS 수신 스레드와 torch 로드 동시 기동 시 futex 데드락 회피. + m = resolve_model.resolve(model_source=args.model_source) + print(f"[motionbev] model source={m['source']} ckpt={m['ckpt']}", flush=True) + if m["source"] == "pretrained": + print("[motionbev] finetuned 없음 — KITTI pretrained 동작(CARLA 품질 낮음). 먼저 Retrain 권장", flush=True) + try: + infer = MotionBEVInfer(m["ckpt"], m["arch_cfg"], m["data_cfg"], device=args.device) + except Exception as e: + if args.device == "cpu" or not _is_cuda_error(e): + raise + print(f"[motionbev] GPU 백엔드 초기화 실패({e}) → CPU 폴백", flush=True) + infer = MotionBEVInfer(m["ckpt"], m["arch_cfg"], m["data_cfg"], device="cpu") + rclpy.init() + node = MotionBEVNode(args, infer, m) + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/polar_projection.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/polar_projection.py new file mode 100755 index 0000000..832a0a8 --- /dev/null +++ b/src/perception_motionbev_pkg/perception_motionbev_pkg/polar_projection.py @@ -0,0 +1,127 @@ +#!/usr/bin/env python3 +"""MotionBEV polar BEV projection (모델 무관, 순수 numpy). + +MotionBEV(RA-L'23) 의 입력 표현 = Polar BEV grid. range-image(LMNet) 와 달리 점을 +극좌표 BEV 격자(거리 bin × 방위각 bin)에 투영해 BEV 채널(최대 높이 등)을 만든다. +시간축 residual(과거-현재 polar BEV 차이)을 만들기 위한 공유 투영기 — 학습(gen_residual)과 +배포(deploy)가 같은 격자 공식을 써야 학습-배포 분포가 일치한다(하드코딩 금지: 격자 파라미터는 +arch_cfg.dataset.grid 에서 읽어 retrain.sh / deploy 가 동일값을 전달). + +LMNet 의 range-image 대비 의도된 차이(04-consistency §1.5): MOS 가치를 위한 시간축 입력은 +공통, **표현방식만 Polar-BEV 로 다양화**(교육 가치). 나머지 수집/로그/그래프/split 은 동일. +""" +from dataclasses import dataclass + +import numpy as np + + +@dataclass +class PolarBEVResult: + bev: np.ndarray # (C, R, A): occupancy, max_z, mean_intensity + rb: np.ndarray # (V,) valid 점들의 거리 bin index + ab: np.ndarray # (V,) valid 점들의 방위각 bin index + valid: np.ndarray # (N,) bool — 격자 범위 안 점 마스크 + + +@dataclass +class PolarPointFeature: + """ptBEVnet 입력용 점별 polar 피처 + 3D polar grid index (업스트림 spherical_dataset 대응). + + spherical_dataset.__getitem__ 의 return_fea(폴라) 레이아웃을 그대로 재현한다: + [bias_rho, bias_theta, bias_z, rho, theta, z, x, y, intensity] (fea_dim=9) + residual(motion) 컬럼은 infer 단계에서 우측에 concat 한다. + grid_ind = [rho_idx, theta_idx, z_idx] (업스트림 grid_size=[480,360,32] 순서). + """ + pt_fea: np.ndarray # (V, 9) valid 점 appearance 피처 + grid_ind: np.ndarray # (V, 3) valid 점 polar grid index + valid: np.ndarray # (N,) bool — 격자 범위(rho·z) 안 점 마스크 + + +class PolarBEVProjector: + def __init__(self, n_range, n_angle, max_range, min_range, z_min=-3.0, z_max=3.0, + n_height=32): + self.R = int(n_range) + self.A = int(n_angle) + self.H = int(n_height) + self.max_range = float(max_range) + self.min_range = float(min_range) + self.z_min = float(z_min) + self.z_max = float(z_max) + + def project(self, points): + """points(N,>=3) → PolarBEVResult. BEV 채널: [occupancy, max_z(정규화), mean_intensity].""" + xyz = points[:, :3] + intensity = points[:, 3] if points.shape[1] > 3 else np.zeros(len(points), np.float32) + x, y, z = xyz[:, 0], xyz[:, 1], xyz[:, 2] + rho = np.sqrt(x * x + y * y) + theta = np.arctan2(y, x) # [-pi, pi] + valid = (rho >= self.min_range) & (rho < self.max_range) + + rb = np.clip(((rho - self.min_range) / (self.max_range - self.min_range) + * self.R).astype(np.int32), 0, self.R - 1) + ab = np.clip(((theta + np.pi) / (2.0 * np.pi) * self.A).astype(np.int32), + 0, self.A - 1) + + bev = np.zeros((3, self.R, self.A), np.float32) + vi = np.where(valid)[0] + # 가까운 점부터(작은 rho) 채워 마지막에 높은 점이 max_z 를 덮어쓰도록 정렬 + order = vi[np.argsort(rho[vi])] + vr, va, vz, vint = rb[order], ab[order], z[order], intensity[order] + zn = np.clip((vz - self.z_min) / max(self.z_max - self.z_min, 1e-6), 0.0, 1.0) + bev[0, vr, va] = 1.0 # occupancy + np.maximum.at(bev[1], (vr, va), zn) # max height (정규화) + bev[2, vr, va] = vint # intensity (마지막 점) + return PolarBEVResult(bev=bev, rb=rb[valid], ab=ab[valid], valid=valid) + + def point_features(self, points): + """points(N,>=3[,4]) → PolarPointFeature (업스트림 spherical_dataset 폴라 피처 재현). + + 볼륨/격자 공식은 업스트림과 동일: polar 좌표 [rho, theta, z] 를 고정 볼륨 + ([min_range,-π,z_min]~[max_range,π,z_max]) 에 grid_size-1 등분해 인덱싱한다. + 하드코딩 금지: R/A/H·range·z 는 모두 생성자(arch_cfg 기반)에서 온다. + """ + xyz = points[:, :3].astype(np.float32) + intensity = (points[:, 3] if points.shape[1] > 3 + else np.zeros(len(points), np.float32)).astype(np.float32) + x, y, z = xyz[:, 0], xyz[:, 1], xyz[:, 2] + rho = np.sqrt(x * x + y * y) + theta = np.arctan2(y, x) # [-π, π] + xyz_pol = np.stack((rho, theta, z), axis=1) # 업스트림 cart2polar 와 동일 순서 + + min_bound = np.array([self.min_range, -np.pi, self.z_min], np.float32) + max_bound = np.array([self.max_range, np.pi, self.z_max], np.float32) + grid_size = np.array([self.R, self.A, self.H], np.int64) + intervals = (max_bound - min_bound) / (grid_size - 1) + + clipped = np.clip(xyz_pol, min_bound, max_bound) + grid_ind = np.floor((clipped - min_bound) / intervals).astype(np.int64) + grid_ind = np.clip(grid_ind, 0, grid_size - 1) + + # 업스트림: voxel 중심 대비 bias + 절대 polar + (x,y) + intensity = 9 차원 + voxel_centers = (grid_ind.astype(np.float32) + 0.5) * intervals + min_bound + bias = xyz_pol - voxel_centers + pt_fea = np.concatenate( + (bias, xyz_pol, xyz[:, :2], intensity[:, None]), axis=1).astype(np.float32) + + # 격자 범위(rho·z) 밖 점은 motion 비교/언프로젝트에서 제외 (방위각은 순환이라 항상 유효) + valid = ((rho >= self.min_range) & (rho < self.max_range) & + (z >= self.z_min) & (z < self.z_max)) + return PolarPointFeature(pt_fea=pt_fea[valid], grid_ind=grid_ind[valid], valid=valid) + + def unproject(self, cell_labels, proj: PolarBEVResult): + """cell_labels (R,A) → 점별 라벨 (N,). valid 아닌 점은 0(unlabeled).""" + n = proj.valid.shape[0] + labels = np.zeros(n, dtype=np.int64) + labels[proj.valid] = cell_labels[proj.rb, proj.ab] + return labels + + +def projector_from_arch(arch_cfg_path): + """arch_cfg(dataset.grid) 에서 polar BEV 격자 파라미터를 읽어 PolarBEVProjector 생성. + 하드코딩 금지: residual 격자는 base 채널(학습 parser)·deploy 가 읽는 arch_cfg 와 같아야 + 학습-배포가 일치한다(다르면 모델이 본 적 없는 residual 분포 → 성능 저하).""" + import yaml + g = yaml.safe_load(open(arch_cfg_path))['dataset']['grid'] + return PolarBEVProjector(g['n_range'], g['n_angle'], + g['max_range'], g['min_range'], + g.get('z_min', -3.0), g.get('z_max', 3.0)) diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/pose_util.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/pose_util.py new file mode 100755 index 0000000..26fb7f4 --- /dev/null +++ b/src/perception_motionbev_pkg/perception_motionbev_pkg/pose_util.py @@ -0,0 +1,25 @@ +"""ego-pose 유틸 — nav_msgs/Odometry 의 (position, orientation) → 4x4, 상대 pose.""" +import numpy as np + + +def pose_to_matrix(x, y, z, qx, qy, qz, qw): + """위치 + 쿼터니언 → 4x4 homogeneous.""" + n = qx * qx + qy * qy + qz * qz + qw * qw + s = 0.0 if n == 0 else 2.0 / n + xx, yy, zz = qx * qx * s, qy * qy * s, qz * qz * s + xy, xz, yz = qx * qy * s, qx * qz * s, qy * qz * s + wx, wy, wz = qw * qx * s, qw * qy * s, qw * qz * s + R = np.array([ + [1 - (yy + zz), xy - wz, xz + wy], + [xy + wz, 1 - (xx + zz), yz - wx], + [xz - wy, yz + wx, 1 - (xx + yy)], + ], dtype=np.float64) + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = [x, y, z] + return T + + +def relative_pose(curr, past): + """과거점을 현재 프레임으로 옮기는 변환 inv(curr) @ past.""" + return np.linalg.inv(curr) @ past diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/residual_lib.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/residual_lib.py new file mode 100755 index 0000000..42dc7ff --- /dev/null +++ b/src/perception_motionbev_pkg/perception_motionbev_pkg/residual_lib.py @@ -0,0 +1,29 @@ +"""MotionBEV polar-BEV residual (deploy). retrain gen_residual.compute_residual 와 동일 로직 복제. +학습-배포 일관성을 위해 같은 PolarBEVProjector 격자 공식을 쓴다.""" +import numpy as np + +from perception_motionbev_pkg.polar_projection import PolarBEVProjector # noqa: F401 (호출측 주입) + + +def _bev_motion_channel(points, projector): + """polar BEV 의 motion 비교용 채널(R,A) = occupancy(0) + max_z(1) 합성.""" + r = projector.project(points) + return r.bev[0] + r.bev[1] + + +def compute_residual(curr_points, past_points, T_past_to_curr, projector): + """현재 프레임 기준 polar-BEV residual(R,A). T_past_to_curr: 과거점→현재 프레임 4x4.""" + past_xyz = past_points[:, :3] + hom = np.concatenate([past_xyz, np.ones((len(past_xyz), 1), np.float32)], axis=1) + past_in_curr = (T_past_to_curr @ hom.T).T[:, :3] + if past_points.shape[1] > 3: + past_aligned = np.concatenate( + [past_in_curr.astype(np.float32), past_points[:, 3:4]], axis=1) + else: + past_aligned = past_in_curr.astype(np.float32) + cur_b = _bev_motion_channel(curr_points, projector) + past_b = _bev_motion_channel(past_aligned, projector) + valid = (cur_b > 0) | (past_b > 0) + res = np.zeros_like(cur_b) + res[valid] = np.abs(cur_b[valid] - past_b[valid]) / np.maximum(cur_b[valid], 1e-6) + return res diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/resolve_model.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/resolve_model.py new file mode 100755 index 0000000..a6c15d9 --- /dev/null +++ b/src/perception_motionbev_pkg/perception_motionbev_pkg/resolve_model.py @@ -0,0 +1,43 @@ +"""모델 경로 결정 — finetuned(retrain 산출물) 우선, 없거나 불완전하면 baked pretrained fallback. +※ 작은 함수 하나로 분리 — model_source=auto|finetuned|pretrained 명시 선택을 한 분기로 끼우기 위함.""" +import os + +# 가중치 공유 단일 위치(docker/local): 호스트 .caches/config/nav2_config/perception/models/motionbev/* +# → 컨테이너 /perception_models/motionbev/{finetuned,pretrained} (SWRule config_mounts). +DEFAULT_FINETUNED = "/perception_models/motionbev/finetuned" +SHARED_PRE_DIR = "/perception_models/motionbev/pretrained" # 공유 pretrained (Phase 2 채움) +DEFAULT_PRE_CKPT = "/opt/motionbev_pretrained/MotionBEV" # 이미지 baked 안전망 +DEFAULT_PRE_ARCH = "/opt/MotionBEV/config/MotionBEV-semantickitti.yaml" +# 업스트림 실제 경로(infer_SemanticKITTI.py 의 data_config_path 와 동일). +# 옛 "config/labels/semantic-kitti-mos.yaml" 는 존재하지 않음(생성기 전용 경로와 혼동). +DEFAULT_PRE_DATA = "/opt/MotionBEV/config/semantic-kitti-MOS.yaml" + + +def _shared_pretrained(): + # 공유 pretrained(.../pretrained: ckpt+arch+data)가 채워져 있으면 baked(/opt) 보다 우선. + ckpt = os.path.join(SHARED_PRE_DIR, "MotionBEV") + arch = os.path.join(SHARED_PRE_DIR, "arch_cfg.yaml") + data = os.path.join(SHARED_PRE_DIR, "data_cfg.yaml") + if os.path.isfile(ckpt) and os.path.isfile(arch) and os.path.isfile(data): + return {"source": "pretrained", "ckpt": ckpt, "arch_cfg": arch, "data_cfg": data} + return None + + +def resolve(model_source="finetuned", + finetuned_dir=DEFAULT_FINETUNED, + pretrained_ckpt=DEFAULT_PRE_CKPT, + pretrained_arch=DEFAULT_PRE_ARCH, + pretrained_data=DEFAULT_PRE_DATA): + # 3단 fallback: finetuned → 공유 pretrained → baked pretrained(/opt 안전망). + # model_source=="pretrained" 면 finetuned 있어도 pretrained 강제(공유 우선, 없으면 baked). + _baked = {"source": "pretrained", "ckpt": pretrained_ckpt, + "arch_cfg": pretrained_arch, "data_cfg": pretrained_data} + _pretrained = _shared_pretrained() or _baked + if model_source == "pretrained": + return _pretrained + ckpt = os.path.join(finetuned_dir, "MotionBEV_valid_best") + arch = os.path.join(finetuned_dir, "arch_cfg.yaml") + data = os.path.join(finetuned_dir, "data_cfg.yaml") + if os.path.isfile(ckpt) and os.path.isfile(arch) and os.path.isfile(data): + return {"source": "finetuned", "ckpt": ckpt, "arch_cfg": arch, "data_cfg": data} + return _pretrained diff --git a/src/perception_motionbev_pkg/perception_motionbev_pkg/scan_buffer.py b/src/perception_motionbev_pkg/perception_motionbev_pkg/scan_buffer.py new file mode 100755 index 0000000..883d9a7 --- /dev/null +++ b/src/perception_motionbev_pkg/perception_motionbev_pkg/scan_buffer.py @@ -0,0 +1,21 @@ +"""N-스캔 링버퍼 — (points, pose_4x4). residual 생성에 현재+과거 N 프레임 필요.""" +from collections import deque + + +class ScanBuffer: + def __init__(self, num_residual): + self.n = num_residual + self._buf = deque(maxlen=num_residual + 1) # 현재 + 과거 N + + def push(self, points, pose): + self._buf.append((points, pose)) + + def ready(self): + return len(self._buf) >= self.n + 1 + + def current(self): + return self._buf[-1] + + def past(self, k): + """k 프레임 전 (1=직전). ready() 일 때만 호출.""" + return self._buf[-1 - k] diff --git a/src/perception_motionbev_pkg/resource/perception_motionbev_pkg b/src/perception_motionbev_pkg/resource/perception_motionbev_pkg new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_motionbev_pkg/setup.cfg b/src/perception_motionbev_pkg/setup.cfg new file mode 100755 index 0000000..dac6b29 --- /dev/null +++ b/src/perception_motionbev_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/perception_motionbev_pkg +[install] +install_scripts=$base/lib/perception_motionbev_pkg diff --git a/src/perception_motionbev_pkg/setup.py b/src/perception_motionbev_pkg/setup.py new file mode 100755 index 0000000..b33ee64 --- /dev/null +++ b/src/perception_motionbev_pkg/setup.py @@ -0,0 +1,35 @@ + +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'perception_motionbev_pkg' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='dgist', + maintainer_email='jang1future@gmail.com', + description='LiDAR moving object segmentation (MotionBEV / Polar-BEV) perception module', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'motionbev = perception_motionbev_pkg.motionbev_node:main' + ], + }, +) diff --git a/src/perception_motionbev_pkg/test/test_copyright.py b/src/perception_motionbev_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/src/perception_motionbev_pkg/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/perception_motionbev_pkg/test/test_flake8.py b/src/perception_motionbev_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/src/perception_motionbev_pkg/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/perception_motionbev_pkg/test/test_pep257.py b/src/perception_motionbev_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/src/perception_motionbev_pkg/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/perception_motionbev_pkg/test/test_polar_projection.py b/src/perception_motionbev_pkg/test/test_polar_projection.py new file mode 100755 index 0000000..6a875f9 --- /dev/null +++ b/src/perception_motionbev_pkg/test/test_polar_projection.py @@ -0,0 +1,31 @@ +import numpy as np +from perception_motionbev_pkg.polar_projection import PolarBEVProjector + + +def test_shape_and_occupancy(): + proj = PolarBEVProjector(n_range=32, n_angle=90, max_range=50.0, min_range=2.0) + pts = np.array([[10.0, 0.0, 0.5, 0.7]], np.float32) + r = proj.project(pts) + assert r.bev.shape == (3, 32, 90) + assert r.bev[0].sum() == 1.0 # 점 1개 → occupancy 1칸 + assert r.valid.sum() == 1 + + +def test_out_of_range_excluded(): + proj = PolarBEVProjector(n_range=32, n_angle=90, max_range=50.0, min_range=2.0) + pts = np.array([[0.5, 0.0, 0.0, 1.0], # rhomax → 제외 + np.float32) + r = proj.project(pts) + assert r.valid.sum() == 0 + + +def test_unproject_round_trip(): + proj = PolarBEVProjector(n_range=16, n_angle=60, max_range=50.0, min_range=2.0) + pts = np.array([[10.0, 0.0, 0.0, 1.0], [20.0, 5.0, 0.0, 1.0]], np.float32) + r = proj.project(pts) + cells = np.zeros((16, 60), np.int64) + cells[r.rb, r.ab] = 2 # 두 점이 떨어진 칸을 moving(2)으로 + labels = proj.unproject(cells, r) + assert labels.shape == (2,) + assert (labels == 2).all() diff --git a/src/perception_motionbev_pkg/test/test_pose_util.py b/src/perception_motionbev_pkg/test/test_pose_util.py new file mode 100755 index 0000000..498e208 --- /dev/null +++ b/src/perception_motionbev_pkg/test/test_pose_util.py @@ -0,0 +1,22 @@ +import numpy as np +from perception_motionbev_pkg import pose_util + + +def test_identity_quaternion_pure_translation(): + T = pose_util.pose_to_matrix(x=1.0, y=2.0, z=3.0, qx=0, qy=0, qz=0, qw=1) + assert np.allclose(T[:3, 3], [1, 2, 3]) + assert np.allclose(T[:3, :3], np.eye(3)) + + +def test_90deg_yaw(): + s = np.sqrt(0.5) + T = pose_util.pose_to_matrix(0, 0, 0, 0, 0, s, s) + assert np.allclose(T[:3, :3] @ np.array([1.0, 0, 0]), [0, 1, 0], atol=1e-6) + + +def test_relative_pose_past_to_curr(): + A = pose_util.pose_to_matrix(0, 0, 0, 0, 0, 0, 1) + B = pose_util.pose_to_matrix(5, 0, 0, 0, 0, 0, 1) + T = pose_util.relative_pose(curr=B, past=A) + p = T @ np.array([0.0, 0, 0, 1]) + assert np.allclose(p[:3], [-5, 0, 0]) diff --git a/src/perception_motionbev_pkg/test/test_residual_lib.py b/src/perception_motionbev_pkg/test/test_residual_lib.py new file mode 100755 index 0000000..ac646b9 --- /dev/null +++ b/src/perception_motionbev_pkg/test/test_residual_lib.py @@ -0,0 +1,21 @@ +import numpy as np +from perception_motionbev_pkg import residual_lib +from perception_motionbev_pkg.polar_projection import PolarBEVProjector + + +def _proj(): + return PolarBEVProjector(n_range=48, n_angle=180, max_range=50.0, min_range=2.0) + + +def test_identical_scan_zero_motion_zero_residual(): + pts = np.random.RandomState(0).uniform(-20, 20, (500, 4)).astype(np.float32) + res = residual_lib.compute_residual(pts, pts, np.eye(4), _proj()) + assert res.shape == (48, 180) + assert np.nanmax(np.abs(res)) < 1e-3 + + +def test_moved_point_makes_nonzero_residual(): + pts = np.array([[10.0, 0, 0, 1]] * 50, np.float32) + T = np.eye(4); T[0, 3] = 8.0 + res = residual_lib.compute_residual(pts, pts, T, _proj()) + assert np.nanmax(np.abs(res)) > 0.0 diff --git a/src/perception_motionbev_pkg/test/test_resolve_model.py b/src/perception_motionbev_pkg/test/test_resolve_model.py new file mode 100755 index 0000000..19075d8 --- /dev/null +++ b/src/perception_motionbev_pkg/test/test_resolve_model.py @@ -0,0 +1,72 @@ +from perception_motionbev_pkg import resolve_model + +PRE_CKPT = "/opt/motionbev_pretrained/MotionBEV" +PRE_ARCH = "/opt/x/MotionBEV-semantickitti.yaml" +PRE_DATA = "/opt/x/semantic-kitti-mos.yaml" + + +def _make_finetuned(tmp_path): + ft = tmp_path / "motionbev_finetuned" + ft.mkdir() + (ft / "MotionBEV_valid_best").write_bytes(b"x") + (ft / "arch_cfg.yaml").write_text("a: 1") + (ft / "data_cfg.yaml").write_text("b: 1") + return ft + + +def test_finetuned_when_present(tmp_path): + ft = _make_finetuned(tmp_path) + r = resolve_model.resolve(finetuned_dir=str(ft), pretrained_ckpt=PRE_CKPT, + pretrained_arch=PRE_ARCH, pretrained_data=PRE_DATA) + assert r["source"] == "finetuned" + assert r["ckpt"] == str(ft / "MotionBEV_valid_best") + assert r["arch_cfg"] == str(ft / "arch_cfg.yaml") + + +def test_pretrained_fallback_when_absent(tmp_path): + ft = tmp_path / "motionbev_finetuned" + r = resolve_model.resolve(finetuned_dir=str(ft), pretrained_ckpt=PRE_CKPT, + pretrained_arch=PRE_ARCH, pretrained_data=PRE_DATA) + assert r["source"] == "pretrained" + assert r["ckpt"] == PRE_CKPT + assert r["arch_cfg"] == PRE_ARCH + + +def test_partial_finetuned_falls_back(tmp_path): + ft = tmp_path / "motionbev_finetuned"; ft.mkdir() + (ft / "MotionBEV_valid_best").write_bytes(b"x") + r = resolve_model.resolve(finetuned_dir=str(ft), pretrained_ckpt=PRE_CKPT, + pretrained_arch=PRE_ARCH, pretrained_data=PRE_DATA) + assert r["source"] == "pretrained" + + +def test_model_source_pretrained_forces_baked_even_if_finetuned_present(tmp_path): + ft = _make_finetuned(tmp_path) + r = resolve_model.resolve(model_source="pretrained", finetuned_dir=str(ft), + pretrained_ckpt=PRE_CKPT, pretrained_arch=PRE_ARCH, + pretrained_data=PRE_DATA) + assert r["source"] == "pretrained" + assert r["ckpt"] == PRE_CKPT + + +def test_model_source_finetuned_uses_finetuned_when_present(tmp_path): + ft = _make_finetuned(tmp_path) + r = resolve_model.resolve(model_source="finetuned", finetuned_dir=str(ft), + pretrained_ckpt=PRE_CKPT, pretrained_arch=PRE_ARCH, + pretrained_data=PRE_DATA) + assert r["source"] == "finetuned" + assert r["ckpt"] == str(ft / "MotionBEV_valid_best") + + +def test_model_source_finetuned_silent_fallback_when_absent(tmp_path): + ft = tmp_path / "motionbev_finetuned" + r = resolve_model.resolve(model_source="finetuned", finetuned_dir=str(ft), + pretrained_ckpt=PRE_CKPT, pretrained_arch=PRE_ARCH, + pretrained_data=PRE_DATA) + assert r["source"] == "pretrained" + + +def test_resolve_no_args_backward_compat(): + r = resolve_model.resolve() + assert r["source"] == "pretrained" + assert r["ckpt"] == resolve_model.DEFAULT_PRE_CKPT diff --git a/src/perception_motionbev_pkg/test/test_scan_buffer.py b/src/perception_motionbev_pkg/test/test_scan_buffer.py new file mode 100755 index 0000000..ee26837 --- /dev/null +++ b/src/perception_motionbev_pkg/test/test_scan_buffer.py @@ -0,0 +1,37 @@ +import numpy as np +from perception_motionbev_pkg.scan_buffer import ScanBuffer + + +def _pts(v): + return np.full((3, 4), float(v), np.float32) + + +def test_not_ready_until_n_plus_one(): + buf = ScanBuffer(num_residual=2) + buf.push(_pts(0), np.eye(4)) + assert not buf.ready() + buf.push(_pts(1), np.eye(4)) + assert not buf.ready() + buf.push(_pts(2), np.eye(4)) + assert buf.ready() + + +def test_past_k_returns_kth_back(): + buf = ScanBuffer(num_residual=2) + for i in range(3): + buf.push(_pts(i), np.eye(4)) + cur_pts, cur_pose = buf.current() + assert cur_pts[0, 0] == 2.0 + p1_pts, _ = buf.past(1) + p2_pts, _ = buf.past(2) + assert p1_pts[0, 0] == 1.0 + assert p2_pts[0, 0] == 0.0 + + +def test_maxlen_evicts_old(): + buf = ScanBuffer(num_residual=1) + for i in range(5): + buf.push(_pts(i), np.eye(4)) + cur, _ = buf.current() + prev, _ = buf.past(1) + assert cur[0, 0] == 4.0 and prev[0, 0] == 3.0 diff --git a/src/perception_overlaptransformer_pkg/config/ot.yaml b/src/perception_overlaptransformer_pkg/config/ot.yaml new file mode 100755 index 0000000..34771b5 --- /dev/null +++ b/src/perception_overlaptransformer_pkg/config/ot.yaml @@ -0,0 +1,19 @@ +overlaptransformer: + ros__parameters: + # 맵 DB(map.npz)는 필터된 스캔(=RTABMap .db 매핑 입력 = /lidar_filtered)으로 빌드되므로, + # 쿼리도 같은 /lidar_filtered 를 써야 분포가 일치해 매칭이 맞는다. /raw/lidar(z 6.6m·88m raw)는 + # 분포가 달라 descriptor 불일치 → 오측위. (FR09 실측: raw 11622pts/88m vs filtered 9572pts/41m) + input_topic: "/lidar_filtered" + model_ckpt_path: "" + map_db_path: "" + match_dist_thresh: 1.0 + # range 투영 파라미터 (모델 OverlapTransformer 가 기대하는 입력 = KITTI Velodyne FOV). + # 센서가 아니라 모델 학습값에 맞춘다. 잘못 주면 descriptor 붕괴(FR09 Ouster 16% 사례). + # build_map_db(맵 빌드)와 ot_node(query)가 같은 값을 써야 매칭이 맞음 (run.sh 가 이 파일로 통일). + proj_H: 64 + proj_W: 900 + fov_up: 3.0 + fov_down: -25.0 + max_range: 50.0 + min_range: 1.0 + use_sim_time: true diff --git a/src/perception_overlaptransformer_pkg/launch/ot.launch.py b/src/perception_overlaptransformer_pkg/launch/ot.launch.py new file mode 100755 index 0000000..3eebe2f --- /dev/null +++ b/src/perception_overlaptransformer_pkg/launch/ot.launch.py @@ -0,0 +1,161 @@ +# File: ot.launch.py +# Description: +# Place Recognition (OverlapTransformer) launch — 단독 deploy run.sh 의 모델/맵 DB +# 해석 로직을 OpaqueFunction 으로 이식 (통합 perception ws 편입, 단일 출처). +# +# resolve 의미론(무회귀 원칙): +# - model_ckpt_path/map_db_path 가 명시되면(선언 기본값 '' 과 다르면) 그대로 사용 +# → 단독 deploy run.sh 가 model_ckpt_path:=$CKPT map_db_path:=$MAPDB 로 +# 명시 전달하는 기존 동작이 그대로 이긴다. +# - 미명시(기본값/빈값)일 때만 model_source(기본 finetuned, 07-model-source-switch 계약) +# + finetuned 파일 존재 검사로 자동 resolve. map.npz 는 /selected_map/*.db(RTABMap) +# → sequences/00 순으로 자동 빌드(staleness 태그로 재빌드 판단). +import glob +import os +import subprocess +import sys + +import yaml + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +# 컨테이너 내부 컨벤션 경로 (perception 공통: /perception_data 마운트·/opt baked·/selected_map 마운트) +# 가중치 공유 단일 위치(docker/local): /perception_models/overlaptransformer/{finetuned,pretrained} +# (호스트 .caches/config/nav2_config/perception/models/overlaptransformer/*, SWRule config_mounts). +FINETUNED_CKPT = '/perception_models/overlaptransformer/finetuned/overlaptransformer.pth.tar' +SHARED_PRE_CKPT = '/perception_models/overlaptransformer/pretrained/overlaptransformer.pth.tar' # 공유 pretrained (Phase 2 채움) +PRETRAINED_CKPT = '/opt/ot_pretrained/overlaptransformer.pth.tar' # 이미지 baked 안전망(Dockerfile COPY) +MAP_DB = '/perception_data/map_descriptors/map.npz' +MAP_SRC = MAP_DB + '.src' # 빌드된 map.npz 의 소스(.db+모델) 식별 → 교체 감지 +MAP_SEQ = '/perception_data/pr/sequences/00' # fallback: 수집 시퀀스(2D/ride-along) +SELECTED_MAP_DIR = '/selected_map' # 선택 맵 .db (config_mounts 로 마운트) + +ARGUMENTS = [ + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('model_ckpt_path', default_value=''), + DeclareLaunchArgument('map_db_path', default_value=''), + # UI 스위치(07 계약): pretrained 면 finetuned 있어도 baked 강제, 그 외 finetuned 우선 + DeclareLaunchArgument('model_source', default_value='finetuned'), +] + + +def _resolve_ckpt(model_source): + """finetuned(재학습 산출물) → 공유 pretrained → baked pretrained(/opt 안전망) 3단 fallback. + model_source=='pretrained' 면 finetuned 건너뛰고 pretrained(공유 우선, 없으면 baked) 강제.""" + if model_source != 'pretrained' and os.path.isfile(FINETUNED_CKPT): + print(f'[overlaptransformer] 재학습 모델 사용: {FINETUNED_CKPT}') + return FINETUNED_CKPT + if os.path.isfile(SHARED_PRE_CKPT): + print(f'[overlaptransformer] 공유 pretrained 사용: {SHARED_PRE_CKPT}') + return SHARED_PRE_CKPT + print(f'[overlaptransformer] base pretrained(baked) 사용: {PRETRAINED_CKPT}') + return PRETRAINED_CKPT + + +def _proj_args(params_file): + """투영 파라미터를 ot.yaml(단일 소스, SWRule override) 에서 읽어 맵 빌드에 적용. + query 노드(ot_node)도 같은 ot.yaml 을 쓰므로 빌드/쿼리 투영이 일치한다(매칭 정합 필수).""" + with open(params_file) as f: + p = yaml.safe_load(f)['overlaptransformer']['ros__parameters'] + return ['--proj_H', str(p.get('proj_H', 64)), '--proj_W', str(p.get('proj_W', 900)), + '--fov_up', str(p.get('fov_up', 3.0)), '--fov_down', str(p.get('fov_down', -25.0)), + '--max_range', str(p.get('max_range', 50.0)), '--min_range', str(p.get('min_range', 1.0))] + + +def _script_dir(): + """db_to_mapdb/build_map_db 가 든 pkg python 모듈 디렉토리 (run.sh 의 find src 대체).""" + import perception_overlaptransformer_pkg as _m + return os.path.dirname(os.path.abspath(_m.__file__)) + + +def _mtime(path): + """stat -c %Y 동치 (없으면 0) — staleness 태그용.""" + try: + return int(os.path.getmtime(path)) + except OSError: + return 0 + + +def _resolve_map_db(ckpt, params_file): + """map.npz 자동 빌드 (run.sh 로직 이식). 선택 맵 .db 우선(맵/모델 바뀌면 재빌드), + 없으면 수집 시퀀스(00) fallback. 끝까지 없으면 '' 반환(노드가 reloc 비활성 경고).""" + try: + pydir = _script_dir() + except ImportError: + print('[overlaptransformer] 에러: pkg 모듈(db_to_mapdb/build_map_db) 미발견 — map.npz 자동 빌드 생략') + return MAP_DB if os.path.isfile(MAP_DB) else '' + proj = _proj_args(params_file) + dbs = sorted(glob.glob(os.path.join(SELECTED_MAP_DIR, '*.db'))) + selected_db = dbs[0] if dbs else '' + # 맵 DB(map.npz) 자동 빌드: 선택 맵 .db 가 마운트돼 있으면 그걸로 빌드(맵/모델 바뀌면 재빌드). + # .db 안의 Admin.opt_poses(최적화 pose)+Data.scan(/lidar_filtered 분포)을 직접 디코드 → rtabmap 불필요. + if selected_db: + # staleness 태그 = ".db 경로+mtime + ckpt 경로+mtime" — run.sh 가 쓴 map.npz.src 와 포맷 호환 + src_tag = '%s %d %s %d' % (selected_db, _mtime(selected_db), ckpt, _mtime(ckpt)) + old_tag = '' + if os.path.isfile(MAP_SRC): + with open(MAP_SRC) as f: + old_tag = f.read().rstrip('\n') + if not os.path.isfile(MAP_DB) or old_tag != src_tag: + print(f"[overlaptransformer] 맵 DB 자동 빌드 (.db→map.npz): {selected_db} ({' '.join(proj)})") + r = subprocess.run([sys.executable, os.path.join(pydir, 'db_to_mapdb.py'), + ckpt, selected_db, MAP_DB] + proj, cwd=pydir) + if r.returncode == 0: + os.makedirs(os.path.dirname(MAP_SRC), exist_ok=True) + with open(MAP_SRC, 'w') as f: + f.write(src_tag + '\n') + else: + print('[overlaptransformer] 맵 DB 자동 빌드 실패(계속)') + else: + print('[overlaptransformer] map.npz 최신 (선택 맵/모델 변경 없음) — 재빌드 생략') + # fallback: .db 미마운트 시 수집 시퀀스(00, 2D/ride-along)로 빌드 + elif (not os.path.isfile(MAP_DB) and os.path.isdir(os.path.join(MAP_SEQ, 'velodyne')) + and os.path.isfile(os.path.join(MAP_SEQ, 'poses.txt'))): + print(f"[overlaptransformer] 맵 DB 빌드 (sequences/00→map.npz) ({' '.join(proj)})") + r = subprocess.run([sys.executable, os.path.join(pydir, 'build_map_db.py'), ckpt, + os.path.join(MAP_SEQ, 'velodyne'), os.path.join(MAP_SEQ, 'poses.txt'), + MAP_DB] + proj, cwd=pydir) + if r.returncode != 0: + print('[overlaptransformer] 맵 DB 빌드 실패(계속)') + if os.path.isfile(MAP_DB): + return MAP_DB + # run.sh 동작 보존: 맵 소스가 없어도 launch 는 계속 — 노드가 "model/DB 미설정" 경고 후 비활성 + print('[overlaptransformer] 에러: 맵 소스 없음 (/selected_map/*.db·sequences/00 모두 부재) — relocalization 비활성') + return '' + + +def launch_setup(context, *args, **kwargs): + pkg = get_package_share_directory('perception_overlaptransformer_pkg') + params_file = os.path.join(pkg, 'config', 'ot.yaml') + ckpt = LaunchConfiguration('model_ckpt_path').perform(context) + map_db = LaunchConfiguration('map_db_path').perform(context) + model_source = LaunchConfiguration('model_source').perform(context) + # 명시 인자(기본값 '' 아님)는 그대로 — 단독 deploy run.sh 의 명시 전달 무회귀 + if not ckpt: + ckpt = _resolve_ckpt(model_source) + if not map_db: + map_db = _resolve_map_db(ckpt, params_file) + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + load_nodes = GroupAction(actions=[ + Node( + package='perception_overlaptransformer_pkg', executable='overlaptransformer', + name='overlaptransformer', output='screen', + parameters=[params_file, { + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'model_ckpt_path': ckpt, + 'map_db_path': map_db, + }], + remappings=remappings, + ) + ]) + return [load_nodes] + + +def generate_launch_description(): + ld = LaunchDescription(ARGUMENTS) + ld.add_action(OpaqueFunction(function=launch_setup)) + return ld diff --git a/src/perception_overlaptransformer_pkg/launch/perception.launch.py b/src/perception_overlaptransformer_pkg/launch/perception.launch.py new file mode 100755 index 0000000..7fec305 --- /dev/null +++ b/src/perception_overlaptransformer_pkg/launch/perception.launch.py @@ -0,0 +1,32 @@ +"""perception_manager 공통 진입점 (통합 perception 모듈). + +LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로 +`ros2 launch perception_overlaptransformer_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..` +를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(ot.launch.py)로 위임한다. +robot 은 공통 계약상 선언만 하고, 모델 launch 가 선언한 경우에만 소비된다. +""" +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + share = get_package_share_directory('perception_overlaptransformer_pkg') + return LaunchDescription([ + DeclareLaunchArgument('robot', default_value='fr09'), + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('model_source', default_value='finetuned'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(share, 'launch', 'ot.launch.py')), + launch_arguments={ + 'robot': LaunchConfiguration('robot'), + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'model_source': LaunchConfiguration('model_source'), + }.items(), + ), + ]) diff --git a/src/perception_overlaptransformer_pkg/package.xml b/src/perception_overlaptransformer_pkg/package.xml new file mode 100755 index 0000000..6cf8767 --- /dev/null +++ b/src/perception_overlaptransformer_pkg/package.xml @@ -0,0 +1,25 @@ + + + + perception_overlaptransformer_pkg + 0.0.0 + LiDAR place recognition (OverlapTransformer) perception module + dgist + TODO: License declaration + + rclpy + sensor_msgs + sensor_msgs_py + geometry_msgs + std_msgs + visualization_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/__init__.py b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/build_map_db.py b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/build_map_db.py new file mode 100755 index 0000000..b7bb91c --- /dev/null +++ b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/build_map_db.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 +"""맵 descriptor DB 빌드 one-shot (spec §7). 맵 시퀀스(velodyne+poses) → map.npz. +deploy run.sh 가 DB 없을 때 수집 시퀀스로 1회 호출. flat import 라 이 파일이 있는 dir 에서 실행. +투영 파라미터는 query 노드(ot_node)와 반드시 같아야 매칭이 맞으므로, run.sh 가 ot.yaml 의 값을 +--fov_up 등으로 넘긴다(미지정 시 모델 학습값 KITTI 기본). 하드코딩 금지. +사용: python build_map_db.py [--fov_up .. --fov_down .. ...]""" +import argparse +from ot_backend import OTBackend +from descriptor_db import build_db + + +def _is_cuda_error(e): + s = str(e).lower() + return 'cuda' in s or 'cudnn' in s or 'out of memory' in s + + +if __name__ == '__main__': + ap = argparse.ArgumentParser() + ap.add_argument('ckpt'); ap.add_argument('scan_dir'); ap.add_argument('poses'); ap.add_argument('out') + ap.add_argument('--proj_H', type=int, default=64); ap.add_argument('--proj_W', type=int, default=900) + ap.add_argument('--fov_up', type=float, default=3.0); ap.add_argument('--fov_down', type=float, default=-25.0) + ap.add_argument('--max_range', type=float, default=50.0); ap.add_argument('--min_range', type=float, default=1.0) + ap.add_argument('--device', default='auto') # auto: cuda 가용 시 GPU, 아니면 CPU (08-gpu-cpu-runtime N3/N4) + a = ap.parse_args() + try: + be = OTBackend(a.ckpt, device=a.device, proj_H=a.proj_H, proj_W=a.proj_W, fov_up=a.fov_up, + fov_down=a.fov_down, max_range=a.max_range, min_range=a.min_range) + except Exception as e: + if a.device == 'cpu' or not _is_cuda_error(e): + raise + print(f"[map_db] 경고: GPU 백엔드 초기화 실패({e}) → CPU 폴백") + be = OTBackend(a.ckpt, device='cpu', proj_H=a.proj_H, proj_W=a.proj_W, fov_up=a.fov_up, + fov_down=a.fov_down, max_range=a.max_range, min_range=a.min_range) + shape = build_db(be, a.scan_dir, a.poses, a.out) + print(f"[map_db] built {a.out} {shape} (fov_up={a.fov_up} fov_down={a.fov_down})") diff --git a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/db_to_mapdb.py b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/db_to_mapdb.py new file mode 100755 index 0000000..967ad19 --- /dev/null +++ b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/db_to_mapdb.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 +"""선택된 3D slam_map(RTABMap .db) → 맵 descriptor DB(map.npz) 직접 빌드 (자동 빌드 경로). + +deploy run.sh 가 map.npz 없을 때(또는 선택 맵이 바뀌었을 때) 호출한다. 기존 흐름 +(rtabmap-export → g2o → sequences/00 → build_map_db.py)은 rtabmap 바이너리가 필요했으나, +deploy 이미지엔 rtabmap 이 없다. 대신 .db 안의 두 BLOB 을 직접 디코드해 한 방에 만든다: + - 최적화 pose: Admin.opt_ids + Admin.opt_poses (둘 다 zlib). loop-closure 보정된 그래프 pose. + → ot_node 가 발행하는 측위 좌표가 nav2 가 쓰는 .db 기하맵과 동일 프레임이 됨. + - 스캔: Data.scan (zlib, XYZ float32). RTABMap 이 저장한 필터/등록된 스캔 = /lidar_filtered + 분포와 일치 → query 와 같은 분포라 descriptor 가 맞는다. +node id 로 둘을 매칭 → OTBackend.describe → (M,D) descriptors + (M,12) poses → map.npz. + +투영 파라미터는 query 노드(ot_node)와 반드시 같아야 매칭이 맞으므로 run.sh 가 ot.yaml 값을 +--fov_up 등으로 넘긴다(미지정 시 모델 학습값 KITTI 기본). 하드코딩 금지. +사용: python db_to_mapdb.py [--fov_up .. --fov_down .. ...]""" +import argparse +import os +import sqlite3 +import sys +import zlib + +import numpy as np + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) # flat import (colcon entry 와 동일) +from ot_backend import OTBackend + + +def _is_cuda_error(e): + s = str(e).lower() + return 'cuda' in s or 'cudnn' in s or 'out of memory' in s + + +def load_opt_poses(db_path): + """Admin.opt_ids(zlib int32) + opt_poses(zlib float32, id당 3x4=12) → {node_id: (12,) float32}.""" + c = sqlite3.connect(db_path) + try: + cur = c.cursor() + cur.execute("SELECT opt_ids, opt_poses FROM Admin ORDER BY time_enter DESC LIMIT 1") + row = cur.fetchone() + finally: + c.close() + if not row or row[0] is None or row[1] is None: + raise SystemExit("[db2map] 에러: Admin.opt_poses 없음 — 최적화 안 된 .db (SLAM 미완료?)") + ids = np.frombuffer(zlib.decompress(row[0]), dtype=np.int32) + poses = np.frombuffer(zlib.decompress(row[1]), dtype=np.float32).reshape(len(ids), 12) + return {int(i): poses[k].copy() for k, i in enumerate(ids)} + + +def load_scans(db_path): + """Data.scan(zlib XYZ float32) → {node_id: (N,4) xyzi}. intensity 0 (OT=range만 써 무관).""" + c = sqlite3.connect(db_path) + out = {} + try: + cur = c.cursor() + cur.execute("SELECT id, scan FROM Data WHERE scan IS NOT NULL AND length(scan)>0 ORDER BY id") + for nid, blob in cur.fetchall(): + raw = zlib.decompress(blob) + nf = len(raw) // 4 + if nf % 3 == 0: + pts = np.frombuffer(raw, dtype=np.float32, count=nf).reshape(-1, 3) + elif nf % 4 == 0: + pts = np.frombuffer(raw, dtype=np.float32, count=nf).reshape(-1, 4)[:, :3] + else: + print(f"[db2map] 경고: node {nid} scan 포맷 불명({nf} floats) — 스킵") + continue + xyzi = np.zeros((pts.shape[0], 4), dtype=np.float32) + xyzi[:, :3] = pts + out[int(nid)] = xyzi + finally: + c.close() + return out + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument('ckpt') + ap.add_argument('db', help='선택된 RTABMap .db (3D slam_map)') + ap.add_argument('out', help='출력 map.npz') + ap.add_argument('--proj_H', type=int, default=64) + ap.add_argument('--proj_W', type=int, default=900) + ap.add_argument('--fov_up', type=float, default=3.0) + ap.add_argument('--fov_down', type=float, default=-25.0) + ap.add_argument('--max_range', type=float, default=50.0) + ap.add_argument('--min_range', type=float, default=1.0) + ap.add_argument('--device', default='auto') # auto: cuda 가용 시 GPU, 아니면 CPU (08-gpu-cpu-runtime N3/N4) + a = ap.parse_args() + + poses = load_opt_poses(a.db) + scans = load_scans(a.db) + ids = sorted(set(poses) & set(scans)) + print(f"[db2map] opt_poses={len(poses)} scans={len(scans)} aligned={len(ids)}") + if not ids: + raise SystemExit("[db2map] 에러: pose∩scan 교집합 0 — 손상된 .db") + + try: + be = OTBackend(a.ckpt, device=a.device, proj_H=a.proj_H, proj_W=a.proj_W, fov_up=a.fov_up, + fov_down=a.fov_down, max_range=a.max_range, min_range=a.min_range) + except Exception as e: + if a.device == 'cpu' or not _is_cuda_error(e): + raise + print(f"[db2map] 경고: GPU 백엔드 초기화 실패({e}) → CPU 폴백") + be = OTBackend(a.ckpt, device='cpu', proj_H=a.proj_H, proj_W=a.proj_W, fov_up=a.fov_up, + fov_down=a.fov_down, max_range=a.max_range, min_range=a.min_range) + descs = np.stack([be.describe(scans[i]) for i in ids]).astype(np.float32) + pose_arr = np.stack([poses[i] for i in ids]).astype(np.float32) # (M,12) 3x4 row-major + os.makedirs(os.path.dirname(a.out), exist_ok=True) + np.savez(a.out, descriptors=descs, poses=pose_arr) + print(f"[db2map] built {a.out} desc={descs.shape} poses={pose_arr.shape} " + f"(fov_up={a.fov_up} fov_down={a.fov_down})") + + +if __name__ == '__main__': + main() diff --git a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/descriptor_db.py b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/descriptor_db.py new file mode 100755 index 0000000..7a6f0f9 --- /dev/null +++ b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/descriptor_db.py @@ -0,0 +1,33 @@ +"""맵 descriptor DB 빌드/검색 (spec §7). 맵 시퀀스(velodyne+poses) → descriptor 행렬 + pose 배열. +검색: 현재 descriptor 와 L2 최근접 → matched 프레임 idx → 그 프레임 pose. faiss-cpu.""" +import os +import numpy as np + + +def build_db(backend, scan_dir, poses_path, out_npz): + """scan_dir/*.bin 전부 describe → (M,D) descriptors + (M,12) poses → npz 저장.""" + bins = sorted(f for f in os.listdir(scan_dir) if f.endswith('.bin')) + descs = [] + for b in bins: + pts = np.fromfile(os.path.join(scan_dir, b), dtype=np.float32).reshape(-1, 4) + descs.append(backend.describe(pts)) + descs = np.stack(descs).astype(np.float32) + poses = np.loadtxt(poses_path).reshape(-1, 12).astype(np.float32) + os.makedirs(os.path.dirname(out_npz), exist_ok=True) + np.savez(out_npz, descriptors=descs, poses=poses) + return descs.shape + + +class DescriptorDB: + def __init__(self, npz_path): + import faiss + z = np.load(npz_path) + self.descs = z['descriptors'].astype(np.float32) + self.poses = z['poses'] + self.index = faiss.IndexFlatL2(self.descs.shape[1]) + self.index.add(self.descs) + + def query(self, descriptor, topk=5): + """descriptor (D,) → (best_idx, best_dist, topk_idxs). pose 는 self.poses[idx].""" + d, i = self.index.search(descriptor.reshape(1, -1), topk) + return int(i[0, 0]), float(d[0, 0]), i[0].tolist() diff --git a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/ot_backend.py b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/ot_backend.py new file mode 100755 index 0000000..a12d262 --- /dev/null +++ b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/ot_backend.py @@ -0,0 +1,43 @@ +"""OverlapTransformer descriptor 추출 backend (torch 격리, lazy import). +scan (N,4)[x,y,z,i] → range image → featureExtracter → global descriptor (D,). +salsanext_backend.py 의 구조 미러. ckpt 는 DataParallel 아님(NOTES §f) → module. strip 불필요.""" +import os +import sys +import numpy as np +from range_projection import RangeProjector + + +class OTBackend: + # 투영 기본값은 모델(OverlapTransformer)이 학습된 KITTI Velodyne 의 수직 FOV(≈+2°/−24.8°)에 + # 맞춘다. 이 값은 "센서"가 아니라 "모델"이 기대하는 입력 레이아웃이라, 잘못 주면(예: ±21.2°) + # 스캔이 range image 의 엉뚱한 행대에 뭉쳐 descriptor 가 붕괴한다(FR09 Ouster 16% 사례). + # 하드코딩 금지 — 호출부(ot_node gp / build_map_db CLI / ot.yaml SWRule)가 Rule 값으로 덮어쓴다. + def __init__(self, ckpt_path, device='cuda:0', proj_H=64, proj_W=900, + fov_up=3.0, fov_down=-25.0, max_range=50.0, min_range=1.0): + import torch + if device == 'auto': + device = 'cuda:0' if torch.cuda.is_available() else 'cpu' + ot_root = os.environ.get('OT_ROOT', '/opt/OverlapTransformer') + # overlap_transformer.py 가 모듈 로드 시 tools.read_samples → utils.utils 를 끌어와 + # ot_root 와 ot_root/tools 도 path 에 있어야 한다 (실 GPU 검증서 확인). modules 만으론 ImportError. + for _p in (ot_root, os.path.join(ot_root, 'tools'), os.path.join(ot_root, 'modules')): + if _p not in sys.path: + sys.path.insert(0, _p) + from overlap_transformer import featureExtracter # NOTES §a 모델 (descriptor dim=256) + self.proj = RangeProjector(proj_H, proj_W, fov_up, fov_down, max_range, min_range) + self.device = device + self._t = torch + model = featureExtracter(channels=1, use_transformer=True) + state = torch.load(ckpt_path, map_location='cpu') + model.load_state_dict(state.get('state_dict', state), strict=True) # module. 없음 + self._model = model.to(device).eval() + + def describe(self, points): + """(N,4) → (D,) float32 L2-정규화 descriptor.""" + pr = self.proj.project(points) + depth = pr.range_image[0:1].copy() # (1,H,W) range 채널만 (OT 입력=depth 1ch) + with self._t.no_grad(): + inp = self._t.from_numpy(depth).unsqueeze(0).to(self.device) # (1,1,H,W) + d = self._model(inp).cpu().numpy().reshape(-1).astype(np.float32) + n = np.linalg.norm(d) + 1e-9 + return d / n diff --git a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/ot_node.py b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/ot_node.py new file mode 100755 index 0000000..79e0e0b --- /dev/null +++ b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/ot_node.py @@ -0,0 +1,135 @@ +"""Place Recognition deploy 노드. /raw/lidar → descriptor → 맵 DB 최근접 → 현재 pose 추정 → +/perception/relocalization(PoseWithCovarianceStamped) + /initialpose(AMCL 씨딩, 접근 A) + +/perception/reloc_markers(★pose+top-k) 발행. (place_match_image 는 §8, best-effort 후속.)""" +import os +import sys +# colcon 설치된 entry point 로 실행되면 같은 패키지의 flat 모듈(ot_backend/descriptor_db/range_projection)이 +# top-level 로 안 잡혀 ModuleNotFoundError → 자기 디렉토리를 path 에 추가해 flat import 를 살린다. +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) +import numpy as np +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2 +from geometry_msgs.msg import PoseWithCovarianceStamped +from visualization_msgs.msg import Marker, MarkerArray +import sensor_msgs_py.point_cloud2 as pc2 +from ot_backend import OTBackend +from descriptor_db import DescriptorDB + + +def _is_cuda_error(e): + s = str(e).lower() + return 'cuda' in s or 'cudnn' in s or 'out of memory' in s + + +def _pose_from_row(row12): + T = np.array(row12, dtype=np.float64).reshape(3, 4) + return T[:, 3], T[:, :3] # translation, R + + +class PlaceRecognition(Node): + def __init__(self): + super().__init__('overlaptransformer') + gp = lambda n, d: self.declare_parameter(n, d).value + self.input_topic = gp('input_topic', '/raw/lidar') + ckpt = gp('model_ckpt_path', '') or os.environ.get('OT_MODEL_CKPT', '') + db_path = gp('map_db_path', '') or os.environ.get('OT_MAP_DB', '') + self.thresh = float(gp('match_dist_thresh', 1.0)) # L2 desc 거리 임계 + self.seed_once = bool(gp('seed_initialpose_once', True)) # /initialpose 1회만 씨딩(스트림 금지) + self._seeded = False + # range 투영 파라미터 — ot.yaml(SWRule config_mounts 로 override) 에서 읽는다. 기본값은 + # 모델 학습값(KITTI). 맵 DB(build_map_db)와 반드시 같은 값이어야 매칭이 맞음. + proj = dict( + proj_H=int(gp('proj_H', 64)), proj_W=int(gp('proj_W', 900)), + fov_up=float(gp('fov_up', 3.0)), fov_down=float(gp('fov_down', -25.0)), + max_range=float(gp('max_range', 50.0)), min_range=float(gp('min_range', 1.0)), + ) + # device: 'auto' → 런타임 감지(cuda 가용 시 cuda:0, 아니면 cpu). CPU-first dual-runtime(08 N3/N4). + # device 는 노드 파라미터(yaml/launch)로만 흐른다 — execute options/SWRule 로 노출 금지. + device = gp('device', 'auto') + self._ckpt, self._proj = ckpt, proj # 런타임 CPU 폴백 시 재생성용 + self._cpu_fallback = False # 런타임 폴백 1회 가드 + self.backend = None + if ckpt: + try: + self.backend = OTBackend(ckpt, device=device, **proj) + except Exception as e: + if device != 'cpu' and _is_cuda_error(e): + self.get_logger().warn(f"[overlaptransformer] GPU 백엔드 초기화 실패({e}) → CPU 폴백") + try: + self.backend = OTBackend(ckpt, device='cpu', **proj) + self._cpu_fallback = True + except Exception as e2: + self.get_logger().error(f"[overlaptransformer] CPU 폴백 초기화도 실패: {e2}") + else: + self.get_logger().error(f"[overlaptransformer] 백엔드 초기화 실패: {e}") + self.db = DescriptorDB(db_path) if db_path and os.path.exists(db_path) else None + self.pub_reloc = self.create_publisher(PoseWithCovarianceStamped, '/perception/relocalization', 5) + self.pub_init = self.create_publisher(PoseWithCovarianceStamped, '/initialpose', 1) + self.pub_mk = self.create_publisher(MarkerArray, '/perception/reloc_markers', 5) + self.create_subscription(PointCloud2, self.input_topic, self.cb, 5) + if self.backend is None or self.db is None: + self.get_logger().warn("model/DB 미설정 — relocalization 비활성(발행 안 함)") + + def cb(self, msg): + if self.backend is None or self.db is None: + return + pts = np.array([[p[0], p[1], p[2], 0.0] + for p in pc2.read_points(msg, ('x', 'y', 'z'), skip_nans=True)], + dtype=np.float32) # intensity 무관(OT=range만). 필터클라우드는 xyz only + if len(pts) == 0: + return + try: + desc = self.backend.describe(pts) + except Exception as e: + if self._cpu_fallback or not _is_cuda_error(e): + raise # CUDA 외 예외/폴백 후엔 기존 동작 유지 + self.get_logger().warn("[overlaptransformer] 추론 중 CUDA 에러 → CPU 백엔드 재구성") + self._cpu_fallback = True + try: # 깨진 CUDA 컨텍스트의 GPU 모델 해제 + bk, self.backend = self.backend, None + del bk + import torch + torch.cuda.empty_cache() + except Exception: + pass + try: # ckpt 에서 CPU 로 재생성 (.to 이동 금지) + self.backend = OTBackend(self._ckpt, device='cpu', **self._proj) + desc = self.backend.describe(pts) # 해당 프레임 1회 재시도 + except Exception as e2: + self.get_logger().error(f"[overlaptransformer] CPU 폴백 재시도 실패 — 프레임 스킵: {e2}") + return + idx, dist, topk = self.db.query(desc, topk=5) + if dist > self.thresh: + return # 신뢰도 낮으면 발행 안 함 + t, R = _pose_from_row(self.db.poses[idx]) + ps = PoseWithCovarianceStamped() + ps.header.frame_id = 'map' + ps.header.stamp = self.get_clock().now().to_msg() + ps.pose.pose.position.x, ps.pose.pose.position.y, ps.pose.pose.position.z = map(float, t) + qw = float(np.sqrt(max(0.0, 1.0 + R[0, 0] + R[1, 1] + R[2, 2])) / 2.0) or 1.0 + ps.pose.pose.orientation.w = qw + self.pub_reloc.publish(ps) + if not self.seed_once or not self._seeded: # 접근 A: 측위 씨딩 (1회만, 스트림 금지) + self.pub_init.publish(ps) + self._seeded = True + self._publish_markers(t, topk) + + def _publish_markers(self, t, topk): + ma = MarkerArray() + m = Marker() + m.header.frame_id = 'map'; m.ns = 'reloc'; m.id = 0 + m.type = Marker.ARROW; m.action = Marker.ADD + m.pose.position.x, m.pose.position.y = float(t[0]), float(t[1]) + m.scale.x, m.scale.y, m.scale.z = 2.0, 0.3, 0.3 + m.color.g = 1.0; m.color.a = 1.0 # ★ 초록 현재 위치 + ma.markers.append(m) + self.pub_mk.publish(ma) + + +def main(): + rclpy.init(); rclpy.spin(PlaceRecognition()); rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/range_projection.py b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/range_projection.py new file mode 100755 index 0000000..3ddda2e --- /dev/null +++ b/src/perception_overlaptransformer_pkg/perception_overlaptransformer_pkg/range_projection.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 +"""LiDAR spherical range-image projection (모델 무관, 순수 numpy). + +RangeNet++/SalsaNext 계열 표준 투영. 점별 (proj_x, proj_y) 를 보존해 +네트워크 픽셀 출력 → 점별 라벨 복원(un-projection)을 가능케 한다. +""" +from dataclasses import dataclass + +import numpy as np + + +@dataclass +class ProjectionResult: + range_image: np.ndarray # (5, H, W): range(depth), x, y, z, intensity (RangeNet/SalsaNext 순서) + px: np.ndarray # (V,) valid 점들의 열 index + py: np.ndarray # (V,) valid 점들의 행 index + valid: np.ndarray # (N,) bool — range 범위 안 점 마스크 + + +class RangeProjector: + def __init__(self, proj_H, proj_W, fov_up, fov_down, max_range, min_range): + self.H = int(proj_H) + self.W = int(proj_W) + self.fov_up = float(fov_up) / 180.0 * np.pi + self.fov_down = float(fov_down) / 180.0 * np.pi + self.fov = abs(self.fov_up) + abs(self.fov_down) + self.max_range = float(max_range) + self.min_range = float(min_range) + + def project(self, points): + xyz = points[:, :3] + intensity = points[:, 3] if points.shape[1] > 3 else np.zeros(len(points), np.float32) + depth = np.linalg.norm(xyz, axis=1) + valid = (depth > self.min_range) & (depth < self.max_range) + + x, y, z = xyz[:, 0], xyz[:, 1], xyz[:, 2] + yaw = -np.arctan2(y, x) + pitch = np.arcsin(np.clip(z / np.maximum(depth, 1e-6), -1.0, 1.0)) + + proj_x = 0.5 * (yaw / np.pi + 1.0) # [0,1] + proj_y = 1.0 - (pitch + abs(self.fov_down)) / self.fov + proj_x = np.clip(np.floor(proj_x * self.W), 0, self.W - 1).astype(np.int32) + proj_y = np.clip(np.floor(proj_y * self.H), 0, self.H - 1).astype(np.int32) + + img = np.zeros((5, self.H, self.W), dtype=np.float32) + # 가까운 점 우선(겹치면 작은 depth 가 픽셀 차지) — depth 내림차순으로 채움 + order = np.argsort(depth)[::-1] + vi = np.where(valid)[0] + # depth 내림차순 중 valid 만 채워 마지막에 가까운 점이 덮어쓰게 + fill = order[np.isin(order, vi)] + py, px = proj_y[fill], proj_x[fill] + img[0, py, px] = depth[fill] # range (SalsaNext img_means[0]=12.12) + img[1, py, px] = x[fill] + img[2, py, px] = y[fill] + img[3, py, px] = z[fill] + img[4, py, px] = intensity[fill] + + return ProjectionResult( + range_image=img, + px=proj_x[valid], py=proj_y[valid], valid=valid, + ) + + def unproject(self, pixel_labels, proj: ProjectionResult): + """pixel_labels (H,W) → 점별 라벨 (N,). valid 아닌 점은 0(unlabeled).""" + n = proj.valid.shape[0] + labels = np.zeros(n, dtype=np.int64) + labels[proj.valid] = pixel_labels[proj.py, proj.px] + return labels diff --git a/src/perception_overlaptransformer_pkg/resource/perception_overlaptransformer_pkg b/src/perception_overlaptransformer_pkg/resource/perception_overlaptransformer_pkg new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_overlaptransformer_pkg/setup.cfg b/src/perception_overlaptransformer_pkg/setup.cfg new file mode 100755 index 0000000..5820b7a --- /dev/null +++ b/src/perception_overlaptransformer_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/perception_overlaptransformer_pkg +[install] +install_scripts=$base/lib/perception_overlaptransformer_pkg diff --git a/src/perception_overlaptransformer_pkg/setup.py b/src/perception_overlaptransformer_pkg/setup.py new file mode 100755 index 0000000..1657da6 --- /dev/null +++ b/src/perception_overlaptransformer_pkg/setup.py @@ -0,0 +1,35 @@ + +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'perception_overlaptransformer_pkg' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='dgist', + maintainer_email='jang1future@gmail.com', + description='LiDAR place recognition (OverlapTransformer) perception module', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'overlaptransformer = perception_overlaptransformer_pkg.ot_node:main' + ], + }, +) diff --git a/src/perception_overlaptransformer_pkg/test/test_copyright.py b/src/perception_overlaptransformer_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/src/perception_overlaptransformer_pkg/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/perception_overlaptransformer_pkg/test/test_flake8.py b/src/perception_overlaptransformer_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/src/perception_overlaptransformer_pkg/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/perception_overlaptransformer_pkg/test/test_pep257.py b/src/perception_overlaptransformer_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/src/perception_overlaptransformer_pkg/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/perception_overlaptransformer_pkg/test/test_range_projection.py b/src/perception_overlaptransformer_pkg/test/test_range_projection.py new file mode 100755 index 0000000..ce0f62c --- /dev/null +++ b/src/perception_overlaptransformer_pkg/test/test_range_projection.py @@ -0,0 +1,29 @@ +import numpy as np +from perception_overlaptransformer_pkg.range_projection import RangeProjector + + +def test_unproject_recovers_per_point_labels(): + # 정면 부채꼴에 흩뿌린 점들 — projection 후 픽셀 라벨을 도로(0)/객체(1)로 칠하고 + # un-projection 이 점별 라벨을 되돌리는지 (충돌 없는 sparse 케이스). + rng = np.random.default_rng(0) + n = 500 + xyz = rng.uniform(-20, 20, size=(n, 3)).astype(np.float32) + xyz[:, 2] = rng.uniform(-2, 1, size=n) # 적당한 높이 + pts = np.concatenate([xyz, np.zeros((n, 1), np.float32)], axis=1) + + proj = RangeProjector(proj_H=64, proj_W=1024, fov_up=3.0, fov_down=-25.0, + max_range=50.0, min_range=1.0) + out = proj.project(pts) + assert out.range_image.shape == (5, 64, 1024) + assert out.px.shape[0] == out.valid.sum() + + # 픽셀 라벨 = (depth>10 ? 1 : 0) 으로 합성 → 점별로 같은 규칙이 복원돼야 함 + depth_img = out.range_image[0] + pixel_labels = (depth_img > 10.0).astype(np.int64) + labels = proj.unproject(pixel_labels, out) + assert labels.shape == (n,) + # valid 점들에 대해 점 depth 기준 기대 라벨과 일치 + vp = pts[out.valid][:, :3] + vdepth = np.linalg.norm(vp, axis=1) + expected = (vdepth > 10.0).astype(np.int64) + assert (labels[out.valid] == expected).mean() > 0.98 diff --git a/src/perception_pkg/config/odom.yaml b/src/perception_pkg/config/odom.yaml new file mode 100755 index 0000000..0e90054 --- /dev/null +++ b/src/perception_pkg/config/odom.yaml @@ -0,0 +1,10 @@ + +odometry: + ros__parameters: + use_sim_time: true + speed_topic: /raw/speed + imu_topic: /raw/imu + clock_topic: /clock + odom_topic: /odom_custom + odom_frame: odom + base_frame: base_link diff --git a/src/perception_pkg/config/tf_fr09.yaml b/src/perception_pkg/config/tf_fr09.yaml new file mode 100755 index 0000000..048c12c --- /dev/null +++ b/src/perception_pkg/config/tf_fr09.yaml @@ -0,0 +1,20 @@ + +lidar: + x: 0.456 + y: 0.0 + z: 1.135 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + frame_id: base_link + child_frame_id: os_lidar + +imu: # 확인 필요. + x: 0.0 + y: 0.0 + z: 0.5 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + frame_id: base_link + child_frame_id: os_imu diff --git a/src/perception_pkg/config/tf_mkmini.yaml b/src/perception_pkg/config/tf_mkmini.yaml new file mode 100755 index 0000000..094e818 --- /dev/null +++ b/src/perception_pkg/config/tf_mkmini.yaml @@ -0,0 +1,20 @@ + +lidar: + x: 0.1 + y: 0.0 + z: 0.6 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + frame_id: base_link + child_frame_id: os_lidar + +imu: # lidar의 imu 활용... + x: 0.1 + y: 0.0 + z: 0.6 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + frame_id: base_link + child_frame_id: os_imu diff --git a/src/perception_pkg/launch/perception.launch.py b/src/perception_pkg/launch/perception.launch.py new file mode 100755 index 0000000..bff03f0 --- /dev/null +++ b/src/perception_pkg/launch/perception.launch.py @@ -0,0 +1,54 @@ + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node + + +def generate_launch_description(): + sim_mode_arg = DeclareLaunchArgument('sim_mode', default_value='true') + sim_mode = LaunchConfiguration('sim_mode') + + # Directories + pkg_perception = get_package_share_directory( + 'perception_pkg') + + params_file = PathJoinSubstitution([ + pkg_perception, + 'config', 'odom.yaml']) + + use_sim_time_param = {'use_sim_time': sim_mode} + + load_nodes = GroupAction( + actions=[ + Node( + package='perception_pkg', + executable='perception_manager', + name='perception_manager', + output='screen', + parameters=[use_sim_time_param], + ), + Node( + package='perception_pkg', + executable='pub_static_tf', + name='pub_static_tf', + output='screen', + parameters=[use_sim_time_param], + ), + Node( # 현재 imu와 speed를 이용한 odom만 발행. 추후 vehicle steering + package='perception_pkg', + executable='odometry', + name='odometry', + output='screen', + parameters=[params_file, use_sim_time_param], + ), + ], + ) + + # Create launch description and add actions + ld = LaunchDescription([sim_mode_arg]) + ld.add_action(load_nodes) + + return ld diff --git a/src/perception_pkg/package.xml b/src/perception_pkg/package.xml new file mode 100755 index 0000000..608e576 --- /dev/null +++ b/src/perception_pkg/package.xml @@ -0,0 +1,18 @@ + + + + perception_pkg + 0.0.0 + TODO: Package description + dgist + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception_pkg/perception_pkg/__init__.py b/src/perception_pkg/perception_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_pkg/perception_pkg/launch_spec.py b/src/perception_pkg/perception_pkg/launch_spec.py new file mode 100755 index 0000000..9936cd5 --- /dev/null +++ b/src/perception_pkg/perception_pkg/launch_spec.py @@ -0,0 +1,20 @@ +"""순수 함수: /vehicle_config dict -> ros2 launch 명령 리스트. rclpy 의존 없음(단위테스트 가능).""" + + +def build_perception_commands(config): + cmds = [] + level = config.get("level") + robot = config.get("robot", "") + if level and level != "None": + cmds.append(["ros2", "launch", f"perception{level}_pkg", + "perception.launch.py", f"robot:={robot}"]) + for entry in config.get("perception") or []: + package = entry.get("package") + if not package: + continue + launch_file = entry.get("launch", "perception.launch.py") + cmd = ["ros2", "launch", package, launch_file] + for key, value in (entry.get("args") or {}).items(): + cmd.append(f"{key}:={value}") + cmds.append(cmd) + return cmds diff --git a/src/perception_pkg/perception_pkg/odometry.py b/src/perception_pkg/perception_pkg/odometry.py new file mode 100755 index 0000000..dca58c3 --- /dev/null +++ b/src/perception_pkg/perception_pkg/odometry.py @@ -0,0 +1,225 @@ +#!/usr/bin/env python3 + +import math +from typing import Optional + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float32 +from sensor_msgs.msg import Imu +from rosgraph_msgs.msg import Clock +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Quaternion, TransformStamped +from tf2_ros import TransformBroadcaster + + +# 좌표 변환 유틸 + +def quat_to_yaw(x: float, y: float, z: float, w: float) -> float: + """쿼터니언 → yaw(rad) 변환""" + siny_cosp = 2.0 * (w * z + x * y) + cosy_cosp = 1.0 - 2.0 * (y * y + z * z) + return math.atan2(siny_cosp, cosy_cosp) + + +def yaw_to_quaternion(yaw: float) -> Quaternion: + """yaw(rad) → 쿼터니언 변환 (2D 평면 운동 가정, roll/pitch = 0)""" + q = Quaternion() + q.x = 0.0 + q.y = 0.0 + q.z = math.sin(yaw * 0.5) + q.w = math.cos(yaw * 0.5) + return q + + +def normalize_angle(angle: float) -> float: + """각도를 [-π, π] 범위로 정규화""" + while angle > math.pi: + angle -= 2.0 * math.pi + while angle < -math.pi: + angle += 2.0 * math.pi + return angle + +def stamp_to_sec(stamp) -> float: + return float(stamp.sec) + float(stamp.nanosec) * 1e-9 + +# 메인 노드 + +class SpeedImuOdomNode(Node): + def __init__(self): + super().__init__('speed_imu_odom_node') + + # 파라미터 선언 및 로드 + self.declare_parameter('speed_topic', '/hero/speedometer') + self.declare_parameter('imu_topic', '/hero/imu') + self.declare_parameter('clock_topic', '/clock') + self.declare_parameter('odom_topic', '/hero/wheel_odom') + self.declare_parameter('odom_frame', 'odom') + self.declare_parameter('base_frame', 'base_link') + + self.speed_topic = self.get_parameter('speed_topic').value + self.imu_topic = self.get_parameter('imu_topic').value + self.clock_topic = self.get_parameter('clock_topic').value + self.odom_topic = self.get_parameter('odom_topic').value + self.odom_frame = self.get_parameter('odom_frame').value + self.base_frame = self.get_parameter('base_frame').value + + # 내부 상태 변수 + self.speed_mps: float = 0.0 # 현재 속도 (m/s) + self.yaw: float = 0.0 + self.last_imu_sec: Optional[float] = None + self.last_time: Optional[float] = None + + self.max_dt = 0.5 + + self.x = 0.0 # odom 프레임 기준 x 위치 (m) + self.y = 0.0 # odom 프레임 기준 y 위치 (m) + self.angular_velocity_z: Optional[float] = None # 현재 각속도 (rad/s) + + self.has_speed = False + self.has_imu = False + + # Publisher / Subscriber + self.odom_pub = self.create_publisher(Odometry, self.odom_topic, 10) + self.tf_broadcaster = TransformBroadcaster(self) + + self.create_subscription(Float32, self.speed_topic, self.speed_cb, 10) + self.create_subscription(Imu, self.imu_topic, self.imu_cb, 50) + self.create_subscription(Clock, self.clock_topic, self.clock_cb, 100) + + self.get_logger().info('SpeedImuOdomNode started') + self.get_logger().info(f'speed_topic: {self.speed_topic}') + self.get_logger().info(f'imu_topic: {self.imu_topic}') + self.get_logger().info(f'clock_topic: {self.clock_topic}') + self.get_logger().info(f'odom_topic: {self.odom_topic}') + + def speed_cb(self, msg: Float32): + self.speed_mps = float(msg.data) + self.has_speed = True + + def imu_cb(self, msg: Imu): + self.last_imu_msg = msg + now_sec = stamp_to_sec(msg.header.stamp) + + if self.last_imu_sec is None: + self.last_imu_sec = now_sec + self.has_imu = True + return + + dt = now_sec - self.last_imu_sec + if dt <= 0.0: + self.has_imu = True + return + + if dt > self.max_dt: + dt = self.max_dt + + self.angular_velocity_z = float(msg.angular_velocity.z) # 현재 imu의 각속도만 참고. 후에 다른 센서 데이터 이용시 추가 필요. + yaw = self.yaw + self.angular_velocity_z * dt + self.yaw = normalize_angle(yaw) + self.last_imu_sec = now_sec + self.has_imu = True + pass + + def clock_cb(self, msg: Clock): + if not self.has_speed or not self.has_imu: + return + + sim_time = float(msg.clock.sec) + float(msg.clock.nanosec) * 1e-9 + self.last_clock_msg = msg + + if self.last_time is None: + self.last_time = sim_time + return + + dt = sim_time - self.last_time + if dt <= 0.0: + return + + # 너무 긴 공백 구간은 적분 오차 방지를 위해 skip + if dt > self.max_dt: + self.last_time = sim_time + return + + self.last_time = sim_time + + yaw = self.yaw + v = self.speed_mps + + # 속도와 yaw로 x, y 위치 적분 (오일러 적분) + self.x += v * math.cos(yaw) * dt + self.y += v * math.sin(yaw) * dt + + if self.angular_velocity_z is not None: + self.publish_odom(msg.clock, yaw, v, self.angular_velocity_z) + + def publish_odom(self, stamp, yaw: float, v: float, omega: float): + """ + Odometry 메시지 발행 및 TF(odom → base_link) broadcast + - covariance: x/y/yaw에만 유효한 값, 나머지(z, roll, pitch)는 999로 무효 처리 + """ + odom = Odometry() + odom.header.stamp = stamp + odom.header.frame_id = self.odom_frame + odom.child_frame_id = self.base_frame + + odom.pose.pose.position.x = self.x + odom.pose.pose.position.y = self.y + odom.pose.pose.position.z = 0.0 + odom.pose.pose.orientation = yaw_to_quaternion(yaw) + + odom.twist.twist.linear.x = v + odom.twist.twist.linear.y = 0.0 + odom.twist.twist.linear.z = 0.0 + odom.twist.twist.angular.x = 0.0 + odom.twist.twist.angular.y = 0.0 + odom.twist.twist.angular.z = omega + + # 6x6 covariance (x, y, z, roll, pitch, yaw 순서) + # z/roll/pitch는 2D 주행에서 의미 없으므로 999(매우 큰 불확실도)로 설정 + odom.pose.covariance = [ + 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 999.0,0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 999.0,0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 999.0,0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.02 + ] + + odom.twist.covariance = [ + 0.02, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 999.0,0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 999.0,0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 999.0,0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.05 + ] + + self.odom_pub.publish(odom) + + tf_msg = TransformStamped() + tf_msg.header.stamp = stamp + tf_msg.header.frame_id = self.odom_frame + tf_msg.child_frame_id = self.base_frame + tf_msg.transform.translation.x = self.x + tf_msg.transform.translation.y = self.y + tf_msg.transform.translation.z = 0.0 + tf_msg.transform.rotation = yaw_to_quaternion(yaw) + self.tf_broadcaster.sendTransform(tf_msg) + + +def main(args=None): + rclpy.init(args=args) + node = SpeedImuOdomNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception_pkg/perception_pkg/perception_manager.py b/src/perception_pkg/perception_pkg/perception_manager.py new file mode 100755 index 0000000..3c218d1 --- /dev/null +++ b/src/perception_pkg/perception_pkg/perception_manager.py @@ -0,0 +1,53 @@ + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy +from std_msgs.msg import String + +import subprocess +import json + +from perception_pkg.launch_spec import build_perception_commands + + +class PerceptionManager(Node): + def __init__(self): + super().__init__('perception_manager') + self.get_logger().info('Perception Manager node has been started.') + qos = QoSProfile(depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE) + self.processes = [] + self.create_subscription(String, '/vehicle_config', self.handle_config, qos_profile=qos) + + def handle_config(self, msg: String): + self.get_logger().info(f"Perception Received config: {msg.data}") + config_data = json.loads(msg.data) + commands = build_perception_commands(config_data) + for cmd in commands: + self.get_logger().info(f"Launching: {' '.join(cmd)}") + self.processes.append(subprocess.Popen(cmd)) + + def destroy_node(self): + for proc in self.processes: + proc.terminate() + for proc in self.processes: + proc.wait() + self.processes = [] + return super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = PerceptionManager() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception_pkg/perception_pkg/pub_static_tf.py b/src/perception_pkg/perception_pkg/pub_static_tf.py new file mode 100755 index 0000000..ab05591 --- /dev/null +++ b/src/perception_pkg/perception_pkg/pub_static_tf.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy +from std_msgs.msg import String +from geometry_msgs.msg import TransformStamped +from tf2_ros import StaticTransformBroadcaster +from tf_transformations import quaternion_from_euler +from ament_index_python.packages import get_package_share_directory + +import os +import yaml +import json + +class StaticTFPublisher(Node): + def __init__(self): + super().__init__('static_tf_publisher') + + qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE + ) + self.create_subscription(String, '/vehicle_config', self.handle_config, qos_profile=qos) + + self.broadcaster = StaticTransformBroadcaster(self) + pass + + def handle_config(self, msg:String): + self.get_logger().info(f"Received config: {msg.data}") + config_data = json.loads(msg.data) + + config_path = os.path.join(get_package_share_directory('perception_pkg'), + 'config', f'tf_{config_data.get("robot")}.yaml') + + transforms = self.load_transforms(config_path) + self.broadcaster.sendTransform(transforms) + pass + + def load_transforms(self, yaml_path): + with open(yaml_path, 'r') as f: + data = yaml.safe_load(f) + + transforms = [] + + for name, cfg in data.items(): + msg = TransformStamped() + + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = cfg['frame_id'] + msg.child_frame_id = cfg['child_frame_id'] + + msg.transform.translation.x = float(cfg.get('x', 0.0)) + msg.transform.translation.y = float(cfg.get('y', 0.0)) + msg.transform.translation.z = float(cfg.get('z', 0.0)) + + roll = float(cfg.get('roll', 0.0)) + pitch = float(cfg.get('pitch', 0.0)) + yaw = float(cfg.get('yaw', 0.0)) + + q = quaternion_from_euler(roll, pitch, yaw) + + msg.transform.rotation.x = q[0] + msg.transform.rotation.y = q[1] + msg.transform.rotation.z = q[2] + msg.transform.rotation.w = q[3] + + transforms.append(msg) + + self.get_logger().info( + f'{name}: {msg.header.frame_id} -> {msg.child_frame_id}' + ) + + return transforms + + +def main(args=None): + rclpy.init(args=args) + + node = StaticTFPublisher() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/perception_pkg/perception_pkg/test.py b/src/perception_pkg/perception_pkg/test.py new file mode 100755 index 0000000..183e0e7 --- /dev/null +++ b/src/perception_pkg/perception_pkg/test.py @@ -0,0 +1,6 @@ +def main(): + print('Hi from perception_pkg.') + + +if __name__ == '__main__': + main() diff --git a/src/perception_pkg/resource/perception_pkg b/src/perception_pkg/resource/perception_pkg new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_pkg/setup.cfg b/src/perception_pkg/setup.cfg new file mode 100755 index 0000000..09ef227 --- /dev/null +++ b/src/perception_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/perception_pkg +[install] +install_scripts=$base/lib/perception_pkg diff --git a/src/perception_pkg/setup.py b/src/perception_pkg/setup.py new file mode 100755 index 0000000..b3c09fc --- /dev/null +++ b/src/perception_pkg/setup.py @@ -0,0 +1,38 @@ + +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'perception_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='TODO: Package description', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'odometry = perception_pkg.odometry:main', + 'perception_manager = perception_pkg.perception_manager:main', + 'pub_static_tf = perception_pkg.pub_static_tf:main', + 'test = perception_pkg.test:main' + ], + }, +) diff --git a/src/perception_pkg/test/test_copyright.py b/src/perception_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/src/perception_pkg/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/perception_pkg/test/test_flake8.py b/src/perception_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/src/perception_pkg/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/perception_pkg/test/test_launch_spec.py b/src/perception_pkg/test/test_launch_spec.py new file mode 100755 index 0000000..109d30b --- /dev/null +++ b/src/perception_pkg/test/test_launch_spec.py @@ -0,0 +1,39 @@ +from perception_pkg.launch_spec import build_perception_commands + + +def test_level_only(): + cmds = build_perception_commands({"level": "3d", "robot": "mkmini"}) + assert ["ros2", "launch", "perception3d_pkg", "perception.launch.py", "robot:=mkmini"] in cmds + assert len(cmds) == 1 + + +def test_level_none_skipped(): + assert build_perception_commands({"level": "None", "robot": "mkmini"}) == [] + + +def test_perception_entries_appended(): + # LocalLauncher buildPerceptionArg 실제 컨벤션: perception__pkg + + # perception.launch.py 진입점 + robot/use_sim_time/model_source args + cmds = build_perception_commands({ + "level": "None", "robot": "mkmini", + "perception": [{"category": "object_detection", + "package": "perception_pointpillars_pkg", + "launch": "perception.launch.py", + "args": {"robot": "mkmini", "use_sim_time": "true", + "model_source": "finetuned"}}], + }) + assert ["ros2", "launch", "perception_pointpillars_pkg", "perception.launch.py", + "robot:=mkmini", "use_sim_time:=true", "model_source:=finetuned"] in cmds + assert len(cmds) == 1 + + +def test_entry_without_package_skipped(): + assert build_perception_commands({"level": "None", "perception": [{"category": "x"}]}) == [] + + +def test_level_and_perception_combined(): + cmds = build_perception_commands({ + "level": "3d", "robot": "mkmini", + "perception": [{"package": "perception_pointpillars_pkg", "args": {}}], + }) + assert len(cmds) == 2 diff --git a/src/perception_pkg/test/test_pep257.py b/src/perception_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/src/perception_pkg/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/perception_pointpillars_pkg/cfgs/custom_dataset.yaml b/src/perception_pointpillars_pkg/cfgs/custom_dataset.yaml new file mode 100755 index 0000000..06ef0a7 --- /dev/null +++ b/src/perception_pointpillars_pkg/cfgs/custom_dataset.yaml @@ -0,0 +1,58 @@ +DATASET: 'CustomDataset' +DATA_PATH: '/perception_data/data' +# 우리 FR09 라이다(360°) → 대칭 range. z[-2,4] 1칸 pillar. x/y 140.8/0.16=880 (16배수) +POINT_CLOUD_RANGE: [-51.2, -51.2, -2, 51.2, 51.2, 4] +MAP_CLASS_TO_KITTI: { + 'Car': 'Car', + 'Pedestrian': 'Pedestrian', + 'Cyclist': 'Cyclist', +} +DATA_SPLIT: { + 'train': train, + 'test': val +} +INFO_PATH: { + 'train': [custom_infos_train.pkl], + 'test': [custom_infos_val.pkl], +} +POINT_FEATURE_ENCODING: { + encoding_type: absolute_coordinates_encoding, + used_feature_list: ['x', 'y', 'z', 'intensity'], + src_feature_list: ['x', 'y', 'z', 'intensity'], +} +DATA_AUGMENTOR: + DISABLE_AUG_LIST: ['gt_sampling'] # 1차 smoke 안정화용. 검증 후 풀어 성능↑ + AUG_CONFIG_LIST: + - NAME: gt_sampling + USE_ROAD_PLANE: False + DB_INFO_PATH: + - custom_dbinfos_train.pkl + PREPARE: { + filter_by_min_points: ['Car:5', 'Pedestrian:5', 'Cyclist:5'], + } + SAMPLE_GROUPS: ['Car:15', 'Pedestrian:10', 'Cyclist:10'] + NUM_POINT_FEATURES: 4 + DATABASE_WITH_FAKELIDAR: False + REMOVE_EXTRA_WIDTH: [0.0, 0.0, 0.0] + LIMIT_WHOLE_SCENE: True + - NAME: random_world_flip + ALONG_AXIS_LIST: ['x', 'y'] + - NAME: random_world_rotation + WORLD_ROT_ANGLE: [-0.78539816, 0.78539816] + - NAME: random_world_scaling + WORLD_SCALE_RANGE: [0.95, 1.05] +DATA_PROCESSOR: + - NAME: mask_points_and_boxes_outside_range + REMOVE_OUTSIDE_BOXES: True + - NAME: shuffle_points + SHUFFLE_ENABLED: { + 'train': True, + 'test': False + } + - NAME: transform_points_to_voxels + VOXEL_SIZE: [0.16, 0.16, 6] + MAX_POINTS_PER_VOXEL: 32 + MAX_NUMBER_OF_VOXELS: { + 'train': 16000, + 'test': 40000 + } diff --git a/src/perception_pointpillars_pkg/cfgs/pointpillar_custom.yaml b/src/perception_pointpillars_pkg/cfgs/pointpillar_custom.yaml new file mode 100755 index 0000000..cb093b4 --- /dev/null +++ b/src/perception_pointpillars_pkg/cfgs/pointpillar_custom.yaml @@ -0,0 +1,119 @@ +CLASS_NAMES: ['Car', 'Pedestrian', 'Cyclist'] + +DATA_CONFIG: + _BASE_CONFIG_: cfgs/dataset_configs/custom_dataset.yaml + +MODEL: + NAME: PointPillar + + VFE: + NAME: PillarVFE + WITH_DISTANCE: False + USE_ABSLOTE_XYZ: True + USE_NORM: True + NUM_FILTERS: [64] + + MAP_TO_BEV: + NAME: PointPillarScatter + NUM_BEV_FEATURES: 64 + + BACKBONE_2D: + NAME: BaseBEVBackbone + LAYER_NUMS: [3, 5, 5] + LAYER_STRIDES: [2, 2, 2] + NUM_FILTERS: [64, 128, 256] + UPSAMPLE_STRIDES: [1, 2, 4] + NUM_UPSAMPLE_FILTERS: [128, 128, 128] + + DENSE_HEAD: + NAME: AnchorHeadSingle + CLASS_AGNOSTIC: False + + USE_DIRECTION_CLASSIFIER: True + DIR_OFFSET: 0.78539 + DIR_LIMIT_OFFSET: 0.0 + NUM_DIR_BINS: 2 + + # anchor_sizes 는 KITTI 와 동일(pretrained 전이 최대). bottom_heights 는 우리 라이다 + # 지면(z≈-1.1) 기준으로 조정. fine-tune 이 미세조정함. + ANCHOR_GENERATOR_CONFIG: [ + { + 'class_name': 'Car', + 'anchor_sizes': [[3.9, 1.6, 1.56]], + 'anchor_rotations': [0, 1.57], + 'anchor_bottom_heights': [-1.0], + 'align_center': False, + 'feature_map_stride': 2, + 'matched_threshold': 0.6, + 'unmatched_threshold': 0.45 + }, + { + 'class_name': 'Pedestrian', + 'anchor_sizes': [[0.8, 0.6, 1.73]], + 'anchor_rotations': [0, 1.57], + 'anchor_bottom_heights': [-1.0], + 'align_center': False, + 'feature_map_stride': 2, + 'matched_threshold': 0.5, + 'unmatched_threshold': 0.35 + }, + { + 'class_name': 'Cyclist', + 'anchor_sizes': [[1.76, 0.6, 1.73]], + 'anchor_rotations': [0, 1.57], + 'anchor_bottom_heights': [-1.0], + 'align_center': False, + 'feature_map_stride': 2, + 'matched_threshold': 0.5, + 'unmatched_threshold': 0.35 + } + ] + + TARGET_ASSIGNER_CONFIG: + NAME: AxisAlignedTargetAssigner + POS_FRACTION: -1.0 + SAMPLE_SIZE: 512 + NORM_BY_NUM_EXAMPLES: False + MATCH_HEIGHT: False + BOX_CODER: ResidualCoder + + LOSS_CONFIG: + LOSS_WEIGHTS: { + 'cls_weight': 1.0, + 'loc_weight': 2.0, + 'dir_weight': 0.2, + 'code_weights': [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + } + + POST_PROCESSING: + RECALL_THRESH_LIST: [0.3, 0.5, 0.7] + SCORE_THRESH: 0.1 + OUTPUT_RAW_SCORE: False + EVAL_METRIC: kitti + NMS_CONFIG: + MULTI_CLASSES_NMS: False + NMS_TYPE: nms_gpu + NMS_THRESH: 0.01 + NMS_PRE_MAXSIZE: 4096 + NMS_POST_MAXSIZE: 500 + +OPTIMIZATION: + BATCH_SIZE_PER_GPU: 2 + NUM_EPOCHS: 80 + + OPTIMIZER: adam_onecycle + LR: 0.003 + WEIGHT_DECAY: 0.01 + MOMENTUM: 0.9 + + MOMS: [0.95, 0.85] + PCT_START: 0.4 + DIV_FACTOR: 10 + DECAY_STEP_LIST: [35, 45] + LR_DECAY: 0.1 + LR_CLIP: 0.0000001 + + LR_WARMUP: False + WARMUP_EPOCH: 1 + + GRAD_NORM_CLIP: 10 diff --git a/src/perception_pointpillars_pkg/config/pointpillars.yaml b/src/perception_pointpillars_pkg/config/pointpillars.yaml new file mode 100755 index 0000000..ffdae27 --- /dev/null +++ b/src/perception_pointpillars_pkg/config/pointpillars.yaml @@ -0,0 +1,21 @@ + + +pointpillars_pointpillars: + ros__parameters: + input_topic: "/raw/lidar" + detection_topic: "/perception/detections" + marker_topic: "/perception/detection_markers" + # 모델 경로는 여기서 지정하거나, 비우면 환경변수 + # (POINTPILLARS_MODEL_CFG / POINTPILLARS_MODEL_CKPT)에서 읽는다. + model_cfg_path: "" + model_ckpt_path: "" + score_threshold: 0.2 + device: "auto" # auto = 런타임 감지(GPU 있으면 cuda, 없으면 cpu). Phase3 dual-runtime + # CARLA 라이다 보정 (KITTI 학습분포에 맞춤). 측정값 기반: + # 지면 z -1.06(CARLA) vs -1.73(KITTI), ~12k 점(KITTI 대비 희박). + z_offset: 1.0 # 입력 z 보정 (스윕상 max score 최적 ~+1.0) + max_range: 25.0 # 이 반경(m) 밖 박스 제거 (근~중거리 신뢰) + # 기하 검증 — 빈 공간/바닥 헛검출 제거 (당신 증상 직격) + min_points_in_box: 5 # 박스 안 점 5개 미만 → 버림 (검정영역 헛박스) + min_box_height: 0.5 # 박스 안 점 높이 0.5m 미만 → 버림 (평평한 도로 오검출) + use_sim_time: true diff --git a/src/perception_pointpillars_pkg/launch/__pycache__/pointpillars.launch.cpython-310.pyc b/src/perception_pointpillars_pkg/launch/__pycache__/pointpillars.launch.cpython-310.pyc new file mode 100644 index 0000000..22406c0 Binary files /dev/null and b/src/perception_pointpillars_pkg/launch/__pycache__/pointpillars.launch.cpython-310.pyc differ diff --git a/src/perception_pointpillars_pkg/launch/perception.launch.py b/src/perception_pointpillars_pkg/launch/perception.launch.py new file mode 100755 index 0000000..1329523 --- /dev/null +++ b/src/perception_pointpillars_pkg/launch/perception.launch.py @@ -0,0 +1,32 @@ +"""perception_manager 공통 진입점 (통합 perception 모듈). + +LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로 +`ros2 launch perception_pointpillars_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..` +를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(pointpillars.launch.py)로 위임한다. +robot 은 공통 계약상 선언만 하고, 모델 launch 가 선언한 경우에만 소비된다. +""" +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + share = get_package_share_directory('perception_pointpillars_pkg') + return LaunchDescription([ + DeclareLaunchArgument('robot', default_value='fr09'), + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('model_source', default_value='finetuned'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(share, 'launch', 'pointpillars.launch.py')), + launch_arguments={ + 'robot': LaunchConfiguration('robot'), + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'model_source': LaunchConfiguration('model_source'), + }.items(), + ), + ]) diff --git a/src/perception_pointpillars_pkg/launch/pointpillars.launch.py b/src/perception_pointpillars_pkg/launch/pointpillars.launch.py new file mode 100755 index 0000000..966c9f5 --- /dev/null +++ b/src/perception_pointpillars_pkg/launch/pointpillars.launch.py @@ -0,0 +1,131 @@ + +import os +import shutil + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import LogInfo +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node + + +# 모델 경로 기본값 — 자동 resolve 의 "명시 인자" 판별 기준이라 상수로 둔다. +# cfg/ckpt(pretrained) 둘 다 perception 통합 이미지에 내장됨 +# (/models 마운트는 공급자가 없어 폐기, /opt/pointpillar_pretrained 로 대체). +DEFAULT_MODEL_CFG = '/opt/OpenPCDet/tools/cfgs/kitti_models/pointpillar.yaml' +DEFAULT_MODEL_CKPT = '/opt/pointpillar_pretrained/pointpillar_7728.pth' +# 가중치 공유 단일 위치(docker/local): /perception_models/pointpillars/{finetuned,pretrained} +# (호스트 .caches/config/nav2_config/perception/models/pointpillars/*, SWRule config_mounts). +FINETUNED_CKPT = '/perception_models/pointpillars/finetuned/pointpillar_finetuned.pth' +SHARED_PRE_CKPT = '/perception_models/pointpillars/pretrained/pointpillar_7728.pth' # 공유 pretrained (Phase 2 채움) +# finetuned cfg 가 OpenPCDet 트리에 놓일 위치 (_BASE_CONFIG_ 상대참조 때문에 고정). +CUSTOM_DATASET_DST = '/opt/OpenPCDet/tools/cfgs/dataset_configs/custom_dataset.yaml' +CUSTOM_MODEL_DST = '/opt/OpenPCDet/tools/cfgs/custom_models/pointpillar_custom.yaml' + + +ARGUMENTS = [ + DeclareLaunchArgument('use_sim_time', default_value='true'), + # 입력 토픽(/raw/lidar)은 sensor_bridge 가 robot 무관하게 정규화하므로 + # robot 별 config 분기는 두지 않는다. robot arg 는 인터페이스 호환용. + DeclareLaunchArgument('robot', default_value='turtlebot3', choices=['turtlebot3', 'mkmini', 'fr09']), + # 모델 소스 스위치 — pretrained 고르면 finetuned 있어도 건너뜀(기본 finetuned). + DeclareLaunchArgument( + 'model_source', + default_value='finetuned', choices=['finetuned', 'pretrained']), + # 모델 경로 — Rule(launcher 의 ROS_PARAM)이 덮어쓸 수 있게 launch arg 로 노출. + # 기본값/빈값이면 아래 launch_setup 에서 model_source 기반 자동 resolve. + DeclareLaunchArgument('model_cfg_path', default_value=DEFAULT_MODEL_CFG), + DeclareLaunchArgument('model_ckpt_path', default_value=DEFAULT_MODEL_CKPT), +] + + +def launch_setup(context, *args, **kwargs): + """모델 cfg/ckpt resolve 후 노드 생성 (단독 deploy run.sh 의 finetuned 우선 로직 이식). + + - 사용자가 model_cfg_path/model_ckpt_path 를 명시적으로 줬으면(선언 기본값과 + 다르면) 그대로 쓴다 — 단독 deploy run.sh 가 MODEL_ARGS 로 명시 전달하는 + 기존 동작이 그대로 이긴다(무회귀). + - 인자가 기본값/빈값일 때만 model_source(기본 finetuned) 와 finetuned 파일 + 존재 여부로 자동 결정한다. + """ + use_sim_time = LaunchConfiguration('use_sim_time') + model_source = LaunchConfiguration('model_source').perform(context) + model_cfg_path = LaunchConfiguration('model_cfg_path').perform(context) + model_ckpt_path = LaunchConfiguration('model_ckpt_path').perform(context) + + pkg_pointpillars = get_package_share_directory('perception_pointpillars_pkg') + params_file = PathJoinSubstitution([pkg_pointpillars, 'config', 'pointpillars.yaml']) + + cfg_explicit = model_cfg_path not in ('', DEFAULT_MODEL_CFG) + ckpt_explicit = model_ckpt_path not in ('', DEFAULT_MODEL_CKPT) + + log_actions = [] + if cfg_explicit or ckpt_explicit: + # 명시 인자 우선 — resolve 건너뜀. + log_actions.append(LogInfo( + msg=f'[pointpillars] 명시 모델 인자 사용: cfg[{model_cfg_path}] ' + f'ckpt[{model_ckpt_path}]')) + elif model_source != 'pretrained' and os.path.isfile(FINETUNED_CKPT): + # 편입된 cfgs/ 를 OpenPCDet 트리로 복사 (pointpillar_custom.yaml 의 + # _BASE_CONFIG_ 가 cfgs/dataset_configs/ 상대경로를 참조하므로 필수). + shutil.copyfile( + os.path.join(pkg_pointpillars, 'cfgs', 'custom_dataset.yaml'), + CUSTOM_DATASET_DST) + os.makedirs(os.path.dirname(CUSTOM_MODEL_DST), exist_ok=True) + shutil.copyfile( + os.path.join(pkg_pointpillars, 'cfgs', 'pointpillar_custom.yaml'), + CUSTOM_MODEL_DST) + model_cfg_path = CUSTOM_MODEL_DST + model_ckpt_path = FINETUNED_CKPT + log_actions.append(LogInfo( + msg=f'[pointpillars] 재학습 모델 사용: {FINETUNED_CKPT}')) + elif os.path.isfile(SHARED_PRE_CKPT): + # 공유 pretrained(KITTI base) — baked(/opt) 보다 우선. cfg 는 base 와 동일. + model_cfg_path = model_cfg_path or DEFAULT_MODEL_CFG + model_ckpt_path = SHARED_PRE_CKPT + log_actions.append(LogInfo( + msg=f'[pointpillars] 공유 pretrained 사용: {SHARED_PRE_CKPT}')) + else: + # 빈값으로 들어온 경우도 기본 pretrained 로 보정. + model_cfg_path = model_cfg_path or DEFAULT_MODEL_CFG + model_ckpt_path = model_ckpt_path or DEFAULT_MODEL_CKPT + log_actions.append(LogInfo( + msg='[pointpillars] 재학습 모델 없음/미선택 → 기본 pretrained(baked) 사용')) + + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # nav2_common.RewrittenYaml 대신 yaml + dict override 로 단순화 + # (future-pointpillars 이미지는 ros-base 만 깔려 nav2_common 이 없음). + load_nodes = GroupAction( + actions=[ + Node( + package='perception_pointpillars_pkg', + executable='pointpillars', + name='pointpillars_pointpillars', + output='screen', + parameters=[ + params_file, + { + 'use_sim_time': use_sim_time, + 'model_cfg_path': model_cfg_path, + 'model_ckpt_path': model_ckpt_path, + }, + ], + remappings=remappings + ) + ], + ) + + return log_actions + [load_nodes] + + +def generate_launch_description(): + ld = LaunchDescription(ARGUMENTS) + ld.add_action(OpaqueFunction(function=launch_setup)) + + return ld diff --git a/src/perception_pointpillars_pkg/package.xml b/src/perception_pointpillars_pkg/package.xml new file mode 100755 index 0000000..2528b0f --- /dev/null +++ b/src/perception_pointpillars_pkg/package.xml @@ -0,0 +1,26 @@ + + + + perception_pointpillars_pkg + 0.0.0 + LiDAR 3D object detection (PointPillars / OpenPCDet) perception module + dgist + TODO: License declaration + + rclpy + sensor_msgs + sensor_msgs_py + vision_msgs + visualization_msgs + geometry_msgs + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception_pointpillars_pkg/perception_pointpillars_pkg/__init__.py b/src/perception_pointpillars_pkg/perception_pointpillars_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_pointpillars_pkg/perception_pointpillars_pkg/pointpillars_backend.py b/src/perception_pointpillars_pkg/perception_pointpillars_pkg/pointpillars_backend.py new file mode 100755 index 0000000..179946d --- /dev/null +++ b/src/perception_pointpillars_pkg/perception_pointpillars_pkg/pointpillars_backend.py @@ -0,0 +1,201 @@ +#!/usr/bin/env python3 +"""OpenPCDet PointPillars 추론 래퍼. + +ROS 의존이 없는 순수 추론 모듈. OpenPCDet(pcdet) / torch 는 이 클래스를 +인스턴스화할 때만 import 한다 → OpenPCDet 미설치 환경에서도 패키지 build/import 는 통과. +런타임(도커 이미지)에 OpenPCDet 가 설치돼 있어야 실제 추론이 동작한다. + +추론 백엔드를 노드에서 떼어내 격리했으므로, 다른 검출 모델로 교체 시 +이 파일만 갈아끼우면 된다(노드 인터페이스 불변). +""" + +import os + +import numpy as np + + +class PointPillarsBackend: + """OpenPCDet config + checkpoint 를 로드해 단일 프레임 포인트클라우드를 추론한다. + + infer(points) -> (boxes, scores, labels) + points : (N, 4) float32 — x, y, z, intensity + boxes : (M, 7) float32 — x, y, z, dx, dy, dz, heading(yaw, rad) + scores : (M,) float32 + labels : (M,) int — 1-based class index (class_names[label-1]) + """ + + def __init__(self, cfg_path, ckpt_path, score_threshold=0.3, device='auto', + z_offset=0.0, min_points_in_box=0, min_box_height=0.0, + logger=None): + # OpenPCDet / torch 는 여기서만 import (미설치 시 패키지 import 자체는 안 깨지게) + import torch + from pcdet.config import cfg, cfg_from_yaml_file + from pcdet.models import build_network, load_data_to_gpu + from pcdet.datasets import DatasetTemplate + from pcdet.utils import common_utils + + self._torch = torch + self._load_data_to_gpu = load_data_to_gpu + self.score_threshold = float(score_threshold) + # device='auto' → 런타임 감지: cuda 가용 시 cuda:0, 아니면 cpu (08-gpu-cpu-runtime N4) + if device == 'auto': + device = 'cuda:0' if torch.cuda.is_available() else 'cpu' + self.device = device + self._cpu = (device == 'cpu') + # CPU 런타임이면 OpenPCDet 의 CUDA 하드코딩(NMS + anchor 생성 .cuda())을 CPU 로 우회 + # (Phase3 dual-runtime). spconv 미사용 모델이라 이 두 패치로 CPU 완결. build_network 전에 적용. + if self._cpu: + self._patch_cpu() + # 보정: 입력 z 에 더하는 오프셋. 라이다 장착 높이/지면 z 가 KITTI 학습분포와 + # 어긋날 때 맞춘다 (CARLA 지면 ~ -1.06, KITTI ~ -1.73). 0 이면 비활성. + self.z_offset = float(z_offset) + # 기하 검증: 박스 안에 실제 점이 충분/입체적으로 있어야 진짜 객체로 인정. + # min_points_in_box : 박스 내부 최소 점 개수 (빈 공간 헛검출 제거) + # min_box_height : 박스 내부 점들의 z 스프레드 최소값(m) (평평한 바닥 오검출 제거) + self.min_points_in_box = int(min_points_in_box) + self.min_box_height = float(min_box_height) + + # OpenPCDet cfg 의 _BASE_CONFIG_ 는 tools/ 기준 상대경로라, 그 디렉토리에서 로드해야 + # dataset base config 가 해석된다. (cfg_path 자체는 절대경로 허용) + opcdet_tools = os.path.join( + os.environ.get('OPENPCDET_ROOT', '/opt/OpenPCDet'), 'tools') + prev_cwd = os.getcwd() + try: + if os.path.isdir(opcdet_tools): + os.chdir(opcdet_tools) + cfg_from_yaml_file(cfg_path, cfg) + finally: + os.chdir(prev_cwd) + self.class_names = list(cfg.CLASS_NAMES) + + log = logger or common_utils.create_logger() + + # 모델의 data_processor / point_feature_encoder 만 빌리기 위한 최소 dataset + self._dataset = DatasetTemplate( + dataset_cfg=cfg.DATA_CONFIG, + class_names=self.class_names, + training=False, + root_path=None, + logger=log, + ) + + self._model = build_network( + model_cfg=cfg.MODEL, + num_class=len(self.class_names), + dataset=self._dataset, + ) + self._model.load_params_from_file( + filename=ckpt_path, logger=log, to_cpu=(device == 'cpu')) + self._model.to(device) + self._model.eval() + + def infer(self, points): + torch = self._torch + points = np.ascontiguousarray(points, dtype=np.float32) + + if self.z_offset: + points = points.copy() + points[:, 2] += self.z_offset + + data_dict = self._dataset.prepare_data( + data_dict={'points': points, 'frame_id': 0}) + data_dict = self._dataset.collate_batch([data_dict]) + if self._cpu: + self._load_data_to_cpu(data_dict) + else: + self._load_data_to_gpu(data_dict) + + with torch.no_grad(): + pred_dicts, _ = self._model.forward(data_dict) + + boxes = pred_dicts[0]['pred_boxes'].cpu().numpy() + scores = pred_dicts[0]['pred_scores'].cpu().numpy() + labels = pred_dicts[0]['pred_labels'].cpu().numpy() + + keep = scores >= self.score_threshold + boxes, scores, labels = boxes[keep], scores[keep], labels[keep] + + # 기하 검증 — 박스 안 실제 점 개수/높이로 헛검출 제거 (points/boxes 동일 좌표계) + if (self.min_points_in_box > 0 or self.min_box_height > 0.0) and len(boxes): + valid = np.array([self._box_is_valid(points, b) for b in boxes], dtype=bool) + boxes, scores, labels = boxes[valid], scores[valid], labels[valid] + + # z_offset 만큼 올려서 추론했으므로 출력 박스 z 는 원래 라이다 좌표계로 되돌림 + if self.z_offset and len(boxes): + boxes = boxes.copy() + boxes[:, 2] -= self.z_offset + + return boxes, scores, labels + + def _box_is_valid(self, points, box): + """박스 내부 점 개수와 z 스프레드로 진짜 객체 여부 판정 (헛검출 컷).""" + cx, cy, cz, dx, dy, dz, yaw = box[:7] + rel = points[:, :3] - np.array([cx, cy, cz], dtype=np.float32) + c, s = np.cos(-yaw), np.sin(-yaw) + xr = rel[:, 0] * c - rel[:, 1] * s + yr = rel[:, 0] * s + rel[:, 1] * c + inside = (np.abs(xr) <= dx / 2) & (np.abs(yr) <= dy / 2) & \ + (np.abs(rel[:, 2]) <= dz / 2) + n = int(inside.sum()) + if n < self.min_points_in_box: + return False + if self.min_box_height > 0.0: + zin = points[inside, 2] + if n == 0 or float(zin.max() - zin.min()) < self.min_box_height: + return False + return True + + def _load_data_to_cpu(self, batch_dict): + """load_data_to_gpu 의 CPU 판: numpy 배열을 .cuda() 대신 cpu float tensor 로. + 라이다 전용 파이프라인이라 camera/image 키는 없음 → voxel/point 계열만 변환.""" + torch = self._torch + skip = ('frame_id', 'metadata', 'calib', 'image_paths', 'ori_shape', + 'img_process_infos') + for key, val in batch_dict.items(): + if not isinstance(val, np.ndarray) or key in skip: + continue + batch_dict[key] = torch.from_numpy(val).float() + + def _patch_cpu(self): + """CPU 런타임용 OpenPCDet CUDA 하드코딩 2종 우회 (device=='cpu' 일 때만, build 전 1회). + + (1) anchor 생성·NMS 모듈이 텐서에 박은 .cuda() → torch.Tensor.cuda 를 no-op(self 반환)로 + shim. forward 경로엔 torch.cuda.*Tensor 직접 alloc 이 없어 이 shim 으로 완결 + (anchor_generator/anchor_head_template 의 build 타임 .cuda() 3곳 커버). + (2) iou3d_nms_utils.nms_gpu(CUDA-only)를 동일결과 CPU greedy NMS 로 교체. 회전 BEV IoU 는 + OpenPCDet 가 같은 확장(iou3d_nms_cuda)에 함께 컴파일해 둔 boxes_bev_iou_cpu(C++ CPU op)를 + 그대로 호출 → GPU 판과 IoU 계산 동일(결과 동등).""" + torch = self._torch + + # (1) .cuda() no-op shim — GPU 없는 클라이언트 전제(애초에 cuda 못 부름)라 전역 안전 + if not getattr(torch.Tensor, '_cpu_cuda_noop', False): + torch.Tensor.cuda = lambda self, *a, **k: self + torch.Tensor._cpu_cuda_noop = True + + # (2) CUDA NMS → CPU NMS monkeypatch (모듈 객체에 1회, getattr 로 호출 시점 해석) + from pcdet.ops.iou3d_nms import iou3d_nms_utils as u + if getattr(u, '_cpu_nms_patched', False): + return + + def nms_cpu(boxes, scores, thresh, pre_maxsize=None, **kwargs): + assert boxes.shape[1] == 7 + order = scores.sort(0, descending=True)[1] + if pre_maxsize is not None: + order = order[:pre_maxsize] + boxes = boxes[order].contiguous() + if boxes.shape[0] == 0: + return order[:0].contiguous(), None + iou = u.boxes_bev_iou_cpu(boxes[:, 0:7], boxes[:, 0:7]) # (N,N) CPU + keep = [] + suppressed = torch.zeros(boxes.shape[0], dtype=torch.bool) + for i in range(boxes.shape[0]): + if suppressed[i]: + continue + keep.append(i) + suppressed |= iou[i] > thresh # 자신 포함 — 이미 append 후라 무해 + keep = torch.as_tensor(keep, dtype=torch.long) + return order[keep].contiguous(), None + + u.nms_gpu = nms_cpu + u.nms_normal_gpu = nms_cpu # objdet 기본 NMS_TYPE=nms_gpu, normal 경로도 crash 방지 + u._cpu_nms_patched = True diff --git a/src/perception_pointpillars_pkg/perception_pointpillars_pkg/pointpillars_node.py b/src/perception_pointpillars_pkg/perception_pointpillars_pkg/pointpillars_node.py new file mode 100755 index 0000000..8abd8e4 --- /dev/null +++ b/src/perception_pointpillars_pkg/perception_pointpillars_pkg/pointpillars_node.py @@ -0,0 +1,313 @@ +#!/usr/bin/env python3 + +import os +import math + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import PointCloud2 +import sensor_msgs_py.point_cloud2 as pc2 + +from vision_msgs.msg import Detection3DArray, Detection3D, ObjectHypothesisWithPose +from visualization_msgs.msg import Marker, MarkerArray +from std_msgs.msg import ColorRGBA + +import numpy as np + +from perception_pointpillars_pkg.pointpillars_backend import PointPillarsBackend + + +def _yaw_to_quat(yaw): + """z축 yaw(rad) -> (x, y, z, w) quaternion.""" + half = yaw * 0.5 + return 0.0, 0.0, math.sin(half), math.cos(half) + + +def _is_cuda_error(e): + s = str(e).lower() + return 'cuda' in s or 'cudnn' in s or 'out of memory' in s + + +class ObjectDetection(Node): + def __init__(self): + super().__init__('pointpillars_pointpillars') + + # 토픽 (구조적 기본값만 — pcd_crop 과 동일 패턴) + self.declare_parameter('input_topic', '/raw/lidar') + self.declare_parameter('detection_topic', '/perception/detections') + self.declare_parameter('marker_topic', '/perception/detection_markers') + + # 모델 경로/임계값 — 노드에 박지 않는다. config(yaml) 또는 환경변수에서 주입. + self.declare_parameter('model_cfg_path', '') + self.declare_parameter('model_ckpt_path', '') + self.declare_parameter('score_threshold', 0.3) + # device: 'auto' → 런타임 감지(cuda 가용 시 cuda:0, 아니면 cpu). Phase3 dual-runtime. + self.declare_parameter('device', 'auto') + + # 보정 파라미터 (Rule 로 조절) — 센서 도메인을 KITTI 학습분포에 맞춤. + # z_offset: 입력 z 보정(지면 높이 차). max_range: 이 반경(m) 밖 박스 버림 + # (라이다 밀도 떨어지는 원거리 false positive 제거). 0 이면 비활성. + self.declare_parameter('z_offset', 0.0) + self.declare_parameter('max_range', 0.0) + # 기하 검증 (빈 공간/바닥 헛검출 제거) — 박스 내부 점 개수/높이 최소값. 0 이면 비활성. + self.declare_parameter('min_points_in_box', 0) + self.declare_parameter('min_box_height', 0.0) + + input_topic = self.get_parameter('input_topic').value + detection_topic = self.get_parameter('detection_topic').value + marker_topic = self.get_parameter('marker_topic').value + + # config 가 비면 환경변수로 fallback (하드코딩 금지 룰) + cfg_path = self.get_parameter('model_cfg_path').value or os.environ.get( + 'POINTPILLARS_MODEL_CFG', '') + ckpt_path = self.get_parameter('model_ckpt_path').value or os.environ.get( + 'POINTPILLARS_MODEL_CKPT', '') + score_threshold = self.get_parameter('score_threshold').value + device = self.get_parameter('device').value + z_offset = self.get_parameter('z_offset').value + self.max_range = float(self.get_parameter('max_range').value) + min_points_in_box = self.get_parameter('min_points_in_box').value + min_box_height = self.get_parameter('min_box_height').value + + self.pub_det = self.create_publisher(Detection3DArray, detection_topic, 10) + self.pub_marker = self.create_publisher(MarkerArray, marker_topic, 10) + + # 런타임 CUDA 폴백 가드 + ckpt 에서 CPU 백엔드 재생성에 필요한 인자 보관 + self._cpu_fallback_done = False + self._backend_args = (cfg_path, ckpt_path, score_threshold, z_offset, + min_points_in_box, min_box_height) + + self.backend = self._init_backend( + cfg_path, ckpt_path, score_threshold, device, z_offset, + min_points_in_box, min_box_height) + + self.sub = self.create_subscription( + PointCloud2, input_topic, self.callback, 10) + + self.get_logger().info( + f"ObjDet(PointPillars) started: in[{input_topic}] " + f"det[{detection_topic}] marker[{marker_topic}] " + f"threshold[{score_threshold}] device[{device}] " + f"z_offset[{z_offset}] max_range[{self.max_range}]" + ) + + def _init_backend(self, cfg_path, ckpt_path, score_threshold, device, z_offset, + min_points_in_box, min_box_height): + if not cfg_path or not ckpt_path: + self.get_logger().error( + "model_cfg_path / model_ckpt_path 미지정 " + "(config 또는 POINTPILLARS_MODEL_CFG/POINTPILLARS_MODEL_CKPT 환경변수). " + "추론 비활성 — 입력 구독만 수행한다." + ) + return None + try: + backend = PointPillarsBackend( + cfg_path=cfg_path, ckpt_path=ckpt_path, + score_threshold=score_threshold, device=device, + z_offset=z_offset, min_points_in_box=min_points_in_box, + min_box_height=min_box_height, logger=None, + ) + self.get_logger().info( + f"OpenPCDet 로드 완료: classes={backend.class_names}") + return backend + except Exception as e: # OpenPCDet 미설치 / 가중치 로드 실패 등 + # GPU 계열 실패(OOM/cuDNN 등)면 CPU 로 1회 재생성 (08-gpu-cpu-runtime N3) + if device != 'cpu' and _is_cuda_error(e): + self.get_logger().warn( + f"[pointpillars] GPU 백엔드 초기화 실패({e}) → CPU 폴백") + try: + backend = PointPillarsBackend( + cfg_path=cfg_path, ckpt_path=ckpt_path, + score_threshold=score_threshold, device='cpu', + z_offset=z_offset, min_points_in_box=min_points_in_box, + min_box_height=min_box_height, logger=None, + ) + self.get_logger().info( + f"OpenPCDet 로드 완료(CPU 폴백): classes={backend.class_names}") + return backend + except Exception as e2: + self.get_logger().error(f"검출 백엔드 초기화 실패: {e2}") + return None + self.get_logger().error(f"검출 백엔드 초기화 실패: {e}") + return None + + def _read_points(self, msg: PointCloud2): + field_names = {f.name for f in msg.fields} + if 'intensity' in field_names: + pts = pc2.read_points( + msg, field_names=("x", "y", "z", "intensity"), skip_nans=True) + if pts.shape[0] == 0: + return None + return np.stack( + (pts["x"], pts["y"], pts["z"], pts["intensity"]), + axis=-1).astype(np.float32, copy=False) + # intensity 없는 라이다(예: CARLA) — 0 으로 채움 + pts = pc2.read_points( + msg, field_names=("x", "y", "z"), skip_nans=True) + if pts.shape[0] == 0: + return None + xyz = np.stack((pts["x"], pts["y"], pts["z"]), axis=-1).astype( + np.float32, copy=False) + intensity = np.zeros((xyz.shape[0], 1), dtype=np.float32) + return np.concatenate([xyz, intensity], axis=1) + + def callback(self, msg: PointCloud2): + if self.backend is None: + return + + points = self._read_points(msg) + if points is None: + return + + try: + boxes, scores, labels = self.backend.infer(points) + except Exception as e: + if self._cpu_fallback_done or not _is_cuda_error(e): + raise + self.get_logger().warn( + f"[pointpillars] 추론 중 CUDA 에러 → CPU 백엔드 재구성: {e}") + self._cpu_fallback_done = True + # 깨진 CUDA 컨텍스트의 GPU 모델 정리 — .to('cpu') 이동 금지, ckpt 재생성 + try: + torch = self.backend._torch + del self.backend._model + self.backend = None + torch.cuda.empty_cache() + except Exception: + self.backend = None + cfg_path, ckpt_path, score_threshold, z_offset, \ + min_points_in_box, min_box_height = self._backend_args + self.backend = self._init_backend( + cfg_path, ckpt_path, score_threshold, 'cpu', z_offset, + min_points_in_box, min_box_height) + if self.backend is None: + return + try: + boxes, scores, labels = self.backend.infer(points) + except Exception as e2: + self.get_logger().error( + f"CPU 폴백 재시도 실패 — 해당 프레임 스킵: {e2}") + return + + # max_range 밖(라이다 밀도 낮은 원거리) 박스 제거 — 박스 중심 수평거리 기준 + if self.max_range > 0.0 and boxes.shape[0] > 0: + r = np.hypot(boxes[:, 0], boxes[:, 1]) + keep = r <= self.max_range + boxes, scores, labels = boxes[keep], scores[keep], labels[keep] + + self.pub_det.publish(self._to_detection_array(msg.header, boxes, scores, labels)) + self.pub_marker.publish(self._to_marker_array(msg.header, boxes, labels)) + + def _to_detection_array(self, header, boxes, scores, labels): + arr = Detection3DArray() + arr.header = header + for box, score, label in zip(boxes, scores, labels): + x, y, z, dx, dy, dz, yaw = [float(v) for v in box] + qx, qy, qz, qw = _yaw_to_quat(yaw) + + det = Detection3D() + det.header = header + + hyp = ObjectHypothesisWithPose() + hyp.hypothesis.class_id = str(int(label)) + hyp.hypothesis.score = float(score) + det.results.append(hyp) + + det.bbox.center.position.x = x + det.bbox.center.position.y = y + det.bbox.center.position.z = z + det.bbox.center.orientation.x = qx + det.bbox.center.orientation.y = qy + det.bbox.center.orientation.z = qz + det.bbox.center.orientation.w = qw + det.bbox.size.x = dx + det.bbox.size.y = dy + det.bbox.size.z = dz + + arr.detections.append(det) + return arr + + def _to_marker_array(self, header, boxes, labels): + markers = MarkerArray() + + # 이전 프레임 마커 제거 + clear = Marker() + clear.header = header + clear.action = Marker.DELETEALL + markers.markers.append(clear) + + for i, (box, label) in enumerate(zip(boxes, labels)): + x, y, z, dx, dy, dz, yaw = [float(v) for v in box] + qx, qy, qz, qw = _yaw_to_quat(yaw) + + m = Marker() + m.header = header + m.ns = 'pointpillars' + m.id = i + m.type = Marker.CUBE + m.action = Marker.ADD + m.pose.position.x = x + m.pose.position.y = y + m.pose.position.z = z + m.pose.orientation.x = qx + m.pose.orientation.y = qy + m.pose.orientation.z = qz + m.pose.orientation.w = qw + m.scale.x = dx + m.scale.y = dy + m.scale.z = dz + m.color = self._label_color(int(label)) + markers.markers.append(m) + + # 박스 위에 클래스명 텍스트 (YOLO 식 라벨) + t = Marker() + t.header = header + t.ns = 'pointpillars_label' + t.id = i + t.type = Marker.TEXT_VIEW_FACING + t.action = Marker.ADD + t.pose.position.x = x + t.pose.position.y = y + t.pose.position.z = z + dz / 2.0 + 0.4 + t.pose.orientation.w = 1.0 + t.scale.z = 0.8 + t.color = self._label_color(int(label)) + t.color.a = 1.0 + t.text = self._label_name(int(label)) + markers.markers.append(t) + + return markers + + @staticmethod + def _label_color(label): + # 클래스별 색 (반투명). 미지정 라벨은 회색. + palette = { + 1: (0.0, 1.0, 0.0), # Car + 2: (1.0, 1.0, 0.0), # Pedestrian + 3: (0.0, 0.6, 1.0), # Cyclist + } + r, g, b = palette.get(label, (0.6, 0.6, 0.6)) + return ColorRGBA(r=r, g=g, b=b, a=0.5) + + @staticmethod + def _label_name(label): + # 클래스 id → 이름 (_label_color 팔레트와 동일 매핑). 미지정은 cls{id}. + names = {1: 'Car', 2: 'Pedestrian', 3: 'Cyclist'} + return names.get(label, f'cls{label}') + + +def main(args=None): + rclpy.init(args=args) + node = ObjectDetection() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception_pointpillars_pkg/resource/perception_pointpillars_pkg b/src/perception_pointpillars_pkg/resource/perception_pointpillars_pkg new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_pointpillars_pkg/setup.cfg b/src/perception_pointpillars_pkg/setup.cfg new file mode 100755 index 0000000..40f23a0 --- /dev/null +++ b/src/perception_pointpillars_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/perception_pointpillars_pkg +[install] +install_scripts=$base/lib/perception_pointpillars_pkg diff --git a/src/perception_pointpillars_pkg/setup.py b/src/perception_pointpillars_pkg/setup.py new file mode 100755 index 0000000..cff02ea --- /dev/null +++ b/src/perception_pointpillars_pkg/setup.py @@ -0,0 +1,37 @@ + +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'perception_pointpillars_pkg' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + # finetuned 용 OpenPCDet cfg — ws 루트 cfgs/ 의 편입본 (launch 가 share 경로로 참조) + (os.path.join('share', package_name, 'cfgs'), glob('cfgs/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='dgist', + maintainer_email='jang1future@gmail.com', + description='LiDAR 3D object detection (PointPillars / OpenPCDet) perception module', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'pointpillars = perception_pointpillars_pkg.pointpillars_node:main' + ], + }, +) diff --git a/src/perception_pointpillars_pkg/test/test_copyright.py b/src/perception_pointpillars_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/src/perception_pointpillars_pkg/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/perception_pointpillars_pkg/test/test_flake8.py b/src/perception_pointpillars_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/src/perception_pointpillars_pkg/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/perception_pointpillars_pkg/test/test_pep257.py b/src/perception_pointpillars_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/src/perception_pointpillars_pkg/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/perception_rangenetpp_pkg/config/rangenetpp.yaml b/src/perception_rangenetpp_pkg/config/rangenetpp.yaml new file mode 100755 index 0000000..a1dfd62 --- /dev/null +++ b/src/perception_rangenetpp_pkg/config/rangenetpp.yaml @@ -0,0 +1,23 @@ +rangenetpp: + ros__parameters: + input_topic: "/raw/lidar" + segmented_topic: "/perception/segmented" + segmented_rgb_topic: "/perception/segmented_rgb" + model_dir: "" # 비우면 env RANGENETPP_MODEL_DIR, launch 기본 /opt/rangenetpp_pretrained + model_ckpt_path: "" # RangeNet++ 는 model_dir 안 backbone/decoder/head 로드 → 미사용(인터페이스 일관성) + device: "auto" # auto = 런타임 감지(GPU 있으면 cuda, 없으면 cpu). dual-runtime + # range projection — 실 센서 FOV 에 맞춘다 (CARLA fr09 ouster: 32ch, +21.2/-21.2). + # proj_H 는 네트워크 학습 높이(64) 유지하고 32 실채널을 KNN 보간으로 채움. + # FOV 가 KITTI(+3/-25)면 ±21.2 점들이 엉뚱한 행에 박혀 추론이 한 클래스로 붕괴됨. + proj_H: 64 + proj_W: 2048 + fov_up: 21.2 + fov_down: -21.2 + max_range: 50.0 + min_range: 1.0 + use_knn: true + # CARLA 도메인갭 보정 (objdet z_offset 측정값 참고: CARLA 지면 -1.06 vs KITTI -1.73) + z_offset: 0.0 + # CARLA intensity(~0.96) 를 KITTI 평균(0.21)에 맞춰 중화 — 안 하면 +4.7σ 로 saturate. + intensity_scale: 0.22 + use_sim_time: true diff --git a/src/perception_rangenetpp_pkg/config/semantic_kitti_labels.yaml b/src/perception_rangenetpp_pkg/config/semantic_kitti_labels.yaml new file mode 100755 index 0000000..4cdc533 --- /dev/null +++ b/src/perception_rangenetpp_pkg/config/semantic_kitti_labels.yaml @@ -0,0 +1,22 @@ +# SemanticKITTI class id → BGR (RViz 색칠용). 표준 color_map 일부. +color_map: + 0: [0, 0, 0] # unlabeled + 10: [245, 150, 100] # car + 11: [245, 230, 100] # bicycle + 15: [150, 60, 30] # motorcycle + 18: [180, 30, 80] # truck + 20: [255, 0, 0] # other-vehicle + 30: [30, 30, 255] # person + 31: [200, 40, 255] # bicyclist + 32: [90, 30, 150] # motorcyclist + 40: [255, 0, 255] # road + 44: [255, 150, 255] # parking + 48: [75, 0, 75] # sidewalk + 49: [75, 0, 175] # other-ground + 50: [0, 200, 255] # building + 51: [50, 120, 255] # fence + 70: [0, 175, 0] # vegetation + 71: [0, 60, 135] # trunk + 72: [80, 240, 150] # terrain + 80: [150, 240, 255] # pole + 81: [0, 0, 255] # traffic-sign diff --git a/src/perception_rangenetpp_pkg/launch/perception.launch.py b/src/perception_rangenetpp_pkg/launch/perception.launch.py new file mode 100755 index 0000000..9807859 --- /dev/null +++ b/src/perception_rangenetpp_pkg/launch/perception.launch.py @@ -0,0 +1,32 @@ +"""perception_manager 공통 진입점 (통합 perception 모듈). + +LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로 +`ros2 launch perception_rangenetpp_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..` +를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(rangenetpp.launch.py)로 위임한다. +robot 은 공통 계약상 선언만 하고, 모델 launch 가 선언한 경우에만 소비된다. +""" +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + share = get_package_share_directory('perception_rangenetpp_pkg') + return LaunchDescription([ + DeclareLaunchArgument('robot', default_value='fr09'), + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('model_source', default_value='finetuned'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(share, 'launch', 'rangenetpp.launch.py')), + launch_arguments={ + 'robot': LaunchConfiguration('robot'), + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'model_source': LaunchConfiguration('model_source'), + }.items(), + ), + ]) diff --git a/src/perception_rangenetpp_pkg/launch/rangenetpp.launch.py b/src/perception_rangenetpp_pkg/launch/rangenetpp.launch.py new file mode 100755 index 0000000..c928ba6 --- /dev/null +++ b/src/perception_rangenetpp_pkg/launch/rangenetpp.launch.py @@ -0,0 +1,86 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node + +# 선언 기본값 — 명시 인자 판별(자동 resolve 게이트)에도 쓰므로 상수로 공유. +DEFAULT_MODEL_DIR = '/opt/rangenetpp_pretrained' +DEFAULT_INTENSITY_SCALE = '0.22' +# 가중치 공유 단일 위치(docker/local): /perception_models/rangenetpp/{finetuned,pretrained} +# (호스트 .caches/config/nav2_config/perception/models/rangenetpp/*, SWRule config_mounts). +FINETUNED_DIR = '/perception_models/rangenetpp/finetuned' +SHARED_PRE_DIR = '/perception_models/rangenetpp/pretrained' # 공유 pretrained (Phase 2 채움) + +ARGUMENTS = [ + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('robot', default_value='mkmini', choices=['mkmini', 'fr09']), + DeclareLaunchArgument('model_dir', default_value=DEFAULT_MODEL_DIR), + DeclareLaunchArgument('model_ckpt_path', default_value=''), + # base(KITTI) pretrained 는 0.22(CARLA intensity 중화), finetuned 는 1.0(raw 로 학습됨). + DeclareLaunchArgument('intensity_scale', default_value=DEFAULT_INTENSITY_SCALE), + # 모델 자동 resolve 스위치: pretrained 선택 시 finetuned 있어도 건너뛰고 base pretrained 사용. + DeclareLaunchArgument('model_source', default_value='finetuned', + choices=['finetuned', 'pretrained']), +] + + +def _resolve_model(context): + """모델 resolve — 단독 deploy run.sh 의 MODEL_ARGS 로직을 launch 안으로 이식. + + - model_dir/intensity_scale 이 명시되면(선언 기본값과 다르면) 그대로 사용 + → 단독 run.sh 가 MODEL_ARGS 로 명시 전달하는 기존 동작이 그대로 이긴다(무회귀). + - 기본값/빈값일 때만 model_source(기본 finetuned) + finetuned 존재 검사로 자동 resolve. + RangeNet++ finetuned 는 단일 ckpt 가 아니라 model_dir 안 backbone 파일 존재로 게이트. + """ + model_dir = LaunchConfiguration('model_dir').perform(context) + intensity_scale = LaunchConfiguration('intensity_scale').perform(context) + model_source = LaunchConfiguration('model_source').perform(context) + + explicit = (model_dir not in ('', DEFAULT_MODEL_DIR) + or intensity_scale not in ('', DEFAULT_INTENSITY_SCALE)) + if not explicit: + if model_source != 'pretrained' and os.path.isfile( + os.path.join(FINETUNED_DIR, 'backbone')): + # finetuned 는 raw CARLA intensity(~0.95)로 학습됨 → 추론도 raw(intensity_scale=1.0). + # base(0.22 중화)와 달라야 train/infer 정규화가 일치(안 그러면 재학습 효과 깎임). + model_dir = FINETUNED_DIR + intensity_scale = '1.0' + print(f'[rangenetpp] 재학습 모델 사용: {FINETUNED_DIR} (intensity_scale=1.0)') + elif os.path.isfile(os.path.join(SHARED_PRE_DIR, 'backbone')): + # 공유 pretrained(KITTI base) — baked(/opt) 보다 우선. intensity 는 base 0.22. + model_dir = SHARED_PRE_DIR + intensity_scale = DEFAULT_INTENSITY_SCALE + print(f'[rangenetpp] 공유 pretrained 사용: {SHARED_PRE_DIR}') + else: + model_dir = model_dir or DEFAULT_MODEL_DIR + intensity_scale = intensity_scale or DEFAULT_INTENSITY_SCALE + print('[rangenetpp] 재학습 모델 없음 → base pretrained(baked) 사용') + + pkg = get_package_share_directory('perception_rangenetpp_pkg') + params_file = PathJoinSubstitution([pkg, 'config', 'rangenetpp.yaml']) + color_map = PathJoinSubstitution([pkg, 'config', 'semantic_kitti_labels.yaml']) + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + load_nodes = GroupAction(actions=[ + Node( + package='perception_rangenetpp_pkg', executable='rangenetpp', + name='rangenetpp', output='screen', + parameters=[params_file, { + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'model_dir': model_dir, + 'model_ckpt_path': LaunchConfiguration('model_ckpt_path'), + 'intensity_scale': float(intensity_scale), + 'color_map_path': color_map, + }], + remappings=remappings, + ) + ]) + return [load_nodes] + + +def generate_launch_description(): + ld = LaunchDescription(ARGUMENTS) + ld.add_action(OpaqueFunction(function=_resolve_model)) + return ld diff --git a/src/perception_rangenetpp_pkg/package.xml b/src/perception_rangenetpp_pkg/package.xml new file mode 100755 index 0000000..0b80c7f --- /dev/null +++ b/src/perception_rangenetpp_pkg/package.xml @@ -0,0 +1,24 @@ + + + + perception_rangenetpp_pkg + 0.0.0 + LiDAR semantic segmentation (RangeNet++) perception module + dgist + TODO: License declaration + + rclpy + sensor_msgs + sensor_msgs_py + geometry_msgs + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/__init__.py b/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/range_projection.py b/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/range_projection.py new file mode 100755 index 0000000..269a070 --- /dev/null +++ b/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/range_projection.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 +"""LiDAR spherical range-image projection (모델 무관, 순수 numpy). + +RangeNet++/SalsaNext 계열 표준 투영. 점별 (proj_x, proj_y) 를 보존해 +네트워크 픽셀 출력 → 점별 라벨 복원(un-projection)을 가능케 한다. +""" +from dataclasses import dataclass + +import numpy as np + + +@dataclass +class ProjectionResult: + range_image: np.ndarray # (5, H, W): range(depth), x, y, z, intensity (RangeNet/SalsaNext 순서) + px: np.ndarray # (V,) valid 점들의 열 index + py: np.ndarray # (V,) valid 점들의 행 index + valid: np.ndarray # (N,) bool — range 범위 안 점 마스크 + + +class RangeProjector: + def __init__(self, proj_H, proj_W, fov_up, fov_down, max_range, min_range): + self.H = int(proj_H) + self.W = int(proj_W) + self.fov_up = float(fov_up) / 180.0 * np.pi + self.fov_down = float(fov_down) / 180.0 * np.pi + self.fov = abs(self.fov_up) + abs(self.fov_down) + self.max_range = float(max_range) + self.min_range = float(min_range) + + def project(self, points): + xyz = points[:, :3] + intensity = points[:, 3] if points.shape[1] > 3 else np.zeros(len(points), np.float32) + depth = np.linalg.norm(xyz, axis=1) + valid = (depth > self.min_range) & (depth < self.max_range) + + x, y, z = xyz[:, 0], xyz[:, 1], xyz[:, 2] + yaw = -np.arctan2(y, x) + pitch = np.arcsin(np.clip(z / np.maximum(depth, 1e-6), -1.0, 1.0)) + + proj_x = 0.5 * (yaw / np.pi + 1.0) # [0,1] + proj_y = 1.0 - (pitch + abs(self.fov_down)) / self.fov + proj_x = np.clip(np.floor(proj_x * self.W), 0, self.W - 1).astype(np.int32) + proj_y = np.clip(np.floor(proj_y * self.H), 0, self.H - 1).astype(np.int32) + + img = np.zeros((5, self.H, self.W), dtype=np.float32) + # 가까운 점 우선(겹치면 작은 depth 가 픽셀 차지) — depth 내림차순으로 채움 + order = np.argsort(depth)[::-1] + vi = np.where(valid)[0] + # depth 내림차순 중 valid 만 채워 마지막에 가까운 점이 덮어쓰게 + fill = order[np.isin(order, vi)] + py, px = proj_y[fill], proj_x[fill] + img[0, py, px] = depth[fill] # range (RangeNet img_means[0]=12.12) + img[1, py, px] = x[fill] + img[2, py, px] = y[fill] + img[3, py, px] = z[fill] + img[4, py, px] = intensity[fill] + + return ProjectionResult( + range_image=img, + px=proj_x[valid], py=proj_y[valid], valid=valid, + ) + + def unproject(self, pixel_labels, proj: ProjectionResult): + """pixel_labels (H,W) → 점별 라벨 (N,). valid 아닌 점은 0(unlabeled).""" + n = proj.valid.shape[0] + labels = np.zeros(n, dtype=np.int64) + labels[proj.valid] = pixel_labels[proj.py, proj.px] + return labels diff --git a/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/rangenetpp_backend.py b/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/rangenetpp_backend.py new file mode 100755 index 0000000..ec2fde8 --- /dev/null +++ b/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/rangenetpp_backend.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python3 +"""RangeNet++ semantic segmentation 추론 래퍼 (ROS 무관). + +torch / lidar-bonnetal(RangeNet++) 모델 코드는 인스턴스화할 때만 import → 미설치 +환경에서도 패키지 import/build 통과(salsanext_backend 와 동일 격리). + +infer(points) -> labels (N,) uint32 (SemanticKITTI class id) + +모델 산출물(arch_cfg/data_cfg + backbone/segmentation_decoder/segmentation_head, +color map)은 컨테이너 /models 에 config_mounts 로 bind-mount 된다. +projection 파라미터는 노드(config/Rule)가 주입. + +⚠️ RangeNet++ 가 SalsaNext 와 다른 점(범위 표현·KNN·정규화는 동일): + - 모델 = lidar-bonnetal `Segmentator`(darknet 백본+디코더+head). 단일 .pth 가 아니라 + model_dir 안 `backbone`/`segmentation_decoder`/`segmentation_head` 3개 weight 파일을 + Segmentator 가 내부적으로 로드(strict). 따라서 backend 는 ckpt_path 를 직접 안 읽고 + model_dir 만 넘긴다(파일이 없으면 random weight 로 fallback — 그건 잘못된 추론이므로 + 노드가 model_dir 유효성을 보장해야 한다). +""" +import os +import sys +import types + +import numpy as np + +from perception_rangenetpp_pkg.range_projection import RangeProjector + + +class RangeNetppBackend: + def __init__(self, model_dir, ckpt_path, device, + proj_H, proj_W, fov_up, fov_down, max_range, min_range, + z_offset=0.0, intensity_scale=1.0, use_knn=True, logger=None): + import torch + import yaml + + # lidar-bonnetal repo(train/) 를 path 에 추가 — 이미지에 clone 됨(Dockerfile). + rnet_root = os.environ.get('RANGENETPP_ROOT', '/opt/lidar-bonnetal/train') + sys.path.append(rnet_root) + # ⚠️ lidar-bonnetal 함정: segmentator.py 가 `import __init__ as booger` 로 잡는 모듈의 + # TRAIN_PATH(상대경로 "../../")를 imp.load_source 백본/디코더 파일 경로에 쓴다 → cwd 의존 + # (deploy 는 cwd=/nav2_ws 라 backbones/*.py 를 못 찾음). segmentator import 전에 + # '__init__' 모듈을 절대경로 TRAIN_PATH 로 sys.modules 에 선등록 — `import __init__` 이 + # 캐시에서 이 stub 을 그대로 집어가므로 어떤 cwd 에서도 안전. (사후 패치는 불가: + # '__init__' 은 segmentator import 시점에야 sys.modules 에 생긴다.) + booger = sys.modules.get('__init__') + if booger is None: + booger = types.ModuleType('__init__') + sys.modules['__init__'] = booger + booger.TRAIN_PATH = rnet_root + booger.DEPLOY_PATH = rnet_root + from tasks.semantic.modules.segmentator import Segmentator + from tasks.semantic.postproc.KNN import KNN + + self._torch = torch + # device='auto' → 런타임 감지: cuda 가용 시 cuda:0, 아니면 cpu (08-gpu-cpu-runtime N4) + if device == 'auto': + device = 'cuda:0' if torch.cuda.is_available() else 'cpu' + self.device = device + self.z_offset = float(z_offset) + self.intensity_scale = float(intensity_scale) + + # 모델 산출물 cfg (mount 된 /models 안). nclasses / 정규화 means·stds / color map. + arch = yaml.safe_load(open(os.path.join(model_dir, 'arch_cfg.yaml'))) + data = yaml.safe_load(open(os.path.join(model_dir, 'data_cfg.yaml'))) + self.nclasses = len(data['learning_map_inv']) + sensor = arch['dataset']['sensor'] + self.img_means = np.array(sensor['img_means'], dtype=np.float32) + self.img_stds = np.array(sensor['img_stds'], dtype=np.float32) + # 추론 결과는 train id → 원래 SemanticKITTI id 로 역매핑 + self.learning_map_inv = data['learning_map_inv'] + + self.proj = RangeProjector(proj_H, proj_W, fov_up, fov_down, max_range, min_range) + + # Segmentator(ARCH, nclasses, path=model_dir) → model_dir 안 backbone/ + # segmentation_decoder/segmentation_head 를 strict 로 로드(없으면 random). + with torch.no_grad(): + model = Segmentator(arch, self.nclasses, model_dir) + self._model = model.to(device).eval() + + self._knn = None + if use_knn: + self._knn = KNN(arch['post']['KNN']['params'], self.nclasses) + if logger: + logger(f"RangeNet++ loaded: nclasses={self.nclasses} device={device}") + + def infer(self, points): + torch = self._torch + points = np.ascontiguousarray(points, dtype=np.float32) + if self.z_offset: + points = points.copy() + points[:, 2] += self.z_offset + if self.intensity_scale != 1.0 and points.shape[1] > 3: + points = points.copy() + points[:, 3] *= self.intensity_scale + + pr = self.proj.project(points) + img = pr.range_image.copy() + # 정규화 (학습 분포) — valid 픽셀만 (range=채널0 이 채워진 픽셀) + mask = (img[0] > 0).astype(np.float32) + for c in range(5): + img[c] = (img[c] - self.img_means[c]) / self.img_stds[c] + img[c] *= mask + + with torch.no_grad(): + inp = torch.from_numpy(img).unsqueeze(0).to(self.device) + logits = self._model(inp) # (1, C, H, W) + pixel_argmax = logits.argmax(dim=1)[0] # (H, W) + + if self._knn is not None: + depth_img = torch.from_numpy(pr.range_image[0]).to(self.device) + py = torch.from_numpy(pr.py.astype(np.int64)).to(self.device) + px = torch.from_numpy(pr.px.astype(np.int64)).to(self.device) + pt_depth = torch.from_numpy( + np.linalg.norm(points[pr.valid, :3], axis=1)).to(self.device) + knn_labels = self._knn(depth_img, pt_depth, pixel_argmax, px, py) + train_ids = np.zeros(len(points), dtype=np.int64) + train_ids[pr.valid] = knn_labels.cpu().numpy() + else: + train_ids = self.proj.unproject(pixel_argmax.cpu().numpy(), pr) + + # train id → SemanticKITTI id + out = np.zeros(len(points), dtype=np.uint32) + for tid, kid in self.learning_map_inv.items(): + out[train_ids == int(tid)] = int(kid) + return out diff --git a/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/rangenetpp_node.py b/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/rangenetpp_node.py new file mode 100755 index 0000000..97bc57c --- /dev/null +++ b/src/perception_rangenetpp_pkg/perception_rangenetpp_pkg/rangenetpp_node.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 +import os +import struct + +import numpy as np +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2, PointField +import sensor_msgs_py.point_cloud2 as pc2 +import yaml + +from perception_rangenetpp_pkg.rangenetpp_backend import RangeNetppBackend + + +class SemanticSegmentation(Node): + def __init__(self): + super().__init__('rangenetpp') + + self.declare_parameter('input_topic', '/raw/lidar') + self.declare_parameter('segmented_topic', '/perception/segmented') + self.declare_parameter('segmented_rgb_topic', '/perception/segmented_rgb') + # 모델 산출물 디렉토리(/models) — RangeNet++ 는 dir 안 backbone/decoder/head 로드. + # ckpt_path 는 인터페이스 일관성(salsanext)용 — RangeNet++ 는 model_dir 만 쓴다. + self.declare_parameter('model_dir', '') + self.declare_parameter('model_ckpt_path', '') + self.declare_parameter('color_map_path', '') + # device: 'auto' → 런타임 감지(cuda 가용 시 cuda:0, 아니면 cpu). dual-runtime + self.declare_parameter('device', 'auto') + # range projection (모델 입력 규격, Rule 로 조절) + self.declare_parameter('proj_H', 64) + self.declare_parameter('proj_W', 2048) + self.declare_parameter('fov_up', 3.0) + self.declare_parameter('fov_down', -25.0) + self.declare_parameter('max_range', 50.0) + self.declare_parameter('min_range', 1.0) + self.declare_parameter('use_knn', True) + # 도메인갭 보정 + self.declare_parameter('z_offset', 0.0) + self.declare_parameter('intensity_scale', 1.0) + + gp = self.get_parameter + input_topic = gp('input_topic').value + seg_topic = gp('segmented_topic').value + seg_rgb_topic = gp('segmented_rgb_topic').value + model_dir = gp('model_dir').value or os.environ.get('RANGENETPP_MODEL_DIR', '') + ckpt = gp('model_ckpt_path').value or os.environ.get('RANGENETPP_MODEL_CKPT', '') + color_map_path = gp('color_map_path').value or os.environ.get('RANGENETPP_COLOR_MAP', '') + + self.color_map = self._load_color_map(color_map_path) + self.pub_seg = self.create_publisher(PointCloud2, seg_topic, 5) + self.pub_rgb = self.create_publisher(PointCloud2, seg_rgb_topic, 5) + + self.backend = self._init_backend(model_dir, ckpt, gp) + self.sub = self.create_subscription(PointCloud2, input_topic, self.callback, 5) + self.get_logger().info( + f"RangeNet++ started: in[{input_topic}] " + f"seg[{seg_topic}] rgb[{seg_rgb_topic}] device[{gp('device').value}]") + + def _init_backend(self, model_dir, ckpt, gp): + # RangeNet++ 는 model_dir 안 backbone/segmentation_decoder/segmentation_head 를 로드. + # 단일 ckpt 파일이 아니라 dir 유효성으로 게이트(arch_cfg 존재로 간이 검증). + if not model_dir or not os.path.isfile(os.path.join(model_dir, 'arch_cfg.yaml')): + self.get_logger().error( + "model_dir 미지정/유효하지 않음 (config 또는 RANGENETPP_MODEL_DIR). " + "arch_cfg.yaml + backbone/segmentation_decoder/segmentation_head 필요. " + "추론 비활성 — 구독만 수행.") + return None + try: + return RangeNetppBackend( + model_dir=model_dir, ckpt_path=ckpt, device=gp('device').value, + proj_H=gp('proj_H').value, proj_W=gp('proj_W').value, + fov_up=gp('fov_up').value, fov_down=gp('fov_down').value, + max_range=gp('max_range').value, min_range=gp('min_range').value, + z_offset=gp('z_offset').value, intensity_scale=gp('intensity_scale').value, + use_knn=gp('use_knn').value, logger=self.get_logger().info) + except Exception as e: + self.get_logger().error(f"분할 백엔드 초기화 실패: {e}") + return None + + def _load_color_map(self, path): + if path and os.path.exists(path): + data = yaml.safe_load(open(path)) + return {int(k): tuple(v) for k, v in data.get('color_map', {}).items()} + return {} + + def _read_points(self, msg): + names = {f.name for f in msg.fields} + if 'intensity' in names: + pts = pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True) + if pts.shape[0] == 0: + return None + return np.stack((pts["x"], pts["y"], pts["z"], pts["intensity"]), axis=-1).astype(np.float32) + pts = pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True) + if pts.shape[0] == 0: + return None + xyz = np.stack((pts["x"], pts["y"], pts["z"]), axis=-1).astype(np.float32) + return np.concatenate([xyz, np.zeros((xyz.shape[0], 1), np.float32)], axis=1) + + def callback(self, msg): + if self.backend is None: + return + points = self._read_points(msg) + if points is None: + return + labels = self.backend.infer(points) # (N,) uint32 SemanticKITTI id + self.pub_seg.publish(self._to_label_cloud(msg.header, points, labels)) + if self.color_map: + self.pub_rgb.publish(self._to_rgb_cloud(msg.header, points, labels)) + + def _to_label_cloud(self, header, points, labels): + fields = [ + PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name='label', offset=12, datatype=PointField.UINT32, count=1), + ] + data = [(float(p[0]), float(p[1]), float(p[2]), int(l)) + for p, l in zip(points, labels)] + return pc2.create_cloud(header, fields, data) + + def _to_rgb_cloud(self, header, points, labels): + fields = [ + PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name='rgb', offset=12, datatype=PointField.FLOAT32, count=1), + ] + data = [] + for p, l in zip(points, labels): + b, g, r = self.color_map.get(int(l), (0, 0, 0)) # SemanticKITTI BGR + rgb = struct.unpack('f', struct.pack('I', (int(r) << 16) | (int(g) << 8) | int(b)))[0] + data.append((float(p[0]), float(p[1]), float(p[2]), rgb)) + return pc2.create_cloud(header, fields, data) + + +def main(args=None): + rclpy.init(args=args) + node = SemanticSegmentation() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception_rangenetpp_pkg/resource/perception_rangenetpp_pkg b/src/perception_rangenetpp_pkg/resource/perception_rangenetpp_pkg new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_rangenetpp_pkg/setup.cfg b/src/perception_rangenetpp_pkg/setup.cfg new file mode 100755 index 0000000..d7495c5 --- /dev/null +++ b/src/perception_rangenetpp_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/perception_rangenetpp_pkg +[install] +install_scripts=$base/lib/perception_rangenetpp_pkg diff --git a/src/perception_rangenetpp_pkg/setup.py b/src/perception_rangenetpp_pkg/setup.py new file mode 100755 index 0000000..de31ad3 --- /dev/null +++ b/src/perception_rangenetpp_pkg/setup.py @@ -0,0 +1,35 @@ + +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'perception_rangenetpp_pkg' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='dgist', + maintainer_email='jang1future@gmail.com', + description='LiDAR semantic segmentation (RangeNet++) perception module', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'rangenetpp = perception_rangenetpp_pkg.rangenetpp_node:main' + ], + }, +) diff --git a/src/perception_rangenetpp_pkg/test/test_copyright.py b/src/perception_rangenetpp_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/src/perception_rangenetpp_pkg/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/perception_rangenetpp_pkg/test/test_flake8.py b/src/perception_rangenetpp_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/src/perception_rangenetpp_pkg/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/perception_rangenetpp_pkg/test/test_pep257.py b/src/perception_rangenetpp_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/src/perception_rangenetpp_pkg/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/perception_rangenetpp_pkg/test/test_range_projection.py b/src/perception_rangenetpp_pkg/test/test_range_projection.py new file mode 100755 index 0000000..56e848b --- /dev/null +++ b/src/perception_rangenetpp_pkg/test/test_range_projection.py @@ -0,0 +1,29 @@ +import numpy as np +from perception_rangenetpp_pkg.range_projection import RangeProjector + + +def test_unproject_recovers_per_point_labels(): + # 정면 부채꼴에 흩뿌린 점들 — projection 후 픽셀 라벨을 도로(0)/객체(1)로 칠하고 + # un-projection 이 점별 라벨을 되돌리는지 (충돌 없는 sparse 케이스). + rng = np.random.default_rng(0) + n = 500 + xyz = rng.uniform(-20, 20, size=(n, 3)).astype(np.float32) + xyz[:, 2] = rng.uniform(-2, 1, size=n) # 적당한 높이 + pts = np.concatenate([xyz, np.zeros((n, 1), np.float32)], axis=1) + + proj = RangeProjector(proj_H=64, proj_W=1024, fov_up=3.0, fov_down=-25.0, + max_range=50.0, min_range=1.0) + out = proj.project(pts) + assert out.range_image.shape == (5, 64, 1024) + assert out.px.shape[0] == out.valid.sum() + + # 픽셀 라벨 = (depth>10 ? 1 : 0) 으로 합성 → 점별로 같은 규칙이 복원돼야 함 + depth_img = out.range_image[3] + pixel_labels = (depth_img > 10.0).astype(np.int64) + labels = proj.unproject(pixel_labels, out) + assert labels.shape == (n,) + # valid 점들에 대해 점 depth 기준 기대 라벨과 일치 + vp = pts[out.valid][:, :3] + vdepth = np.linalg.norm(vp, axis=1) + expected = (vdepth > 10.0).astype(np.int64) + assert (labels[out.valid] == expected).mean() > 0.98 diff --git a/src/perception_salsanext_pkg/config/salsanext.yaml b/src/perception_salsanext_pkg/config/salsanext.yaml new file mode 100755 index 0000000..2205033 --- /dev/null +++ b/src/perception_salsanext_pkg/config/salsanext.yaml @@ -0,0 +1,23 @@ +salsanext: + ros__parameters: + input_topic: "/raw/lidar" + segmented_topic: "/perception/segmented" + segmented_rgb_topic: "/perception/segmented_rgb" + model_dir: "" # 비우면 env SALSANEXT_MODEL_DIR, launch 기본 /models + model_ckpt_path: "" # 비우면 env SALSANEXT_MODEL_CKPT, launch 기본 /models/SalsaNext + device: "auto" # auto: cuda 가용 시 GPU, 아니면 CPU (08-gpu-cpu-runtime N3/N4) + # range projection — 실 센서 FOV 에 맞춘다 (CARLA fr09 ouster: 32ch, +21.2/-21.2). + # proj_H 는 네트워크 학습 높이(64) 유지하고 32 실채널을 KNN 보간으로 채움. + # FOV 가 KITTI(+3/-25)면 ±21.2 점들이 엉뚱한 행에 박혀 추론이 한 클래스로 붕괴됨. + proj_H: 64 + proj_W: 2048 + fov_up: 21.2 + fov_down: -21.2 + max_range: 50.0 + min_range: 1.0 + use_knn: true + # CARLA 도메인갭 보정 (objdet z_offset 측정값 참고: CARLA 지면 -1.06 vs KITTI -1.73) + z_offset: 0.0 + # CARLA intensity(~0.96) 를 KITTI 평균(0.21)에 맞춰 중화 — 안 하면 +4.7σ 로 saturate. + intensity_scale: 0.22 + use_sim_time: true diff --git a/src/perception_salsanext_pkg/config/semantic_kitti_labels.yaml b/src/perception_salsanext_pkg/config/semantic_kitti_labels.yaml new file mode 100755 index 0000000..4cdc533 --- /dev/null +++ b/src/perception_salsanext_pkg/config/semantic_kitti_labels.yaml @@ -0,0 +1,22 @@ +# SemanticKITTI class id → BGR (RViz 색칠용). 표준 color_map 일부. +color_map: + 0: [0, 0, 0] # unlabeled + 10: [245, 150, 100] # car + 11: [245, 230, 100] # bicycle + 15: [150, 60, 30] # motorcycle + 18: [180, 30, 80] # truck + 20: [255, 0, 0] # other-vehicle + 30: [30, 30, 255] # person + 31: [200, 40, 255] # bicyclist + 32: [90, 30, 150] # motorcyclist + 40: [255, 0, 255] # road + 44: [255, 150, 255] # parking + 48: [75, 0, 75] # sidewalk + 49: [75, 0, 175] # other-ground + 50: [0, 200, 255] # building + 51: [50, 120, 255] # fence + 70: [0, 175, 0] # vegetation + 71: [0, 60, 135] # trunk + 72: [80, 240, 150] # terrain + 80: [150, 240, 255] # pole + 81: [0, 0, 255] # traffic-sign diff --git a/src/perception_salsanext_pkg/launch/perception.launch.py b/src/perception_salsanext_pkg/launch/perception.launch.py new file mode 100755 index 0000000..333df31 --- /dev/null +++ b/src/perception_salsanext_pkg/launch/perception.launch.py @@ -0,0 +1,32 @@ +"""perception_manager 공통 진입점 (통합 perception 모듈). + +LocalLauncher buildPerceptionArg 가 모든 모델 pkg 에 동일한 계약으로 +`ros2 launch perception_salsanext_pkg perception.launch.py robot:=.. use_sim_time:=.. model_source:=..` +를 스폰하므로(perception_pkg/launch_spec.py), 모델 launch(salsanext.launch.py)로 위임한다. +robot 은 공통 계약상 선언만 하고, 모델 launch 가 선언한 경우에만 소비된다. +""" +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + share = get_package_share_directory('perception_salsanext_pkg') + return LaunchDescription([ + DeclareLaunchArgument('robot', default_value='fr09'), + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('model_source', default_value='finetuned'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(share, 'launch', 'salsanext.launch.py')), + launch_arguments={ + 'robot': LaunchConfiguration('robot'), + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'model_source': LaunchConfiguration('model_source'), + }.items(), + ), + ]) diff --git a/src/perception_salsanext_pkg/launch/salsanext.launch.py b/src/perception_salsanext_pkg/launch/salsanext.launch.py new file mode 100755 index 0000000..d3e84c7 --- /dev/null +++ b/src/perception_salsanext_pkg/launch/salsanext.launch.py @@ -0,0 +1,80 @@ +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node + +# resolve 분기/경로 상수는 pkg 모듈 resolve_model 이 단일 출처 (launch 는 게이트만 담당). +from perception_salsanext_pkg import resolve_model + +# 선언 기본값 — 명시 인자 판별(자동 resolve 게이트)에도 쓰므로 resolve_model 상수 공유. +DEFAULT_MODEL_DIR = resolve_model.DEFAULT_PRE_DIR +DEFAULT_MODEL_CKPT = resolve_model.DEFAULT_PRE_CKPT +DEFAULT_INTENSITY_SCALE = resolve_model.PRETRAINED_INTENSITY_SCALE + +ARGUMENTS = [ + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('robot', default_value='mkmini', choices=['mkmini', 'fr09']), + DeclareLaunchArgument('model_dir', default_value=DEFAULT_MODEL_DIR), + DeclareLaunchArgument('model_ckpt_path', default_value=DEFAULT_MODEL_CKPT), + # base(KITTI) pretrained 는 0.22(CARLA intensity 중화), finetuned 는 1.0(raw 로 학습됨). + DeclareLaunchArgument('intensity_scale', default_value=DEFAULT_INTENSITY_SCALE), + # 모델 자동 resolve 스위치: pretrained 선택 시 finetuned 있어도 건너뛰고 base pretrained 사용. + DeclareLaunchArgument('model_source', default_value='finetuned', + choices=['finetuned', 'pretrained']), +] + + +def _resolve_model(context): + """모델 resolve — 단독 deploy run.sh 의 MODEL_ARGS 로직을 launch 안으로 이식. + + - model_dir/model_ckpt_path/intensity_scale 이 명시되면(선언 기본값과 다르면) 그대로 사용 + → 단독 run.sh 가 MODEL_ARGS 로 명시 전달하는 기존 동작이 그대로 이긴다(무회귀). + - 기본값/빈값일 때만 model_source(기본 finetuned) + finetuned ckpt 존재 검사로 자동 resolve. + 분기 자체는 pkg 모듈 resolve_model.resolve() 사용 — intensity_scale 까지 함께 결정 + (finetuned=raw CARLA 1.0 / pretrained=0.22, train/infer 정규화 일치 목적). + """ + model_dir = LaunchConfiguration('model_dir').perform(context) + model_ckpt_path = LaunchConfiguration('model_ckpt_path').perform(context) + intensity_scale = LaunchConfiguration('intensity_scale').perform(context) + model_source = LaunchConfiguration('model_source').perform(context) + + explicit = (model_dir not in ('', DEFAULT_MODEL_DIR) + or model_ckpt_path not in ('', DEFAULT_MODEL_CKPT) + or intensity_scale not in ('', DEFAULT_INTENSITY_SCALE)) + if not explicit: + resolved = resolve_model.resolve(model_source) + model_dir = resolved['model_dir'] + model_ckpt_path = resolved['ckpt'] + intensity_scale = resolved['intensity_scale'] + if resolved['source'] == 'finetuned': + print(f'[salsanext] 재학습 모델 사용: {model_dir} ' + f'(intensity_scale={intensity_scale})') + else: + print('[salsanext] 재학습 모델 없음 → base pretrained 사용') + + pkg = get_package_share_directory('perception_salsanext_pkg') + params_file = PathJoinSubstitution([pkg, 'config', 'salsanext.yaml']) + color_map = PathJoinSubstitution([pkg, 'config', 'semantic_kitti_labels.yaml']) + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + load_nodes = GroupAction(actions=[ + Node( + package='perception_salsanext_pkg', executable='salsanext', + name='salsanext', output='screen', + parameters=[params_file, { + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'model_dir': model_dir, + 'model_ckpt_path': model_ckpt_path, + 'intensity_scale': float(intensity_scale), + 'color_map_path': color_map, + }], + remappings=remappings, + ) + ]) + return [load_nodes] + + +def generate_launch_description(): + ld = LaunchDescription(ARGUMENTS) + ld.add_action(OpaqueFunction(function=_resolve_model)) + return ld diff --git a/src/perception_salsanext_pkg/package.xml b/src/perception_salsanext_pkg/package.xml new file mode 100755 index 0000000..5bf0a38 --- /dev/null +++ b/src/perception_salsanext_pkg/package.xml @@ -0,0 +1,24 @@ + + + + perception_salsanext_pkg + 0.0.0 + LiDAR semantic segmentation (SalsaNext) perception module + dgist + TODO: License declaration + + rclpy + sensor_msgs + sensor_msgs_py + geometry_msgs + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception_salsanext_pkg/perception_salsanext_pkg/__init__.py b/src/perception_salsanext_pkg/perception_salsanext_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_salsanext_pkg/perception_salsanext_pkg/range_projection.py b/src/perception_salsanext_pkg/perception_salsanext_pkg/range_projection.py new file mode 100755 index 0000000..3ddda2e --- /dev/null +++ b/src/perception_salsanext_pkg/perception_salsanext_pkg/range_projection.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 +"""LiDAR spherical range-image projection (모델 무관, 순수 numpy). + +RangeNet++/SalsaNext 계열 표준 투영. 점별 (proj_x, proj_y) 를 보존해 +네트워크 픽셀 출력 → 점별 라벨 복원(un-projection)을 가능케 한다. +""" +from dataclasses import dataclass + +import numpy as np + + +@dataclass +class ProjectionResult: + range_image: np.ndarray # (5, H, W): range(depth), x, y, z, intensity (RangeNet/SalsaNext 순서) + px: np.ndarray # (V,) valid 점들의 열 index + py: np.ndarray # (V,) valid 점들의 행 index + valid: np.ndarray # (N,) bool — range 범위 안 점 마스크 + + +class RangeProjector: + def __init__(self, proj_H, proj_W, fov_up, fov_down, max_range, min_range): + self.H = int(proj_H) + self.W = int(proj_W) + self.fov_up = float(fov_up) / 180.0 * np.pi + self.fov_down = float(fov_down) / 180.0 * np.pi + self.fov = abs(self.fov_up) + abs(self.fov_down) + self.max_range = float(max_range) + self.min_range = float(min_range) + + def project(self, points): + xyz = points[:, :3] + intensity = points[:, 3] if points.shape[1] > 3 else np.zeros(len(points), np.float32) + depth = np.linalg.norm(xyz, axis=1) + valid = (depth > self.min_range) & (depth < self.max_range) + + x, y, z = xyz[:, 0], xyz[:, 1], xyz[:, 2] + yaw = -np.arctan2(y, x) + pitch = np.arcsin(np.clip(z / np.maximum(depth, 1e-6), -1.0, 1.0)) + + proj_x = 0.5 * (yaw / np.pi + 1.0) # [0,1] + proj_y = 1.0 - (pitch + abs(self.fov_down)) / self.fov + proj_x = np.clip(np.floor(proj_x * self.W), 0, self.W - 1).astype(np.int32) + proj_y = np.clip(np.floor(proj_y * self.H), 0, self.H - 1).astype(np.int32) + + img = np.zeros((5, self.H, self.W), dtype=np.float32) + # 가까운 점 우선(겹치면 작은 depth 가 픽셀 차지) — depth 내림차순으로 채움 + order = np.argsort(depth)[::-1] + vi = np.where(valid)[0] + # depth 내림차순 중 valid 만 채워 마지막에 가까운 점이 덮어쓰게 + fill = order[np.isin(order, vi)] + py, px = proj_y[fill], proj_x[fill] + img[0, py, px] = depth[fill] # range (SalsaNext img_means[0]=12.12) + img[1, py, px] = x[fill] + img[2, py, px] = y[fill] + img[3, py, px] = z[fill] + img[4, py, px] = intensity[fill] + + return ProjectionResult( + range_image=img, + px=proj_x[valid], py=proj_y[valid], valid=valid, + ) + + def unproject(self, pixel_labels, proj: ProjectionResult): + """pixel_labels (H,W) → 점별 라벨 (N,). valid 아닌 점은 0(unlabeled).""" + n = proj.valid.shape[0] + labels = np.zeros(n, dtype=np.int64) + labels[proj.valid] = pixel_labels[proj.py, proj.px] + return labels diff --git a/src/perception_salsanext_pkg/perception_salsanext_pkg/resolve_model.py b/src/perception_salsanext_pkg/perception_salsanext_pkg/resolve_model.py new file mode 100755 index 0000000..7dec380 --- /dev/null +++ b/src/perception_salsanext_pkg/perception_salsanext_pkg/resolve_model.py @@ -0,0 +1,44 @@ +"""모델 경로 결정 — finetuned(retrain 산출물) 우선 → 공유 pretrained → baked pretrained. +※ 작은 함수 하나로 분리 — model_source=auto|finetuned|pretrained 명시 선택을 한 분기로 끼우기 위함. +salsanext 는 intensity 정규화가 모델별로 달라 intensity_scale 까지 함께 resolve 한다.""" +import os + +# 가중치 공유 단일 위치(docker/local 공유): +# 호스트 .caches/config/nav2_config/perception/models/salsanext/{finetuned,pretrained} +# → 컨테이너 /perception_models/salsanext/* (SWRule config_mounts). +DEFAULT_FINETUNED = "/perception_models/salsanext/finetuned" +SHARED_PRE_DIR = "/perception_models/salsanext/pretrained" # 공유 pretrained (Phase 2 채움) +DEFAULT_PRE_DIR = "/opt/salsanext_pretrained" # 이미지 baked 안전망(오프라인/Jetson) +DEFAULT_PRE_CKPT = "/opt/salsanext_pretrained/SalsaNext" +# base(KITTI) pretrained 는 0.22(CARLA intensity ~0.96 을 KITTI 평균 0.21 로 중화), +# finetuned 는 raw CARLA intensity(~0.95)로 학습됨 → 추론도 raw(1.0). +# 달라야 train/infer 정규화가 일치(안 그러면 재학습 효과 깎임). +PRETRAINED_INTENSITY_SCALE = "0.22" +FINETUNED_INTENSITY_SCALE = "1.0" + + +def _shared_pretrained(): + # 공유 pretrained(.../pretrained)가 채워져 있으면 baked(/opt) 보다 우선 — 공유 단일 출처. + ckpt = os.path.join(SHARED_PRE_DIR, "SalsaNext") + if os.path.isfile(ckpt): + return {"source": "pretrained", "model_dir": SHARED_PRE_DIR, "ckpt": ckpt, + "intensity_scale": PRETRAINED_INTENSITY_SCALE} + return None + + +def resolve(model_source="finetuned", + finetuned_dir=DEFAULT_FINETUNED, + pretrained_dir=DEFAULT_PRE_DIR, + pretrained_ckpt=DEFAULT_PRE_CKPT): + # 3단 fallback: finetuned → 공유 pretrained → baked pretrained(/opt 안전망). + # model_source=="pretrained" 면 finetuned 있어도 pretrained 강제(공유 우선, 없으면 baked). + _baked = {"source": "pretrained", "model_dir": pretrained_dir, "ckpt": pretrained_ckpt, + "intensity_scale": PRETRAINED_INTENSITY_SCALE} + _pretrained = _shared_pretrained() or _baked + if model_source == "pretrained": + return _pretrained + ckpt = os.path.join(finetuned_dir, "SalsaNext_valid_best") + if os.path.isfile(ckpt): + return {"source": "finetuned", "model_dir": finetuned_dir, "ckpt": ckpt, + "intensity_scale": FINETUNED_INTENSITY_SCALE} + return _pretrained diff --git a/src/perception_salsanext_pkg/perception_salsanext_pkg/salsanext_backend.py b/src/perception_salsanext_pkg/perception_salsanext_pkg/salsanext_backend.py new file mode 100755 index 0000000..0a34990 --- /dev/null +++ b/src/perception_salsanext_pkg/perception_salsanext_pkg/salsanext_backend.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 +"""SalsaNext semantic segmentation 추론 래퍼 (ROS 무관). + +torch / SalsaNext 모델 코드는 인스턴스화할 때만 import → 미설치 환경에서도 +패키지 import/build 통과(objdet 의 pointpillars_backend 와 동일 격리). + +infer(points) -> labels (N,) int (SemanticKITTI class id) + +모델 산출물(arch_cfg/data_cfg/checkpoint, color map)은 컨테이너 /models 에 +config_mounts 로 bind-mount 된다. projection 파라미터는 노드(config/Rule)가 주입. +""" +import os +import sys + +import numpy as np + +from perception_salsanext_pkg.range_projection import RangeProjector + + +class SalsaNextBackend: + def __init__(self, model_dir, ckpt_path, device, + proj_H, proj_W, fov_up, fov_down, max_range, min_range, + z_offset=0.0, intensity_scale=1.0, use_knn=True, logger=None): + import torch + import yaml + + # SalsaNext repo(train/) 를 path 에 추가 — 이미지에 clone 됨(Dockerfile). + salsa_root = os.environ.get('SALSANEXT_ROOT', '/opt/SalsaNext') + sys.path.append(os.path.join(salsa_root, 'train')) + from tasks.semantic.modules.SalsaNext import SalsaNext + from tasks.semantic.postproc.KNN import KNN + + # device='auto' → 런타임 감지: cuda 가용 시 cuda:0, 아니면 cpu (08-gpu-cpu-runtime N4) + if device == 'auto': + device = 'cuda:0' if torch.cuda.is_available() else 'cpu' + + self._torch = torch + self.device = device + self.z_offset = float(z_offset) + self.intensity_scale = float(intensity_scale) + + # 모델 산출물 cfg (mount 된 /models 안). nclasses / 정규화 means·stds / color map. + arch = yaml.safe_load(open(os.path.join(model_dir, 'arch_cfg.yaml'))) + data = yaml.safe_load(open(os.path.join(model_dir, 'data_cfg.yaml'))) + self.nclasses = len(data['learning_map_inv']) + sensor = arch['dataset']['sensor'] + self.img_means = np.array(sensor['img_means'], dtype=np.float32) + self.img_stds = np.array(sensor['img_stds'], dtype=np.float32) + # 추론 결과는 train id → 원래 SemanticKITTI id 로 역매핑 + self.learning_map_inv = data['learning_map_inv'] + + self.proj = RangeProjector(proj_H, proj_W, fov_up, fov_down, max_range, min_range) + + model = SalsaNext(self.nclasses) + state = torch.load(ckpt_path, map_location='cpu') + sd = state.get('state_dict', state) + sd = {k.replace('module.', ''): v for k, v in sd.items()} + model.load_state_dict(sd, strict=True) + self._model = model.to(device).eval() + + self._knn = None + if use_knn: + self._knn = KNN(arch['post']['KNN']['params'], self.nclasses) + if logger: + logger(f"SalsaNext loaded: nclasses={self.nclasses} device={device}") + + def infer(self, points): + torch = self._torch + points = np.ascontiguousarray(points, dtype=np.float32) + if self.z_offset: + points = points.copy() + points[:, 2] += self.z_offset + if self.intensity_scale != 1.0 and points.shape[1] > 3: + points = points.copy() + points[:, 3] *= self.intensity_scale + + pr = self.proj.project(points) + img = pr.range_image.copy() + # 정규화 (학습 분포) — valid 픽셀만 (range=채널0 이 채워진 픽셀) + mask = (img[0] > 0).astype(np.float32) + for c in range(5): + img[c] = (img[c] - self.img_means[c]) / self.img_stds[c] + img[c] *= mask + + with torch.no_grad(): + inp = torch.from_numpy(img).unsqueeze(0).to(self.device) + logits = self._model(inp) # (1, C, H, W) + pixel_argmax = logits.argmax(dim=1)[0] # (H, W) + + if self._knn is not None: + depth_img = torch.from_numpy(pr.range_image[0]).to(self.device) + py = torch.from_numpy(pr.py.astype(np.int64)).to(self.device) + px = torch.from_numpy(pr.px.astype(np.int64)).to(self.device) + pt_depth = torch.from_numpy( + np.linalg.norm(points[pr.valid, :3], axis=1)).to(self.device) + knn_labels = self._knn(depth_img, pt_depth, pixel_argmax, px, py) + train_ids = np.zeros(len(points), dtype=np.int64) + train_ids[pr.valid] = knn_labels.cpu().numpy() + else: + train_ids = self.proj.unproject(pixel_argmax.cpu().numpy(), pr) + + # train id → SemanticKITTI id + out = np.zeros(len(points), dtype=np.uint32) + for tid, kid in self.learning_map_inv.items(): + out[train_ids == int(tid)] = int(kid) + return out diff --git a/src/perception_salsanext_pkg/perception_salsanext_pkg/salsanext_node.py b/src/perception_salsanext_pkg/perception_salsanext_pkg/salsanext_node.py new file mode 100755 index 0000000..3403ea9 --- /dev/null +++ b/src/perception_salsanext_pkg/perception_salsanext_pkg/salsanext_node.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python3 +import os +import struct + +import numpy as np +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2, PointField +import sensor_msgs_py.point_cloud2 as pc2 +import yaml + +from perception_salsanext_pkg.salsanext_backend import SalsaNextBackend + + +def _is_cuda_error(e): + s = str(e).lower() + return 'cuda' in s or 'cudnn' in s or 'out of memory' in s + + +class SemanticSegmentation(Node): + def __init__(self): + super().__init__('salsanext') + + self.declare_parameter('input_topic', '/raw/lidar') + self.declare_parameter('segmented_topic', '/perception/segmented') + self.declare_parameter('segmented_rgb_topic', '/perception/segmented_rgb') + # 모델 산출물 디렉토리(/models) + checkpoint — 하드코딩 금지, env fallback + self.declare_parameter('model_dir', '') + self.declare_parameter('model_ckpt_path', '') + self.declare_parameter('color_map_path', '') + # device: 'auto' → 런타임 감지(cuda 가용 시 cuda:0, 아니면 cpu). + # 'cuda:0'/'cpu' 등 명시값은 그대로 존중(override). (08-gpu-cpu-runtime N3/N4) + self.declare_parameter('device', 'auto') + # range projection (모델 입력 규격, Rule 로 조절) + self.declare_parameter('proj_H', 64) + self.declare_parameter('proj_W', 2048) + self.declare_parameter('fov_up', 3.0) + self.declare_parameter('fov_down', -25.0) + self.declare_parameter('max_range', 50.0) + self.declare_parameter('min_range', 1.0) + self.declare_parameter('use_knn', True) + # 도메인갭 보정 + self.declare_parameter('z_offset', 0.0) + self.declare_parameter('intensity_scale', 1.0) + + gp = self.get_parameter + input_topic = gp('input_topic').value + seg_topic = gp('segmented_topic').value + seg_rgb_topic = gp('segmented_rgb_topic').value + model_dir = gp('model_dir').value or os.environ.get('SALSANEXT_MODEL_DIR', '') + ckpt = gp('model_ckpt_path').value or os.environ.get('SALSANEXT_MODEL_CKPT', '') + color_map_path = gp('color_map_path').value or os.environ.get('SALSANEXT_COLOR_MAP', '') + + self.color_map = self._load_color_map(color_map_path) + self.pub_seg = self.create_publisher(PointCloud2, seg_topic, 5) + self.pub_rgb = self.create_publisher(PointCloud2, seg_rgb_topic, 5) + + self._model_dir = model_dir + self._ckpt = ckpt + self._cpu_fallback_done = False + self.backend = self._init_backend(model_dir, ckpt, gp) + self.sub = self.create_subscription(PointCloud2, input_topic, self.callback, 5) + self.get_logger().info( + f"SalsaNext started: in[{input_topic}] " + f"seg[{seg_topic}] rgb[{seg_rgb_topic}] device[{gp('device').value}]") + + def _init_backend(self, model_dir, ckpt, gp): + if not model_dir or not ckpt: + self.get_logger().error( + "model_dir / model_ckpt_path 미지정 (config 또는 " + "SALSANEXT_MODEL_DIR/SALSANEXT_MODEL_CKPT). 추론 비활성 — 구독만 수행.") + return None + try: + return self._create_backend(model_dir, ckpt, gp('device').value) + except Exception as e: + if gp('device').value != 'cpu' and _is_cuda_error(e): + self.get_logger().warn(f"[salsanext] GPU 백엔드 초기화 실패({e}) → CPU 폴백") + try: + return self._create_backend(model_dir, ckpt, 'cpu') + except Exception as e: + self.get_logger().error(f"분할 백엔드 초기화 실패: {e}") + return None + self.get_logger().error(f"분할 백엔드 초기화 실패: {e}") + return None + + def _create_backend(self, model_dir, ckpt, device): + gp = self.get_parameter + return SalsaNextBackend( + model_dir=model_dir, ckpt_path=ckpt, device=device, + proj_H=gp('proj_H').value, proj_W=gp('proj_W').value, + fov_up=gp('fov_up').value, fov_down=gp('fov_down').value, + max_range=gp('max_range').value, min_range=gp('min_range').value, + z_offset=gp('z_offset').value, intensity_scale=gp('intensity_scale').value, + use_knn=gp('use_knn').value, logger=self.get_logger().info) + + def _load_color_map(self, path): + if path and os.path.exists(path): + data = yaml.safe_load(open(path)) + return {int(k): tuple(v) for k, v in data.get('color_map', {}).items()} + return {} + + def _read_points(self, msg): + names = {f.name for f in msg.fields} + if 'intensity' in names: + pts = pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True) + if pts.shape[0] == 0: + return None + return np.stack((pts["x"], pts["y"], pts["z"], pts["intensity"]), axis=-1).astype(np.float32) + pts = pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True) + if pts.shape[0] == 0: + return None + xyz = np.stack((pts["x"], pts["y"], pts["z"]), axis=-1).astype(np.float32) + return np.concatenate([xyz, np.zeros((xyz.shape[0], 1), np.float32)], axis=1) + + def callback(self, msg): + if self.backend is None: + return + points = self._read_points(msg) + if points is None: + return + try: + labels = self.backend.infer(points) # (N,) uint32 SemanticKITTI id + except Exception as e: + if self._cpu_fallback_done or not _is_cuda_error(e): + raise + self.get_logger().warn("[salsanext] 추론 중 CUDA 에러 → CPU 백엔드 재구성") + self._cpu_fallback_done = True + try: + del self.backend._model + self.backend._torch.cuda.empty_cache() + except Exception: + pass + try: + self.backend = self._create_backend(self._model_dir, self._ckpt, 'cpu') + labels = self.backend.infer(points) + except Exception as e2: + self.get_logger().error(f"분할 백엔드 CPU 폴백 실패: {e2}") + self.backend = None + return + self.pub_seg.publish(self._to_label_cloud(msg.header, points, labels)) + if self.color_map: + self.pub_rgb.publish(self._to_rgb_cloud(msg.header, points, labels)) + + def _to_label_cloud(self, header, points, labels): + fields = [ + PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name='label', offset=12, datatype=PointField.UINT32, count=1), + ] + data = [(float(p[0]), float(p[1]), float(p[2]), int(l)) + for p, l in zip(points, labels)] + return pc2.create_cloud(header, fields, data) + + def _to_rgb_cloud(self, header, points, labels): + fields = [ + PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name='rgb', offset=12, datatype=PointField.FLOAT32, count=1), + ] + data = [] + for p, l in zip(points, labels): + b, g, r = self.color_map.get(int(l), (0, 0, 0)) # SemanticKITTI BGR + rgb = struct.unpack('f', struct.pack('I', (int(r) << 16) | (int(g) << 8) | int(b)))[0] + data.append((float(p[0]), float(p[1]), float(p[2]), rgb)) + return pc2.create_cloud(header, fields, data) + + +def main(args=None): + rclpy.init(args=args) + node = SemanticSegmentation() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception_salsanext_pkg/resource/perception_salsanext_pkg b/src/perception_salsanext_pkg/resource/perception_salsanext_pkg new file mode 100755 index 0000000..e69de29 diff --git a/src/perception_salsanext_pkg/setup.cfg b/src/perception_salsanext_pkg/setup.cfg new file mode 100755 index 0000000..2451944 --- /dev/null +++ b/src/perception_salsanext_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/perception_salsanext_pkg +[install] +install_scripts=$base/lib/perception_salsanext_pkg diff --git a/src/perception_salsanext_pkg/setup.py b/src/perception_salsanext_pkg/setup.py new file mode 100755 index 0000000..afd16df --- /dev/null +++ b/src/perception_salsanext_pkg/setup.py @@ -0,0 +1,35 @@ + +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'perception_salsanext_pkg' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='dgist', + maintainer_email='jang1future@gmail.com', + description='LiDAR semantic segmentation (SalsaNext) perception module', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'salsanext = perception_salsanext_pkg.salsanext_node:main' + ], + }, +) diff --git a/src/perception_salsanext_pkg/test/test_copyright.py b/src/perception_salsanext_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/src/perception_salsanext_pkg/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/perception_salsanext_pkg/test/test_flake8.py b/src/perception_salsanext_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/src/perception_salsanext_pkg/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/perception_salsanext_pkg/test/test_pep257.py b/src/perception_salsanext_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/src/perception_salsanext_pkg/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/perception_salsanext_pkg/test/test_range_projection.py b/src/perception_salsanext_pkg/test/test_range_projection.py new file mode 100755 index 0000000..739bbce --- /dev/null +++ b/src/perception_salsanext_pkg/test/test_range_projection.py @@ -0,0 +1,29 @@ +import numpy as np +from perception_salsanext_pkg.range_projection import RangeProjector + + +def test_unproject_recovers_per_point_labels(): + # 정면 부채꼴에 흩뿌린 점들 — projection 후 픽셀 라벨을 도로(0)/객체(1)로 칠하고 + # un-projection 이 점별 라벨을 되돌리는지 (충돌 없는 sparse 케이스). + rng = np.random.default_rng(0) + n = 500 + xyz = rng.uniform(-20, 20, size=(n, 3)).astype(np.float32) + xyz[:, 2] = rng.uniform(-2, 1, size=n) # 적당한 높이 + pts = np.concatenate([xyz, np.zeros((n, 1), np.float32)], axis=1) + + proj = RangeProjector(proj_H=64, proj_W=1024, fov_up=3.0, fov_down=-25.0, + max_range=50.0, min_range=1.0) + out = proj.project(pts) + assert out.range_image.shape == (5, 64, 1024) + assert out.px.shape[0] == out.valid.sum() + + # 픽셀 라벨 = (depth>10 ? 1 : 0) 으로 합성 → 점별로 같은 규칙이 복원돼야 함 + depth_img = out.range_image[3] + pixel_labels = (depth_img > 10.0).astype(np.int64) + labels = proj.unproject(pixel_labels, out) + assert labels.shape == (n,) + # valid 점들에 대해 점 depth 기준 기대 라벨과 일치 + vp = pts[out.valid][:, :3] + vdepth = np.linalg.norm(vp, axis=1) + expected = (vdepth > 10.0).astype(np.int64) + assert (labels[out.valid] == expected).mean() > 0.98