C++에서 ROS2 노드의 구조

ROS2에서 C++로 노드를 작성할 때는 rclcpp 라이브러리를 사용하여 노드를 생성하고, ROS2의 퍼블리셔(Publisher), 서브스크라이버(Subscriber), 서비스(Service), 액션(Action) 등을 관리할 수 있다. C++ 노드 작성의 주요 단계는 아래와 같다.

  1. 헤더 파일 포함 노드를 구현하기 위해 필요한 헤더 파일을 포함한다. rclcpp 라이브러리의 핵심 구성 요소는 rclcpp::Node 클래스를 상속하여 구현할 수 있다.
#include "rclcpp/rclcpp.hpp"
  1. 노드 클래스 정의 rclcpp::Node 클래스를 상속한 노드 클래스를 정의한다. 예시로, 퍼블리셔 노드를 정의하는 기본 구조는 아래와 같다.
class SimplePublisher : public rclcpp::Node {
public:
  SimplePublisher()
    : Node("simple_publisher") {
      publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
      timer_ = this->create_wall_timer(
        std::chrono::milliseconds(500),
        std::bind(&SimplePublisher::publish_message, this));
  }

private:
  void publish_message() {
    auto message = std_msgs::msg::String();
    message.data = "Hello, ROS2!";
    publisher_->publish(message);
  }

  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
};

이 코드는 rclcpp::Node에서 상속된 기본적인 노드 구조를 갖추며, 노드 이름으로 "simple_publisher"를 정의하고 있다.

노드 초기화 및 실행

ROS2 노드는 rclcpp::init과 rclcpp::spin 함수를 사용하여 초기화되고 실행된다. 아래와 같은 코드에서 노드 실행 환경을 설정할 수 있다.

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<SimplePublisher>());
  rclcpp::shutdown();
  return 0;
}

퍼블리셔 및 서브스크라이버 구현

퍼블리셔는 노드가 메시지를 특정 토픽으로 내보내는 역할을 하고, 서브스크라이버는 특정 토픽의 메시지를 받아 처리하는 역할을 한다.

퍼블리셔

퍼블리셔는 특정 토픽에 대해 데이터를 발행할 수 있다. 퍼블리셔를 생성할 때 create_publisher 메서드를 사용하며, 퍼블리시할 메시지 타입을 명시해야 한다. 

publisher_ = this->create_publisher<std_msgs::msg::String>("topic_name", 10);

서브스크라이버

서브스크라이버는 특정 토픽의 데이터를 수신하고, 이를 처리하기 위해 콜백 함수를 정의한다. 서브스크라이버를 생성할 때 create_subscription 메서드를 사용하며, 수신할 메시지 타입을 명시해야 한다.

subscription_ = this->create_subscription<std_msgs::msg::String>(
  "topic_name", 10,
  std::bind(&SimpleSubscriber::handle_message, this, std::placeholders::_1));

퍼블리셔-서브스크라이버 관계 시각화

퍼블리셔와 서브스크라이버 간의 관계는 다음과 같이 시각화할 수 있다.

graph LR A[퍼블리셔] -->|메시지 발행| B[토픽] B -->|메시지 구독| C[서브스크라이버]

퍼블리셔는 메시지를 발행하며, 서브스크라이버는 해당 메시지를 구독하여 처리한다.

QoS 설정

퍼블리셔와 서브스크라이버의 통신 품질을 제어하기 위해 QoS(품질 서비스)를 설정할 수 있다. QoS는 네트워크 상태에 따라 메시지의 신뢰성이나 내구성을 조정하는 데 사용된다.

auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
publisher_ = this->create_publisher<std_msgs::msg::String>("topic_name", qos);

퍼블리셔 및 서브스크라이버 구현 예시

퍼블리셔와 서브스크라이버를 활용한 예시는 아래와 같다.

// 퍼블리셔 예시
class MinimalPublisher : public rclcpp::Node {
public:
  MinimalPublisher() : Node("minimal_publisher") {
    publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(500),
      std::bind(&MinimalPublisher::publish_message, this));
  }

private:
  void publish_message() {
    auto message = std_msgs::msg::String();
    message.data = "Hello, ROS2!";
    publisher_->publish(message);
  }

  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
};

// 서브스크라이버 예시
class MinimalSubscriber : public rclcpp::Node {
public:
  MinimalSubscriber() : Node("minimal_subscriber") {
    subscription_ = this->create_subscription<std_msgs::msg::String>(
      "topic", 10,
      std::bind(&MinimalSubscriber::handle_message, this, std::placeholders::_1));
  }

private:
  void handle_message(const std_msgs::msg::String::SharedPtr msg) {
    RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
  }

  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
};

퍼블리셔는 0.5초마다 "Hello, ROS2!" 메시지를 발행하고, 서브스크라이버는 이 메시지를 수신하여 출력한다.

퍼블리셔-서브스크라이버 성능 최적화

퍼블리셔와 서브스크라이버의 성능을 최적화하려면 네트워크 트래픽을 줄이고 메시지 처리 시간을 최소화해야 한다. 주요 방법으로는 다음과 같은 사항이 있다.

  1. 메시지 주기 최적화: 불필요한 메시지 발행을 줄이고, 필요한 메시지 주기를 적절히 설정한다.
  2. QoS 정책 조정: QoS 설정을 통해 네트워크 상태에 맞춰 신뢰성과 내구성을 조정한다.
  3. 병렬 처리: 멀티스레딩을 적용하여 메시지 수신과 처리를 병렬로 처리함으로써 성능을 향상시킬 수 있다.
this->create_subscription<std_msgs::msg::String>(
  "topic_name", 10, rclcpp::QoS(rclcpp::KeepLast(10)),
  [this](const std_msgs::msg::String::SharedPtr msg) {
    // 메시지 처리 코드
  });

노드의 멀티스레딩 적용

멀티스레딩은 노드의 성능을 최적화하는 데 중요한 요소이다. ROS2는 기본적으로 싱글 스레드로 동작하지만, 멀티스레드를 지원하는 실행기를 사용할 수 있다.

rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(node);
executor.spin();

위 코드에서는 멀티스레딩 실행기를 사용하여, 여러 콜백 함수가 병렬로 실행될 수 있도록 설정한다.

서비스와 액션의 C++ 노드에서의 구현

ROS2에서 서비스와 액션은 C++로도 구현할 수 있다. 서비스는 요청-응답 형태로 동작하고, 액션은 더 복잡한 비동기 작업을 수행할 수 있도록 설계된다. 이 섹션에서는 서비스와 액션을 rclcpp 기반으로 C++에서 어떻게 구현할 수 있는지 설명한다.

서비스 구현

서비스는 클라이언트가 서버에 요청을 보내고 응답을 받는 방식으로 동작한다. C++에서 서비스 서버와 클라이언트를 각각 구현할 수 있다.

  1. 서비스 서버 생성

서버는 클라이언트의 요청을 처리하기 위해 create_service 메서드를 사용하여 생성된다.

service_ = this->create_service<example_interfaces::srv::AddTwoInts>(
  "add_two_ints", std::bind(&SimpleService::handle_service, this, std::placeholders::_1, std::placeholders::_2));

서비스 요청을 처리하는 콜백 함수는 요청과 응답을 매개변수로 받아서 구현된다.

void handle_service(const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
                    std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response) {
  response->sum = request->a + request->b;
  RCLCPP_INFO(this->get_logger(), "Incoming request: a=%ld, b=%ld", request->a, request->b);
}

서비스 클라이언트 생성

서비스 클라이언트는 서버에 요청을 보내고 응답을 기다린다. create_client 메서드를 통해 클라이언트를 생성한다.

auto client = this->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");

서비스 클라이언트는 요청을 생성하고 서버에 전송한다.

auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
request->a = 5;
request->b = 3;

auto result = client->async_send_request(request);

액션 구현

액션은 비동기 작업을 관리하며, 장시간에 걸쳐 진행되는 작업에 적합하다. 액션은 서버와 클라이언트로 나뉘며, 서버는 작업을 수행하고 클라이언트는 작업의 상태와 결과를 모니터링한다.

  1. 액션 서버 생성

액션 서버는 클라이언트가 요청한 작업을 수행한다. create_action_server 메서드를 사용하여 액션 서버를 생성한다.

action_server_ = rclcpp_action::create_server<example_interfaces::action::Fibonacci>(
  this,
  "fibonacci",
  std::bind(&FibonacciActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
  std::bind(&FibonacciActionServer::handle_cancel, this, std::placeholders::_1),
  std::bind(&FibonacciActionServer::handle_accepted, this, std::placeholders::_1));

서버는 클라이언트의 요청을 처리하며, 작업을 수행하는 동안 피드백을 전송할 수 있다.

void execute_goal(const std::shared_ptr<GoalHandleFibonacci> goal_handle) {
  const auto goal = goal_handle->get_goal();
  auto feedback = std::make_shared<example_interfaces::action::Fibonacci::Feedback>();
  auto result = std::make_shared<example_interfaces::action::Fibonacci::Result>();

  feedback->sequence.push_back(0);
  feedback->sequence.push_back(1);

  for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
    feedback->sequence.push_back(feedback->sequence[i] + feedback->sequence[i - 1]);
    goal_handle->publish_feedback(feedback);
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }

  result->sequence = feedback->sequence;
  goal_handle->succeed(result);
}

액션 클라이언트 생성

액션 클라이언트는 서버에 작업을 요청하고, 작업의 상태와 피드백을 확인할 수 있다. create_client 메서드를 사용하여 액션 클라이언트를 생성한다.

auto action_client = rclcpp_action::create_client<example_interfaces::action::Fibonacci>(node, "fibonacci");
  1. 액션 요청 전송

클라이언트는 서버에 액션 요청을 전송한다. 

auto goal_msg = example_interfaces::action::Fibonacci::Goal();
goal_msg.order = 10;

auto goal_handle_future = action_client->async_send_goal(goal_msg);

클라이언트는 서버로부터 피드백을 수신할 수 있다.

action_client->async_get_feedback([&](auto feedback) {
  RCLCPP_INFO(node->get_logger(), "Received feedback: %d", feedback->sequence.back());
});