ROS 프로그래밍 기초(C++)

두부김치·2024년 2월 12일
CRoboticsros2-foxy
1

ROS2

목록 보기
15/29

1. ROS의 Hello World, rclcpp 버전

이 장에서는 C++ 언어로 ROS2 의 가장 간단한 구조의 토픽(Topic) 퍼블리셔(Publisher)와 서브스크라이버(Subscriber)를 작성하고 동작시켜 보겠다. 앞에 Python 와 동일한 기능을 갖기때문에 언어에 따른 차이점을 보면 좋을거 같다.

2. 패키지 생성

ROS2 패키지 생성 명령어는 다음과 같다.

$ ros2 pkg create <패키지이름> --build-type <빌드 타입> --dependencies <의존하는패키지1> <의존하는패키지n>

ros2 pkg create 명령어를 사용하고 그 뒤에 옵션을 붙여 주게 된다. 사용자 작업 폴더는 파이썬때와 동일하다.

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

앞의 명령어에서 의존하는 패키지로 rclcpp와 std_msgs를 옵션으로 달아주었다. ROS의 ROS C++ 클라이언트 라이브러리(rclcpp)와 표준 메시지 패키지(std_msgs)를 사용하겠다는 것으로 패키지 생성에 앞서 미리 설치되어 있어야 한다. 이러한 의존성 패키지 설정은 앞의 예제처럼 패키지를 생성할 때 지정할 수 도 있지만, 생성한 다음 package.xml 에서 직접 입력해도 된다.

패키지를 생성하였다면 ~/robot_ws/src 에 my_first_ros_rclcpp_pkg 패키지 폴더와 ROS 패키지가 갖추어야 할 기본 폴더 그리고 package.xml 파일 등이 생성된다.

.
├── include
│   └── my_first_ros_rclcpp_pkg
├── src
├── CMakeLists.txt
└── package.xml

3 directories, 2 files

3. 패키지 설정

"Hello World" 예제를 위해 작성할 파일은 my_first_ros_rclcpp_pkg 패키지의 패키지 설정 파일(package.xml), 빌드 설정 파일(CMakeLists.txt) 이다.

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

패키지 설정 파일(package.xml)은 사용할 RCL(ROS2 client libraries)에 따라 달라지는데 C++ 라면 build_type으로 ament_cmake 을 설정하고 Python이라면 ament_python 으로 설정하면 된다.

<?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>my_first_ros_rclcpp_pkg</name>
  <version>0.0.1</version>
  <description>ROS 2 rclcpp basic package for the ROS 2 seminar</description>
  <maintainer email="choi@naver.com">choi</maintainer>
  <license>Apache License 2.0</license>
  <author>Mikael Arguedas</author>
  <author>Morgan Quigley</author>
  <author email="jacob@openrobotics.org">Jacob Perron</author>
  <author email="choi@naver.com">choi</author>

  <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>

3.2 빌드 설정 파일(CMakeLists.txt)

빌드 설정 파일(CMakeLists.txt)에서 주의할 점은 의존성 패키지의 설정과 빌드 및 설치 관련 설정이다. 이 패키지에서 빌드 시에 필요한 ament_cmake 패키지, 클라이언트 라이브러리 rclcpp 패키지, 메시지 인터페이스 std_msgs 패키지가 의존성 패키지로 사용됨을 명시하였다. 그리고 helloworld_publisher와 helloworld_subscriber 실행 파일의 빌드 및 설치를 위한 설정이 포함 되어 있다.

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

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(helloworld_publisher src/helloworld_publisher.cpp)
ament_target_dependencies(helloworld_publisher rclcpp std_msgs)

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

# Install
install(TARGETS
  helloworld_publisher
  helloworld_subscriber
  DESTINATION lib/${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()

4. 퍼블리셔 노드 작성

퍼블리셔 노드의 C++코드는 ~/robot_ws/src/my_first_ros_rclcpp_pkg/src/ 폴더에 helloworld_publisher.cpp 소스 파일을 작성해주자

#include <chrono>
#include <functional>
#include <memory>
#include <string>

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

using namespace std::chrono_literals;


class HelloworldPublisher : public rclcpp::Node
{
public:
  HelloworldPublisher()
  : Node("helloworld_publisher"), count_(0)
  {
    auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));
    helloworld_publisher_ = this->create_publisher<std_msgs::msg::String>(
      "helloworld", qos_profile);
    timer_ = this->create_wall_timer(
      1s, std::bind(&HelloworldPublisher::publish_helloworld_msg, this));
  }

private:
  void publish_helloworld_msg()
  {
    auto msg = std_msgs::msg::String();
    msg.data = "Hello World: " + std::to_string(count_++);
    RCLCPP_INFO(this->get_logger(), "Published message: '%s'", msg.data.c_str());
    helloworld_publisher_->publish(msg);
  }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr helloworld_publisher_;
  size_t count_;
};


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

첫 구절은 include 및 namespace 구문이다. 코드에서 사용되는 std계열의 헤더를 우선 선언하고 있으며, 이어서 rclcpp의 Node 클래스를 사용하기 위한 rclcpp.hpp 헤더 파일과 퍼블리시하는 메시지의 타입인 String 메시지 인터페이스를 사용하기 위해 string.hpp 헤더 파일을 포함시켰다. chrono_literals는 추후에 "500ms", "1s"와 같이 시간을 가식성이 높은 문자로 표현하기 위해 namespace를 사용할 수 있도록 선언하였다.

#include <chrono>
#include <functional>
#include <memory>
#include <string>

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

using namespace std::chrono_literals;

이 노드의 메인 클래스는 HelloworldPublisher 로 rclcpp 의 Node 클래스를 상속해 사용할 예정이다.

class HelloworldPublisher : public rclcpp::Node

다음은 클래스 생성자의 정의로 Node("helloworld_publisher"),count(0) 와 같이 부모 클래스(Node)의 생성자를 호출하고 노드 이름을 helloworldpublisher로 지정하였다. count변수는 0으로 초기화한다.

그 다음 퍼블리셔의 QoS 설정을 위하여 rclcpp::QoS(rclcpp::KeepLast(10))과 같이 기본 QoS에서 KeepLast 형태로 depth를 10으로 설정하여 통신 상태가 원할하지 못한 상황 등 예기치 못한 경우 퍼블리시할 데이터를 버퍼에 10개까지 저장하라는 설정이다.

그 다음으로는 Node 클래스의 create_publisher 함수를 이용하여 퍼블리셔를 설정하고 있다. 매개변수의 토픽 메시지 타입은 String, 토픽 이름은 helloworld, QoS는 qos_profile로 설정하였다.

마지막으로 Node 클래스의 create_wall_timer 함수를 이용해 콜백 함수를 실행시킨다. 첫번재 매개변수는 주기이며 1s 로 설정하면 1초마다 지정한 콜백 함수가 실행된다. 다음 코드는 1초마다 publish_helloworld_msg 함수를 실행시킨다.

public:
  HelloworldPublisher()
  : Node("helloworld_publisher"), count_(0)
  {
    auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));
    helloworld_publisher_ = this->create_publisher<std_msgs::msg::String>(
      "helloworld", qos_profile);
    timer_ = this->create_wall_timer(
      1s, std::bind(&HelloworldPublisher::publish_helloworld_msg, this));
  }

다음은 앞에서 지정한 publish_helloworld_msg 콜백 함수이다. 퍼블리시할 메시지는 String 타입으로 선언하였고, 실제 데이터는 msg.data 변수에 저장하게 된다. 여기서는 "Hello World:1"과 같이 매번 콜백 함수가 실행될 때마다 1씩 증가하는 count 변숫값을 문자열에 포함시켜 publish 함수를 통해 퍼블리시 하게 된다.
RCLCPP_INFO 함수는 콘솔창에 출력하는 함수로 로거의 종류에 따라 RCLCPP_DEBUG, RCLCPP_INFO, RCLCPP_WARN, RCLCPP_ERROR, RCLCPP_FATAL과 같이 5가지 종류가 있다. 일반적인 정보 전달에는 RCLCPP_INFO를 사용하고 있기에 RCLCPP_INFO 함수를 통해 현재 퍼블리시되는 메시지를 콘솔창에 출력시키는 구문을 마지막에 넣어주었다. RCLCPP_XXXX 계열의 함수는 프로그래밍에서 흔히 사용되는 print 함수라고 생각하면 이해하기 편할 것이다. 이하 클래스에서 private 변수로 사용되는 timer, helloworldpublisher, count_ 을 선언하였다.

참고로 콜백함수의 구현에는 member function, lambda, local function 방법이 있는데 이 예제 코드에서는 member function 방식을 택하였다. 시퀀스에 의해 처리되는 local function 방법은 잘 사용되지 않고 member function 또는 lambda 방식이 많이 사용된다. 자신이 좋아하는 구현 방법이 있다면 그 방법으로 작성하도록 하자.

private:
  void publish_helloworld_msg()
  {
    auto msg = std_msgs::msg::String();
    msg.data = "Hello World: " + std::to_string(count_++);
    RCLCPP_INFO(this->get_logger(), "Published message: '%s'", msg.data.c_str());
    helloworld_publisher_->publish(msg);
  }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr helloworld_publisher_;
  size_t count_;

마지막은 main 함수로 rclcpp::init 를 이용하여 초기화하고 작성한 HelloworldPublisher 클래스를 node 변수로 생성한 다음 rclcpp::spin 함수를 이용하여 생성한 노드를 spin시켜 지정된 콜백 함수가 실행될 수 있도록 하고 있다. 종료 Ctrl + c와 같은 인터럽트 시그널 예외 상황에서는 rclcpp::shutdown 함수로 노드를 소멸하고 프로세스를 종료하게 된다.

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

5. 서브스크라이버 노드 작성

서브스크라이버 노드의 C++ 코드도 퍼블리셔 노드와 마찬가지로 ~/robot_ws/src/my_first_ros_rclcpp_pkg/src/ 폴더에 helloworld_subscriber.cpp라는 이름으로 소스 코드 파일을 직접 생성하여 넣어주고 서브스크라이버 노드의 전체 코드는 다음과 같이 작성하면 된다. 코드 내용은 이어지는 설명에서 하나씩 알아보도록 하자.

#include <functional>
#include <memory>

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

using std::placeholders::_1;


class HelloworldSubscriber : public rclcpp::Node
{
public:
  HelloworldSubscriber()
  : Node("Helloworld_subscriber")
  {
    auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));
    helloworld_subscriber_ = this->create_subscription<std_msgs::msg::String>(
      "helloworld",
      qos_profile,
      std::bind(&HelloworldSubscriber::subscribe_topic_message, this, _1));
  }

private:
  void subscribe_topic_message(const std_msgs::msg::String::SharedPtr msg) const
  {
    RCLCPP_INFO(this->get_logger(), "Received message: '%s'", msg->data.c_str());
  }
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr helloworld_subscriber_;
};


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

첫 구절은 include 및 namespace 구문으로 퍼블리셔 노드와 비슷하다. placeholders zmffotmsms bind 함수의 대체자 역할을 위하여 _1로 선언하였다.

#include <functional>
#include <memory>

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

using std::placeholders::_1;

이 노드의 메인 클래스는 HelloworldSubscriber로 rclcpp 의 Ndoe 클래스를 상속해 사용할 예정이다.

class HelloworldSubscriber : public rclcpp::Node

다음은 클래스 생성자의 정의로 Node("Helloworld_subscriber")과 같이 Node 클래스의 생성자를 호출하고 노드 이름을 Helloworld_subscriber으로 지정하였다.

그 다음 퍼블리셔의 QoS 설정을 위하여 rclcpp::QoS(rclcpp::KeepLast(10))과 같이 기본 QoS에서 KeepLast 형태로 depth를 10으로 설정하여 통신 상태가 원할하지 못한 상황 등 예기치 못한 경우 서브스크라이브 데이터를 버퍼에 10개까지 저장하라는 설정이다.

그 다음으로는 Node 클래스의 createsubscription 함수를 이용하여 `helloworld_subscriber라는 서브스크라이버를 설정하고 있다. 매개변수로는 토픽에 사용할 토픽 메시지 타입과 토픽의 이름, QoS 설정, 수신받은 메시지를 처리할 콜백함수를 기입하도록 되어 있다. 여기서는 토픽 메시지 타입으로String, 토픽 이름으로 helloworld, QoS 설정으로 좀전에 설정한 qos_profile`, 콜백함수는 subscribe_topic_message으로 설정하였다.

public:
  HelloworldSubscriber()
  : Node("Helloworld_subscriber")
  {
    auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));
    helloworld_subscriber_ = this->create_subscription<std_msgs::msg::String>(
      "helloworld",
      qos_profile,
      std::bind(&HelloworldSubscriber::subscribe_topic_message, this, _1));
  }

다음은 위에서 지정한 콜백함수인 subscribetopic_message 함수이다. 서브스크라이브한 메시지는 String타입으로 msg이라는 이름을 사용하며 받은 메시지는 msg.data에 저장하게 되어 있다. 여기서는 Hello World: 1과 같은 메시지를 서브스크라이브하게 된다. 이 msg.data를 RCLCPP_INFO 함수를 이용하여 서브스크라이브된 메시지를 콘솔창에 출력시키는 구문을 마지막에 넣어주었다. 이하 클래스에서 private 변수로 사용되는 helloworld_subscriber을 선언하였다.

private:
  void subscribe_topic_message(const std_msgs::msg::String::SharedPtr msg) const
  {
    RCLCPP_INFO(this->get_logger(), "Received message: '%s'", msg->data.c_str());
  }
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr helloworld_subscriber_;

마지막으로 main 함수로 HelloworldSubscriber 를 node로 선언하여 사용한다는 것과 이외에는 퍼블리셔 노드와 동일하다.

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

퍼블리셔 노드와 서브스크라이버 노드작성을 완료하였다 빌드 부분은 파이썬 부분과 동일하기 때문에 생략하고 넘어가겠다

6. 실행

각 노드의 실행은 ros2 run 명령어를 통해 다음과 같이 실행하면 된다.

$ ros2 run my_first_ros_rclcpp_pkg helloworld_publisher
[INFO]: Published message: 'Hello World: 0'
[INFO]: Published message: 'Hello World: 1'
[INFO]: Published message: 'Hello World: 2'
[INFO]: Published message: 'Hello World: 3'
[INFO]: Published message: 'Hello World: 4'
[INFO]: Published message: 'Hello World: 5'
$ ros2 run my_first_ros_rclcpp_pkg helloworld_subscriber
[INFO]: Received message: 'Hello World: 0'
[INFO]: Received message: 'Hello World: 1'
[INFO]: Received message: 'Hello World: 2'
[INFO]: Received message: 'Hello World: 3'
[INFO]: Received message: 'Hello World: 4'
[INFO]: Received message: 'Hello World: 5'

지금까지 기초적인 방법으로 Python, C++ 프로그래밍을 이용해 프로그래밍 해보았다. 다음 장부터는 토픽, 서비스, 액션, 파라미터, 런치 등의 실습을 진행해 보겠다.

profile
두부김치
Robotics
이전 포스트

ROS 프로그래밍 기초(Python)

다음 포스트

토픽, 서비스, 액션 인터페이스 작성

0개의 댓글