diff --git a/image_tools/CMakeLists.txt b/image_tools/CMakeLists.txt index 8d6931e51..86effeb18 100644 --- a/image_tools/CMakeLists.txt +++ b/image_tools/CMakeLists.txt @@ -17,11 +17,11 @@ find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(OpenCV REQUIRED COMPONENTS core highgui imgcodecs imgproc videoio) +find_package(cv_bridge REQUIRED) add_library(${PROJECT_NAME} SHARED src/burger.cpp src/cam2image.cpp - src/cv_mat_sensor_msgs_image_type_adapter.cpp src/showimage.cpp ) target_compile_definitions(${PROJECT_NAME} @@ -34,6 +34,7 @@ target_link_libraries(${PROJECT_NAME} ${sensor_msgs_TARGETS} ${std_msgs_TARGETS} rclcpp_components::component + cv_bridge::cv_bridge ${OpenCV_LIBS}) rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "image_tools::Cam2Image" EXECUTABLE cam2image) rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "image_tools::ShowImage" EXECUTABLE showimage) diff --git a/image_tools/include/image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp b/image_tools/include/image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp deleted file mode 100644 index b069cc200..000000000 --- a/image_tools/include/image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp +++ /dev/null @@ -1,272 +0,0 @@ -// 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 -#include -#include // 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. -/// A potentially owning, potentially non-owning, container of a cv::Mat and ROS header. -/** - * The two main use cases for this are publishing user controlled data, and - * recieving data from the middleware that may have been a ROS message - * originally or may have been an cv::Mat originally. - * - * In the first case, publishing user owned data, the user will want to provide - * their own cv::Mat. - * The cv::Mat may own the data or it may not, so in the latter case, it is up - * to the user to ensure the data the cv::Mat points to remains valid as long - * as the middleware needs it. - * - * In the second case, receiving data from the middleware, the middleware will - * either give a new ROSCvMatContainer which owns a sensor_msgs::msg::Image or - * it will give a ROSCvMatContainer that was previously published by the user - * (in the case of intra-process communication). - * If the container owns the sensor_msgs::msg::Image, then the cv::Mat will just - * reference data field of this message, so the container needs to be kept. - * If the container was published by the user it may or may not own the data - * and the cv::Mat it contains may or may not own the data. - * - * For these reasons, it is advisable to use cv::Mat::clone() if you intend to - * copy the cv::Mat and let this container go. - * - * For more details about the ownership behavior of cv::Mat see documentation - * for these methods of cv::Mat: - * - * - template cv::Mat::Mat(const std::vector<_Tp> &, bool) - * - Mat & cv::Mat::operator=(const Mat &) - * - void cv::Mat::addref() - * - void cv::Mat::release() - * - */ -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, - std::shared_ptr - >; - - 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>(other.storage_)) { - storage_ = std::get>(other.storage_); - } else if (std::holds_alternative>(other.storage_)) { - storage_ = std::make_unique( - *std::get>(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>(other.storage_)) { - storage_ = std::get>(other.storage_); - } else if (std::holds_alternative>(other.storage_)) { - storage_ = std::make_unique( - *std::get>(other.storage_)); - } else if (std::holds_alternative(other.storage_)) { - storage_ = nullptr; - } - } - return *this; - } - - /// Store an owning pointer to a sensor_msg::msg::Image, and create a cv::Mat that references it. - IMAGE_TOOLS_PUBLIC - explicit ROSCvMatContainer(std::unique_ptr unique_sensor_msgs_image); - - /// Store an owning pointer to a sensor_msg::msg::Image, and create a cv::Mat that references it. - IMAGE_TOOLS_PUBLIC - explicit ROSCvMatContainer(std::shared_ptr shared_sensor_msgs_image); - - /// Shallow copy the given cv::Mat into this class, but do not own the data directly. - IMAGE_TOOLS_PUBLIC - ROSCvMatContainer( - const cv::Mat & mat_frame, - const std_msgs::msg::Header & header, - bool is_bigendian = is_bigendian_system); - - /// Move the given cv::Mat into this class. - IMAGE_TOOLS_PUBLIC - ROSCvMatContainer( - cv::Mat && mat_frame, - const std_msgs::msg::Header & header, - bool is_bigendian = is_bigendian_system); - - /// Copy the sensor_msgs::msg::Image into this contain and create a cv::Mat that references it. - IMAGE_TOOLS_PUBLIC - explicit ROSCvMatContainer(const sensor_msgs::msg::Image & sensor_msgs_image); - - /// Return true if this class owns the data the cv_mat references. - /** - * Note that this does not check if the cv::Mat owns its own data, only if - * this class owns a sensor_msgs::msg::Image that the cv::Mat references. - */ - IMAGE_TOOLS_PUBLIC - bool - is_owning() const; - - /// Const access the cv::Mat in this class. - IMAGE_TOOLS_PUBLIC - const cv::Mat & - cv_mat() const; - - /// Get a shallow copy of the cv::Mat that is in this class. - /** - * Note that if you want to let this container go out of scope you should - * make a deep copy with cv::Mat::clone() beforehand. - */ - IMAGE_TOOLS_PUBLIC - cv::Mat - cv_mat(); - - /// Const access the ROS Header. - IMAGE_TOOLS_PUBLIC - const std_msgs::msg::Header & - header() const; - - /// Access the ROS Header. - IMAGE_TOOLS_PUBLIC - std_msgs::msg::Header & - header(); - - /// Get shared const pointer to the sensor_msgs::msg::Image if available, otherwise nullptr. - IMAGE_TOOLS_PUBLIC - std::shared_ptr - get_sensor_msgs_msg_image_pointer() const; - - /// Get copy as a unique pointer to the sensor_msgs::msg::Image. - IMAGE_TOOLS_PUBLIC - std::unique_ptr - get_sensor_msgs_msg_image_pointer_copy() const; - - /// Get a copy of the image as a sensor_msgs::msg::Image. - IMAGE_TOOLS_PUBLIC - sensor_msgs::msg::Image - get_sensor_msgs_msg_image_copy() const; - - /// Get a copy of the image as a sensor_msgs::msg::Image. - IMAGE_TOOLS_PUBLIC - void - get_sensor_msgs_msg_image_copy(sensor_msgs::msg::Image & sensor_msgs_image) const; - - /// Return true if the data is stored in big endian, otherwise return false. - 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 -{ - 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(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_ diff --git a/image_tools/package.xml b/image_tools/package.xml index 614da99c0..b782747d0 100644 --- a/image_tools/package.xml +++ b/image_tools/package.xml @@ -14,12 +14,14 @@ Mabel Zhang ament_cmake + cv_bridge libopencv-dev rclcpp rclcpp_components sensor_msgs std_msgs + cv_bridge libopencv-dev rclcpp rclcpp_components diff --git a/image_tools/src/cam2image.cpp b/image_tools/src/cam2image.cpp index 2e254a992..a60b0f122 100644 --- a/image_tools/src/cam2image.cpp +++ b/image_tools/src/cam2image.cpp @@ -24,19 +24,19 @@ #include "opencv2/highgui.hpp" #include "opencv2/videoio.hpp" +#include "cv_bridge/cv_mat_sensor_msgs_image_type_adapter.hpp" #include "rcl_interfaces/msg/parameter_descriptor.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "std_msgs/msg/bool.hpp" -#include "image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp" #include "image_tools/visibility_control.h" #include "./burger.hpp" #include "./policy_maps.hpp" RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( - image_tools::ROSCvMatContainer, + cv_bridge::ROSCvMatContainer, sensor_msgs::msg::Image); namespace image_tools @@ -81,7 +81,7 @@ class Cam2Image : public rclcpp::Node // ensure that every message gets received in order, or best effort, meaning that the transport // makes no guarantees about the order or reliability of delivery. qos.reliability(reliability_policy_); - pub_ = create_publisher("image", qos); + pub_ = create_publisher("image", qos); // Subscribe to a message that will toggle flipping or not flipping, and manage the state in a // callback @@ -148,7 +148,7 @@ class Cam2Image : public rclcpp::Node std_msgs::msg::Header header; header.frame_id = frame_id_; header.stamp = this->now(); - image_tools::ROSCvMatContainer container(frame, header); + cv_bridge::ROSCvMatContainer container(frame, header); // Publish the image message and increment the publish_number_. RCLCPP_INFO(get_logger(), "Publishing image #%zd", publish_number_++); @@ -255,7 +255,7 @@ class Cam2Image : public rclcpp::Node burger::Burger burger_cap; rclcpp::Subscription::SharedPtr sub_; - rclcpp::Publisher::SharedPtr pub_; + rclcpp::Publisher::SharedPtr pub_; rclcpp::TimerBase::SharedPtr timer_; // ROS parameters diff --git a/image_tools/src/cv_mat_sensor_msgs_image_type_adapter.cpp b/image_tools/src/cv_mat_sensor_msgs_image_type_adapter.cpp deleted file mode 100644 index a22b54cb1..000000000 --- a/image_tools/src/cv_mat_sensor_msgs_image_type_adapter.cpp +++ /dev/null @@ -1,207 +0,0 @@ -// 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. - -#include -#include -#include -#include -#include // NOLINT[build/include_order] - -#include "opencv2/core/mat.hpp" - -#include "sensor_msgs/msg/image.hpp" -#include "std_msgs/msg/header.hpp" - -#include "image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp" - -namespace image_tools -{ - -namespace -{ -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; - } else if (encoding == "bgra8") { - return CV_8UC4; - } else if (encoding == "32FC1") { - return CV_32FC1; - } else if (encoding == "rgb8") { - return CV_8UC3; - } else if (encoding == "yuv422") { - return CV_8UC2; - } else { - throw std::runtime_error("Unsupported encoding type"); - } -} - -template -struct NotNull -{ - NotNull(const T * pointer_in, const char * msg) - : pointer(pointer_in) - { - if (pointer == nullptr) { - throw std::invalid_argument(msg); - } - } - - const T * pointer; -}; - -} // namespace - -ROSCvMatContainer::ROSCvMatContainer( - std::unique_ptr unique_sensor_msgs_image) -: header_(NotNull( - unique_sensor_msgs_image.get(), - "unique_sensor_msgs_image cannot be nullptr" -).pointer->header), - frame_( - unique_sensor_msgs_image->height, - unique_sensor_msgs_image->width, - encoding2mat_type(unique_sensor_msgs_image->encoding), - unique_sensor_msgs_image->data.data(), - unique_sensor_msgs_image->step), - storage_(std::move(unique_sensor_msgs_image)) -{} - -ROSCvMatContainer::ROSCvMatContainer( - std::shared_ptr shared_sensor_msgs_image) -: header_(shared_sensor_msgs_image->header), - frame_( - shared_sensor_msgs_image->height, - shared_sensor_msgs_image->width, - encoding2mat_type(shared_sensor_msgs_image->encoding), - shared_sensor_msgs_image->data.data(), - shared_sensor_msgs_image->step), - storage_(shared_sensor_msgs_image) -{} - -ROSCvMatContainer::ROSCvMatContainer( - const cv::Mat & mat_frame, - const std_msgs::msg::Header & header, - bool is_bigendian) -: header_(header), - frame_(mat_frame), - storage_(nullptr), - is_bigendian_(is_bigendian) -{} - -ROSCvMatContainer::ROSCvMatContainer( - cv::Mat && mat_frame, - const std_msgs::msg::Header & header, - bool is_bigendian) -: header_(header), - frame_(std::forward(mat_frame)), - storage_(nullptr), - is_bigendian_(is_bigendian) -{} - -ROSCvMatContainer::ROSCvMatContainer( - const sensor_msgs::msg::Image & sensor_msgs_image) -: ROSCvMatContainer(std::make_unique(sensor_msgs_image)) -{} - -bool -ROSCvMatContainer::is_owning() const -{ - return std::holds_alternative(storage_); -} - -const cv::Mat & -ROSCvMatContainer::cv_mat() const -{ - return frame_; -} - -cv::Mat -ROSCvMatContainer::cv_mat() -{ - return frame_; -} - -const std_msgs::msg::Header & -ROSCvMatContainer::header() const -{ - return header_; -} - -std_msgs::msg::Header & -ROSCvMatContainer::header() -{ - return header_; -} - -std::shared_ptr -ROSCvMatContainer::get_sensor_msgs_msg_image_pointer() const -{ - if (!std::holds_alternative>(storage_)) { - return nullptr; - } - return std::get>(storage_); -} - -std::unique_ptr -ROSCvMatContainer::get_sensor_msgs_msg_image_pointer_copy() const -{ - auto unique_image = std::make_unique(); - this->get_sensor_msgs_msg_image_copy(*unique_image); - return unique_image; -} - -void -ROSCvMatContainer::get_sensor_msgs_msg_image_copy( - sensor_msgs::msg::Image & sensor_msgs_image) const -{ - sensor_msgs_image.height = frame_.rows; - sensor_msgs_image.width = frame_.cols; - switch (frame_.type()) { - case CV_8UC1: - sensor_msgs_image.encoding = "mono8"; - break; - case CV_8UC3: - sensor_msgs_image.encoding = "bgr8"; - break; - case CV_16SC1: - sensor_msgs_image.encoding = "mono16"; - break; - case CV_8UC4: - sensor_msgs_image.encoding = "rgba8"; - break; - default: - throw std::runtime_error("unsupported encoding type"); - } - sensor_msgs_image.step = static_cast(frame_.step); - size_t size = frame_.step * frame_.rows; - sensor_msgs_image.data.resize(size); - memcpy(&sensor_msgs_image.data[0], frame_.data, size); - sensor_msgs_image.header = header_; -} - -bool -ROSCvMatContainer::is_bigendian() const -{ - return is_bigendian_; -} - -} // namespace image_tools diff --git a/image_tools/src/showimage.cpp b/image_tools/src/showimage.cpp index c221b1f5b..297ef3515 100644 --- a/image_tools/src/showimage.cpp +++ b/image_tools/src/showimage.cpp @@ -26,13 +26,13 @@ #include "rclcpp_components/register_node_macro.hpp" #include "sensor_msgs/msg/image.hpp" -#include "image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp" +#include "cv_bridge/cv_mat_sensor_msgs_image_type_adapter.hpp" #include "image_tools/visibility_control.h" #include "./policy_maps.hpp" RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( - image_tools::ROSCvMatContainer, + cv_bridge::ROSCvMatContainer, sensor_msgs::msg::Image); namespace image_tools @@ -77,12 +77,12 @@ class ShowImage : public rclcpp::Node // makes no guarantees about the order or reliability of delivery. qos.reliability(reliability_policy_); auto callback = - [this](const image_tools::ROSCvMatContainer & container) { + [this](const cv_bridge::ROSCvMatContainer & container) { process_image(container, show_image_, this->get_logger()); }; RCLCPP_INFO(this->get_logger(), "Subscribing to topic '%s'", topic_.c_str()); - sub_ = create_subscription(topic_, qos, callback); + sub_ = create_subscription(topic_, qos, callback); if (window_name_ == "") { // If no custom window name is given, use the topic name @@ -171,7 +171,7 @@ class ShowImage : public rclcpp::Node // \param[in] container The image message to show. IMAGE_TOOLS_LOCAL void process_image( - const image_tools::ROSCvMatContainer & container, bool show_image, rclcpp::Logger logger) + const cv_bridge::ROSCvMatContainer & container, bool show_image, rclcpp::Logger logger) { RCLCPP_INFO(logger, "Received image #%s", container.header().frame_id.c_str()); std::cerr << "Received image #" << container.header().frame_id.c_str() << std::endl; @@ -193,7 +193,7 @@ class ShowImage : public rclcpp::Node } } - rclcpp::Subscription::SharedPtr sub_; + rclcpp::Subscription::SharedPtr sub_; size_t depth_ = rmw_qos_profile_default.depth; rmw_qos_reliability_policy_t reliability_policy_ = rmw_qos_profile_default.reliability; rmw_qos_history_policy_t history_policy_ = rmw_qos_profile_default.history;