Multi-Robot 운용

여러 대의 로봇을 동시에 제어하는 방법을 설명합니다.

---

1. Multi-Robot Launch

multi_robot.launch.py로 여러 로봇을 한 번에 실행합니다:

ros2 launch neuromeka_robot_driver multi_robot.launch.py

각 로봇은 robot_id와 master_index로 구분됩니다. Launch 파일 내에서 로봇별 파라미터를 설정합니다.

---

2. robot_id와 master_index 설정

파라미터	설명
robot_id	ROS2 네임스페이스. 각 로봇마다 고유해야 함
master_index	EtherCAT 마스터 인덱스. 각 로봇마다 별도 마스터 사용

예시: 두 대의 Indy7을 운용하는 경우

로봇	robot_id	master_index
1번 로봇	indy_left	0
2번 로봇	indy_right	1

---

3. 네임스페이스 패턴

모든 ROS2 인터페이스(토픽, 액션, 서비스)는 /{robot_id}/ prefix를 가집니다.

1번 로봇 (indy_left)

종류	경로
Joint States 토픽	/indy_left/joint_states
FJT 액션	/indy_left/joint_trajectory_controller/follow_joint_trajectory
모드 전환 액션	/indy_left/wim_rt_driver/set_mode
그리퍼 액션	/indy_left/gripper_action_controller/gripper_cmd

2번 로봇 (indy_right)

종류	경로
Joint States 토픽	/indy_right/joint_states
FJT 액션	/indy_right/joint_trajectory_controller/follow_joint_trajectory
모드 전환 액션	/indy_right/wim_rt_driver/set_mode
그리퍼 액션	/indy_right/gripper_action_controller/gripper_cmd

---

4. 개별 제어 예제 (Python)

두 로봇에 각각 다른 궤적을 전송하는 예제입니다.

#!/usr/bin/env python3
"""Multi-robot 개별 제어 예제."""

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 neuromeka_msgs.action import SetMode
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from builtin_interfaces.msg import Duration

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


class MultiRobotController(Node):
    """여러 로봇을 개별 제어하는 노드."""

    MODE_BRAKED = 0
    MODE_TRAJECTORY = 2

    def __init__(self, robot_ids: list[str]):
        super().__init__("multi_robot_controller")
        self._robot_ids = robot_ids

        # 로봇별 액션 클라이언트 생성
        self._mode_clients: dict[str, ActionClient] = {}
        self._traj_clients: dict[str, ActionClient] = {}

        for rid in robot_ids:
            self._mode_clients[rid] = ActionClient(
                self,
                SetMode,
                f"/{rid}/wim_rt_driver/set_mode",
            )
            self._traj_clients[rid] = ActionClient(
                self,
                FollowJointTrajectory,
                f"/{rid}/joint_trajectory_controller/follow_joint_trajectory",
            )

    def set_mode(self, robot_id: str, mode: int) -> bool:
        """특정 로봇의 모드를 전환한다."""
        client = self._mode_clients[robot_id]
        if not client.wait_for_server(timeout_sec=5.0):
            self.get_logger().error(f"[{robot_id}] SetMode 서버 연결 실패")
            return False

        goal = SetMode.Goal()
        goal.target_robot_mode.mode = mode

        future = client.send_goal(goal)
        if future.status != GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().error(
                f"[{robot_id}] 모드 전환 실패: status={future.status}"
            )
            return False

        self.get_logger().info(f"[{robot_id}] 모드 전환 완료: {mode}")
        return True

    def send_trajectory(
        self,
        robot_id: str,
        positions_list: list[list[float]],
        durations_sec: list[int],
    ) -> bool:
        """특정 로봇에 궤적을 전송한다."""
        client = self._traj_clients[robot_id]
        if not client.wait_for_server(timeout_sec=5.0):
            self.get_logger().error(f"[{robot_id}] FJT 서버 연결 실패")
            return False

        trajectory = JointTrajectory()
        trajectory.joint_names = JOINT_NAMES
        for positions, sec in zip(positions_list, durations_sec):
            point = JointTrajectoryPoint()
            point.positions = positions
            point.time_from_start = Duration(sec=sec)
            trajectory.points.append(point)

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

        future = client.send_goal(goal)
        if future.status != GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().error(
                f"[{robot_id}] 궤적 실행 실패: status={future.status}"
            )
            return False

        self.get_logger().info(f"[{robot_id}] 궤적 실행 완료")
        return True


def main():
    rclpy.init()
    node = MultiRobotController(
        robot_ids=["indy_left", "indy_right"],
    )

    try:
        # 1. 두 로봇 모두 TRAJECTORY 모드로 전환
        node.set_mode("indy_left", MultiRobotController.MODE_TRAJECTORY)
        node.set_mode("indy_right", MultiRobotController.MODE_TRAJECTORY)

        # 2. 각 로봇에 서로 다른 궤적 전송
        node.send_trajectory(
            "indy_left",
            positions_list=[
                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.5, -0.3, 1.0, 0.0, 0.5, 0.0],
            ],
            durations_sec=[3, 6],
        )

        node.send_trajectory(
            "indy_right",
            positions_list=[
                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                [-0.5, 0.3, -1.0, 0.0, -0.5, 0.0],
            ],
            durations_sec=[3, 6],
        )

        # 3. 두 로봇 모두 BRAKED 모드로 복귀
        node.set_mode("indy_left", MultiRobotController.MODE_BRAKED)
        node.set_mode("indy_right", MultiRobotController.MODE_BRAKED)

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


if __name__ == "__main__":
    main()

팁

위 예제는 순차 실행입니다. 두 로봇을 동시에 움직이려면 asyncio나 별도 스레드를 사용하여 send_goal을 병렬로 호출하세요.

---

문제 해결

네임스페이스 충돌

두 로봇의 robot_id가 같으면 토픽이 충돌합니다. 반드시 서로 다른 robot_id를 사용하세요.

EtherCAT 마스터 충돌

각 로봇은 별도의 EtherCAT 마스터를 사용해야 합니다. master_index가 중복되지 않는지 확인하세요.