퍼블리셔, 서브스크라이버, 서비스 및 액션 구현

퍼블리셔 (Publisher) 구현

퍼블리셔는 데이터를 다른 노드로 전송하는 역할을 한다. ROS2에서는 퍼블리셔를 사용하여 특정 토픽에 데이터를 발행할 수 있다. C++에서 퍼블리셔를 구현하는 방법은 rclcpp 라이브러리를 사용하여 이루어진다. 아래는 기본적인 퍼블리셔 구현의 절차이다.

1. 퍼블리셔 클래스 정의

C++에서는 퍼블리셔를 정의하기 위해 rclcpp::Node 클래스를 상속받는 노드를 생성해야 한다.

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

class PublisherNode : public rclcpp::Node
{
public:
  PublisherNode() : Node("publisher_node")
  {
    publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(500), 
      std::bind(&PublisherNode::publishMessage, this));
  }

private:
  void publishMessage()
  {
    auto message = std_msgs::msg::String();
    message.data = "Hello, ROS2!";
    RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
    publisher_->publish(message);
  }

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

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

2. 주요 부분 설명

노드 생성: PublisherNode 클래스는 rclcpp::Node를 상속받아 노드를 정의한다.
퍼블리셔 생성: this->create_publisher를 사용하여 퍼블리셔를 생성하고, 퍼블리싱할 토픽의 이름은 "topic"이며, 큐 사이즈는 10이다.
타이머: create_wall_timer를 사용하여 500ms 간격으로 메시지를 발행한다.
메시지 발행: std_msgs::msg::String 타입의 메시지를 생성하고, 해당 메시지를 퍼블리셔로 발행한다.

3. 수식 관련

만약 퍼블리싱 데이터가 단순한 텍스트가 아닌 벡터 데이터를 포함할 경우, 벡터 형태의 데이터를 다룰 수 있다. 예를 들어, $\mathbf{v} \in \mathbb{R}^3$ 인 3차원 벡터 데이터를 퍼블리싱할 수 있다.

$\mathbf{v} = \begin{bmatrix} x \\ y \\ z \end{bmatrix}$

여기서 $x$ , $y$ , $z$ 는 각각 퍼블리싱할 실수 값이다.

서브스크라이버 (Subscriber) 구현

서브스크라이버는 퍼블리셔가 발행한 데이터를 받아 처리하는 역할을 한다. 퍼블리셔와 마찬가지로 rclcpp 라이브러리를 사용하여 서브스크라이버를 구현할 수 있다.

1. 서브스크라이버 클래스 정의

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

class SubscriberNode : public rclcpp::Node
{
public:
  SubscriberNode() : Node("subscriber_node")
  {
    subscription_ = this->create_subscription<std_msgs::msg::String>(
      "topic", 10, std::bind(&SubscriberNode::receiveMessage, this, std::placeholders::_1));
  }

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

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

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

2. 주요 부분 설명

노드 생성: SubscriberNode 클래스는 rclcpp::Node를 상속받아 노드를 정의한다.
서브스크라이버 생성: this->create_subscription을 사용하여 서브스크라이버를 생성하고, 토픽의 이름은 "topic"이며, 큐 사이즈는 10이다.
메시지 수신: 메시지가 수신될 때 호출되는 콜백 함수 receiveMessage에서 수신된 데이터를 출력한다.

3. 수식 관련

만약 수신된 데이터가 벡터 데이터인 경우, 이를 처리할 수 있다. 예를 들어, 수신된 벡터 데이터를 처리할 수 있으며, 데이터는 다음과 같은 3차원 벡터일 수 있다.

$\mathbf{v} = \begin{bmatrix} x \\ y \\ z \end{bmatrix}$

데이터는 각 축의 좌표로 해석되며, 수신 후 처리 로직에서 사용될 수 있다.

서비스 (Service) 구현

서비스는 요청(request)과 응답(response)으로 이루어진 통신 방식이다. ROS2에서 서비스는 서버와 클라이언트 구조로 동작하며, 서버는 클라이언트의 요청을 받아 처리하고, 클라이언트는 서버에 요청을 보내는 역할을 한다. 이를 구현하기 위해 rclcpp를 사용하여 C++로 서비스 서버와 클라이언트를 구현할 수 있다.

1. 서비스 서버 구현

먼저 서비스 서버를 구현하여 클라이언트로부터 요청을 받아 처리하는 노드를 작성한다.

#include <rclcpp/rclcpp.hpp>
#include <example_interfaces/srv/add_two_ints.hpp>

class ServiceServerNode : public rclcpp::Node
{
public:
  ServiceServerNode() : Node("service_server_node")
  {
    service_ = this->create_service<example_interfaces::srv::AddTwoInts>(
      "add_two_ints", 
      std::bind(&ServiceServerNode::handleService, this, std::placeholders::_1, std::placeholders::_2));
  }

private:
  void handleService(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(), "Request: a=%ld, b=%ld", request->a, request->b);
    RCLCPP_INFO(this->get_logger(), "Sending back response: [%ld]", response->sum);
  }

  rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service_;
};

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

2. 주요 부분 설명

서비스 생성: create_service를 사용하여 "add_two_ints"라는 이름의 서비스 서버를 생성한다. 이 서비스는 두 개의 정수를 더하는 기능을 제공한다.
서비스 처리: handleService 함수에서 클라이언트가 보낸 요청 데이터를 받아 처리하고 응답을 생성한다. 클라이언트가 보낸 두 정수 $a$ 와 $b$ 를 더하여 응답으로 반환한다.

3. 수식 관련

요청에서 받은 값 $a$ 와 $b$ 는 각각 정수이며, 서버는 이 두 값을 더하여 응답으로 돌려준다.

$\text{sum} = a + b$

여기서 $a, b \in \mathbb{Z}$ 이며, 결과는 요청된 값의 합계이다.

4. 서비스 클라이언트 구현

클라이언트는 서버에 요청을 보내고 응답을 받는 역할을 한다.

#include <rclcpp/rclcpp.hpp>
#include <example_interfaces/srv/add_two_ints.hpp>

class ServiceClientNode : public rclcpp::Node
{
public:
  ServiceClientNode() : Node("service_client_node")
  {
    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;

    while (!client_->wait_for_service(std::chrono::seconds(1))) {
      RCLCPP_INFO(this->get_logger(), "Waiting for service...");
    }

    auto result_future = client_->async_send_request(request);
    auto result = result_future.get();

    RCLCPP_INFO(this->get_logger(), "Result of add_two_ints: %ld", result->sum);
  }

private:
  rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client_;
};

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

5. 주요 부분 설명

클라이언트 생성: create_client를 사용하여 서버에 연결되는 클라이언트를 생성한다.
서비스 요청: 요청 객체를 만들어 서버에 요청을 보낸다. 여기서 클라이언트는 두 정수 $5$ 와 $3$ 을 서버에 전달한다.

6. 수식 관련

클라이언트에서 서버에 보낸 요청은 다음과 같다.

$a = 5, \quad b = 3$

서버는 이 값을 받아 더한 결과 $\text{sum} = 8$ 을 응답으로 반환한다.

액션 (Action) 구현

액션은 장시간 수행되는 작업을 비동기적으로 처리할 수 있도록 도와주는 통신 방식이다. 액션은 액션 서버(Action Server)와 액션 클라이언트(Action Client)로 구성되며, 상태(State)와 피드백(Feedback)을 통해 작업의 진행 상황을 모니터링할 수 있다. 이를 통해 작업이 완료될 때까지 기다리거나 중간에 피드백을 받을 수 있다.

1. 액션 서버 구현

ROS2에서 액션 서버를 C++로 구현하기 위해서는 rclcpp_action 라이브러리를 사용한다. 액션 서버는 작업을 받아 비동기적으로 처리한 후 결과를 클라이언트에게 전달한다.

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <example_interfaces/action/fibonacci.hpp>

class ActionServerNode : public rclcpp::Node
{
public:
  using Fibonacci = example_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;

  ActionServerNode() : Node("action_server_node")
  {
    this->action_server_ = rclcpp_action::create_server<Fibonacci>(
      this, "fibonacci",
      std::bind(&ActionServerNode::handleGoal, this, std::placeholders::_1, std::placeholders::_2),
      std::bind(&ActionServerNode::handleCancel, this, std::placeholders::_1),
      std::bind(&ActionServerNode::handleAccepted, this, std::placeholders::_1));
  }

private:
  rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;

  rclcpp_action::GoalResponse handleGoal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const Fibonacci::Goal> goal)
  {
    RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
    (void)uuid;
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  rclcpp_action::CancelResponse handleCancel(
    const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
    (void)goal_handle;
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  void handleAccepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    std::thread{std::bind(&ActionServerNode::execute, this, goal_handle)}.detach();
  }

  void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "Executing goal...");
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<Fibonacci::Feedback>();
    auto & sequence = feedback->sequence;
    sequence.push_back(0);
    sequence.push_back(1);

    auto result = std::make_shared<Fibonacci::Result>();

    for (int i = 1; i < goal->order; ++i) {
      if (goal_handle->is_canceling()) {
        result->sequence = sequence;
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal canceled");
        return;
      }

      sequence.push_back(sequence[i] + sequence[i - 1]);
      goal_handle->publish_feedback(feedback);
      RCLCPP_INFO(this->get_logger(), "Publish feedback");
      std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }

    if (rclcpp::ok()) {
      result->sequence = sequence;
      goal_handle->succeed(result);
      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
    }
  }
};

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

2. 주요 부분 설명

액션 서버 생성: create_server를 사용하여 "fibonacci" 액션 서버를 생성하고, 목표(Goal)와 피드백(Feedback), 취소 요청을 처리하는 콜백 함수를 정의한다.
액션 처리: 목표를 수락하고 execute 함수를 통해 피보나치 수열을 계산하며, 중간 결과를 피드백으로 퍼블리싱한다.
피드백 및 완료: 목표가 완료되면 결과를 클라이언트에게 전송하고, 진행 중 취소 요청이 들어오면 이를 처리한다.

3. 수식 관련

피보나치 수열은 다음과 같은 재귀적인 수식으로 정의된다.

$F(0) = 0, \quad F(1) = 1, \quad F(n) = F(n-1) + F(n-2) \quad \text{for } n \geq 2$

액션 서버는 이러한 피보나치 수열을 계산하여 클라이언트에게 피드백을 제공한다.

4. 액션 클라이언트 구현

클라이언트는 서버에 작업을 요청하고, 그 작업의 진행 상황을 실시간으로 모니터링할 수 있다.

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <example_interfaces/action/fibonacci.hpp>

class ActionClientNode : public rclcpp::Node
{
public:
  using Fibonacci = example_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;

  ActionClientNode() : Node("action_client_node")
  {
    this->client_ = rclcpp_action::create_client<Fibonacci>(this, "fibonacci");

    if (!this->client_->wait_for_action_server()) {
      RCLCPP_ERROR(this->get_logger(), "Action server not available");
      return;
    }

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

    RCLCPP_INFO(this->get_logger(), "Sending goal");
    auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
    send_goal_options.feedback_callback =
      std::bind(&ActionClientNode::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2);
    send_goal_options.result_callback =
      std::bind(&ActionClientNode::resultCallback, this, std::placeholders::_1);

    this->client_->async_send_goal(goal_msg, send_goal_options);
  }

private:
  rclcpp_action::Client<Fibonacci>::SharedPtr client_;

  void feedbackCallback(
    GoalHandleFibonacci::SharedPtr, const std::shared_ptr<const Fibonacci::Feedback> feedback)
  {
    RCLCPP_INFO(this->get_logger(), "Received feedback: %ld", feedback->sequence.back());
  }

  void resultCallback(const GoalHandleFibonacci::WrappedResult & result)
  {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        RCLCPP_INFO(this->get_logger(), "Goal succeeded");
        break;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_INFO(this->get_logger(), "Goal canceled");
        break;
      default:
        RCLCPP_INFO(this->get_logger(), "Goal failed");
        break;
    }
    RCLCPP_INFO(this->get_logger(), "Result: %ld", result.result->sequence.back());
  }
};

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

5. 주요 부분 설명

액션 클라이언트 생성: create_client를 사용하여 "fibonacci" 액션 클라이언트를 생성한다.
액션 요청: 서버에 목표(Goal)를 보내고, 피드백 콜백과 결과 콜백을 설정하여 작업의 진행 상황과 결과를 수신한다.
피드백 처리: 서버로부터 피드백을 받을 때마다 콜백 함수가 호출되어 수열의 최신 값을 출력한다.

6. 수식 관련

피드백으로 받는 수열의 값은 피보나치 수열의 각 항이다. $F(n)$ 은 서버에서 계산되어 클라이언트로 전달된다.