[ROS2] Debug ROS2 Control Interface (GDB)

Debug ROS2 Control Interface with GDB

1. 개요

ROS2 Control 하드웨어 플러그인을 직접 개발할 경우 코드 단위로 디버깅이 필요할 때가 있는데, hardware_interface는 라이브러리 형태로 빌드되기 때문에 런타임 디버깅을 위해서는 controller_manager 패키지의 ros2_control_node 노드를 실행할 때 디버거를 연동해줘야 한다

본 글에서는 ros2 control 패키지가 바이너리로 설치되어 있는 환경에서 ros2_control_node를 커스터마이즈하여 GDB(GND Debugger)를 통해 remote debug하는 방법에 대해 간략히 알아보도록 한다

[구동 환경]
- Linux Ubuntu 22.04.5 LTS
- ROS2 Iron
- GDB 12.1
- IDE: VSCode 1.108.1

2. GDB (GNU Debugger) 설치

remote debugging을 위해 gdb와 gdbserver 패키지를 apt로 설치해준다

$ sudo apt install -y gdb gdbserver

3. 테스트용 ROS2 패키지 구성

[~/ros2_ws/src/my_package]
.vscode
---- launch.json
config
---- ros2_controllers.yaml
description
---- robot.urdf.xacro
include
---- my_package
-------- my_hardware_interface.hpp
launch
---- test.launch.py
src
---- my_hardware_interface.cpp
---- my_ros2_control_node.cpp
CMakeLists.txt
hardware_interface_plugin.xml
package.xml

실제 테스트를 위해 사용한 ROS2 패키지 예시는 아래 깃허브 리포지터리에서 전체 코드를 확인할 수 있다

※ Github repository: https://github.com/YOGYUI/ros2_control_debug_hardware_interface

3.1. 테스트용 Hardware Interface 구현

파일 경로: my_package/include/my_package/my_hardware_interface.hpp

파일 경로: my_package/src/my_hardware_interface.cpp

 

hardware_interface::SystemInterface 클래스를 상속받은 인터페이스 클래스를 구현한 것이며, 직접 개발하는 ros2 control hardware plugin은 동일한 방식으로 아래 메서드들 중 필요한 것들을 오버라이드해 제작하게 된다

• on_init
• on_configure
• on_cleanup
• on_shutdown
• on_activate
• on_deavtivate
• export_state_interfaces
• export_command_interfaces
• prepare_command_mode_switch
• perform_command_mode_switch
• read
• write

인터페이스 클래스명은 임의로 myHardwareInterface로 정했다

namespace my_hardware_interface
{
class myHardwareInterface : public hardware_interface::SystemInterface
{
public:
    RCLCPP_SHARED_PTR_DEFINITIONS(myHardwareInterface);
    myHardwareInterface();
    virtual ~myHardwareInterface();
    /* 후략 */
};

 

cpp 파일 후반부에 PLUGINLIB_EXPORT_CLASS를 제대로 지정해야 ROS2에서 플러그인을 사용할 수 있다

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(my_hardware_interface::myHardwareInterface, hardware_interface::SystemInterface)

 

그리고 플러그인 정보가 담긴 xml 파일을 아래와 같이 생성해줘야 한다 (CMake 빌드 시 사용해줘야 한다)

파일 경로: my_package/hardware_interface_plugin.xml 

<library path="my_hardware_interface">
    <class name="my_hardware_interface/myHardwareInterface"
           type="my_hardware_interface::myHardwareInterface"
           base_class_type="hardware_interface::SystemInterface">
        <description>
            ROS2 Hardware Interface for Debugging Test.
        </description>
  </class>
</library>

※ 자세한 코드는 리포지터리를 참고하도록 한다

3.2. ros2_control_node 커스터마이즈

파일 경로: my_package/src/my_ros2_control_node.cpp

 

ros2_control_node를 아래와 같이 ros2 control 깃허브에서 코드를 그대로 복사해와 빌드해 사용하도록 한다

※ 필요할 경우 코드를 수정해도 무방

iron버전의 원본 코드 주소: https://github.com/ros-controls/ros2_control/blob/iron/controller_manager/src/ros2_control_node.cpp

#include <errno.h>
#include <algorithm>
#include <chrono>
#include <memory>
#include <string>
#include <thread>
#include "controller_manager/controller_manager.hpp"
#include "rclcpp/rclcpp.hpp"
#include "realtime_tools/realtime_helpers.hpp"

using namespace std::chrono_literals;

namespace {
    // Reference: https://man7.org/linux/man-pages/man2/sched_setparam.2.html
    // This value is used when configuring the main loop to use SCHED_FIFO scheduling
    // We use a midpoint RT priority to allow maximum flexibility to users
    int const kSchedPriority = 50;
}

int main(int argc, char ** argv)
{
    rclcpp::init(argc, argv);

    // create executor
    std::shared_ptr<rclcpp::Executor> executor = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
    
    // create controller manager instance
    std::string manager_node_name = "controller_manager";
    auto controller_manager = std::make_shared<controller_manager::ControllerManager>(executor, manager_node_name);

    // configure real-time attributes
    const bool lock_memory = controller_manager->get_parameter_or<bool>("lock_memory", true);
    std::string message;
    if (lock_memory && !realtime_tools::lock_memory(message)) {
        RCLCPP_WARN(controller_manager->get_logger(), "Unable to lock the memory : '%s'", message.c_str());
    }
    
    const int cpu_affinity = controller_manager->get_parameter_or<int>("cpu_affinity", -1);
    if (cpu_affinity >= 0) {
        const auto affinity_result = realtime_tools::set_current_thread_affinity(cpu_affinity);
        if (!affinity_result.first) {
            RCLCPP_WARN(controller_manager->get_logger(), "Unable to set the CPU affinity : '%s'", affinity_result.second.c_str());
        }
    }

    RCLCPP_INFO(controller_manager->get_logger(), "update rate is %d Hz", controller_manager->get_update_rate());
    
    const int thread_priority = controller_manager->get_parameter_or<int>("thread_priority", kSchedPriority);
    RCLCPP_INFO(controller_manager->get_logger(), "Spawning %s RT thread with scheduler priority: %d", controller_manager->get_name(), thread_priority);

    // control loop thread
    std::thread control_loop_thread([controller_manager, thread_priority]() {
        if (realtime_tools::has_realtime_kernel()) {
            if (!realtime_tools::configure_sched_fifo(thread_priority)) {
                RCLCPP_WARN(
                    controller_manager->get_logger(),
                    "Could not enable FIFO RT scheduling policy: with error number <%i>(%s). See "
                    "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] "
                    "for details on how to enable realtime scheduling.",
                    errno, strerror(errno));
                } else {
                    RCLCPP_INFO(
                        controller_manager->get_logger(), "Successful set up FIFO RT scheduling policy with priority %i.",
                        thread_priority);
                }
            } else {
                RCLCPP_WARN(
                    controller_manager->get_logger(),
                    "No real-time kernel detected on this system. See "
                    "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] "
                    "for details on how to enable realtime scheduling.");
            }

            // for calculating sleep time
            auto const period = std::chrono::nanoseconds(1'000'000'000 / controller_manager->get_update_rate());
            auto const cm_now = std::chrono::nanoseconds(controller_manager->now().nanoseconds());
            std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> next_iteration_time{ cm_now };

            // for calculating the measured period of the loop
            rclcpp::Time previous_time = controller_manager->now();

            while (rclcpp::ok()) {
                // calculate measured period
                auto const current_time = controller_manager->now();
                auto const measured_period = current_time - previous_time;
                previous_time = current_time;

                // execute update loop
                controller_manager->read(controller_manager->now(), measured_period);
                controller_manager->update(controller_manager->now(), measured_period);
                controller_manager->write(controller_manager->now(), measured_period);

                // wait until we hit the end of the period
                next_iteration_time += period;
                std::this_thread::sleep_until(next_iteration_time);
            }
        }
    );

    // spin the executor with controller manager node
    executor->add_node(controller_manager);
    executor->spin();

    // wait for control loop to finish
    control_loop_thread.join();

    // shutdown
    rclcpp::shutdown();

    return 0;
}

3.3. ROS2 Control URDF 파일 생성

파일 경로: my_package/description/robot.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="my_robot">
    <link name="world" />

    <joint name="joint_1" type="continuous">
        <parent link="world"/>
        <child link="link_1" />
        <origin xyz="0 0 0" rpy="0 0 0"/>
    </joint>
    <link name="link_1"/>

    <ros2_control name="my_robot_control" type="system">
        <hardware>
            <plugin>my_hardware_interface/myHardwareInterface</plugin>
        </hardware>

        <joint name="joint_1">
            <command_interface name="position" />
            <state_interface name="position" />
            <state_interface name="velocity" />
        </joint>
    </ros2_control>
    
</robot>

command/state 인터페이스 테스트를 위해 joint와 link를 하나씩 추가해줬다

<ros2_control> 태그의 <hardware>-<plugin> 태그값은 앞서 만든 myHardwareInterface를 기입해준다

3.4. Launch 스크립트 생성

파일 경로: my_package/launch/test.launch.py

import os
import sys
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import Command
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterFile

PACKAGE_NAME = "ros2_control_debug_test"

def generate_launch_description() -> LaunchDescription:
    ld = LaunchDescription()

    pkg_share_dir = get_package_share_directory(PACKAGE_NAME)
    urdf_file_path = os.path.join(pkg_share_dir, "description", "robot.urdf.xacro") 
    cfg_file_path = os.path.join(pkg_share_dir, "config", "ros2_controllers.yaml")

    robot_description = {
        "robot_description": Command([
            "xacro ", urdf_file_path,
        ])
    }

    robot_state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="screen",
        parameters=[
            robot_description
        ]
    )
    ld.add_action(robot_state_publisher_node)

    ros2_control_node = Node(
        package="ros2_control_debug_test",
        executable="my_ros2_control_node",
        output="screen",
        parameters=[
            robot_description,
            ParameterFile(cfg_file_path, allow_substs=True),
        ],
        prefix=['gdbserver localhost:1234'],
        emulate_tty=True
    )
    ld.add_action(ros2_control_node)

    return ld

여기서 눈여겨볼 사항은 ros2_control_debug_test 노드 실행 시 prefix 인자로 gdbserver 명령어를 추가한 것이다

이렇게 되면 노드 실행 시 localhost:1234 주소로 gdbserver가 실행되어 gdb클라이언트를 적절하게 붙여주면 원격 디버깅이 가능하다 (ip주소 및 포트는 입맛대로 변경하면 된다)

gdbserver 명령어는 https://man7.org/linux/man-pages/man1/gdbserver.1.html 에서 참고하면 된다

4. 패키지 빌드

4.1. CMake 빌드 설정

my_package/CMakeLists.txt

cmake_minimum_required(VERSION 3.12)
project(my_package)

# my_hardware_interface
add_library(my_hardware_interface SHARED 
   src/my_hardware_interface.cpp)
target_link_libraries(my_hardware_interface PUBLIC 
    ${controller_manager_msgs_TARGETS}
    controller_manager::controller_manager
    hardware_interface::hardware_interface
    pluginlib::pluginlib
    rclcpp::rclcpp
    rclcpp_lifecycle::rclcpp_lifecycle
)
target_include_directories(my_hardware_interface PRIVATE include)
pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml)
install(TARGETS my_hardware_interface
    EXPORT export_${PROJECT_NAME}
    DESTINATION lib
)

# my_ros2_control_node
add_executable(my_ros2_control_node
    src/my_ros2_control_node.cpp
)
ament_target_dependencies(my_ros2_control_node
    controller_manager
    rclcpp
)
install(TARGETS my_ros2_control_node 
    DESTINATION lib/${PROJECT_NAME}
)
ament_export_libraries(my_ros2_control_node)
# etc
install(DIRECTORY
    config
    description
    launch
    DESTINATION share/${PROJECT_NAME}
)
ament_export_targets(export_${PROJECT_NAME})
ament_package()

4.2. Colcon Build 

$ colcon build --packages-select my_package --cmake-args -DCMAKE_BUILD_TYPE=Debug --symlink-install

빌드 시 CMake 인자로 CMAKE_BUILD_TYPE을 Debug로 설정해야만 breakpoint를 찍어가며 제대로 디버깅할 수 있다

5. VSCode Launch 파일 설정

.vscode/launch.json

{
    "version": "0.2.0",
    "configurations": [
        {
            "name": "ROS2 Control Node",
            "type": "cppdbg",
            "request": "launch",
            "program": "~/ros2_ws/install/ros2_control_debug_test/lib/ros2_control_debug_test/my_ros2_control_node",
            "miDebuggerServerAddress": "localhost:1234",
            "stopAtEntry": false,
            "cwd": "${workspaceFolder}/src",
            "environment": [],
            "externalConsole": true,
            "linux": {
              "MIMode": "gdb"
            },
            "osx": {
              "MIMode": "gdb"
            },
            "windows": {
              "MIMode": "gdb"
            }
          }
    ]
}

VSCode에서 원격 디버깅을 위해 launch.json 파일을 작성해준다

다음 3가지 인자는 정확히 기입해줘야 정상적으로 디버깅할 수 있다

• program: 실행되는 my_ros2_control_node 바이너리 파일 경로 (리모트 디버깅 시 로컬에도 빌드되어 있어야 한다)
• miDebuggerServerAddress: 실행 중인 gdbserver 주소 (앞서 ros2_control_node 실행 시 prefix에서 설정한 바 있다)
• cwd: 소스코드 경로 (cpp 파일)

6. 디버깅

6.1. 패키지 실행

$ ros2 launch my_package test.launch.py

launch하면 gdbserver가 실행되면서 gdb 클라이언트가 접속되기를 기다리는 상태가 된다

6.2. VSCode Launch

원하는 곳에 breakpoint는 지정한 뒤 vscode에서 F5를 눌러서 디버깅을 시작해본다

Variables, Watch, Call Stack 등 VSCode에서 디버깅 툴들을 사용해 디버깅을 할 수 있다

 

[Reference]

https://control.ros.org/kilted/doc/ros2_control/doc/debugging.html