add images

src/image_tools/CHANGELOG.rst

@@ -0,0 +1,203 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package image_tools
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.28.0 (2023-04-27)
+-------------------
+
+0.27.0 (2023-04-13)
+-------------------
+
+0.26.0 (2023-04-11)
+-------------------
+
+0.25.0 (2023-03-01)
+-------------------
+
+0.24.1 (2023-02-24)
+-------------------
+
+0.24.0 (2023-02-14)
+-------------------
+* Added README.md for image_tools - [Clean] (`#596 <https://github.com/ros2/demos/issues/596>`_)
+* Update the demos to C++17. (`#594 <https://github.com/ros2/demos/issues/594>`_)
+* [rolling] Update maintainers - 2022-11-07 (`#589 <https://github.com/ros2/demos/issues/589>`_)
+* Contributors: Audrow Nash, Chris Lalancette, Gary Bey
+
+0.23.0 (2022-11-02)
+-------------------
+
+0.22.0 (2022-09-13)
+-------------------
+
+0.21.0 (2022-04-29)
+-------------------
+
+0.20.1 (2022-04-08)
+-------------------
+
+0.20.0 (2022-03-01)
+-------------------
+* Install includes to include/${PROJECT_NAME} (`#548 <https://github.com/ros2/demos/issues/548>`_)
+* Contributors: Shane Loretz
+
+0.19.0 (2022-01-14)
+-------------------
+* Fix include order and relative paths for cpplint (`#551 <https://github.com/ros2/demos/issues/551>`_)
+* Reduce the number of OpenCV libraries image_tools links against. (`#549 <https://github.com/ros2/demos/issues/549>`_)
+* Adds copy constructor and assignment operator to ROSCvMatContainer (`#546 <https://github.com/ros2/demos/issues/546>`_)
+* Contributors: Chris Lalancette, Gonzo, Jacob Perron
+
+0.18.0 (2021-12-17)
+-------------------
+* Fixes for uncrustify 0.72 (`#545 <https://github.com/ros2/demos/issues/545>`_)
+* Update maintainers to Audrow Nash and Michael Jeronimo (`#543 <https://github.com/ros2/demos/issues/543>`_)
+* Additional fixes for documentation in demos. (`#538 <https://github.com/ros2/demos/issues/538>`_)
+* Contributors: Audrow Nash, Chris Lalancette
+
+0.17.0 (2021-10-18)
+-------------------
+* Fixing deprecated subscriber callback warnings (`#532 <https://github.com/ros2/demos/issues/532>`_)
+* Contributors: Abrar Rahman Protyasha
+
+0.16.0 (2021-08-11)
+-------------------
+* ambigulity: unknown type name 'nullptr_t' (`#528 <https://github.com/ros2/demos/issues/528>`_)
+* Add type masquerading demos (`#482 <https://github.com/ros2/demos/issues/482>`_)
+* Contributors: Audrow Nash, William Woodall, xwnb
+
+0.15.0 (2021-05-14)
+-------------------
+* Add support for visualizing yuv422 (`#499 <https://github.com/ros2/demos/issues/499>`_)
+* Contributors: joshua-qnx
+
+0.14.2 (2021-04-26)
+-------------------
+
+0.14.1 (2021-04-19)
+-------------------
+
+0.14.0 (2021-04-06)
+-------------------
+
+0.13.0 (2021-03-25)
+-------------------
+
+0.12.1 (2021-03-18)
+-------------------
+
+0.12.0 (2021-01-25)
+-------------------
+
+0.11.0 (2020-12-10)
+-------------------
+* Initialize time stamp for published image messages (`#475 <https://github.com/ros2/demos/issues/475>`_)
+* Update the package.xml files with the latest Open Robotics maintainers (`#466 <https://github.com/ros2/demos/issues/466>`_)
+* Added more parameters for camera topic examples (`#465 <https://github.com/ros2/demos/issues/465>`_)
+* Contributors: Jacob Perron, Michael Jeronimo
+
+0.10.1 (2020-09-21)
+-------------------
+
+0.10.0 (2020-06-17)
+-------------------
+
+0.9.3 (2020-06-01)
+------------------
+
+0.9.2 (2020-05-26)
+------------------
+
+0.9.1 (2020-05-12)
+------------------
+
+0.9.0 (2020-04-30)
+------------------
+* Replace deprecated launch_ros usage (`#437 <https://github.com/ros2/demos/issues/437>`_)
+* Fix frame_id  (`#433 <https://github.com/ros2/demos/issues/433>`_)
+* Update launch_ros action usage (`#431 <https://github.com/ros2/demos/issues/431>`_)
+* code style only: wrap after open parenthesis if not in one line (`#429 <https://github.com/ros2/demos/issues/429>`_)
+* Contributors: Dirk Thomas, Gonzo, Jacob Perron
+
+0.8.4 (2019-11-19)
+------------------
+
+0.8.3 (2019-11-11)
+------------------
+* image_tools should start with reliable policy (`#411 <https://github.com/ros2/demos/issues/411>`_)
+* Contributors: Shane Loretz
+
+0.8.2 (2019-11-08)
+------------------
+
+0.8.1 (2019-10-23)
+------------------
+* Fix burguer mode parameter typo in help text (`#406 <https://github.com/ros2/demos/issues/406>`_)
+* Replace ready_fn with ReadyToTest action (`#404 <https://github.com/ros2/demos/issues/404>`_)
+* [image_tools] Use ROS parameters instead of regular CLI arguments (`#398 <https://github.com/ros2/demos/issues/398>`_)
+* Contributors: Brian Marchi, Jacob Perron, Peter Baughman
+
+0.8.0 (2019-09-26)
+------------------
+* Adding visibility macros to demos (`#381 <https://github.com/ros2/demos/issues/381>`_)
+* Demos using composition (`#375 <https://github.com/ros2/demos/issues/375>`_)
+* Contributors: Siddharth Kucheria
+
+0.7.6 (2019-05-30)
+------------------
+
+0.7.5 (2019-05-29)
+------------------
+* Remove debugging prints from showimage. (`#358 <https://github.com/ros2/demos/issues/358>`_)
+* Contributors: Chris Lalancette
+
+0.7.4 (2019-05-20)
+------------------
+
+0.7.3 (2019-05-10)
+------------------
+
+0.7.2 (2019-05-08)
+------------------
+* changes to avoid deprecated API's (`#332 <https://github.com/ros2/demos/issues/332>`_)
+* Corrected publish calls with shared_ptr signature (`#327 <https://github.com/ros2/demos/issues/327>`_)
+* Migrate launch tests to new launch_testing features & API (`#318 <https://github.com/ros2/demos/issues/318>`_)
+* Contributors: Michel Hidalgo, William Woodall, ivanpauno
+
+0.7.1 (2019-04-26)
+------------------
+* Removed support for OpenCV 2. (`#322 <https://github.com/ros2/demos/issues/322>`_)
+* Contributors: Jacob Perron
+
+0.7.0 (2019-04-14)
+------------------
+* Added launch along with launch_testing as test dependencies. (`#313 <https://github.com/ros2/demos/issues/313>`_)
+* Contributors: Michel Hidalgo
+
+0.6.2 (2019-01-15)
+------------------
+* Updated to support OpenCV 2, 3 and 4 (`#307 <https://github.com/ros2/demos/issues/307>`_)
+* Updated for OpenCV v4.0 compatibility (`#306 <https://github.com/ros2/demos/issues/306>`_)
+* Updated to show freq parameter on help only when necessary (`#296 <https://github.com/ros2/demos/issues/296>`_)
+* Contributors: Gonzo, Jacob Perron
+
+0.6.1 (2018-12-13)
+------------------
+
+0.6.0 (2018-12-07)
+------------------
+* Updated to prevent frame going out of scope when converting RGB -> BGR (`#288 <https://github.com/ros2/demos/issues/288>`_)
+* Added semicolons to all RCLCPP and RCUTILS macros. (`#278 <https://github.com/ros2/demos/issues/278>`_)
+* Updated to keep only the last sample in the image tools by default. (`#238 <https://github.com/ros2/demos/issues/238>`_)
+* Contributors: Chris Lalancette, sgvandijk
+
+0.5.1 (2018-06-28)
+------------------
+
+0.5.0 (2018-06-27)
+------------------
+* Fixed linting errors in ``burger.cpp`` (`#260 <https://github.com/ros2/demos/issues/260>`_)
+  * Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>
+* Fixed a bug that occurred when the resolution was less than the bugger. (`#258 <https://github.com/ros2/demos/issues/258>`_)
+* Updated launch files to account for the "old launch" getting renamespaced as ``launch`` -> ``launch.legacy``. (`#239 <https://github.com/ros2/demos/issues/239>`_)
+* Contributors: Chris Lalancette, Dirk Thomas, William Woodall

src/image_tools/CMakeLists.txt

@@ -0,0 +1,98 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(image_tools)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(OpenCV REQUIRED COMPONENTS core highgui imgcodecs imgproc videoio)
+
+add_library(${PROJECT_NAME} SHARED
+  src/burger.cpp
+  src/cam2image.cpp
+  src/cv_mat_sensor_msgs_image_type_adapter.cpp
+  src/showimage.cpp
+)
+target_compile_definitions(${PROJECT_NAME}
+  PRIVATE "IMAGE_TOOLS_BUILDING_DLL")
+target_include_directories(${PROJECT_NAME} PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(${PROJECT_NAME}
+  rclcpp::rclcpp
+  ${sensor_msgs_TARGETS}
+  ${std_msgs_TARGETS}
+  rclcpp_components::component
+  ${OpenCV_LIBS})
+rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "image_tools::Cam2Image" EXECUTABLE cam2image)
+rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "image_tools::ShowImage" EXECUTABLE showimage)
+
+install(
+  TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
+
+install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
+
+# TODO(sloretz) stop exporting old-style CMake variables in the future
+ament_export_libraries(${PROJECT_NAME})
+
+ament_export_targets(${PROJECT_NAME})
+
+ament_export_dependencies(rclcpp_components)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_pytest REQUIRED)
+  find_package(launch_testing_ament_cmake REQUIRED)
+  find_package(rmw_implementation_cmake REQUIRED)
+
+  # These are the regex's for validating the output of the executables.
+  set(RCLCPP_DEMO_SHOWIMAGE_EXPECTED_OUTPUT "${CMAKE_CURRENT_SOURCE_DIR}/test/showimage")
+  set(RCLCPP_DEMO_CAM2IMAGE_EXPECTED_OUTPUT "${CMAKE_CURRENT_SOURCE_DIR}/test/cam2image")
+
+  macro(testing_targets)
+    set(RCLCPP_DEMO_CAM2IMAGE_EXECUTABLE $<TARGET_FILE:cam2image>)
+    set(RCLCPP_DEMO_SHOWIMAGE_EXECUTABLE $<TARGET_FILE:showimage>)
+
+    configure_file(
+      test/test_executables_demo.py.in
+      test_showimage_cam2image${target_suffix}.py.genexp
+      @ONLY
+    )
+
+    file(GENERATE
+      OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test_showimage_cam2image${target_suffix}_$<CONFIG>.py"
+      INPUT "${CMAKE_CURRENT_BINARY_DIR}/test_showimage_cam2image${target_suffix}.py.genexp"
+    )
+
+    add_launch_test(
+      "${CMAKE_CURRENT_BINARY_DIR}/test_showimage_cam2image${target_suffix}_$<CONFIG>.py"
+      TARGET test_showimage_cam2image${target_suffix}
+      ENV
+      RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation}
+      RMW_IMPLEMENTATION=${rmw_implementation}
+      TIMEOUT 30
+    )
+  endmacro()
+
+  call_for_each_rmw_implementation(testing_targets)
+
+endif()
+
+ament_package()

src/image_tools/Doxyfile

@@ -0,0 +1,21 @@
+# All settings not listed here will use the Doxygen default values.
+
+PROJECT_NAME           = "image_tools"
+PROJECT_NUMBER         = master
+PROJECT_BRIEF          = "Tools to capture and play back images to and from DDS subscriptions and publications."
+
+INPUT                  = ./include
+RECURSIVE              = YES
+OUTPUT_DIRECTORY       = docs_output
+
+EXTRACT_ALL            = YES
+SORT_MEMBER_DOCS       = NO
+
+GENERATE_LATEX         = NO
+GENERATE_XML           = YES
+EXCLUDE_SYMBOLS        = detail details
+
+ENABLE_PREPROCESSING   = YES
+MACRO_EXPANSION        = YES
+EXPAND_ONLY_PREDEF     = YES
+PREDEFINED             += IMAGE_TOOLS_PUBLIC

src/image_tools/README.md

@@ -0,0 +1,36 @@
+## **What Is This?**
+
+This demo provides simple utilities to connect to a workstation default camera device and display it in a window like below:
+
+![](img/result.png)
+
+## **Build**
+
+```bash
+colcon build --packages-select image_tools
+```
+
+## **Run**
+
+In `image_tools` ROS 2 package, two executables are provided, namely `cam2image` and `showimage` with different functions.
+
+## **1 - cam2image**
+Running this executable connects to your workstation's default camera device's video stream and publishes the images on '/image' and '/flipimage' topics using a ROS 2 publisher.
+
+```bash
+# Open new terminal
+ros2 run image_tools cam2image
+```
+
+Note that `cam2image` provides many useful command-line options. Run `ros2 run image_tools cam2image --help` to see the list of options available.
+>
+> Eg. If a camera device is not available, run `ros2 run image_tools cam2image --ros-args -p burger_mode:=true`.
+
+## **2 - showimage**
+Running this executable creates a ROS 2 node, `showimage`, which subscribes to the `sensor_msg/msg/Image` topic, `/image` and displays the images in a window.
+
+```bash
+# Open new terminal
+# Run showimage ROS 2 node to display the cam2image sensor_msg/msg/Image messages.
+ros2 run image_tools showimage
+```

src/image_tools/include/image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp

@@ -0,0 +1,272 @@
+// Copyright 2021 Open Source Robotics Foundation, 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_TOOLS__CV_MAT_SENSOR_MSGS_IMAGE_TYPE_ADAPTER_HPP_
+#define IMAGE_TOOLS__CV_MAT_SENSOR_MSGS_IMAGE_TYPE_ADAPTER_HPP_
+
+#include <cstddef>
+#include <memory>
+#include <variant>  // NOLINT[build/include_order]
+
+#include "opencv2/core/mat.hpp"
+
+#include "rclcpp/type_adapter.hpp"
+#include "sensor_msgs/msg/image.hpp"
+
+#include "image_tools/visibility_control.h"
+
+namespace image_tools
+{
+namespace detail
+{
+// TODO(audrow): Replace with std::endian when C++ 20 is available
+// https://en.cppreference.com/w/cpp/types/endian
+enum class endian
+{
+#ifdef _WIN32
+  little = 0,
+  big    = 1,
+  native = little
+#else
+  little = __ORDER_LITTLE_ENDIAN__,
+  big    = __ORDER_BIG_ENDIAN__,
+  native = __BYTE_ORDER__
+#endif
+};
+
+}  // namespace detail
+
+
+// TODO(wjwwood): make this as a contribution to vision_opencv's cv_bridge package.
+//   Specifically the CvImage class, which is this is most similar to.
+/// A potentially owning, potentially non-owning, container of a cv::Mat and ROS header.
+/**
+ * The two main use cases for this are publishing user controlled data, and
+ * recieving data from the middleware that may have been a ROS message
+ * originally or may have been an cv::Mat originally.
+ *
+ * In the first case, publishing user owned data, the user will want to provide
+ * their own cv::Mat.
+ * The cv::Mat may own the data or it may not, so in the latter case, it is up
+ * to the user to ensure the data the cv::Mat points to remains valid as long
+ * as the middleware needs it.
+ *
+ * In the second case, receiving data from the middleware, the middleware will
+ * either give a new ROSCvMatContainer which owns a sensor_msgs::msg::Image or
+ * it will give a ROSCvMatContainer that was previously published by the user
+ * (in the case of intra-process communication).
+ * If the container owns the sensor_msgs::msg::Image, then the cv::Mat will just
+ * reference data field of this message, so the container needs to be kept.
+ * If the container was published by the user it may or may not own the data
+ * and the cv::Mat it contains may or may not own the data.
+ *
+ * For these reasons, it is advisable to use cv::Mat::clone() if you intend to
+ * copy the cv::Mat and let this container go.
+ *
+ * For more details about the ownership behavior of cv::Mat see documentation
+ * for these methods of cv::Mat:
+ *
+ *   - template<typename _Tp > cv::Mat::Mat(const std::vector<_Tp> &, bool)
+ *   - Mat & cv::Mat::operator=(const Mat &)
+ *   - void cv::Mat::addref()
+ *   - void cv::Mat::release()
+ *
+ */
+class ROSCvMatContainer
+{
+  static constexpr bool is_bigendian_system = detail::endian::native == detail::endian::big;
+
+public:
+  using SensorMsgsImageStorageType = std::variant<
+    std::nullptr_t,
+    std::unique_ptr<sensor_msgs::msg::Image>,
+    std::shared_ptr<sensor_msgs::msg::Image>
+  >;
+
+  IMAGE_TOOLS_PUBLIC
+  ROSCvMatContainer() = default;
+
+  IMAGE_TOOLS_PUBLIC
+  explicit ROSCvMatContainer(const ROSCvMatContainer & other)
+  : header_(other.header_), frame_(other.frame_.clone()), is_bigendian_(other.is_bigendian_)
+  {
+    if (std::holds_alternative<std::shared_ptr<sensor_msgs::msg::Image>>(other.storage_)) {
+      storage_ = std::get<std::shared_ptr<sensor_msgs::msg::Image>>(other.storage_);
+    } else if (std::holds_alternative<std::unique_ptr<sensor_msgs::msg::Image>>(other.storage_)) {
+      storage_ = std::make_unique<sensor_msgs::msg::Image>(
+        *std::get<std::unique_ptr<sensor_msgs::msg::Image>>(other.storage_));
+    }
+  }
+
+  IMAGE_TOOLS_PUBLIC
+  ROSCvMatContainer & operator=(const ROSCvMatContainer & other)
+  {
+    if (this != &other) {
+      header_ = other.header_;
+      frame_ = other.frame_.clone();
+      is_bigendian_ = other.is_bigendian_;
+      if (std::holds_alternative<std::shared_ptr<sensor_msgs::msg::Image>>(other.storage_)) {
+        storage_ = std::get<std::shared_ptr<sensor_msgs::msg::Image>>(other.storage_);
+      } else if (std::holds_alternative<std::unique_ptr<sensor_msgs::msg::Image>>(other.storage_)) {
+        storage_ = std::make_unique<sensor_msgs::msg::Image>(
+          *std::get<std::unique_ptr<sensor_msgs::msg::Image>>(other.storage_));
+      } else if (std::holds_alternative<std::nullptr_t>(other.storage_)) {
+        storage_ = nullptr;
+      }
+    }
+    return *this;
+  }
+
+  /// Store an owning pointer to a sensor_msg::msg::Image, and create a cv::Mat that references it.
+  IMAGE_TOOLS_PUBLIC
+  explicit ROSCvMatContainer(std::unique_ptr<sensor_msgs::msg::Image> unique_sensor_msgs_image);
+
+  /// Store an owning pointer to a sensor_msg::msg::Image, and create a cv::Mat that references it.
+  IMAGE_TOOLS_PUBLIC
+  explicit ROSCvMatContainer(std::shared_ptr<sensor_msgs::msg::Image> shared_sensor_msgs_image);
+
+  /// Shallow copy the given cv::Mat into this class, but do not own the data directly.
+  IMAGE_TOOLS_PUBLIC
+  ROSCvMatContainer(
+    const cv::Mat & mat_frame,
+    const std_msgs::msg::Header & header,
+    bool is_bigendian = is_bigendian_system);
+
+  /// 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);
+
+  /// Copy the sensor_msgs::msg::Image into this contain and create a cv::Mat that references it.
+  IMAGE_TOOLS_PUBLIC
+  explicit ROSCvMatContainer(const sensor_msgs::msg::Image & sensor_msgs_image);
+
+  /// Return true if this class owns the data the cv_mat references.
+  /**
+   * Note that this does not check if the cv::Mat owns its own data, only if
+   * this class owns a sensor_msgs::msg::Image that the cv::Mat references.
+   */
+  IMAGE_TOOLS_PUBLIC
+  bool
+  is_owning() const;
+
+  /// Const access the cv::Mat in this class.
+  IMAGE_TOOLS_PUBLIC
+  const cv::Mat &
+  cv_mat() const;
+
+  /// Get a shallow copy of the cv::Mat that is in this class.
+  /**
+   * Note that if you want to let this container go out of scope you should
+   * make a deep copy with cv::Mat::clone() beforehand.
+   */
+  IMAGE_TOOLS_PUBLIC
+  cv::Mat
+  cv_mat();
+
+  /// Const access the ROS Header.
+  IMAGE_TOOLS_PUBLIC
+  const std_msgs::msg::Header &
+  header() const;
+
+  /// Access the ROS Header.
+  IMAGE_TOOLS_PUBLIC
+  std_msgs::msg::Header &
+  header();
+
+  /// Get shared const pointer to the sensor_msgs::msg::Image if available, otherwise nullptr.
+  IMAGE_TOOLS_PUBLIC
+  std::shared_ptr<const sensor_msgs::msg::Image>
+  get_sensor_msgs_msg_image_pointer() const;
+
+  /// Get copy as a unique pointer to the sensor_msgs::msg::Image.
+  IMAGE_TOOLS_PUBLIC
+  std::unique_ptr<sensor_msgs::msg::Image>
+  get_sensor_msgs_msg_image_pointer_copy() const;
+
+  /// Get a copy of the image as a sensor_msgs::msg::Image.
+  IMAGE_TOOLS_PUBLIC
+  sensor_msgs::msg::Image
+  get_sensor_msgs_msg_image_copy() const;
+
+  /// Get a copy of the image as a sensor_msgs::msg::Image.
+  IMAGE_TOOLS_PUBLIC
+  void
+  get_sensor_msgs_msg_image_copy(sensor_msgs::msg::Image & sensor_msgs_image) const;
+
+  /// Return true if the data is stored in big endian, otherwise return false.
+  IMAGE_TOOLS_PUBLIC
+  bool
+  is_bigendian() const;
+
+private:
+  std_msgs::msg::Header header_;
+  cv::Mat frame_;
+  SensorMsgsImageStorageType storage_;
+  bool is_bigendian_;
+};
+
+}  // namespace image_tools
+
+template<>
+struct rclcpp::TypeAdapter<image_tools::ROSCvMatContainer, sensor_msgs::msg::Image>
+{
+  using is_specialized = std::true_type;
+  using custom_type = image_tools::ROSCvMatContainer;
+  using ros_message_type = sensor_msgs::msg::Image;
+
+  static
+  void
+  convert_to_ros_message(
+    const custom_type & source,
+    ros_message_type & destination)
+  {
+    destination.height = source.cv_mat().rows;
+    destination.width = source.cv_mat().cols;
+    switch (source.cv_mat().type()) {
+      case CV_8UC1:
+        destination.encoding = "mono8";
+        break;
+      case CV_8UC3:
+        destination.encoding = "bgr8";
+        break;
+      case CV_16SC1:
+        destination.encoding = "mono16";
+        break;
+      case CV_8UC4:
+        destination.encoding = "rgba8";
+        break;
+      default:
+        throw std::runtime_error("unsupported encoding type");
+    }
+    destination.step = static_cast<sensor_msgs::msg::Image::_step_type>(source.cv_mat().step);
+    size_t size = source.cv_mat().step * source.cv_mat().rows;
+    destination.data.resize(size);
+    memcpy(&destination.data[0], source.cv_mat().data, size);
+    destination.header = source.header();
+  }
+
+  static
+  void
+  convert_to_custom(
+    const ros_message_type & source,
+    custom_type & destination)
+  {
+    destination = image_tools::ROSCvMatContainer(source);
+  }
+};
+
+#endif  // IMAGE_TOOLS__CV_MAT_SENSOR_MSGS_IMAGE_TYPE_ADAPTER_HPP_

src/image_tools/include/image_tools/visibility_control.h

@@ -0,0 +1,58 @@
+// Copyright 2016 Open Source Robotics Foundation, 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_TOOLS__VISIBILITY_CONTROL_H_
+#define IMAGE_TOOLS__VISIBILITY_CONTROL_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define IMAGE_TOOLS_EXPORT __attribute__ ((dllexport))
+    #define IMAGE_TOOLS_IMPORT __attribute__ ((dllimport))
+  #else
+    #define IMAGE_TOOLS_EXPORT __declspec(dllexport)
+    #define IMAGE_TOOLS_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef IMAGE_TOOLS_BUILDING_DLL
+    #define IMAGE_TOOLS_PUBLIC IMAGE_TOOLS_EXPORT
+  #else
+    #define IMAGE_TOOLS_PUBLIC IMAGE_TOOLS_IMPORT
+  #endif
+  #define IMAGE_TOOLS_PUBLIC_TYPE IMAGE_TOOLS_PUBLIC
+  #define IMAGE_TOOLS_LOCAL
+#else
+  #define IMAGE_TOOLS_EXPORT __attribute__ ((visibility("default")))
+  #define IMAGE_TOOLS_IMPORT
+  #if __GNUC__ >= 4
+    #define IMAGE_TOOLS_PUBLIC __attribute__ ((visibility("default")))
+    #define IMAGE_TOOLS_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define IMAGE_TOOLS_PUBLIC
+    #define IMAGE_TOOLS_LOCAL
+  #endif
+  #define IMAGE_TOOLS_PUBLIC_TYPE
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // IMAGE_TOOLS__VISIBILITY_CONTROL_H_

src/image_tools/package.xml

@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>image_tools</name>
+  <version>0.28.0</version>
+  <description>Tools to capture and play back images to and from DDS subscriptions and publications.</description>
+
+  <maintainer email="aditya.pande@openrobotics.org">Aditya Pande</maintainer>
+  <maintainer email="audrow@openrobotics.org">Audrow Nash</maintainer>
+  <maintainer email="michael.jeronimo@openrobotics.org">Michael Jeronimo</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <author email="dthomas@osrfoundation.org">Dirk Thomas</author>
+  <author email="mabel@openrobotics.org">Mabel Zhang</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>libopencv-dev</build_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_depend>rclcpp_components</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <exec_depend>libopencv-dev</exec_depend>
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclcpp_components</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>launch_ros</test_depend>
+  <test_depend>launch_testing</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>launch_testing_ros</test_depend>
+  <test_depend>rmw_implementation_cmake</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/image_tools/src/burger.cpp

@@ -0,0 +1,163 @@
+// Copyright 2016 Open Source Robotics Foundation, 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.
+
+#ifdef _WIN32
+#include <Windows.h>  // For GetTickCount().
+#endif
+
+#include <cstring>
+#include <cstdio>
+#include <string>
+#include <vector>
+
+#include "burger.hpp"
+
+#include "opencv2/core.hpp"
+#include "opencv2/core/mat.hpp"
+#include "opencv2/core/types.hpp"
+#include "opencv2/imgcodecs.hpp"
+#include "opencv2/imgproc.hpp"
+
+using burger::Burger;  // i've always wanted to write that
+
+#ifndef _WIN32
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wchar-subscripts"
+#endif
+// here lies the world's slowest portable base64 decoder
+void decode_base64(const char * cstr, std::vector<uint8_t> & out)
+{
+  int len = static_cast<int>(strlen(cstr));
+  if (len < 2) {
+    return;  // would have to think too hard about trivial inputs
+  }
+  out.resize(len * 3 / 4);  // deal with padding bytes later
+  uint8_t base64_map[256] = {0};
+  for (uint8_t i = 'A'; i <= 'Z'; i++) {
+    base64_map[i] = i - 'A';
+  }
+  for (uint8_t i = 'a'; i <= 'z'; i++) {
+    base64_map[i] = i - 'a' + 26;
+  }
+  for (uint8_t i = '0'; i <= '9'; i++) {
+    base64_map[i] = i - '0' + 52;
+  }
+  base64_map['+'] = 62;
+  base64_map['/'] = 63;
+  int ridx = 0, widx = 0;
+  for (ridx = 0; ridx < len; ridx += 4) {
+    // we expand each group of 4 code bytes into 3 output bytes
+    uint32_t block = 0;
+    block = (base64_map[cstr[ridx]] << 18) |
+      (base64_map[cstr[ridx + 1]] << 12) |
+      (base64_map[cstr[ridx + 2]] << 6) |
+      (base64_map[cstr[ridx + 3]] << 0);
+    out[widx++] = (block >> 16) & 0xff;
+    out[widx++] = (block >> 8) & 0xff;
+    out[widx++] = (block >> 0) & 0xff;
+  }
+  // fix padding now. (these branches are untested so they're probably wrong)
+  if (cstr[len - 1] == '=' && cstr[len - 2] == '=') {
+    // there were two padding bytes. remove the two last output bytes
+    out.pop_back();
+    out.pop_back();
+  } else if (cstr[len - 1] == '=') {
+    // there was only one padding byte. remove the last output byte.
+    out.pop_back();
+  }
+}
+#ifndef _WIN32
+#pragma GCC diagnostic pop
+#endif
+
+Burger::Burger()
+{
+  size_t burger_size = strlen(BURGER);
+  std::vector<uint8_t> burger_png;
+  burger_png.resize(burger_size);
+  decode_base64(BURGER, burger_png);
+  burger_template = cv::imdecode(burger_png, cv::ImreadModes::IMREAD_COLOR);
+  cv::floodFill(burger_template, cv::Point(1, 1), CV_RGB(1, 1, 1));
+  cv::compare(burger_template, 1, burger_mask, cv::CMP_NE);
+#ifndef _WIN32
+  srand(time(NULL));
+#else
+  srand(GetTickCount());
+#endif
+}
+
+cv::Mat & Burger::render_burger(size_t width, size_t height)
+{
+  int width_i = static_cast<int>(width);
+  int height_i = static_cast<int>(height);
+  if (width_i < burger_template.size().width || height_i < burger_template.size().height) {
+    std::string msg = "Target resolution must be at least the burger size (" +
+      std::to_string(burger_template.size().width) + " x " +
+      std::to_string(burger_template.size().height) + ")";
+    throw std::runtime_error(msg.c_str());
+  }
+  if (burger_buf.size().width != width_i || burger_buf.size().height != height_i) {
+    int num_burgers = rand() % 10 + 2;  // NOLINT
+    x.resize(num_burgers);
+    y.resize(num_burgers);
+    x_inc.resize(num_burgers);
+    y_inc.resize(num_burgers);
+    for (int b = 0; b < num_burgers; b++) {
+      if (width - burger_template.size().width > 0) {
+        x[b] = rand() % (width - burger_template.size().width);  // NOLINT
+      } else {
+        x[b] = 0;
+      }
+      if (height - burger_template.size().height > 0) {
+        y[b] = rand() % (height - burger_template.size().height);  // NOLINT
+      } else {
+        y[b] = 0;
+      }
+      x_inc[b] = rand() % 3 + 1;  // NOLINT
+      y_inc[b] = rand() % 3 + 1;  // NOLINT
+    }
+    burger_buf = cv::Mat(height_i, width_i, CV_8UC3);
+  }
+  burger_buf = cv::Scalar(0, 0, 0);
+  for (int b = 0; b < static_cast<int>(x.size()); b++) {
+    burger_template.copyTo(
+      burger_buf(
+        cv::Rect(
+          x[b],
+          y[b],
+          burger_template.size().height,
+          burger_template.size().width
+        )), burger_mask);
+    x[b] += x_inc[b];
+    y[b] += y_inc[b];
+    // bounce as needed
+    if (x[b] < 0 || x[b] > width_i - burger_template.size().width - 1) {
+      x_inc[b] *= -1;
+      if (x[b] < 0) {
+        x[b] = 0;
+      } else {
+        x[b] = width_i - burger_template.size().width;
+      }
+    }
+    if (y[b] < 0 || y[b] > height_i - burger_template.size().height - 1) {
+      y_inc[b] *= -1;
+      if (y[b] < 0) {
+        y[b] = 0;
+      } else {
+        y[b] = height_i - burger_template.size().height;
+      }
+    }
+  }
+  return burger_buf;
+}

src/image_tools/src/cam2image.cpp

@@ -0,0 +1,281 @@
+// Copyright 2015 Open Source Robotics Foundation, 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.
+
+#include <chrono>
+#include <memory>
+#include <sstream>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "opencv2/core/mat.hpp"
+#include "opencv2/core.hpp"
+#include "opencv2/highgui.hpp"
+#include "opencv2/videoio.hpp"
+
+#include "rcl_interfaces/msg/parameter_descriptor.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+#include "std_msgs/msg/bool.hpp"
+
+#include "image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp"
+#include "image_tools/visibility_control.h"
+
+#include "./burger.hpp"
+#include "./policy_maps.hpp"
+
+RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(
+  image_tools::ROSCvMatContainer,
+  sensor_msgs::msg::Image);
+
+namespace image_tools
+{
+class Cam2Image : public rclcpp::Node
+{
+public:
+  IMAGE_TOOLS_PUBLIC
+  explicit Cam2Image(const rclcpp::NodeOptions & options)
+  : Node("cam2image", options),
+    is_flipped_(false),
+    publish_number_(1u)
+  {
+    setvbuf(stdout, NULL, _IONBF, BUFSIZ);
+    // Do not execute if a --help option was provided
+    if (help(options.arguments())) {
+      // TODO(jacobperron): Replace with a mechanism for a node to "unload" itself
+      // from a container.
+      exit(0);
+    }
+    parse_parameters();
+    initialize();
+  }
+
+private:
+  IMAGE_TOOLS_LOCAL
+  void initialize()
+  {
+    auto qos = rclcpp::QoS(
+      rclcpp::QoSInitialization(
+        // The history policy determines how messages are saved until taken by
+        // the reader.
+        // KEEP_ALL saves all messages until they are taken.
+        // KEEP_LAST enforces a limit on the number of messages that are saved,
+        // specified by the "depth" parameter.
+        history_policy_,
+        // Depth represents how many messages to store in history when the
+        // history policy is KEEP_LAST.
+        depth_
+    ));
+    // The reliability policy can be reliable, meaning that the underlying transport layer will try
+    // ensure that every message gets received in order, or best effort, meaning that the transport
+    // makes no guarantees about the order or reliability of delivery.
+    qos.reliability(reliability_policy_);
+    pub_ = create_publisher<image_tools::ROSCvMatContainer>("image", qos);
+
+    // Subscribe to a message that will toggle flipping or not flipping, and manage the state in a
+    // callback
+    auto callback = [this](std_msgs::msg::Bool::ConstSharedPtr msg) -> void
+      {
+        this->is_flipped_ = msg->data;
+        RCLCPP_INFO(this->get_logger(), "Set flip mode to: %s", this->is_flipped_ ? "on" : "off");
+      };
+    // Set the QoS profile for the subscription to the flip message.
+    sub_ = create_subscription<std_msgs::msg::Bool>(
+      "flip_image", rclcpp::SensorDataQoS(), callback);
+
+    if (!burger_mode_) {
+      // Initialize OpenCV video capture stream.
+      cap.open(device_id_);
+
+      // Set the width and height based on command line arguments.
+      cap.set(cv::CAP_PROP_FRAME_WIDTH, static_cast<double>(width_));
+      cap.set(cv::CAP_PROP_FRAME_HEIGHT, static_cast<double>(height_));
+      if (!cap.isOpened()) {
+        RCLCPP_ERROR(this->get_logger(), "Could not open video stream");
+        throw std::runtime_error("Could not open video stream");
+      }
+    }
+
+    // Start main timer loop
+    timer_ = this->create_wall_timer(
+      std::chrono::milliseconds(static_cast<int>(1000.0 / freq_)),
+      std::bind(&Cam2Image::timerCallback, this));
+  }
+
+  /// Publish camera, or burger, image.
+  IMAGE_TOOLS_LOCAL
+  void timerCallback()
+  {
+    cv::Mat frame;
+
+    // Initialize a shared pointer to an Image message.
+    // Get the frame from the video capture.
+    if (burger_mode_) {
+      frame = burger_cap.render_burger(width_, height_);
+    } else {
+      cap >> frame;
+    }
+
+    // If no frame was grabbed, return early
+    if (frame.empty()) {
+      return;
+    }
+
+    // Conditionally flip the image
+    if (is_flipped_) {
+      cv::flip(frame, frame, 1);
+    }
+
+    // Conditionally show image
+    if (show_camera_) {
+      // Show the image in a window called "cam2image".
+      cv::imshow("cam2image", frame);
+      // Draw the image to the screen and wait 1 millisecond.
+      cv::waitKey(1);
+    }
+
+    std_msgs::msg::Header header;
+    header.frame_id = frame_id_;
+    header.stamp = this->now();
+    image_tools::ROSCvMatContainer container(frame, header);
+
+    // Publish the image message and increment the publish_number_.
+    RCLCPP_INFO(get_logger(), "Publishing image #%zd", publish_number_++);
+    pub_->publish(std::move(container));
+  }
+
+  IMAGE_TOOLS_LOCAL
+  bool help(const std::vector<std::string> & args)
+  {
+    if (std::find(args.begin(), args.end(), "--help") != args.end() ||
+      std::find(args.begin(), args.end(), "-h") != args.end())
+    {
+      std::stringstream ss;
+      ss << "Usage: cam2image [-h] [--ros-args [-p param:=value] ...]" << std::endl;
+      ss << "Publish images from a camera stream." << std::endl;
+      ss << "Example: ros2 run image_tools cam2image --ros-args -p reliability:=best_effort";
+      ss << std::endl << std::endl;
+      ss << "Options:" << std::endl;
+      ss << "  -h, --help\tDisplay this help message and exit";
+      ss << std::endl << std::endl;
+      ss << "Parameters:" << std::endl;
+      ss << "  reliability\tReliability QoS setting. Either 'reliable' (default) or 'best_effort'";
+      ss << std::endl;
+      ss << "  history\tHistory QoS setting. Either 'keep_last' (default) or 'keep_all'.";
+      ss << std::endl;
+      ss << "\t\tIf 'keep_last', then up to N samples are stored where N is the depth";
+      ss << std::endl;
+      ss << "  depth\t\tDepth of the publisher queue. Only honored if history QoS is 'keep_last'.";
+      ss << " Default value is 10";
+      ss << std::endl;
+      ss << "  frequency\tPublish frequency in Hz. Default value is 30";
+      ss << std::endl;
+      ss << "  burger_mode\tProduce images of burgers rather than connecting to a camera";
+      ss << std::endl;
+      ss << "  show_camera\tShow camera stream. Either 'true' or 'false' (default)";
+      ss << std::endl;
+      ss << "  device_id\tDevice ID of the camera. 0 (default) selects the default camera device.";
+      ss << std::endl;
+      ss << "  width\t\tWidth component of the camera stream resolution. Default value is 320";
+      ss << std::endl;
+      ss << "  height\tHeight component of the camera stream resolution. Default value is 240";
+      ss << std::endl;
+      ss << "  frame_id\t\tID of the sensor frame. Default value is 'camera_frame'";
+      ss << std::endl << std::endl;
+      ss << "Note: try running v4l2-ctl --list-formats-ext to obtain a list of valid values.";
+      ss << std::endl;
+      std::cout << ss.str();
+      return true;
+    }
+    return false;
+  }
+
+  IMAGE_TOOLS_LOCAL
+  void parse_parameters()
+  {
+    // Parse 'reliability' parameter
+    rcl_interfaces::msg::ParameterDescriptor reliability_desc;
+    reliability_desc.description = "Reliability QoS setting for the image publisher";
+    reliability_desc.additional_constraints = "Must be one of: ";
+    for (auto entry : name_to_reliability_policy_map) {
+      reliability_desc.additional_constraints += entry.first + " ";
+    }
+    const std::string reliability_param = this->declare_parameter(
+      "reliability", "reliable", reliability_desc);
+    auto reliability = name_to_reliability_policy_map.find(reliability_param);
+    if (reliability == name_to_reliability_policy_map.end()) {
+      std::ostringstream oss;
+      oss << "Invalid QoS reliability setting '" << reliability_param << "'";
+      throw std::runtime_error(oss.str());
+    }
+    reliability_policy_ = reliability->second;
+
+    // Parse 'history' parameter
+    rcl_interfaces::msg::ParameterDescriptor history_desc;
+    history_desc.description = "History QoS setting for the image publisher";
+    history_desc.additional_constraints = "Must be one of: ";
+    for (auto entry : name_to_history_policy_map) {
+      history_desc.additional_constraints += entry.first + " ";
+    }
+    const std::string history_param = this->declare_parameter(
+      "history", name_to_history_policy_map.begin()->first, history_desc);
+    auto history = name_to_history_policy_map.find(history_param);
+    if (history == name_to_history_policy_map.end()) {
+      std::ostringstream oss;
+      oss << "Invalid QoS history setting '" << history_param << "'";
+      throw std::runtime_error(oss.str());
+    }
+    history_policy_ = history->second;
+
+    // Declare and get remaining parameters
+    depth_ = this->declare_parameter("depth", 10);
+    freq_ = this->declare_parameter("frequency", 30.0);
+    show_camera_ = this->declare_parameter("show_camera", false);
+    device_id_ = static_cast<int>(this->declare_parameter("device_id", 0));
+    width_ = this->declare_parameter("width", 320);
+    height_ = this->declare_parameter("height", 240);
+    rcl_interfaces::msg::ParameterDescriptor burger_mode_desc;
+    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");
+  }
+
+  cv::VideoCapture cap;
+  burger::Burger burger_cap;
+
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_;
+  rclcpp::Publisher<image_tools::ROSCvMatContainer>::SharedPtr pub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  // ROS parameters
+  bool show_camera_;
+  size_t depth_;
+  double freq_;
+  rmw_qos_reliability_policy_t reliability_policy_;
+  rmw_qos_history_policy_t history_policy_;
+  size_t width_;
+  size_t height_;
+  bool burger_mode_;
+  std::string frame_id_;
+  int device_id_;
+
+  /// If true, will cause the incoming camera image message to flip about the y-axis.
+  bool is_flipped_;
+  /// The number of images published.
+  size_t publish_number_;
+};
+
+}  // namespace image_tools
+
+RCLCPP_COMPONENTS_REGISTER_NODE(image_tools::Cam2Image)

src/image_tools/src/cv_mat_sensor_msgs_image_type_adapter.cpp

@@ -0,0 +1,207 @@
+// Copyright 2021 Open Source Robotics Foundation, 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.
+
+#include <cstddef>
+#include <memory>
+#include <string>
+#include <utility>
+#include <variant>  // NOLINT[build/include_order]
+
+#include "opencv2/core/mat.hpp"
+
+#include "sensor_msgs/msg/image.hpp"
+#include "std_msgs/msg/header.hpp"
+
+#include "image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp"
+
+namespace image_tools
+{
+
+namespace
+{
+int
+encoding2mat_type(const std::string & encoding)
+{
+  if (encoding == "mono8") {
+    return CV_8UC1;
+  } else if (encoding == "bgr8") {
+    return CV_8UC3;
+  } else if (encoding == "mono16") {
+    return CV_16SC1;
+  } else if (encoding == "rgba8") {
+    return CV_8UC4;
+  } else if (encoding == "bgra8") {
+    return CV_8UC4;
+  } else if (encoding == "32FC1") {
+    return CV_32FC1;
+  } else if (encoding == "rgb8") {
+    return CV_8UC3;
+  } else if (encoding == "yuv422") {
+    return CV_8UC2;
+  } else {
+    throw std::runtime_error("Unsupported encoding type");
+  }
+}
+
+template<typename T>
+struct NotNull
+{
+  NotNull(const T * pointer_in, const char * msg)
+  : pointer(pointer_in)
+  {
+    if (pointer == nullptr) {
+      throw std::invalid_argument(msg);
+    }
+  }
+
+  const T * pointer;
+};
+
+}  // namespace
+
+ROSCvMatContainer::ROSCvMatContainer(
+  std::unique_ptr<sensor_msgs::msg::Image> unique_sensor_msgs_image)
+: header_(NotNull(
+      unique_sensor_msgs_image.get(),
+      "unique_sensor_msgs_image cannot be nullptr"
+).pointer->header),
+  frame_(
+    unique_sensor_msgs_image->height,
+    unique_sensor_msgs_image->width,
+    encoding2mat_type(unique_sensor_msgs_image->encoding),
+    unique_sensor_msgs_image->data.data(),
+    unique_sensor_msgs_image->step),
+  storage_(std::move(unique_sensor_msgs_image))
+{}
+
+ROSCvMatContainer::ROSCvMatContainer(
+  std::shared_ptr<sensor_msgs::msg::Image> shared_sensor_msgs_image)
+: header_(shared_sensor_msgs_image->header),
+  frame_(
+    shared_sensor_msgs_image->height,
+    shared_sensor_msgs_image->width,
+    encoding2mat_type(shared_sensor_msgs_image->encoding),
+    shared_sensor_msgs_image->data.data(),
+    shared_sensor_msgs_image->step),
+  storage_(shared_sensor_msgs_image)
+{}
+
+ROSCvMatContainer::ROSCvMatContainer(
+  const cv::Mat & mat_frame,
+  const std_msgs::msg::Header & header,
+  bool is_bigendian)
+: header_(header),
+  frame_(mat_frame),
+  storage_(nullptr),
+  is_bigendian_(is_bigendian)
+{}
+
+ROSCvMatContainer::ROSCvMatContainer(
+  cv::Mat && mat_frame,
+  const std_msgs::msg::Header & header,
+  bool is_bigendian)
+: header_(header),
+  frame_(std::forward<cv::Mat>(mat_frame)),
+  storage_(nullptr),
+  is_bigendian_(is_bigendian)
+{}
+
+ROSCvMatContainer::ROSCvMatContainer(
+  const sensor_msgs::msg::Image & sensor_msgs_image)
+: ROSCvMatContainer(std::make_unique<sensor_msgs::msg::Image>(sensor_msgs_image))
+{}
+
+bool
+ROSCvMatContainer::is_owning() const
+{
+  return std::holds_alternative<std::nullptr_t>(storage_);
+}
+
+const cv::Mat &
+ROSCvMatContainer::cv_mat() const
+{
+  return frame_;
+}
+
+cv::Mat
+ROSCvMatContainer::cv_mat()
+{
+  return frame_;
+}
+
+const std_msgs::msg::Header &
+ROSCvMatContainer::header() const
+{
+  return header_;
+}
+
+std_msgs::msg::Header &
+ROSCvMatContainer::header()
+{
+  return header_;
+}
+
+std::shared_ptr<const sensor_msgs::msg::Image>
+ROSCvMatContainer::get_sensor_msgs_msg_image_pointer() const
+{
+  if (!std::holds_alternative<std::shared_ptr<sensor_msgs::msg::Image>>(storage_)) {
+    return nullptr;
+  }
+  return std::get<std::shared_ptr<sensor_msgs::msg::Image>>(storage_);
+}
+
+std::unique_ptr<sensor_msgs::msg::Image>
+ROSCvMatContainer::get_sensor_msgs_msg_image_pointer_copy() const
+{
+  auto unique_image = std::make_unique<sensor_msgs::msg::Image>();
+  this->get_sensor_msgs_msg_image_copy(*unique_image);
+  return unique_image;
+}
+
+void
+ROSCvMatContainer::get_sensor_msgs_msg_image_copy(
+  sensor_msgs::msg::Image & sensor_msgs_image) const
+{
+  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");
+  }
+  sensor_msgs_image.step = static_cast<sensor_msgs::msg::Image::_step_type>(frame_.step);
+  size_t size = frame_.step * frame_.rows;
+  sensor_msgs_image.data.resize(size);
+  memcpy(&sensor_msgs_image.data[0], frame_.data, size);
+  sensor_msgs_image.header = header_;
+}
+
+bool
+ROSCvMatContainer::is_bigendian() const
+{
+  return is_bigendian_;
+}
+
+}  // namespace image_tools

src/image_tools/src/policy_maps.hpp

@@ -0,0 +1,37 @@
+// Copyright 2019 Open Source Robotics Foundation, 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 POLICY_MAPS_HPP_
+#define POLICY_MAPS_HPP_
+
+#include <map>
+#include <string>
+
+namespace image_tools
+{
+
+static
+std::map<std::string, rmw_qos_reliability_policy_t> name_to_reliability_policy_map = {
+  {"reliable", RMW_QOS_POLICY_RELIABILITY_RELIABLE},
+  {"best_effort", RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT}
+};
+
+static
+std::map<std::string, rmw_qos_history_policy_t> name_to_history_policy_map = {
+  {"keep_last", RMW_QOS_POLICY_HISTORY_KEEP_LAST},
+  {"keep_all", RMW_QOS_POLICY_HISTORY_KEEP_ALL}
+};
+
+}  // namespace image_tools
+
+#endif  // POLICY_MAPS_HPP_

src/image_tools/src/showimage.cpp

@@ -0,0 +1,207 @@
+// Copyright 2015 Open Source Robotics Foundation, 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.
+
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include "opencv2/core/mat.hpp"
+#include "opencv2/highgui.hpp"
+#include "opencv2/imgproc.hpp"
+
+#include "rcl_interfaces/msg/parameter_descriptor.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+#include "sensor_msgs/msg/image.hpp"
+
+#include "image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp"
+#include "image_tools/visibility_control.h"
+
+#include "./policy_maps.hpp"
+
+RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(
+  image_tools::ROSCvMatContainer,
+  sensor_msgs::msg::Image);
+
+namespace image_tools
+{
+class ShowImage : public rclcpp::Node
+{
+public:
+  IMAGE_TOOLS_PUBLIC
+  explicit ShowImage(const rclcpp::NodeOptions & options)
+  : Node("showimage", options)
+  {
+    setvbuf(stdout, NULL, _IONBF, BUFSIZ);
+    // Do not execute if a --help option was provided
+    if (help(options.arguments())) {
+      // TODO(jacobperron): Replace with a mechanism for a node to "unload" itself
+      // from a container.
+      exit(0);
+    }
+    parse_parameters();
+    initialize();
+  }
+
+private:
+  IMAGE_TOOLS_LOCAL
+  void initialize()
+  {
+    // Set quality of service profile based on command line options.
+    auto qos = rclcpp::QoS(
+      rclcpp::QoSInitialization(
+        // The history policy determines how messages are saved until taken by
+        // the reader.
+        // KEEP_ALL saves all messages until they are taken.
+        // KEEP_LAST enforces a limit on the number of messages that are saved,
+        // specified by the "depth" parameter.
+        history_policy_,
+        // Depth represents how many messages to store in history when the
+        // history policy is KEEP_LAST.
+        depth_
+    ));
+    // The reliability policy can be reliable, meaning that the underlying transport layer will try
+    // ensure that every message gets received in order, or best effort, meaning that the transport
+    // makes no guarantees about the order or reliability of delivery.
+    qos.reliability(reliability_policy_);
+    auto callback =
+      [this](const image_tools::ROSCvMatContainer & container) {
+        process_image(container, show_image_, this->get_logger());
+      };
+
+    RCLCPP_INFO(this->get_logger(), "Subscribing to topic '%s'", topic_.c_str());
+    sub_ = create_subscription<image_tools::ROSCvMatContainer>(topic_, qos, callback);
+
+    if (window_name_ == "") {
+      // If no custom window name is given, use the topic name
+      window_name_ = sub_->get_topic_name();
+    }
+  }
+
+  IMAGE_TOOLS_LOCAL
+  bool help(const std::vector<std::string> args)
+  {
+    if (std::find(args.begin(), args.end(), "--help") != args.end() ||
+      std::find(args.begin(), args.end(), "-h") != args.end())
+    {
+      std::stringstream ss;
+      ss << "Usage: showimage [-h] [--ros-args [-p param:=value] ...]" << std::endl;
+      ss << "Subscribe to an image topic and show the images." << std::endl;
+      ss << "Example: ros2 run image_tools showimage --ros-args -p reliability:=best_effort";
+      ss << std::endl << std::endl;
+      ss << "Options:" << std::endl;
+      ss << "  -h, --help\tDisplay this help message and exit";
+      ss << std::endl << std::endl;
+      ss << "Parameters:" << std::endl;
+      ss << "  reliability\tReliability QoS setting. Either 'reliable' (default) or 'best_effort'";
+      ss << std::endl;
+      ss << "  history\tHistory QoS setting. Either 'keep_last' (default) or 'keep_all'.";
+      ss << std::endl;
+      ss << "\t\tIf 'keep_last', then up to N samples are stored where N is the depth";
+      ss << std::endl;
+      ss << "  depth\t\tDepth of the publisher queue. Only honored if history QoS is 'keep_last'.";
+      ss << " Default value is 10";
+      ss << std::endl;
+      ss << "  show_image\tShow the image. Either 'true' (default) or 'false'";
+      ss << std::endl;
+      ss << "  window_name\tName of the display window. Default value is the topic name";
+      ss << std::endl;
+      std::cout << ss.str();
+      return true;
+    }
+    return false;
+  }
+
+  IMAGE_TOOLS_LOCAL
+  void parse_parameters()
+  {
+    // Parse 'reliability' parameter
+    rcl_interfaces::msg::ParameterDescriptor reliability_desc;
+    reliability_desc.description = "Reliability QoS setting for the image subscription";
+    reliability_desc.additional_constraints = "Must be one of: ";
+    for (auto entry : name_to_reliability_policy_map) {
+      reliability_desc.additional_constraints += entry.first + " ";
+    }
+    const std::string reliability_param = this->declare_parameter(
+      "reliability", "reliable", reliability_desc);
+    auto reliability = name_to_reliability_policy_map.find(reliability_param);
+    if (reliability == name_to_reliability_policy_map.end()) {
+      std::ostringstream oss;
+      oss << "Invalid QoS reliability setting '" << reliability_param << "'";
+      throw std::runtime_error(oss.str());
+    }
+    reliability_policy_ = reliability->second;
+
+    // Parse 'history' parameter
+    rcl_interfaces::msg::ParameterDescriptor history_desc;
+    history_desc.description = "History QoS setting for the image subscription";
+    history_desc.additional_constraints = "Must be one of: ";
+    for (auto entry : name_to_history_policy_map) {
+      history_desc.additional_constraints += entry.first + " ";
+    }
+    const std::string history_param = this->declare_parameter(
+      "history", name_to_history_policy_map.begin()->first, history_desc);
+    auto history = name_to_history_policy_map.find(history_param);
+    if (history == name_to_history_policy_map.end()) {
+      std::ostringstream oss;
+      oss << "Invalid QoS history setting '" << history_param << "'";
+      throw std::runtime_error(oss.str());
+    }
+    history_policy_ = history->second;
+
+    // Declare and get remaining parameters
+    depth_ = this->declare_parameter("depth", 10);
+    show_image_ = this->declare_parameter("show_image", true);
+    window_name_ = this->declare_parameter("window_name", "");
+  }
+
+  /// Convert the ROS Image message to an OpenCV matrix and display it to the user.
+  // \param[in] container The image message to show.
+  IMAGE_TOOLS_LOCAL
+  void process_image(
+    const image_tools::ROSCvMatContainer & container, bool show_image, rclcpp::Logger logger)
+  {
+    RCLCPP_INFO(logger, "Received image #%s", container.header().frame_id.c_str());
+    std::cerr << "Received image #" << container.header().frame_id.c_str() << std::endl;
+
+    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);
+      }
+
+      // Show the image in a window
+      cv::imshow(window_name_, frame);
+      // Draw the screen and wait for 1 millisecond.
+      cv::waitKey(1);
+    }
+  }
+
+  rclcpp::Subscription<image_tools::ROSCvMatContainer>::SharedPtr sub_;
+  size_t depth_ = rmw_qos_profile_default.depth;
+  rmw_qos_reliability_policy_t reliability_policy_ = rmw_qos_profile_default.reliability;
+  rmw_qos_history_policy_t history_policy_ = rmw_qos_profile_default.history;
+  bool show_image_ = true;
+  std::string topic_ = "image";
+  std::string window_name_;
+};
+
+}  // namespace image_tools
+
+RCLCPP_COMPONENTS_REGISTER_NODE(image_tools::ShowImage)

src/image_tools/test/cam2image.txt

@@ -0,0 +1,2 @@
+Publishing image #1
+Publishing image #2

src/image_tools/test/showimage.regex

@@ -0,0 +1 @@
+Received image #\w+

src/image_tools/test/test_executables_demo.py.in

@@ -0,0 +1,99 @@
+# Copyright 2016 Open Source Robotics Foundation, 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.
+
+import os
+import unittest
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+from launch.actions import SetEnvironmentVariable
+from launch_ros.actions import Node
+
+import launch_testing
+import launch_testing.actions
+import launch_testing.asserts
+import launch_testing_ros
+
+
+def generate_test_description():
+    launch_description = LaunchDescription()
+    publisher_node_parameters = {
+        'reliability': 'reliable',
+        'show_camera': False,
+        'burger_mode': True,
+        'frequency': 5.0,
+    }
+    subscriber_node_parameters = {
+        'reliability': 'reliable',
+        'show_image': False,
+    }
+
+    launch_description.add_action(SetEnvironmentVariable('OSPL_VERBOSITY', '8'))  # 8 = OS_NONE
+    # bare minimum formatting for console output matching
+    launch_description.add_action(
+        SetEnvironmentVariable('RCUTILS_CONSOLE_OUTPUT_FORMAT', '{message}'))
+    launch_description.add_action(
+        SetEnvironmentVariable('RMW_IMPLEMENTATION', '@rmw_implementation@'))
+
+    # Launch the process that will receive the images.
+    # This is the process that gets to decide when to tear the launcher down.
+    # It will exit when the regex for receiving images is matched.
+    showimage_executable = '@RCLCPP_DEMO_SHOWIMAGE_EXECUTABLE@'
+    showimage_name = 'test_showimage'
+
+    showimage_node = Node(
+        executable=showimage_executable,
+        name=showimage_name,
+        parameters=[subscriber_node_parameters],
+        output='screen'
+    )
+    launch_description.add_action(showimage_node)
+
+    # Launch the process that will publish the images.
+    # This process will be exited when the launcher is torn down.
+    cam2image_executable = '@RCLCPP_DEMO_CAM2IMAGE_EXECUTABLE@'
+    cam2image_name = 'test_cam2image'
+
+    cam2image_node = Node(
+        executable=cam2image_executable,
+        name=cam2image_name,
+        parameters=[publisher_node_parameters],
+        output='screen'
+    )
+    launch_description.add_action(cam2image_node)
+
+    launch_description.add_action(
+        launch_testing.actions.ReadyToTest()
+    )
+
+    return launch_description, locals()
+
+
+class TestExecutablesDemo(unittest.TestCase):
+
+    def test_reliable_qos(self, proc_output, showimage_node, cam2image_node):
+        """Test QoS settings for both processes work as expected."""
+        output_filter = launch_testing_ros.tools.basic_output_filter(
+            filtered_rmw_implementation='@rmw_implementation@'
+        )
+        proc_output.assertWaitFor(
+            expected_output=launch_testing.tools.expected_output_from_file(
+                path='@RCLCPP_DEMO_SHOWIMAGE_EXPECTED_OUTPUT@'
+            ), process=showimage_node, output_filter=output_filter, timeout=10
+        )
+        proc_output.assertWaitFor(
+            expected_output=launch_testing.tools.expected_output_from_file(
+                path='@RCLCPP_DEMO_CAM2IMAGE_EXPECTED_OUTPUT@'
+            ), process=cam2image_node, output_filter=output_filter, timeout=10
+        )