From a18fcfb718af3ec78d0760108197e3173d9c01c3 Mon Sep 17 00:00:00 2001 From: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Date: Fri, 3 Dec 2021 13:16:59 +0900 Subject: [PATCH] feat: image_transport_decompressor package (#43) * Add nodelets of tlr nodes (#715) * Add classifier nodelet Signed-off-by: Daisuke Nishimatsu * Replace boost::shared_ptr into std::shared_ptr Signed-off-by: Daisuke Nishimatsu * Add lock guard Signed-off-by: Daisuke Nishimatsu * Add detetcor nodelet Signed-off-by: Daisuke Nishimatsu * Integrate main into node Signed-off-by: Daisuke Nishimatsu * Add SubscriberStatusCallback Signed-off-by: Daisuke Nishimatsu * add image_transport_decompresser nodelet Signed-off-by: Yukihiro Saito * Add visualizer nodelet Signed-off-by: Daisuke Nishimatsu * fixed bug Signed-off-by: Yukihiro Saito * Fix plugin name Signed-off-by: Daisuke Nishimatsu * Launch nodelet Signed-off-by: Daisuke Nishimatsu * Fix classifier constructor Signed-off-by: Daisuke Nishimatsu * add decompresser node Signed-off-by: Yukihiro Saito * fix typo Signed-off-by: Yukihiro Saito * fixed bug Signed-off-by: Yukihiro Saito * fixed bug Signed-off-by: Yukihiro Saito * cosmetic change Signed-off-by: Yukihiro Saito * add param Signed-off-by: Yukihiro Saito * fix bug Signed-off-by: Yukihiro Saito * Fix build warning Signed-off-by: Daisuke Nishimatsu * change rgb Signed-off-by: Yukihiro Saito * change rgb Signed-off-by: Yukihiro Saito Co-authored-by: Yukihiro Saito * remove ROS1 packages temporarily Signed-off-by: mitsudome-r * Revert "remove ROS1 packages temporarily" This reverts commit 4f557b764f89acf6fa4dfdad198abe53021aefe7. Signed-off-by: mitsudome-r * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r * Rename h files to hpp (#142) * Change includes * Rename files * Adjustments to make things compile * Other packages * Adjust copyright notice on 532 out of 699 source files (#143) * Use quotes for includes where appropriate (#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Run uncrustify on the entire Pilot.Auto codebase (#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs * Unify Apache-2.0 license name (#1242) * Refine BSD license name (#1244) * Porting image transport decompressor (#1428) Signed-off-by: wep21 * Cosmetic change (#1443) * Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * fix spell * Fix lint issues Signed-off-by: Kenji Miyake * Ignore flake8 warnings Signed-off-by: Kenji Miyake Co-authored-by: Hiroki OTA * Change formatter to clang-format and black (#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake * remove COLCON_IGNORE (#515) * doc image transport decompressor (#602) * add README * minimal doc * Remove template comments from node documents (#608) * rename document file * remove template comment * feat: add launch file for image transport decompressor Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Yukihiro Saito Co-authored-by: mitsudome-r Co-authored-by: Nikolai Morin Co-authored-by: Kazuki Miyahara Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Hiroki OTA Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Tomoya Kimura --- .../CMakeLists.txt | 42 ++++ .../image_transport_decompressor/README.md | 33 ++++ .../image_transport_decompressor.hpp | 45 +++++ .../image_transport_decompressor.launch.xml | 9 + .../image_transport_decompressor/package.xml | 29 +++ .../src/image_transport_decompressor.cpp | 181 ++++++++++++++++++ 6 files changed, 339 insertions(+) create mode 100644 sensing/image_transport_decompressor/CMakeLists.txt create mode 100644 sensing/image_transport_decompressor/README.md create mode 100644 sensing/image_transport_decompressor/include/image_transport_decompressor/image_transport_decompressor.hpp create mode 100644 sensing/image_transport_decompressor/launch/image_transport_decompressor.launch.xml create mode 100644 sensing/image_transport_decompressor/package.xml create mode 100644 sensing/image_transport_decompressor/src/image_transport_decompressor.cpp 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)