ROS2 Driver Demo

ROS2 Driver Demo#

What you’ll do

  • ROS2 노드를 생성하여 KMC_HARDWARE::Driver를 구동합니다.

  • ROS2 메시지인 cmd_vel(geometry_msgs/msg/Twist)을 subscribe하여 차량에 명령 입력을 전달합니다.

  • 마지막으로 수신된 명령을 주기적으로 재전송(Latch)하는 기능을 구현합니다.

  • 디버깅을 위해 현재 적용 중인 명령을 cmd_echo 토픽으로 발행합니다.

Prerequisites

Next

이 튜토리얼은 기본 SDK의 driver_demo 예제를 ROS2 노드 구조로 변환하는 과정을 다룹니다.

ROS2의 토픽 시스템(cmd_vel)과 SDK의 Driver 클래스(UART 통신 스레드)를 결합하여 차량을 제어하는 가장 기초적인 노드입니다.

목표#

  1. 노드 구성: rclcpp::Node를 상속받아 UART 포트를 열고 Driver를 시작합니다.

  2. 명령 Subscribe: /cmd_vel 토픽을 구독하고, 수신된 메시지를 Driver::setCommand(v, omega)로 전달합니다.

  3. 명령 유지 (Optional): /cmd_vel 발행이 일시적으로 끊겨도 차량이 멈추지 않도록, 마지막 명령을 주기적으로 Driver에 갱신하는 타이머를 구현합니다.

  4. 피드백 Publish: 현재 Driver에 설정된 목표값을 /cmd_echo로 발행하여 동작을 검증합니다.

준비물#

  • ROS2 환경 사용 가능

  • KMC_HARDWARE::Driver 기본 동작 이해

  • UART 장치 접근 권한 확보 (/dev/ttyKMC)

단계별 구현 가이드#

단계 1) include 준비#

SDK의 드라이버 헤더와 ROS2 필수 헤더를 포함합니다.

#include "KMC_driver.hpp"

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>

#include <chrono>
#include <optional>
#include <string>

단계 2) 노드 클래스 정의#

rclcpp::Node를 상속받는 클래스를 정의하고, Driver 객체를 멤버로 가집니다. 소멸자에서는 반드시 driver_.stop()을 호출하여 UART 스레드를 안전하게 종료해야 합니다.

class KmcHardwareDriverDemoNode final : public rclcpp::Node {
public:
  KmcHardwareDriverDemoNode() : Node("kmc_hardware_driver_demo_node") {
    // 생성자에서 파라미터 선언 및 Driver 시작 로직을 구현합니다.
  }

  ~KmcHardwareDriverDemoNode() override { driver_.stop(); } // 노드 종료 시 안전 정지 및 포트 닫기

private:
  KMC_HARDWARE::Driver driver_;
};

단계 3) 멤버 변수 선언#

ROS 파라미터로 받을 설정 변수들과 Publisher/Subscription 객체를 선언합니다.

  // ROS2 통신 객체
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_echo_pub_;
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
  rclcpp::TimerBase::SharedPtr cmd_timer_;

  // 설정 파라미터
  std::string port_;
  int baud_{115200};
  double control_rate_hz_{100.0};
  double vehicle_speed_rate_hz_{1.0};
  int command_timeout_ms_{200};
  double command_refresh_hz_{50.0};
  int realtime_priority_{-1};
  int cpu_affinity_{-1};

  // 명령 상태 저장
  float last_cmd_v_{0.0f};
  float last_cmd_w_{0.0f};
  std::optional<rclcpp::Time> last_cmd_received_;

단계 4) 파라미터 선언#

런타임에 yaml 파일이나 ros2 run 인자로 설정을 변경할 수 있도록 파라미터를 선언합니다.

    port_ = declare_parameter<std::string>("port", "/dev/ttyKMC"); //포트 설정
    baud_ = declare_parameter<int>("baud", 115200); //baudrate 설정
    control_rate_hz_ = declare_parameter<double>("control_rate_hz", 100.0);  // Driver 송신 주기
    vehicle_speed_rate_hz_ = declare_parameter<double>("vehicle_speed_rate_hz", 1.0); // 속도 요청 주기
    command_timeout_ms_ = declare_parameter<int>("command_timeout_ms", 200); // Driver 타임아웃
    command_refresh_hz_ = declare_parameter<double>("command_refresh_hz", 50.0); // 명령 재전송(Latch) 주기
    realtime_priority_ = declare_parameter<int>("realtime_priority", -1);
    cpu_affinity_ = declare_parameter<int>("cpu_affinity", -1);

단계 5) Driver 옵션 구성 + 시작#

ROS 파라미터 값을 Driver::Options 구조체에 매핑하고, driver_.start()를 호출하여 통신 스레드를 시작합니다. 옵션 필드 의미는 Driver::Options를 기준으로 합니다.

    KMC_HARDWARE::Driver::Options opt;
    opt.port = port_;
    opt.serial.baudrate = baud_;
    opt.serial.hw_flow_control = true;
    opt.control_rate_hz = control_rate_hz_;
    opt.vehicle_speed_rate_hz = vehicle_speed_rate_hz_;
    opt.command_timeout_ms = command_timeout_ms_;
    opt.realtime_priority = realtime_priority_;
    opt.cpu_affinity = cpu_affinity_;

    if (!driver_.start(opt)) {
      throw std::runtime_error("Failed to open UART port: " + port_);
    }

단계 6) 토픽 연결 (cmd_vel 구독 + cmd_echo 발행)#

/cmd_vel 토픽을 구독하여 수신된 메시지(Twist)의 선속도와 각속도를 추출하고,Driver::setCommand() 함수를 통해 하드웨어 제어 스레드로 전달합니다.

    cmd_echo_pub_ = create_publisher<geometry_msgs::msg::Twist>("cmd_echo", 10);
    cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>(
        "cmd_vel", 10, [this](geometry_msgs::msg::Twist::SharedPtr msg) {
          const float v = static_cast<float>(msg->linear.x);
          const float w = static_cast<float>(msg->angular.z);
          last_cmd_v_ = v;
          last_cmd_w_ = w;
          last_cmd_received_ = now();
          driver_.setCommand(v, w);
          cmd_echo_pub_->publish(*msg);
        });

매핑 규칙:

  • msg->linear.x → v (m/s)

  • msg->angular.z → omega (rad/s)

단계 7) 명령 재전송 타이머 (선택)#

Driver는 command_timeout_ms 동안 setCommand 호출이 없으면 안전을 위해 차량에 정지 명령을 전송합니다.

  • 만약 ROS2 네트워크 지연 등으로 /cmd_vel 주기가 불안정하더라도 차량이 멈추지 않게 하려면, 마지막 명령을 주기적으로 다시 설정해주는 타이머가 필요합니다.

  • command_timeout_ms = 0이면 정지명령을 전송하지 않습니다.

    if (command_refresh_hz_ > 0.0) {
      const auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
          std::chrono::duration<double>(1.0 / command_refresh_hz_));
      cmd_timer_ = create_wall_timer(period, [this]() {
        if (!last_cmd_received_.has_value()) return;
        driver_.setCommand(last_cmd_v_, last_cmd_w_);
        geometry_msgs::msg::Twist msg;
        msg.linear.x = last_cmd_v_;
        msg.angular.z = last_cmd_w_;
        cmd_echo_pub_->publish(msg);
      });
    }

단계 8) main 함수#

노드 생성 후 spin으로 유지합니다.

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  try {
    auto node = std::make_shared<KmcHardwareDriverDemoNode>();
    rclcpp::spin(node);
  } catch (const std::exception& e) {
    RCLCPP_ERROR(rclcpp::get_logger("kmc_hardware_driver_demo_node"), "Fatal: %s",
                 e.what());
  }
  rclcpp::shutdown();
  return 0;
}

Result#

완성된 소스는 examples/Driver_ROS2/src/driver_demo_node.cpp에 있습니다.

#include "KMC_driver.hpp"

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>

#include <chrono>
#include <optional>
#include <string>

class KmcHardwareDriverDemoNode final : public rclcpp::Node {
public:
  KmcHardwareDriverDemoNode() : Node("kmc_hardware_driver_demo_node") {
    port_ = declare_parameter<std::string>("port", "/dev/ttyKMC");
    baud_ = declare_parameter<int>("baud", 115200);
    control_rate_hz_ = declare_parameter<double>("control_rate_hz", 100.0);
    vehicle_speed_rate_hz_ = declare_parameter<double>("vehicle_speed_rate_hz", 1.0);
    command_timeout_ms_ = declare_parameter<int>("command_timeout_ms", 200);
    command_refresh_hz_ = declare_parameter<double>("command_refresh_hz", 50.0);
    realtime_priority_ = declare_parameter<int>("realtime_priority", -1);
    cpu_affinity_ = declare_parameter<int>("cpu_affinity", -1);

    KMC_HARDWARE::Driver::Options opt;
    opt.port = port_;
    opt.serial.baudrate = baud_;
    opt.serial.hw_flow_control = true;
    opt.control_rate_hz = control_rate_hz_;
    opt.vehicle_speed_rate_hz = vehicle_speed_rate_hz_;
    opt.command_timeout_ms = command_timeout_ms_;
    opt.realtime_priority = realtime_priority_;
    opt.cpu_affinity = cpu_affinity_;

    if (!driver_.start(opt)) {
      throw std::runtime_error("Failed to open UART port: " + port_);
    }

    cmd_echo_pub_ = create_publisher<geometry_msgs::msg::Twist>("cmd_echo", 10);
    cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>(
        "cmd_vel", 10, [this](geometry_msgs::msg::Twist::SharedPtr msg) {
          const float v = static_cast<float>(msg->linear.x);
          const float w = static_cast<float>(msg->angular.z);
          last_cmd_v_ = v;
          last_cmd_w_ = w;
          last_cmd_received_ = now();
          driver_.setCommand(v, w);
          cmd_echo_pub_->publish(*msg);
        });

    if (command_refresh_hz_ > 0.0) {
      const auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
          std::chrono::duration<double>(1.0 / command_refresh_hz_));
      cmd_timer_ = create_wall_timer(period, [this]() {
        if (!last_cmd_received_.has_value()) return;
        driver_.setCommand(last_cmd_v_, last_cmd_w_);
        geometry_msgs::msg::Twist msg;
        msg.linear.x = last_cmd_v_;
        msg.angular.z = last_cmd_w_;
        cmd_echo_pub_->publish(msg);
      });
    }
  }

  ~KmcHardwareDriverDemoNode() override { driver_.stop(); }

private:
  KMC_HARDWARE::Driver driver_;

  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_echo_pub_;
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
  rclcpp::TimerBase::SharedPtr cmd_timer_;

  std::string port_;
  int baud_{115200};
  double control_rate_hz_{100.0};
  double vehicle_speed_rate_hz_{1.0};
  int command_timeout_ms_{200};
  double command_refresh_hz_{50.0};
  int realtime_priority_{-1};
  int cpu_affinity_{-1};

  float last_cmd_v_{0.0f};
  float last_cmd_w_{0.0f};
  std::optional<rclcpp::Time> last_cmd_received_;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  try {
    auto node = std::make_shared<KmcHardwareDriverDemoNode>();
    rclcpp::spin(node);
  } catch (const std::exception& e) {
    RCLCPP_ERROR(rclcpp::get_logger("kmc_hardware_driver_demo_node"), "Fatal: %s",
                 e.what());
  }
  rclcpp::shutdown();
  return 0;
}
Contents