From 48133ae71810bd204d5fbad977efa7ea9e5c7be9 Mon Sep 17 00:00:00 2001 From: Matthew Foran Date: Wed, 20 Nov 2024 22:47:01 +0000 Subject: [PATCH] add test for compressed image display --- rviz_default_plugins/CMakeLists.txt | 4 + rviz_default_plugins/package.xml | 2 + .../image/image_display_visual_test.cpp | 19 ++++- .../publishers/compressed_image_publisher.hpp | 84 +++++++++++++++++++ 4 files changed, 108 insertions(+), 1 deletion(-) create mode 100644 rviz_default_plugins/test/rviz_default_plugins/publishers/compressed_image_publisher.hpp diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index 1bdc96b17..1e2d73f64 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -883,6 +883,9 @@ if(BUILD_TESTING) ${SKIP_VISUAL_TESTS} TIMEOUT 180) if(TARGET image_display_visual_test) + find_package(OpenCV REQUIRED) + find_package(image_transport_plugins REQUIRED) + include_directories(${OpenCV_INCLUDE_DIRS}) target_include_directories(image_display_visual_test PRIVATE test) target_link_libraries(image_display_visual_test Qt5::Test @@ -890,6 +893,7 @@ if(BUILD_TESTING) rclcpp::rclcpp ${sensor_msgs_TARGETS} ${std_msgs_TARGETS} + ${OpenCV_LIBRARIES} ) endif() diff --git a/rviz_default_plugins/package.xml b/rviz_default_plugins/package.xml index 36730baf0..54e9a5440 100644 --- a/rviz_default_plugins/package.xml +++ b/rviz_default_plugins/package.xml @@ -64,6 +64,8 @@ ament_lint_auto rviz_rendering_tests rviz_visual_testing_framework + image_transport_plugins + vision_opencv ament_cmake diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/image/image_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/image/image_display_visual_test.cpp index a2ef35fbb..e40e2a958 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/image/image_display_visual_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/image/image_display_visual_test.cpp @@ -36,9 +36,11 @@ #include "../../page_objects/image_display_page_object.hpp" #include "../../publishers/image_publisher.hpp" +#include "../../publishers/compressed_image_publisher.hpp" + TEST_F(VisualTestFixture, test_image_display_with_published_image) { - auto path_publisher = std::make_unique( + auto image_publisher = std::make_unique( std::make_shared(), "image_frame"); setCamPose(Ogre::Vector3(0, 0, 16)); @@ -51,3 +53,18 @@ TEST_F(VisualTestFixture, test_image_display_with_published_image) { assertScreenShotsIdentity(); } + +TEST_F(VisualTestFixture, test_compressed_image_display_with_published_image) { + auto compressed_image_publisher = std::make_unique( + std::make_shared(), "image_frame"); + + setCamPose(Ogre::Vector3(0, 0, 16)); + setCamLookAt(Ogre::Vector3(0, 0, 0)); + + auto image_display = addDisplay(); + image_display->setTopic("/image"); + + captureRenderWindow(image_display); + + assertScreenShotsIdentity(); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/publishers/compressed_image_publisher.hpp b/rviz_default_plugins/test/rviz_default_plugins/publishers/compressed_image_publisher.hpp new file mode 100644 index 000000000..858866f4f --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/publishers/compressed_image_publisher.hpp @@ -0,0 +1,84 @@ +// Copyright (c) 2018, Bosch Software Innovations GmbH. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + + +#ifndef RVIZ_DEFAULT_PLUGINS__PUBLISHERS__COMPRESSED_IMAGE_PUBLISHER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PUBLISHERS__COMPRESSED_IMAGE_PUBLISHER_HPP_ + +#include +#include +#include +#include + +#include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/clock.hpp" +#include "sensor_msgs/msg/compressed_image.hpp" +#include "std_msgs/msg/header.hpp" + +using namespace std::chrono_literals; // NOLINT + +namespace nodes +{ + +class CompressedImagePublisher : public rclcpp::Node +{ +public: + explicit CompressedImagePublisher(const std::string & topic_name = "image") + : Node("compressed_image_publisher") + { + publisher = this->create_publisher(topic_name, 10); + timer = this->create_wall_timer(100ms, + std::bind(&CompressedImagePublisher::timer_callback, this)); + } + +private: + void timer_callback() + { + auto message = sensor_msgs::msg::CompressedImage(); + message.header = std_msgs::msg::Header(); + message.header.frame_id = "image_frame"; + message.header.stamp = rclcpp::Clock().now(); + + cv::Mat image(200, 300, CV_8UC3, cv::Scalar(0, 255, 0)); + std::vector compressed_image; + cv::imencode(".jpg", image, compressed_image); + + message.data.assign(compressed_image.begin(), compressed_image.end()); + message.format = "jpeg"; + publisher->publish(message); + } + + rclcpp::TimerBase::SharedPtr timer; + rclcpp::Publisher::SharedPtr publisher; +}; + +} // namespace nodes + +#endif // RVIZ_DEFAULT_PLUGINS__PUBLISHERS__COMPRESSED_IMAGE_PUBLISHER_HPP_