|
| 1 | +// Copyright 2020 Tier IV, Inc. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +/********************************************************************* |
| 16 | + * Software License Agreement (BSD License) |
| 17 | + * |
| 18 | + * Copyright (c) 2012, Willow Garage, Inc. |
| 19 | + * All rights reserved. |
| 20 | + * |
| 21 | + * Redistribution and use in source and binary forms, with or without |
| 22 | + * modification, are permitted provided that the following conditions |
| 23 | + * are met: |
| 24 | + * |
| 25 | + * * Redistributions of source code must retain the above copyright |
| 26 | + * notice, this list of conditions and the following disclaimer. |
| 27 | + * * Redistributions in binary form must reproduce the above |
| 28 | + * copyright notice, this list of conditions and the following |
| 29 | + * disclaimer in the documentation and/or other materials provided |
| 30 | + * with the distribution. |
| 31 | + * * Neither the name of the Willow Garage nor the names of its |
| 32 | + * contributors may be used to endorse or promote products derived |
| 33 | + * from this software without specific prior written permission. |
| 34 | + * |
| 35 | + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| 36 | + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| 37 | + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
| 38 | + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
| 39 | + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
| 40 | + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
| 41 | + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
| 42 | + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
| 43 | + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
| 44 | + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
| 45 | + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 46 | + * POSSIBILITY OF SUCH DAMAGE. |
| 47 | + *********************************************************************/ |
| 48 | + |
| 49 | +#include "image_transport_decompressor/image_transport_decompressor.hpp" |
| 50 | + |
| 51 | +#include <opencv2/highgui/highgui.hpp> |
| 52 | +#include <opencv2/imgproc/imgproc.hpp> |
| 53 | + |
| 54 | +#include <sensor_msgs/image_encodings.hpp> |
| 55 | + |
| 56 | +#include <cv_bridge/cv_bridge.h> |
| 57 | + |
| 58 | +#include <limits> |
| 59 | +#include <memory> |
| 60 | +#include <string> |
| 61 | +#include <utility> |
| 62 | +#include <vector> |
| 63 | + |
| 64 | +namespace image_preprocessor |
| 65 | +{ |
| 66 | +ImageTransportDecompressor::ImageTransportDecompressor(const rclcpp::NodeOptions & node_options) |
| 67 | +: rclcpp::Node("image_transport_decompressor", node_options), |
| 68 | + encoding_(declare_parameter("encoding", "default")) |
| 69 | +{ |
| 70 | + compressed_image_sub_ = create_subscription<sensor_msgs::msg::CompressedImage>( |
| 71 | + "~/input/compressed_image", rclcpp::SensorDataQoS(), |
| 72 | + std::bind(&ImageTransportDecompressor::onCompressedImage, this, std::placeholders::_1)); |
| 73 | + raw_image_pub_ = |
| 74 | + create_publisher<sensor_msgs::msg::Image>("~/output/raw_image", rclcpp::SensorDataQoS()); |
| 75 | +} |
| 76 | + |
| 77 | +void ImageTransportDecompressor::onCompressedImage( |
| 78 | + const sensor_msgs::msg::CompressedImage::ConstSharedPtr input_compressed_image_msg) |
| 79 | +{ |
| 80 | + cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage); |
| 81 | + // Copy message header |
| 82 | + cv_ptr->header = input_compressed_image_msg->header; |
| 83 | + |
| 84 | + // Decode color/mono image |
| 85 | + try { |
| 86 | + cv_ptr->image = cv::imdecode(cv::Mat(input_compressed_image_msg->data), cv::IMREAD_COLOR); |
| 87 | + |
| 88 | + // Assign image encoding string |
| 89 | + const size_t split_pos = input_compressed_image_msg->format.find(';'); |
| 90 | + if (split_pos == std::string::npos) { |
| 91 | + // Older version of compressed_image_transport does not signal image format |
| 92 | + switch (cv_ptr->image.channels()) { |
| 93 | + case 1: |
| 94 | + cv_ptr->encoding = sensor_msgs::image_encodings::MONO8; |
| 95 | + break; |
| 96 | + case 3: |
| 97 | + cv_ptr->encoding = sensor_msgs::image_encodings::BGR8; |
| 98 | + break; |
| 99 | + default: |
| 100 | + RCLCPP_ERROR( |
| 101 | + get_logger(), "Unsupported number of channels: %i", cv_ptr->image.channels()); |
| 102 | + break; |
| 103 | + } |
| 104 | + } else { |
| 105 | + std::string image_encoding; |
| 106 | + if (encoding_ == std::string("default")) { |
| 107 | + image_encoding = input_compressed_image_msg->format.substr(0, split_pos); |
| 108 | + } else if (encoding_ == std::string("rgb8")) { |
| 109 | + image_encoding = "rgb8"; |
| 110 | + } else if (encoding_ == std::string("bgr8")) { |
| 111 | + image_encoding = "bgr8"; |
| 112 | + } else { |
| 113 | + image_encoding = input_compressed_image_msg->format.substr(0, split_pos); |
| 114 | + } |
| 115 | + |
| 116 | + cv_ptr->encoding = image_encoding; |
| 117 | + |
| 118 | + if (sensor_msgs::image_encodings::isColor(image_encoding)) { |
| 119 | + std::string compressed_encoding = input_compressed_image_msg->format.substr(split_pos); |
| 120 | + bool compressed_bgr_image = |
| 121 | + (compressed_encoding.find("compressed bgr") != std::string::npos); |
| 122 | + |
| 123 | + // Revert color transformation |
| 124 | + if (compressed_bgr_image) { |
| 125 | + // if necessary convert colors from bgr to rgb |
| 126 | + if ( |
| 127 | + (image_encoding == sensor_msgs::image_encodings::RGB8) || |
| 128 | + (image_encoding == sensor_msgs::image_encodings::RGB16)) { |
| 129 | + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGB); |
| 130 | + } |
| 131 | + |
| 132 | + if ( |
| 133 | + (image_encoding == sensor_msgs::image_encodings::RGBA8) || |
| 134 | + (image_encoding == sensor_msgs::image_encodings::RGBA16)) { |
| 135 | + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGBA); |
| 136 | + } |
| 137 | + |
| 138 | + if ( |
| 139 | + (image_encoding == sensor_msgs::image_encodings::BGRA8) || |
| 140 | + (image_encoding == sensor_msgs::image_encodings::BGRA16)) { |
| 141 | + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2BGRA); |
| 142 | + } |
| 143 | + } else { |
| 144 | + // if necessary convert colors from rgb to bgr |
| 145 | + if ( |
| 146 | + (image_encoding == sensor_msgs::image_encodings::BGR8) || |
| 147 | + (image_encoding == sensor_msgs::image_encodings::BGR16)) { |
| 148 | + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGR); |
| 149 | + } |
| 150 | + |
| 151 | + if ( |
| 152 | + (image_encoding == sensor_msgs::image_encodings::BGRA8) || |
| 153 | + (image_encoding == sensor_msgs::image_encodings::BGRA16)) { |
| 154 | + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGRA); |
| 155 | + } |
| 156 | + |
| 157 | + if ( |
| 158 | + (image_encoding == sensor_msgs::image_encodings::RGBA8) || |
| 159 | + (image_encoding == sensor_msgs::image_encodings::RGBA16)) { |
| 160 | + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2RGBA); |
| 161 | + } |
| 162 | + } |
| 163 | + } |
| 164 | + } |
| 165 | + } catch (cv::Exception & e) { |
| 166 | + RCLCPP_ERROR(get_logger(), "%s", e.what()); |
| 167 | + } |
| 168 | + |
| 169 | + size_t rows = cv_ptr->image.rows; |
| 170 | + size_t cols = cv_ptr->image.cols; |
| 171 | + |
| 172 | + if ((rows > 0) && (cols > 0)) { |
| 173 | + // Publish message to user callback |
| 174 | + auto image_ptr = std::make_unique<sensor_msgs::msg::Image>(*cv_ptr->toImageMsg()); |
| 175 | + raw_image_pub_->publish(std::move(image_ptr)); |
| 176 | + } |
| 177 | +} |
| 178 | +} // namespace image_preprocessor |
| 179 | + |
| 180 | +#include <rclcpp_components/register_node_macro.hpp> |
| 181 | +RCLCPP_COMPONENTS_REGISTER_NODE(image_preprocessor::ImageTransportDecompressor) |
0 commit comments