diff --git a/rviz_common/CMakeLists.txt b/rviz_common/CMakeLists.txt index 8b93f3643..d44df2236 100644 --- a/rviz_common/CMakeLists.txt +++ b/rviz_common/CMakeLists.txt @@ -111,6 +111,7 @@ set(rviz_common_headers_to_moc include/rviz_common/properties/property_tree_with_help.hpp include/rviz_common/properties/qos_profile_property.hpp include/rviz_common/properties/ros_topic_property.hpp + include/rviz_common/properties/ros_topic_multi_property.hpp include/rviz_common/properties/status_list.hpp include/rviz_common/properties/status_property.hpp include/rviz_common/properties/string_property.hpp @@ -191,6 +192,7 @@ set(rviz_common_source_files src/rviz_common/properties/property_tree_with_help.cpp src/rviz_common/properties/property.cpp src/rviz_common/properties/ros_topic_property.cpp + src/rviz_common/properties/ros_topic_multi_property.cpp src/rviz_common/properties/quaternion_property.cpp src/rviz_common/properties/qos_profile_property.cpp src/rviz_common/properties/regex_filter_property.cpp diff --git a/rviz_common/include/rviz_common/properties/ros_topic_multi_property.hpp b/rviz_common/include/rviz_common/properties/ros_topic_multi_property.hpp new file mode 100644 index 000000000..df4f248a8 --- /dev/null +++ b/rviz_common/include/rviz_common/properties/ros_topic_multi_property.hpp @@ -0,0 +1,85 @@ +// Copyright (c) 2012, Willow Garage, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the 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_COMMON__PROPERTIES__ROS_TOPIC_MULTI_PROPERTY_HPP_ +#define RVIZ_COMMON__PROPERTIES__ROS_TOPIC_MULTI_PROPERTY_HPP_ + +#include + +#include +#include + +#include "rviz_common/properties/ros_topic_property.hpp" +#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" +#include "rviz_common/visibility_control.hpp" + +namespace rviz_common +{ +namespace properties +{ + +// Like RosTopicProperty but can accept multiple message types +class RVIZ_COMMON_PUBLIC RosTopicMultiProperty : public RosTopicProperty +{ + Q_OBJECT + +public: + explicit RosTopicMultiProperty( + const QString & name = QString(), const QString & default_value = QString(), + const std::vector & message_types = std::vector(), + const QString & description = QString(), Property * parent = nullptr, + const char * changed_slot = nullptr, QObject * receiver = nullptr) + : RosTopicProperty(name, default_value, "", description, parent, changed_slot, receiver), + message_types_(message_types) + { + } + + void setMessageTypes(const std::vector & message_types) + { + message_types_ = message_types; + } + + std::vector getMessageTypes() const {return message_types_;} + +protected Q_SLOTS: + void fillTopicList() override; + +private: + // Hide the parent class methods which only take a single type + using RosTopicProperty::getMessageType; + using RosTopicProperty::setMessageType; + + // Instead of one message type, store a list of allowed types + std::vector message_types_; +}; + +} // end namespace properties +} // end namespace rviz_common + +#endif // RVIZ_COMMON__PROPERTIES__ROS_TOPIC_MULTI_PROPERTY_HPP_ diff --git a/rviz_common/include/rviz_common/properties/ros_topic_property.hpp b/rviz_common/include/rviz_common/properties/ros_topic_property.hpp index 8e423065d..9913a7560 100644 --- a/rviz_common/include/rviz_common/properties/ros_topic_property.hpp +++ b/rviz_common/include/rviz_common/properties/ros_topic_property.hpp @@ -50,40 +50,31 @@ class RVIZ_COMMON_PUBLIC RosTopicProperty : public EditableEnumProperty public: explicit RosTopicProperty( - const QString & name = QString(), - const QString & default_value = QString(), - const QString & message_type = QString(), - const QString & description = QString(), - Property * parent = nullptr, - const char * changed_slot = nullptr, - QObject * receiver = nullptr); + const QString & name = QString(), const QString & default_value = QString(), + const QString & message_type = QString(), const QString & description = QString(), + Property * parent = nullptr, const char * changed_slot = nullptr, QObject * receiver = nullptr); void initialize(ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node); void setMessageType(const QString & message_type); - QString getMessageType() const - {return message_type_;} + QString getMessageType() const {return message_type_;} - QString getTopic() const - {return getValue().toString();} + QString getTopic() const {return getValue().toString();} - std::string getTopicStd() const - {return getValue().toString().toStdString();} + std::string getTopicStd() const {return getValue().toString().toStdString();} - bool isEmpty() const - {return getTopicStd().empty();} + bool isEmpty() const {return getTopicStd().empty();} protected Q_SLOTS: virtual void fillTopicList(); -private: +protected: ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node_; QString message_type_; }; -class RVIZ_COMMON_PUBLIC RosFilteredTopicProperty - : public rviz_common::properties::RosTopicProperty +class RVIZ_COMMON_PUBLIC RosFilteredTopicProperty : public rviz_common::properties::RosTopicProperty { Q_OBJECT diff --git a/rviz_common/src/rviz_common/properties/ros_topic_multi_property.cpp b/rviz_common/src/rviz_common/properties/ros_topic_multi_property.cpp new file mode 100644 index 000000000..b49c0e39a --- /dev/null +++ b/rviz_common/src/rviz_common/properties/ros_topic_multi_property.cpp @@ -0,0 +1,69 @@ +// Copyright (c) 2012, Willow Garage, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the 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. + +#include "rviz_common/properties/ros_topic_multi_property.hpp" + +#include // NOLINT: cpplint can't handle Qt imports +#include +#include +#include +#include + +#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" + +namespace rviz_common +{ +namespace properties +{ + +void RosTopicMultiProperty::fillTopicList() +{ + QApplication::setOverrideCursor(QCursor(Qt::WaitCursor)); + clearOptions(); + + std::map> published_topics = + rviz_ros_node_.lock()->get_topic_names_and_types(); + + for (const auto & topic : published_topics) { + // Only add topics whose type matches. + for (const auto & type : topic.second) { + if ( + std::find(message_types_.begin(), message_types_.end(), QString::fromStdString(type)) != + message_types_.end()) + { + addOptionStd(topic.first); + } + } + } + sortOptions(); + QApplication::restoreOverrideCursor(); +} + +} // end namespace properties +} // end namespace rviz_common diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index 868184054..679cbb975 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -921,6 +921,8 @@ if(BUILD_TESTING) ${SKIP_VISUAL_TESTS} TIMEOUT 180) if(TARGET image_display_visual_test) + find_package(OpenCV REQUIRED) + include_directories(${OpenCV_INCLUDE_DIRS}) target_include_directories(image_display_visual_test PRIVATE test) target_link_libraries(image_display_visual_test Qt${QT_VERSION_MAJOR}::Test @@ -928,6 +930,7 @@ if(BUILD_TESTING) rclcpp::rclcpp ${sensor_msgs_TARGETS} ${std_msgs_TARGETS} + ${OpenCV_LIBRARIES} ) endif() diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp index d896eb7bb..18d9c66b7 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp @@ -53,7 +53,7 @@ # include "sensor_msgs/msg/camera_info.hpp" # include "tf2_ros/message_filter.hpp" -# include "rviz_default_plugins/displays/image/image_transport_display.hpp" +# include "rviz_default_plugins/displays/image/image_display.hpp" # include "rviz_default_plugins/displays/image/ros_image_texture_iface.hpp" # include "rviz_default_plugins/visibility_control.hpp" # include "rviz_rendering/render_window.hpp" @@ -102,7 +102,7 @@ struct ImageDimensions * */ class RVIZ_DEFAULT_PLUGINS_PUBLIC CameraDisplay - : public rviz_default_plugins::displays::ImageTransportDisplay, + : public ImageDisplay, public Ogre::RenderTargetListener { Q_OBJECT @@ -192,9 +192,6 @@ private Q_SLOTS: std::shared_ptr> tf_filter_; - std::unique_ptr texture_; - std::unique_ptr render_panel_; - std::shared_ptr> cache_images_; rviz_common::properties::FloatProperty * alpha_property_; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp index d29106434..68998d4a1 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp @@ -29,37 +29,43 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. - #ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__IMAGE_DISPLAY_HPP_ #define RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__IMAGE_DISPLAY_HPP_ #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 -# include -# include - -# include // NOLINT cpplint cannot handle include order here - -# include -# include -# include - -# include "rviz_common/message_filter_display.hpp" -# include "rviz_common/render_panel.hpp" -# include "rviz_common/properties/bool_property.hpp" -# include "rviz_common/properties/float_property.hpp" -# include "rviz_common/properties/int_property.hpp" - -# include "rviz_default_plugins/displays/image/ros_image_texture_iface.hpp" -# include "rviz_default_plugins/visibility_control.hpp" -#include "rviz_default_plugins/displays/image/image_transport_display.hpp" +#include +#include +#include + +#include // NOLINT cpplint cannot handle include order here +#include // NOLINT cpplint cannot handle include order here + +#include +#include +#include +#include + +#include + +#include "rviz_common/message_filter_display.hpp" +#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/enum_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/int_property.hpp" +#include "rviz_common/render_panel.hpp" +#include "rviz_default_plugins/displays/image/ros_image_texture_iface.hpp" +#include "rviz_default_plugins/visibility_control.hpp" +#include "get_transport_from_topic.hpp" +#include "image_transport/image_transport.hpp" +#include "image_transport/subscriber_filter.hpp" +#include "rviz_common/ros_topic_display.hpp" #endif - namespace Ogre { class SceneNode; class Rectangle2D; -} +} // namespace Ogre namespace rviz_default_plugins { @@ -70,8 +76,7 @@ namespace displays * \class ImageDisplay * */ -class RVIZ_DEFAULT_PLUGINS_PUBLIC ImageDisplay : public - rviz_default_plugins::displays::ImageTransportDisplay +class RVIZ_DEFAULT_PLUGINS_PUBLIC ImageDisplay : public rviz_common::_RosTopicDisplay { Q_OBJECT @@ -80,21 +85,44 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC ImageDisplay : public ImageDisplay(); ~ImageDisplay() override; - // Overrides from Display void onInitialize() override; - void update(float wall_dt, float ros_dt) override; + void update(float wall_dt, float ros_dt); void reset() override; public Q_SLOTS: virtual void updateNormalizeOptions(); +protected Q_SLOTS: + virtual void subscribe(); + protected: - // overrides from Display void onEnable() override; void onDisable() override; + virtual void unsubscribe(); + void updateTopic() override; + void transformerChangedCallback() override; + void resetSubscription(); + void incomingMessage(const sensor_msgs::msg::Image::ConstSharedPtr & img_msg); + void setTopic(const QString & topic, const QString & datatype) override; /* This is called by incomingMessage(). */ - void processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg) override; + virtual void processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg); + + std::shared_ptr subscription_; + uint32_t messages_received_; + rclcpp::Time subscription_start_time_; + message_filters::Connection subscription_callback_; + const std::unordered_map transport_message_types_ = { + /* *INDENT-OFF* */ + {"raw", "sensor_msgs/msg/Image"}, + {"compressed", "sensor_msgs/msg/CompressedImage"}, + {"compressedDepth", "sensor_msgs/msg/CompressedImage"}, + {"theora", "theora_image_transport/msg/Packet"}, + {"zstd", "sensor_msgs/msg/CompressedImage"}, + /* *INDENT-ON* */ + }; + std::unique_ptr texture_; + std::unique_ptr render_panel_; private: void setupScreenRectangle(); @@ -105,10 +133,7 @@ public Q_SLOTS: std::unique_ptr screen_rect_; Ogre::MaterialPtr material_; - std::unique_ptr texture_; - - std::unique_ptr render_panel_; - + rviz_common::properties::EnumProperty * transport_override_property_; rviz_common::properties::BoolProperty * normalize_property_; rviz_common::properties::FloatProperty * min_property_; rviz_common::properties::FloatProperty * max_property_; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp deleted file mode 100644 index 9ec3e97d1..000000000 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp +++ /dev/null @@ -1,231 +0,0 @@ -// Copyright (c) 2012, Willow Garage, Inc. -// Copyright (c) 2017, Bosch Software Innovations GmbH. -// Copyright (c) 2020, TNG Technology Consulting 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__DISPLAYS__IMAGE__IMAGE_TRANSPORT_DISPLAY_HPP_ -#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__IMAGE_TRANSPORT_DISPLAY_HPP_ - -#include -#include - -#include // NOLINT: cpplint is unable to handle the include order here - -#include "get_transport_from_topic.hpp" -#include "image_transport/image_transport.hpp" -#include "image_transport/subscriber_filter.hpp" -#include "rviz_common/ros_topic_display.hpp" - -namespace rviz_default_plugins -{ -namespace displays -{ - -template -class ImageTransportDisplay : public rviz_common::_RosTopicDisplay -{ -// No Q_OBJECT macro here, moc does not support Q_OBJECT in a templated class. - -public: -/// Convenience typedef so subclasses don't have to use -/// the long templated class name to refer to their super class. - typedef ImageTransportDisplay ITDClass; - - ImageTransportDisplay() - : messages_received_(0) - { - QString message_type = QString::fromStdString(rosidl_generator_traits::name()); - topic_property_->setMessageType(message_type); - topic_property_->setDescription(message_type + " topic to subscribe to."); - } - -/** -* When overriding this method, the onInitialize() method of this superclass has to be called. -* Otherwise, the ros node will not be initialized. -*/ - void onInitialize() override - { - _RosTopicDisplay::onInitialize(); - } - - ~ImageTransportDisplay() override - { - unsubscribe(); - } - - void reset() override - { - Display::reset(); - messages_received_ = 0; - } - - void setTopic(const QString & topic, const QString & datatype) override - { - (void) datatype; - topic_property_->setString(topic); - } - -protected: - void updateTopic() override - { - resetSubscription(); - } - - virtual void subscribe() - { - if (!isEnabled()) { - return; - } - - if (topic_property_->isEmpty()) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - QString("Error subscribing: Empty topic name")); - return; - } - - try { - subscription_ = std::make_shared(); - rclcpp::Node::SharedPtr node = rviz_ros_node_.lock()->get_raw_node(); - subscription_->subscribe( - *node, - getBaseTopicFromTopic(topic_property_->getTopicStd()), - getTransportFromTopic(topic_property_->getTopicStd()), - qos_profile); - subscription_start_time_ = node->now(); - subscription_callback_ = subscription_->registerCallback( - std::bind( - &ImageTransportDisplay::incomingMessage, this, std::placeholders::_1)); - setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", "OK"); - } catch (rclcpp::exceptions::InvalidTopicNameError & e) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - QString("Error subscribing: ") + e.what()); - } catch (image_transport::TransportLoadException & e) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - QString("Error subscribing: ") + e.what()); - } - } - - void transformerChangedCallback() override - { - resetSubscription(); - } - - void resetSubscription() - { - unsubscribe(); - reset(); - subscribe(); - context_->queueRender(); - } - - virtual void unsubscribe() - { - subscription_.reset(); - } - - void onEnable() override - { - subscribe(); - } - - void onDisable() override - { - unsubscribe(); - reset(); - } - -/// Incoming message callback. -/** -* Checks if the message pointer -* is valid, increments messages_received_, then calls -* processMessage(). -*/ - void incomingMessage(const typename MessageType::ConstSharedPtr msg) - { - if (!msg || !isEnabled()) { - return; - } - - ++messages_received_; - QString topic_str = QString::number(messages_received_) + " messages received"; - rviz_common::properties::StatusProperty::Level topic_status_level = - rviz_common::properties::StatusProperty::Ok; - // Append topic subscription frequency if we can lock rviz_ros_node_. - std::shared_ptr node_interface = - rviz_ros_node_.lock(); - if (node_interface != nullptr) { - try { - const double duration = - (node_interface->get_raw_node()->now() - subscription_start_time_).seconds(); - const double subscription_frequency = - static_cast(messages_received_) / duration; - topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz."; - } catch (const std::runtime_error & e) { - if (std::string(e.what()).find("can't subtract times with different time sources") != - std::string::npos) - { - topic_status_level = rviz_common::properties::StatusProperty::Warn; - topic_str += ". "; - topic_str += e.what(); - } else { - throw; - } - } - } - setStatus( - topic_status_level, - "Topic", - topic_str); - - processMessage(msg); - } - - -/// Implement this to process the contents of a message. -/** -* This is called by incomingMessage(). -*/ - virtual void processMessage(typename MessageType::ConstSharedPtr msg) = 0; - - uint32_t messages_received_; - - std::shared_ptr subscription_; - rclcpp::Time subscription_start_time_; - message_filters::Connection subscription_callback_; -}; - -} // end namespace displays -} // end namespace rviz_default_plugins - - -#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__IMAGE_TRANSPORT_DISPLAY_HPP_ diff --git a/rviz_default_plugins/package.xml b/rviz_default_plugins/package.xml index 0f8ac63e7..ba01e455b 100644 --- a/rviz_default_plugins/package.xml +++ b/rviz_default_plugins/package.xml @@ -65,6 +65,7 @@ ament_lint_auto rviz_rendering_tests rviz_visual_testing_framework + vision_opencv ament_cmake diff --git a/rviz_default_plugins/plugins_description.xml b/rviz_default_plugins/plugins_description.xml index 6b0b581f6..6f145885c 100644 --- a/rviz_default_plugins/plugins_description.xml +++ b/rviz_default_plugins/plugins_description.xml @@ -120,6 +120,8 @@ The Image display creates a new rendering window with an image. sensor_msgs/msg/Image + sensor_msgs/msg/CompressedImage + theora_image_transport/msg/Packet ()), new_caminfo_(false), caminfo_ok_(false), force_render_(false) @@ -167,7 +166,7 @@ CameraDisplay::~CameraDisplay() void CameraDisplay::onInitialize() { - ITDClass::onInitialize(); + ImageDisplay::onInitialize(); setupSceneNodes(); setupRenderPanel(); @@ -311,7 +310,7 @@ void CameraDisplay::fixedFrameChanged() void CameraDisplay::subscribe() { - ITDClass::subscribe(); + ImageDisplay::subscribe(); if (!subscription_) { return; @@ -380,7 +379,7 @@ void CameraDisplay::createCameraInfoSubscription() void CameraDisplay::unsubscribe() { - ITDClass::unsubscribe(); + ImageDisplay::unsubscribe(); caminfo_sub_.reset(); tf_filter_.reset(); } @@ -671,7 +670,7 @@ void CameraDisplay::processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg) void CameraDisplay::reset() { - ITDClass::reset(); + ImageDisplay::reset(); clear(); } diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp index afcae40a4..db76e7d6f 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp @@ -28,10 +28,7 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include +#include "rviz_default_plugins/displays/image/image_display.hpp" #include #include @@ -44,22 +41,32 @@ #include #include #include +#include #include // NOLINT: cpplint is unable to handle the include order here -#include "sensor_msgs/image_encodings.hpp" +#include +#include +#include +#include +#include +#include +#include "image_transport/image_transport.hpp" +#include "image_transport/subscriber.hpp" #include "rviz_common/display_context.hpp" #include "rviz_common/frame_manager_iface.hpp" +#include "rviz_common/properties/ros_topic_multi_property.hpp" #include "rviz_common/render_panel.hpp" -#include "rviz_common/validate_floats.hpp" #include "rviz_common/uniform_string_stream.hpp" +#include "rviz_common/validate_floats.hpp" +#include "rviz_default_plugins/displays/image/get_transport_from_topic.hpp" +#include "rviz_default_plugins/displays/image/ros_image_texture.hpp" +#include "rviz_default_plugins/displays/image/ros_image_texture_iface.hpp" #include "rviz_rendering/material_manager.hpp" #include "rviz_rendering/render_window.hpp" +#include "sensor_msgs/image_encodings.hpp" -#include "rviz_default_plugins/displays/image/ros_image_texture.hpp" -#include "rviz_default_plugins/displays/image/image_display.hpp" -#include "rviz_default_plugins/displays/image/ros_image_texture_iface.hpp" namespace rviz_default_plugins { @@ -70,67 +77,247 @@ ImageDisplay::ImageDisplay() : ImageDisplay(std::make_unique()) {} ImageDisplay::ImageDisplay(std::unique_ptr texture) -: texture_(std::move(texture)) +: messages_received_(0), + texture_(std::move(texture)) { + // Remove the default single-type topic and replace with a multi-type topic property + // This allows us to display image and compressed image topics in the topic list + delete this->topic_property_; + this->topic_property_ = new rviz_common::properties::RosTopicMultiProperty( + "Topic", "", std::vector(), "Image transport topic to subscribe to.", this, + SLOT(updateTopic())); + + delete this->qos_profile_property_; + this->qos_profile_property_ = + new rviz_common::properties::QosProfileProperty(this->topic_property_, rclcpp::QoS(5)); + + transport_override_property_ = new rviz_common::properties::EnumProperty( + "Transport Override", "", QString("By default this display uses the topic name to ") + + QString("determine the image_transport type. If this is not possible, use this ") + + QString("field to manually set the transport."), + this->topic_property_, SLOT(subscribe()), this + ); + normalize_property_ = new rviz_common::properties::BoolProperty( - "Normalize Range", - true, + "Normalize Range", true, "If set to true, will try to estimate the range of possible values from the received images.", - this, - SLOT(updateNormalizeOptions())); + this, SLOT(updateNormalizeOptions())); min_property_ = new rviz_common::properties::FloatProperty( - "Min Value", - 0.0, - "Value which will be displayed as black.", - this, + "Min Value", 0.0, "Value which will be displayed as black.", this, SLOT(updateNormalizeOptions())); max_property_ = new rviz_common::properties::FloatProperty( - "Max Value", - 1.0, - "Value which will be displayed as white.", - this, + "Max Value", 1.0, "Value which will be displayed as white.", this, SLOT(updateNormalizeOptions())); median_buffer_size_property_ = new rviz_common::properties::IntProperty( - "Median window", - 5, - "Window size for median filter used for computing min/max.", - this, + "Median window", 5, "Window size for median filter used for computing min/max.", this, SLOT(updateNormalizeOptions())); got_float_image_ = false; } -void ImageDisplay::onInitialize() +// Need to override this method because of the new type RosTopicMultiProperty +void ImageDisplay::setTopic(const QString & topic, const QString & datatype) { - ITDClass::onInitialize(); + (void) datatype; + ((rviz_common::properties::RosTopicMultiProperty *)topic_property_) + ->setString(topic); +} +void ImageDisplay::onInitialize() +{ + _RosTopicDisplay::onInitialize(); updateNormalizeOptions(); setupScreenRectangle(); - setupRenderPanel(); render_panel_->getRenderWindow()->setupSceneAfterInit( - [this](Ogre::SceneNode * scene_node) { - scene_node->attachObject(screen_rect_.get()); - }); + [this](Ogre::SceneNode * scene_node) {scene_node->attachObject(screen_rect_.get());}); + + // Populate message types and transport overrides based on installed image_transport plugins + std::shared_ptr node = rviz_ros_node_.lock()->get_raw_node(); + image_transport::ImageTransport image_transport_( + image_transport::RequiredInterfaces( + node->get_node_base_interface(), + node->get_node_parameters_interface(), + node->get_node_logging_interface(), + node->get_node_timers_interface(), + node->get_node_topics_interface() + )); + std::vector loadable_transports = image_transport_.getLoadableTransports(); + std::vector message_types; + // Map to message types + transport_override_property_->clearOptions(); + transport_override_property_->addOptionStd(""); + for (std::string & transport : loadable_transports) { + transport = transport.substr(transport.find_last_of('/') + 1); + try { + message_types.push_back(QString::fromStdString(transport_message_types_.at(transport))); + transport_override_property_->addOptionStd(transport); + } catch (const std::out_of_range & e) { + // This case will be handled in subscribe + } + } + // Remove duplicates + message_types.erase( + std::unique(message_types.begin(), message_types.end()), message_types.end()); + // Update the message types to allow in the topic_property_ + ((rviz_common::properties::RosTopicMultiProperty *)topic_property_) + ->setMessageTypes(message_types); } -ImageDisplay::~ImageDisplay() = default; - -void ImageDisplay::onEnable() +ImageDisplay::~ImageDisplay() { - ITDClass::subscribe(); + unsubscribe(); } +void ImageDisplay::onEnable() {subscribe();} + void ImageDisplay::onDisable() { - ITDClass::unsubscribe(); - clear(); + unsubscribe(); + reset(); } +/// Incoming message callback. +/** +* Checks if the message pointer +* is valid, increments messages_received_, then calls +* processMessage(). +*/ +void ImageDisplay::incomingMessage(const sensor_msgs::msg::Image::ConstSharedPtr & img_msg) +{ + if (!img_msg) { + return; + } + + ++messages_received_; + QString topic_str = QString::number(messages_received_) + " messages received"; + rviz_common::properties::StatusProperty::Level topic_status_level = + rviz_common::properties::StatusProperty::Ok; + // Append topic subscription frequency if we can lock rviz_ros_node_. + std::shared_ptr node_interface = + rviz_ros_node_.lock(); + if (node_interface != nullptr) { + try { + const double duration = + (node_interface->get_raw_node()->now() - subscription_start_time_).seconds(); + const double subscription_frequency = + static_cast(messages_received_) / duration; + topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz."; + } catch (const std::runtime_error & e) { + if (std::string(e.what()).find("can't subtract times with different time sources") != + std::string::npos) + { + topic_status_level = rviz_common::properties::StatusProperty::Warn; + topic_str += ". "; + topic_str += e.what(); + } else { + throw; + } + } + } + setStatus( + topic_status_level, + "Topic", + topic_str); + + processMessage(img_msg); +} + + +void ImageDisplay::subscribe() +{ + if (!isEnabled()) { + return; + } + + if (topic_property_->isEmpty()) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + QString("Error subscribing: Empty topic name")); + return; + } + try { + rclcpp::Node::SharedPtr node = rviz_ros_node_.lock()->get_raw_node(); + image_transport::ImageTransport image_transport_(image_transport::RequiredInterfaces( + node->get_node_base_interface(), + node->get_node_parameters_interface(), + node->get_node_logging_interface(), + node->get_node_timers_interface(), + node->get_node_topics_interface() + )); + // Check which image_transport plugins are installed + std::vector transports = image_transport_.getLoadableTransports(); + std::string transports_str = ""; + rviz_common::properties::StatusProperty::Level transports_status_level = + rviz_common::properties::StatusProperty::Ok; + // Strip down to basic transport names, construct string for status display + for (std::string & transport : transports) { + transport = transport.substr(transport.find_last_of('/') + 1); + if (transport_message_types_.find(transport) == transport_message_types_.end()) { + transports_status_level = rviz_common::properties::StatusProperty::Warn; + transports_str += "(unknown: " + transport + "), "; + } else { + transports_str += transport + ", "; + } + } + setStatusStd(transports_status_level, "Image Transports Installed", transports_str); + // Use override property for transport hint if set, otherwise deduce from topic name + std::string transport_hint = transport_override_property_->getStdString(); + if (transport_hint.empty()) { + transport_hint = getTransportFromTopic(topic_property_->getStdString()); + } + // Check if the specified transport is in the list of loadable transports + if (std::find(transports.begin(), transports.end(), transport_hint) == transports.end()) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + QString("Error subscribing: Specified image transport is not installed")); + return; + } + // image_transport::Subscriber only requires one callback for "raw" and the other types are + // automatically converted. + subscription_ = std::make_shared(); + subscription_->subscribe( + image_transport::RequiredInterfaces( + node->get_node_base_interface(), + node->get_node_parameters_interface(), + node->get_node_logging_interface(), + node->get_node_timers_interface(), + node->get_node_topics_interface() + ), + getBaseTopicFromTopic(topic_property_->getTopicStd()), + transport_hint, + qos_profile); + subscription_start_time_ = node->now(); + subscription_callback_ = subscription_->registerCallback( + std::bind( + &rviz_default_plugins::displays::ImageDisplay::incomingMessage, + this, std::placeholders::_1)); + setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", "OK"); + } catch (rclcpp::exceptions::InvalidTopicNameError & e) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + QString("Error subscribing: ") + e.what()); + } +} + +void ImageDisplay::updateTopic() {resetSubscription();} + +void ImageDisplay::transformerChangedCallback() {resetSubscription();} + +void ImageDisplay::resetSubscription() +{ + unsubscribe(); + reset(); + subscribe(); + context_->queueRender(); +} + +void ImageDisplay::unsubscribe() {subscription_.reset();} + void ImageDisplay::updateNormalizeOptions() { if (got_float_image_) { @@ -152,15 +339,12 @@ void ImageDisplay::updateNormalizeOptions() } } -void ImageDisplay::clear() -{ - texture_->clear(); -} +void ImageDisplay::clear() {texture_->clear();} void ImageDisplay::update(float wall_dt, float ros_dt) { - (void) wall_dt; - (void) ros_dt; + (void)wall_dt; + (void)ros_dt; try { texture_->update(); @@ -190,14 +374,16 @@ void ImageDisplay::update(float wall_dt, float ros_dt) void ImageDisplay::reset() { - ITDClass::reset(); + Display::reset(); + messages_received_ = 0; clear(); } /* This is called by incomingMessage(). */ void ImageDisplay::processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg) { - bool got_float_image = msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1 || + bool got_float_image = + msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1 || msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1 || msg->encoding == sensor_msgs::image_encodings::TYPE_16SC1 || msg->encoding == sensor_msgs::image_encodings::MONO16; @@ -225,8 +411,7 @@ void ImageDisplay::setupScreenRectangle() material_->setDepthWriteEnabled(false); material_->setDepthCheckEnabled(false); - Ogre::TextureUnitState * tu = - material_->getTechnique(0)->getPass(0)->createTextureUnitState(); + Ogre::TextureUnitState * tu = material_->getTechnique(0)->getPass(0)->createTextureUnitState(); tu->setTextureName(texture_->getName()); tu->setTextureFiltering(Ogre::TFO_NONE); tu->setTextureAddressingMode(Ogre::TextureUnitState::TAM_CLAMP); 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..630398865 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/publishers/compressed_image_publisher.hpp @@ -0,0 +1,89 @@ +// 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 +#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); + +private: + void timer_callback(); + + rclcpp::TimerBase::SharedPtr timer; + rclcpp::Publisher::SharedPtr publisher; +}; + +CompressedImagePublisher::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)); +} + +void CompressedImagePublisher::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); +} + +} // namespace nodes + +#endif // RVIZ_DEFAULT_PLUGINS__PUBLISHERS__COMPRESSED_IMAGE_PUBLISHER_HPP_