[Robocup@Home 2026] 2026.01.02

 

/home/nvidia/rby1_ws/src/rby1-ros2/rby1_bringup/launch/bringup.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import Command, PathJoinSubstitution, LaunchConfiguration
from launch_ros.substitutions import FindPackageShare
from launch_ros.parameter_descriptions import ParameterValue
from launch.actions import ExecuteProcess, RegisterEventHandler
from launch.event_handlers import OnProcessStart

def generate_launch_description():
    xacro_file = PathJoinSubstitution([
        FindPackageShare('rby1_description'),
        'urdf',
        'rby1.urdf.xacro'
    ])

    robot_description_content = Command([
        'xacro ', xacro_file,
        ' initial_positions_file:=',
        PathJoinSubstitution([
            FindPackageShare('rby1_description'),
            'urdf',
            'initial_positions.yaml'
        ]),
        ' robot_ip:=', LaunchConfiguration('robot_ip')
    ])

    robot_description = ParameterValue(robot_description_content, value_type=str)

    controllers_yaml = PathJoinSubstitution([
        FindPackageShare('rby1_bringup'),
        'config',
        'rby1_controllers.yaml'
    ])

    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{'robot_description': robot_description}],
    )

    ros2_control_node = Node(
        package='controller_manager',
        executable='ros2_control_node',
        parameters=[
            {'robot_description': robot_description},
            controllers_yaml
        ],
        remappings=[('robot_description', '/robot_description')],
        output='screen'
    )

    dualarm_controller_spawner = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['rby1_left_arm_controller'],
        output='screen'
    )
    

    controller_event_handler = RegisterEventHandler(
        OnProcessStart(
            target_action=ros2_control_node,
            on_start=[dualarm_controller_spawner]
            
            
       )
    )
    
    
    

    rqt_controller_manager = ExecuteProcess(
        cmd=['ros2', 'run', 'rqt_controller_manager', 'rqt_controller_manager', '--force-discover'],
        output='screen'
    )

    return LaunchDescription([
        robot_state_publisher_node,
        ros2_control_node,
        controller_event_handler,
        rqt_controller_manager,
    ])

cd /home/nvidia/rby1_ws
ros2 launch rby1_bringup bringup.launch.py robot_ip:=192.168.30.1:50051

이 코드는 "하드웨어 드라이버 로드 -> TF 계산기 실행 -> 컨트롤러 매니저 실행 -> (매니저 준비 완료 시) 팔 제어기 실행 -> GUI 실행"의 순서로 로봇을 구동.

 

/home/nvidia/rby1_ws/src/rby1-ros2/rby1_bringup/launch/bringup_sh.launch.py

# bringup_sh.launch.py
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, PathJoinSubstitution, LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    # -------------------------
    # Launch arguments
    # -------------------------
    robot_ip_arg = DeclareLaunchArgument(
        "robot_ip",
        default_value="192.168.30.1:50051",
        description="RBY1 gRPC address in ip:port format",
    )
    publish_rate_arg = DeclareLaunchArgument(
        "publish_rate",
        default_value="100.0",
        description="Joint state publish rate (Hz)",
    )

    use_teleop_arg = DeclareLaunchArgument(
        "use_teleop",
        default_value="true",
        description="Run 99_teleoperation node",
    )
    teleop_delay_arg = DeclareLaunchArgument(
        "teleop_delay",
        default_value="0",
        description="Delay before starting 99_teleoperation (seconds)",
    )
    scan0_topic_arg = DeclareLaunchArgument("scan0_topic", default_value="/scan0")
    scan1_topic_arg = DeclareLaunchArgument("scan1_topic", default_value="/scan1")
    merged_scan_topic_arg = DeclareLaunchArgument("merged_scan_topic", default_value="/scan_merged")
    # -------------------------
    # robot_description (URDF/XACRO)
    # -------------------------
    xacro_file = PathJoinSubstitution([
        FindPackageShare("rby1_description"),
        "urdf",
        "rby1_urdf_by_sh.urdf.xacro",
    ])

    robot_description_content = Command([
        "xacro ", xacro_file,
        " initial_positions_file:=",
        PathJoinSubstitution([
            FindPackageShare("rby1_description"),
            "urdf",
            "initial_positions.yaml",
        ]),
        " robot_ip:=", LaunchConfiguration("robot_ip"),
    ])
    robot_description = ParameterValue(robot_description_content, value_type=str)

    
    robot_state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="screen",
        parameters=[{"robot_description": robot_description}],
    )

    joint_state_publisher_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
                FindPackageShare("rby1_state_only"),
                "launch",
                "rby1_state_only_publisher.launch.py",
            ])
        ),
        launch_arguments={
            "robot_ip": LaunchConfiguration("robot_ip"),
            "publish_rate": LaunchConfiguration("publish_rate"),
        }.items(),
    )

    # -------------------------
    # Your extra nodes (odometry / lidar merger / joy / lakibeam)
    # -------------------------
    lakibeam_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
                FindPackageShare("lakibeam1"),
                "launch",
                "lakibeam1_scan_dual_lidar.launch.py",
            ])
        )
    )

    lidar_merger_node = Node(
        package="rby1_subscriber_pkg",
        executable="99_lidar",
        name="lidar_node",
        output="screen",
        remappings=[
            ("/scan0", LaunchConfiguration("scan0_topic")),
            ("/scan1", LaunchConfiguration("scan1_topic")),
            ("/scan_merged", LaunchConfiguration("merged_scan_topic")),
        ],
    )

    odom_node = Node(
        package="rby1_subscriber_pkg",
        executable="99_odom2",
        name="odom_node",
        output="screen",
    )

    teleop_node = Node(
        package="rby1_subscriber_pkg",
        executable="99_teleoperation",
        name="teleop_node",
        output="screen",
        condition=IfCondition(LaunchConfiguration("use_teleop")),
    )

    delayed_teleop = TimerAction(
        period=LaunchConfiguration("teleop_delay"),
        actions=[teleop_node],
        condition=IfCondition(LaunchConfiguration("use_teleop")),
    )

    joy_node = Node(
        package="joy",
        executable="joy_node",
        name="joy_node",
        output="screen",
    )

    world_to_map_tf = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="world_to_map_tf",
        arguments=["0", "0", "0", "0", "0", "0", "world", "map"],
        output="screen",
    )

    return LaunchDescription([
        robot_ip_arg,
        publish_rate_arg,
        use_teleop_arg,
        teleop_delay_arg,
        scan0_topic_arg,
        scan1_topic_arg,
        merged_scan_topic_arg,

        robot_state_publisher_node,
        joint_state_publisher_launch,

        lakibeam_launch,
        lidar_merger_node,
        odom_node,
        delayed_teleop,
        joy_node,
        world_to_map_tf,
    ])

cd /home/nvidia/rby1_ws
ros2 launch rby1_bringup bringup_sh.launch.py robot_ip:=192.168.30.1:50051

 

이 파일은 "로봇의 뼈대(URDF)를 등록하고, 관절 상태를 읽어오며, 2개의 라이다를 켜서 데이터를 하나로 합치고, 조이스틱으로 조종할 준비를 하며, 위치 추정(Odom)을 시작하라"는 명령서이다.

 

'/home/nvidia/rby1_ws/src/slam_toolbox/config/mapper_params_online_async.yaml'

 해당 파일의 'ROS Parameters'에서 base_frames: base / scan_topic: /scan_merged로 변경해준다.

 

터미널 1에서 SLAM 실행:

ros2 launch slam_toolbox online_async_launch.py \
    slam_params_file:=/home/nvidia/rby1_ws/src/slam_toolbox/config/mapper_params_online_async.yaml \
    use_sim_time:=True

 

터미널 2에서 Bag 재생:

ros2 bag play my_slam_data --clock

 

 

SLAM을 통해 지도(Map)를 만들었다면, 다음 작업은 그 지도를 보고 목적지까지 찾아가는 것, 즉 내비게이션(Navigation)이다. ROS 2에서 로봇이 맵을 바탕으로 움직이는 핵심 원리는 크게 3가지 단계로 나뉜다. 

 

1. Localization / AMCL

 

• 원리: 로봇은 현재 라이다 센서로 들어오는 데이터(실시간 거리 정보)와, 아까 만들어둔 지도(저장된 벽 정보)를 비교한다.
• 알고리즘 (AMCL): Adaptive Monte Carlo Localization이라는 확률 기반 알고리즘을 사용한다.
  • 로봇은 처음에 자기 위치를 정확히 모르니, 지도 위에 수천 개의 가상 점(Particle)을 뿌린다.
  • 로봇이 조금씩 움직이면서 센서 데이터와 맵이 일치하는 점들은 남기고, 틀린 점들은 지워나간다.
  • 결국 점들이 한곳으로 모이게 되는데, 그곳이 바로 로봇의 현재 위치라고 추정한다.

2. Global Path Planning

 

• 역할: 현재 위치에서 목적지까지 가장 짧은 경로를 계산한다.
• 특징:
  • 아까 만든 정적 지도(Static Map)를 기반으로 계산한다.
  • 벽이나 고정된 가구를 피해서 길을 그린다.
  • 하지만 갑자기 나타난 사람이나 의자 등 움직이는 장애물은 고려하지 않는다. (오로지 저장된 지도 기준)
• 알고리즘: 주로 A* 또는 Dijkstra (다익스트라) 알고리즘을 사용하여 최단 거리를 수학적으로 계산한다.

3. Local Path Planning / Controller

 해당 단계는 운전자가 핸들과 브레이크를 조작하는 것과 같다.

• 역할: Global Planner가 정해준 큰 길을 따라가되, 눈앞의 장애물을 피하면서 로봇에게 구체적인 속도 명령(cmd_vel)을 내린다.
• 특징:
  • 라이다 센서의 실시간 데이터를 최우선으로 한다.
  • 매우 짧은 시간(예: 0.1초) 단위로 계속 계산한다.
  • 로봇의 물리적 한계(회전 반경, 최대 속도 등)를 고려한다.
• 알고리즘: DWB (Dynamic Window Approach) 또는 TEB (Timed Elastic Band) 등을 사용하여, 장애물에 부딪히지 않으면서 목표 방향으로 가는 최적의 속도와 각도를 찾아냅니다.

로봇은 사람이 보는 '벽'이나 '식탁'을 그대로 이해하지 못하는 대신 Costmap(비용 지도)이라는 개념을 사용한다.

1. 검은색 (Lethal Cost): 벽. 절대 가면 안 됨.
2. 회색 (Inflation Layer): 벽 주변의 위험 지역.
3. 흰색 (Free Space): 안전함. 지나가도 됨.

로봇은 이 Cost 값이 낮은 쪽(안전한 쪽)으로만 골라서 경로를 생성한다.

 

이 모든 과정을 ROS 2에서는 Nav2 (Navigation 2) 라는 패키지 묶음이 처리해 준다. SLAM으로 만든 지도를 가지고 Nav2를 실행시키기만 하면 Navigation을 진행할 수 있다.