diff --git a/drone_img/droneimage_2023-05-10_11-31-50.jpg b/drone_img/droneimage_2023-05-10_11-31-50.jpg deleted file mode 100644 index 41358657..00000000 Binary files a/drone_img/droneimage_2023-05-10_11-31-50.jpg and /dev/null 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 deleted file mode 100644 index bd84defb..00000000 Binary files a/drone_img/droneimage_2023-05-10_11-32-01.jpg and /dev/null 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 deleted file mode 100644 index e4992ee4..00000000 Binary files a/drone_img/droneimage_2023-05-10_11-32-13.jpg and /dev/null differ diff --git a/src/camera/camera/camera_controller.py b/src/camera/camera/camera_controller.py index 2b39bbe3..e589c9e9 100644 --- a/src/camera/camera/camera_controller.py +++ b/src/camera/camera/camera_controller.py @@ -14,61 +14,24 @@ RES_4K_W = 4656 class CameraController(Node): def __init__(self): super().__init__('camera_controller') - # self.capture = cv2.VideoCapture(0,cv2.CAP_DSHOW) - - # CAMERA_PROP_WIDTH = 3 - # CAMERA_PROP_HEIGHT = 4 - # self.capture.set(CAMERA_PROP_WIDTH, RES_4K_W) - # self.capture.set(CAMERA_PROP_HEIGHT, RES_4K_H) - # self.get_logger().info("Camera resolution set to " + str(self.capture.get(CAMERA_PROP_WIDTH)) + "x" + str(self.capture.get(CAMERA_PROP_HEIGHT))) self.get_logger().info("Camera controller started. Waiting for service call...") - self.srv = self.create_service(TakePicture, 'drone/picture', self.take_picture_callback) + self.srv = self.create_service( + TakePicture, 'drone/picture', self.take_picture_callback) def take_picture_callback(self, request, response): - # result, image = self.capture.read() - # if (result): - if (request.input_name == "default"): - self.get_logger().info("Taking picture with default filename") - now = datetime.now().strftime("droneimage_%Y-%m-%d_%H-%M-%S") - imagename = "/home/ubuntu/drone_img/" + now + ".jpg" - # image = self.maintain_aspect_ratio_resize(image, width=RES_4K_W, height=RES_4K_H) - # cv2.imwrite(imagename, image) - response.filename = imagename - else: - # cv2.imwrite(request.input_name, image) - response.filename = request.input_name - os.system('fswebcam -r 4656x3496 ' + response.filename) - self.get_logger().info("Picture saved as " + response.filename) - # else: - # self.get_logger().error("Could not take picture") - # response.filename = "/dev/null" + if (request.input_name == "default"): + self.get_logger().info("Taking picture with default filename") + now = datetime.now().strftime("droneimage_%Y-%m-%d_%H-%M-%S") + imagename = "/home/ubuntu/drone_img/" + now + ".jpg" + response.filename = imagename + else: + response.filename = request.input_name + os.system('fswebcam -r ' + RES_4K_W + 'x' + RES_4K_H + ' ' + response.filename) + self.get_logger().info("Picture saved as " + response.filename) - return response - - # def maintain_aspect_ratio_resize(self, image, width=None, height=None, inter=cv2.INTER_AREA): - # # Grab the image size and initialize dimensions - # dim = None - # (h, w) = image.shape[:2] + return response - # # Return original image if no need to resize - # if width is None and height is None: - # return image - - # # We are resizing height if width is none - # if width is None: - # # Calculate the ratio of the height and construct the dimensions - # r = height / float(h) - # dim = (int(w * r), height) - # # We are resizing width if height is none - # else: - # # Calculate the ratio of the 0idth and construct the dimensions - # r = width / float(w) - # dim = (width, int(h * r)) - - # # Return the resized image - # return cv2.resize(image, dim, interpolation=inter) - def main(args=None): rclpy.init(args=args) @@ -84,4 +47,4 @@ def main(args=None): if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/src/image_tools/CHANGELOG.rst b/src/image_tools/CHANGELOG.rst deleted file mode 100644 index 7dd87464..00000000 --- a/src/image_tools/CHANGELOG.rst +++ /dev/null @@ -1,203 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -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 deleted file mode 100644 index d38c9d22..00000000 --- a/src/image_tools/CMakeLists.txt +++ /dev/null @@ -1,98 +0,0 @@ -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 deleted file mode 100644 index 509de475..00000000 --- a/src/image_tools/Doxyfile +++ /dev/null @@ -1,21 +0,0 @@ -# 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 deleted file mode 100644 index 3c305603..00000000 --- a/src/image_tools/README.md +++ /dev/null @@ -1,36 +0,0 @@ -## **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 deleted file mode 100644 index 69bc48e2..00000000 Binary files a/src/image_tools/doc/qos-best-effort.png and /dev/null differ diff --git a/src/image_tools/img/result.png b/src/image_tools/img/result.png deleted file mode 100644 index b45b85f4..00000000 Binary files a/src/image_tools/img/result.png and /dev/null 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 deleted file mode 100644 index b069cc20..00000000 --- a/src/image_tools/include/image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp +++ /dev/null @@ -1,272 +0,0 @@ -// 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 deleted file mode 100644 index 1a2b3eb7..00000000 --- a/src/image_tools/include/image_tools/visibility_control.h +++ /dev/null @@ -1,58 +0,0 @@ -// 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 deleted file mode 100644 index 14b3e851..00000000 --- a/src/image_tools/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - 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 deleted file mode 100644 index c7951e86..00000000 --- a/src/image_tools/src/burger.cpp +++ /dev/null @@ -1,163 +0,0 @@ -// 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 deleted file mode 100644 index 02b34f34..00000000 --- a/src/image_tools/src/burger.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// 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 deleted file mode 100644 index 2e254a99..00000000 --- a/src/image_tools/src/cam2image.cpp +++ /dev/null @@ -1,281 +0,0 @@ -// 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 deleted file mode 100644 index a22b54cb..00000000 --- a/src/image_tools/src/cv_mat_sensor_msgs_image_type_adapter.cpp +++ /dev/null @@ -1,207 +0,0 @@ -// 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 deleted file mode 100644 index 1d39b93d..00000000 --- a/src/image_tools/src/policy_maps.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// 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 deleted file mode 100644 index c221b1f5..00000000 --- a/src/image_tools/src/showimage.cpp +++ /dev/null @@ -1,207 +0,0 @@ -// 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 deleted file mode 100644 index 6cab299f..00000000 --- a/src/image_tools/test/cam2image.txt +++ /dev/null @@ -1,2 +0,0 @@ -Publishing image #1 -Publishing image #2 diff --git a/src/image_tools/test/showimage.regex b/src/image_tools/test/showimage.regex deleted file mode 100644 index f820b799..00000000 --- a/src/image_tools/test/showimage.regex +++ /dev/null @@ -1 +0,0 @@ -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 deleted file mode 100644 index 369ca40f..00000000 --- a/src/image_tools/test/test_executables_demo.py.in +++ /dev/null @@ -1,99 +0,0 @@ -# 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 ffc3a4cd..b64ef047 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 +Subproject commit b64ef0475c1d44605688f4770899fe453d532be4