feat: image_transport_decompressor package (#43)

* Add nodelets of tlr nodes (#715) * Add classifier nodelet Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp> * Replace boost::shared_ptr into std::shared_ptr Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp> * Add lock guard Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp> * Add detetcor nodelet Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp> * Integrate main into node Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp> * Add SubscriberStatusCallback Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp> * add image_transport_decompresser nodelet Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * Add visualizer nodelet Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp> * fixed bug Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * Fix plugin name Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp> * Launch nodelet Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp> * Fix classifier constructor Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp> * add decompresser node Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * fix typo Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * fixed bug Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * fixed bug Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * cosmetic change Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * add param Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * fix bug Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * Fix build warning Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp> * change rgb Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * change rgb Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> * remove ROS1 packages temporarily Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Revert "remove ROS1 packages temporarily" This reverts commit 4f557b764f89acf6fa4dfdad198abe53021aefe7. Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * 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 <border_goldenmarket@yahoo.co.jp> * Cosmetic change (#1443) * Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mistake Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix spell * Fix lint issues Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore flake8 warnings Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> * 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 <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * 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 <yukky.saito@gmail.com> Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> Co-authored-by: Nikolai Morin <nnmmgit@gmail.com> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Tomoya Kimura <tomoya.kimura@tier4.jp>
autowarefoundation · Dec 3, 2021 · a18fcfb · a18fcfb
1 parent e0fc816
commit a18fcfb
Show file tree

Hide file tree

Showing 6 changed files with 339 additions and 0 deletions.
diff --git a/sensing/image_transport_decompressor/CMakeLists.txt 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
@@ -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/...nsport_decompressor/include/image_transport_decompressor/image_transport_decompressor.hpp b/...nsport_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 <rclcpp/rclcpp.hpp>
+
+#include <sensor_msgs/msg/compressed_image.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+#include <memory>
+#include <string>
+#include <utility>
+
+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<sensor_msgs::msg::CompressedImage>::SharedPtr compressed_image_sub_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::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
@@ -0,0 +1,9 @@
+<launch>
+    <arg name="input_topic_name" default="/input/compressed_image" />
+    <arg name="output_topic_name" default="/output/raw_image" />
+
+    <node pkg="image_transport_decompressor" exec="image_transport_decompressor_node" name="image_transport_decompressor_node">
+      <remap from="input" to="$(var input_topic_name)" />
+      <remap from="output" to="$(var output_topic_name)" />
+    </node>
+  </launch>
diff --git a/sensing/image_transport_decompressor/package.xml b/sensing/image_transport_decompressor/package.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>image_transport_decompressor</name>
+  <version>0.0.0</version>
+  <description>The image_transport_decompressor package</description>
+
+  <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
+  <author email="yukihiro.saito@tier4.jp">Yukihiro Saito</author>
+  <license>Apache License 2.0</license>
+  <author>Patrick Mihelich</author>
+  <author>Julius Kammerl</author>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>cv_bridge</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+
+</package>
diff --git a/sensing/image_transport_decompressor/src/image_transport_decompressor.cpp 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 <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <sensor_msgs/image_encodings.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include <limits>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+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<sensor_msgs::msg::CompressedImage>(
+    "~/input/compressed_image", rclcpp::SensorDataQoS(),
+    std::bind(&ImageTransportDecompressor::onCompressedImage, this, std::placeholders::_1));
+  raw_image_pub_ =
+    create_publisher<sensor_msgs::msg::Image>("~/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<sensor_msgs::msg::Image>(*cv_ptr->toImageMsg());
+    raw_image_pub_->publish(std::move(image_ptr));
+  }
+}
+}  // namespace image_preprocessor
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(image_preprocessor::ImageTransportDecompressor)