diff --git a/drone_img/droneimage_2023-05-10_11-31-50.jpg b/drone_img/droneimage_2023-05-10_11-31-50.jpg new file mode 100644 index 00000000..41358657 Binary files /dev/null and b/drone_img/droneimage_2023-05-10_11-31-50.jpg differ diff --git a/drone_img/droneimage_2023-05-10_11-32-01.jpg b/drone_img/droneimage_2023-05-10_11-32-01.jpg new file mode 100644 index 00000000..bd84defb Binary files /dev/null and b/drone_img/droneimage_2023-05-10_11-32-01.jpg differ diff --git a/drone_img/droneimage_2023-05-10_11-32-13.jpg b/drone_img/droneimage_2023-05-10_11-32-13.jpg new file mode 100644 index 00000000..e4992ee4 Binary files /dev/null and b/drone_img/droneimage_2023-05-10_11-32-13.jpg differ diff --git a/src/image_tools/CHANGELOG.rst b/src/image_tools/CHANGELOG.rst new file mode 100644 index 00000000..7dd87464 --- /dev/null +++ b/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 `_) +* Update the demos to C++17. (`#594 `_) +* [rolling] Update maintainers - 2022-11-07 (`#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 `_) +* Contributors: Shane Loretz + +0.19.0 (2022-01-14) +------------------- +* Fix include order and relative paths for cpplint (`#551 `_) +* Reduce the number of OpenCV libraries image_tools links against. (`#549 `_) +* Adds copy constructor and assignment operator to ROSCvMatContainer (`#546 `_) +* Contributors: Chris Lalancette, Gonzo, Jacob Perron + +0.18.0 (2021-12-17) +------------------- +* Fixes for uncrustify 0.72 (`#545 `_) +* Update maintainers to Audrow Nash and Michael Jeronimo (`#543 `_) +* Additional fixes for documentation in demos. (`#538 `_) +* Contributors: Audrow Nash, Chris Lalancette + +0.17.0 (2021-10-18) +------------------- +* Fixing deprecated subscriber callback warnings (`#532 `_) +* Contributors: Abrar Rahman Protyasha + +0.16.0 (2021-08-11) +------------------- +* ambigulity: unknown type name 'nullptr_t' (`#528 `_) +* Add type masquerading demos (`#482 `_) +* Contributors: Audrow Nash, William Woodall, xwnb + +0.15.0 (2021-05-14) +------------------- +* Add support for visualizing yuv422 (`#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 `_) +* Update the package.xml files with the latest Open Robotics maintainers (`#466 `_) +* Added more parameters for camera topic examples (`#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 `_) +* Fix frame_id (`#433 `_) +* Update launch_ros action usage (`#431 `_) +* code style only: wrap after open parenthesis if not in one line (`#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 `_) +* Contributors: Shane Loretz + +0.8.2 (2019-11-08) +------------------ + +0.8.1 (2019-10-23) +------------------ +* Fix burguer mode parameter typo in help text (`#406 `_) +* Replace ready_fn with ReadyToTest action (`#404 `_) +* [image_tools] Use ROS parameters instead of regular CLI arguments (`#398 `_) +* Contributors: Brian Marchi, Jacob Perron, Peter Baughman + +0.8.0 (2019-09-26) +------------------ +* Adding visibility macros to demos (`#381 `_) +* Demos using composition (`#375 `_) +* Contributors: Siddharth Kucheria + +0.7.6 (2019-05-30) +------------------ + +0.7.5 (2019-05-29) +------------------ +* Remove debugging prints from showimage. (`#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 `_) +* Corrected publish calls with shared_ptr signature (`#327 `_) +* Migrate launch tests to new launch_testing features & API (`#318 `_) +* Contributors: Michel Hidalgo, William Woodall, ivanpauno + +0.7.1 (2019-04-26) +------------------ +* Removed support for OpenCV 2. (`#322 `_) +* Contributors: Jacob Perron + +0.7.0 (2019-04-14) +------------------ +* Added launch along with launch_testing as test dependencies. (`#313 `_) +* Contributors: Michel Hidalgo + +0.6.2 (2019-01-15) +------------------ +* Updated to support OpenCV 2, 3 and 4 (`#307 `_) +* Updated for OpenCV v4.0 compatibility (`#306 `_) +* Updated to show freq parameter on help only when necessary (`#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 `_) +* Added semicolons to all RCLCPP and RCUTILS macros. (`#278 `_) +* Updated to keep only the last sample in the image tools by default. (`#238 `_) +* Contributors: Chris Lalancette, sgvandijk + +0.5.1 (2018-06-28) +------------------ + +0.5.0 (2018-06-27) +------------------ +* Fixed linting errors in ``burger.cpp`` (`#260 `_) + * Signed-off-by: Chris Lalancette +* Fixed a bug that occurred when the resolution was less than the bugger. (`#258 `_) +* Updated launch files to account for the "old launch" getting renamespaced as ``launch`` -> ``launch.legacy``. (`#239 `_) +* Contributors: Chris Lalancette, Dirk Thomas, William Woodall diff --git a/src/image_tools/CMakeLists.txt b/src/image_tools/CMakeLists.txt new file mode 100644 index 00000000..d38c9d22 --- /dev/null +++ b/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 + "$" + "$") +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 $) + set(RCLCPP_DEMO_SHOWIMAGE_EXECUTABLE $) + + 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}_$.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}_$.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() diff --git a/src/image_tools/Doxyfile b/src/image_tools/Doxyfile new file mode 100644 index 00000000..509de475 --- /dev/null +++ b/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 diff --git a/src/image_tools/README.md b/src/image_tools/README.md new file mode 100644 index 00000000..3c305603 --- /dev/null +++ b/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 +``` diff --git a/src/image_tools/doc/qos-best-effort.png b/src/image_tools/doc/qos-best-effort.png new file mode 100644 index 00000000..69bc48e2 Binary files /dev/null and b/src/image_tools/doc/qos-best-effort.png differ diff --git a/src/image_tools/img/result.png b/src/image_tools/img/result.png new file mode 100644 index 00000000..b45b85f4 Binary files /dev/null and b/src/image_tools/img/result.png differ diff --git a/src/image_tools/include/image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp b/src/image_tools/include/image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp new file mode 100644 index 00000000..b069cc20 --- /dev/null +++ b/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 +#include +#include // 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 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, + std::shared_ptr + >; + + 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>(other.storage_)) { + storage_ = std::get>(other.storage_); + } else if (std::holds_alternative>(other.storage_)) { + storage_ = std::make_unique( + *std::get>(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>(other.storage_)) { + storage_ = std::get>(other.storage_); + } else if (std::holds_alternative>(other.storage_)) { + storage_ = std::make_unique( + *std::get>(other.storage_)); + } else if (std::holds_alternative(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 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 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 + 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 + 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 +{ + 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(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_ diff --git a/src/image_tools/include/image_tools/visibility_control.h b/src/image_tools/include/image_tools/visibility_control.h new file mode 100644 index 00000000..1a2b3eb7 --- /dev/null +++ b/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_ diff --git a/src/image_tools/package.xml b/src/image_tools/package.xml new file mode 100644 index 00000000..14b3e851 --- /dev/null +++ b/src/image_tools/package.xml @@ -0,0 +1,43 @@ + + + + image_tools + 0.28.0 + Tools to capture and play back images to and from DDS subscriptions and publications. + + Aditya Pande + Audrow Nash + Michael Jeronimo + + Apache License 2.0 + + Dirk Thomas + Mabel Zhang + + ament_cmake + libopencv-dev + rclcpp + rclcpp_components + sensor_msgs + std_msgs + + libopencv-dev + rclcpp + rclcpp_components + sensor_msgs + std_msgs + + ament_cmake_pytest + ament_lint_auto + ament_lint_common + launch + launch_ros + launch_testing + launch_testing_ament_cmake + launch_testing_ros + rmw_implementation_cmake + + + ament_cmake + + diff --git a/src/image_tools/src/burger.cpp b/src/image_tools/src/burger.cpp new file mode 100644 index 00000000..c7951e86 --- /dev/null +++ b/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 // For GetTickCount(). +#endif + +#include +#include +#include +#include + +#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 & out) +{ + int len = static_cast(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 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(width); + int height_i = static_cast(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(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; +} diff --git a/src/image_tools/src/burger.hpp b/src/image_tools/src/burger.hpp new file mode 100644 index 00000000..02b34f34 --- /dev/null +++ b/src/image_tools/src/burger.hpp @@ -0,0 +1,49 @@ +// 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 BURGER_HPP_ +#define BURGER_HPP_ + +#include + +#include "opencv2/core/mat.hpp" + +#include "image_tools/visibility_control.h" + +namespace burger +{ + +class Burger +{ +public: + IMAGE_TOOLS_PUBLIC + Burger(); + IMAGE_TOOLS_PUBLIC + cv::Mat & render_burger(size_t width, size_t height); + + cv::Mat burger_buf; + +private: + cv::Mat burger_template, burger_mask; + std::vector x, y, x_inc, y_inc; +}; + +// THE FOLLOWING IS A BURGER IN AN AWESOME C BASE64 MACRO. RESPECT IT + +#define BURGER \ + "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" // NOLINT + +} // namespace burger + +#endif // BURGER_HPP_ diff --git a/src/image_tools/src/cam2image.cpp b/src/image_tools/src/cam2image.cpp new file mode 100644 index 00000000..2e254a99 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +#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", 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( + "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(width_)); + cap.set(cv::CAP_PROP_FRAME_HEIGHT, static_cast(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(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 & 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(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::SharedPtr sub_; + rclcpp::Publisher::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) diff --git a/src/image_tools/src/cv_mat_sensor_msgs_image_type_adapter.cpp b/src/image_tools/src/cv_mat_sensor_msgs_image_type_adapter.cpp new file mode 100644 index 00000000..a22b54cb --- /dev/null +++ b/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 +#include +#include +#include +#include // 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 +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 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 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(mat_frame)), + storage_(nullptr), + is_bigendian_(is_bigendian) +{} + +ROSCvMatContainer::ROSCvMatContainer( + const sensor_msgs::msg::Image & sensor_msgs_image) +: ROSCvMatContainer(std::make_unique(sensor_msgs_image)) +{} + +bool +ROSCvMatContainer::is_owning() const +{ + return std::holds_alternative(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 +ROSCvMatContainer::get_sensor_msgs_msg_image_pointer() const +{ + if (!std::holds_alternative>(storage_)) { + return nullptr; + } + return std::get>(storage_); +} + +std::unique_ptr +ROSCvMatContainer::get_sensor_msgs_msg_image_pointer_copy() const +{ + auto unique_image = std::make_unique(); + 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(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 diff --git a/src/image_tools/src/policy_maps.hpp b/src/image_tools/src/policy_maps.hpp new file mode 100644 index 00000000..1d39b93d --- /dev/null +++ b/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 +#include + +namespace image_tools +{ + +static +std::map name_to_reliability_policy_map = { + {"reliable", RMW_QOS_POLICY_RELIABILITY_RELIABLE}, + {"best_effort", RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT} +}; + +static +std::map 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_ diff --git a/src/image_tools/src/showimage.cpp b/src/image_tools/src/showimage.cpp new file mode 100644 index 00000000..c221b1f5 --- /dev/null +++ b/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 +#include +#include +#include + +#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(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 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::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) diff --git a/src/image_tools/test/cam2image.txt b/src/image_tools/test/cam2image.txt new file mode 100644 index 00000000..6cab299f --- /dev/null +++ b/src/image_tools/test/cam2image.txt @@ -0,0 +1,2 @@ +Publishing image #1 +Publishing image #2 diff --git a/src/image_tools/test/showimage.regex b/src/image_tools/test/showimage.regex new file mode 100644 index 00000000..f820b799 --- /dev/null +++ b/src/image_tools/test/showimage.regex @@ -0,0 +1 @@ +Received image #\w+ diff --git a/src/image_tools/test/test_executables_demo.py.in b/src/image_tools/test/test_executables_demo.py.in new file mode 100644 index 00000000..369ca40f --- /dev/null +++ b/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 + ) diff --git a/src/px4_msgs b/src/px4_msgs index b64ef047..ffc3a4cd 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 +Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266