< >
Home » ROS2与C++入门教程 » ROS2与C++入门教程-进程内(intra_process)图像处理演示

ROS2与C++入门教程-进程内(intra_process)图像处理演示

ROS2与C++入门教程-进程内(intra_process)图像处理演示

说明:

  • 介绍ros2下实现进程内(intra_process)话题发布和订阅
  • 在同一进程内的不同节点,可以通过共享指针方式实现内容读取,减少消息的拷贝开销
  • 对于图像之类数据量比较大的节点间处理的效率和性能将大大提高
  • 本演示实现图像处理，启动相机，订阅图像消息并处理

步骤:

  • 新建一个包cpp_intra_process_image
cd ~/dev_ws/src
ros2 pkg create --build-type ament_cmake cpp_intra_process_image
  • 进入include目录，新建文件common.hpp
cd ~/dev_ws/src/cpp_intra_process_image/include/cpp_intra_process_image
touch common.hpp
  • 内容如下：　
#ifndef IMAGE_PIPELINE__COMMON_HPP_
#define IMAGE_PIPELINE__COMMON_HPP_

#ifdef _WIN32
#include <process.h>
#define GETPID _getpid
#else
#include <unistd.h>
#define GETPID getpid
#endif

#include <chrono>
#include <string>

#include "opencv2/opencv.hpp"
#include "builtin_interfaces/msg/time.hpp"

int
encoding2mat_type(const std::string & encoding)
{
  if (encoding == "mono8") {
    return CV_8UC1;
  } else if (encoding == "bgr8") {
    return CV_8UC3;
  } else if (encoding == "mono16") {
    return CV_16SC1;
  } else if (encoding == "rgba8") {
    return CV_8UC4;
  }
  throw std::runtime_error("Unsupported mat type");
}

std::string
mat_type2encoding(int mat_type)
{
  switch (mat_type) {
    case CV_8UC1:
      return "mono8";
    case CV_8UC3:
      return "bgr8";
    case CV_16SC1:
      return "mono16";
    case CV_8UC4:
      return "rgba8";
    default:
      throw std::runtime_error("Unsupported encoding type");
  }
}

void set_now(builtin_interfaces::msg::Time & time)
{
  std::chrono::nanoseconds now = std::chrono::high_resolution_clock::now().time_since_epoch();
  if (now <= std::chrono::nanoseconds(0)) {
    time.sec = time.nanosec = 0;
  } else {
    time.sec = static_cast<builtin_interfaces::msg::Time::_sec_type>(now.count() / 1000000000);
    time.nanosec = now.count() % 1000000000;
  }
}

void
draw_on_image(cv::Mat & image, const std::string & text, int height)
{
  cv::Mat c_mat = image;
  cv::putText(
    c_mat,
    text.c_str(),
    cv::Point(10, height),
    cv::FONT_HERSHEY_SIMPLEX,
    0.3,
    cv::Scalar(0, 255, 0));
}

#endif  // IMAGE_PIPELINE__COMMON_HPP_
  • 进入include目录，新建文件camera_node.hpp
cd ~/dev_ws/src/cpp_intra_process_image/include/cpp_intra_process_image
touch camera_node.hpp
  • 内容如下：
#ifndef IMAGE_PIPELINE__CAMERA_NODE_HPP_
#define IMAGE_PIPELINE__CAMERA_NODE_HPP_

#include <chrono>
#include <sstream>
#include <string>
#include <thread>
#include <utility>

#include "opencv2/highgui/highgui.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"

#include "cpp_intra_process_image/common.hpp"

/// Node which captures images from a camera using OpenCV and publishes them.
/// Images are annotated with this process's id as well as the message's ptr.
class CameraNode final : public rclcpp::Node
{
public:
  /// \brief Construct a new CameraNode object for capturing video
  /// \param output The output topic name to use
  /// \param node_name The node name to use
  /// \param watermark Whether to add a watermark to the image before publishing
  /// \param device Which camera device to use
  /// \param width What video width to capture at
  /// \param height What video height to capture at
  explicit CameraNode(
    const std::string & output, const std::string & node_name = "camera_node",
    bool watermark = true, int device = 0, int width = 800, int height = 600)
  : Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)),
    canceled_(false), watermark_(watermark)
  {
    // Initialize OpenCV
    cap_.open(device);
    // TODO(jacobperron): Remove pre-compiler check when we drop support for Xenial
#if CV_MAJOR_VERSION < 3
    cap_.set(CV_CAP_PROP_FRAME_WIDTH, static_cast<double>(width));
    cap_.set(CV_CAP_PROP_FRAME_HEIGHT, static_cast<double>(height));
#else
    cap_.set(cv::CAP_PROP_FRAME_WIDTH, static_cast<double>(width));
    cap_.set(cv::CAP_PROP_FRAME_HEIGHT, static_cast<double>(height));
#endif
    if (!cap_.isOpened()) {
      throw std::runtime_error("Could not open video stream!");
    }
    // Create a publisher on the output topic.
    pub_ = this->create_publisher<sensor_msgs::msg::Image>(output, rclcpp::SensorDataQoS());
    // Create the camera reading loop.
    thread_ = std::thread(std::bind(&CameraNode::loop, this));
  }

  ~CameraNode()
  {
    // Make sure to join the thread on shutdown.
    canceled_.store(true);
    if (thread_.joinable()) {
      thread_.join();
    }
  }

  /// \brief Capture and publish data until the program is closed
  void loop()
  {
    // While running...
    while (rclcpp::ok() && !canceled_.load()) {
      // Capture a frame from OpenCV.
      cap_ >> frame_;
      if (frame_.empty()) {
        continue;
      }
      // Create a new unique_ptr to an Image message for storage.
      sensor_msgs::msg::Image::UniquePtr msg(new sensor_msgs::msg::Image());

      if (watermark_) {
        std::stringstream ss;
        // Put this process's id and the msg's pointer address on the image.
        ss << "pid: " << GETPID() << ", ptr: " << msg.get();
        draw_on_image(frame_, ss.str(), 20);
      }
      // Pack the OpenCV image into the ROS image.
      set_now(msg->header.stamp);
      msg->header.frame_id = "camera_frame";
      msg->height = frame_.rows;
      msg->width = frame_.cols;
      msg->encoding = mat_type2encoding(frame_.type());
      msg->is_bigendian = false;
      msg->step = static_cast<sensor_msgs::msg::Image::_step_type>(frame_.step);
      msg->data.assign(frame_.datastart, frame_.dataend);
      pub_->publish(std::move(msg));  // Publish.
    }
  }

private:
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_;
  std::thread thread_;
  std::atomic<bool> canceled_;

  /// whether or not to add a watermark to the image showing process id and
  /// pointer location
  bool watermark_;

  cv::VideoCapture cap_;
  cv::Mat frame_;
};

#endif  // IMAGE_PIPELINE__CAMERA_NODE_HPP_
  • 进入include目录，新建文件image_view_node.hpp
cd ~/dev_ws/src/cpp_intra_process_image/include/cpp_intra_process_image
touch image_view_node.hpp
  • 内容如下:
#ifndef IMAGE_PIPELINE__IMAGE_VIEW_NODE_HPP_
#define IMAGE_PIPELINE__IMAGE_VIEW_NODE_HPP_

#include <sstream>
#include <string>

#include "opencv2/highgui/highgui.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"

#include "cpp_intra_process_image/common.hpp"

/// Node which receives sensor_msgs/Image messages and renders them using OpenCV.
class ImageViewNode final : public rclcpp::Node
{
public:
  /// \brief Construct a new ImageViewNode for visualizing image data
  /// \param input The topic name to subscribe to
  /// \param node_name The node name to use
  /// \param watermark Whether to add a watermark to the image before displaying
  explicit ImageViewNode(
    const std::string & input, const std::string & node_name = "image_view_node",
    bool watermark = true)
  : Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true))
  {
    // Create a subscription on the input topic.
    sub_ = this->create_subscription<sensor_msgs::msg::Image>(
      input,
      rclcpp::SensorDataQoS(),
      [node_name, watermark](sensor_msgs::msg::Image::ConstSharedPtr msg) {
        // Create a cv::Mat from the image message (without copying).
        cv::Mat cv_mat(
          msg->height, msg->width,
          encoding2mat_type(msg->encoding),
          const_cast<unsigned char *>(msg->data.data()));
        if (watermark) {
          // Annotate with the pid and pointer address.
          std::stringstream ss;
          ss << "pid: " << GETPID() << ", ptr: " << msg.get();
          draw_on_image(cv_mat, ss.str(), 60);
        }
        // Show the image.
        cv::Mat c_mat = cv_mat;
        cv::imshow(node_name.c_str(), c_mat);
        char key = cv::waitKey(1);    // Look for key presses.
        if (key == 27 /* ESC */ || key == 'q') {
          rclcpp::shutdown();
        }
        if (key == ' ') {    // If <space> then pause until another <space>.
          key = '\0';
          while (key != ' ') {
            key = cv::waitKey(1);
            if (key == 27 /* ESC */ || key == 'q') {
              rclcpp::shutdown();
            }
            if (!rclcpp::ok()) {
              break;
            }
          }
        }
      });
  }

private:
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;

  cv::VideoCapture cap_;
  cv::Mat frame_;
};

#endif  // IMAGE_PIPELINE__IMAGE_VIEW_NODE_HPP_
  • 进入include目录，新建文件watermark_node.hpp
cd ~/dev_ws/src/cpp_intra_process_image/include/cpp_intra_process_image
touch watermark_node.hpp
  • 内容如下:
#ifndef IMAGE_PIPELINE__WATERMARK_NODE_HPP_
#define IMAGE_PIPELINE__WATERMARK_NODE_HPP_

#include <memory>
#include <sstream>
#include <string>
#include <utility>

#include "opencv2/opencv.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"

#include "cpp_intra_process_image/common.hpp"

/// Node that receives an image, adds some text as a watermark, and publishes it again.
class WatermarkNode final : public rclcpp::Node
{
public:
  /// \brief Construct a WatermarkNode that accepts an image, adds a watermark, and republishes it
  /// \param input The name of the topic to subscribe to
  /// \param output The topic to publish watermarked images to
  /// \param text The text to add to the image
  /// \param node_name The node name to use
  explicit WatermarkNode(
    const std::string & input, const std::string & output, const std::string & text,
    const std::string & node_name = "watermark_node")
  : Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true))
  {
    rclcpp::SensorDataQoS qos;
    // Create a publisher on the input topic.
    pub_ = this->create_publisher<sensor_msgs::msg::Image>(output, qos);
    std::weak_ptr<std::remove_pointer<decltype(pub_.get())>::type> captured_pub = pub_;
    // Create a subscription on the output topic.
    sub_ = this->create_subscription<sensor_msgs::msg::Image>(
      input,
      qos,
      [captured_pub, text](sensor_msgs::msg::Image::UniquePtr msg) {
        auto pub_ptr = captured_pub.lock();
        if (!pub_ptr) {
          return;
        }
        // Create a cv::Mat from the image message (without copying).
        cv::Mat cv_mat(
          msg->height, msg->width,
          encoding2mat_type(msg->encoding),
          msg->data.data());
        // Annotate the image with the pid, pointer address, and the watermark text.
        std::stringstream ss;
        ss << "pid: " << GETPID() << ", ptr: " << msg.get() << " " << text;
        draw_on_image(cv_mat, ss.str(), 40);
        pub_ptr->publish(std::move(msg));  // Publish it along.
      });
  }

private:
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_;
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;

  cv::VideoCapture cap_;
  cv::Mat frame_;
};

#endif  // IMAGE_PIPELINE__WATERMARK_NODE_HPP_
  • 进入src目录，新建文件camera_node.cpp
cd ~/dev_ws/src/cpp_intra_process_image/src
touch camera_node.cpp
  • 内容如下:
#include <memory>

#include "cpp_intra_process_image/camera_node.hpp"

#include "rclcpp/rclcpp.hpp"

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

  std::shared_ptr<CameraNode> camera_node = nullptr;
  try {
    camera_node = std::make_shared<CameraNode>("image");
  } catch (const std::exception & e) {
    fprintf(stderr, "%s Exiting..\n", e.what());
    return 1;
  }

  rclcpp::spin(camera_node);

  rclcpp::shutdown();

  return 0;
}
  • 进入src目录，新建文件image_view_node.cpp
cd ~/dev_ws/src/cpp_intra_process_image/src
touch image_view_node.cpp
  • 内容如下：
#include <memory>

#include "cpp_intra_process_image/image_view_node.hpp"

#include "rclcpp/rclcpp.hpp"

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto image_view_node = std::make_shared<ImageViewNode>("watermarked_image");
  rclcpp::spin(image_view_node);

  rclcpp::shutdown();

  return 0;
}
  • 进入src目录，新建文件watermark_node.cpp
cd ~/dev_ws/src/cpp_intra_process_image/src
touch watermark_node.cpp
  • 内容如下:
#include <memory>

#include "cpp_intra_process_image/watermark_node.hpp"

#include "rclcpp/rclcpp.hpp"

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto watermark_node =
    std::make_shared<WatermarkNode>("image", "watermarked_image", "Hello world!");
  rclcpp::spin(watermark_node);

  rclcpp::shutdown();

  return 0;
}
  • 进入src目录，新建文件two_image_view.cpp
cd ~/dev_ws/src/cpp_intra_process_image/src
touch two_image_view.cpp
  • 内容如下：　
#include <memory>

#include "rclcpp/rclcpp.hpp"

#include "cpp_intra_process_image/camera_node.hpp"
#include "cpp_intra_process_image/image_view_node.hpp"
#include "cpp_intra_process_image/watermark_node.hpp"

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

  // Connect the nodes as a pipeline: camera_node -> watermark_node -> image_view_node
  // And the extra image view as a fork:                           \-> image_view_node2
  std::shared_ptr<CameraNode> camera_node = nullptr;
  try {
    camera_node = std::make_shared<CameraNode>("image");
  } catch (const std::exception & e) {
    fprintf(stderr, "%s Exiting..\n", e.what());
    return 1;
  }
  auto watermark_node =
    std::make_shared<WatermarkNode>("image", "watermarked_image", "Hello world!");
  auto image_view_node = std::make_shared<ImageViewNode>("watermarked_image");
  auto image_view_node2 = std::make_shared<ImageViewNode>("watermarked_image", "image_view_node2");

  executor.add_node(camera_node);
  executor.add_node(watermark_node);
  executor.add_node(image_view_node);
  executor.add_node(image_view_node2);

  executor.spin();

  rclcpp::shutdown();

  return 0;
}
  • 进入src目录，新建文件all_in_one.cpp
cd ~/dev_ws/src/cpp_intra_process_image/src
touch all_in_one.cpp
  • 内容如下：　
#include <memory>

#include "rclcpp/rclcpp.hpp"

#include "cpp_intra_process_image/camera_node.hpp"
#include "cpp_intra_process_image/image_view_node.hpp"
#include "cpp_intra_process_image/watermark_node.hpp"

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

  // Connect the nodes as a pipeline: camera_node -> watermark_node -> image_view_node
  std::shared_ptr<CameraNode> camera_node = nullptr;
  try {
    camera_node = std::make_shared<CameraNode>("image");
  } catch (const std::exception & e) {
    fprintf(stderr, "%s Exiting ..\n", e.what());
    return 1;
  }
  auto watermark_node =
    std::make_shared<WatermarkNode>("image", "watermarked_image", "Hello world!");
  auto image_view_node = std::make_shared<ImageViewNode>("watermarked_image");

  executor.add_node(camera_node);
  executor.add_node(watermark_node);
  executor.add_node(image_view_node);

  executor.spin();

  rclcpp::shutdown();

  return 0;
}
  • 编译package.xml
  • 在<buildtool_depend>ament_cmake</buildtool_depend>后增加
<depend>libopencv-dev</depend>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
  • 编译CMakelist.txt
  • 在find_package(ament_cmake REQUIRED)后增加
find_package(builtin_interfaces REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rmw REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(OpenCV REQUIRED COMPONENTS core highgui imgproc videoio)
  • 生成执行文件
include_directories(include)

# A single program with one of each of the image pipeline demo nodes.
add_executable(all_in_one
  src/all_in_one.cpp)
target_link_libraries(all_in_one
  rclcpp::rclcpp
  ${builtin_interfaces_TARGETS}
  ${sensor_msgs_TARGETS}
  opencv_core
  opencv_highgui)

# A single program with one of each of the image pipeline demo nodes, but two image views.
add_executable(two_image_view
  src/two_image_view.cpp)
target_link_libraries(two_image_view
  rclcpp::rclcpp
  ${builtin_interfaces_TARGETS}
  ${sensor_msgs_TARGETS}
  opencv_core
  opencv_highgui)

# A stand alone node which produces images from a camera using OpenCV.
add_executable(camera_node
  src/camera_node.cpp)
target_link_libraries(camera_node
  rclcpp::rclcpp
  ${builtin_interfaces_TARGETS}
  ${sensor_msgs_TARGETS}
  opencv_core
  opencv_highgui
  opencv_imgproc)

# A stand alone node which adds some text to an image using OpenCV before passing it along.
add_executable(watermark_node
  src/watermark_node.cpp)
target_link_libraries(watermark_node
  rclcpp::rclcpp
  ${builtin_interfaces_TARGETS}
  ${sensor_msgs_TARGETS}
  opencv_core
  opencv_videoio)

# A stand alone node which consumes images and displays them using OpenCV.
add_executable(image_view_node
  src/image_view_node.cpp)
target_link_libraries(image_view_node
  rclcpp::rclcpp
  ${builtin_interfaces_TARGETS}
  ${sensor_msgs_TARGETS}
  opencv_core
  opencv_highgui)
  • 安装相关库文件和执行文件
install(TARGETS
  all_in_one
  two_image_view
  camera_node
  watermark_node
  image_view_node
  DESTINATION lib/${PROJECT_NAME})
  • 编译包
colcon build --symlink-install --packages-select cpp_intra_process_image
  • 加载工作空间
. install/setup.bash
  • 测试1:　
ros2 run cpp_intra_process_image all_in_one
  • 测试2:　
ros2 run cpp_intra_process_image two_image_view

　

参考:

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ROS2与C++入门教程