이번에는 파이썬과 유사하게 기초 프로그래밍을 응용하여 C++ 패키지를 설계하고, 토픽 프로그래밍을 한번 해보자.
참고한 링크는 다음과 같다.
https://cafe.naver.com/openrt/24798
https://cafe.naver.com/openrt/24802

---

패키지 설계

이 부분의 경우 파이썬과 완전히 동일한 패키지를 만들 것이기 때문에, https://cafe.naver.com/openrt/24450
혹은
https://velog.io/@hy_k/ROS2-%ED%8C%8C%EC%9D%B4%EC%8D%AC-%ED%8C%A8%ED%82%A4%EC%A7%80-%EC%84%A4%EA%B3%84-%EB%B0%8F-%ED%94%84%EB%A1%9C%EA%B7%B8%EB%9E%98%EB%B0%8D%ED%86%A0%ED%94%BD
이곳을 참고하면 된다.

그리고 전체 코드를 보고 싶다면
https://github.com/robotpilot/ros2-seminar-examples/tree/main
이곳에서 보면 된다.

만약, 별도로 파일을 만들지 않고 이곳에서 코드를 받아서 진행하려면 다음과 같이 수행하면 된다.

$ cd robot_ws/src
$ git clone https://github.com/robotpilot/ros2-seminar-examples.git
$ cd ..
$ colcon build (혹은) colcon build --symlink-install
$ source install/local_setup.bash

위 코드는 ROS2 Foxy에서 작성하신 코드기에 그보다 상위 버젼인 ROS2 Humble 혹은 Jazzy에서는 빌드가 되지 않을 수도 있다.

만약 빌드가 성공적으로 되었으면,

$ ros2 run
혹은
$ ros2 launch

명령어를 통해서 각 노드를 실행하면 된다.

---

패키지 설정 파일(package.xml)

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>topic_service_action_rclcpp_example</name>
  <version>0.2.0</version>
  <description>ROS 2 rclcpp example package for the topic, service, action</description>
  <maintainer email="passionvirus@gmail.com">Pyo</maintainer>
  <license>Apache License 2.0</license>
  <author email="passionvirus@gmail.com">Pyo</author>
  <author email="routiful@gmail.com">Darby Lim</author>
  
  <buildtool_depend>ament_cmake</buildtool_depend>
  
  <depend>rclcpp</depend>
  <depend>rclcpp_action</depend>
  <depend>msg_srv_action_interface_example</depend>
  
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
  
  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

이전에 만들어놓은 인터페이스 패키지를 사용할 것이기 때문에 참고한다고 나와있다.

---

빌드 설정 파일(CMakeLists.txt)

# Set minimum required version of cmake, project name and compile options
cmake_minimum_required(VERSION 3.8)
project(topic_service_action_rclcpp_example)

if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# Find dependencies
find_package(ament_cmake REQUIRED)
find_package(msg_srv_action_interface_example REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)

include_directories(include)

# Build
add_executable(argument src/arithmetic/argument.cpp)
ament_target_dependencies(argument
  msg_srv_action_interface_example
  rclcpp
)

add_executable(calculator src/calculator/main.cpp src/calculator/calculator.cpp)
ament_target_dependencies(calculator
  msg_srv_action_interface_example
  rclcpp
  rclcpp_action
)

add_executable(checker src/checker/main.cpp src/checker/checker.cpp)
ament_target_dependencies(checker
  msg_srv_action_interface_example
  rclcpp
  rclcpp_action
)

add_executable(operator src/arithmetic/operator.cpp)
ament_target_dependencies(operator
  msg_srv_action_interface_example
  rclcpp
)

# Install
install(TARGETS
  argument
  calculator
  checker
  operator
  DESTINATION lib/${PROJECT_NAME}
)

install(DIRECTORY launch param
  DESTINATION share/${PROJECT_NAME}
)

# Test
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

# Macro for ament package
ament_package()

크게 어려운 내용이 없어서 설명을 하지 않아도 될 것 같다.

---

토픽 프로그래밍

💡 토픽(topic)
비동기식 단방향 메시지 송수신 방식,

• 토픽을 퍼블리시(publish) 하는 퍼블리셔(publisher)
• 토픽을 서브스크라이브(subscribe) 하는 서브스크라이버(subscriber)
  이렇게 2가지로 구성된다.

퍼블리셔 노드

토픽 퍼블리셔 역할은 argument 노드가 수행한다.
include 폴더 안에 argument.hpp 파일을 작성하고, src 폴더 안에 argument.cpp 파일을 작성하여 헤더 파일과 구현 파일을 분리한다.

먼저 argument.hpp 파일이다.

#ifndef ARITHMETIC__ARGUMENT_HPP_
#define ARITHMETIC__ARGUMENT_HPP_

#include <chrono>
#include <memory>
#include <string>
#include <utility>

#include "rclcpp/rclcpp.hpp"

#include "msg_srv_action_interface_example/msg/arithmetic_argument.hpp"


class Argument : public rclcpp::Node
{
public:
  using ArithmeticArgument = msg_srv_action_interface_example::msg::ArithmeticArgument;

  explicit Argument(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions());
  virtual ~Argument();

private:
  void publish_random_arithmetic_arguments();
  void update_parameter();

  float min_random_num_;
  float max_random_num_;

  rclcpp::Publisher<ArithmeticArgument>::SharedPtr arithmetic_argument_publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_event_sub_;
  rclcpp::AsyncParametersClient::SharedPtr parameters_client_;
};
#endif  // ARITHMETIC__ARGUMENT_HPP_

헤더 파일에 대해서 먼저 살펴보자.
우선, ROS2의 각종 필수 라이브러리들과 커스텀 헤더를 선언하였다.

💡 라이브러리
1. chrono : 시간을 다루는 라이브러리
2. memory : 동적 메모리와 스마트 포인터를 다루는 라이브러리
3. string : 문자열을 다루는 라이브러리
4. utility : 다양한 기능들을 담고 있는 라이브러리
5. rclcpp/rclcpp.hpp : 필수적인 rclcpp 라이브러리 헤더
6. 커스텀 인터페이스 패키지의 헤더 파일

#include "msg_srv_action_interface_example/msg/arithmetic_argument.hpp"

그리고 클래스에 대해서도 선언하였다.

💡 Argument 클래스
1. 생성자는 NodeOptions 객체를 인자로 받아, context, arguments, intro-process communication, parameter, allocator와 같은 다양한 옵션을 정할 수 있다.
2. 스마트 포인터 타입의 멤버 변수 Publisher와 TimerBase가 선언되어있다. 이들은 콜백 함수 실행을 위한 타이머와 토픽 퍼블리셔 역할을 수행한다.

---

그 다음으로 argument.cpp 파일이다.

#include <cstdio>
#include <memory>
#include <string>
#include <utility>
#include <random>

#include "rclcpp/rclcpp.hpp"
#include "rcutils/cmdline_parser.h"

#include "arithmetic/argument.hpp"

using namespace std::chrono_literals;

Argument::Argument(const rclcpp::NodeOptions & node_options)
: Node("argument", node_options),
  min_random_num_(0.0),
  max_random_num_(0.0)
{
  this->declare_parameter("qos_depth", 10);
  int8_t qos_depth = this->get_parameter("qos_depth").get_value<int8_t>();
  this->declare_parameter("min_random_num", 0.0);
  min_random_num_ = this->get_parameter("min_random_num").get_value<float>();
  this->declare_parameter("max_random_num", 9.0);
  max_random_num_ = this->get_parameter("max_random_num").get_value<float>();
  this->update_parameter();

  const auto QOS_RKL10V =
    rclcpp::QoS(rclcpp::KeepLast(qos_depth)).reliable().durability_volatile();

  arithmetic_argument_publisher_ =
    this->create_publisher<ArithmeticArgument>("arithmetic_argument", QOS_RKL10V);

  timer_ =
    this->create_wall_timer(1s, std::bind(&Argument::publish_random_arithmetic_arguments, this));
}

Argument::~Argument()
{
}

void Argument::publish_random_arithmetic_arguments()
{
  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_real_distribution<float> distribution(min_random_num_, max_random_num_);

  msg_srv_action_interface_example::msg::ArithmeticArgument msg;
  msg.stamp = this->now();
  msg.argument_a = distribution(gen);
  msg.argument_b = distribution(gen);
  arithmetic_argument_publisher_->publish(msg);

  RCLCPP_INFO(this->get_logger(), "Published argument_a %.2f", msg.argument_a);
  RCLCPP_INFO(this->get_logger(), "Published argument_b %.2f", msg.argument_b);
}

void Argument::update_parameter()
{
  parameters_client_ = std::make_shared<rclcpp::AsyncParametersClient>(this);
  while (!parameters_client_->wait_for_service(1s)) {
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
      return;
    }
    RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
  }

  auto param_event_callback =
    [this](const rcl_interfaces::msg::ParameterEvent::SharedPtr event) -> void
    {
      for (auto & changed_parameter : event->changed_parameters) {
        if (changed_parameter.name == "min_random_num") {
          auto value = rclcpp::Parameter::from_parameter_msg(changed_parameter).as_double();
          min_random_num_ = value;
        } else if (changed_parameter.name == "max_random_num") {
          auto value = rclcpp::Parameter::from_parameter_msg(changed_parameter).as_double();
          max_random_num_ = value;
        }
      }
    };

  parameter_event_sub_ = parameters_client_->on_parameter_event(param_event_callback);
}

void print_help()
{
  printf("For argument node:\n");
  printf("node_name [-h]\n");
  printf("Options:\n");
  printf("\t-h Help           : Print this help function.\n");
}

int main(int argc, char * argv[])
{
  if (rcutils_cli_option_exist(argv, argv + argc, "-h")) {
    print_help();
    return 0;
  }

  rclcpp::init(argc, argv);

  auto argument = std::make_shared<Argument>();

  rclcpp::spin(argument);

  rclcpp::shutdown();

  return 0;
}

먼저 hpp 파일에 없는 라이브러리를 추가적으로 include 하였다. 예를 들면 C언어 표준 입출력 라이브러리인 cstdio와 랜덤 정수 생성용 라이브러리인 random이다.

그리고 Argument 클래스 생성자에서는 노드 클래스를 활용해서 노드를 생성하고, 퍼블리셔 옵션을 지정하게 된다. 그리고 timer 변수를 통해서 멤버 함수를 콜백함수처럼 호출한다.

멤버 함수인 publish_random_arithmetic_arguments 멤버 함수는 timer에 의해 호출되어, 랜덤 숫자를 생성하고, 선언한 인터페이스를 통해서 인터페이스를 퍼블리시 한다. 그리고 로그를 작성한다.

---

서브스크라이브 코드

전체 코드가 굉장히 길기 때문에 토픽과 관련된 코드만 살펴본다.

  arithmetic_argument_subscriber_ = this->create_subscription<ArithmeticArgument>(
    "arithmetic_argument",
    QOS_RKL10V,
    [this](const ArithmeticArgument::SharedPtr msg) -> void
    {
      argument_a_ = msg->argument_a;
      argument_b_ = msg->argument_b;

      RCLCPP_INFO(
        this->get_logger(),
        "Timestamp of the message: sec %ld nanosec %ld",
        msg->stamp.sec,
        msg->stamp.nanosec);

      RCLCPP_INFO(this->get_logger(), "Subscribed argument a: %.2f", argument_a_);
      RCLCPP_INFO(this->get_logger(), "Subscribed argument b: %.2f", argument_b_);
    }
  );

노드 클래스를 활용해서 생성자를 통해 calculator 노드로 초기화 하고, 이후 QoS 설정을 맞춰준 다음에 서브스크라이브 함수를 선언하였다. 콜백 함수는 서브스크라이브 한 토픽 메시지에 접근하여 멤버 변수에 저장하고, 이를 로그로 나타내고 있다.