Program Listing for File cv_mat_sensor_msgs_image_type_adapter.hpp

↰ Return to documentation for file (include/image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp)

// Copyright 2021 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef IMAGE_TOOLS__CV_MAT_SENSOR_MSGS_IMAGE_TYPE_ADAPTER_HPP_
#define IMAGE_TOOLS__CV_MAT_SENSOR_MSGS_IMAGE_TYPE_ADAPTER_HPP_

#include <cstddef>
#include <memory>
#include <variant>  // NOLINT[build/include_order]

#include "opencv2/core/mat.hpp"

#include "rclcpp/type_adapter.hpp"
#include "sensor_msgs/msg/image.hpp"

#include "image_tools/visibility_control.h"

namespace image_tools
{
namespace detail
{
// TODO(audrow): Replace with std::endian when C++ 20 is available
// https://en.cppreference.com/w/cpp/types/endian
enum class endian
{
#ifdef _WIN32
  little = 0,
  big    = 1,
  native = little
#else
  little = __ORDER_LITTLE_ENDIAN__,
  big    = __ORDER_BIG_ENDIAN__,
  native = __BYTE_ORDER__
#endif
};

}  // namespace detail


// TODO(wjwwood): make this as a contribution to vision_opencv's cv_bridge package.
//   Specifically the CvImage class, which is this is most similar to.

class ROSCvMatContainer
{
  static constexpr bool is_bigendian_system = detail::endian::native == detail::endian::big;

public:
  using SensorMsgsImageStorageType = std::variant<
    std::nullptr_t,
    std::unique_ptr<sensor_msgs::msg::Image>,
    std::shared_ptr<sensor_msgs::msg::Image>
  >;

  IMAGE_TOOLS_PUBLIC
  ROSCvMatContainer() = default;

  IMAGE_TOOLS_PUBLIC
  explicit ROSCvMatContainer(const ROSCvMatContainer & other)
  : header_(other.header_), frame_(other.frame_.clone()), is_bigendian_(other.is_bigendian_)
  {
    if (std::holds_alternative<std::shared_ptr<sensor_msgs::msg::Image>>(other.storage_)) {
      storage_ = std::get<std::shared_ptr<sensor_msgs::msg::Image>>(other.storage_);
    } else if (std::holds_alternative<std::unique_ptr<sensor_msgs::msg::Image>>(other.storage_)) {
      storage_ = std::make_unique<sensor_msgs::msg::Image>(
        *std::get<std::unique_ptr<sensor_msgs::msg::Image>>(other.storage_));
    }
  }

  IMAGE_TOOLS_PUBLIC
  ROSCvMatContainer & operator=(const ROSCvMatContainer & other)
  {
    if (this != &other) {
      header_ = other.header_;
      frame_ = other.frame_.clone();
      is_bigendian_ = other.is_bigendian_;
      if (std::holds_alternative<std::shared_ptr<sensor_msgs::msg::Image>>(other.storage_)) {
        storage_ = std::get<std::shared_ptr<sensor_msgs::msg::Image>>(other.storage_);
      } else if (std::holds_alternative<std::unique_ptr<sensor_msgs::msg::Image>>(other.storage_)) {
        storage_ = std::make_unique<sensor_msgs::msg::Image>(
          *std::get<std::unique_ptr<sensor_msgs::msg::Image>>(other.storage_));
      } else if (std::holds_alternative<std::nullptr_t>(other.storage_)) {
        storage_ = nullptr;
      }
    }
    return *this;
  }

  IMAGE_TOOLS_PUBLIC
  explicit ROSCvMatContainer(std::unique_ptr<sensor_msgs::msg::Image> unique_sensor_msgs_image);

  IMAGE_TOOLS_PUBLIC
  explicit ROSCvMatContainer(std::shared_ptr<sensor_msgs::msg::Image> shared_sensor_msgs_image);

  IMAGE_TOOLS_PUBLIC
  ROSCvMatContainer(
    const cv::Mat & mat_frame,
    const std_msgs::msg::Header & header,
    bool is_bigendian = is_bigendian_system);

  IMAGE_TOOLS_PUBLIC
  ROSCvMatContainer(
    cv::Mat && mat_frame,
    const std_msgs::msg::Header & header,
    bool is_bigendian = is_bigendian_system);

  IMAGE_TOOLS_PUBLIC
  explicit ROSCvMatContainer(const sensor_msgs::msg::Image & sensor_msgs_image);


  IMAGE_TOOLS_PUBLIC
  bool
  is_owning() const;

  IMAGE_TOOLS_PUBLIC
  const cv::Mat &
  cv_mat() const;


  IMAGE_TOOLS_PUBLIC
  cv::Mat
  cv_mat();

  IMAGE_TOOLS_PUBLIC
  const std_msgs::msg::Header &
  header() const;

  IMAGE_TOOLS_PUBLIC
  std_msgs::msg::Header &
  header();

  IMAGE_TOOLS_PUBLIC
  std::shared_ptr<const sensor_msgs::msg::Image>
  get_sensor_msgs_msg_image_pointer() const;

  IMAGE_TOOLS_PUBLIC
  std::unique_ptr<sensor_msgs::msg::Image>
  get_sensor_msgs_msg_image_pointer_copy() const;

  IMAGE_TOOLS_PUBLIC
  sensor_msgs::msg::Image
  get_sensor_msgs_msg_image_copy() const;

  IMAGE_TOOLS_PUBLIC
  void
  get_sensor_msgs_msg_image_copy(sensor_msgs::msg::Image & sensor_msgs_image) const;

  IMAGE_TOOLS_PUBLIC
  bool
  is_bigendian() const;

private:
  std_msgs::msg::Header header_;
  cv::Mat frame_;
  SensorMsgsImageStorageType storage_;
  bool is_bigendian_;
};

}  // namespace image_tools

template<>
struct rclcpp::TypeAdapter<image_tools::ROSCvMatContainer, sensor_msgs::msg::Image>
{
  using is_specialized = std::true_type;
  using custom_type = image_tools::ROSCvMatContainer;
  using ros_message_type = sensor_msgs::msg::Image;

  static
  void
  convert_to_ros_message(
    const custom_type & source,
    ros_message_type & destination)
  {
    destination.height = source.cv_mat().rows;
    destination.width = source.cv_mat().cols;
    switch (source.cv_mat().type()) {
      case CV_8UC1:
        destination.encoding = "mono8";
        break;
      case CV_8UC3:
        destination.encoding = "bgr8";
        break;
      case CV_16SC1:
        destination.encoding = "mono16";
        break;
      case CV_8UC4:
        destination.encoding = "rgba8";
        break;
      default:
        throw std::runtime_error("unsupported encoding type");
    }
    destination.step = static_cast<sensor_msgs::msg::Image::_step_type>(source.cv_mat().step);
    size_t size = source.cv_mat().step * source.cv_mat().rows;
    destination.data.resize(size);
    memcpy(&destination.data[0], source.cv_mat().data, size);
    destination.header = source.header();
  }

  static
  void
  convert_to_custom(
    const ros_message_type & source,
    custom_type & destination)
  {
    destination = image_tools::ROSCvMatContainer(source);
  }
};

#endif  // IMAGE_TOOLS__CV_MAT_SENSOR_MSGS_IMAGE_TYPE_ADAPTER_HPP_