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 index b069cc200..1d7cf6250 100644 --- 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 @@ -17,6 +17,7 @@ #include #include +#include #include // NOLINT[build/include_order] #include "opencv2/core/mat.hpp" @@ -85,6 +86,7 @@ enum class endian */ class ROSCvMatContainer { +public: static constexpr bool is_bigendian_system = detail::endian::native == detail::endian::big; public: @@ -141,14 +143,16 @@ class ROSCvMatContainer ROSCvMatContainer( const cv::Mat & mat_frame, const std_msgs::msg::Header & header, - bool is_bigendian = is_bigendian_system); + bool is_bigendian = is_bigendian_system, + std::optional encoding_override = std::nullopt); /// 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); + bool is_bigendian = is_bigendian_system, + std::optional encoding_override = std::nullopt); /// Copy the sensor_msgs::msg::Image into this contain and create a cv::Mat that references it. IMAGE_TOOLS_PUBLIC @@ -212,11 +216,17 @@ class ROSCvMatContainer bool is_bigendian() const; + /// Return the encoding override if provided. + IMAGE_TOOLS_PUBLIC + std::optional + encoding_override() const; + private: std_msgs::msg::Header header_; cv::Mat frame_; SensorMsgsImageStorageType storage_; bool is_bigendian_; + std::optional encoding_override_; }; } // namespace image_tools @@ -236,7 +246,14 @@ struct rclcpp::TypeAdapter(source.cv_mat().step); size_t size = source.cv_mat().step * source.cv_mat().rows; diff --git a/image_tools/src/cam2image.cpp b/image_tools/src/cam2image.cpp index 2e254a992..8c22b722b 100644 --- a/image_tools/src/cam2image.cpp +++ b/image_tools/src/cam2image.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -148,7 +149,12 @@ 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); + image_tools::ROSCvMatContainer container( + frame, + header, + ROSCvMatContainer::is_bigendian_system, + encoding_override_ + ); // Publish the image message and increment the publish_number_. RCLCPP_INFO(get_logger(), "Publishing image #%zd", publish_number_++); @@ -249,6 +255,15 @@ class Cam2Image : public rclcpp::Node burger_mode_desc.description = "Produce images of burgers rather than connecting to a camera"; burger_mode_ = this->declare_parameter("burger_mode", false, burger_mode_desc); frame_id_ = this->declare_parameter("frame_id", "camera_frame"); + std::string encoding_override = this->declare_parameter("encoding_override", ""); + if (encoding_override.empty()) + { + encoding_override_ = std::nullopt; + } + else + { + encoding_override_ = std::move(encoding_override); + } } cv::VideoCapture cap; @@ -269,6 +284,7 @@ class Cam2Image : public rclcpp::Node bool burger_mode_; std::string frame_id_; int device_id_; + std::optional encoding_override_; /// If true, will cause the incoming camera image message to flip about the y-axis. bool is_flipped_; 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 index a22b54cb1..919348659 100644 --- a/image_tools/src/cv_mat_sensor_msgs_image_type_adapter.cpp +++ b/image_tools/src/cv_mat_sensor_msgs_image_type_adapter.cpp @@ -100,21 +100,25 @@ ROSCvMatContainer::ROSCvMatContainer( ROSCvMatContainer::ROSCvMatContainer( const cv::Mat & mat_frame, const std_msgs::msg::Header & header, - bool is_bigendian) + bool is_bigendian, + std::optional encoding_override) : header_(header), frame_(mat_frame), storage_(nullptr), - is_bigendian_(is_bigendian) + is_bigendian_(is_bigendian), + encoding_override_(encoding_override) {} ROSCvMatContainer::ROSCvMatContainer( cv::Mat && mat_frame, const std_msgs::msg::Header & header, - bool is_bigendian) + bool is_bigendian, + std::optional encoding_override) : header_(header), frame_(std::forward(mat_frame)), storage_(nullptr), - is_bigendian_(is_bigendian) + is_bigendian_(is_bigendian), + encoding_override_(encoding_override) {} ROSCvMatContainer::ROSCvMatContainer( @@ -175,21 +179,28 @@ ROSCvMatContainer::get_sensor_msgs_msg_image_copy( { 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"); + if (encoding_override_.has_value() && !encoding_override_.value().empty()) + { + sensor_msgs_image.encoding = encoding_override_.value(); + } + else + { + 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; @@ -204,4 +215,10 @@ ROSCvMatContainer::is_bigendian() const return is_bigendian_; } +std::optional +ROSCvMatContainer::encoding_override() const +{ + return encoding_override_; +} + } // namespace image_tools diff --git a/image_tools/src/showimage.cpp b/image_tools/src/showimage.cpp index c221b1f5b..217091b74 100644 --- a/image_tools/src/showimage.cpp +++ b/image_tools/src/showimage.cpp @@ -179,12 +179,12 @@ class ShowImage : public rclcpp::Node if (show_image) { cv::Mat frame = container.cv_mat(); - if (frame.type() == CV_8UC3 /* rgb8 */) { - cv::cvtColor(frame, frame, cv::COLOR_RGB2BGR); - } 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); - } + // if (frame.type() == CV_8UC3 /* rgb8 */) { + // cv::cvtColor(frame, frame, cv::COLOR_RGB2BGR); + // } 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); + // } // Show the image in a window cv::imshow(window_name_, frame);