ROS2, 밑바닥부터
딥러닝 로봇팔까지.

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
최소 ROS2 노드 / Minimal ROS2 node
"""
import rclpy
from rclpy.node import Node


class HelloNode(Node):
    def __init__(self):
        # 노드명은 DDS 네임스페이스 내에서 유니크해야 한다
        # Node name must be unique within DDS namespace
        super().__init__("hello_node")
        self.get_logger().info("안녕, ROS2 — node up")
        # 1Hz 타이머 / 1Hz timer
        self.create_timer(1.0, self.tick)
        self.counter = 0

    def tick(self):
        self.counter += 1
        self.get_logger().info(f"tick #{self.counter}")


def main():
    rclpy.init()                  # DDS 컨텍스트 초기화
    node = HelloNode()
    try:
        rclpy.spin(node)          # 이벤트 루프 — blocking
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()

from setuptools import find_packages, setup

package_name = 'my_py_pkg'

setup(
    name=package_name,
    version='0.0.1',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
         ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='zer0',
    maintainer_email='[email protected]',
    description='ROS2 learning playground',
    license='MIT',
    entry_points={
        'console_scripts': [
            # 'exe_name = pkg.module:main_func' 형태
            'hello = my_py_pkg.hello_node:main',
        ],
    },
)

cmake_minimum_required(VERSION 3.8)
project(my_interfaces)

find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/WeldScanPoint.msg"
  DEPENDENCIES std_msgs
)

ament_export_dependencies(rosidl_default_runtime)
ament_package()

<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<build_depend>std_msgs</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

from my_interfaces.msg import WeldScanPoint

msg = WeldScanPoint()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = "sensor_link"
msg.x, msg.y, msg.z = 0.12, -0.03, 0.45
msg.intensity = 0.87
msg.class_id = 2

# 요청 / request
string mode            # "idle" | "inspect" | "weld"
---
# 응답 / response
bool success
string message

import rclpy
from rclpy.node import Node
from my_interfaces.srv import SetMode


class ModeServer(Node):
    ALLOWED = {"idle", "inspect", "weld"}

    def __init__(self):
        super().__init__("mode_server")
        self.mode = "idle"
        self.srv = self.create_service(
            SetMode, "set_mode", self.on_request)

    def on_request(self, req: SetMode.Request,
                   res: SetMode.Response):
        if req.mode not in self.ALLOWED:
            res.success = False
            res.message = f"unknown mode: {req.mode}"
            return res
        self.mode = req.mode
        res.success = True
        res.message = f"switched to {self.mode}"
        self.get_logger().info(res.message)
        return res

# Goal — 클라이언트가 보내는 요청
uint32 num_samples      # 스캔할 샘플 수
---
# Result — 완료 시 반환
uint32 total_captured
string bag_path
---
# Feedback — 진행 중 주기적으로 발행
uint32 current
float32 percentage

import time
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from my_interfaces.action import Scan


class ScanServer(Node):
    def __init__(self):
        super().__init__("scan_server")
        self._server = ActionServer(
            self, Scan, "scan",
            execute_callback=self.execute,
            goal_callback=self.accept_goal,
            cancel_callback=self.accept_cancel,
        )

    def accept_goal(self, goal_request):
        self.get_logger().info(
            f"goal received: N={goal_request.num_samples}")
        return GoalResponse.ACCEPT

    def accept_cancel(self, goal_handle):
        return CancelResponse.ACCEPT

    def execute(self, goal_handle):
        req: Scan.Goal = goal_handle.request
        fb = Scan.Feedback()
        for i in range(req.num_samples):
            # 취소 체크 / cooperative cancellation
            if goal_handle.is_cancel_requested:
                goal_handle.canceled()
                return Scan.Result()
            # 실제 센서 샘플링 자리 / real acquisition here
            time.sleep(0.05)
            fb.current = i + 1
            fb.percentage = (i + 1) / req.num_samples * 100.0
            goal_handle.publish_feedback(fb)

        goal_handle.succeed()
        result = Scan.Result()
        result.total_captured = req.num_samples
        result.bag_path = "/tmp/scan_001.bag"
        return result

from rcl_interfaces.msg import ParameterDescriptor, FloatingPointRange
from rcl_interfaces.msg import SetParametersResult
import rclpy
from rclpy.node import Node


class Controller(Node):
    def __init__(self):
        super().__init__("controller")

        # 파라미터 선언 (타입 · 기본값 · 범위 · 설명)
        # Declare parameter with type, default, range, description
        self.declare_parameter(
            "max_speed", 1.5,
            ParameterDescriptor(
                description="로봇팔 엔드이펙터 최대 속도 m/s",
                floating_point_range=[FloatingPointRange(
                    from_value=0.0, to_value=3.0, step=0.1)],
            ),
        )
        self.declare_parameter("gains.kp", 2.0)
        self.declare_parameter("gains.ki", 0.1)
        self.declare_parameter("gains.kd", 0.05)

        # 런타임 변경 콜백 / runtime change callback
        self.add_on_set_parameters_callback(self.on_param_change)

    def on_param_change(self, params):
        for p in params:
            self.get_logger().info(f"{p.name} = {p.value}")
        return SetParametersResult(successful=True)

"""
로봇 시스템 통합 런치 / System bring-up
"""
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction, LogInfo
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node, PushRosNamespace
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    # 인자 선언 / declare args
    use_sim = LaunchConfiguration("use_sim_time")
    robot_ns = LaunchConfiguration("namespace")

    params_file = PathJoinSubstitution([
        FindPackageShare("my_py_pkg"),
        "config", "controller.yaml"
    ])

    return LaunchDescription([
        DeclareLaunchArgument(
            "use_sim_time", default_value="false"),
        DeclareLaunchArgument(
            "namespace", default_value="arm0"),

        LogInfo(msg=["Launching system for ns=", robot_ns]),

        GroupAction([
            PushRosNamespace(robot_ns),

            Node(
                package="my_py_pkg",
                executable="controller",
                name="controller",
                parameters=[params_file,
                            {"use_sim_time": use_sim}],
                output="screen",
                emulate_tty=True,
            ),

            Node(
                package="my_py_pkg",
                executable="hello",
                name="hello",
                condition=IfCondition(LaunchConfiguration("debug",
                    default="false")),
            ),
        ]),
    ])

import rclpy
from rclpy.node import Node
from rclpy.duration import Duration
from tf2_ros import Buffer, TransformListener
from tf2_ros import LookupException, ConnectivityException, ExtrapolationException


class TFUser(Node):
    def __init__(self):
        super().__init__("tf_user")
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)
        self.create_timer(0.1, self.tick)   # 10 Hz

    def tick(self):
        try:
            # target_frame ← source_frame at time
            # "source 기준 target의 위치/자세를 알려줘"
            t = self.buffer.lookup_transform(
                target_frame="base_link",
                source_frame="camera_optical",
                time=rclpy.time.Time(),        # 최신
                timeout=Duration(seconds=0.05),
            )
            x = t.transform.translation.x
            y = t.transform.translation.y
            z = t.transform.translation.z
            self.get_logger().info(f"cam in base: ({x:+.3f}, {y:+.3f}, {z:+.3f})")
        except (LookupException, ConnectivityException,
                ExtrapolationException) as e:
            self.get_logger().warn(f"tf not ready: {e}")

from geometry_msgs.msg import PointStamped
from tf2_geometry_msgs import do_transform_point

# 카메라 프레임의 3D 점을 base_link로
pt_in_cam = PointStamped()
pt_in_cam.header.frame_id = "camera_optical"
pt_in_cam.header.stamp = self.get_clock().now().to_msg()
pt_in_cam.point.x, pt_in_cam.point.y, pt_in_cam.point.z = 0.1, 0.05, 0.8

tf = self.buffer.lookup_transform(
    "base_link", "camera_optical", pt_in_cam.header.stamp,
    timeout=Duration(seconds=0.1))
pt_in_base = do_transform_point(pt_in_cam, tf)

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
from tf_transformations import quaternion_from_euler
import math


class StaticCamTF(Node):
    def __init__(self):
        super().__init__("static_cam_tf")
        self.br = StaticTransformBroadcaster(self)

        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = "tool0"           # parent
        t.child_frame_id = "camera_optical"   # child
        t.transform.translation.x = 0.05
        t.transform.translation.y = 0.00
        t.transform.translation.z = 0.12
        q = quaternion_from_euler(0.0, math.radians(15.0), 0.0)
        t.transform.rotation.x, t.transform.rotation.y, \
            t.transform.rotation.z, t.transform.rotation.w = q

        self.br.sendTransform(t)              # latched on /tf_static

<?xml version="1.0"?>
<robot name="simple_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:property name="pi" value="3.14159"/>

  <link name="base_link">
    <visual>
      <geometry><cylinder radius="0.08" length="0.1"/></geometry>
      <material name="steel"><color rgba="0.5 0.5 0.6 1"/></material>
    </visual>
    <collision>
      <geometry><cylinder radius="0.08" length="0.1"/></geometry>
    </collision>
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.01" iyy="0.01" izz="0.01"
               ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <link name="link1">
    <visual>
      <origin xyz="0 0 0.15"/>
      <geometry><box size="0.04 0.04 0.3"/></geometry>
    </visual>
  </link>

  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0.05"/>
    <axis xyz="0 0 1"/>
    <limit lower="${-pi}" upper="${pi}"
           effort="10" velocity="2.0"/>
  </joint>

</robot>

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from launch.substitutions import Command
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution

def generate_launch_description():
    urdf_path = PathJoinSubstitution([
        FindPackageShare("my_robot_description"),
        "urdf", "simple_arm.urdf.xacro"
    ])
    robot_description = {"robot_description": Command(["xacro ", urdf_path])}

    return LaunchDescription([
        Node(package="robot_state_publisher",
             executable="robot_state_publisher",
             parameters=[robot_description],
             output="screen"),
        Node(package="joint_state_publisher_gui",
             executable="joint_state_publisher_gui"),
        ExecuteProcess(cmd=["rviz2"], output="screen"),
    ])

from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy

sensor_qos = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,  # 센서 데이터
    durability=DurabilityPolicy.VOLATILE,
    history=HistoryPolicy.KEEP_LAST,
    depth=5,
)
self.sub = self.create_subscription(
    LaserScan, "/scan", self.on_scan, sensor_qos)

# latched 스타일 — 나중에 구독해도 마지막 맵을 받음
map_qos = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    durability=DurabilityPolicy.TRANSIENT_LOCAL,
    history=HistoryPolicy.KEEP_LAST,
    depth=1,
)

import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
from std_srvs.srv import Trigger
import time


class HeavyNode(Node):
    def __init__(self):
        super().__init__("heavy_node")

        # 병렬 허용 / parallel-safe
        self.cb_fast = ReentrantCallbackGroup()
        # 상호배제 / critical section
        self.cb_critical = MutuallyExclusiveCallbackGroup()

        self.create_timer(0.05, self.fast_tick,
                          callback_group=self.cb_fast)  # 20Hz
        self.create_service(Trigger, "slow_work",
                            self.slow_work,
                            callback_group=self.cb_critical)

    def fast_tick(self):
        self.get_logger().debug("tick")

    def slow_work(self, req, res):
        time.sleep(2.0)   # fast_tick가 블록되지 않음
        res.success = True
        res.message = "done"
        return res


def main():
    rclpy.init()
    node = HeavyNode()
    executor = MultiThreadedExecutor(num_threads=4)
    executor.add_node(node)
    try:
        executor.spin()
    finally:
        executor.shutdown()

from launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode


def generate_launch_description():
    return LaunchDescription([
        ComposableNodeContainer(
            name="perception_container",
            namespace="",
            package="rclcpp_components",
            executable="component_container_mt",  # multi-threaded
            composable_node_descriptions=[
                ComposableNode(
                    package="image_proc",
                    plugin="image_proc::RectifyNode",
                    name="rectify"),
                ComposableNode(
                    package="depth_image_proc",
                    plugin="depth_image_proc::PointCloudXyzNode",
                    name="xyz_cloud",
                    remappings=[
                        ("image_rect", "/camera/depth/image_rect"),
                        ("camera_info", "/camera/depth/camera_info"),
                    ]),
            ],
            output="screen",
        ),
    ])

<CycloneDDS xmlns="https://cdds.io/config">
  <Domain Id="any">
    <General>
      <!-- 공유 메모리 전송 활성 (같은 호스트 intra-process용) -->
      <Interfaces><NetworkInterface autodetermine="true"/></Interfaces>
      <AllowMulticast>true</AllowMulticast>
      <MaxMessageSize>65500B</MaxMessageSize>
    </General>
    <Internal>
      <SocketReceiveBufferSize min="10MB"/>
      <SocketSendBufferSize min="10MB"/>
    </Internal>
  </Domain>
</CycloneDDS>

"""
MoveIt2 Python API로 팔 움직이기 / Move arm with moveit_py
"""
import rclpy
from moveit.planning import MoveItPy
from geometry_msgs.msg import PoseStamped


def main():
    rclpy.init()
    moveit = MoveItPy(node_name="moveit_py_demo")
    arm = moveit.get_planning_component("manipulator")

    # 목표 pose / target pose
    target = PoseStamped()
    target.header.frame_id = "base_link"
    target.pose.position.x = 0.45
    target.pose.position.y = 0.0
    target.pose.position.z = 0.35
    target.pose.orientation.w = 1.0

    arm.set_start_state_to_current_state()
    arm.set_goal_state(pose_stamped_msg=target,
                       pose_link="tool0")

    # 계획 / plan
    plan_result = arm.plan()
    if plan_result:
        moveit.execute(plan_result.trajectory, controllers=[])
    else:
        print("planning failed")

    rclpy.shutdown()

# PointCloud2 주요 필드 / Key fields
# header           : std_msgs/Header (frame_id, stamp)
# height, width    : 구조화 여부 (organized면 h>1)
# fields[]         : name, offset, datatype, count
# is_bigendian     : 바이트 순서
# point_step       : 한 점의 바이트 크기
# row_step         : 한 행의 바이트 크기
# data[]           : 실제 바이너리 (bytes)
# is_dense         : NaN 제거 여부

import numpy as np
import sensor_msgs_py.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2, PointField
from std_msgs.msg import Header


def pc2_to_xyz(msg: PointCloud2) -> np.ndarray:
    """PointCloud2 → (N, 3) float32 numpy array."""
    pts = np.array(list(pc2.read_points(
        msg,
        field_names=("x", "y", "z"),
        skip_nans=True,
    )), dtype=np.float32)
    return pts.reshape(-1, 3) if pts.size else np.empty((0, 3), np.float32)


def xyz_to_pc2(points: np.ndarray, frame_id: str, stamp) -> PointCloud2:
    """(N, 3) → PointCloud2."""
    header = Header()
    header.stamp = stamp
    header.frame_id = frame_id
    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),
    ]
    return pc2.create_cloud(header, fields, points)

import numpy as np


def pc2_to_xyz_fast(msg) -> np.ndarray:
    """
    빠른 경로 / fast path — point_step이 정확히 12B(XYZ float32)인 일반 케이스.
    intensity 등이 섞이면 structured dtype 경로를 쓴다.
    """
    if msg.point_step == 12 and not msg.is_bigendian:
        return np.frombuffer(msg.data, dtype=np.float32).reshape(-1, 3).copy()

    field_to_dtype = {
        1: np.int8, 2: np.uint8, 3: np.int16, 4: np.uint16,
        5: np.int32, 6: np.uint32, 7: np.float32, 8: np.float64,
    }
    dtype_list, cursor = [], 0
    for f in msg.fields:
        if f.offset > cursor:
            dtype_list.append((f"_pad{cursor}", f"V{f.offset - cursor}"))
            cursor = f.offset
        np_dt = field_to_dtype[f.datatype]
        dtype_list.append((f.name, np_dt))
        cursor += np.dtype(np_dt).itemsize
    if msg.point_step > cursor:
        dtype_list.append(("_tail", f"V{msg.point_step - cursor}"))

    s = np.frombuffer(msg.data, dtype=np.dtype(dtype_list))
    xyz = np.stack([s['x'], s['y'], s['z']], axis=-1)
    return xyz.astype(np.float32)

import numpy as np
import open3d as o3d


def preprocess(xyz: np.ndarray, voxel_size: float = 0.005):
    """다운샘플 · outlier 제거 · 노멀 추정 / downsample, denoise, normals."""
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(xyz.astype(np.float64))

    pcd = pcd.voxel_down_sample(voxel_size=voxel_size)
    pcd, _ = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
    pcd.estimate_normals(
        search_param=o3d.geometry.KDTreeSearchParamHybrid(
            radius=voxel_size * 3, max_nn=30))
    pcd.orient_normals_towards_camera_location(np.array([0, 0, 0]))
    return pcd

import numpy as np
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import PointCloud2


class PCProcessor(Node):
    def __init__(self):
        super().__init__("pc_processor")
        self.declare_parameter("voxel_size", 0.005)

        self.sub = self.create_subscription(
            PointCloud2, "/lvs/points",
            self.on_cloud, qos_profile_sensor_data)
        self.pub = self.create_publisher(
            PointCloud2, "/lvs/points_filtered", 5)

    def on_cloud(self, msg: PointCloud2):
        xyz = pc2_to_xyz_fast(msg)
        if xyz.shape[0] < 100:
            return

        vs = self.get_parameter("voxel_size").value
        pcd = preprocess(xyz, voxel_size=vs)
        out_xyz = np.asarray(pcd.points, dtype=np.float32)

        out_msg = xyz_to_pc2(out_xyz,
                             frame_id=msg.header.frame_id,
                             stamp=msg.header.stamp)
        self.pub.publish(out_msg)
        self.get_logger().debug(f"{xyz.shape[0]} → {out_xyz.shape[0]} pts")

"""
PTv3 추론 노드 스케치 / PTv3 inference node sketch
실제 Pointcept API에 맞게 import / input dict 구성을 조정해야 한다.
"""
import numpy as np
import torch
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from rclpy.callback_groups import ReentrantCallbackGroup
from sensor_msgs.msg import PointCloud2, PointField
from std_msgs.msg import Header
import threading


class SegInferenceNode(Node):
    def __init__(self):
        super().__init__("seg_infer")
        self.declare_parameter("model_ckpt", "")
        self.declare_parameter("device", "cuda:0")

        self.device = torch.device(self.get_parameter("device").value)
        self.model = self._load_model(self.get_parameter("model_ckpt").value)
        self.model.eval()

        # 백프레셔 해결: 최신 프레임만 보관
        # backpressure: keep only latest frame
        self._latest = None
        self._lock = threading.Lock()
        cb = ReentrantCallbackGroup()

        self.sub = self.create_subscription(
            PointCloud2, "/lvs/points_filtered",
            self.on_cloud, qos_profile_sensor_data,
            callback_group=cb)
        self.pub = self.create_publisher(
            PointCloud2, "/lvs/points_labeled", 5)

        # 추론 루프 (sub와 별도 콜백) / inference loop
        self.create_timer(0.0, self._infer_loop, callback_group=cb)

    def _load_model(self, ckpt_path: str):
        # Pointcept의 build_model(cfg) 패턴을 그대로 사용
        # 여기서는 의사 코드 / pseudo
        ...

    def on_cloud(self, msg: PointCloud2):
        with self._lock:
            self._latest = msg     # 최신만 / overwrite

    @torch.inference_mode()
    def _infer_loop(self):
        with self._lock:
            msg = self._latest
            self._latest = None
        if msg is None:
            return

        xyz = pc2_to_xyz_fast(msg)
        if xyz.shape[0] < 1024:
            return

        # PTv3 입력은 보통 dict: coord, feat, offset
        coord = torch.from_numpy(xyz).to(self.device)
        feat = coord.clone()
        offset = torch.tensor([coord.shape[0]], device=self.device)

        out = self.model(dict(coord=coord, feat=feat, offset=offset))
        logits = out["seg_logits"] if isinstance(out, dict) else out
        labels = logits.argmax(dim=-1).to(torch.uint8).cpu().numpy()

        # 라벨을 추가 필드로 담은 PointCloud2 / labeled cloud
        labeled = np.empty(xyz.shape[0], dtype=np.dtype([
            ('x', 'f4'), ('y', 'f4'), ('z', 'f4'), ('label', 'u1')
        ]))
        labeled['x'] = xyz[:, 0]
        labeled['y'] = xyz[:, 1]
        labeled['z'] = xyz[:, 2]
        labeled['label'] = labels

        out_msg = PointCloud2()
        out_msg.header = Header(stamp=msg.header.stamp,
                                frame_id=msg.header.frame_id)
        out_msg.height, out_msg.width = 1, labeled.shape[0]
        out_msg.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.UINT8,   count=1),
        ]
        out_msg.is_bigendian = False
        out_msg.point_step = 13
        out_msg.row_step = 13 * labeled.shape[0]
        out_msg.is_dense = True
        out_msg.data = labeled.tobytes()
        self.pub.publish(out_msg)


def main():
    rclpy.init()
    node = SegInferenceNode()
    from rclpy.executors import MultiThreadedExecutor
    ex = MultiThreadedExecutor(num_threads=2)
    ex.add_node(node)
    try:
        ex.spin()
    finally:
        ex.shutdown()

"""
간이 파지 제안 / Minimal grasp proposer
- 대상 클래스 포인트만 추출 → 주성분 분석으로 방향, 센트로이드로 위치 결정.
"""
import numpy as np
from typing import Optional
from geometry_msgs.msg import PoseStamped
from tf_transformations import quaternion_from_matrix


TARGET_CLASS = 1   # 예: 용접 비드 / weld bead


def propose_grasp(xyz: np.ndarray, labels: np.ndarray,
                  frame_id: str, stamp) -> Optional[PoseStamped]:
    mask = labels == TARGET_CLASS
    if mask.sum() < 50:
        return None

    pts = xyz[mask]
    centroid = pts.mean(axis=0)

    # PCA — 점군의 주축 / principal axes
    centered = pts - centroid
    cov = np.cov(centered.T)
    eigvals, eigvecs = np.linalg.eigh(cov)   # ascending order
    # 가장 큰 축 = 대상의 길이 방향 / longest axis
    x_axis = eigvecs[:, 2]
    z_axis = eigvecs[:, 0]   # 가장 작은 분산 = 표면 법선 추정 / surface normal proxy
    # 직교화 / orthogonalize
    y_axis = np.cross(z_axis, x_axis)
    y_axis /= np.linalg.norm(y_axis) + 1e-9
    x_axis = np.cross(y_axis, z_axis)

    # 4x4 변환 / homogeneous transform
    R = np.eye(4)
    R[:3, 0] = x_axis
    R[:3, 1] = y_axis
    R[:3, 2] = z_axis
    qx, qy, qz, qw = quaternion_from_matrix(R)

    pose = PoseStamped()
    pose.header.frame_id = frame_id
    pose.header.stamp = stamp
    pose.pose.position.x = float(centroid[0])
    pose.pose.position.y = float(centroid[1])
    pose.pose.position.z = float(centroid[2])
    pose.pose.orientation.x = qx
    pose.pose.orientation.y = qy
    pose.pose.orientation.z = qz
    pose.pose.orientation.w = qw
    return pose

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.duration import Duration
from sensor_msgs.msg import PointCloud2
from geometry_msgs.msg import PoseStamped
from tf2_ros import Buffer, TransformListener
from tf2_geometry_msgs import do_transform_pose_stamped
from control_msgs.action import FollowJointTrajectory   # 또는 MoveGroup


class Orchestrator(Node):
    def __init__(self):
        super().__init__("orchestrator")
        self.buffer = Buffer()
        self.tf_listener = TransformListener(self.buffer, self)
        self.busy = False

        self.sub = self.create_subscription(
            PointCloud2, "/lvs/points_labeled",
            self.on_labeled, 5)

        # MoveIt 액션 (실제로는 move_action 또는 moveit_py를 권장)
        self.move_client = ActionClient(self,
            FollowJointTrajectory, "/joint_trajectory_controller/follow_joint_trajectory")

    def on_labeled(self, msg: PointCloud2):
        if self.busy:
            return
        xyz, labels = parse_labeled(msg)             # CH.16/17 헬퍼
        pose_cam = propose_grasp(xyz, labels,
                                 msg.header.frame_id,
                                 msg.header.stamp)
        if pose_cam is None:
            return

        # camera_optical → base_link 변환 / TF transform
        try:
            tf = self.buffer.lookup_transform(
                "base_link", pose_cam.header.frame_id,
                pose_cam.header.stamp,
                timeout=Duration(seconds=0.1))
        except Exception as e:
            self.get_logger().warn(f"tf failed: {e}")
            return
        pose_base = do_transform_pose_stamped(pose_cam, tf)

        # 실제로는 moveit_py로 plan & execute 하는 것이 현실적
        # In practice, use moveit_py to plan & execute
        self.busy = True
        self._send_to_moveit(pose_base)

    def _send_to_moveit(self, pose: PoseStamped):
        # 의사 코드 — moveit_py 인스턴스에 위임
        # Pseudo: delegate to moveit_py instance from CH.15
        ...
        self.busy = False