👩‍💻ROS2 RealSense C++ Node

ros2
realsense
code
C++로 ROS2 RealSense 카메라 노드 만들기
Published

February 2, 2025

이번 포스팅에서는 Intel RealSense 카메라로 촬영한 비디오를 실시간 스트리밍하고, 영상에서 감지한 AprilTag을 OpenCV를 활용해 시각적으로 표시하는 ROS 2 기능을 C++ 노드로 구현하는 실습을 진행하겠습니다. AprilTag는 로봇 비전, 증강 현실, 마커 기반 위치 추정 등에 활용되는 태그 시스템입니다. 또한, 본 포스팅에서는 RealSense 카메라를 ROS 2에서 사용하기 위해 librealsense2를 설정하는 과정과 일반적으로 발생할 수 있는 빌드 문제 해결 방법도 함께 다룰 예정입니다.

이 간단한 ROS2 프로젝트를 시작하기 앞서, 아래의 사전 준비 사항들을 확인해서 먼저 환경을 셋팅해주시기 바랍니다.

  • ROS 2 (Galactic, Humble, Iron, etc. — 각자 Ubuntu 환경에 맞는 ROS2 version을 준비해주세요)

  • Intel RealSense SDK (librealsense2)

    • Ubuntu에서는 아래의 명령어로 설치할 수 있습니다:

      sudo apt-get install librealsense2-dev

  • OpenCV (e.g., sudo apt-get install libopencv-dev)

  • cv_bridge (from vision_opencv) - 일반적으로 ROS 2 패키지를 통해 설치됩니다:

    sudo apt-get install ros-${ROS_DISTRO}-vision-opencv

    ( ${ROS_DISTRO} 부분을 각자 ROS2 버젼에 맞는 이름으로 대체해주세요. e.g.) galactic or humble)

Overview

앞서 설명한 것처럼, 가장 단순한 ROS2 패키지 구조를 가지고 진행할 예정입니다. 2개의 C++ Node들, 즉 Publisher와 Subcriber를 만들어 Realsense Camera D457에서 이미지를 보내주면(Publisher) 받은 이미지를 토대로 Tag ID를 인식하여 OpenCV로 간단한 ploting을 한 창(Subsciber)을 띄워볼 예정입니다. 각 노드들의 역할을 정리해보면 아래와 같습니다.

  1. Publisher Node: RealSense 카메라에서 이미지를 수집하여 ROS 2 토픽으로 퍼블리싱하는 노드
    • librealsense2 패키지를 이용하여 RealSense camera를 연결
    • 640×480 해상도, 30 FPS 칼라 이미지 프레임을 스트리밍
    • ROS 2 토픽으로 (image_raw) 이미지 프레임 Publish
  2. Subscriber Node: RealSense 카메라에서 영상을 받아 OpenCV로 처리하고, AprilTag 라이브러리를 활용하여 태그를 감지하는 기능을 수행하는 노드
    • image_raw 토픽을 Subscribe
    • cv_bridge를 이용하여 OpenCV Mat으로 이미지를 변환
    • OpenCV window에 간단한 표시를한 이미지 띄우기

Creating the ROS 2 Package

my_realsense_example 라는 새로운 ROS 2 package를 만들어 시작해보겠습니다. ROS2의 workspace에 있는 src/폴더에서 C++ 패키지를 생성해줍니다.

cd ~/ros2_ws/src
ros2 pkg create my_realsense_example \
  --build-type ament_cmake \
  --dependencies rclcpp sensor_msgs cv_bridge

그다음, my_realsense_example/src/ 에 각 노드에 대한 C++ 스크립트 파일을 작성합니다. - realsense_publisher.cpp - image_subscriber.cpp

Publisher Node: realsense_publisher.cpp

우선, 카메라 센서에서 이미지를 받아 송출하는 Publisher 노드 코드를 살펴보겠습니다. 먼저, 아래와 같이 필요한 헤더와 패키지들을 포함합니다.

  • librealsense2/rs.hpp: RealSense 카메라를 제어
  • rclcpp/rclcpp.hpp: ROS 2 노드를 생성 및 관리
  • sensor_msgs/msg/image.hpp: ROS 2의 이미지 메시지를 처리
  • cv_bridge/cv_bridge.h: ROS 메시지와 OpenCV 이미지 간 변환
  • opencv2/opencv.hpp: OpenCV 기능 활용

이러한 요소들을 포함하여, RealSense 카메라에서 영상을 받아 ROS 2 메시지로 변환하고 퍼블리싱하는 노드를 구현할 수 있습니다.

#include <memory>
#include <chrono>
#include <librealsense2/rs.hpp>           // Intel RealSense SDK
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"     // ROS 2 Image message
#include "cv_bridge/cv_bridge.h"         // cv_bridge for ROS <-> OpenCV
#include "opencv2/opencv.hpp"            // OpenCV

본격적으로 RealSensePubliserClass를 살펴보면, RealSense 카메라에서 수집한 영상을 OpenCV 형식으로 변환한 후, 이를 ROS 2의 이미지 메시지로 변환하여 주기적으로 퍼블리싱하는 과정이 진행됩니다. 이 클래스 노드를 통해 송출된 이미지를 다른 ROS 2 노드가 Subscribe하고 활용할 수 있습니다.

크게 2 부분으로 나누어서 볼 수 있습니다.

  1. 노드 초기화 (RealSensePublisher)
    • rclcpp::Node를 상속받아 "realsense_publisher"라는 이름의 ROS 2 노드를 생성합
    • rs2::pipeline을 설정하여 RealSense 카메라에서 데이터를 스트리밍할 준비
    • rs2::config를 사용해 컬러 영상 스트리밍을 640x480 해상도, BGR8 포맷, 30 FPS로 설정
    • create_publisher<sensor_msgs::msg::Image>("image_raw", 10);를 통해 "image_raw" 토픽으로 영상을 퍼블리싱할 퍼블리셔를 생성
    • create_wall_timer를 사용하여 100ms(약 10Hz) 간격으로 프레임을 가져와 퍼블리싱하는 타이머를 설정
  2. 프레임 캡처 및 퍼블리싱 (publishFrame)
    • pipeline_->wait_for_frames();를 사용해 RealSense 카메라에서 새로운 프레임을 가져옴
    • frameset.get_color_frame();을 통해 컬러 프레임을 추출하고, 존재하지 않으면 경고 메시지를 출력
    • cv::Mat을 사용하여 RealSense의 컬러 프레임 데이터를 OpenCV 형식으로 변환
    • cv_bridge::CvImage를 활용해 OpenCV 이미지를 ROS 2의 sensor_msgs::msg::Image 형식으로 변환한 후 타임스탬프와 프레임 ID를 추가
    • publisher_->publish(*image_msg);를 통해 변환된 이미지를 "image_raw" 토픽으로 퍼블리싱
class RealSensePublisher : public rclcpp::Node
{
public:
  RealSensePublisher()
  : Node("realsense_publisher")
  {
    pipeline_ = std::make_shared<rs2::pipeline>();

    // Configure and start the pipeline to stream color images
    rs2::config cfg;
    cfg.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 30);
    pipeline_->start(cfg);

    publisher_ = this->create_publisher<sensor_msgs::msg::Image>("image_raw", 10);

    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(100),  // ~10 Hz
      std::bind(&RealSensePublisher::publishFrame, this)
    );

    RCLCPP_INFO(this->get_logger(), "RealSense Publisher Node started.");
  }

private:
  void publishFrame()
  {
    // Wait for the next set of frames
    rs2::frameset frameset = pipeline_->wait_for_frames();
    rs2::video_frame color_frame = frameset.get_color_frame();
    if (!color_frame)
    {
      RCLCPP_WARN(this->get_logger(), "No color frame received.");
      return;
    }

    // Convert RealSense frame to OpenCV Mat
    cv::Mat color_image(
      cv::Size(color_frame.get_width(), color_frame.get_height()),
      CV_8UC3,
      (void*)color_frame.get_data(),
      cv::Mat::AUTO_STEP
    );

    // Convert to ROS Image message via cv_bridge
    auto image_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", color_image).toImageMsg();
    image_msg->header.stamp = this->get_clock()->now();
    image_msg->header.frame_id = "realsense_color_frame";

    publisher_->publish(*image_msg);
  }

  std::shared_ptr<rs2::pipeline> pipeline_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
};

마지막으로 main에서는 ROS 2 노드 실행의 진입점으로, RealSensePublisher 노드를 생성하고 실행하는 역할을 합니다. 이를 통해 RealSense 카메라에서 받은 영상을 ROS 2의 "image_raw" 토픽으로 지속적으로 퍼블리싱하는 노드가 실행됩니다.

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv); // ROS 2 노드 실행을 위한 초기화

  rclcpp::spin(std::make_shared<RealSensePublisher>()); // `RealSensePublisher` 노드를 생성하고 실행
  /* 
  `spin()` 함수는 노드가 지속적으로 실행되도록 유지하며, 
  콜백 함수(`publishFrame()`)를 주기적으로 호출하여 RealSense 카메라에서 영상을 가져오고 퍼블리싱합니다. 
  */

  rclcpp::shutdown(); // 프로그램 종료 시 ROS 2를 안전하게 종료
  return 0;
}

각 부분을 살펴본 Publisher 노드를 통합하면 아래와 같이 realsense_publisher.cpp가 완성됩니다.

#include <memory>
#include <chrono>
#include <librealsense2/rs.hpp>           // Intel RealSense SDK
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"     // ROS 2 Image message
#include "cv_bridge/cv_bridge.h"         // cv_bridge for ROS <-> OpenCV
#include "opencv2/opencv.hpp"            // OpenCV

class RealSensePublisher : public rclcpp::Node
{
public:
  RealSensePublisher()
  : Node("realsense_publisher")
  {
    pipeline_ = std::make_shared<rs2::pipeline>();

    // Configure and start the pipeline to stream color images
    rs2::config cfg;
    cfg.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 30);
    pipeline_->start(cfg);

    publisher_ = this->create_publisher<sensor_msgs::msg::Image>("image_raw", 10);

    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(100),  // ~10 Hz
      std::bind(&RealSensePublisher::publishFrame, this)
    );

    RCLCPP_INFO(this->get_logger(), "RealSense Publisher Node started.");
  }

private:
  void publishFrame()
  {
    // Wait for the next set of frames
    rs2::frameset frameset = pipeline_->wait_for_frames();
    rs2::video_frame color_frame = frameset.get_color_frame();
    if (!color_frame)
    {
      RCLCPP_WARN(this->get_logger(), "No color frame received.");
      return;
    }

    // Convert RealSense frame to OpenCV Mat
    cv::Mat color_image(
      cv::Size(color_frame.get_width(), color_frame.get_height()),
      CV_8UC3,
      (void*)color_frame.get_data(),
      cv::Mat::AUTO_STEP
    );

    // Convert to ROS Image message via cv_bridge
    auto image_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", color_image).toImageMsg();
    image_msg->header.stamp = this->get_clock()->now();
    image_msg->header.frame_id = "realsense_color_frame";

    publisher_->publish(*image_msg);
  }

  std::shared_ptr<rs2::pipeline> pipeline_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
};

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

Subscriber Node: image_subscriber.cpp

다음으로 위에서 Publisher 노드가 보내준 이미지 데이터를 받는 Subscriber 노드에 대해 살펴보겠습니다.

우선 아래와 같이 필요한 헤더 파일들을 포함합니다.

  1. C++ 표준 라이브러리 포함 (<memory>, <string>, <iostream>)
    • 스마트 포인터(std::shared_ptr), 문자열(std::string), 표준 입출력(std::cout)을 사용하기 위한 헤더 파일
  2. ROS 2 관련 헤더 포함
    • rclcpp/rclcpp.hpp: ROS 2 노드 및 퍼블리셔/구독자 기능을 사용하기 위한 기본 라이브러리
    • sensor_msgs/msg/image.hpp: ROS 2에서 이미지 메시지(sensor_msgs::msg::Image) 를 다루기 위한 메시지 타입
    • cv_bridge/cv_bridge.h: ROS 이미지 메시지를 OpenCV의 cv::Mat으로 변환하거나, 반대로 OpenCV 이미지를 ROS 메시지로 변환하기 위해 사용
  3. OpenCV 관련 헤더 포함
    • opencv2/highgui.hpp: OpenCV의 GUI 기능 (cv::imshow(), cv::waitKey())을 사용하여 이미지를 표시할 때 필요
    • opencv2/imgproc.hpp: 이미지 처리 기능(예: 필터링, 변형, 색 변환 등)을 수행하기 위한 라이브러리
  4. AprilTag 감지를 위한 라이브러리 포함
    • extern "C" {} 블록을 사용하여 C 스타일로 작성된 AprilTag 라이브러리를 C++ 코드에서 사용할 수 있도록 합니다.
    • #include <apriltag.h>: AprilTag 감지 시스템의 핵심 헤더
    • #include <tag25h9.h>: AprilTag 25h9 태그 세트를 사용하기 위한 헤더 파일(25x9 해상도를 가진 태그 세트)
    • #include <tag36h11.h>는 주석 처리되어 있으며, 필요 시 AprilTag 36h11 세트(더 정밀한 태그)를 사용할 수 있도록 변경 가능
#include <memory>
#include <string>
#include <iostream>

// ROS 2
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"

// OpenCV
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"

// AprilTag library headers
extern "C" {
#include <apriltag.h>
// #include <tag36h11.h>
#include <tag25h9.h>
}

다음으로 ImageSubscriber Class를 보겠습니다. RealSense 카메라에서 수신한 영상을 처리하고, AprilTag을 검출하여 화면에 표시하는 ROS 2 노드를 구현합니다. OpenCV를 사용하여 영상을 변환하고, AprilTag 라이브러리를 이용해 태그를 감지 및 시각화하여 검출된 태그의 ID, 꼭짓점, 중심을 화면에 시각적으로 표시하는 과정이 구현되어 있습니다. 크게 세 부분으로 나누어서 보면,

1. 노드 초기화 (ImageSubscriber)

  • "image_raw" 토픽을 구독(subscription_)하여 카메라 영상을 수신
  • OpenCV 창 "RealSense Camera + AprilTag"를 생성하여 화면에 영상을 표시할 준비
  • AprilTag 검출기 초기화:
    • tag_family_ = tag25h9_create(); → AprilTag 25h9 태그 세트를 사용하도록 설정
    • tag_detector_ = apriltag_detector_create(); → AprilTag 검출기를 생성
    • apriltag_detector_add_family(tag_detector_, tag_family_); → 특정 태그 패밀리를 검출기에 추가
    • 검출기의 매개변수(예: quad_decimate, quad_sigma)를 조정하여 성능 최적화 가능

2. 이미지 수신 및 AprilTag 검출 (imageCallback): ROS 2의 "image_raw" 토픽에서 새로운 이미지가 수신될 때 호출

  1. ROS 2 이미지 메시지 → OpenCV cv::Mat 변환
    • cv_bridge::toCvCopy(msg, "bgr8")->image;를 사용하여 ROS 2의 sensor_msgs::msg::Image를 OpenCV 형식(cv::Mat)으로 변환합니다
    • 변환 중 오류 발생 시 예외를 처리
  2. 그레이스케일 변환 (AprilTag 검출은 흑백 영상이 필요)
    • cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
  3. OpenCV cv::Mat → AprilTag 전용 이미지 형식 (image_u8_t) 변환
    • AprilTag 라이브러리는 image_u8_t 구조체를 사용하므로 변환이 필요
    • gray.data를 buf로 전달하여 메모리 복사를 최소화
  4. AprilTag 검출 실행
    • apriltag_detector_detect(tag_detector_, &apriltag_image);을 호출하여 AprilTag을 감지
    • 검출된 태그는 zarray_t* detections에 저장
  5. 검출된 AprilTag에 대한 시각적 표시
    • 각 태그의 네 개의 꼭짓점 좌표(p[]) 를 가져와 초록색(cv::Scalar(0, 255, 0))으로 사각형을 그림
    • 태그의 중앙 좌표(c[]) 에 빨간색 원을 표시(cv::circle())
    • 태그의 ID를 노란색(cv::Scalar(0, 255, 255)) 텍스트로 화면에 출력
  6. 메모리 정리
    • apriltag_detections_destroy(detections);를 호출하여 검출된 태그 리스트를 정리하여 메모리 누수를 방지
  7. 결과 이미지 표시
    • cv::imshow("RealSense Camera + AprilTag", frame);을 통해 검출 결과를 화면에 표시
    • cv::waitKey(1);를 호출하여 OpenCV 창이 반응하도록 유지

3. 노드 종료 시 AprilTag 리소스 정리 (~ImageSubscriber())

  • apriltag_detector_remove_family(tag_detector_, tag_family_); → 검출기에서 패밀리를 제거
  • apriltag_detector_destroy(tag_detector_); → 검출기 삭제
  • tag25h9_destroy(tag_family_); → 태그 패밀리 메모리 해제
class ImageSubscriber : public rclcpp::Node
{
public:
  ImageSubscriber()
  : Node("image_subscriber")
  {
    subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
      "image_raw",
      10,
      std::bind(&ImageSubscriber::imageCallback, this, std::placeholders::_1)
    );

    // Create an OpenCV window once
    cv::namedWindow("RealSense Camera", cv::WINDOW_AUTOSIZE);

    RCLCPP_INFO(this->get_logger(), "Image Subscriber Node started.");
  }

private:
  void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg)
  {
    cv::Mat frame;
    try
    {
      frame = cv_bridge::toCvCopy(msg, "bgr8")->image;
    }
    catch (cv_bridge::Exception &e)
    {
      RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
      return;
    }

    if (!frame.empty())
    {
      cv::imshow("RealSense Camera", frame);
      cv::waitKey(1);
    }
  }

  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
};

마지막으로 main을 살펴보면 ROS 2 이미지 구독 노드(ImageSubscriber)를 실행하는 메인 함수로, ROS 2 이벤트 루프를 관리하며 이미지 구독 노드를 실행하면서 30Hz의 주기로 ROS 이벤트를 처리합니다.

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv); // ROS 2 노드를 실행할 준비
  auto node = std::make_shared<ImageSubscriber>(); // 노드 생성: `ImageSubscriber` 노드를 생성하여 RealSense 카메라에서 퍼블리싱되는 `"image_raw"` 토픽을 구독할 준비

  rclcpp::Rate rate(30); // 초당 30회(30Hz) 실행
  while (rclcpp::ok()) // 주기적 실행 루프
  {
    rclcpp::spin_some(node); // 노드가 메시지를 수신하고, 콜백 함수(`imageCallback`)를 실행할 수 있도록 합니다. 
    rate.sleep(); // 루프가 30Hz의 일정한 주기로 실행되도록 대기
  }

  rclcpp::shutdown(); // 프로그램 종료 시 ROS 2를 안전하게 종료
  return 0;
}

각 부분을 살펴본 Subsciber 노드를 통합하면 아래와 같이 image_subscriber.cpp가 완성됩니다.

#include <memory>
#include <string>
#include <iostream>

// ROS 2
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"

// OpenCV
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"

// AprilTag library headers
extern "C" {
#include <apriltag.h>
// #include <tag36h11.h>
#include <tag25h9.h>
}

class ImageSubscriber : public rclcpp::Node
{
public:
  ImageSubscriber()
  : Node("image_subscriber")
  {
    subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
      "image_raw",
      10,
      std::bind(&ImageSubscriber::imageCallback, this, std::placeholders::_1)
    );

    // Create an OpenCV window once
    cv::namedWindow("RealSense Camera + AprilTag", cv::WINDOW_AUTOSIZE);

    RCLCPP_INFO(this->get_logger(), "Image Subscriber Node started.");

    // Initialize the AprilTag detector
    // tag_family_ = tag36h11_create();  // Create specific tag family (36h11)
    tag_family_ = tag25h9_create(); // Create specific tag family (36h11)
    tag_detector_ = apriltag_detector_create();
    apriltag_detector_add_family(tag_detector_, tag_family_);

    // Optionally adjust detection parameters
    // e.g., tag_detector_->quad_decimate = 1.0; // For performance
    //       tag_detector_->quad_sigma = 0.0;
    // etc.
  }

  ~ImageSubscriber()
  {
    // Cleanup AprilTag resources
    apriltag_detector_remove_family(tag_detector_, tag_family_);
    apriltag_detector_destroy(tag_detector_);
    // tag36h11_destroy(tag_family_);
    tag25h9_destroy(tag_family_);
  }

private:
  void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg)
  {
    // 1. Convert ROS Image to OpenCV Mat
    cv::Mat frame;
    try
    {
      frame = cv_bridge::toCvCopy(msg, "bgr8")->image;
    }
    catch (cv_bridge::Exception &e)
    {
      RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
      return;
    }

    // 2. Convert color to grayscale (AprilTag needs grayscale)
    cv::Mat gray;
    cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);

    // 3. Convert OpenCV Mat to the format required by AprilTag library
    //    (AprilTag library uses its own image type: image_u8_t)
    image_u8_t apriltag_image = {
      .width  = gray.cols,
      .height = gray.rows,
      .stride = static_cast<int>(gray.step),
      .buf    = gray.data
    };

    // 4. Detect tags
    zarray_t* detections = apriltag_detector_detect(tag_detector_, &apriltag_image);

    // 5. For each detection, draw a bounding box on the image
    for (int i = 0; i < zarray_size(detections); i++)
    {
      apriltag_detection_t *det;
      zarray_get(detections, i, &det);

      // Access the four corners of the AprilTag
      // p[] is in image coordinates (x=horizontal, y=vertical)
      cv::Point corners[4];
      for (int c = 0; c < 4; c++)
      {
        corners[c] = cv::Point(det->p[c][0], det->p[c][1]);
      }

      // Draw lines between corners
      for (int c = 0; c < 4; c++)
      {
        cv::line(frame, corners[c], corners[(c+1)%4], cv::Scalar(0, 255, 0), 2);
      }

      // Draw the tag center
      cv::circle(frame,
                 cv::Point(det->c[0], det->c[1]),
                 5, cv::Scalar(0, 0, 255), -1);

      // Put tag ID text near the center
      std::string text = "ID: " + std::to_string(det->id);
      cv::putText(frame, text,
                  cv::Point(det->c[0] + 10, det->c[1] + 10),
                  cv::FONT_HERSHEY_SIMPLEX,
                  0.5,
                  cv::Scalar(0, 255, 255),
                  2);
    }

    // Always remember to destroy detection array to avoid memory leaks
    apriltag_detections_destroy(detections);

    // 6. Show the image
    cv::imshow("RealSense Camera + AprilTag", frame);
    cv::waitKey(1);
  }

  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;

  // AprilTag fields
  apriltag_family_t *tag_family_;
  apriltag_detector_t *tag_detector_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);

  auto node = std::make_shared<ImageSubscriber>();

  // Spin in a loop so that callbacks can be processed
  rclcpp::Rate rate(30);
  while (rclcpp::ok())
  {
    rclcpp::spin_some(node);
    rate.sleep();
  }

  rclcpp::shutdown();
  return 0;
}

CMakeLists.txt Configuration

그 다음으로 ROS2 패키지 빌드에 대한 내용을 담고 있는 CMakeLists.txt 파일을 작성해보겠습니다. CMakeLists.txt ( my_realsense_example 루트에 위치)에서 RealSense 카메라에서 이미지를 퍼블리싱하고, AprilTag을 검출하는 ROS 2 노드들을 빌드 및 설치하는 설정을 정의합니다. default로 생성되어있던 내용을 아래 내용으로 업데이트 합니다.

cmake_minimum_required(VERSION 3.8)
project(realsense_apriltag_pubsub)

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(OpenCV REQUIRED)
find_package(realsense2 REQUIRED)
find_package(apriltag REQUIRED)  # <-- Add this!

include_directories(
  include
  ${OpenCV_INCLUDE_DIRS}
)

# Publisher
add_executable(realsense_publisher src/realsense_publisher.cpp)
ament_target_dependencies(realsense_publisher
  rclcpp
  sensor_msgs
  cv_bridge
)
target_link_libraries(realsense_publisher
  ${OpenCV_LIBS}
  realsense2
)

# Subscriber with AprilTag
add_executable(image_subscriber src/image_subscriber.cpp)
ament_target_dependencies(image_subscriber
  rclcpp
  sensor_msgs
  cv_bridge
)
target_link_libraries(image_subscriber
  ${OpenCV_LIBS}
  realsense2
  apriltag          # <-- Link AprilTag
)

install(TARGETS
  realsense_publisher
  image_subscriber
  DESTINATION lib/${PROJECT_NAME}
)

# Install the launch directory
install(DIRECTORY launch/
  DESTINATION share/${PROJECT_NAME}/launch
)

ament_package()

이 CMakeLists.txt는 RealSense 카메라에서 이미지를 퍼블리싱하는 노드와, 해당 영상을 구독하여 AprilTag을 검출하는 노드를 빌드할 수 있도록 설정합니다. 특히, apriltag 라이브러리를 추가하여 AprilTag 검출 기능이 포함된 구독자(image_subscriber) 노드를 지원하는 것이 특징입니다.

1. 필수 패키지 찾기 (find_package)

  • ament_cmake, rclcpp, sensor_msgs, cv_bridge, OpenCV, realsense2 등의 패키지를 포함하여 ROS 2, OpenCV, RealSense SDK를 사용할 수 있도록 설정
  • AprilTag 추가 (find_package(apriltag REQUIRED))
    • AprilTag 검출을 위해 apriltag 라이브러리를 필수적으로 포함

2. 헤더 포함 경로 설정 (include_directories)

  • include/ 폴더와 OpenCV의 헤더 파일이 포함된 디렉토리를 추가

3. 퍼블리셔 실행 파일 생성 (realsense_publisher)

  • src/realsense_publisher.cpp 파일을 빌드하여 실행 가능한 퍼블리셔 노드 생성
  • ament_target_dependencies(realsense_publisher ...)를 통해 ROS 2 라이브러리(rclcpp, sensor_msgs, cv_bridge)와의 의존성을 설정
  • target_link_libraries(realsense_publisher ${OpenCV_LIBS} realsense2)을 사용해 OpenCV 및 RealSense SDK를 링크

4. 구독자(AprilTag 포함) 실행 파일 생성 (image_subscriber)

  • src/image_subscriber.cpp 파일을 빌드하여 실행 가능한 구독자 노드 생성
  • ament_target_dependencies(image_subscriber ...)를 통해 ROS 2 라이브러리 의존성 추가
  • target_link_libraries(image_subscriber ${OpenCV_LIBS} realsense2 apriltag)을 통해 OpenCV, RealSense SDK, AprilTag 라이브러리를 링크하여 AprilTag 검출 기능을 활성화

5. 실행 파일 설치 (install(TARGETS ...))

  • 빌드된 실행 파일(realsense_publisher, image_subscriber)을 ROS 2 패키지의 실행 가능 경로에 배포

6. Launch 파일 설치 (install(DIRECTORY launch/ ...))

  • launch/ 디렉토리를 ROS 2 패키지의 share/ 디렉토리에 복사하여 실행 시 사용할 수 있도록 설정
  • 이를 통해 ros2 launch realsense_apriltag_pubsub some_launch_file.py 형태로 실행 가능

7. 패키지 선언 (ament_package())

  • ament_package()를 호출하여 ROS 2 빌드 시스템(ament_cmake)과 호환되도록 패키지를 설정

⚠️발생할 수 있는 Build 에러 이슈들

  • Undefined reference to rs2_…: realsense2 library를 링크하지 않았을 때 발생하는 에러입니다. find_package(realsense2 REQUIRED)와 target_link_libraries(... realsense2)가 CMakeLists.txt에 포함되어 있는지 확인하세요.
  • Cannot find image_subscriber.cpp: CMakeLists.txt에서 지정한 파일 이름과 경로가 src/ 폴더 내의 실제 파일과 정확히 일치하는지 확인하세요.
  • cv_bridge or OpenCV not found: package.xml에 작성되어 있는 디펜던시들을 확인하고, 프로그래밍하는 환경 시스템에 cv_bridge 와 OpenCV가 설치되어 있는지 확인하세요.

Build & 실행

이제 패키지를 빌드할 모든 준비는 끝났습니다. 이제 소스 코드들을 작성한 workspace 경로(e.g., ~/ros2_ws)로 이동하여 빌드해줍니다.

colcon build --packages-select my_realsense_example
source install/setup.bash

잘 빌드가 되었다면 터미널을 2개 띄워서 각각 publisher 와 subscriber 노드를 실행시켜 줍니다.

  • Terminal 1:

    ros2 run my_realsense_example realsense_publisher

  • Terminal 2:

    ros2 run my_realsense_example image_subscriber

그러면 “RealSense Camera + AprilTag” 창이 나오면서 인식된 AprilTag ID와 박스가 표시된 창이 아래와 같이 나올 것 입니다!

최종 결과물

Conclusion

이제 RealSense 카메라에서 컬러 프레임을 퍼블리싱하고, 이를 OpenCV 창에 표시하는 최소한의 ROS 2 시스템을 구축하였습니다. 단순히 영상을 수신하는 것뿐만 아니라, AprilTag 인식을 활용하여 간단한 이미지 후처리도 수행해 보았습니다. 이를 확장하면 로보틱스에서 필요한 위치 추정과 같은 고급 기능도 구현할 수 있으며, 깊이 프레임, 포인트 클라우드 등 RealSense에서 지원하는 다양한 스트림을 활용할 수도 있습니다. 이번 실습을 통해 ROS 2에서 C++ 기반 패키지를 빌드하고 활용하는 과정에 대한 이해를 돕고, 더 복잡한 기능을 구현하는 프로젝트로 나아가는 데 도움이 되기를 바랍니다.