ROS2 - Multiple Robots 환경을 위한 Namespace 설정 방법

ROS2 - Configure namespace for multiple robots environment

[ROS2 구동 환경]
- ROS2 iron
- Ubuntu 22.04.4 LTS 
- Python + XML + YAML 조합으로 런치 스크립트(launch script) 구성
  (모든 노드는 rclpy의 Node 혹은 ComposableNode로 생성)
- 로봇 모델은 xml 포맷의 URDF로 구성하며, Xacro 사용
- 필수 설치 ROS 패키지에 대해서는 이글에서는 다루지 않음 (정보 필요시 댓글)

1. Launch Argument 추가

ROS2 실행 인자로 'namespace'를 추가해 ros2 launch로 실행할 때 네임스페이스를 변경할 수 있게 해준다

launch 파일명은 임의로 my_robot.launch.py로 정했다

# file name: my_robot.launch.py
from typing import List
from launch import LaunchDescription
from launch.action import Action
from launch.actions import DeclareLaunchArgument
from launch.substitutions import TextSubstitution

def declare_launch_arguments() -> List[Action]:
    namespace_arg = DeclareLaunchArgument(
        "namespace", default_value=TextSubstitution(text=""), description="robot namespace")
    return [namespace_arg]

def generate_launch_description() -> LaunchDescription:
    ld = LaunchDescription()
    # add launch arguments
    launch_args = declare_launch_arguments()
    for arg in launch_args:
        ld.add_action(arg)
    return ld

위와 같이 실행 인자를 추가해준뒤 인자를 변경하고자 하면 CLI를 다음과 같이 입력하면 된다 (:=를 입력해야 함에 유의)

$ ros2 launch <package_name> my_robot.launch.py namespace:=my_namespace

2. URDF(Unified Robot Description Format) 구성

URDF는 gazebo와 ros2_control 패키지를 사용하지 않는 이상 네임스페이스 관련 변경 내용은 없다

만약 gazebo + ros2_control 조합을 사용하고자 한다면 아래와 같이 ros2 control 관련 <gazebo> 태그 하위에 <ros> 태그를 추가해주면 된다

<!-- file name: robot.urdf.xacro -->
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="my_robot">
    <xacro:arg name="namespace" default=""/>
    <xacro:include filename="robot.core.xacro"/>
    <xacro:include filename="robot.control.xacro"/>
</robot>

<!-- file name: robot.core.xacro -->
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <link name="world"/>
    
    <joint name="joint_1" type="revolute">
        <parent link="world"/>
        <child link="link_1"/>
        <!-- 중략 -->
    </joint>
    <link name="link_1">
        <!-- 중략 -->
    </link>
    
    <!-- 중략 -->
</robot>

<!-- file name: robot.control.xacro -->
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <ros2_control name="my_robot_control" type="system">
        <hardware>
            <plugin></plugin>
        </hardware>
        <joint name="joint_1">
            <!-- 중략 -->
        </joint>
    </ros2_control>
    
    <gazebo>
        <plugin name="libgazebo_ros2_plugin" filename="libgazebo_ros2_control.so">
            <ros>
                <namespace>$(arg namespace)</namespace>
                <remapping>/tf:=tf</remapping>
                <remapping>/tf_static:=tf_static</remapping>
            </ros>
        </plugin>
    </gazebo>
</robot>

최상위 xacro 파일인 robot.urdf.xacro 파일에서 xacro 인자로 설정한 namespace가 <gazebo>의 <ros> 태그 내 <namespace> 값으로 사용되는 것을 보면 된다

※ 위 코드의 <gazebo> 설정은 gazebo classic을 위한 설정인데, ignition gazebo에도 <ros> 태그 설정방법은 동일하니 설명은 생략하도록 한다

3. YAML 설정 파일 구성 (ROS2 Control)

ROS2 노드 설정(configuration)을 위해 주로 사용되는 yaml 포맷의 설정 파일들 중 ros2_control 패키지의 controller_manager 관련 설정 파일은 네임스페이스 관련 설정을 추가해줘야 한다

이 글에서는 ros2_controllers 패키지 중 Joint State Broadcaster와 Joint Trajectory Controller 두 컨트롤러만 구현한 예시를 다루도록 한다 (다관절 매니퓰레이터 연동 시 필수적으로 쓰이는 컨트롤러 2개~)

# file name: ros2_controllers.yaml
/**:
  controller_manager:
    ros__parameters:
      update_rate: 1000
      
      joint_state_broadcaster:
        type: joint_state_broadcaster/JointStateBroadcaster
        
      joint_trajectory_controller:
        type: joint_trajectory_controller/JointTrajectoryController

/**:
  joint_state_broadcaster:
    ros__parameters:
      joints:
        - joint_1

/**:
  joint_trajectory_controller:
    ros__parameters:
      joints:
        - joint_1
      command_interfaces:
        - position
        - velocity
      state_interfaces:
        - position
        - velocity

여기서 중요하게 봐야할 것은 와일드카드 구문인 /** 를 사용했다는 점이다

네임스페이스가 지정된 로봇을 컨트롤하려면 위 구문은 원칙대로라면 아래와 같이 구현되어야 한다 (네임스페이스를 my_robot_namespace로 지정할 시)

/my_robot_namespace/joint_state_broadcaster:
    ros__parameters:
      joints:
        - joint_1

만약 yaml 파일을 이런 식으로 하드코딩하게 되면 네임스페이스를 바꿀 때마다 yaml 파일도 같이 변경해야 되기 때문에 굉장히 번거로운데, 와일드카드를 사용하면 node 생성 시 파라미터로 입력되는 네임스페이스를 알아서 적용해주기 때문에 yaml 파일 변경 없이 여러 로봇을 무리없이 한 환경에 연동할 수 있다

※ 이 외에도 ros__parameters 항목이 사용되는 yaml 파일은 모두 와일드카드를 적용해주도록 한다

 

YAML 설정 파일의 와일드카드 관련 내용은 ROS2 공식 문서를 참고하기 바란다

4. rclpy Node 생성 구문 구성

ROS2 노드 생성을 위해 가장 일반적으로 사용하는 Node 클래스는 launch_ros.actions 모듈의 node.py 파일 내부에 구현되어 있다

Node 클래스의 생성자를 보면 namespace 인자가 있는 것을 확인할 수 있다

# file name: /opt/ros/iron/lib/python3.10/site-packages/launch_ros/actions/node.py
@expose_action('node')
class Node(ExecuteProcess):
    """Action that executes a ROS node."""

    UNSPECIFIED_NODE_NAME = '<node_name_unspecified>'
    UNSPECIFIED_NODE_NAMESPACE = '<node_namespace_unspecified>'

    def __init__(
        self, *,
        executable: SomeSubstitutionsType,
        package: Optional[SomeSubstitutionsType] = None,
        name: Optional[SomeSubstitutionsType] = None,
        namespace: Optional[SomeSubstitutionsType] = None,
        exec_name: Optional[SomeSubstitutionsType] = None,
        parameters: Optional[SomeParameters] = None,
        remappings: Optional[SomeRemapRules] = None,
        ros_arguments: Optional[Iterable[SomeSubstitutionsType]] = None,
        arguments: Optional[Iterable[SomeSubstitutionsType]] = None,
        **kwargs
    ) -> None:
        """
        The namespace can either be absolute (i.e. starts with /) or
        relative.
        If absolute, then nothing else is considered and this is passed
        directly to the node to set the namespace.
        If relative, the namespace in the 'ros_namespace' LaunchConfiguration
        will be prepended to the given relative node namespace.
        If no namespace is given, then the default namespace `/` is
        assumed.
        
        :param: namespace the ROS namespace for this Node
        """

ROS2 CLI에서 __ns 인자를 사용하지 않고 노드 생성 시 네임스페이스를 손쉽게 적용할 수 있다!

앞서 구현해둔 declare_launch_arguments 함수에서 생성한 'namespace'를 사용해 노드 생성 시 아래와 같이 활용할 수 있다

4.1. URDF 파일 읽기

Xacro를 사용해서 작성된 xml 포맷의 URDF 파일은 launch_param_builder의 load_xacro 함수를 활용해 읽어들이면 되는데, declare_launch_arguments 함수에서 추가한 'namespace' Action 객체를 문자열로 파싱(LaunchContext 사용)해야 하는 번거로움이 있기에, moveit_config_builder의 MoveItConfigsBuilder 인스턴스를 활용하도록 하자

※ MoveIt의 Motion Planning을 위한 SRDF, planning_pipeline, joint_limit, kinematics, sensor_3d 등 yaml 설정 파일들도 손쉽게 설정할 수 있으므로 사용하기를 권장

import os
from moveit_configs_utils import MoveItConfigsBuilder
from ament_index_python.packages import get_package_share_directory

def generate_moveit_config_builder() -> MoveItConfigsBuilder:
    builder = MoveItConfigsBuilder('robot', package_name='my_ros_package')
    pkg_share_dir = get_package_share_directory('my_ros_package')
    builder.robot_description(
        file_path=os.path.join(pkg_share_dir, "description/robot.urdf.xacro"),
        mappings={
            'namespace': LaunchConfiguration('namespace')
        }
    )
    return builder

def generate_launch_description() -> LaunchDescription:
    ld = LaunchDescription()
    # add launch arguments
    launch_args = declare_launch_arguments()
    for arg in launch_args:
        ld.add_action(arg)
    # generate moveit config
    moveit_config_builder = generate_moveit_config_builder()
    moveit_config = moveit_config_builder.to_moveit_configs()

mappings 인자에 LaunchConfiguration을 다이렉트로 입력하면 된다! 

 

※ MoveItConfigsBuilder를 사용하지 않는다면 아래와 같이 robot_description xml 문자열을 로드하면 된다

(LaunchContext를 사용해 LaunchConfiguration을 perform하여 text를 추출해야 하기 때문에 조금 복잡하다)

import os
from pathlib import Path
from launch import LaunchContext
from launch.actions import OpaqueFunction
from launch_param_builder import load_xacro
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory

__robot_description: str = ''

def load_robot_description(context: LaunchContext, *args, **kwargs):
    global __robot_description

    pkg_share_dir = get_package_share_directory('my_ros_package')
    urdf_path = os.path.join(pkg_share_dir, "description/robot.urdf.xacro")
    namespace_arg = LaunchConfiguration('namespace')
    __robot_description = load_xacro(
        Path(urdf_path),
        mappings={
            'namespace': namespace_arg.perform(context)
        }
    )

def generate_launch_description() -> LaunchDescription:
    ld = LaunchDescription()
    # add launch arguments
    launch_args = declare_launch_arguments()
    for arg in launch_args:
        ld.add_action(arg)

    ld.add_action(OpaqueFunction(
        function=load_robot_description))

4.2. Robot State Publisher 노드 생성

robot_state_publisher 패키지 노드는 다음과 같이 같단히 만들 수 있다

Node 클래스 생성자의 namespace 파라미터에 앞서 추가해둔 LaunchConfiguration을 입력하면 된다

from launch_ros.actions import Node

def generate_rsp_node(robot_description) -> Node:
    return Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        respawn=True,
        output="screen",
        namespace=LaunchConfiguration("namespace"),
        parameters=[
            robot_description
        ],
        remappings=[
            ("/tf", "tf"),
            ("/tf_static", "tf_static"),
        ]
    )

def generate_launch_description() -> LaunchDescription:
    ld = LaunchDescription()
    # add launch arguments
    launch_args = declare_launch_arguments()
    for arg in launch_args:
        ld.add_action(arg)
    # generate moveit config
    moveit_config_builder = generate_moveit_config_builder()
    moveit_config = moveit_config_builder.to_moveit_configs()
    # generate robot state publisher
    rsp_node = generate_rsp_node(moveit_config.robot_description)
    ld.add_action(rsp_node)

    return ld

4.3. ROS2 Control - Controller Manager 관련 노드 생성

gazebo 시뮬레이션 환경이라면 다음과 같이 gazebo_ros 패키지의 spawn_entity.py를 실행하는 노드를 생성해주면 된다

(1) 만약 gazebo classic이라면 gz_server와 gz_classic을 따로 실행해둔 뒤, gazebo_ros 패키지의 spawn_entity.py를 실행하는 노드를 생성하여 런치하면 gazebo에 로봇을 spawn할 수 있다

def declare_launch_arguments() -> List[Action]:
    namespace_arg = DeclareLaunchArgument(
        "namespace", default_value=TextSubstitution(text=""), description="robot namespace")
    robot_name_arg = DeclareLaunchArgument(
        "robot_name", default_value=TextSubstitution(text="my_robot"), description="robot name for gazebo")
    return [namespace_arg]

node = Node(
    package='gazebo_ros', 
    executable='spawn_entity.py',
    arguments=[
        '-topic', 'robot_description',
        '-entity', LaunchConfiguration('robot_name'),
        '-robot_namespace', LaunchConfiguration("namespace"),
    ],
    output='screen',
    namespace=LaunchConfiguration("namespace")
)

(2) 만약 ignition gazebo라면 ros_gz_sim 패키지의 create를 실행하는 노드를 생성

node = Node(
    package='ros_gz_sim', 
    executable='create',
    arguments=[
        '-topic', 'robot_description',
        '-name', LaunchConfiguration('robot_name'),
        '-allow_renaming', 'true'
    ],
    output='screen',
    namespace=LaunchConfiguration("namespace")
)

(1), (2) 코드에서 중요한 것은 gazebo 상에 spawn되는 로봇 또한 namespace와 마찬가지로 서로 구별되는 이름을 가져야하기에 namespace와 마찬가지로 'robot_name'이라는 이름을 가진 LaunchConfiguration을 하나 추가해줘서 사용자가 CLI에서 입력할 수 있도록 구현할 수 있다

$ ros2 launch <package_name> my_robot.launch.py namespace:=my_namespace robot_name:=my_robot_name

 

(3) 실제 로봇이라면 controller_manager 노드의 ros2_control_node 실행 노드 생성

Node(
    package="controller_manager",
    executable="ros2_control_node",
    parameters=[
        moveit_config.robot_description,
        os.path.join(pkg_share_dir, "config/ros2_controllers.yaml"),
    ],
    output="screen",
    namespace=LaunchConfiguration("namespace")
)

4.4. ROS2 Control - Controller spawning 관련 노드 생성

Joint State Broadcaster와 Joint Trajectory Controller 두 컨트롤러를 만드는 노드는 controller_manager 패키지의 spawner에 yaml 파일에서 설정한 컨트롤러 이름을 인자로 넣어주면 된다

Node(
    package="controller_manager",
    executable="spawner",
    arguments=['joint_state_broadcaster'],
    output="screen",
    namespace=LaunchConfiguration("namespace")
)

Node(
    package="controller_manager",
    executable="spawner",
    arguments=['joint_trajectory_controller'],
    output="screen",
    namespace=LaunchConfiguration("namespace")
)

4.5. 기타

이 외에도 다른 서드파티 패키지 혹은 본인이 직접 만든 ROS2 노드를 실행할 때 위와같이 namespace 인자를 입력하면 된다

5. Gazebo Classic + ROS2 Control 네임스페이스 적용 시 버그

안타깝게도 apt로 설치하는 gazebo_ros2_control 패키지 바이너리는 multiple robot을 위한 네임스페이스 적용시 오류가 발생하면서 gz_server가 crash 발생 후 그냥 죽어버린다

 

해당 문제를 해결하는 방법은 깃허브 PR로 등록되어 있는데, 아직 2024년 7월 23일 현재까지 merge가 진행되지 않고 있다 (왜지;;;)

Fix namespacing for multiple instances of gazebo_ros2_control plugin by bobbleballs · Pull Request #181 · ros-controls/gazebo_ros2_control · GitHub

Fix namespacing for multiple instances of gazebo_ros2_control plugin by bobbleballs · Pull Request #181 · ros-controls/gazebo_

gazebo classic을 사용하는 개발자라면 해당 PR을 참고해서 소스코드 빌드 후 사용하면 된다

6. 최종 구현 Launch Script

# file name: my_robot.launch.py
from typing import List
from launch import LaunchDescription
from launch.action import Action
from launch.actions import DeclareLaunchArgument
from launch.substitutions import TextSubstitution
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
import os
from moveit_configs_utils import MoveItConfigsBuilder
from ament_index_python.packages import get_package_share_directory

def declare_launch_arguments() -> List[Action]:
    namespace_arg = DeclareLaunchArgument(
        "namespace", default_value=TextSubstitution(text=""), description="robot namespace")
    namespace_arg = DeclareLaunchArgument(
        "robot_name", default_value=TextSubstitution(text="my_robot"), description="robot name for gazebo")
    return [namespace_arg]

def generate_moveit_config_builder() -> MoveItConfigsBuilder:
    builder = MoveItConfigsBuilder('robot', package_name='ky_geniant_ros')
    pkg_share_dir = get_package_share_directory('ky_geniant_ros')
    builder.robot_description(
        file_path=os.path.join(pkg_share_dir, "description/robot.urdf.xacro"),
        mappings={
            'namespace': LaunchConfiguration('namespace')
        }
    )
    return builder

def generate_rsp_node(robot_description) -> Node:
    return Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        respawn=True,
        output="screen",
        namespace=LaunchConfiguration("namespace"),
        parameters=[
            robot_description
        ],
        remappings=[
            ("/tf", "tf"),
            ("/tf_static", "tf_static"),
        ]
    )

def generate_gazebo_ros_node() -> Node:
    return Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
        arguments=[
            '-topic', 'robot_description',
            '-entity', LaunchConfiguration('robot_name'),
            '-robot_namespace', LaunchConfiguration("namespace")
        ],
        output='screen',
        namespace=LaunchConfiguration("namespace")
    )

def generate_joint_state_broadcaster_node() -> Node:
    return Node(
        package="controller_manager",
        executable="spawner",
        arguments=['joint_state_broadcaster'],
        output="screen",
        namespace=LaunchConfiguration("namespace")
    )

def generate_joint_trajectory_controller_node() -> Node:
    return Node(
        package="controller_manager",
        executable="spawner",
        arguments=['joint_trajectory_controller'],
        output="screen",
        namespace=LaunchConfiguration("namespace")
    )

def generate_launch_description() -> LaunchDescription:
    ld = LaunchDescription()
    # add launch arguments
    launch_args = declare_launch_arguments()
    for arg in launch_args:
        ld.add_action(arg)
    # generate moveit config
    moveit_config_builder = generate_moveit_config_builder()
    moveit_config = moveit_config_builder.to_moveit_configs()
    # generate robot state publisher
    rsp_node = generate_rsp_node(moveit_config.robot_description)
    ld.add_action(rsp_node)
    # generate controller manager node (for gazebo classic)
    gz_node = generate_gazebo_ros_node()
    ld.add_action(gz_node)
    # generate joint state publisher
    jsb_node = generate_joint_state_broadcaster_node()
    ld.add_action(jsb_node)
    # generate joint trajectory controller 
    jtc_node = generate_joint_trajectory_controller_node()
    ld.add_action(jtc_node)
    return ld

7. 실행 결과

RQT의 Node Graph로 namespace가 어떻게 적용되는지 시각화하여 알아보자

$ ros2 launch <package_name> my_robot.launch.py namespace:=my_namespace1 robot_name:=my_robot1
$ ros2 launch <package_name> my_robot.launch.py namespace:=my_namespace2 robot_name:=my_robot2
$ ros2 launch <package_name> my_robot.launch.py namespace:=my_namespace3 robot_name:=my_robot3

Topic, Service, Action, Parameter 등의 이름 앞에 /{namespace} 가 첨두어로 추가되는 것을 볼 수 있다

따라서, 로봇이 namespace별로 구별되어 있으므로 각각의 로봇을 독립적으로 제어 및 모니터링할 수 있다