diff --git a/sensing/image_transport_decompressor/CMakeLists.txt b/sensing/image_transport_decompressor/CMakeLists.txt new file mode 100644 index 0000000000000..e67a6a9cf4eb4 --- /dev/null +++ b/sensing/image_transport_decompressor/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.5) +project(image_transport_decompressor) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +find_package(ament_cmake_auto) +ament_auto_find_build_dependencies() + +find_package(OpenCV REQUIRED) + +include_directories( + ${OpenCV_INCLUDE_DIRS} +) + +ament_auto_add_library(image_transport_decompressor SHARED + src/image_transport_decompressor.cpp +) + +target_link_libraries(image_transport_decompressor + ${OpenCV_LIBRARIES} +) + +rclcpp_components_register_node(image_transport_decompressor + PLUGIN "image_preprocessor::ImageTransportDecompressor" + EXECUTABLE image_transport_decompressor_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/sensing/image_transport_decompressor/README.md b/sensing/image_transport_decompressor/README.md new file mode 100644 index 0000000000000..1a2b18f91c17a --- /dev/null +++ b/sensing/image_transport_decompressor/README.md @@ -0,0 +1,33 @@ +# image_transport_decompressor + +## Purpose + +The `image_transport_decompressor` is a node that decompresses images. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------------------- | ----------------------------------- | ---------------- | +| `~/input/compressed_image` | `sensor_msgs::msg::CompressedImage` | compressed image | + +### Output + +| Name | Type | Description | +| -------------------- | ------------------------- | ------------------ | +| `~/output/raw_image` | `sensor_msgs::msg::Image` | decompressed image | + +## Parameters + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/image_transport_decompressor/include/image_transport_decompressor/image_transport_decompressor.hpp b/sensing/image_transport_decompressor/include/image_transport_decompressor/image_transport_decompressor.hpp new file mode 100644 index 0000000000000..d192429dd150e --- /dev/null +++ b/sensing/image_transport_decompressor/include/image_transport_decompressor/image_transport_decompressor.hpp @@ -0,0 +1,45 @@ +// Copyright 2020 Tier IV, 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_TRANSPORT_DECOMPRESSOR__IMAGE_TRANSPORT_DECOMPRESSOR_HPP_ +#define IMAGE_TRANSPORT_DECOMPRESSOR__IMAGE_TRANSPORT_DECOMPRESSOR_HPP_ + +#include + +#include +#include + +#include +#include +#include + +namespace image_preprocessor +{ +class ImageTransportDecompressor : public rclcpp::Node +{ +public: + explicit ImageTransportDecompressor(const rclcpp::NodeOptions & node_options); + +private: + void onCompressedImage( + const sensor_msgs::msg::CompressedImage::ConstSharedPtr input_compressed_image_msg); + + rclcpp::Subscription::SharedPtr compressed_image_sub_; + rclcpp::Publisher::SharedPtr raw_image_pub_; + std::string encoding_; +}; + +} // namespace image_preprocessor + +#endif // IMAGE_TRANSPORT_DECOMPRESSOR__IMAGE_TRANSPORT_DECOMPRESSOR_HPP_ diff --git a/sensing/image_transport_decompressor/launch/image_transport_decompressor.launch.xml b/sensing/image_transport_decompressor/launch/image_transport_decompressor.launch.xml new file mode 100644 index 0000000000000..13b8884f2fc0e --- /dev/null +++ b/sensing/image_transport_decompressor/launch/image_transport_decompressor.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/sensing/image_transport_decompressor/package.xml b/sensing/image_transport_decompressor/package.xml new file mode 100644 index 0000000000000..c6238d4fd7ae6 --- /dev/null +++ b/sensing/image_transport_decompressor/package.xml @@ -0,0 +1,29 @@ + + + image_transport_decompressor + 0.0.0 + The image_transport_decompressor package + + Yukihiro Saito + Yukihiro Saito + Apache License 2.0 + Patrick Mihelich + Julius Kammerl + BSD-3-Clause + + ament_cmake_auto + + cv_bridge + rclcpp + rclcpp_components + sensor_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + + diff --git a/sensing/image_transport_decompressor/src/image_transport_decompressor.cpp b/sensing/image_transport_decompressor/src/image_transport_decompressor.cpp new file mode 100644 index 0000000000000..9d0b7997e0be0 --- /dev/null +++ b/sensing/image_transport_decompressor/src/image_transport_decompressor.cpp @@ -0,0 +1,181 @@ +// Copyright 2020 Tier IV, 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. + +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "image_transport_decompressor/image_transport_decompressor.hpp" + +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include + +namespace image_preprocessor +{ +ImageTransportDecompressor::ImageTransportDecompressor(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("image_transport_decompressor", node_options), + encoding_(declare_parameter("encoding", "default")) +{ + compressed_image_sub_ = create_subscription( + "~/input/compressed_image", rclcpp::SensorDataQoS(), + std::bind(&ImageTransportDecompressor::onCompressedImage, this, std::placeholders::_1)); + raw_image_pub_ = + create_publisher("~/output/raw_image", rclcpp::SensorDataQoS()); +} + +void ImageTransportDecompressor::onCompressedImage( + const sensor_msgs::msg::CompressedImage::ConstSharedPtr input_compressed_image_msg) +{ + cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage); + // Copy message header + cv_ptr->header = input_compressed_image_msg->header; + + // Decode color/mono image + try { + cv_ptr->image = cv::imdecode(cv::Mat(input_compressed_image_msg->data), cv::IMREAD_COLOR); + + // Assign image encoding string + const size_t split_pos = input_compressed_image_msg->format.find(';'); + if (split_pos == std::string::npos) { + // Older version of compressed_image_transport does not signal image format + switch (cv_ptr->image.channels()) { + case 1: + cv_ptr->encoding = sensor_msgs::image_encodings::MONO8; + break; + case 3: + cv_ptr->encoding = sensor_msgs::image_encodings::BGR8; + break; + default: + RCLCPP_ERROR( + get_logger(), "Unsupported number of channels: %i", cv_ptr->image.channels()); + break; + } + } else { + std::string image_encoding; + if (encoding_ == std::string("default")) { + image_encoding = input_compressed_image_msg->format.substr(0, split_pos); + } else if (encoding_ == std::string("rgb8")) { + image_encoding = "rgb8"; + } else if (encoding_ == std::string("bgr8")) { + image_encoding = "bgr8"; + } else { + image_encoding = input_compressed_image_msg->format.substr(0, split_pos); + } + + cv_ptr->encoding = image_encoding; + + if (sensor_msgs::image_encodings::isColor(image_encoding)) { + std::string compressed_encoding = input_compressed_image_msg->format.substr(split_pos); + bool compressed_bgr_image = + (compressed_encoding.find("compressed bgr") != std::string::npos); + + // Revert color transformation + if (compressed_bgr_image) { + // if necessary convert colors from bgr to rgb + if ( + (image_encoding == sensor_msgs::image_encodings::RGB8) || + (image_encoding == sensor_msgs::image_encodings::RGB16)) { + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGB); + } + + if ( + (image_encoding == sensor_msgs::image_encodings::RGBA8) || + (image_encoding == sensor_msgs::image_encodings::RGBA16)) { + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGBA); + } + + if ( + (image_encoding == sensor_msgs::image_encodings::BGRA8) || + (image_encoding == sensor_msgs::image_encodings::BGRA16)) { + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2BGRA); + } + } else { + // if necessary convert colors from rgb to bgr + if ( + (image_encoding == sensor_msgs::image_encodings::BGR8) || + (image_encoding == sensor_msgs::image_encodings::BGR16)) { + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGR); + } + + if ( + (image_encoding == sensor_msgs::image_encodings::BGRA8) || + (image_encoding == sensor_msgs::image_encodings::BGRA16)) { + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGRA); + } + + if ( + (image_encoding == sensor_msgs::image_encodings::RGBA8) || + (image_encoding == sensor_msgs::image_encodings::RGBA16)) { + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2RGBA); + } + } + } + } + } catch (cv::Exception & e) { + RCLCPP_ERROR(get_logger(), "%s", e.what()); + } + + size_t rows = cv_ptr->image.rows; + size_t cols = cv_ptr->image.cols; + + if ((rows > 0) && (cols > 0)) { + // Publish message to user callback + auto image_ptr = std::make_unique(*cv_ptr->toImageMsg()); + raw_image_pub_->publish(std::move(image_ptr)); + } +} +} // namespace image_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_preprocessor::ImageTransportDecompressor)