본문 바로가기
Unreal Engine/언리얼-ROS-Physical 통합 프로젝트

[UnrealRobotics: SO-101] (14) Unreal Engine → MoveIt → 실물 로봇: 양방향 디지털 트윈 End-to-End

by 테크앤아트 2026. 5. 13.
728x90
반응형

 

 

 

 

 

이제 프로젝트가 거의 막바지로 접어들었다. Unreal에서 명령이 publish 되면 실제 로봇이 움직이는 과정을 완성할 예정이다.

전체 과정을 정리하면,

Unreal (C++)
→ rosbridge publish "/moveit_goal_pose" (PoseStamped)
→ 중간 Python 노드 (MoveGroupInterface)
→ Plan
→ Execute
→ FollowJointTrajectory action server (이미 전에 만듦)
→ ZMQ
→ worker
→ 실물 로봇 동작

 

 

 

1단계: 중간 ROS2 Python 노드 작성

Unreal이 rosbridge로 간단한 토픽(예: /moveit_goal_pose)에 목표를 publish → 중간 Python 노드가 MoveIt Python API(MoveGroupInterface)로 플래닝+실행 하는 방법으로 진행한다.

 

 

현재 프로젝트에서 Humble MoveIt Python 인터페이스 선택지는 두 가지이다.

 

  • MoveItPy — MoveIt 2 공식 Python 바인딩. 하지만 Humble에서 apt로 제공되는지 불확실하고, 소스 빌드가 필요할 수 있음
  • MoveGroup action client 직접 작성 — moveit_msgs/action/MoveGroup action을 rclpy로 직접 호출. 추가 설치 없이 기존 환경에서 바로 동작. move_group 노드가 이미 demo.launch.py로 띄워져 있으므로 action만 보내면 됨

 

 

 

추가 설치나 소스 빌드 없이 rclpy + moveit_msgs만으로 구현 가능하고, 이미 이전 단계에서 move_group이 정상 동작하는 걸 확인했기 때문에 두 번째 방법으로 진행한다.

중간 Python 노드 (moveit_goal_node.py)
- /moveit_goal_pose (PoseStamped) 구독
- /move_action (MoveGroup action) 호출
- MoveIt move_group이 플래닝 + 실행

 

 

먼저, moveit_msgs가 설치되어 있는지 확인

ros2 interface show moveit_msgs/action/MoveGroup

큰 구조가 나오면 성공

 

 

이 구조 중 우리가 채워할 필드는 일부이다.

Goal (보내는 것):
  request.group_name = "arm"
  request.goal_constraints[0].position_constraints → 목표 위치
  request.goal_constraints[0].orientation_constraints → 목표 자세
  request.num_planning_attempts = 5
  request.allowed_planning_time = 5.0
  request.max_velocity_scaling_factor = 0.3
  planning_options.plan_only = false  (plan + execute)

Result (돌아오는 것):
  error_code.val → 1이면 성공
  planned_trajectory → 실행된 경로

 

 

이 action interface에 rclpy로 goal을 보내는 중간 노드를 작성하면,

Unreal → rosbridge → 토픽 → 중간 노드 → MoveGroup action → move_group → FollowJointTrajectory → ZMQ → worker → 실물 로봇

순서로 동작하게 된다.

 

 

중간 노드 moveit_goal_node.py를 작성

더보기 
#!/usr/bin/env python3
"""
moveit_goal_node.py — 중간 ROS2 노드

Unreal → rosbridge → /moveit_goal_pose (PoseStamped) → 이 노드 → MoveGroup action → move_group

사용법 (ROS2 터미널):
  python3 moveit_goal_node.py

테스트 (별도 ROS2 터미널):
  ros2 topic pub --once /moveit_goal_pose geometry_msgs/msg/PoseStamped \
    '{header: {frame_id: "base_link"}, pose: {position: {x: 0.1, y: 0.0, z: 0.15}, orientation: {w: 1.0}}}'
"""

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.callback_groups import ReentrantCallbackGroup

from geometry_msgs.msg import PoseStamped
from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import (
    Constraints,
    PositionConstraint,
    OrientationConstraint,
    BoundingVolume,
    MoveItErrorCodes,
)
from shape_msgs.msg import SolidPrimitive


class MoveItGoalNode(Node):
    """
    /moveit_goal_pose 토픽을 구독하여 MoveGroup action으로 전달하는 중간 노드.

    Parameters (ROS2 params):
      - group_name: planning group 이름 (default: "arm")
      - end_effector_link: end effector 링크 (default: "gripper_frame_link")
      - planning_frame: 플래닝 기준 프레임 (default: "base_link")
      - num_planning_attempts: 플래닝 시도 횟수 (default: 5)
      - allowed_planning_time: 플래닝 허용 시간 초 (default: 5.0)
      - max_velocity_scaling: 속도 스케일링 (default: 0.3)
      - max_acceleration_scaling: 가속도 스케일링 (default: 0.3)
      - position_tolerance: 위치 허용 오차 m (default: 0.01)
      - orientation_tolerance: 자세 허용 오차 rad (default: 0.05)
    """

    def __init__(self):
        super().__init__('moveit_goal_node')

        # --- Parameters ---
        self.declare_parameter('group_name', 'arm')
        self.declare_parameter('end_effector_link', 'gripper_frame_link')
        self.declare_parameter('planning_frame', 'base_link')
        self.declare_parameter('num_planning_attempts', 5)
        self.declare_parameter('allowed_planning_time', 5.0)
        self.declare_parameter('max_velocity_scaling', 0.3)
        self.declare_parameter('max_acceleration_scaling', 0.3)
        self.declare_parameter('position_tolerance', 0.01)
        self.declare_parameter('orientation_tolerance', 0.05)

        self.group_name = self.get_parameter('group_name').value
        self.ee_link = self.get_parameter('end_effector_link').value
        self.planning_frame = self.get_parameter('planning_frame').value
        self.num_attempts = self.get_parameter('num_planning_attempts').value
        self.planning_time = self.get_parameter('allowed_planning_time').value
        self.vel_scaling = self.get_parameter('max_velocity_scaling').value
        self.acc_scaling = self.get_parameter('max_acceleration_scaling').value
        self.pos_tol = self.get_parameter('position_tolerance').value
        self.orient_tol = self.get_parameter('orientation_tolerance').value

        # --- Callback group (allow concurrent callbacks) ---
        self._cb_group = ReentrantCallbackGroup()

        # --- Action client for MoveGroup ---
        self._action_client = ActionClient(
            self,
            MoveGroup,
            'move_action',
            callback_group=self._cb_group,
        )

        # --- Subscriber ---
        self._sub = self.create_subscription(
            PoseStamped,
            '/moveit_goal_pose',
            self._on_goal_pose,
            10,
            callback_group=self._cb_group,
        )

        # --- State ---
        self._executing = False

        self.get_logger().info(
            f'MoveIt Goal Node ready. '
            f'group={self.group_name}, ee={self.ee_link}, frame={self.planning_frame}'
        )
        self.get_logger().info('Subscribing to /moveit_goal_pose (geometry_msgs/PoseStamped)')
        self.get_logger().info('Waiting for /move_action action server...')

    def _on_goal_pose(self, msg: PoseStamped):
        """토픽 수신 콜백 — MoveGroup action goal 조립 후 전송."""

        if self._executing:
            self.get_logger().warn('Previous motion still executing, ignoring new goal')
            return

        if not self._action_client.wait_for_server(timeout_sec=3.0):
            self.get_logger().error('/move_action server not available!')
            return

        self.get_logger().info(
            f'Received goal: pos=({msg.pose.position.x:.3f}, '
            f'{msg.pose.position.y:.3f}, {msg.pose.position.z:.3f}), '
            f'quat=({msg.pose.orientation.x:.3f}, {msg.pose.orientation.y:.3f}, '
            f'{msg.pose.orientation.z:.3f}, {msg.pose.orientation.w:.3f})'
        )

        # --- Build MoveGroup goal ---
        goal = MoveGroup.Goal()

        # MotionPlanRequest
        req = goal.request
        req.group_name = self.group_name
        req.num_planning_attempts = self.num_attempts
        req.allowed_planning_time = self.planning_time
        req.max_velocity_scaling_factor = self.vel_scaling
        req.max_acceleration_scaling_factor = self.acc_scaling

        # Use frame_id from the incoming message, fallback to planning_frame
        frame_id = msg.header.frame_id if msg.header.frame_id else self.planning_frame

        # --- Position constraint ---
        pos_constraint = PositionConstraint()
        pos_constraint.header.frame_id = frame_id
        pos_constraint.link_name = self.ee_link
        pos_constraint.target_point_offset.x = 0.0
        pos_constraint.target_point_offset.y = 0.0
        pos_constraint.target_point_offset.z = 0.0

        # Bounding volume: small sphere around target position
        bounding = BoundingVolume()
        primitive = SolidPrimitive()
        primitive.type = SolidPrimitive.SPHERE
        primitive.dimensions = [self.pos_tol]
        bounding.primitives.append(primitive)

        from geometry_msgs.msg import Pose
        target_pose = Pose()
        target_pose.position = msg.pose.position
        target_pose.orientation.w = 1.0  # identity for bounding volume pose
        bounding.primitive_poses.append(target_pose)

        pos_constraint.constraint_region = bounding
        pos_constraint.weight = 1.0

        # --- Orientation constraint ---
        orient_constraint = OrientationConstraint()
        orient_constraint.header.frame_id = frame_id
        orient_constraint.link_name = self.ee_link
        orient_constraint.orientation = msg.pose.orientation
        orient_constraint.absolute_x_axis_tolerance = self.orient_tol
        orient_constraint.absolute_y_axis_tolerance = self.orient_tol
        orient_constraint.absolute_z_axis_tolerance = self.orient_tol
        orient_constraint.weight = 1.0

        # --- Assemble constraints ---
        constraints = Constraints()
        constraints.position_constraints.append(pos_constraint)
        constraints.orientation_constraints.append(orient_constraint)
        req.goal_constraints.append(constraints)

        # --- Planning options ---
        goal.planning_options.plan_only = False  # plan + execute
        goal.planning_options.planning_scene_diff.is_diff = True
        goal.planning_options.planning_scene_diff.robot_state.is_diff = True
        goal.planning_options.replan = True
        goal.planning_options.replan_attempts = 3

        # --- Send goal ---
        self._executing = True
        self.get_logger().info('Sending MoveGroup goal...')

        send_future = self._action_client.send_goal_async(
            goal,
            feedback_callback=self._on_feedback,
        )
        send_future.add_done_callback(self._on_goal_response)

    def _on_goal_response(self, future):
        """Action server가 goal을 accept/reject 했을 때."""
        goal_handle = future.result()

        if not goal_handle.accepted:
            self.get_logger().error('MoveGroup goal REJECTED')
            self._executing = False
            return

        self.get_logger().info('MoveGroup goal accepted, waiting for result...')
        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(self._on_result)

    def _on_feedback(self, feedback_msg):
        """실행 중 피드백."""
        state = feedback_msg.feedback.state
        if state:
            self.get_logger().info(f'MoveGroup feedback: {state}')

    def _on_result(self, future):
        """실행 완료."""
        result = future.result().result
        error_code = result.error_code.val

        if error_code == MoveItErrorCodes.SUCCESS:
            self.get_logger().info(
                f'Motion SUCCEEDED! Planning time: {result.planning_time:.2f}s'
            )
        else:
            error_name = self._error_code_to_string(error_code)
            self.get_logger().error(
                f'Motion FAILED: {error_name} (code={error_code})'
            )

        self._executing = False

    @staticmethod
    def _error_code_to_string(code: int) -> str:
        """MoveItErrorCodes int → 사람이 읽을 수 있는 문자열."""
        code_map = {
            1: 'SUCCESS',
            -1: 'PLANNING_FAILED',
            -2: 'INVALID_MOTION_PLAN',
            -3: 'MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE',
            -4: 'CONTROL_FAILED',
            -5: 'UNABLE_TO_ACQUIRE_SENSOR_DATA',
            -6: 'TIMED_OUT',
            -7: 'PREEMPTED',
            -10: 'START_STATE_IN_COLLISION',
            -11: 'START_STATE_VIOLATES_PATH_CONSTRAINTS',
            -12: 'GOAL_IN_COLLISION',
            -13: 'GOAL_VIOLATES_PATH_CONSTRAINTS',
            -14: 'GOAL_CONSTRAINTS_VIOLATED',
            -15: 'INVALID_GROUP_NAME',
            -16: 'INVALID_GOAL_CONSTRAINTS',
            -17: 'INVALID_ROBOT_STATE',
            -18: 'INVALID_LINK_NAME',
            -19: 'INVALID_OBJECT_NAME',
            -21: 'FRAME_TRANSFORM_FAILURE',
            -22: 'COLLISION_CHECKING_UNAVAILABLE',
            -23: 'ROBOT_STATE_STALE',
            -24: 'SENSOR_INFO_STALE',
            -25: 'COMMUNICATION_FAILURE',
            -26: 'START_STATE_INVALID',
            -27: 'GOAL_STATE_INVALID',
            -28: 'UNRECOGNIZED_GOAL_TYPE',
            -29: 'CRASH',
            -30: 'ABORT',
            -31: 'NO_IK_SOLUTION',
            99999: 'FAILURE',
        }
        return code_map.get(code, f'UNKNOWN({code})')


def main():
    rclpy.init()
    node = MoveItGoalNode()

    # MultiThreadedExecutor for ReentrantCallbackGroup
    from rclpy.executors import MultiThreadedExecutor
    executor = MultiThreadedExecutor()
    executor.add_node(node)

    try:
        executor.spin()
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

파이썬 파일을 ~/UnrealRobotics/src/lerobot_ros2_bridge/scripts/ 경로에 배치

 

 

 

  • /moveit_goal_pose (geometry_msgs/PoseStamped) 구독
  • 수신하면 MoveGroup.Goal을 조립 - group_name="arm", end_effector="gripper_frame_link", position/orientation constraint 설정
  • /move_action action server에 goal 전송 (plan + execute)
  • 결과 로그 출력 (SUCCESS / 에러코드)
  • 이전 동작 실행 중이면 새 goal 무시 (안전)

 

 

 

Plan이 잘 작동하는지 로봇을 연결해서 확인해본다.

 

 

로봇 연결

#PowerShell에서
Attach-Both

 

 

터미널 1 (conda - worker):

source ~/miniforge3/etc/profile.d/conda.sh
conda activate lerobot
sudo chmod 666 /dev/ttyACM*
cd ~/UnrealRobotics/src/lerobot_ros2_bridge
python scripts/lerobot_worker.py --cmd-joints all --cmd-limit 120

 

 

터미널 2 (ROS2 - bridge node):

export PATH=$(echo $PATH | tr ':' '\n' | grep -v miniforge | tr '\n' ':' | sed 's/:$//')
source /opt/ros/humble/setup.bash
source ~/UnrealRobotics/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_LOCALHOST_ONLY=1
ros2 run lerobot_ros2_bridge bridge_node

 

 

터미널 3 (ROS2 - action server):

export PATH=$(echo $PATH | tr ':' '\n' | grep -v miniforge | tr '\n' ':' | sed 's/:$//')
source /opt/ros/humble/setup.bash
source ~/UnrealRobotics/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_LOCALHOST_ONLY=1
python3 ~/UnrealRobotics/src/lerobot_ros2_bridge/scripts/joint_trajectory_action_server.py

 

 

터미널 4 (ROS2 - MoveIt demo):

export PATH=$(echo $PATH | tr ':' '\n' | grep -v miniforge | tr '\n' ':' | sed 's/:$//')
source /opt/ros/humble/setup.bash
source ~/UnrealRobotics/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_LOCALHOST_ONLY=1
ros2 launch so101_moveit_config demo.launch.py

 

 

터미널 5 (ROS2 - robot_state_publisher):

export PATH=$(echo $PATH | tr ':' '\n' | grep -v miniforge | tr '\n' ':' | sed 's/:$//')
source /opt/ros/humble/setup.bash
source ~/UnrealRobotics/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_LOCALHOST_ONLY=1
xacro ~/UnrealRobotics/src/so101_description/urdf/so101_arm.urdf.xacro variant:=follower > /tmp/so101_follower.urdf
python3 -c "
import yaml
with open('/tmp/so101_follower.urdf') as f: urdf = f.read()
params = {'robot_state_publisher': {'ros__parameters': {'robot_description': urdf}}}
with open('/tmp/rsp_params.yaml', 'w') as f: yaml.dump(params, f, default_flow_style=False)
"
ros2 run robot_state_publisher robot_state_publisher --ros-args --params-file /tmp/rsp_params.yaml

 

 

터미널 6 (ROS2 - moveit_goal_node):

export PATH=$(echo $PATH | tr ':' '\n' | grep -v miniforge | tr '\n' ':' | sed 's/:$//')
source /opt/ros/humble/setup.bash
source ~/UnrealRobotics/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_LOCALHOST_ONLY=1
python3 ~/UnrealRobotics/src/lerobot_ros2_bridge/scripts/moveit_goal_node.py

 

 

여기까지 총 6개의 터미널 실행 완료.

 

 

터미널 7 (ROS2 - 테스트):

base_link 기준으로 (x=0.1m, y=0, z=0.15m) 위치에 end effector를 보내라는 목표

export PATH=$(echo $PATH | tr ':' '\n' | grep -v miniforge | tr '\n' ':' | sed 's/:$//')
source /opt/ros/humble/setup.bash
source ~/UnrealRobotics/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_LOCALHOST_ONLY=1
ros2 topic pub --once /moveit_goal_pose geometry_msgs/msg/PoseStamped '{header: {frame_id: "base_link"}, pose: {position: {x: 0.1, y: 0.0, z: 0.15}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}'

 

*실행 했지만 터미널 6에서 에러 발생

터미널 4에서도 에러 메시지 확인 가능

문제의 원인은 Cartesian pose goal (x,y,z 좌표)로 보내면 IK solver가 그 위치에 도달하면서 orientation까지 만족하는 해를 찾아야 하는데, SO-ARM-101 같은 5DOF arm은 임의의 6DOF pose를 만족시킬 수 없다. 그래서 "valid states를 샘플링 못함" 에러가 나온 것으로 보임

해결 방법: Cartesian pose 대신 Joint space goal로 보내는 게 이 로봇에 맞다. 이전에 home/ready named target으로 성공한 것도 joint space goal이었기 때문.

 

  • /moveit_goal_pose (PoseStamped) → Cartesian goal ❌ (5DOF에서 불안정)
  • /moveit_goal_joints (JointState) → Joint space goal ✅ (확실히 동작)
  • 추후 Cartesian goal도 지원하되, orientation constraint를 제거하고 position-only로

moveit_goal_node.py를 수정해서 joint space goal을 기본으로, Cartesian goal은 position-only 옵션으로 지원하도록 한다.

 

 

moveit_goal_node.py 수정

더보기 
#!/usr/bin/env python3
"""
moveit_goal_node.py — 중간 ROS2 노드

SO-ARM-101은 5DOF arm이므로 임의의 6DOF Cartesian pose를 만족시킬 수 없음.
따라서 Joint space goal을 기본으로 사용하고, Cartesian goal은 position-only 모드로 지원.

=== 토픽 인터페이스 ===

1) /moveit_goal_joints (sensor_msgs/JointState) — Joint space goal (추천)
   - name: 목표로 설정할 관절 이름 배열
   - position: 각 관절의 목표 각도 (radians)

2) /moveit_goal_pose (geometry_msgs/PoseStamped) — Cartesian position-only goal
   - position만 사용, orientation은 무시 (5DOF 제약)

3) /moveit_goal_named (std_msgs/String) — Named target (SRDF에 정의된 포즈)
   - "home", "ready" 등

=== 테스트 명령 (ROS2 터미널) ===

# Named target (가장 간단):
ros2 topic pub --once /moveit_goal_named std_msgs/msg/String '{data: "home"}'
ros2 topic pub --once /moveit_goal_named std_msgs/msg/String '{data: "ready"}'

# Joint space goal (radians):
ros2 topic pub --once /moveit_goal_joints sensor_msgs/msg/JointState \
  '{name: ["shoulder_pan","shoulder_lift","elbow_flex","wrist_flex","wrist_roll"], \
    position: [0.0, -0.5, 0.5, 0.0, 0.0]}'

# Cartesian position-only goal (meters, base_link 기준):
ros2 topic pub --once /moveit_goal_pose geometry_msgs/msg/PoseStamped \
  '{header: {frame_id: "base_link"}, pose: {position: {x: 0.1, y: 0.0, z: 0.15}}}'
"""

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.callback_groups import ReentrantCallbackGroup

from geometry_msgs.msg import PoseStamped, Pose
from sensor_msgs.msg import JointState
from std_msgs.msg import String
from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import (
    Constraints,
    JointConstraint,
    PositionConstraint,
    BoundingVolume,
    MoveItErrorCodes,
)
from shape_msgs.msg import SolidPrimitive


# SO-ARM-101 arm group joints (from SRDF)
ARM_JOINTS = [
    'shoulder_pan',
    'shoulder_lift',
    'elbow_flex',
    'wrist_flex',
    'wrist_roll',
]

# Named targets (from SRDF — Phase 7.2)
NAMED_TARGETS = {
    'home': {
        'shoulder_pan': 0.0,
        'shoulder_lift': 0.0,
        'elbow_flex': 0.0,
        'wrist_flex': 0.0,
        'wrist_roll': 0.0,
    },
    'ready': {
        'shoulder_pan': 0.0,
        'shoulder_lift': -0.5,
        'elbow_flex': 0.5,
        'wrist_flex': 0.0,
        'wrist_roll': 0.0,
    },
}


class MoveItGoalNode(Node):
    """
    3가지 토픽을 구독하여 MoveGroup action으로 전달하는 중간 노드.
    - /moveit_goal_joints  (sensor_msgs/JointState)   → joint space goal
    - /moveit_goal_pose    (geometry_msgs/PoseStamped) → Cartesian position-only goal
    - /moveit_goal_named   (std_msgs/String)           → named target
    """

    def __init__(self):
        super().__init__('moveit_goal_node')

        # --- Parameters ---
        self.declare_parameter('group_name', 'arm')
        self.declare_parameter('end_effector_link', 'gripper_frame_link')
        self.declare_parameter('planning_frame', 'base_link')
        self.declare_parameter('num_planning_attempts', 5)
        self.declare_parameter('allowed_planning_time', 5.0)
        self.declare_parameter('max_velocity_scaling', 0.3)
        self.declare_parameter('max_acceleration_scaling', 0.3)
        self.declare_parameter('position_tolerance', 0.01)
        self.declare_parameter('joint_tolerance', 0.01)

        self.group_name = self.get_parameter('group_name').value
        self.ee_link = self.get_parameter('end_effector_link').value
        self.planning_frame = self.get_parameter('planning_frame').value
        self.num_attempts = self.get_parameter('num_planning_attempts').value
        self.planning_time = self.get_parameter('allowed_planning_time').value
        self.vel_scaling = self.get_parameter('max_velocity_scaling').value
        self.acc_scaling = self.get_parameter('max_acceleration_scaling').value
        self.pos_tol = self.get_parameter('position_tolerance').value
        self.joint_tol = self.get_parameter('joint_tolerance').value

        # --- Callback group ---
        self._cb_group = ReentrantCallbackGroup()

        # --- Action client ---
        self._action_client = ActionClient(
            self, MoveGroup, 'move_action',
            callback_group=self._cb_group,
        )

        # --- Subscribers ---
        self.create_subscription(
            JointState, '/moveit_goal_joints', self._on_goal_joints, 10,
            callback_group=self._cb_group,
        )
        self.create_subscription(
            PoseStamped, '/moveit_goal_pose', self._on_goal_pose, 10,
            callback_group=self._cb_group,
        )
        self.create_subscription(
            String, '/moveit_goal_named', self._on_goal_named, 10,
            callback_group=self._cb_group,
        )

        # --- State ---
        self._executing = False

        self.get_logger().info(
            f'MoveIt Goal Node ready. group={self.group_name}, ee={self.ee_link}'
        )
        self.get_logger().info('Listening on:')
        self.get_logger().info('  /moveit_goal_joints  (sensor_msgs/JointState)')
        self.get_logger().info('  /moveit_goal_pose    (geometry_msgs/PoseStamped) — position only')
        self.get_logger().info('  /moveit_goal_named   (std_msgs/String) — "home", "ready"')

    # ─────────────────────────────────────────────
    # Topic callbacks
    # ─────────────────────────────────────────────

    def _on_goal_named(self, msg: String):
        """Named target → joint space goal로 변환."""
        name = msg.data.strip().lower()

        if name not in NAMED_TARGETS:
            self.get_logger().error(
                f'Unknown named target "{name}". Available: {list(NAMED_TARGETS.keys())}'
            )
            return

        self.get_logger().info(f'Named target: "{name}"')
        joint_values = NAMED_TARGETS[name]
        self._send_joint_goal(joint_values)

    def _on_goal_joints(self, msg: JointState):
        """Joint space goal — name/position 쌍으로 목표 설정."""
        if len(msg.name) == 0 or len(msg.name) != len(msg.position):
            self.get_logger().error('Invalid JointState: name/position length mismatch or empty')
            return

        joint_values = {}
        for name, pos in zip(msg.name, msg.position):
            if name not in ARM_JOINTS:
                self.get_logger().warn(f'Ignoring unknown joint: {name}')
                continue
            joint_values[name] = pos

        if not joint_values:
            self.get_logger().error('No valid arm joints in message')
            return

        names_str = ', '.join(f'{k}={v:.3f}' for k, v in joint_values.items())
        self.get_logger().info(f'Joint goal: {names_str}')
        self._send_joint_goal(joint_values)

    def _on_goal_pose(self, msg: PoseStamped):
        """Cartesian position-only goal (orientation 무시 — 5DOF 제약)."""
        self.get_logger().info(
            f'Cartesian goal (position only): '
            f'({msg.pose.position.x:.3f}, {msg.pose.position.y:.3f}, {msg.pose.position.z:.3f})'
        )
        self._send_pose_goal(msg)

    # ─────────────────────────────────────────────
    # Goal builders
    # ─────────────────────────────────────────────

    def _send_joint_goal(self, joint_values: dict):
        """Joint space goal을 MoveGroup action으로 전송."""
        if not self._pre_send_check():
            return

        goal = self._make_base_goal()
        req = goal.request

        constraints = Constraints()
        for joint_name, position in joint_values.items():
            jc = JointConstraint()
            jc.joint_name = joint_name
            jc.position = position
            jc.tolerance_above = self.joint_tol
            jc.tolerance_below = self.joint_tol
            jc.weight = 1.0
            constraints.joint_constraints.append(jc)

        req.goal_constraints.append(constraints)
        self._send_goal(goal)

    def _send_pose_goal(self, msg: PoseStamped):
        """Cartesian position-only goal — orientation constraint 없음 (5DOF)."""
        if not self._pre_send_check():
            return

        goal = self._make_base_goal()
        req = goal.request

        frame_id = msg.header.frame_id if msg.header.frame_id else self.planning_frame

        # Position constraint only (no orientation — 5DOF arm)
        pos_constraint = PositionConstraint()
        pos_constraint.header.frame_id = frame_id
        pos_constraint.link_name = self.ee_link
        pos_constraint.target_point_offset.x = 0.0
        pos_constraint.target_point_offset.y = 0.0
        pos_constraint.target_point_offset.z = 0.0

        bounding = BoundingVolume()
        primitive = SolidPrimitive()
        primitive.type = SolidPrimitive.SPHERE
        primitive.dimensions = [self.pos_tol]
        bounding.primitives.append(primitive)

        target_pose = Pose()
        target_pose.position = msg.pose.position
        target_pose.orientation.w = 1.0
        bounding.primitive_poses.append(target_pose)

        pos_constraint.constraint_region = bounding
        pos_constraint.weight = 1.0

        constraints = Constraints()
        constraints.position_constraints.append(pos_constraint)
        req.goal_constraints.append(constraints)

        self._send_goal(goal)

    # ─────────────────────────────────────────────
    # Helpers
    # ─────────────────────────────────────────────

    def _pre_send_check(self) -> bool:
        """공통 사전 검사."""
        if self._executing:
            self.get_logger().warn('Previous motion still executing, ignoring new goal')
            return False
        if not self._action_client.wait_for_server(timeout_sec=3.0):
            self.get_logger().error('/move_action server not available!')
            return False
        return True

    def _make_base_goal(self) -> MoveGroup.Goal:
        """공통 MoveGroup goal 템플릿."""
        goal = MoveGroup.Goal()
        req = goal.request
        req.group_name = self.group_name
        req.num_planning_attempts = self.num_attempts
        req.allowed_planning_time = self.planning_time
        req.max_velocity_scaling_factor = self.vel_scaling
        req.max_acceleration_scaling_factor = self.acc_scaling

        goal.planning_options.plan_only = False
        goal.planning_options.planning_scene_diff.is_diff = True
        goal.planning_options.planning_scene_diff.robot_state.is_diff = True
        goal.planning_options.replan = True
        goal.planning_options.replan_attempts = 3

        return goal

    def _send_goal(self, goal: MoveGroup.Goal):
        """Action goal 전송."""
        self._executing = True
        self.get_logger().info('Sending MoveGroup goal...')
        send_future = self._action_client.send_goal_async(
            goal, feedback_callback=self._on_feedback,
        )
        send_future.add_done_callback(self._on_goal_response)

    def _on_goal_response(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().error('MoveGroup goal REJECTED')
            self._executing = False
            return
        self.get_logger().info('MoveGroup goal accepted, waiting for result...')
        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(self._on_result)

    def _on_feedback(self, feedback_msg):
        state = feedback_msg.feedback.state
        if state:
            self.get_logger().info(f'MoveGroup feedback: {state}')

    def _on_result(self, future):
        result = future.result().result
        error_code = result.error_code.val

        if error_code == MoveItErrorCodes.SUCCESS:
            self.get_logger().info(
                f'Motion SUCCEEDED! Planning time: {result.planning_time:.2f}s'
            )
        else:
            error_name = self._error_code_to_string(error_code)
            self.get_logger().error(
                f'Motion FAILED: {error_name} (code={error_code})'
            )

        self._executing = False

    @staticmethod
    def _error_code_to_string(code: int) -> str:
        code_map = {
            1: 'SUCCESS',
            -1: 'PLANNING_FAILED',
            -2: 'INVALID_MOTION_PLAN',
            -3: 'MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE',
            -4: 'CONTROL_FAILED',
            -5: 'UNABLE_TO_ACQUIRE_SENSOR_DATA',
            -6: 'TIMED_OUT',
            -7: 'PREEMPTED',
            -10: 'START_STATE_IN_COLLISION',
            -11: 'START_STATE_VIOLATES_PATH_CONSTRAINTS',
            -12: 'GOAL_IN_COLLISION',
            -13: 'GOAL_VIOLATES_PATH_CONSTRAINTS',
            -14: 'GOAL_CONSTRAINTS_VIOLATED',
            -15: 'INVALID_GROUP_NAME',
            -16: 'INVALID_GOAL_CONSTRAINTS',
            -17: 'INVALID_ROBOT_STATE',
            -18: 'INVALID_LINK_NAME',
            -19: 'INVALID_OBJECT_NAME',
            -21: 'FRAME_TRANSFORM_FAILURE',
            -22: 'COLLISION_CHECKING_UNAVAILABLE',
            -23: 'ROBOT_STATE_STALE',
            -24: 'SENSOR_INFO_STALE',
            -25: 'COMMUNICATION_FAILURE',
            -26: 'START_STATE_INVALID',
            -27: 'GOAL_STATE_INVALID',
            -28: 'UNRECOGNIZED_GOAL_TYPE',
            -29: 'CRASH',
            -30: 'ABORT',
            -31: 'NO_IK_SOLUTION',
            99999: 'FAILURE',
        }
        return code_map.get(code, f'UNKNOWN({code})')


def main():
    rclpy.init()
    node = MoveItGoalNode()

    from rclpy.executors import MultiThreadedExecutor
    executor = MultiThreadedExecutor()
    executor.add_node(node)

    try:
        executor.spin()
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

변경 내용

 

  • 3가지 토픽 지원: /moveit_goal_named (String), /moveit_goal_joints (JointState), /moveit_goal_pose (PoseStamped - position only)
  • Cartesian goal에서 orientation constraint 제거 - 5DOF arm에서 IK 실패 방지
  • Named target 내장 - "home", "ready"를 joint값으로 변환

 

 

 

터미널 6 (재시작):

python3 ~/UnrealRobotics/src/lerobot_ros2_bridge/scripts/moveit_goal_node.py

 

 

터미널 7 (named target으로 테스트):

ros2 topic pub --once /moveit_goal_named std_msgs/msg/String '{data: "ready"}'

성공했다면 터미널 6에서 SUCCEEDED 글자를 볼 수 있고, 로봇에 ready 포지션으로 움직인다. MoveIt RViz에서도 동작을 확인할 수 있다.

 

 

터미널 7에서 home 포지션 이름으로 테스트

ros2 topic pub --once /moveit_goal_named std_msgs/msg/String '{data: "home"}'

 

 

잘 되었다면 터미널 7에서 joint space goal도 테스트

ros2 topic pub --once /moveit_goal_joints sensor_msgs/msg/JointState '{name: ["shoulder_pan","shoulder_lift","elbow_flex","wrist_flex","wrist_roll"], position: [0.3, -0.5, 0.5, 0.2, 0.0]}'

성공했다. 로봇이 움직였고, 움직이는 경로가 MoveIt에도 잘 표시되었다.

 

 

 

 

2단계: Unreal Publish 연동

이번 단계에서 해야할 일

 

  • Unreal C++에서 rosbridge를 통해 /moveit_goal_named, /moveit_goal_joints에 publish
  • 이전에 만든 Advertise()/Publish() API 그대로 사용 (RosBridgeSubsystem.h/cpp)
  • 우선 하드코딩된 테스트 명령으로 검증 (예: 키 입력으로 "home"/"ready" 전환)

 

 

ARobotVisualizer에 MoveIt 명령 기능을 추가한다.

 

 

RobotVisualizer.h 수정

 

더보기 
#pragma once

#include "CoreMinimal.h"
#include "GameFramework/Actor.h"
#include "RobotVisualizer.generated.h"

class UStaticMesh;
class UStaticMeshComponent;
class USceneComponent;
class URosBridgeSubsystem;

/**
 * Visualizes the SO-ARM-101 follower arm in Unreal Engine and provides
 * MoveIt command interface via rosbridge.
 *
 * The component hierarchy mirrors the URDF link/joint structure:
 *   BaseLink -> ShoulderPanJoint -> ShoulderLink -> ShoulderLiftJoint -> ...
 *
 * Each "Joint" SceneComponent is where the ROS joint angle gets applied
 * as a local Z-axis rotation. All child links/meshes rotate with it.
 *
 * Phase 8 additions:
 *   - SendNamedTarget(): publish to /moveit_goal_named (std_msgs/String)
 *   - SendJointGoal(): publish to /moveit_goal_joints (sensor_msgs/JointState)
 *   - SendPoseGoal(): publish to /moveit_goal_pose (geometry_msgs/PoseStamped)
 *   - Blueprint-callable + editor-testable via UPROPERTY buttons
 */
UCLASS()
class SO101_TWIN_API ARobotVisualizer : public AActor
{
	GENERATED_BODY()

public:
	ARobotVisualizer();

protected:
	virtual void BeginPlay() override;
	virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override;

	// =================================================================
	// Configuration
	// =================================================================

	UPROPERTY(EditAnywhere, Category = "ROS|Bridge")
	FString RosBridgeUrl = TEXT("ws://127.0.0.1:9090/?x=1");

	UPROPERTY(EditAnywhere, Category = "ROS|Topics")
	FString JointStateTopic = TEXT("/joint_states");

	UPROPERTY(EditAnywhere, Category = "ROS|Topics")
	FString JointStateType = TEXT("sensor_msgs/JointState");

	// =================================================================
	// MoveIt Command Interface (Phase 8)
	// =================================================================

	// --- Named target ---

	/** Named target to send (e.g. "home", "ready"). Set in Details panel, then call SendNamedTarget(). */
	UPROPERTY(EditAnywhere, Category = "ROS|MoveIt")
	FString MoveItNamedTarget = TEXT("home");

	/** Send the named target to MoveIt via /moveit_goal_named topic. */
	UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|MoveIt")
	void SendNamedTarget();

	// --- Joint goal ---

	/** Joint goal values in radians. Set in Details panel, then call SendJointGoal(). */
	UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Joints")
	float GoalShoulderPan = 0.0f;

	UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Joints")
	float GoalShoulderLift = 0.0f;

	UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Joints")
	float GoalElbowFlex = 0.0f;

	UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Joints")
	float GoalWristFlex = 0.0f;

	UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Joints")
	float GoalWristRoll = 0.0f;

	/** Send joint goal to MoveIt via /moveit_goal_joints topic. Values are in radians. */
	UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|MoveIt|Joints")
	void SendJointGoal();

	// --- Pose goal (Cartesian, position-only for 5DOF) ---

	/** Target position in UE coordinates (cm). Converted to ROS (meters) on send. */
	UPROPERTY(EditAnywhere, Category = "ROS|MoveIt|Pose")
	FVector GoalPositionUE = FVector(10.0f, 0.0f, 15.0f);

	/** Send position-only goal to MoveIt via /moveit_goal_pose topic.
	 *  GoalPositionUE is in Unreal cm, auto-converted to ROS meters with Y flip. */
	UFUNCTION(BlueprintCallable, CallInEditor, Category = "ROS|MoveIt|Pose")
	void SendPoseGoal();

private:
	// =================================================================
	// Component hierarchy
	// =================================================================

	UPROPERTY(VisibleAnywhere, Category = "Robot")
	TObjectPtr<USceneComponent> RobotRoot;

	// Link SceneComponents
	UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
	TObjectPtr<USceneComponent> BaseLink;

	UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
	TObjectPtr<USceneComponent> ShoulderLink;

	UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
	TObjectPtr<USceneComponent> UpperArmLink;

	UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
	TObjectPtr<USceneComponent> LowerArmLink;

	UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
	TObjectPtr<USceneComponent> WristLink;

	UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
	TObjectPtr<USceneComponent> GripperLink;

	UPROPERTY(VisibleAnywhere, Category = "Robot|Links")
	TObjectPtr<USceneComponent> MovingJawLink;

	// Joint SceneComponents
	UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
	TObjectPtr<USceneComponent> ShoulderPanJoint;

	UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
	TObjectPtr<USceneComponent> ShoulderLiftJoint;

	UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
	TObjectPtr<USceneComponent> ElbowFlexJoint;

	UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
	TObjectPtr<USceneComponent> WristFlexJoint;

	UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
	TObjectPtr<USceneComponent> WristRollJoint;

	UPROPERTY(VisibleAnywhere, Category = "Robot|Joints")
	TObjectPtr<USceneComponent> GripperJoint;

	// Joint name -> component mapping
	UPROPERTY()
	TMap<FName, TObjectPtr<USceneComponent>> JointComponentMap;

	// Mesh components
	UPROPERTY()
	TArray<TObjectPtr<UStaticMeshComponent>> AllMeshComponents;

	// =================================================================
	// ROS connection
	// =================================================================

	UFUNCTION()
	void OnRosBridgeConnected();

	UFUNCTION()
	void OnRosMessage(const FString& Topic, const FString& MessageJson);

	void ParseAndApplyJointStates(const FString& MessageJson);

	// =================================================================
	// MoveIt publish helpers
	// =================================================================

	/** Advertise MoveIt command topics. Called once on connect. */
	void AdvertiseMoveItTopics();

	/** Whether MoveIt topics have been advertised in this connection session. */
	bool bMoveItTopicsAdvertised = false;

	// =================================================================
	// Helpers (declared in original header, kept for compatibility)
	// =================================================================

	USceneComponent* CreateJointComponent(const FName& Name, USceneComponent* Parent,
		const FVector& Location, const FRotator& Rotation);

	USceneComponent* CreateLinkComponent(const FName& Name, USceneComponent* Parent);

	UStaticMeshComponent* AttachMesh(USceneComponent* Parent, UStaticMesh* Mesh,
		const FName& Name, const FVector& Location, const FRotator& Rotation,
		bool bIsMotor);
};

 

 

RobotVisualizer.cpp 수정

#include "RobotVisualizer.h"
#include "RosCoordConv.h"
#include "RosBridgeSubsystem.h"
#include "RosBridgeLog.h"

#include "Components/SceneComponent.h"
#include "Components/StaticMeshComponent.h"
#include "Engine/StaticMesh.h"
#include "Engine/Engine.h"
#include "Engine/World.h"
#include "Kismet/GameplayStatics.h"
#include "Dom/JsonObject.h"
#include "Dom/JsonValue.h"
#include "Serialization/JsonReader.h"
#include "Serialization/JsonSerializer.h"
#include "Serialization/JsonWriter.h"
#include "UObject/ConstructorHelpers.h"

// =============================================================================
// Mesh asset path helper
// =============================================================================

static UStaticMesh* LoadMeshAsset(const TCHAR* AssetName)
{
	// All meshes live under /Game/Robot/Meshes/
	FString Path = FString::Printf(TEXT("/Game/Robot/Meshes/%s.%s"), AssetName, AssetName);
	UStaticMesh* Mesh = Cast<UStaticMesh>(StaticLoadObject(UStaticMesh::StaticClass(), nullptr, *Path));
	if (!Mesh)
	{
		UE_LOG(LogRosBridge, Warning, TEXT("Failed to load mesh: %s"), *Path);
	}
	return Mesh;
}

// =============================================================================
// Constructor — build the entire component hierarchy
// =============================================================================

ARobotVisualizer::ARobotVisualizer()
{
	PrimaryActorTick.bCanEverTick = false;

	// --- Root ---
	RobotRoot = CreateDefaultSubobject<USceneComponent>(TEXT("RobotRoot"));
	RootComponent = RobotRoot;

	// =========================================================================
	// URDF data converted to UE coordinates:
	//   Position: meters * 100 = cm, Y flipped
	//   Rotation: RPY radians -> FRotator degrees, pitch & yaw negated
	//
	// All values below are pre-computed from so101_follower.urdf.
	// =========================================================================

	// --- base_link (attached directly to root, no joint) ---
	BaseLink = CreateDefaultSubobject<USceneComponent>(TEXT("BaseLink"));
	BaseLink->SetupAttachment(RobotRoot);

	// --- shoulder_pan joint ---
	// URDF origin: xyz(0.0388353, 0, 0.0624) rpy(3.14159, 0, -3.14159)
	ShoulderPanJoint = CreateDefaultSubobject<USceneComponent>(TEXT("ShoulderPanJoint"));
	ShoulderPanJoint->SetupAttachment(BaseLink);
	ShoulderPanJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(0.0388353, 0.0, 0.0624));
	ShoulderPanJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(3.14159, 0.0, -3.14159));

	ShoulderLink = CreateDefaultSubobject<USceneComponent>(TEXT("ShoulderLink"));
	ShoulderLink->SetupAttachment(ShoulderPanJoint);

	// --- shoulder_lift joint ---
	// URDF origin: xyz(-0.0303992, -0.0182778, -0.0542) rpy(-1.5708, -1.5708, 0)
	ShoulderLiftJoint = CreateDefaultSubobject<USceneComponent>(TEXT("ShoulderLiftJoint"));
	ShoulderLiftJoint->SetupAttachment(ShoulderLink);
	ShoulderLiftJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(-0.0303992, -0.0182778, -0.0542));
	ShoulderLiftJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(-1.5708, -1.5708, 0.0));

	UpperArmLink = CreateDefaultSubobject<USceneComponent>(TEXT("UpperArmLink"));
	UpperArmLink->SetupAttachment(ShoulderLiftJoint);

	// --- elbow_flex joint ---
	// URDF origin: xyz(-0.11257, -0.028, 0) rpy(0, 0, 1.5708)
	ElbowFlexJoint = CreateDefaultSubobject<USceneComponent>(TEXT("ElbowFlexJoint"));
	ElbowFlexJoint->SetupAttachment(UpperArmLink);
	ElbowFlexJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(-0.11257, -0.028, 0.0));
	ElbowFlexJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(0.0, 0.0, 1.5708));

	LowerArmLink = CreateDefaultSubobject<USceneComponent>(TEXT("LowerArmLink"));
	LowerArmLink->SetupAttachment(ElbowFlexJoint);

	// --- wrist_flex joint ---
	// URDF origin: xyz(-0.1349, 0.0052, 0) rpy(0, 0, -1.5708)
	WristFlexJoint = CreateDefaultSubobject<USceneComponent>(TEXT("WristFlexJoint"));
	WristFlexJoint->SetupAttachment(LowerArmLink);
	WristFlexJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(-0.1349, 0.0052, 0.0));
	WristFlexJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(0.0, 0.0, -1.5708));

	WristLink = CreateDefaultSubobject<USceneComponent>(TEXT("WristLink"));
	WristLink->SetupAttachment(WristFlexJoint);

	// --- wrist_roll joint ---
	// URDF origin: xyz(0, -0.0611, 0.0181) rpy(1.5708, 0.0486795, 3.14159)
	WristRollJoint = CreateDefaultSubobject<USceneComponent>(TEXT("WristRollJoint"));
	WristRollJoint->SetupAttachment(WristLink);
	WristRollJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(0.0, -0.0611, 0.0181));
	WristRollJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(1.5708, 0.0486795, 3.14159));

	GripperLink = CreateDefaultSubobject<USceneComponent>(TEXT("GripperLink"));
	GripperLink->SetupAttachment(WristRollJoint);

	// --- gripper joint ---
	// URDF origin: xyz(0.0202, 0.0188, -0.0234) rpy(1.5708, 0, 0)
	GripperJoint = CreateDefaultSubobject<USceneComponent>(TEXT("GripperJoint"));
	GripperJoint->SetupAttachment(GripperLink);
	GripperJoint->SetRelativeLocation(RosCoordConv::RosToUePosition(0.0202, 0.0188, -0.0234));
	GripperJoint->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(1.5708, 0.0, 0.0));

	MovingJawLink = CreateDefaultSubobject<USceneComponent>(TEXT("MovingJawLink"));
	MovingJawLink->SetupAttachment(GripperJoint);

	// --- Joint name mapping (matches ROS /joint_states names) ---
	JointComponentMap.Add(FName("shoulder_pan"),  ShoulderPanJoint);
	JointComponentMap.Add(FName("shoulder_lift"), ShoulderLiftJoint);
	JointComponentMap.Add(FName("elbow_flex"),    ElbowFlexJoint);
	JointComponentMap.Add(FName("wrist_flex"),    WristFlexJoint);
	JointComponentMap.Add(FName("wrist_roll"),    WristRollJoint);
	JointComponentMap.Add(FName("gripper"),       GripperJoint);
}

// =============================================================================
// BeginPlay — load meshes and attach, connect to ROS
// =============================================================================

void ARobotVisualizer::BeginPlay()
{
	Super::BeginPlay();

	// --- Load meshes and attach to links ---
	// Meshes are loaded at runtime (not in constructor) because
	// StaticLoadObject is safer to call here and allows hot-reload.

	// Helper lambda to reduce repetition
	auto Attach = [this](USceneComponent* Parent, const TCHAR* MeshName,
		double RosX, double RosY, double RosZ,
		double RosRoll, double RosPitch, double RosYaw,
		bool bIsMotor = false)
	{
		UStaticMesh* Mesh = LoadMeshAsset(MeshName);
		if (!Mesh) return;

		UStaticMeshComponent* SMC = NewObject<UStaticMeshComponent>(this);
		SMC->SetStaticMesh(Mesh);
		SMC->SetRelativeLocation(RosCoordConv::RosToUePosition(RosX, RosY, RosZ));
		SMC->SetRelativeRotation(RosCoordConv::RosRpyToUeRotator(RosRoll, RosPitch, RosYaw));
		SMC->SetCollisionEnabled(ECollisionEnabled::NoCollision);
		SMC->AttachToComponent(Parent, FAttachmentTransformRules::KeepRelativeTransform);
		SMC->RegisterComponent();
		AllMeshComponents.Add(SMC);
	};

	// === base_link meshes ===
	Attach(BaseLink, TEXT("base_motor_holder_so101_v1"),
		-0.00636471, -0.0000994414, -0.0024,
		1.5708, 0.0, 1.5708);
	Attach(BaseLink, TEXT("base_so101_v2"),
		-0.00636471, 0.0, -0.0024,
		1.5708, 0.0, 1.5708);
	Attach(BaseLink, TEXT("sts3215_03a_v1"),
		0.0263353, 0.0, 0.0437,
		0.0, 0.0, 0.0, true);
	Attach(BaseLink, TEXT("waveshare_mounting_plate_so101_v2"),
		-0.0309827, -0.000199441, 0.0474,
		1.5708, 0.0, 1.5708);

	// === shoulder_link meshes ===
	Attach(ShoulderLink, TEXT("sts3215_03a_v1"),
		-0.0303992, 0.000422241, -0.0417,
		1.5708, 1.5708, 0.0, true);
	Attach(ShoulderLink, TEXT("motor_holder_so101_base_v1"),
		-0.0675992, -0.000177759, 0.0158499,
		1.5708, -1.5708, 0.0);
	Attach(ShoulderLink, TEXT("rotation_pitch_so101_v1"),
		0.0122008, 0.0000222413, 0.0464,
		-1.5708, 0.0, 0.0);

	// === upper_arm_link meshes ===
	Attach(UpperArmLink, TEXT("sts3215_03a_v1"),
		-0.11257, -0.0155, 0.0187,
		-3.14159, 0.0, -1.5708, true);
	Attach(UpperArmLink, TEXT("upper_arm_so101_v1"),
		-0.065085, 0.012, 0.0182,
		3.14159, 0.0, 0.0);

	// === lower_arm_link meshes ===
	Attach(LowerArmLink, TEXT("under_arm_so101_v1"),
		-0.0648499, -0.032, 0.0182,
		3.14159, 0.0, 0.0);
	Attach(LowerArmLink, TEXT("motor_holder_so101_wrist_v1"),
		-0.0648499, -0.032, 0.018,
		-3.14159, 0.0, 0.0);
	Attach(LowerArmLink, TEXT("sts3215_03a_v1"),
		-0.1224, 0.0052, 0.0187,
		-3.14159, 0.0, -3.14159, true);

	// === wrist_link meshes ===
	Attach(WristLink, TEXT("sts3215_03a_no_horn_v1"),
		0.0, -0.0424, 0.0306,
		1.5708, 1.5708, 0.0, true);
	Attach(WristLink, TEXT("wrist_roll_pitch_so101_v2"),
		0.0, -0.028, 0.0181,
		-1.5708, -1.5708, 0.0);

	// === gripper_link meshes ===
	Attach(GripperLink, TEXT("sts3215_03a_v1"),
		0.0077, 0.0001, -0.0234,
		-1.5708, 0.0, 0.0, true);
	Attach(GripperLink, TEXT("wrist_roll_follower_so101_v1"),
		0.0, -0.000218214, 0.000949706,
		-3.14159, 0.0, 0.0);

	// === moving_jaw_link meshes ===
	Attach(MovingJawLink, TEXT("moving_jaw_so101_v1"),
		0.0, 0.0, 0.0189,
		0.0, 0.0, 0.0);

	UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: %d mesh components created"), AllMeshComponents.Num());

	// --- Connect to ROS via Subsystem ---
	UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
	if (!GI) return;

	URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
	if (!Ros) return;

	// Bind delegates.
	Ros->OnTopicMessage.AddDynamic(this, &ARobotVisualizer::OnRosMessage);
	Ros->OnConnected.AddDynamic(this, &ARobotVisualizer::OnRosBridgeConnected);

	// Subscribe is now queued even before connection — the subsystem will
	// send it automatically when connected (including on reconnect).
	Ros->Subscribe(JointStateTopic, JointStateType);

	// Queue MoveIt topic advertisements (sent on connect).
	AdvertiseMoveItTopics();

	// Initiate connection if not already connected.
	if (!Ros->IsConnected())
	{
		Ros->Connect(RosBridgeUrl);
	}
	else
	{
		// Already connected (e.g. another actor already called Connect).
		UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: already connected, subscription sent."));
	}
}

// =============================================================================
// EndPlay
// =============================================================================

void ARobotVisualizer::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
	if (UGameInstance* GI = UGameplayStatics::GetGameInstance(this))
	{
		if (URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>())
		{
			Ros->OnTopicMessage.RemoveDynamic(this, &ARobotVisualizer::OnRosMessage);
			Ros->OnConnected.RemoveDynamic(this, &ARobotVisualizer::OnRosBridgeConnected);
		}
	}

	bMoveItTopicsAdvertised = false;

	Super::EndPlay(EndPlayReason);
}

// =============================================================================
// ROS connection callback
// =============================================================================

void ARobotVisualizer::OnRosBridgeConnected()
{
	UE_LOG(LogRosBridge, Log,
		TEXT("RobotVisualizer: rosbridge connected — subscriptions restored by subsystem."));
}

// =============================================================================
// MoveIt topic advertisements
// =============================================================================

void ARobotVisualizer::AdvertiseMoveItTopics()
{
	UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
	if (!GI) return;

	URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
	if (!Ros) return;

	// These are queued and sent automatically when connected.
	Ros->Advertise(TEXT("/moveit_goal_named"), TEXT("std_msgs/String"));
	Ros->Advertise(TEXT("/moveit_goal_joints"), TEXT("sensor_msgs/JointState"));
	Ros->Advertise(TEXT("/moveit_goal_pose"), TEXT("geometry_msgs/PoseStamped"));

	bMoveItTopicsAdvertised = true;
	UE_LOG(LogRosBridge, Log, TEXT("RobotVisualizer: MoveIt command topics advertised."));
}

// =============================================================================
// MoveIt commands — SendNamedTarget
// =============================================================================

void ARobotVisualizer::SendNamedTarget()
{
	UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
	if (!GI) return;

	URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
	if (!Ros || !Ros->IsConnected())
	{
		UE_LOG(LogRosBridge, Warning, TEXT("SendNamedTarget: not connected to rosbridge."));
		return;
	}

	// std_msgs/String: {"data": "home"}
	FString MsgJson = FString::Printf(TEXT("{\"data\":\"%s\"}"), *MoveItNamedTarget);
	Ros->Publish(TEXT("/moveit_goal_named"), MsgJson);

	UE_LOG(LogRosBridge, Log, TEXT("SendNamedTarget: published '%s' to /moveit_goal_named"), *MoveItNamedTarget);
}

// =============================================================================
// MoveIt commands — SendJointGoal
// =============================================================================

void ARobotVisualizer::SendJointGoal()
{
	UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
	if (!GI) return;

	URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
	if (!Ros || !Ros->IsConnected())
	{
		UE_LOG(LogRosBridge, Warning, TEXT("SendJointGoal: not connected to rosbridge."));
		return;
	}

	// sensor_msgs/JointState:
	// {
	//   "header": {"stamp": {"sec": 0, "nanosec": 0}, "frame_id": ""},
	//   "name": ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"],
	//   "position": [0.0, -0.5, 0.5, 0.0, 0.0],
	//   "velocity": [],
	//   "effort": []
	// }

	FString MsgJson = FString::Printf(
		TEXT("{\"header\":{\"stamp\":{\"sec\":0,\"nanosec\":0},\"frame_id\":\"\"},")
		TEXT("\"name\":[\"shoulder_pan\",\"shoulder_lift\",\"elbow_flex\",\"wrist_flex\",\"wrist_roll\"],")
		TEXT("\"position\":[%f,%f,%f,%f,%f],")
		TEXT("\"velocity\":[],\"effort\":[]}"),
		GoalShoulderPan, GoalShoulderLift, GoalElbowFlex, GoalWristFlex, GoalWristRoll
	);

	Ros->Publish(TEXT("/moveit_goal_joints"), MsgJson);

	UE_LOG(LogRosBridge, Log,
		TEXT("SendJointGoal: [%.3f, %.3f, %.3f, %.3f, %.3f] to /moveit_goal_joints"),
		GoalShoulderPan, GoalShoulderLift, GoalElbowFlex, GoalWristFlex, GoalWristRoll);
}

// =============================================================================
// MoveIt commands — SendPoseGoal
// =============================================================================

void ARobotVisualizer::SendPoseGoal()
{
	UGameInstance* GI = UGameplayStatics::GetGameInstance(this);
	if (!GI) return;

	URosBridgeSubsystem* Ros = GI->GetSubsystem<URosBridgeSubsystem>();
	if (!Ros || !Ros->IsConnected())
	{
		UE_LOG(LogRosBridge, Warning, TEXT("SendPoseGoal: not connected to rosbridge."));
		return;
	}

	// Convert UE position (cm, left-handed) to ROS (meters, right-handed)
	double RosX, RosY, RosZ;
	RosCoordConv::UeToRosPosition(GoalPositionUE, RosX, RosY, RosZ);

	// geometry_msgs/PoseStamped (orientation defaults to identity — position-only goal)
	FString MsgJson = FString::Printf(
		TEXT("{\"header\":{\"stamp\":{\"sec\":0,\"nanosec\":0},\"frame_id\":\"base_link\"},")
		TEXT("\"pose\":{\"position\":{\"x\":%f,\"y\":%f,\"z\":%f},")
		TEXT("\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":1.0}}}"),
		RosX, RosY, RosZ
	);

	Ros->Publish(TEXT("/moveit_goal_pose"), MsgJson);

	UE_LOG(LogRosBridge, Log,
		TEXT("SendPoseGoal: UE(%.1f, %.1f, %.1f)cm -> ROS(%.4f, %.4f, %.4f)m to /moveit_goal_pose"),
		GoalPositionUE.X, GoalPositionUE.Y, GoalPositionUE.Z,
		RosX, RosY, RosZ);
}

// =============================================================================
// Message handling
// =============================================================================

void ARobotVisualizer::OnRosMessage(const FString& Topic, const FString& MessageJson)
{
	if (Topic == JointStateTopic)
	{
		ParseAndApplyJointStates(MessageJson);
	}
}

void ARobotVisualizer::ParseAndApplyJointStates(const FString& MessageJson)
{
	// Parse sensor_msgs/JointState JSON:
	// { "name": ["shoulder_pan", ...], "position": [0.1, ...], ... }

	TSharedPtr<FJsonObject> Json;
	TSharedRef<TJsonReader<>> Reader = TJsonReaderFactory<>::Create(MessageJson);
	if (!FJsonSerializer::Deserialize(Reader, Json) || !Json.IsValid())
	{
		return;
	}

	const TArray<TSharedPtr<FJsonValue>>* NameArray = nullptr;
	const TArray<TSharedPtr<FJsonValue>>* PosArray = nullptr;

	if (!Json->TryGetArrayField(TEXT("name"), NameArray) ||
		!Json->TryGetArrayField(TEXT("position"), PosArray))
	{
		return;
	}

	const int32 Count = FMath::Min(NameArray->Num(), PosArray->Num());
	for (int32 i = 0; i < Count; ++i)
	{
		const FName JointName(*(*NameArray)[i]->AsString());
		const double AngleRad = (*PosArray)[i]->AsNumber();

		TObjectPtr<USceneComponent>* JointComp = JointComponentMap.Find(JointName);
		if (JointComp && *JointComp)
		{
			const float AngleDeg = RosCoordConv::RosJointAngleToUeDegrees(AngleRad);

			FRotator BaseRot;
			if (JointName == FName("shoulder_pan"))
				BaseRot = RosCoordConv::RosRpyToUeRotator(3.14159, 0.0, -3.14159);
			else if (JointName == FName("shoulder_lift"))
				BaseRot = RosCoordConv::RosRpyToUeRotator(-1.5708, -1.5708, 0.0);
			else if (JointName == FName("elbow_flex"))
				BaseRot = RosCoordConv::RosRpyToUeRotator(0.0, 0.0, 1.5708);
			else if (JointName == FName("wrist_flex"))
				BaseRot = RosCoordConv::RosRpyToUeRotator(0.0, 0.0, -1.5708);
			else if (JointName == FName("wrist_roll"))
				BaseRot = RosCoordConv::RosRpyToUeRotator(1.5708, 0.0486795, 3.14159);
			else if (JointName == FName("gripper"))
				BaseRot = RosCoordConv::RosRpyToUeRotator(1.5708, 0.0, 0.0);

			const FQuat BaseQuat = BaseRot.Quaternion();
			const FQuat JointQuat = FQuat(FVector::UpVector, FMath::DegreesToRadians(AngleDeg));
			const FQuat FinalQuat = BaseQuat * JointQuat;

			(*JointComp)->SetRelativeRotation(FinalQuat.Rotator());
		}
	}
}

 

 

추가된 기능

에디터 Details 패널에 3개 버튼이 생긴다 (CallInEditor 속성):

  1. SendNamedTarget — MoveItNamedTarget 필드에 "home" 또는 "ready" 입력 후 클릭
  2. SendJointGoal — 5개 관절 값(radians)을 설정 후 클릭
  3. SendPoseGoal — GoalPositionUE에 UE 좌표(cm) 입력 후 클릭 (자동으로 ROS 좌표로 변환)

BeginPlay에서 /moveit_goal_named, /moveit_goal_joints, /moveit_goal_pose 3개 토픽을 자동 advertise한다. 기존 /joint_states 구독은 그대로 유지.

 

 

변경된 파일이 있으므로 빌드해서 오류가 없는지 확인한다.

 

 

빌드가 성공했다면 이제 언리얼 에디터로 간다.

rosbridge 터미널을 하나 더 띄운다.

터미널 7 - rosbridge:

export PATH=$(echo $PATH | tr ':' '\n' | grep -v miniforge | tr '\n' ':' | sed 's/:$//')
source /opt/ros/humble/setup.bash
source ~/UnrealRobotics/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_LOCALHOST_ONLY=1
ros2 launch rosbridge_server rosbridge_websocket_launch.xml

 

 

Robot Visualizer 액터를 레벨에 올려둔 후 Play 한다.

[Details]에서 [ROS] - [MoveIt]이 있는지 확인, [MoveItNamedTarget]을 ready로 변경 후 [SendNamedTarget] 버튼 클릭. -성공

 

 

[Details] 패널에서 [ROS] - [MoveIt] - [Joints] 섹션의 값 입력 후 [SendJointGoal] 버튼 클릭. -성공

 

 

 

 

이번 작업 완료된 것

  • moveit_goal_node.py - 3가지 토픽 → MoveGroup action → 실물 동작 확인
    • /moveit_goal_named (String) → named target
    • /moveit_goal_joints (JointState) → joint space goal
    • /moveit_goal_pose (PoseStamped) → position-only Cartesian goal
    • MoveGroup action client로 move_group에 직접 전달
    • 5DOF arm 제약 발견 → orientation constraint 제거로 해결
  • RobotVisualizer.h/.cpp (수정)
    • SendNamedTarget() / SendJointGoal() / SendPoseGoal() 추가
    • 3개 MoveIt 토픽 자동 advertise
    • Details 패널에서 CallInEditor 버튼으로 즉시 테스트 가능
    • 기존 /joint_states 구독 + 3D 모델 업데이트는 그대로 유지
  • end-to-end (Unreal → rosbridge → 중간 노드 → MoveIt → action server → ZMQ → worker → 실물) 흐름을 확인
    • ROS2 터미널에서 ros2 topic pub → 로봇 동작 ✅
    • Unreal PIE → SendNamedTarget("ready"/"home") → 로봇 동작 ✅
    • Unreal PIE → SendJointGoal → 로봇 동작 ✅

 

 

 

 

 

728x90
반응형
저작자표시 비영리 변경금지 (새창열림)

'Unreal Engine > 언리얼-ROS-Physical 통합 프로젝트' 카테고리의 다른 글

[UnrealRobotics: SO-101] (13) MoveIt 통합 - 로봇 Motion Planning  (0) 2026.05.12
[UnrealRobotics: SO-101] (12) Unreal에서 ROS2 로봇 명령 송신 파이프 확장  (0) 2026.05.12
[UnrealRobotics: SO-101] (11) Unreal에 3D 로봇 파트 올리기, 연동 확인  (0) 2026.05.07
[UnrealRobotics: SO-101] (10) Joint Command 역방향 경로 검증 (ROS → 로봇)  (0) 2026.04.27
[UnrealRobotics: SO-101] (9) 실물 로봇 + RViz 실시간 동기화  (0) 2026.04.22

관련글

티스토리툴바