ROS2 한 노드에서 publish와 subscribe 동시에 동작시키기 (C++)

ROS는 C++과 Python을 이용하여 사용할 수 있다.
필자는 주로 Python을 많이 사용하지만 이번 기회에 C++에 익숙해지기 위하여 이전 글과 동일한 동작을 하는 코드를 작성해보았다.

pkg 생성

$ cd ~/robot_ws/src
$ ros2 pkg create my_test_pkg_cpp --build-type ament_cmake --dependencies rclcpp std_msgs

pkg를 생성하기 위해 workspace로 들어가서 pkg를 생성해준다.
C++로 코드를 작성할 것이므로 ament_cmake와 rclcpp를 사용했다.

코드 작성

master_order.cpp

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

class MasterOrder : public rclcpp::Node
{
public:
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr order_pub_;
  MasterOrder() : Node("master_order")
  {
    auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));
    order_pub_ = this->create_publisher<std_msgs::msg::String>(
      "order",
      qos_profile
    );
  }

  void publish_order_msg(std::string order)
  {
    auto msg = std_msgs::msg::String();
    msg.data = order;
    RCLCPP_INFO(this->get_logger(), "Order publish");
    order_pub_->publish(msg);
  }
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto master_order = std::make_shared<MasterOrder>();
  std::string order;
  while(rclcpp::ok())
  {
    std::cout << "Order: ";
    std::cin >> order;

    if(order == "1" || order == "2")
    {
      master_order->publish_order_msg(order);
    }
    else if(order == "stop")
    {
      RCLCPP_INFO(master_order->get_logger(), "Order Stop");
      master_order->publish_order_msg(order);
    }
    else
    {
      RCLCPP_INFO(master_order->get_logger(), "Try again");
    }
  }

  rclcpp::shutdown();
  return 0;
}

C++을 읽기만 해보고 코드를 직접 작성한 경험은 처음이라 시간이 오래 걸렸다.
기본적인 구조는 python과 유사하다.

node1.cpp

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using std::placeholders::_1;

class Node1 : public rclcpp::Node
{
public:
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr node1_pub_;
  std::string order_msg;
  Node1() : Node("node1")
  {
    auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));
    order_sub_ = this->create_subscription<std_msgs::msg::String>(
      "order",
      qos_profile,
      std::bind(&Node1::subscribe_order_msg, this, _1)
    );
    node1_pub_ = this->create_publisher<std_msgs::msg::String>(
      "node1_pub",
      qos_profile
    );
    node2_sub_ = this->create_subscription<std_msgs::msg::String>(
      "node2_pub",
      qos_profile,
      std::bind(&Node1::subscribe_node2_msg, this, _1)
    );
  }

  void publish_node1_msg() const
  {
    auto msg = std_msgs::msg::String();
    msg.data = "node1 talk";
    node1_pub_->publish(msg);
  }

private:
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr order_sub_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr node2_sub_;
  void subscribe_order_msg(const std_msgs::msg::String::SharedPtr msg)
  {
    RCLCPP_INFO(this->get_logger(), "Order publish");
    order_msg = msg->data;

    if(order_msg == "stop")
    {
      RCLCPP_INFO(this->get_logger(), "node1 stop");
    }
    else if(order_msg == "1")
    {
      RCLCPP_INFO(this->get_logger(), "node1 activate");
      publish_node1_msg();
    }
  }

  void subscribe_node2_msg(const std_msgs::msg::String::SharedPtr msg) const
  {
    if(order_msg != "stop")
    {
      RCLCPP_INFO(this->get_logger(), "node1 listen: %s", msg->data.c_str());
      publish_node1_msg();
    }
  }
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node1 = std::make_shared<Node1>();
  RCLCPP_INFO(node1->get_logger(), "node1 ready");
  rclcpp::spin(node1);
  rclcpp::shutdown();
  return 0;
}

node2.cpp

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using std::placeholders::_1;

class Node2 : public rclcpp::Node
{
public:
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr node2_pub_;
  std::string order_msg;
  Node2() : Node("node2")
  {
    auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));
    order_sub_ = this->create_subscription<std_msgs::msg::String>(
      "order",
      qos_profile,
      std::bind(&Node2::subscribe_order_msg, this, _1)
    );
    node2_pub_ = this->create_publisher<std_msgs::msg::String>(
      "node2_pub",
      qos_profile
    );
    node1_sub_ = this->create_subscription<std_msgs::msg::String>(
      "node1_pub",
      qos_profile,
      std::bind(&Node2::subscribe_node1_msg, this, _1)
    );
  }
  void publish_node2_msg() const
  {
    auto msg = std_msgs::msg::String();
    msg.data = "node2 talk";
    node2_pub_->publish(msg);
  }

private:
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr order_sub_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr node1_sub_;
  void subscribe_order_msg(const std_msgs::msg::String::SharedPtr msg)
  {
    RCLCPP_INFO(this->get_logger(), "Order publish");
    order_msg = msg->data;

    if(order_msg == "stop")
    {
      RCLCPP_INFO(this->get_logger(), "node2 stop");
    }
    else if(order_msg == "2")
    {
      RCLCPP_INFO(this->get_logger(), "node2 activate");
      publish_node2_msg();
    }
  }

  void subscribe_node1_msg(const std_msgs::msg::String::SharedPtr msg) const
  {
    if(order_msg != "stop")
    {
      RCLCPP_INFO(this->get_logger(), "node2 listen: %s", msg->data.c_str());
      publish_node2_msg();
    }
  }
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node2 = std::make_shared<Node2>();
  RCLCPP_INFO(node2->get_logger(), "node2 ready");
  rclcpp::spin(node2);
  rclcpp::shutdown();
  return 0;
}

구현한 동작이 이전에 python으로 작성한 것과 동일하므로 코드에 대한 설명은 생략하도록 하겠다.

코드를 작성할 때 어려웠던 점들

C++로 코드를 작성하는 것이 처음이라 생각보다 많은 삽질을 하였다.
포인터를 대학교 C언어 수업에서 사용한 이후로 처음 써봤으므로... 상당히 헷갈렸지만 사용법을 얼추 익힌 것 같다. 아마 추후 코드를 작성하면서 확인할 수 있지 않을까?

create_subscription을 통해 subscribe를 선언할 때 사용한 bind에 대한 내용을 이해하는 것이 생각보다 어려웠다.

std::bind

구글링을 했을 때 bind의 역할은 호출이 가능한 객체에 인수를 바인딩 해준다고 한다.
역시 말로 이해하는 것보다 코드로 확인해보자.

#include <iostream>
#include <functional>

void hello(const std::string& s)
{
	std::cout << s << std::endl;
}

int main()
{
	auto func = std::bind(hello, "hello world");
    func();
}

출력 결과 hello world를 확인할 수 있다.
hello의 입력으로 s가 입력이 되어야 하는데 그 입력을 "hello world"로 지정한 것으로 확인할 수 있다.
bind를 사용하면 함수의 이름과 함수의 인수를 지정해줄 수 있다.

std::placeholders

placeholders 또한 이해하기 쉽도록 예시 코드로 확인해보자

int sum(int a, int b, int c)
{
	return a+b+c;
}

int main()
{
	auto func1 = std::bind(sum, std::placeholders::_1, 2, 3);
    std::cout << func1(1) << std::endl;
}

출력 결과 6을 확인할 수 있다.
bind를 통해 인수를 지정해주며 placeholders 위치를 제외한 2번째 3번째 인수는 2와 3으로 고정된 것으로 이해할 수 있다.
즉 placeholders를 이용하면 bind를 사용할 때 특정 위치를 제외하고 고정할 수 있게 된다.
_ 뒤의 숫자는 인수의 위치를 얘기한다.

CMakeLists.txt 수정

cmake_minimum_required(VERSION 3.5)
project(my_test_pkg_cpp)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
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(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

# build
add_executable(master_order src/master_order.cpp)
ament_target_dependencies(master_order rclcpp std_msgs)

add_executable(node1 src/node1.cpp)
ament_target_dependencies(node1 rclcpp std_msgs)

add_executable(node2 src/node2.cpp)
ament_target_dependencies(node2 rclcpp std_msgs)

# install
install(TARGETS
  master_order
  node1
  node2
  DESTINATION  lib/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

find dependencies 부분에서 pkg를 생성할 때 지정한 dependencies가 제대로 들어가 있는지 확인한다.
build 부분에서 add_executable에 노드 이름, 작성된 파일의 경로를 입력한다.
ament_target_dependencies에 노드에 사용된 dependencies를 입력한다.
install 부분에 노드의 이름을 적어주면 된다.

동작 확인