ROS2 C++ Publisher / Subscriber

Xeno·2024년 2월 1일
ros2
1

Publisher and Subscriber

ROS 에서 실행되는 각 프로그램은 Node 라고 표현한다. 네비게이션 시스템을 예로 들면 아래와 같은 노드들로 나눌 수 있다.

  • GPS 위치 수신 노드
  • 화면 표시 노드
  • 경로 탐색 노드
  • 실시간 교통상황 수신 노드

실제로는 더 복잡한 시스템으로 구성되어 네비게이션이라는 하나의 시스템을 구성하겠지만, 지금은 예시일 뿐이니 대충 넘어가자.
GPS 위치 수신 노드에서는 센서에서 가져온 실시간 위치를 다른 노드에 알려주어야 하고, 경로 탐색 노드는 GPS 센서에서 얻은 현재 위치를 알아야만 현 위치에서 목적지까지 가는 경로를 계산할 수 있다.

일반적으로는 IPC (Inter Process Communication) 을 이용해 데이터를 주고받는데, 이 방식으로 통신을 하려면 송신부와 수신부의 통신 인터페이스를 일치시키고, 데이터 구조를 정의하여야 한다. 그리고 정의된 데이터 구조와 인터페이스를 통해 각 프로세스간 통신을 진행한다.

하지만 ROS 에서는 이 두가지 과정 중 인터페이스 부분을 Publisher / Subscriber 구조를 통해 쉽게 통합할 수 있게 해준다. 이 글에서 각 노드간 데이터 통신을 도와주는 Publisher / Subscriber 를 직접 구현하고 이해해 보자.

Let's Do

Create Workspace

먼저 작업을 위한 디렉토리를 먼저 설정해보자. 작업 디렉토리의 구조는 사람마다 달라질 수 있기 때문에 자유롭게 사용해도 무관하다.

.
├── install
│   └── ...
├── build
│   └── ...
├── log
│   └── ...
├── src
│   └── <package>
│   └── <package>
│   └── ...

필자의 경우 코드 관리를 용이하게 하기 위해, 개발하는 모든 패키지는 src 디렉토리를 하나 만들어 이 디렉토리 안에서 관리하고 있다. 이외 install, build, log 디렉토리의 경우 패키지를 빌드하게 되면 자동으로 생성되기 때문에 굳이 수동으로 만들 필요는 없다.

Create Package

ROS2 에서 패키지를 생성하려면 아래와 같이 입력하면 된다. package_name 을 제외한 다른 파라미터를 주지 않는다면 기본적으로 C++ 개발에 맞게 패키지가 생성된다.

ros2 pkg create <package_name>

해당 명령어를 입력했을때 아래와 같이 ros2 명령어를 찾을 수 없다고 뜨는 경우 ROS2 설치 경로의 local_setup.bat 을 호출해 ros2 환경세팅을 해주어야 한다.

'ros2' is not recognized as an internal or external command,
operable program or batch file.
call <ROS2_humble_installation_path>\setup.bat

위와 같이 입력하면 package_name 으로 폴더가 하나 생성되며 내부에 들어가면 아래와 같은 구조가 나타난다.

.
├── include
│   └── <package_name>
│       └── ...
├── src
│   └── ...
├── CMakeLists.txt
├── README
└── package.xml

자세한 구조는 다른 글에서 다룰 예정이고, 만약 해당 구조를 먼저 확인하고 싶다면 아래 링크에서 ROS2 에서 권장하는 패키지의 파일 구조를 확인할 수 있다.

우선 Publisher 와 Subscriber 코드를 작성하기 위해 src 디렉토리의 아래에 publisher_node.cpp 와 subscriber_node.cpp 라는 파일 두개를 생성하였다.

Write publisher node

우선 데이터를 전송하는 Publisher Node의 코드를 먼저 작성해보자.

Full code

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

using namespace std::chrono_literals;

namespace aa_pub_sub
{
  class PublisherNode : public rclcpp::Node
  {
    public:
      PublisherNode() : Node("publisher_node")
      {
        publisher_ = this->create_publisher<std_msgs::msg::String>("/string_topic", 1);
        timer_ = this->create_wall_timer(1s, std::bind(&PublisherNode::timer_callback, this));
      }

    private:
      rclcpp::TimerBase::SharedPtr timer_;
      rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
      int count_ = 0;

    private:
      void timer_callback()
      {
        std_msgs::msg::String message;
        message.data = "Hello, ROS2 Humble #" + std::to_string(count_);
        ++count_;

        RCLCPP_INFO_STREAM(this->get_logger(), message.data);
        publisher_->publish(message);
      }
  };
};

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<aa_pub_sub::PublisherNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();
}

Look into code

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

이 부분에서는 해당 코드에서 사용할 기본적인 header 파일들을 include 한다.
include 되는 파일은 아래와 같다.

  • rclcpp/rclcpp.hpp
    • ROS2의 기능이 포함된 header 파일
  • std_msgs/msg/string.hpp
    • 테스트 코드에서 사용할 메시지의 데이터 구조
int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<aa_pub_sub::PublisherNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();
}

class 의 구조를 확인하기 전에 우선 프로그램의 entry-point 인 main 함수를 먼저 확인하자.
기본적으로 C++ main 함수의 argument 인 argc 인자와 argv 인자를 이용해 ROS2 설정을 초기화한다. 이후 실제 기능이 담겨있는 Node 객체를 Pointer 형태로 생성하고, rclcpp::spin 함수를 통해 노드 객체를 동작시킨다.
Node 가 종료될 때에는 rclcpp::shutdown 함수를 호출해 ROS2 에 노드가 종료되었음을 알린다.

namespace aa_pub_sub
{
  class PublisherNode : public rclcpp::Node
  {
    ...
    ...
    ...
  };
};

이후 Node 객체를 정의한다. Namespace 의 경우 정의하지 않아도 무관하다. Node class 를 정의할 때 rclcpp::Node 객체의 상속을 받아야 해당 Node 객체를 이용해 ROS2 의 기능을 사용할 수 있다.

class PublisherNode : public rclcpp::Node
{
  private:
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    int count_ = 0;
};

코드 순서가 달라지긴 하지만 우선 클래스 멤버 변수 선언 부분부터 먼저 확인하자. 총 3개의 멤버 변수가 선언되어 있고, 중요하게 보아야 하는것은 위의 두가지 변수이다.

timer_ 변수의 경우 rclcpp::TimerBase 의 Pointer 형태로 정의가 되어있는데, 이는 ROS2 에서 제공하는 Timer 객체로, 일정 시간마다 특정 Method 를 호출해주는 기능을 사용하기 위해 해당 변수를 선언하였다. Pointer 형태로 정의한 이유는 생성자를 설명할 때 함께 설명할 예정이다.

publisher_ 변수의 경우 rclcpp::Publisher<std_msgs::msg::String>의 Pointer 형태로 정의되어 있다. 여기에서 이번 단계의 목표인 Publisher 객체를 확인할 수 있는데, 이 객체를 통해 데이터를 다른 ROS2 노드로 전송할 수 있다. 데이터 구조는 template 에 정의하면, 해당 데이터를 송신할 수 있게 된다.

class PublisherNode : public rclcpp::Node
{
  public:
    PublisherNode() : Node("publisher_node")
    {
      publisher_ = this->create_publisher<std_msgs::msg::String>("/string_topic", 1);
      timer_ = this->create_wall_timer(1s, std::bind(&PublisherNode::timer_callback, this));
    }
};

다음은 생성자이다.
생성자에서는 우선 상속받은 rclcpp::Node 객체의 생성자를 실행해 주어야 한다. 기본적으로는 Node 의 이름을 지정해주면 된다. 이 샘플에서는 노드의 이름을 publisher_node 로 지정하였다.

이후 앞에서 선언한 publisher_ 변수와 timer_ 변수의 값을 정의한다. 우선 publisher_ 객체를 생성하는 코드를 확인해보자.

publisher_ 생성시 송신할 메시지의 구조를 지정하는 template과, topic_name 파라미터, qos 파라미터 2개가 들어간다.

  • template 의 경우 앞서 생성한 변수의 template 과 일치하여야 한다.
  • topic_name 은 여러 종류의 데이터가 송/수신될 때 각각의 데이터를 구분하기 위해 붙여주는 이름이다. 간단하게 식별자라고 생각하면 편하다.
  • qos 는 통신 설정에 관련한 파라미터이다. 메시지를 송수신할때 사용하는 Message Queue의 크기나, 마지막에 보낸 데이터의 유지 여부 등을 설정할 수 있다. 여기에서는 qos 값을 1로 주어 Message Queue 의 크기를 1로 설정한다.

timer_ 객체를 생성할 때에는 period 와 callback 의 두가지 파라미터가 들어간다.

  • period 의 경우 timer의 callback 함수를 호출할 주기를 말한다. 지금 코드에서는 1s 를 넣어 1초로 정의하였다.
    • 코드상에서 1s 와 같이 시간을 나타내기 위해 클래스를 정의하기 전에 using namespace std::chrono_literals 를 이용해 Literal 형태로 시간을 표현할 수 있도록 설정하였다.
    • 만약 Literal 형태로 정의하고 싶지 않다면 1s 자리에 std::chrono::seconds::duration(1) 를 입력해 대체할 수 있다.
  • callback 의 경우 Timer 에서 period 시간이 경과했을 때 실행할 함수를 정의한다.
    • function 이 static function 이라면 단순히 &PublisherNode::timer_callback 으로 작성해도 무관하지만, 지금은 class 객체에 선언된 method 이기 때문에 std::bind 함수를 이용해 객체와 함수포인터를 바인딩 해 주어야 한다.
class PublisherNode : public rclcpp::Node
{
  private:
    void timer_callback()
    {
      std_msgs::msg::String message;
      message.data = "Hello, ROS2 Humble #" + std::to_string(count_);
      ++count_;

      RCLCPP_INFO_STREAM(this->get_logger(), message.data);
      publisher_->publish(message);
    }
};

다음은 Callback 함수이다. ROS2 Timer 의 callback 함수는 return type 이 void 이고, arguments 가 없어야 한다. 따라서 위의 함수와 같은 형태로 method를 정의하였다.

다음은 송신할 데이터를 정의한다. 먼저 메시지 객체를 생성하고, 데이터를 넣어준다. 수신측에서 메시지 확인을 쉽게 하기 위해서 Hello, ROS2 Humble #<n> 과 같은 형태로 메시지가 송신되도록 데이터를 작성하였다.

그런 다음 생성된 메시지를 ROS2 의 로깅 기능을 이용해 출력한다. RCLCPP_INFO_STREAM 매크로의 경우 logger 와 stream 의 두가지 파라미터를 받는다.

  • logger 파라미터의 경우 Node class 의 method 라면 this->get_logger() 함수를 이용해 생성자에서 지정한 node_name 으로 로깅되는 logger 를 가져올 수 있다.
    • 만약 Node class 가 아니라면 rclcpp::get_logger(<logger_name>) 함수를 이용해 logger_name 으로 된 logger 를 받아올 수 있다.
  • stream 파라미터는 출력할 메시지로, std::cout 과 비슷하게 1 << "ABC" << 1.34f 와 같이 입력하면 1ABC1.34 와 같이 출력된다.

마지막으로 생성한 데이터를 Publisher 를 통해 publish 한다.

Write subscriber node

다음으로 데이터 수신부의 코드를 살펴보자.

Full code

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

using namespace std::chrono_literals;

namespace aa_pub_sub
{
  class SubscriberNode : public rclcpp::Node
  {
    public:
      SubscriberNode() : Node("publisher_node")
      {
        subscription_ = this->create_subscription<std_msgs::msg::String>(
            "/string_topic", 1, std::bind(&SubscriberNode::subscription_callback, this, std::placeholders::_1));
      }

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

    private:
      void subscription_callback(std_msgs::msg::String::SharedPtr message)
      {
        RCLCPP_INFO_STREAM(this->get_logger(), message->data);
      }
  };
};

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<aa_pub_sub::SubscriberNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();
}

Look into code

클래스 선언부나 main 함수의 경우 클래스 명만 바뀌었을 뿐 Publisher Node 와 동일하기 때문에 넘어가고, 변수 선언부 부터 살펴보자.

class SubscriberNode : public rclcpp::Node
{
  private:
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
};

Publisher Node 때와는 다르게 rclcpp::Subscription 을 사용해 변수를 선언하였다. Publisher 와 마찬가지로 수신하려는 데이터 구조를 template 에 넣어 수신할 데이터 구조를 정의할 수 있다.

class SubscriberNode : public rclcpp::Node
{
  SubscriberNode() : Node("publisher_node")
  {
    subscription_ = this->create_subscription<std_msgs::msg::String>(
        "/string_topic", 1, std::bind(&SubscriberNode::subscription_callback, this, std::placeholders::_1));
  }
};

생성자에서는 멤버 변수로 선언한 subscription_ 변수에 값을 정의한다. subscription 을 정의할때는 Publisher 와 다르게 총 3가지 인자가 사용된다.

  • topic_name (string)
    • 데이터를 수신받을 Topic 명
  • qos
    • Publisher 의 QoS 와 동일
  • callback
    • 데이터를 수신하였을 때 호출할 함수

callback 함수를 정의할 때 std::bind 를 통해 정의한 이유는 Publisher 때와 마찬가지이니 자세한 설명은 위쪽을 참고하자.

class SubscriberNode : public rclcpp::Node
{
  private:
    void subscription_callback(std_msgs::msg::String::SharedPtr message)
    {
      RCLCPP_INFO_STREAM(this->get_logger(), message->data);
    }
};

callback 함수의 경우 위와 같이 인자값으로 메시지의 SharedPtr 를 받고, 반환값이 없는 형태로 정의된다. 이 규칙을 지키지 않아도 VSCode 의 내장 Intellicense 는 오류를 표시하지 않으니 주의해야 한다.

이번에는 Publisher / Subscriber 의 실습을 하는 것이기 때문에 단순히 받은 데이터를 출력하도록 callback 함수를 작성한다.

Modify CMakeLists.txt

다음은 CMakeFiles 를 수정해야한다. CMakeFiles 는 CMake 에서 지원하는 빌드 도구로, 자세한 내용은 인터넷이나 공식 문서를 통해 확인하자.
우선은 방금 작성한 코드로 프로그램을 빌드할 수 있도록 내용을 수정해보자.

기존에 있던 내용에서 테스트나 주석 등 필요없는 부분은 삭제하였으니 참고하자.

Full code

cmake_minimum_required(VERSION 3.8)
project(aa_pub_sub)

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

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

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

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

install(
  TARGETS publisher_node subscriber_node
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

Look into code

기존에 작성되어 있는 부분은 넘어가고 추가된 부분만 살펴보자.

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

먼저 dependency 를 찾는다. Publisher 나 Subscriber 코드의 가장 상단에 보면 include 를 통해 rclcpp 패키지와 std_msgs 패키지에서 header 파일을 불러오고 있다.
따라서 rclcpp 와 std_msgs 패캐지의 의존성을 받아와 빌드 과정에서 연결해 주어야 정상적으로 빌드가 가능하다.

ament_cmake 의 경우 ROS2 를 사용할 때 cmake 에서 설정해야 하는 설정들을 단순화 시켜 놓은 매크로 등이 정의되어 있어 CMakeFile 의 작성을 쉽게 해 주기 때문에 지우지 않고 유지하였다.

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

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

add_executable 은 실행 파일을 만드는 명령어이다. add_executable 은 첫번째로 받는 인자를 이름으로 설정하고, 이후에 오는 인자를 프로그램 소스 파일로 인식한다.
우리는 프로그램을 각각 publisher_node 라는 이름과 subscriber_node 라는 이름으로 만들 것이기 때문에 이를 이름으로 하고, 각각 이름에 맞는 소스 파일을 두번째 인자로 사용해 소스 코드를 인식시켜 주었다.

이후 코드에서 사용한 rclcpp 패키지와 std_msgs 패키지의 의존성을 해결하기 위해 ament_target_dependencies 라는 명령어를 이용하였다. 이 명령어는 첫번째로 받는 인자를 대상 프로그램으로 설정하고, 이후에 오는 인자를 의존성으로 인식해 관련된 헤더 파일 경로나 Lib 파일, Dll 파일 등을 대상 프로그램에 연결시켜준다.

install(
  TARGETS publisher_node subscriber_node
  DESTINATION lib/${PROJECT_NAME}
)

마지막으로 생성된 프로그램을 install 명령어로 설치한다. install 명령어는 다른 인자값도 많이 받지만 여기에서는 사용하는 두 인자만 설명한다.

  • TARGETS : 설치할 실행파일 또는 라이브러리의 이름
  • DESTINATION : 실행파일 또는 라이브러리를 설치할 경로

TARGETS 에는 조금 전에 add_executable 명령어로 선언한 실행 파일의 이름 2개를 넣어주고, DESTINATION 에는 ROS2 공식 문서에서 권장하는 경로인 lib/${PROJECT_NAME} 으로 정의하였다.

Modify package.xml

여기에서는 기본적으로 제공되는 package 만 사용하기 떄문에 사실 이 과정은 건너뛰어도 무관하지만, 이후에 여러 패키지를 생성하고, 각각의 패키지 간에 의존성이 생기게 되면 빌드 순서에 따라 오류가 나거나 링크 에러가 발생할 수 있다.
이런 오류가 발생하는 것을 피하기 위해 package.xml 에 패키지 간의 의존성을 정의해 colcon 에서 자동으로 빌드 순서를 결정하게 만들 수 있다.

Full code

<?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>aa_pub_sub</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="xeno.song@outlook.com">xeno</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>std_msgs</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

Look into code

패키지 명이나 maintainer, license 등은 설정이나 환경에 따라 바뀌기 때문에 자세한 설명은 넘어가고, 추가된 두줄만 살펴보자.

<depend>rclcpp</depend>
<depend>std_msgs</depend>

정석으로 하려면 build 의존성, 실행 의존성 등 정확하게 의존성을 명시하여야 하지만, 간단하게 하려면 위와 같이 xml 문법에 따라 <depend> 태그 안에 원하는 패키지 명을 넣으면 끝이다. 간단하지 않은가?

Build and Test

Build packages

이제 모든 코드의 수정이 마무리 되었으니 코드를 빌드해보자.
우선 Developer Command Prompt 를 켜서 ROS2 환경 설정을 진행한다.

call <ROS_INSTALL_PATH>/setup.bat

이후 패키지가 위치한 디렉토리로 이동한다.
필자의 경우 패키지를 src 디렉토리에 모아두었기 때문에, src 디렉토리가 위치한 경로로 이동하였다.

. <<<<< CMD Directory
├── install
├── build
├── log
├── src
│   └── ...

이후 아래 명령어를 입력해 패키지를 빌드한다.

colcon build

빌드가 정상적으로 완료되었다면 아래와 같이 패키지명 옆에 Finished 가 표시된다.

colcon build
Starting >>> aa_pub_sub
Finished <<< aa_pub_sub [4.62s]

Summary: 1 package finished [4.94s]

만약 Failed 가 뜬다면 log 디렉토리에 빌드 시간으로 생성된 폴더에 기록된 로그를 참고해 에러가 난 부분을 수정한 후, 다시 빌드하자.
만약 로그 파일을 열었을 때, 파일의 텍스트가 깨져서 보인다면, EUC-KR 인코딩을 이용해 파일을 다시 열면 정상적으로 텍스트를 확인할 수 있다.

Test nodes

이제 빌드까지 완료되었으니 노드를 실행시켜보자.
Publisher 와 Subscriber 두개의 노드를 켜야 하니, Developer Command Prompt 를 2개 열고, ROS2 환경 설정을 진행한다.

설정이 완료되었다면, 빌드할 때 이동한 프로젝트 경로로 이동한다.
이후 아래의 명령어를 실행해 빌드된 프로젝트에 대한 설정을 진행한다.

call .\install\setup.bat

두개의 터미널 모두 설정이 완료되었다면, 하나의 터미널에 먼저 아래의 명령어를 입력해 Publisher 노드를 실행시킨다.

ros2 run <package_name> publisher_node

그러면 매 1초마다 아래와 같이 메시지가 출력된다.

[INFO] [1706796614.215731100] [publisher_node]: Hello, ROS2 Humble #0
[INFO] [1706796615.206128700] [publisher_node]: Hello, ROS2 Humble #1
[INFO] [1706796616.213163200] [publisher_node]: Hello, ROS2 Humble #2
[INFO] [1706796617.211738700] [publisher_node]: Hello, ROS2 Humble #3
[INFO] [1706796618.211434400] [publisher_node]: Hello, ROS2 Humble #4
...

메시지가 이렇게 출력되도록 두고, 다른 터미널로 이동해 아래의 명령어를 입력해서 Subscriber 노드를 실행시킨다.

ros2 run <package_name> subscriber_node

정상적으로 실행이 완료되면 Publisher 노드를 실행한 터미널에서 출력되는 내용과 동일한 내용이 Subscriber 노드쪽의 터미널에서 표시된다.

[INFO] [1706796795.000498200] [subscription_node]: Hello, ROS2 Humble #2
[INFO] [1706796795.692108100] [subscription_node]: Hello, ROS2 Humble #3
[INFO] [1706796796.717635500] [subscription_node]: Hello, ROS2 Humble #4
[INFO] [1706796797.742216700] [subscription_node]: Hello, ROS2 Humble #5
...

만약 프로그램은 정상적으로 실행되는데 값이 정상적으로 표시되지 않는다면, Publisher Node 와 Subscriber Node 의 코드에서 Topic Name 이 서로 같은지 확인해보자.

프로그램을 중단할 때에는 Ctrl + C 키를 이용해 중단시킬 수 있다.

profile
Xeno
이전 포스트

ROS2 Windows 설치

다음 포스트

NURBS Curve

0개의 댓글

관련 채용 정보