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_