diff --git a/image_tools/CMakeLists.txt b/image_tools/CMakeLists.txt index ba8419969..a82ee1074 100644 --- a/image_tools/CMakeLists.txt +++ b/image_tools/CMakeLists.txt @@ -20,27 +20,42 @@ find_package(OpenCV REQUIRED) include_directories(include) -add_library(imagetools SHARED +add_library(${PROJECT_NAME} SHARED + src/burger.cpp src/cam2image.cpp + src/cv_mat_sensor_msgs_image_type_adapter.cpp src/showimage.cpp - src/burger.cpp) -target_compile_definitions(imagetools +) +target_compile_definitions(${PROJECT_NAME} PRIVATE "IMAGE_TOOLS_BUILDING_DLL") -ament_target_dependencies(imagetools +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$") +ament_target_dependencies(${PROJECT_NAME} "rclcpp" "sensor_msgs" "std_msgs" "rclcpp_components" "OpenCV") -rclcpp_components_register_node(imagetools PLUGIN "image_tools::Cam2Image" EXECUTABLE cam2image) -rclcpp_components_register_node(imagetools PLUGIN "image_tools::ShowImage" EXECUTABLE showimage) +rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "image_tools::Cam2Image" EXECUTABLE cam2image) +rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "image_tools::ShowImage" EXECUTABLE showimage) -install(TARGETS - imagetools +install( + TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) +install( + DIRECTORY include/ + DESTINATION include +) + +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(${PROJECT_NAME}) + +ament_export_dependencies(rclcpp_components) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() 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 new file mode 100644 index 000000000..25b94682c --- /dev/null +++ b/image_tools/include/image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp @@ -0,0 +1,239 @@ +// 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 // NOLINT[build/include_order] + +#include "opencv2/opencv.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< + nullptr_t, + std::unique_ptr, + std::shared_ptr + >; + + IMAGE_TOOLS_PUBLIC + ROSCvMatContainer() = default; + + /// 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/src/cam2image.cpp b/image_tools/src/cam2image.cpp index 6d387c4f7..193923db7 100644 --- a/image_tools/src/cam2image.cpp +++ b/image_tools/src/cam2image.cpp @@ -24,14 +24,18 @@ #include "rcl_interfaces/msg/parameter_descriptor.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_components/register_node_macro.hpp" -#include "sensor_msgs/msg/image.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, + sensor_msgs::msg::Image); + namespace image_tools { class Cam2Image : public rclcpp::Node @@ -74,7 +78,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 @@ -113,9 +117,6 @@ class Cam2Image : public rclcpp::Node cv::Mat frame; // Initialize a shared pointer to an Image message. - auto msg = std::make_unique(); - msg->is_bigendian = false; - // Get the frame from the video capture. if (burger_mode_) { frame = burger_cap.render_burger(width_, height_); @@ -133,25 +134,26 @@ class Cam2Image : public rclcpp::Node cv::flip(frame, frame, 1); } - // Convert to a ROS image - convert_frame_to_message(frame, *msg); - // Conditionally show image if (show_camera_) { - cv::Mat cvframe = frame; // Show the image in a window called "cam2image". - cv::imshow("cam2image", cvframe); + cv::imshow("cam2image", frame); // Draw the image to the screen and wait 1 millisecond. cv::waitKey(1); } - // Publish the image message and increment the frame_id. + std_msgs::msg::Header header; + header.frame_id = frame_id_; + header.stamp = this->now(); + image_tools::ROSCvMatContainer container(frame, header); + + // Publish the image message and increment the publish_number_. RCLCPP_INFO(get_logger(), "Publishing image #%zd", publish_number_++); - pub_->publish(std::move(msg)); + pub_->publish(std::move(container)); } IMAGE_TOOLS_LOCAL - bool help(const std::vector args) + bool help(const std::vector & args) { if (std::find(args.begin(), args.end(), "--help") != args.end() || std::find(args.begin(), args.end(), "-h") != args.end()) @@ -246,55 +248,11 @@ class Cam2Image : public rclcpp::Node frame_id_ = this->declare_parameter("frame_id", "camera_frame"); } - /// Convert an OpenCV matrix encoding type to a string format recognized by sensor_msgs::Image. - /** - * \param[in] mat_type The OpenCV encoding type. - * \return A string representing the encoding type. - */ - IMAGE_TOOLS_LOCAL - 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"); - } - } - - /// Convert an OpenCV matrix (cv::Mat) to a ROS Image message. - /** - * \param[in] frame The OpenCV matrix/image to convert. - * \param[in] frame_id ID for the ROS message. - * \param[out] Allocated shared pointer for the ROS Image message. - */ - IMAGE_TOOLS_LOCAL - void convert_frame_to_message( - const cv::Mat & frame, sensor_msgs::msg::Image & msg) - { - // copy cv information into ros message - msg.height = frame.rows; - msg.width = frame.cols; - msg.encoding = mat_type2encoding(frame.type()); - msg.step = static_cast(frame.step); - size_t size = frame.step * frame.rows; - msg.data.resize(size); - memcpy(&msg.data[0], frame.data, size); - msg.header.frame_id = frame_id_; - msg.header.stamp = this->now(); - } - cv::VideoCapture cap; 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 new file mode 100644 index 000000000..ac5ea52c9 --- /dev/null +++ b/image_tools/src/cv_mat_sensor_msgs_image_type_adapter.cpp @@ -0,0 +1,204 @@ +// 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 // NOLINT[build/include_order] + +#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 ce54fb714..c0e095895 100644 --- a/image_tools/src/showimage.cpp +++ b/image_tools/src/showimage.cpp @@ -25,10 +25,15 @@ #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 "image_tools/visibility_control.h" #include "./policy_maps.hpp" +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( + image_tools::ROSCvMatContainer, + sensor_msgs::msg::Image); + namespace image_tools { class ShowImage : public rclcpp::Node @@ -70,13 +75,13 @@ class ShowImage : 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_); - auto callback = [this](const sensor_msgs::msg::Image::SharedPtr msg) - { - process_image(msg, show_image_, this->get_logger()); + auto callback = + [this](const image_tools::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 @@ -161,67 +166,33 @@ class ShowImage : public rclcpp::Node window_name_ = this->declare_parameter("window_name", ""); } - /// Convert a sensor_msgs::Image encoding type (stored as a string) to an OpenCV encoding type. - /** - * \param[in] encoding A string representing the encoding type. - * \return The OpenCV encoding type. - */ - IMAGE_TOOLS_LOCAL - 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"); - } - } - /// Convert the ROS Image message to an OpenCV matrix and display it to the user. - // \param[in] msg The image message to show. + // \param[in] container The image message to show. IMAGE_TOOLS_LOCAL void process_image( - const sensor_msgs::msg::Image::SharedPtr msg, bool show_image, rclcpp::Logger logger) + const image_tools::ROSCvMatContainer & container, bool show_image, rclcpp::Logger logger) { - RCLCPP_INFO(logger, "Received image #%s", msg->header.frame_id.c_str()); - std::cerr << "Received image #" << msg->header.frame_id.c_str() << std::endl; + RCLCPP_INFO(logger, "Received image #%s", container.header().frame_id.c_str()); + std::cerr << "Received image #" << container.header().frame_id.c_str() << std::endl; if (show_image) { - // Convert to an OpenCV matrix by assigning the data. - cv::Mat frame( - msg->height, msg->width, encoding2mat_type(msg->encoding), - const_cast(msg->data.data()), msg->step); + cv::Mat frame = container.cv_mat(); - if (msg->encoding == "rgb8") { + if (frame.type() == CV_8UC3 /* rgb8 */) { cv::cvtColor(frame, frame, cv::COLOR_RGB2BGR); - } else if (msg->encoding == "yuv422") { - msg->is_bigendian ? cv::cvtColor(frame, frame, cv::COLOR_YUV2BGR_UYVY) : + } else if (frame.type() == CV_8UC2) { + container.is_bigendian() ? cv::cvtColor(frame, frame, cv::COLOR_YUV2BGR_UYVY) : cv::cvtColor(frame, frame, cv::COLOR_YUV2BGR_YUYV); } - cv::Mat cvframe = frame; - // Show the image in a window - cv::imshow(window_name_, cvframe); + cv::imshow(window_name_, frame); // Draw the screen and wait for 1 millisecond. cv::waitKey(1); } } - 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;