본문으로 건너뛰기

프로젝트 생성

자체 ROS2 패키지를 만들어 PLEM 로봇을 제어하는 애플리케이션을 개발합니다.

사전 요구​

1. 패키지 생성​

cd ~/ros2_ws/src

ros2 pkg create --build-type ament_python my_robot_app \
  --dependencies rclpy control_msgs neuromeka_msgs trajectory_msgs

생성되는 디렉토리 구조:

my_robot_app/
├── my_robot_app/
│   └── __init__.py
├── resource/
│   └── my_robot_app
├── test/
├── package.xml
├── setup.cfg
└── setup.py

2. 궤적 전송 노드 작성​

my_robot_app/trajectory_sender.py를 생성합니다:

#!/usr/bin/env python3
"""FollowJointTrajectory 액션 클라이언트 템플릿."""

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from action_msgs.msg import GoalStatus
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from neuromeka_msgs.action import SetMode
from builtin_interfaces.msg import Duration


class TrajectorySender(Node):
    """궤적을 전송하는 노드. 모드 전환을 포함한다."""

    JOINT_NAMES = [
        "joint0", "joint1", "joint2",
        "joint3", "joint4", "joint5",
    ]

    def __init__(self, robot_id: str = "indy"):
        super().__init__("trajectory_sender")
        self._robot_id = robot_id

        self._mode_client = ActionClient(
            self, SetMode,
            f"/{robot_id}/wim_rt_driver/set_mode",
        )
        self._traj_client = ActionClient(
            self, FollowJointTrajectory,
            f"/{robot_id}/joint_trajectory_controller/follow_joint_trajectory",
        )

    def set_mode(self, mode: int) -> bool:
        """모드 전환. 0=BRAKED, 2=TRAJECTORY, 3=FREEDRIVE."""
        if not self._mode_client.wait_for_server(timeout_sec=5.0):
            self.get_logger().error("SetMode 서버 미응답")
            return False

        goal = SetMode.Goal()
        goal.target_robot_mode.mode = mode
        result = self._mode_client.send_goal(goal)
        return result.status == GoalStatus.STATUS_SUCCEEDED

    def send_trajectory(
        self,
        waypoints: list[list[float]],
        times_sec: list[int],
    ) -> bool:
        """웨이포인트 궤적을 전송한다."""
        if not self._traj_client.wait_for_server(timeout_sec=5.0):
            self.get_logger().error("FollowJointTrajectory 서버 미응답")
            return False

        trajectory = JointTrajectory()
        trajectory.joint_names = self.JOINT_NAMES

        for positions, sec in zip(waypoints, times_sec):
            point = JointTrajectoryPoint()
            point.positions = positions
            point.time_from_start = Duration(sec=sec)
            trajectory.points.append(point)

        goal = FollowJointTrajectory.Goal()
        goal.trajectory = trajectory

        result = self._traj_client.send_goal(goal)
        return result.status == GoalStatus.STATUS_SUCCEEDED


def main(args=None):
    rclpy.init(args=args)
    node = TrajectorySender(robot_id="indy")

    try:
        # TRAJECTORY 모드로 전환
        if not node.set_mode(2):
            node.get_logger().error("TRAJECTORY 모드 전환 실패")
            return

        # 궤적 전송
        success = node.send_trajectory(
            waypoints=[
                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.3, -0.2, 0.8, 0.0, 0.4, 0.0],
            ],
            times_sec=[3, 6],
        )

        if success:
            node.get_logger().info("궤적 실행 완료")
        else:
            node.get_logger().error("궤적 실행 실패")

        # BRAKED 모드로 복귀
        node.set_mode(0)

    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()

3. Entry Point 등록​

setup.py에 콘솔 스크립트를 추가합니다:

entry_points={
    'console_scripts': [
        'trajectory_sender = my_robot_app.trajectory_sender:main',
    ],
},

4. Launch 파일 작성​

launch/my_app.launch.py를 생성하여 PLEM launch를 포함하고 자체 노드를 함께 시작합니다:

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution


def generate_launch_description():
    # PLEM launch 포함
    plem_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
                FindPackageShare("neuromeka_robot_driver"),
                "launch",
                "plem_launch.py",
            ])
        ),
        launch_arguments={
            "robot_type": "indy7_v2",
            "gripper": "none",
        }.items(),
    )

    # 자체 노드 (PLEM 시작 후 5초 뒤에 실행)
    my_node = TimerAction(
        period=5.0,
        actions=[
            Node(
                package="my_robot_app",
                executable="trajectory_sender",
                name="trajectory_sender",
                output="screen",
            ),
        ],
    )

    return LaunchDescription([plem_launch, my_node])

setup.py의 data_files에 launch 디렉토리를 등록합니다:

import os
from glob import glob

data_files=[
    ('share/ament_index/resource_index/packages', ['resource/my_robot_app']),
    ('share/my_robot_app', ['package.xml']),
    (os.path.join('share', 'my_robot_app', 'launch'), glob('launch/*.py')),
],

5. 빌드 및 실행​

cd ~/ros2_ws
colcon build --packages-select my_robot_app
source install/setup.bash

노드 단독 실행​

PLEM이 이미 실행 중인 별도 터미널에서:

ros2 run my_robot_app trajectory_sender

Launch로 통합 실행​

ros2 launch my_robot_app my_app.launch.py

다음 단계​

이 시작 가이드로 기본적인 로봇 제어가 가능합니다. 더 자세한 내용은 다음 문서를 참고하세요:

  • 사용 가이드 — 모드 전환, 궤적 제어, MoveIt 통합 등 심화 내용
  • 레퍼런스 — ROS2 인터페이스 전체 목록, 설정 파라미터 상세