From 467164b555615ebdb40340ebc9f4d517301b8d48 Mon Sep 17 00:00:00 2001 From: Andreas Greimel Date: Fri, 15 Jun 2018 06:52:53 +0200 Subject: [PATCH] Migrate MarkerArrayDisplay (#296) * Migrate MarkerArrayDisplay * Fix uncrustify * Extract common code in marker displays into marker_common * Add visual test * Add visual test for MarkerArray messages in MarkerDisplay * Implement review comments * [REMOVE BEFORE MERGE] Fix moved file includes --- .../default_plugin/marker_array_display.cpp | 88 ---- .../properties/queue_size_property.hpp | 2 + .../properties/queue_size_property.cpp | 5 + rviz_default_plugins/CMakeLists.txt | 30 +- .../displays/marker/marker_common.hpp | 202 +++++++++ .../displays/marker/marker_display.hpp | 162 +------- .../displays/marker/markers/arrow_marker.hpp | 2 +- .../marker/markers/line_list_marker.hpp | 2 +- .../marker/markers/line_marker_base.hpp | 2 +- .../marker/markers/line_strip_marker.hpp | 2 +- .../displays/marker/markers/marker_base.hpp | 6 +- .../marker/markers/marker_factory.hpp | 6 +- .../marker/markers/mesh_resource_marker.hpp | 2 +- .../displays/marker/markers/points_marker.hpp | 2 +- .../displays/marker/markers/shape_marker.hpp | 2 +- .../markers/text_view_facing_marker.hpp | 2 +- .../marker/markers/triangle_list_marker.hpp | 2 +- .../marker_array/marker_array_display.hpp | 79 ++++ rviz_default_plugins/plugins_description.xml | 11 + .../displays/marker/marker_common.cpp | 382 ++++++++++++++++++ .../displays/marker/marker_display.cpp | 377 +---------------- .../displays/marker/markers/arrow_marker.cpp | 10 +- .../marker/markers/line_list_marker.cpp | 6 +- .../marker/markers/line_marker_base.cpp | 11 +- .../marker/markers/line_strip_marker.cpp | 8 +- .../displays/marker/markers/marker_base.cpp | 8 +- .../marker/markers/marker_factory.cpp | 20 +- .../marker/markers/mesh_resource_marker.cpp | 8 +- .../displays/marker/markers/points_marker.cpp | 4 +- .../displays/marker/markers/shape_marker.cpp | 10 +- .../markers/text_view_facing_marker.cpp | 7 +- .../marker/markers/triangle_list_marker.cpp | 12 +- .../marker_array/marker_array_display.cpp | 88 ++++ ...rker_array_with_three_marker_types_ref.png | Bin 0 -> 26908 bytes .../test_marker_with_marker_array_ref.png | Bin 0 -> 26012 bytes ...isplay_test.cpp => marker_common_test.cpp} | 157 +++---- .../marker/marker_display_visual_test.cpp | 17 +- .../marker/markers/markers_test_fixture.cpp | 6 +- .../marker/markers/markers_test_fixture.hpp | 11 +- .../marker_array_display_visual_test.cpp | 53 +++ .../marker_array_display_page_object.cpp | 36 ++ .../marker_array_display_page_object.hpp | 40 +- .../publishers/marker_array_publisher.hpp | 99 +++++ .../publishers/marker_publisher.hpp | 43 +- 44 files changed, 1218 insertions(+), 804 deletions(-) delete mode 100644 rviz/src/rviz/default_plugin/marker_array_display.cpp create mode 100644 rviz_default_plugins/include/rviz_default_plugins/displays/marker/marker_common.hpp create mode 100644 rviz_default_plugins/include/rviz_default_plugins/displays/marker_array/marker_array_display.hpp create mode 100644 rviz_default_plugins/src/rviz_default_plugins/displays/marker/marker_common.cpp create mode 100644 rviz_default_plugins/src/rviz_default_plugins/displays/marker_array/marker_array_display.cpp create mode 100644 rviz_default_plugins/test/reference_images/test_marker_array_with_three_marker_types_ref.png create mode 100644 rviz_default_plugins/test/reference_images/test_marker_with_marker_array_ref.png rename rviz_default_plugins/test/rviz_default_plugins/displays/marker/{marker_display_test.cpp => marker_common_test.cpp} (75%) create mode 100644 rviz_default_plugins/test/rviz_default_plugins/displays/marker_array/marker_array_display_visual_test.cpp create mode 100644 rviz_default_plugins/test/rviz_default_plugins/page_objects/marker_array_display_page_object.cpp rename rviz/src/rviz/default_plugin/marker_array_display.h => rviz_default_plugins/test/rviz_default_plugins/page_objects/marker_array_display_page_object.hpp (58%) create mode 100644 rviz_default_plugins/test/rviz_default_plugins/publishers/marker_array_publisher.hpp diff --git a/rviz/src/rviz/default_plugin/marker_array_display.cpp b/rviz/src/rviz/default_plugin/marker_array_display.cpp deleted file mode 100644 index 9bec13f81..000000000 --- a/rviz/src/rviz/default_plugin/marker_array_display.cpp +++ /dev/null @@ -1,88 +0,0 @@ -/* - * Copyright (c) 2011, 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 Willow Garage, Inc. 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 OWNER 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/properties/ros_topic_property.h" -#include "rviz/properties/int_property.h" - -#include "marker_array_display.h" - -namespace rviz -{ - -MarkerArrayDisplay::MarkerArrayDisplay() - : MarkerDisplay() -{ - marker_topic_property_->setMessageType( QString::fromStdString( ros::message_traits::datatype() )); - marker_topic_property_->setValue( "visualization_marker_array" ); - marker_topic_property_->setDescription( "visualization_msgs::MarkerArray topic to subscribe to." ); - - queue_size_property_->setDescription( "Advanced: set the size of the incoming Marker message queue. " - " This should generally be at least a few times larger than the number of Markers in each MarkerArray." ); -} - -void MarkerArrayDisplay::subscribe() -{ - if ( !isEnabled() ) - { - return; - } - - std::string topic = marker_topic_property_->getTopicStd(); - if( !topic.empty() ) - { - array_sub_.shutdown(); - - try - { - array_sub_ = update_nh_.subscribe( topic, queue_size_property_->getInt(), &MarkerArrayDisplay::handleMarkerArray, this ); - setStatus( StatusProperty::Ok, "Topic", "OK" ); - } - catch( ros::Exception& e ) - { - setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() ); - } - } -} - -void MarkerArrayDisplay::unsubscribe() -{ - array_sub_.shutdown(); -} - -// I seem to need this wrapper function to make the compiler like my -// function pointer in the .subscribe() call above. -void MarkerArrayDisplay::handleMarkerArray( const visualization_msgs::MarkerArray::ConstPtr& array ) -{ - incomingMarkerArray( array ); -} - -} // end namespace rviz - -#include -PLUGINLIB_EXPORT_CLASS( rviz::MarkerArrayDisplay, rviz::Display ) diff --git a/rviz_common/include/rviz_common/properties/queue_size_property.hpp b/rviz_common/include/rviz_common/properties/queue_size_property.hpp index 7eca83656..7dd9a706d 100644 --- a/rviz_common/include/rviz_common/properties/queue_size_property.hpp +++ b/rviz_common/include/rviz_common/properties/queue_size_property.hpp @@ -46,6 +46,8 @@ class RVIZ_COMMON_PUBLIC QueueSizeProperty : public QObject public: QueueSizeProperty(_RosTopicDisplay * display, uint32_t default_size); + void setDescription(const QString & description); + private Q_SLOTS: void updateQueueSize(); diff --git a/rviz_common/src/rviz_common/properties/queue_size_property.cpp b/rviz_common/src/rviz_common/properties/queue_size_property.cpp index 6494ecb60..e0fa17fe1 100644 --- a/rviz_common/src/rviz_common/properties/queue_size_property.cpp +++ b/rviz_common/src/rviz_common/properties/queue_size_property.cpp @@ -45,6 +45,11 @@ QueueSizeProperty::QueueSizeProperty(_RosTopicDisplay * display, uint32_t defaul updateQueueSize(); } +void QueueSizeProperty::setDescription(const QString & description) +{ + queue_size_property_->setDescription(description); +} + void QueueSizeProperty::updateQueueSize() { display_->updateQoSProfile([this](rmw_qos_profile_t profile) -> rmw_qos_profile_t { diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index 69f6ade04..038984259 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -56,7 +56,9 @@ set(rviz_default_plugins_headers_to_moc include/rviz_default_plugins/displays/grid_cells/grid_cells_display.hpp include/rviz_default_plugins/displays/image/image_display.hpp include/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp + include/rviz_default_plugins/displays/marker/marker_common.hpp include/rviz_default_plugins/displays/marker/marker_display.hpp + include/rviz_default_plugins/displays/marker_array/marker_array_display.hpp include/rviz_default_plugins/displays/odometry/odometry_display.hpp include/rviz_default_plugins/displays/path/path_display.hpp include/rviz_default_plugins/displays/point/point_stamped_display.hpp @@ -109,7 +111,9 @@ set(rviz_default_plugins_source_files src/rviz_default_plugins/displays/marker/markers/shape_marker.cpp src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp src/rviz_default_plugins/displays/marker/markers/marker_factory.cpp + src/rviz_default_plugins/displays/marker/marker_common.cpp src/rviz_default_plugins/displays/marker/marker_display.cpp + src/rviz_default_plugins/displays/marker_array/marker_array_display.cpp src/rviz_default_plugins/displays/odometry/odometry_display.cpp src/rviz_default_plugins/displays/path/path_display.cpp src/rviz_default_plugins/displays/point/point_stamped_display.cpp @@ -259,6 +263,7 @@ if(BUILD_TESTING) ${urdf_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} ${nav_msgs_INCLUDE_DIRS} ${sensor_msgs_INCLUDE_DIRS} ${visualization_msgs_INCLUDE_DIRS} @@ -388,16 +393,16 @@ if(BUILD_TESTING) ${TEST_LINK_LIBRARIES}) endif() - ament_add_gmock(marker_display_test - test/rviz_default_plugins/displays/marker/marker_display_test.cpp + ament_add_gmock(marker_common_test + test/rviz_default_plugins/displays/marker/marker_common_test.cpp test/rviz_default_plugins/displays/display_test_fixture.cpp test/rviz_default_plugins/displays/marker/marker_messages.cpp test/rviz_default_plugins/scene_graph_introspection_helper.cpp APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_INSTALL_PREFIX} PATH=${CMAKE_INSTALL_PREFIX}/bin;${CMAKE_INSTALL_PREFIX}/opt/rviz_assimp_vendor/bin;${CMAKE_INSTALL_PREFIX}/opt/rviz_yaml_cpp_vendor/bin;${CMAKE_INSTALL_PREFIX}/opt/rviz_ogre_vendor/bin) - if(TARGET marker_display_test) - target_include_directories(marker_display_test PUBLIC + if(TARGET marker_common_test) + target_include_directories(marker_common_test PUBLIC ${TEST_INCLUDE_DIRS}) - target_link_libraries(marker_display_test + target_link_libraries(marker_common_test ${TEST_LINK_LIBRARIES}) endif() @@ -559,6 +564,21 @@ if(EnableVisualTests STREQUAL "True") rviz_visual_testing_framework::rviz_visual_testing_framework) endif() + ament_add_gtest(marker_array_display_visual_test + test/rviz_default_plugins/displays/marker_array/marker_array_display_visual_test.cpp + test/rviz_default_plugins/page_objects/marker_array_display_page_object.cpp + test/rviz_default_plugins/page_objects/marker_display_page_object.cpp + test/rviz_default_plugins/publishers/marker_array_publisher.hpp + TIMEOUT 180) + if(TARGET marker_array_display_visual_test) + target_include_directories(marker_array_display_visual_test PUBLIC + ${TEST_INCLUDE_DIRS} + ${rviz_visual_testing_framework_INCLUDE_DIRS}) + target_link_libraries(marker_array_display_visual_test + ${TEST_LINK_LIBRARIES} + rviz_visual_testing_framework::rviz_visual_testing_framework) + endif() + ament_add_gtest(marker_display_visual_test test/rviz_default_plugins/displays/marker/marker_display_visual_test.cpp test/rviz_default_plugins/page_objects/marker_display_page_object.cpp diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/marker_common.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/marker_common.hpp new file mode 100644 index 000000000..d673c0624 --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/marker_common.hpp @@ -0,0 +1,202 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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 OWNER 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__MARKER__MARKER_COMMON_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__MARKER__MARKER_COMMON_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "visualization_msgs/msg/marker.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/status_property.hpp" +#include "rviz_common/interaction/forwards.hpp" + +#include "rviz_default_plugins/visibility_control.hpp" + +namespace rviz_common +{ +class Display; +class DisplayContext; +class QueueSizeProperty; + +namespace properties +{ +class IntProperty; +} +} // namespace rviz_common + +using rviz_common::properties::StatusLevel; + +namespace rviz_default_plugins +{ +namespace displays +{ +class MarkerNamespace; + +namespace markers +{ +class MarkerBase; +class MarkerSelectionHandler; +class MarkerFactory; +} + +typedef std::shared_ptr MarkerSelectionHandlerPtr; +typedef std::shared_ptr MarkerBasePtr; +typedef std::pair MarkerID; + +/** + * \class MarkerCommon + * Common code shared by MarkerDisplay and MarkerArrayDisplay + */ +class RVIZ_DEFAULT_PLUGINS_PUBLIC MarkerCommon +{ +public: + explicit MarkerCommon(rviz_common::Display * display); + ~MarkerCommon(); + + void initialize(rviz_common::DisplayContext * context, Ogre::SceneNode * scene_node); + void load(const rviz_common::Config & config); + + void update(float wall_dt, float ros_dt); + + void deleteMarker(MarkerID id); + + /** @brief Add a new message to the queue, will be drawn on the next update(). */ + void addMessage(visualization_msgs::msg::Marker::ConstSharedPtr marker); + void addMessage(visualization_msgs::msg::MarkerArray::ConstSharedPtr array); + + /** + * \brief Processes a marker message + * @param message The message to process + */ + void processMessage(visualization_msgs::msg::Marker::ConstSharedPtr message); + + /** + * \brief Removes all the markers + */ + void clearMarkers(); + + /** @brief Delete all known markers to this plugin, regardless of id or namespace **/ + void deleteAllMarkers(); + + void setMarkerStatus(MarkerID id, StatusLevel level, const std::string & text); + void deleteMarkerStatus(MarkerID id); + +private: + /** @brief Delete all the markers within the given namespace. */ + void deleteMarkersInNamespace(const std::string & ns); + + /** + * \brief Processes an "Add" marker message + * @param message The message to process + */ + void processAdd(visualization_msgs::msg::Marker::ConstSharedPtr message); + /** + * \brief Processes a "Delete" marker message + * @param message The message to process + */ + void processDelete(visualization_msgs::msg::Marker::ConstSharedPtr message); + + typedef std::vector V_MarkerMessage; + V_MarkerMessage takeSnapshotOfMessageQueue(); + void processNewMessages(const V_MarkerMessage & local_queue); + void removeExpiredMarkers(); + + void updateMarkersWithLockedFrame() const; + QHash::const_iterator getMarkerNamespace( + const visualization_msgs::msg::Marker::ConstSharedPtr & message); + MarkerBasePtr createOrGetOldMarker( + const visualization_msgs::msg::Marker::ConstSharedPtr & message); + MarkerBasePtr createMarker(const visualization_msgs::msg::Marker::ConstSharedPtr & message); + void configureMarker( + const visualization_msgs::msg::Marker::ConstSharedPtr & message, MarkerBasePtr & marker); + + typedef std::map M_IDToMarker; + typedef std::set S_MarkerBase; + M_IDToMarker markers_; ///< Map of marker id to the marker info structure + S_MarkerBase markers_with_expiration_; + S_MarkerBase frame_locked_markers_; + ///< Marker message queue. Messages are added to this as they are received, and then processed + ///< in our update() function + V_MarkerMessage message_queue_; + std::mutex queue_mutex_; + + typedef QHash M_Namespace; + M_Namespace namespaces_; + + rviz_common::properties::Property * namespaces_category_; + + typedef std::map M_EnabledState; + M_EnabledState namespace_config_enabled_state_; + + std::unique_ptr marker_factory_; + + rviz_common::Display * display_; + rviz_common::DisplayContext * context_; + Ogre::SceneNode * scene_node_; + + friend class MarkerNamespace; +}; + +/** @brief Manager of a single marker namespace. Keeps a hash from + * marker IDs to MarkerBasePtr, and creates or destroys them when necessary. */ +class RVIZ_DEFAULT_PLUGINS_PUBLIC MarkerNamespace : public rviz_common::properties::BoolProperty +{ + Q_OBJECT + +public: + MarkerNamespace( + const QString & name, + rviz_common::properties::Property * parent_property, + MarkerCommon * owner + ); + bool isEnabled() const {return getBool();} + +public Q_SLOTS: + void onEnableChanged(); + +private: + MarkerCommon * owner_; +}; + +} // namespace displays +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__MARKER__MARKER_COMMON_HPP_ diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/marker_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/marker_display.hpp index 03027f5eb..c1bfa79c7 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/marker_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/marker_display.hpp @@ -1,5 +1,6 @@ /* * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -30,52 +31,21 @@ #ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__MARKER__MARKER_DISPLAY_HPP_ #define RVIZ_DEFAULT_PLUGINS__DISPLAYS__MARKER__MARKER_DISPLAY_HPP_ -#include #include -#include -#include -#include -#include -#include #include "visualization_msgs/msg/marker.hpp" #include "visualization_msgs/msg/marker_array.hpp" -#include "rviz_common/display.hpp" -#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/queue_size_property.hpp" #include "rviz_common/ros_topic_display.hpp" -#include "rviz_common/interaction/forwards.hpp" +#include "rviz_default_plugins/displays/marker/marker_common.hpp" #include "rviz_default_plugins/visibility_control.hpp" -namespace rviz_common -{ -class QueueSizeProperty; - -namespace properties -{ -class IntProperty; -} -} - -using rviz_common::properties::StatusLevel; - namespace rviz_default_plugins { namespace displays { -class RVIZ_DEFAULT_PLUGINS_PUBLIC MarkerNamespace; - -namespace markers -{ -class MarkerBase; -class MarkerSelectionHandler; -class MarkerFactory; -} - -typedef std::shared_ptr MarkerSelectionHandlerPtr; -typedef std::shared_ptr MarkerBasePtr; -typedef std::pair MarkerID; /** * \class MarkerDisplay @@ -84,144 +54,32 @@ typedef std::pair MarkerID; * Markers come in as visualization_msgs::msg::Marker messages. * See the Marker message for more information. */ -class RVIZ_DEFAULT_PLUGINS_PUBLIC MarkerDisplay : public - rviz_common::RosTopicDisplay +class RVIZ_DEFAULT_PLUGINS_PUBLIC MarkerDisplay + : public rviz_common::RosTopicDisplay { - Q_OBJECT - public: - // TODO(Martin-Idel-SI): Constructor for testing, remove once ros_nodes can be mocked and call - // initialize instead - explicit MarkerDisplay( - std::unique_ptr factory, - rviz_common::DisplayContext * display_context); MarkerDisplay(); - ~MarkerDisplay() override; void onInitialize() override; + void load(const rviz_common::Config & config) override; void update(float wall_dt, float ros_dt) override; - void fixedFrameChanged() override; void reset() override; - void deleteMarker(MarkerID id); - - /** @brief Delete all known markers to this plugin, regardless of id or namespace **/ - void deleteAllMarkers(); - - void setMarkerStatus(MarkerID id, StatusLevel level, const std::string & text); - void deleteMarkerStatus(MarkerID id); - - /** - * \brief Processes a marker message - * @param message The message to process - */ - void processMessage(visualization_msgs::msg::Marker::ConstSharedPtr message) override; - protected: - void onEnable() override; - void onDisable() override; - - void load(const rviz_common::Config & config) override; - - /** @brief Subscribes to the "visualization_marker" and - * "visualization_marker_array" topics. */ void subscribe() override; - - /** @brief Unsubscribes from the "visualization_marker" - * "visualization_marker_array" topics. */ void unsubscribe() override; - /** @brief Process a MarkerArray message. */ - void incomingMarkerArray(visualization_msgs::msg::MarkerArray::ConstSharedPtr array); - - rclcpp::Subscription::SharedPtr array_sub_; - std::unique_ptr queue_size_property_; - private: - /** @brief Delete all the markers within the given namespace. */ - void deleteMarkersInNamespace(const std::string & ns); - - /** - * \brief Removes all the markers - */ - void clearMarkers(); - - /** - * \brief Processes an "Add" marker message - * @param message The message to process - */ - void processAdd(visualization_msgs::msg::Marker::ConstSharedPtr message); - /** - * \brief Processes a "Delete" marker message - * @param message The message to process - */ - void processDelete(visualization_msgs::msg::Marker::ConstSharedPtr message); - - /** - * \brief ROS callback notifying us of a new marker - */ - void incomingMarker(visualization_msgs::msg::Marker::ConstSharedPtr marker); + void processMessage(visualization_msgs::msg::Marker::ConstSharedPtr msg) override; void createMarkerArraySubscription(); - typedef std::vector V_MarkerMessage; - V_MarkerMessage takeSnapshotOfMessageQueue(); - void processNewMessages(const V_MarkerMessage & local_queue); - void removeExpiredMarkers(); - - void updateMarkersWithLockedFrame() const; - QHash::const_iterator getMarkerNamespace( - const visualization_msgs::msg::Marker::ConstSharedPtr & message); - MarkerBasePtr createOrGetOldMarker( - const visualization_msgs::msg::Marker::ConstSharedPtr & message); - MarkerBasePtr createMarker(const visualization_msgs::msg::Marker::ConstSharedPtr & message); - void configureMarker( - const visualization_msgs::msg::Marker::ConstSharedPtr & message, MarkerBasePtr & marker); - - typedef std::map M_IDToMarker; - typedef std::set S_MarkerBase; - M_IDToMarker markers_; ///< Map of marker id to the marker info structure - S_MarkerBase markers_with_expiration_; - S_MarkerBase frame_locked_markers_; - ///< Marker message queue. Messages are added to this as they are received, and then processed - ///< in our update() function - V_MarkerMessage message_queue_; - std::mutex queue_mutex_; - - typedef QHash M_Namespace; - M_Namespace namespaces_; - - rviz_common::properties::Property * namespaces_category_; - - typedef std::map M_EnabledState; - M_EnabledState namespace_config_enabled_state_; - - std::unique_ptr marker_factory_; - - friend class MarkerNamespace; -}; - -/** @brief Manager of a single marker namespace. Keeps a hash from - * marker IDs to MarkerBasePtr, and creates or destroys them when necessary. */ -class MarkerNamespace : public rviz_common::properties::BoolProperty -{ - Q_OBJECT - -public: - MarkerNamespace( - const QString & name, - rviz_common::properties::Property * parent_property, - MarkerDisplay * owner - ); - bool isEnabled() const {return getBool();} - -public Q_SLOTS: - void onEnableChanged(); + rclcpp::Subscription::SharedPtr array_sub_; -private: - MarkerDisplay * owner_; + std::unique_ptr marker_common_; + std::unique_ptr queue_size_property_; }; } // namespace displays diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/arrow_marker.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/arrow_marker.hpp index 1afe7ea19..34185f437 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/arrow_marker.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/arrow_marker.hpp @@ -59,7 +59,7 @@ class ArrowMarker : public MarkerBase public: RVIZ_DEFAULT_PLUGINS_PUBLIC ArrowMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); RVIZ_DEFAULT_PLUGINS_PUBLIC ~ArrowMarker() override = default; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/line_list_marker.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/line_list_marker.hpp index b6e194c33..6b926ff29 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/line_list_marker.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/line_list_marker.hpp @@ -49,7 +49,7 @@ class LineListMarker : public LineMarkerBase public: RVIZ_DEFAULT_PLUGINS_PUBLIC LineListMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); RVIZ_DEFAULT_PLUGINS_PUBLIC ~LineListMarker() override = default; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/line_marker_base.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/line_marker_base.hpp index d06e2d74f..af17a28b4 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/line_marker_base.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/line_marker_base.hpp @@ -59,7 +59,7 @@ class LineMarkerBase : public MarkerBase public: RVIZ_DEFAULT_PLUGINS_PUBLIC LineMarkerBase( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); RVIZ_DEFAULT_PLUGINS_PUBLIC S_MaterialPtr getMaterials() override; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/line_strip_marker.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/line_strip_marker.hpp index 7c1ea76bd..bf8046cdc 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/line_strip_marker.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/line_strip_marker.hpp @@ -47,7 +47,7 @@ class LineStripMarker : public LineMarkerBase public: RVIZ_DEFAULT_PLUGINS_PUBLIC LineStripMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); RVIZ_DEFAULT_PLUGINS_PUBLIC ~LineStripMarker() override = default; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/marker_base.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/marker_base.hpp index cdbbe83ac..f213b6f51 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/marker_base.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/marker_base.hpp @@ -59,7 +59,7 @@ namespace rviz_default_plugins { namespace displays { -class MarkerDisplay; +class MarkerCommon; namespace markers { @@ -76,7 +76,7 @@ class MarkerBase RVIZ_DEFAULT_PLUGINS_PUBLIC MarkerBase( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); RVIZ_DEFAULT_PLUGINS_PUBLIC virtual ~MarkerBase(); @@ -136,7 +136,7 @@ class MarkerBase void extractMaterials(Ogre::Entity * entity, S_MaterialPtr & materials); - MarkerDisplay * owner_; + MarkerCommon * owner_; rviz_common::DisplayContext * context_; Ogre::SceneNode * scene_node_; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/marker_factory.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/marker_factory.hpp index 821275f10..72e9ccfde 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/marker_factory.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/marker_factory.hpp @@ -39,7 +39,7 @@ #include "visualization_msgs/msg/marker.hpp" #include "rviz_default_plugins/displays/marker/markers/marker_base.hpp" -#include "rviz_default_plugins/displays/marker/marker_display.hpp" +#include "rviz_default_plugins/displays/marker/marker_common.hpp" #include "rviz_default_plugins/visibility_control.hpp" namespace rviz_default_plugins @@ -71,7 +71,7 @@ class MarkerFactory */ RVIZ_DEFAULT_PLUGINS_PUBLIC void initialize( - MarkerDisplay * owner, rviz_common::DisplayContext * context, + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); /// Creates a new marker. @@ -85,7 +85,7 @@ class MarkerFactory createMarkerForType(visualization_msgs::msg::Marker::_type_type marker_type); private: - MarkerDisplay * owner_display_; + MarkerCommon * owner_; rviz_common::DisplayContext * context_; Ogre::SceneNode * parent_node_; }; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/mesh_resource_marker.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/mesh_resource_marker.hpp index 00df640b3..85b2b342a 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/mesh_resource_marker.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/mesh_resource_marker.hpp @@ -62,7 +62,7 @@ class MeshResourceMarker : public MarkerBase public: RVIZ_DEFAULT_PLUGINS_PUBLIC MeshResourceMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); RVIZ_DEFAULT_PLUGINS_PUBLIC ~MeshResourceMarker() override; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/points_marker.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/points_marker.hpp index e6c77749f..73680f0df 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/points_marker.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/points_marker.hpp @@ -53,7 +53,7 @@ class PointsMarker : public MarkerBase public: RVIZ_DEFAULT_PLUGINS_PUBLIC PointsMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); RVIZ_DEFAULT_PLUGINS_PUBLIC ~PointsMarker() override; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/shape_marker.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/shape_marker.hpp index 6694e510f..ebaa4a91f 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/shape_marker.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/shape_marker.hpp @@ -53,7 +53,7 @@ class ShapeMarker : public MarkerBase public: RVIZ_DEFAULT_PLUGINS_PUBLIC ShapeMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); RVIZ_DEFAULT_PLUGINS_PUBLIC S_MaterialPtr getMaterials() override; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/text_view_facing_marker.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/text_view_facing_marker.hpp index e17289b6a..2da68d811 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/text_view_facing_marker.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/text_view_facing_marker.hpp @@ -56,7 +56,7 @@ class TextViewFacingMarker : public MarkerBase public: RVIZ_DEFAULT_PLUGINS_PUBLIC TextViewFacingMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); RVIZ_DEFAULT_PLUGINS_PUBLIC ~TextViewFacingMarker() override; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/triangle_list_marker.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/triangle_list_marker.hpp index fb4434d3e..d321b6ff6 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/triangle_list_marker.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/triangle_list_marker.hpp @@ -72,7 +72,7 @@ class TriangleListMarker : public MarkerBase public: RVIZ_DEFAULT_PLUGINS_PUBLIC TriangleListMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node); RVIZ_DEFAULT_PLUGINS_PUBLIC ~TriangleListMarker() override; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker_array/marker_array_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker_array/marker_array_display.hpp new file mode 100644 index 000000000..b0401b32e --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/marker_array/marker_array_display.hpp @@ -0,0 +1,79 @@ +/* + * Copyright (c) 2011, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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 OWNER 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__MARKER_ARRAY__MARKER_ARRAY_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__MARKER_ARRAY__MARKER_ARRAY_DISPLAY_HPP_ + +#include "visualization_msgs/msg/marker_array.hpp" + +#include + +#include "rviz_common/properties/queue_size_property.hpp" +#include "rviz_common/ros_topic_display.hpp" + +#include "rviz_default_plugins/displays/marker/marker_common.hpp" +#include "rviz_default_plugins/visibility_control.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ + +/** + * \class MarkerArrayDisplay + * \brief Displays arrays of "markers" sent in by other ROS nodes on the "visualization_marker" topic + * + * Marker arrays come in as visualization_msgs::msg::MarkerArray messages. + * See the Marker message for more information. + */ +class RVIZ_DEFAULT_PLUGINS_PUBLIC MarkerArrayDisplay + : public rviz_common::RosTopicDisplay +{ +public: + MarkerArrayDisplay(); + + void onInitialize() override; + void load(const rviz_common::Config & config) override; + + void update(float wall_dt, float ros_dt) override; + + void reset() override; + +private: + void processMessage(visualization_msgs::msg::MarkerArray::ConstSharedPtr array) override; + + std::unique_ptr marker_common_; + std::unique_ptr queue_size_property_; +}; + +} // end namespace displays +} // end namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__MARKER_ARRAY__MARKER_ARRAY_DISPLAY_HPP_ diff --git a/rviz_default_plugins/plugins_description.xml b/rviz_default_plugins/plugins_description.xml index 2c6f98561..542d37b76 100644 --- a/rviz_default_plugins/plugins_description.xml +++ b/rviz_default_plugins/plugins_description.xml @@ -133,6 +133,17 @@ visualization_msgs/Marker + + + Displays visualization_msgs::MarkerArray messages without presuming the topic name ends in "_array". + More Information. + + visualization_msgs/MarkerArray + + +#include +#include +#include + +#include "rclcpp/duration.hpp" + +#include "rviz_common/properties/queue_size_property.hpp" +#include "rviz_common/validate_floats.hpp" + +#include "rviz_default_plugins/displays/marker/markers/marker_factory.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ + +MarkerCommon::MarkerCommon(rviz_common::Display * display) +: display_(display) +{ + namespaces_category_ = new rviz_common::properties::Property( + "Namespaces", QVariant(), "", display_); + marker_factory_ = std::make_unique(); +} + +MarkerCommon::~MarkerCommon() +{ + clearMarkers(); +} + +void MarkerCommon::initialize(rviz_common::DisplayContext * context, Ogre::SceneNode * scene_node) +{ + context_ = context; + scene_node_ = scene_node; + + namespace_config_enabled_state_.clear(); + + marker_factory_->initialize(this, context_, scene_node_); + + // TODO(greimela): Revisit after MessageFilter is available in ROS2 +// tf_filter_ = new tf::MessageFilter( *context_->getTFClient(), +// fixed_frame_.toStdString(), +// queue_size_property_->getInt(), +// update_nh_ ); +// +// tf_filter_->connectInput(sub_); +// tf_filter_->registerCallback(boost::bind(&MarkerCommon::incomingMarker, this, _1)); +// tf_filter_->registerFailureCallback(boost::bind(&MarkerCommon::failedMarker, this, _1, _2)); +} + +void MarkerCommon::load(const rviz_common::Config & config) +{ + rviz_common::Config c = config.mapGetChild("Namespaces"); + for (rviz_common::Config::MapIterator iter = c.mapIterator(); iter.isValid(); iter.advance() ) { + QString key = iter.currentKey(); + const rviz_common::Config & child = iter.currentChild(); + namespace_config_enabled_state_[key] = child.getValue().toBool(); + } +} + +void MarkerCommon::clearMarkers() +{ + markers_.clear(); + markers_with_expiration_.clear(); + frame_locked_markers_.clear(); + namespaces_category_->removeChildren(); + namespaces_.clear(); +} + +void MarkerCommon::deleteMarker(MarkerID id) +{ + deleteMarkerStatus(id); + + auto it = markers_.find(id); + if (it != markers_.end()) { + markers_with_expiration_.erase(it->second); + frame_locked_markers_.erase(it->second); + markers_.erase(it); + } +} + +void MarkerCommon::deleteMarkersInNamespace(const std::string & ns) +{ + std::vector to_delete; + + // TODO(anon): this is inefficient, should store every in-use id per namespace and lookup by that + for (auto const & marker : markers_) { + if (marker.first.first == ns) { + to_delete.push_back(marker.first); + } + } + + for (auto & marker : to_delete) { + deleteMarker(marker); + } +} + +void MarkerCommon::deleteAllMarkers() +{ + std::vector to_delete; + for (auto const & marker : markers_) { + to_delete.push_back(marker.first); + } + + for (auto & marker : to_delete) { + deleteMarker(marker); + } +} + +void MarkerCommon::setMarkerStatus(MarkerID id, StatusLevel level, const std::string & text) +{ + std::string marker_name = id.first + "/" + std::to_string(id.second); + display_->setStatusStd(level, marker_name, text); +} + +void MarkerCommon::deleteMarkerStatus(MarkerID id) +{ + std::string marker_name = id.first + "/" + std::to_string(id.second); + display_->deleteStatusStd(marker_name); +} + +void MarkerCommon::addMessage(const visualization_msgs::msg::Marker::ConstSharedPtr marker) +{ + std::unique_lock lock(queue_mutex_); + + message_queue_.push_back(marker); +} + +void MarkerCommon::addMessage( + const visualization_msgs::msg::MarkerArray::ConstSharedPtr array) +{ + for (auto const & marker : array->markers) { + addMessage(std::make_shared(marker)); + } +} + +// TODO(greimela): Revisit after MessageFilter is available in ROS2 +// void MarkerCommon::failedMarker(const ros::MessageEvent& +// marker_evt, tf::FilterFailureReason reason) +// { +// visualization_msgs::Marker::ConstPtr marker = marker_evt.getConstMessage(); +// if (marker->action == visualization_msgs::msg::Marker::DELETE || +// marker->action == visualization_msgs::msg::Marker::DELETEALL) +// { +// return this->processMessage(marker); +// } +// std::string authority = marker_evt.getPublisherName(); +// std::string error = context_->getFrameManager() +// ->discoverFailureReason(marker->header.frame_id, marker->header.stamp, authority, reason); +// setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error); +// } + +bool validateFloats(const visualization_msgs::msg::Marker & msg) +{ + bool valid = true; + valid = valid && rviz_common::validateFloats(msg.pose); + valid = valid && rviz_common::validateFloats(msg.scale); + valid = valid && rviz_common::validateFloats(msg.color); + valid = valid && rviz_common::validateFloats(msg.points); + return valid; +} + +void MarkerCommon::processMessage(const visualization_msgs::msg::Marker::ConstSharedPtr message) +{ + if (!validateFloats(*message)) { + setMarkerStatus( + MarkerID(message->ns, message->id), + rviz_common::properties::StatusProperty::Error, + "Contains invalid floating point values (nans or infs)"); + return; + } + + switch (message->action) { + case visualization_msgs::msg::Marker::ADD: + processAdd(message); + break; + + case visualization_msgs::msg::Marker::DELETE: + processDelete(message); + break; + + case visualization_msgs::msg::Marker::DELETEALL: + deleteAllMarkers(); + break; + + default: + RVIZ_COMMON_LOG_ERROR_STREAM("Unknown marker action: " << message->action); + } +} + +QHash::const_iterator MarkerCommon::getMarkerNamespace( + const visualization_msgs::msg::Marker::ConstSharedPtr & message) +{ + QString namespace_name = QString::fromStdString(message->ns); + auto ns_it = namespaces_.find(namespace_name); + if (ns_it == namespaces_.end() ) { + ns_it = namespaces_.insert( + namespace_name, new MarkerNamespace(namespace_name, namespaces_category_, this)); + + // Adding a new namespace, determine if it's configured to be disabled + if (namespace_config_enabled_state_.count(namespace_name) > 0 && + !namespace_config_enabled_state_[namespace_name]) + { + ns_it.value()->setValue(false); // Disable the namespace + } + } + return ns_it; +} + +void MarkerCommon::processAdd(const visualization_msgs::msg::Marker::ConstSharedPtr message) +{ + auto ns_it = getMarkerNamespace(message); + + if (!ns_it.value()->isEnabled() ) { + return; + } + + deleteMarkerStatus(MarkerID(message->ns, message->id)); + + MarkerBasePtr marker = createOrGetOldMarker(message); + + if (marker) { + configureMarker(message, marker); + } +} + +MarkerBasePtr MarkerCommon::createOrGetOldMarker( + const visualization_msgs::msg::Marker::ConstSharedPtr & message) +{ + MarkerBasePtr marker; + auto it = markers_.find(MarkerID(message->ns, message->id)); + if (it != markers_.end()) { + marker = it->second; + markers_with_expiration_.erase(marker); + if (message->type != marker->getMessage()->type) { + markers_.erase(it); + marker = createMarker(message); + } + } else { + marker = createMarker(message); + } + return marker; +} + +MarkerBasePtr MarkerCommon::createMarker( + const visualization_msgs::msg::Marker::ConstSharedPtr & message) +{ + auto marker = marker_factory_->createMarkerForType(message->type); + markers_.insert(make_pair(MarkerID(message->ns, message->id), marker)); + return marker; +} + +void MarkerCommon::configureMarker( + const visualization_msgs::msg::Marker::ConstSharedPtr & message, MarkerBasePtr & marker) +{ + marker->setMessage(message); + + if (rclcpp::Duration(message->lifetime).nanoseconds() > 100000) { + markers_with_expiration_.insert(marker); + } + + if (message->frame_locked) { + frame_locked_markers_.insert(marker); + } + + context_->queueRender(); +} + +void MarkerCommon::processDelete(const visualization_msgs::msg::Marker::ConstSharedPtr message) +{ + deleteMarker(MarkerID(message->ns, message->id)); + + context_->queueRender(); +} + +void MarkerCommon::update(float wall_dt, float ros_dt) +{ + (void) wall_dt; + (void) ros_dt; + + MarkerCommon::V_MarkerMessage local_queue = takeSnapshotOfMessageQueue(); + processNewMessages(local_queue); + removeExpiredMarkers(); + updateMarkersWithLockedFrame(); +} + +MarkerCommon::V_MarkerMessage MarkerCommon::takeSnapshotOfMessageQueue() +{ + std::unique_lock lock(queue_mutex_); + + V_MarkerMessage local_queue; + local_queue.swap(message_queue_); + + return local_queue; +} + +void MarkerCommon::processNewMessages(const MarkerCommon::V_MarkerMessage & local_queue) +{ + if (!local_queue.empty()) { + for (auto const & message : local_queue) { + processMessage(message); + } + } +} + +void MarkerCommon::removeExpiredMarkers() +{ + auto marker_it = markers_with_expiration_.begin(); + auto end = markers_with_expiration_.end(); + for (; marker_it != end; ) { + MarkerBasePtr marker = *marker_it; + if (marker->expired()) { + ++marker_it; + deleteMarker(marker->getID()); + } else { + ++marker_it; + } + } +} + +void MarkerCommon::updateMarkersWithLockedFrame() const +{ + for (auto const & locked_marker : frame_locked_markers_) { + locked_marker->updateFrameLocked(); + } +} + +MarkerNamespace::MarkerNamespace( + const QString & name, rviz_common::properties::Property * parent_property, MarkerCommon * owner) +: BoolProperty(name, true, "Enable/disable all markers in this namespace.", parent_property), + owner_(owner) +{ + // Can't do this connect in chained constructor above because at + // that point it doesn't really know that "this" is a + // MarkerNamespace*, so the signal doesn't get connected. + connect(this, SIGNAL(changed()), this, SLOT(onEnableChanged())); +} + +void MarkerNamespace::onEnableChanged() +{ + if (!isEnabled()) { + owner_->deleteMarkersInNamespace(getName().toStdString()); + } + + // Update the configuration that stores the enabled state of all markers + owner_->namespace_config_enabled_state_[getName()] = isEnabled(); +} + +} // namespace displays +} // namespace rviz_default_plugins diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/marker_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/marker_display.cpp index 1627f3a23..2a53303fd 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/marker_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/marker_display.cpp @@ -1,5 +1,6 @@ /* * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -30,109 +31,33 @@ #include "rviz_default_plugins/displays/marker/marker_display.hpp" #include -#include -#include -#include -#include "rclcpp/duration.hpp" - -#include "rviz_rendering/objects/arrow.hpp" -#include "rviz_rendering/objects/billboard_line.hpp" -#include "rviz_rendering/objects/shape.hpp" -#include "rviz_common/properties/int_property.hpp" #include "rviz_common/properties/queue_size_property.hpp" -#include "rviz_common/validate_floats.hpp" - -#include "rviz_default_plugins/displays/marker/markers/arrow_marker.hpp" -#include "rviz_default_plugins/displays/marker/markers/line_list_marker.hpp" -#include "rviz_default_plugins/displays/marker/markers/line_strip_marker.hpp" -#include "rviz_default_plugins/displays/marker/markers/mesh_resource_marker.hpp" -#include "rviz_default_plugins/displays/marker/markers/points_marker.hpp" -#include "rviz_default_plugins/displays/marker/markers/shape_marker.hpp" -#include "rviz_default_plugins/displays/marker/markers/text_view_facing_marker.hpp" -#include "rviz_default_plugins/displays/marker/markers/triangle_list_marker.hpp" -#include "rviz_default_plugins/displays/marker/markers/marker_factory.hpp" namespace rviz_default_plugins { namespace displays { -MarkerDisplay::MarkerDisplay( - std::unique_ptr factory, rviz_common::DisplayContext * display_context) -: MarkerDisplay() -{ - marker_factory_ = std::move(factory); - context_ = display_context; -} - MarkerDisplay::MarkerDisplay() -: queue_size_property_(std::make_unique(this, 10)) -{ - topic_property_->setDescription( - "visualization_msgs::msg::Marker topic to subscribe to. _array will also" - " automatically be subscribed with type visualization_msgs::msg::MarkerArray."); - - namespaces_category_ = new rviz_common::properties::Property("Namespaces", QVariant(), "", this); - marker_factory_ = std::make_unique(); -} +: rviz_common::RosTopicDisplay(), + marker_common_(std::make_unique(this)), + queue_size_property_(std::make_unique(this, 10)) {} void MarkerDisplay::onInitialize() { RTDClass::onInitialize(); - namespace_config_enabled_state_.clear(); - - marker_factory_->initialize(this, context_, scene_node_); - - // TODO(greimela): Revisit after MessageFilter is available in ROS2 -// tf_filter_ = new tf::MessageFilter( *context_->getTFClient(), -// fixed_frame_.toStdString(), -// queue_size_property_->getInt(), -// update_nh_ ); -// -// tf_filter_->connectInput(sub_); -// tf_filter_->registerCallback(boost::bind(&MarkerDisplay::incomingMarker, this, _1)); -// tf_filter_->registerFailureCallback(boost::bind(&MarkerDisplay::failedMarker, this, _1, _2)); -} + marker_common_->initialize(context_, scene_node_); -MarkerDisplay::~MarkerDisplay() -{ - if (initialized()) { - unsubscribe(); - clearMarkers(); - } + topic_property_->setDescription( + "visualization_msgs::msg::Marker topic to subscribe to. _array will also" + " automatically be subscribed with type visualization_msgs::msg::MarkerArray."); } void MarkerDisplay::load(const rviz_common::Config & config) { rviz_common::Display::load(config); - - rviz_common::Config c = config.mapGetChild("Namespaces"); - for (rviz_common::Config::MapIterator iter = c.mapIterator(); iter.isValid(); iter.advance() ) { - QString key = iter.currentKey(); - const rviz_common::Config & child = iter.currentChild(); - namespace_config_enabled_state_[key] = child.getValue().toBool(); - } -} - -void MarkerDisplay::clearMarkers() -{ - markers_.clear(); - markers_with_expiration_.clear(); - frame_locked_markers_.clear(); - namespaces_category_->removeChildren(); - namespaces_.clear(); -} - -void MarkerDisplay::onEnable() -{ - subscribe(); -} - -void MarkerDisplay::onDisable() -{ - unsubscribe(); - clearMarkers(); + marker_common_->load(config); } void MarkerDisplay::subscribe() @@ -154,7 +79,7 @@ void MarkerDisplay::createMarkerArraySubscription() template create_subscription( topic_property_->getTopicStd() + "_array", [this](visualization_msgs::msg::MarkerArray::ConstSharedPtr msg) { - incomingMarkerArray(msg); + marker_common_->addMessage(msg); }, qos_profile); setStatus(StatusLevel::Ok, "Array Topic", "OK"); @@ -169,295 +94,21 @@ void MarkerDisplay::unsubscribe() array_sub_.reset(); } -void MarkerDisplay::deleteMarker(MarkerID id) -{ - deleteMarkerStatus(id); - - auto it = markers_.find(id); - if (it != markers_.end()) { - markers_with_expiration_.erase(it->second); - frame_locked_markers_.erase(it->second); - markers_.erase(it); - } -} - -void MarkerDisplay::deleteMarkersInNamespace(const std::string & ns) -{ - std::vector to_delete; - - // TODO(anon): this is inefficient, should store every in-use id per namespace and lookup by that - for (auto const & marker : markers_) { - if (marker.first.first == ns) { - to_delete.push_back(marker.first); - } - } - - for (auto & marker : to_delete) { - deleteMarker(marker); - } -} - -void MarkerDisplay::deleteAllMarkers() -{ - std::vector to_delete; - for (auto const & marker : markers_) { - to_delete.push_back(marker.first); - } - - for (auto & marker : to_delete) { - deleteMarker(marker); - } -} - -void MarkerDisplay::setMarkerStatus(MarkerID id, StatusLevel level, const std::string & text) -{ - std::string marker_name = id.first + "/" + std::to_string(id.second); - setStatusStd(level, marker_name, text); -} - -void MarkerDisplay::deleteMarkerStatus(MarkerID id) -{ - std::string marker_name = id.first + "/" + std::to_string(id.second); - deleteStatusStd(marker_name); -} -void MarkerDisplay::incomingMarkerArray( - const visualization_msgs::msg::MarkerArray::ConstSharedPtr array) +void MarkerDisplay::processMessage(const visualization_msgs::msg::Marker::ConstSharedPtr msg) { - for (auto const & marker : array->markers) { - incomingMarker(std::make_shared(marker)); - } -} - -void MarkerDisplay::incomingMarker(const visualization_msgs::msg::Marker::ConstSharedPtr marker) -{ - std::unique_lock lock(queue_mutex_); - - message_queue_.push_back(marker); -} - -// TODO(greimela): Revisit after MessageFilter is available in ROS2 -// void MarkerDisplay::failedMarker(const ros::MessageEvent& -// marker_evt, tf::FilterFailureReason reason) -// { -// visualization_msgs::Marker::ConstPtr marker = marker_evt.getConstMessage(); -// if (marker->action == visualization_msgs::msg::Marker::DELETE || -// marker->action == visualization_msgs::msg::Marker::DELETEALL) -// { -// return this->processMessage(marker); -// } -// std::string authority = marker_evt.getPublisherName(); -// std::string error = context_->getFrameManager() -// ->discoverFailureReason(marker->header.frame_id, marker->header.stamp, authority, reason); -// setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error); -// } - -bool validateFloats(const visualization_msgs::msg::Marker & msg) -{ - bool valid = true; - valid = valid && rviz_common::validateFloats(msg.pose); - valid = valid && rviz_common::validateFloats(msg.scale); - valid = valid && rviz_common::validateFloats(msg.color); - valid = valid && rviz_common::validateFloats(msg.points); - return valid; -} - -void MarkerDisplay::processMessage(const visualization_msgs::msg::Marker::ConstSharedPtr message) -{ - if (!validateFloats(*message)) { - setMarkerStatus( - MarkerID(message->ns, message->id), - rviz_common::properties::StatusProperty::Error, - "Contains invalid floating point values (nans or infs)"); - return; - } - - switch (message->action) { - case visualization_msgs::msg::Marker::ADD: - processAdd(message); - break; - - case visualization_msgs::msg::Marker::DELETE: - processDelete(message); - break; - - case visualization_msgs::msg::Marker::DELETEALL: - deleteAllMarkers(); - break; - - default: - RVIZ_COMMON_LOG_ERROR_STREAM("Unknown marker action: " << message->action); - } -} - -QHash::const_iterator MarkerDisplay::getMarkerNamespace( - const visualization_msgs::msg::Marker::ConstSharedPtr & message) -{ - QString namespace_name = QString::fromStdString(message->ns); - auto ns_it = namespaces_.find(namespace_name); - if (ns_it == namespaces_.end() ) { - ns_it = namespaces_.insert( - namespace_name, new MarkerNamespace(namespace_name, namespaces_category_, this)); - - // Adding a new namespace, determine if it's configured to be disabled - if (namespace_config_enabled_state_.count(namespace_name) > 0 && - !namespace_config_enabled_state_[namespace_name]) - { - ns_it.value()->setValue(false); // Disable the namespace - } - } - return ns_it; -} - -void MarkerDisplay::processAdd(const visualization_msgs::msg::Marker::ConstSharedPtr message) -{ - auto ns_it = getMarkerNamespace(message); - - if (!ns_it.value()->isEnabled() ) { - return; - } - - deleteMarkerStatus(MarkerID(message->ns, message->id)); - - MarkerBasePtr marker = createOrGetOldMarker(message); - - if (marker) { - configureMarker(message, marker); - } -} - -MarkerBasePtr MarkerDisplay::createOrGetOldMarker( - const visualization_msgs::msg::Marker::ConstSharedPtr & message) -{ - MarkerBasePtr marker; - auto it = markers_.find(MarkerID(message->ns, message->id)); - if (it != markers_.end()) { - marker = it->second; - markers_with_expiration_.erase(marker); - if (message->type != marker->getMessage()->type) { - markers_.erase(it); - marker = createMarker(message); - } - } else { - marker = createMarker(message); - } - return marker; -} - -MarkerBasePtr MarkerDisplay::createMarker( - const visualization_msgs::msg::Marker::ConstSharedPtr & message) -{ - auto marker = marker_factory_->createMarkerForType(message->type); - markers_.insert(make_pair(MarkerID(message->ns, message->id), marker)); - return marker; -} - -void MarkerDisplay::configureMarker( - const visualization_msgs::msg::Marker::ConstSharedPtr & message, MarkerBasePtr & marker) -{ - marker->setMessage(message); - - if (rclcpp::Duration(message->lifetime).nanoseconds() > 100000) { - markers_with_expiration_.insert(marker); - } - - if (message->frame_locked) { - frame_locked_markers_.insert(marker); - } - - context_->queueRender(); -} - -void MarkerDisplay::processDelete(const visualization_msgs::msg::Marker::ConstSharedPtr message) -{ - deleteMarker(MarkerID(message->ns, message->id)); - - context_->queueRender(); + marker_common_->addMessage(msg); } void MarkerDisplay::update(float wall_dt, float ros_dt) { - (void) wall_dt; - (void) ros_dt; - - MarkerDisplay::V_MarkerMessage local_queue = takeSnapshotOfMessageQueue(); - processNewMessages(local_queue); - removeExpiredMarkers(); - updateMarkersWithLockedFrame(); -} - -MarkerDisplay::V_MarkerMessage MarkerDisplay::takeSnapshotOfMessageQueue() -{ - std::unique_lock lock(queue_mutex_); - - V_MarkerMessage local_queue; - local_queue.swap(message_queue_); - - return local_queue; -} - -void MarkerDisplay::processNewMessages(const MarkerDisplay::V_MarkerMessage & local_queue) -{ - if (!local_queue.empty()) { - for (auto const & message : local_queue) { - processMessage(message); - } - } -} - -void MarkerDisplay::removeExpiredMarkers() -{ - auto marker_it = markers_with_expiration_.begin(); - auto end = markers_with_expiration_.end(); - for (; marker_it != end; ) { - MarkerBasePtr marker = *marker_it; - if (marker->expired()) { - ++marker_it; - deleteMarker(marker->getID()); - } else { - ++marker_it; - } - } -} - -void MarkerDisplay::updateMarkersWithLockedFrame() const -{ - for (auto const & locked_marker : frame_locked_markers_) { - locked_marker->updateFrameLocked(); - } -} - -void MarkerDisplay::fixedFrameChanged() -{ - RTDClass::fixedFrameChanged(); - clearMarkers(); + marker_common_->update(wall_dt, ros_dt); } void MarkerDisplay::reset() { RTDClass::reset(); - clearMarkers(); -} - -MarkerNamespace::MarkerNamespace( - const QString & name, rviz_common::properties::Property * parent_property, MarkerDisplay * owner) -: BoolProperty(name, true, "Enable/disable all markers in this namespace.", parent_property), - owner_(owner) -{ - // Can't do this connect in chained constructor above because at - // that point it doesn't really know that "this" is a - // MarkerNamespace*, so the signal doesn't get connected. - connect(this, SIGNAL(changed()), this, SLOT(onEnableChanged())); -} - -void MarkerNamespace::onEnableChanged() -{ - if (!isEnabled()) { - owner_->deleteMarkersInNamespace(getName().toStdString()); - } - - // Update the configuration that stores the enabled state of all markers - owner_->namespace_config_enabled_state_[getName()] = isEnabled(); + marker_common_->clearMarkers(); } } // namespace displays diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/arrow_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/arrow_marker.cpp index 07ee17eb7..3eb198c35 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/arrow_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/arrow_marker.cpp @@ -50,12 +50,12 @@ #include #include -#include "rviz_default_plugins/displays/marker/marker_display.hpp" -#include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" -#include "rviz_common/display_context.hpp" -#include "rviz_common/properties/status_property.hpp" #include "rviz_rendering/objects/arrow.hpp" #include "rviz_rendering/objects/shape.hpp" +#include "rviz_common/display_context.hpp" + +#include "rviz_default_plugins/displays/marker/marker_common.hpp" +#include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" namespace rviz_default_plugins { @@ -65,7 +65,7 @@ namespace markers { ArrowMarker::ArrowMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) : MarkerBase(owner, context, parent_node), arrow_(nullptr), last_arrow_set_from_points_(false) {} diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_list_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_list_marker.cpp index 77a022da2..4265e4583 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_list_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_list_marker.cpp @@ -36,10 +36,10 @@ #include #include -#include "rviz_default_plugins/displays/marker/marker_display.hpp" - #include "rviz_rendering/objects/billboard_line.hpp" +#include "rviz_default_plugins/displays/marker/marker_common.hpp" + namespace rviz_default_plugins { namespace displays @@ -48,7 +48,7 @@ namespace markers { LineListMarker::LineListMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) : LineMarkerBase(owner, context, parent_node) {} diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_marker_base.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_marker_base.cpp index 92f82581d..d89076c46 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_marker_base.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_marker_base.cpp @@ -38,12 +38,11 @@ #include #include -#include "rviz_default_plugins/displays/marker/marker_display.hpp" -#include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" - -#include "rviz_common/display_context.hpp" -#include "rviz_common/properties/status_property.hpp" #include "rviz_rendering/objects/billboard_line.hpp" +#include "rviz_common/display_context.hpp" + +#include "rviz_default_plugins/displays/marker/marker_common.hpp" +#include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" namespace rviz_default_plugins { @@ -53,7 +52,7 @@ namespace markers { LineMarkerBase::LineMarkerBase( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) : MarkerBase(owner, context, parent_node), lines_(nullptr), has_per_point_color_(false) {} diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_strip_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_strip_marker.cpp index 0f29439a8..c85cdd3bb 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_strip_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_strip_marker.cpp @@ -36,10 +36,10 @@ #include #include -#include "rviz_default_plugins/displays/marker/marker_display.hpp" - -#include "rviz_common/display_context.hpp" #include "rviz_rendering/objects/billboard_line.hpp" +#include "rviz_common/display_context.hpp" + +#include "rviz_default_plugins/displays/marker/marker_common.hpp" namespace rviz_default_plugins { @@ -49,7 +49,7 @@ namespace markers { LineStripMarker::LineStripMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) : LineMarkerBase(owner, context, parent_node) {} diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_base.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_base.cpp index ab13ee772..749262ceb 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_base.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_base.cpp @@ -51,11 +51,11 @@ #include "rclcpp/rclcpp.hpp" -#include "rviz_default_plugins/displays/marker/marker_display.hpp" -#include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" #include "rviz_common/display_context.hpp" #include "rviz_common/frame_manager_iface.hpp" -#include "rviz_common/properties/status_property.hpp" + +#include "rviz_default_plugins/displays/marker/marker_common.hpp" +#include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" namespace rviz_default_plugins { @@ -65,7 +65,7 @@ namespace markers { MarkerBase::MarkerBase( - MarkerDisplay * owner, + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) : owner_(owner), diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_factory.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_factory.cpp index 0ff635154..d8dac4254 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_factory.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_factory.cpp @@ -61,9 +61,9 @@ namespace markers { void MarkerFactory::initialize( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) { - owner_display_ = owner; + owner_ = owner; context_ = context; parent_node_ = parent_node; } @@ -75,30 +75,30 @@ std::shared_ptr MarkerFactory::createMarkerForType( case visualization_msgs::msg::Marker::CUBE: case visualization_msgs::msg::Marker::CYLINDER: case visualization_msgs::msg::Marker::SPHERE: - return std::make_shared(owner_display_, context_, parent_node_); + return std::make_shared(owner_, context_, parent_node_); case visualization_msgs::msg::Marker::ARROW: - return std::make_shared(owner_display_, context_, parent_node_); + return std::make_shared(owner_, context_, parent_node_); case visualization_msgs::msg::Marker::LINE_STRIP: - return std::make_shared(owner_display_, context_, parent_node_); + return std::make_shared(owner_, context_, parent_node_); case visualization_msgs::msg::Marker::LINE_LIST: - return std::make_shared(owner_display_, context_, parent_node_); + return std::make_shared(owner_, context_, parent_node_); case visualization_msgs::msg::Marker::SPHERE_LIST: case visualization_msgs::msg::Marker::CUBE_LIST: case visualization_msgs::msg::Marker::POINTS: - return std::make_shared(owner_display_, context_, parent_node_); + return std::make_shared(owner_, context_, parent_node_); case visualization_msgs::msg::Marker::TEXT_VIEW_FACING: - return std::make_shared(owner_display_, context_, parent_node_); + return std::make_shared(owner_, context_, parent_node_); case visualization_msgs::msg::Marker::MESH_RESOURCE: - return std::make_shared(owner_display_, context_, parent_node_); + return std::make_shared(owner_, context_, parent_node_); case visualization_msgs::msg::Marker::TRIANGLE_LIST: - return std::make_shared(owner_display_, context_, parent_node_); + return std::make_shared(owner_, context_, parent_node_); default: RVIZ_COMMON_LOG_ERROR_STREAM("Unknown marker type: " << marker_type); diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/mesh_resource_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/mesh_resource_marker.cpp index f146f1b9e..ad8b75115 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/mesh_resource_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/mesh_resource_marker.cpp @@ -50,9 +50,11 @@ #include #include -#include "rviz_default_plugins/displays/marker/marker_display.hpp" -#include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" #include "rviz_rendering/mesh_loader.hpp" +#include "rviz_common/display_context.hpp" + +#include "rviz_default_plugins/displays/marker/marker_common.hpp" +#include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" namespace rviz_default_plugins { @@ -62,7 +64,7 @@ namespace markers { MeshResourceMarker::MeshResourceMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) : MarkerBase(owner, context, parent_node), entity_(nullptr) {} diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/points_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/points_marker.cpp index 331822dfa..0e34772f9 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/points_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/points_marker.cpp @@ -34,7 +34,7 @@ #include "rviz_common/interaction/selection_manager.hpp" -#include "rviz_default_plugins/displays/marker/marker_display.hpp" +#include "rviz_default_plugins/displays/marker/marker_common.hpp" #include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" namespace rviz_default_plugins @@ -44,7 +44,7 @@ namespace displays namespace markers { PointsMarker::PointsMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) : MarkerBase(owner, context, parent_node), points_(nullptr) {} diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/shape_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/shape_marker.cpp index 16c542fd6..3f768461e 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/shape_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/shape_marker.cpp @@ -32,11 +32,11 @@ #include -#include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" -#include "rviz_default_plugins/displays/marker/marker_display.hpp" -#include "rviz_common/display_context.hpp" -#include "rviz_common/properties/status_property.hpp" #include "rviz_rendering/objects/shape.hpp" +#include "rviz_common/display_context.hpp" + +#include "rviz_default_plugins/displays/marker/marker_common.hpp" +#include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" namespace rviz_default_plugins { @@ -46,7 +46,7 @@ namespace markers { ShapeMarker::ShapeMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) : MarkerBase(owner, context, parent_node), shape_(nullptr) {} diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/text_view_facing_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/text_view_facing_marker.cpp index 35a4442f1..c662b9509 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/text_view_facing_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/text_view_facing_marker.cpp @@ -33,9 +33,10 @@ #include #include -#include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" -#include "rviz_common/display_context.hpp" #include "rviz_rendering/objects/movable_text.hpp" +#include "rviz_common/display_context.hpp" + +#include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" namespace rviz_default_plugins { @@ -45,7 +46,7 @@ namespace markers { TextViewFacingMarker::TextViewFacingMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) : MarkerBase(owner, context, parent_node), text_(nullptr) {} diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp index 6b64b1202..d5438be4b 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp @@ -39,13 +39,13 @@ #include #include -#include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" -#include "rviz_default_plugins/displays/marker/marker_display.hpp" -#include "rviz_common/properties/status_property.hpp" -#include "rviz_common/uniform_string_stream.hpp" +#include "rviz_rendering/mesh_loader.hpp" #include "rviz_common/display_context.hpp" #include "rviz_common/logging.hpp" -#include "rviz_rendering/mesh_loader.hpp" +#include "rviz_common/uniform_string_stream.hpp" + +#include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" +#include "rviz_default_plugins/displays/marker/marker_common.hpp" namespace rviz_default_plugins { @@ -55,7 +55,7 @@ namespace markers { TriangleListMarker::TriangleListMarker( - MarkerDisplay * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) + MarkerCommon * owner, rviz_common::DisplayContext * context, Ogre::SceneNode * parent_node) : MarkerBase(owner, context, parent_node), manual_object_(nullptr) { diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker_array/marker_array_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker_array/marker_array_display.cpp new file mode 100644 index 000000000..136dbfb1a --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker_array/marker_array_display.cpp @@ -0,0 +1,88 @@ +/* + * Copyright (c) 2011, Willow Garage, Inc.' + * 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 Willow Garage, Inc. 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 OWNER 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 + +#include "rviz_default_plugins/displays/marker_array/marker_array_display.hpp" + +#include "rviz_common/properties/int_property.hpp" +#include "rviz_common/properties/ros_topic_property.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ + +MarkerArrayDisplay::MarkerArrayDisplay() +: rviz_common::RosTopicDisplay(), + marker_common_(std::make_unique(this)), + queue_size_property_(std::make_unique(this, 10)) {} + +void MarkerArrayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + marker_common_->initialize(context_, scene_node_); + + topic_property_->setValue("visualization_marker_array"); + topic_property_->setDescription("visualization_msgs::MarkerArray topic to subscribe to."); + + queue_size_property_->setDescription( + "Advanced: set the size of the incoming Marker message queue. " + "This should generally be at least a few times larger " + "than the number of Markers in each MarkerArray."); +} + +void MarkerArrayDisplay::load(const rviz_common::Config & config) +{ + Display::load(config); + marker_common_->load(config); +} + +void MarkerArrayDisplay::processMessage(visualization_msgs::msg::MarkerArray::ConstSharedPtr msg) +{ + marker_common_->addMessage(msg); +} + +void MarkerArrayDisplay::update(float wall_dt, float ros_dt) +{ + marker_common_->update(wall_dt, ros_dt); +} + +void MarkerArrayDisplay::reset() +{ + RosTopicDisplay::reset(); + marker_common_->clearMarkers(); +} + +} // end namespace displays +} // end namespace rviz_default_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::MarkerArrayDisplay, rviz_common::Display) diff --git a/rviz_default_plugins/test/reference_images/test_marker_array_with_three_marker_types_ref.png b/rviz_default_plugins/test/reference_images/test_marker_array_with_three_marker_types_ref.png new file mode 100644 index 0000000000000000000000000000000000000000..5b3ca309617f3039ee69a9a46c37c3cac09e000c GIT binary patch literal 26908 zcmeIbcT|(v_6D3r2$(<=)D&6-5epI^6pP3 z{O5U5v>DnM42I(4J!>HbgByhZza`<|H``9v(lHppBcE9wixXC?;;`9#nue6cX5jwz zvCLB#&Y9xH<#PzUzkWDwlM5b&aa=ORasf&5pAWXgK8B}P_19maems z!TINk7`PNW0?#Xk*Yflq(=5{#DQX)8TaEr482u@_OH%yz_o2T&!D2hDwav5thj7F{ zkx(HF{$q4RT$Dd0_2VZ#jFp|Qkj?mq;7@q+5g`=(V{}AX;s4wW4uKd+l4QU%8~!F1 ziYRC-4iN&uX zFEfQ{uKUNP2B>w$;~zSMNUP;TJFYJk?P5hZ-BduPQgw|!wd@| z6F7d^V{l2*RW(wrFb<)_5WPk@b_`sru1N8rLGb$o8v<{sz0@a^LlBI{Nm~}0rA;x# z;|b}-bqeZGqa5QQ@W7rpvm*13Z+fxrS}tFfMfaPjqRM}&lMqLLMK08=98aO?@)Klo z6q)S4i{5pM?iXZgOAt=HV@wqHkjYo&kGHh?BektlByrwKN**-1oGlVM$+Kc>^c z&FiCY`nVejT+GV@`bAm2p?u5)KFhWuo`21J0{;;)pbqmf z%%_I%sl@Rv(kFMeH$=&e#+F?RB7Qu+nLlcqBDYkrb$=xllQTtbpN_$&kjjm(J2Ns& z?Fe5*!}HeAX~N`Vhm}XdIF_&@g1toH6P=mm*G6L<;sDN=+MgT2HX35)8u0ZrZRwcR zmUJFWG3Q#UC#D@2cHJL8IV7RV#ezi?D|9SsNHJYyDV>~vQMrm&+uR5|Tb=O91V_U= zJU|U+XuV*)`rEi1>0^5=q|gM z4n1+>ZKS!QwvZ%kccha;V5g076ZocH8$M+eTW)2U*f4k~{NRvKXn2-E!RB#dge?^5 zrea>pICRK#1oGY#kp6>x$vdc+J9erRL#K`_biXqJT#U`{H*{CX9T>CF!4`uG0py#7 zY70TkP+5^ytn66Vr`bmBUMUDR3@e<~%hp50tXx?w7jk z{2m>(?MP=}sQTRsWn730WXO^^`l(4k&~3;+lJ3Nnc13O6A#*dgL#Tqj+3 zVVI~DLI+idF-!`!LyR%Sr_k{DECZK@HJs8ay&8c(3SpZ-2Cz6i& zok8Gz^hq!N25VW0;&$^3@Z&~OY}JLY8Hb)a1TSKSbm_F;e!^&oMc*a^jRCAZ1>TmI zhKaTDjE5;*o5V@5p*5^kKLPUzFI8~t8Mr|bmcP%i+%F8CG0W_Vl^p7J2y6!dT~@E^ zB&^B>euTdsJExdYs#pjXuFn9~4l@S^E*{(w5&B(S?z2ik}xD+XFX&QyRMFU9jc~F{ZQccB^;9eYQ}VAI;gF zjyOn74yf}wIR?ZB7X(;@d3nt4Np^u(VE2V;iB-e56Vrqyczh!aK=R%opD~#~YgES5 zGE}Y=@D6ZjkDyOsrs7j9+V6naY8?AP_*m1tUpZ;$tNsIcQ>nCrxeTo{&Q9>2JouaY zHguL9AU`Y#izN~tfH@92I4CrAkXwwSHLO6@1CL%&(&sQ$q6tBGS=+koJe-djn-Pzv zMZyKn9mxikD`8iHknJIXU3m=C58qrf^ORTP8>VFg{00NZYo&GG7kJ8u6x8zOL)8n}k=^4~Pbsqqmx6U@N?jN52{}tF3UZc{wG0u4*LKZuZ%-an-M$;se zD_Df;?_ZqM{|T-D4%zt>sfSv2nW&FmjHtI4TxSu|=TYDN@4oMrUwtb6WCH%n6ayF` z_Yt|k@lQISX)=1Ce4>9$TpuTm?ot#XMokN~fM9_B&cytenIAX`1@?(|8U&uJ_%uJF zv=peBK0VZw?)hI6_d&4vOqF2acM&2s9pR*ostpW!C&+%CnRf>cc#PV3MxnOj&yW8J z>ZQq)(5jOm9zZV?SQBEgT4B!f0QCQnZhrndM^!K%rQ!Z-2RF+I_d=7${h`XJ)kOJ1 zkT1&Lf$Fe3Dh&Nr1GxDEJ-Y}B_#2z{-)8Un^l=f=XI+w4GW$FXuRae<3*Z$5?sg*5 z|6enQKNR&(y9OwOo!ESHyI-_)fm>h`U?3MYNPO^*8wTZr|FWL%%On4va*%NS`OkD@ z)S>Gb1dt7oF9MLkXW&gK4tX6v6h}vWI>C+o8@K)6KAlZggjQXN_``ox1Jj@u_K)ja z_!K67%Fr(aG`>-NTLIAf-_rQ;YK?CN4EtWH(EtFz7QK_6|9Ozr?E8P2%mzp*@5@mt z2mshoJ&>s#62=ef-T}w+xAmNDfVmE;x8dlPQbnuQIJ24UE$dRWWgzi|LHZblNPs zPC)O5$ChHm`A&64zlO$pKx7^?+!i`Rn?~E|F+H+RBm4bjX+I~x6VqHgqky6&~*`e`+a zW~i_qaNGWpA;ye&D$P^qiky*9{?K)~TQ-NH?XY)EVoZoiOz%#Xo-}Z}M!N#TCyo(E%j;vtW&G);eZSJ-CBi-7leZt|?_Lzp z?bvavFtK&?-C|Mu6q~$ts3H)f)u#NJfJA;;ONm|k!YaO>VzfHr!deRncT5L|n!kOy z7kR_IJ~7Zch>J)^1wM$NF*0_uLC#3+Ym!o??|k^{1ZA6IiB5mER#X#aEdJwK?njr= z(g9sxgPV02Nw{FGIidI&6^zR$Q^cpwVqK7BN znZUEUW8ilL{>_Zx5aPItQn~xUJ`q;p?sveRSo@8fPBEQ+i@BUen@KZhq3L+rvcC{a z>w=4~hG<*|k;|IENfsm8Gy%0|m~C=DV&q!9X+}6E;SkO*F+e}i<>~-zy`xLz56Tt^A(jAc5X2zH_dY% z>#=TUS1_B=payn-c`bMnswJ^!Uw;PrkUKR^C-avnO<@+}iFuMX$ju({bPU(}8RRu`)5EgR zfeObXbg8ry27)uZ$8WB}Ay4Rc0RO>z39b=eR2lyMpr|la8+@F3+K6}4h5J*S;iZSY&wFDkp?5S)NI_*i0CG1uT zomLGFT~Ry?<`8H|0Coqee;5XCabT>i@O8e2O@ghk#(uHjzGV6(y5(p9d#>u>a5E@8 zX`7s6T)Du=@B+Qz6gq)x2=W#mh<|5%;`uiL!XYK5$>sJj#3U6jlye=HRR|l7Gq$I0K9Q(w8-2m!TOiB z#3e+c$*Oo|q}jfihk>u1j1ABlsUdB1-5K;4$DcEVo2MnOcxqK+VPc8goMsL&!d3QfVa#nhWx6wg zlQ~0wV_g30u!e7N&P+K^_gmS?9AbznOr91kI6MD^Rrw3g`dL{DCA7@{=pD_yqnPH1pb_`i2)Lt?6d#?PH&Mt#D=IOk zzOG8_#~oof-ow25W34qCGx~;VnYB~iT~Zq6vGcCeWhobw!2SMuh3)H5nasV58L@Rd zlBSd5kP^}^&Ks7mv}RX&8NRRBs((;*R>N`cjKub9+bzW=GoM}w+GF``V?w}0v(4{R zl5tH01nh7ZPxxfU!Y=;!vnQ_kR!~C!!^%z-PUB=z1fK3Rkt2_Dc1VFGtdQ{O!nJ$$8YH3T1=xzS->E!#3z_ zuh)flebF>-BwfsXGqw4RP+?E`71B8#i6eS<+`sjyPOciAblrUI&FgIKJ{qo(M&5De z^$gEki&>Y=Y8C`@=2#nuENY^9z7XfScR0xU`wx4pTgADnxkmh{jIy?#m{3PK^-eU{ zT2C=@Bu;b)9>aEipOND;sD)L=PObDCV1_*rULhni|4)C+;^*AOj^F(R+Qq~wRl6B*$Iuz8zB=qeU#?oWzE^aj%?XG z#;M?3K8w5zM-FdE!XCa)>VFS2YCD6>D2TM`!rBKZ4tFra7oC+$$BN9D_QQu~vJad* zcBRH_$8_H!oiWkF*ok6%)6-E4XNr4g9Pz_jYX|t$>3%*d+2p8@q9@=g? zuyN!fKK!~c&NL+sKh9FVpwmiOHEiWQ^P5*ECw}TGFQ&sFDFSx?^mwZW$f;x|bX&)| zr)WBFnjLwJdN_Q?Pg+rJoFZ$?qBp zQp^*77JC?(%0HQV=RX|~r`_ix>kI|l?F-7enZeRQev^J0a^Tcw+Fwk%}tI*Rh?9h1l&jHXCZwPNcZ;Yu$B zs4bZvVG!L#_Ekz@x347h>m@xRm+i$b+cVI7U27meWwoC*`*8`} z&n4pMA+M#6Wm;J6u*I4yhl!4c!=xmgI+&>clYM~PFkYHrQ=i!lsZu<5f5*HkXi+cwS zYdXq5g=P`;Zdx)g7V7nMaV^8Y(nEBLV^v{g%Q3Ex$8{w!owW9!U4QY>X7NZy%CY2{ zBS&x---Zvyq*m?5d9C3x;*VmB_S!B3Ye4xWPb}w0cDWPR)D65d=`yo(vYdUXb#DJ& zGx>)GrNwd~-d^hPi%#{{&A^1wB73aNQ`WYNA6qlM+cWO?q?df4%)EHTFsjC0YRL>d zwsdFy(-dj4t@m${;%ptbp2$*R3Dx9WsBP-gqH^9MoSmnz5`3H>X2j7er@52pi@M5Z zDCKNH?HbG>Y}EQt5W(?40}E4p}xsRauu9-%kRX7auPw*G@P_T6H}27|s24Si?xJ@&tVg-Ext z-%tx=p|%|#4bPPRHdMt_mNl3h*u9HsIlgZUt~D|*Gowdh{!6Qxc_10E+{5d`WQhe! zn?GAy==Jw08-%m!?-edNvf!;Fo>Pv?Qe zXf3y~mU6z+L3(}|0>^0Nco8;&B5P z2qkT2S)1p*82`L*{;r&!qW#};^{hw%HE<6 zSbDlv-kWgTg#%x$*=$pdnPF{VymbAFC&}8UwU0SZ?)9l~uerJ^punD^$L=z|Ht9~) zZiz@MfV$zu^}}UxMO5X}$pz%)9S#NSV!|?(OiY=I;IfqIvvqE7^v@_HlL1FIX(zsW zv8qt9Y~DwzaPZZ)3HOKh#XB^WM5q3$5*M9|_KrUIh2|&XvI%u_FW(-@n?<=>z|DHO zl-E*2g7_r3XlGaR1sviZQM|SODoHV(NX)yCZjwUCIw*Wm@|?2nRfz0YPoG2r@m<#3 z#;Z5qvkPZ+`rksLiP=Xx5noTupcl*`DNDnImdv{8hc@x^LwHH65;->&)&!k)wxa=^ z(w_~7<{=zbG zVz^6LLQ8_celS63Wa_Z;N?!)=WdX`jfbD2Xo*Y_(#2E``HBzZ`${QGd1IYl1B*yKC z0kd(op8lfICmn*SZOb4BwB*%tnx7oMR~WvL8oFxY$>jGBXFnF)F-9_Y@s1EpeY2hu zm&L?g)!fijb*~GJrN)Va9M?XIP2#M8OGO%v+3-kwCo-nQZIWF_zoFj)6pe(O?Hk>JZ!ivpnY?x>ukDpav#q3x^GPa!eeF`MUrno29 zkm&TBq75X(f)`)JYGVV}>|g8XT2Y*UgbpsBfezig0cx%ow{nia}g|&~`I1i|<6;H$bkG=(^;_t{%+`T5%a0_Lr zHZ^IQ#5Oc259c836h7~3^tU4LIvr9b5n*B}$KvoS{G#Tcew8d7cYKG}RIy^5I=aZG zV1Z6~9aj8mzJ&Khy?fhzvycRiCry}COl)98yhp#_7V{2Fus8w9P)L)e$(vUCkp~s9 zy>jzcJr3KL{!Ag-UnA_&|PxY7@lS23juyXy!K zFk5Q^;Y!jcYlZ>3wde;2(iCz_v$N|EI({*ow{X6Epp+U&}z}&YAjqB212mE!h6~1%Rjec^*+1x@78|N>%-exTJp2mK> z)g?N}6=BHo+_}P2$9II6&yR@r-oo)g$wcxtD(!Mw5F(yq%`8ISA9+K<$AVOymw+}H zSJ8>4a)nQs)xYvq1<66110&2PR;MhK4ZcRsh{(Pb0yGre1afm^H_Z}kwC$plLKE<4 z$Vt;omgu#7&dOJu8|LLhk`BthW5B9VdTU(h2ng`y9eaJC)&K3g45Nn*fsZJV#q(m4 zzV3a$Ez)O0lwxNNoL^;oLC?uivV*%BCmEVtk|ajs7<(L&C^Bz#qZmo62)OY9mM#Zy z=9G_YvkVK&5c;-wEs@nq-aUQY*o90`%l+B;_=VEzKZ4Wb&(wl@7_+&v+amc6nX7M5{}s3W19QUGS8X@ zb10dW)v>Z<+oVTHa8R=1h96${a{6WdfS*!2wni{@BT8$QK>C4Lg$!`Yuo^gzNp^Wj zC_O9oC6RY4Zoqcq^B;mf5N*CFK8j{a?o5^}@qo4X!^58)-!(tL{vJb0JGc(7g**$B z&0d<<@*G4wV=~Ss%JHDmp~KZE+7QR(4;ow*%_pKaP=g?7Ry|NUkQ=n}`z{El5(F1t zg~5D<&tqU&a@^s~pc!wjEBe1Eeuw-g@N0Ln+;3pL8DLNHrV-J~gB0`E3PDvvUudrB z@%T@+f%{RyC~F*Uaa2#i^h+E2C#>a6N9^)+O4F(;O7!Vvw1x}t#shGuFl$xU%#%C& zq|EBHRX+BhkNh)pGHWRZr`H>t9Lrlelg;yuU3)x91 z07zOiP^E~Jd;Ru1x}_}OWuuE1Mc${p*c!}l$~-mM2n_Zwp-t>SKWFmiOU}}vU^i;z09{USQT6b>F|A zdR0+x;}!WM+GJu!u;^iLi@QU)rX_vN{5+{vkO%OS8Ipm9_Mf0YjY$cYl zP3}$x#Fk*YY}Wu{koXKJs~Csd!dn%OD%EXjLBs9VzP#Yilj;B)U9LIs6!--=2#A5& zL58%9u*n+-VFr>J&1fLTk>St^ykW)sod<>tE^iK!nFD1E0SG+Da(3@+{R^QE$YQ)= z#2ZK3RzeC^o91^#a=!$N*E<0-c3Qm`W?VsZZ1`EPWz*qSBsXizhJ@BP(#acTdx@j4 zfzhOUbV_g{8g!r?{X;QzJa1GcswQyv1Jqd4WH8Q0fcR&Er2dC`>xWCuK7CrC-`(;U z29lYhbZEf>61gjG52D|jj~IddlWq+ke04<;2*Ci9$hXkN&Iv2#HTE+NCda~9R}*OK z;YLe~CPv%YB;xTt3vBatc`dSWfz#q=?b31|Sb;<7%)mo_&J|iovKpuFR&6nmxRzkl z7x`W#+_K3V4?KbVV0uq%fLDL<4SXe;-1wZC@Af>#H2sUF^Rux1hQH!{!%}P`-Hi8W z+$r<9scJH`_eoSPe22vo(?&fu`*~Qed{nKXW-MH{HRl>M4on-$=wAIne^Yz!xEgHu zXJV%nQ9Qn8!V%%?>Y%prPq*Z9CktsD7+-&-8CKFQolv=c{}3W~7;PdhZGQa6 z+k$-?WB}ceBspYTjwB3^dc5goulAD5W5x`7>1m^ZZSK86`-)d#oJ~J+YWTX?v{BMH z4qRzq!zBy{gn8E(V?p3GZZI)5lw^45RNuCBpL;{la+bc_&-aT6E z7hK5H>1=XyLjDGPJ&Am7d6;xq?zMYRMG|52_$QBj1+)d1NqT0$Eo#OuSkk{z``0BQRs@wen8A&R0wclDVUR01?79n%T*Ww@jD({SWSkfcm|q z9($d~ELhWb8Sa4)=%?_^OJEABV@Af{n0?d9~oq zMLL8gvi3NF;Dvl+KsJFdnjyKo@gmDK-b>TO%XZ>PI6rp%V0;KqrgvOe!`s)Lm9yuH zBLb)N6dL0eJ73X#bRb_b= zq~Pc5Ukdz=Et@qz)9(6UWRIjxTYA14xnMA3xqOq3K91uRUD^ah;nMR*|I8tuc3C8y z2<|Q#DDt055Z?sYU1N{mKM^r@Yc_H~bIsc95FJ@c0~R@vRN6dzmNVCxmsZn!1_AsCUMXu%Qia9eZ`eL}8Imnyk7OfVqZT~uPmawuJS910 z4z?36Jpsf|LEOK6{{Bv|`lG=`Y*|?2QvLi7R+3EhJrx3biz|F1&6Lf!vIe$Y9)`pf z-NoPfW^9)Jggp;H=m7I9H$J|l$K+;4{QMeOyJpOmjj~t-5cYB8kNh4T-GWFx^!9gI zE48s52jId{hRGLo0mLQM4Gs!=Bp(vJBGp#{i-wcl_(*99LBccGCkjMQ=cvQhb&v;| zaaD0`4uEPH-&_ilw}(utss6ORQhb)_X%5W=rU@8Tr)V}Q$+~bvYppzda$civ`k_Y- zd2XPHSuljwX1wv}L5p@TTZ0GcpoTh)8dYFZ8-gG%sYkGoVSol6=##UJKu~Fen$DK# zZ}7$^1H_4tuzND2IOv5sXKmt-oHaAptv=If6PunweC`#8hT@{EGwxS6{*pND61_oz zz5tXdh}5)UFgpiyYhO1xDhyf6I9@9_$Sd9o0@Zrp1i3=h4Q2CABKo?BrbxQqfZX@N z%omUiD)w5)P;Voz{#7p(7hjPU#5qIelO)q?4g5B9T?o7um4Hz^56R~yhs^mL{AJsz zwoEvz^+?bbQ8XkYxIekZX(Hk$*gBnuLk&KQB`k$c60wrE_o!yv5k5%1r?{O4_aZ3P zo(Vjw+-iE~;k4OKsQD2ZQ?L+ZfLLVjp@pEPrlr$pms5jekKy-8D4s5RzAC<|LGZOG z5Dfqa1T&Vew+&1r2avu5@rRrlUqBufp280|KBga9XInNJp%dy<#=_ROs&HA9;rH|4 zLC3`(_}Q>q)3s{=$y!L=yY>U;wNqxs3tj$N#ux7AEp!@jOJ^du2*L?*K)K;2Yy?wK z&2Z3xc`N1(V<0&NAjvx>0o_m#cG$HbP%_xn-S6e8^1}*bDV5;H79?;){%~s|4uUo1 z_Re#FOr;ui@J!G5XWUQQIm9DJ!J}VZ`~mSa!nxPM0G(zFZ-#ptthF5i&k1Co4cN7Y zg_0;%hMDu`2JINIl0KoQBWe3~6|cl?;prPV){h{q>d`Qd_ZiWEy0tJbFxYAlnd;pagt><)|3qrx=x@?Ne+cQ|5L>*Jvv&h{(1^uzci< z7l5_8+5VFwO5&5eU`q7{Cs^*MXRku+84m|{y`*j{(b3Yb;$4_*XCc(408D6Q&(A{2Rz6Sn0EoRK-9Y*52Dm$LJ8SoJYF z5~Cf~fz$;P2@itzza#oQo`J8iGJ7*g!sL9)3h@Fkn5VvN0q}vkIZ|dghdN>gT?3bU zZnVR{8rG(q=Nr0Q)7|otKszkZez0rIbwSlInfunZvbb>Kg4=D4f=<$mFA2Pbh2k8Y zUcZQ_o9BDm1-pYFy&SpDj(tc}y>iEQ7+?=Y5HBvQYB6J2x)aR^f)1y`YhG6^baZ59 zpL1)YrG@QI#xHVp0`z(#iZPdYJ~fPSHu4<5kvJz!y}kqaxzZ=SpfWI|eBpxv$DUH7 zS0ls;cq}?3ZhcGW4%-D;*P6zgxm?(O&$HS8+S2Cbp0&X`52Jhed?n3tR$MBkG+Mfb zOir4mY3|oz)-~4bzUQSqzLz+@j|k3G%Y4Nb$1+rx`zL3~K2WTyW`7wid+S3U`6!c| zd@J2Hn&PP({~{uvHM-W=C%h{w+q|d6QC}Q9r#Q5wtJfOeeItrDYBv^mknV-j9a$o5 z`<3v5U`PJ+@rIO;KuBb<+E$^Jz`5RROAt8sm2bH#h|hrA25#VhBHdGwUL{>uHaZgq zn(q}J_@=q~QJ95JC!7E3F(dH#)=1d|FVKaY25%^o0Yy228Ha-y1^&F}s(6+7(DMig zW4tIF370J^SVd}zdQ2>Dtqn4n8Hf;yd{+iy34^^^Wrszgh}ZdoZf9@_i&L4 zwn0X%Az@$0*6>yr)5PoFyJx>l>x;-&Gz;&5TM6TRC{d)|mLphY|6+Fgv*6N}EA+tb zp3_ndbV$A|+%=>HR~Bl!f86Ly$f+pVgAgZ{jd7{&E>Bptf!rq6uarCIhBv1_J&x5D z6`o(Zv!irF*7+#jx+*X}Fayq~UJ_P$>*2^A%{zT(ACBHH7kkyiEloMO`;7p2{|^wG zLml#s$<{{N4t5I*lWkLT#_9qMl@fcrwXduJvm}r!_(T6ESoJCho=p4^IH-ic!7vop z>}=DDDzl3b`_+dHmN4_oyR)0m(itP|y#W;DFf$!eJ%!ESLaSU~H@EkdQTztpf`@FO zpDAD7BfUY*Y+FS`HOfzaRw||}nfvw0B5sM8ww758?x!GcOIp{?Gb~x&{yBg?p52$U z@g3E)KWFsYibU?EO9dXj1-ze-J-@lqi~F=kp?s4o+P^xwZyjZ*Gv;{vA+uH1 z`ZXF^mZ_tHZJ7a6!ZX97eizFNZdFP)^xdf~jdSpHdZaIiItr*@Dp2!Eq*& z*OJLo$YgCY8Am2dOijD8cbZ&DAWzk2bxn`eK=4cD_O5 zgHW@v|Jp2_S$7N~N{#m47W+$=9on;5Z^43Ix#~cjg=tCipaXY@z zX|8OxGoN2xBUf$5TJs^dukhC>U{_0ws|C>ve*N+skw>n`BU|JlxF!oH`dH8QWs*v7 zYDlK`ZPp9$@Rb|aX*iYXZ@M9mJuK>-ju#9XTp4&_i?6XJ-h4z}(W;{-@>n#Uy2bA) zh_z~0@9#75!FIZb-ihkQIWNSG+h(R^vk5%4Dh)n z?BvMf7}*Nu9Lz@9n@1^_%1LZs9i8&?J%+3gqTs`Uc2V2DDsNJ zCZv5IOMc2K_M<*E*M?n5$Q;IP@{-$x$o6vgmKZm96ls_S=8F$vWxk6?JI}*MwUj|1 zJbNtes{Ckka_l2wx$51djGT0i;<(KYohu%@)_FPjWFJt93|R@IXpo43cYdWwzlzP! z$jF&wddwmSoB0u+_Qh{O{cgyXoEVNKs`M3}4-crCn=-M=lBqP9k~M1Cv=70uAlqtB zr2~{v5&h3;w4cCEpZGOh8j6p?j#}sz0;VN?ZuH_6>+!y1c`I&>rs*GRHdhzh^_cFp zls>uTJqoK4yHxXH6g~P$RyGlyCgA*3_2Z>8H<;{nH7!lne9++l#ld1s7|G=D4YI*) zJwl_6zg+WIFN?z7cf=dNuc(#7MR7qgZsM#|;oxuM$Vm{7=TfO249LG>)}#xyNnHAFCYsbQ|g-@PGz+$LEc zWbbtVWehy~3w2J?)S(jI0@c$Mr-Ep;rV-~UcB_laP!=^yzRJF<)$L*&`xV|8N~v?BXjR8ZTzj5J#%aR_5eoak3x|o!@VMoe*rXDy zAf#c6zNOP?Ny-N^%HnElop;|Nb(zgR80ru~E}&26V{yK;tZM_h&-rGLQ#pZkk{oMt z0%V7&s*rqDX7fUl-Vm9|ziP0Zr`yN8d2v*LE%xI}+%tV4DWSDtOKX`>a$hy!pf6?F zx^=%f-J6@P(@9Q?*PP}fyotBjJV3k4m6U8FKN3u|-^#^Y%=+YzHwNK_YAH9yvVWb!~jg8U5s$=xCY_}VkOrmG1mBQvkN<${~$(J|1st?Pc!2ic6fo*^qT2~ z{hu`7xMFoHvE!`do=4lpN#bO zBO$W(Bs+ei_=d=jXgY4j8$j4G7HYcj2XcZn@43V;8*2?%ocFGbGPX_sxL%x}OGpEy zGK+30^@?KQ4a%@j9r$zjM%~q(i4$lp*D?=C@TIqT<3FnpuW6F!i${V)@IMfCbT-#V z0LGcO?j=x7g=k1YUiX=k_Y=yXi07bgRLcvf1283&#E(>C;Msel7Zt`yYuO#149i&X z^^rmz@EQ#uH;-M*>WOB@$1IB;7}K_0WScbtDS4ilmy7jh#m19XbKauwYyv(KP?k?5 zgf3^1$h%JLbY-VZeI>`+?{ao_F4rgDXB|*18sg2BfXr}eX&1wihPeC!akR1Q zdH-rvg=L<2&E)H?_C8sSoqn*_)YSa0m#(9cWfbi33iVqf(YFdKlXf2O#SL109hoDy zr_eP!v)KHU^E>cs+nUc51?one14fGq&u?wC6|OYgvcLqAVC?RUOx`=Ua)x zRGqJzXhUae-}Px-mrg|LR8b!qEzE){O8GsjSFCOuNb0q(aO@vn`dd^&Lc5t+YJb#@ z361x;L8f>0Dpzc>$ty*-n22&-(#f%qkPY=OZRHKzM+IxB){`z6gq?;ltc~(s;p!{JGH(lLpKs7jzo3v^0K)rERdIkA zv%}QkgiM#PaQywIBsnosPt(FN@j$;I7iH6twj2+%UGoCrOPg5*^Ez%?%Zx6tPKLs% zUa%m$@cCl>Q~NPGy)+uloBU+%^}AkwP6cUNa1qo=NyY~1c4oDnr3+yy0rckMu}Exn z6jZPAQ3?xE*=RYe0^TdgF}OE+cbWChn5j|Qcl7tlv{!R7)%7UQl>nS+v@3xfVf)hc zHNLhE*|MYre{wsYcs+|*O$QIjG8Co>v3O+LkShFkPmE&}S6WNavds6e1BFP&JFnC% zzcp09&nm@D12HpmbAi8K2P}hy5%LxHdsNlLq>I?}w$b?U_o*9K>Gw4SjUBu3_rrI* zx`jli`Lkz-dY`s2YS;wBL*EAAtAHRVg2oFeQwRcC#{TvvH7OuQ+uwx{<#kK7t@sJ8 zT}K~7I-k|dAdpT@F1}vYVD<1#wE+WfzkkOnm2NMRvN0|KCh$zn#vyZgOO;ns0gECS4cP*N zp!P7!#ZZ})67pDFp|^0BYQo0=ZJQyxP56YjSX) zea&ilSP8uMDH^E8g8P7=gnVYSYu$+^FF|^$e2iMsR6-3 zTorh?Jfu@zQ7R^(Mt@?BeP4Z=$dAr%7j<%$Q9lTRC(v*z=Jj5`1LKjtIqJeqHncN= zf|C#^WWwvgiNCtWI?EveAAR4QVV%{Rs~}P)o~gqV-Sx6opRh1)MU{OSyre*F;E`#T zaAG#&kMoJ*En~)faUY*~AxtnnS~c3RV64hqhaPpg4EQQG-Q^(YJypm>f5Nk}{=QjCMJjwV`(5H8yvR@$l1Co{lV+Ljvv460zKR^DMMY6%{kJP7a;N;;6%&N;ZzI)z8>y-T>}Zr7{rLaBiGheL`X;S%Gl0u7 z(5vM=c+OF%pS%e2%g7<5BnIVcFGTwkPI;&Pf>0id<@Jg>;|xHuC#BDkj=w)nt$_3e z+NYeT7U7u^V5u*`Of+#l!!>7SK5iQUpeZ zgXnLitJX?2AR!W&5`qckfcE!Ohw6bBj2bXDI;3!<&0}G9`12osHB7&U`rr^iK+sAv zKuid#=_+TG&L1;II-32(Q#najH#Y1=ji6OsND2rZ?NnPr#e8_>HJ?jAWvvr+?rU25 z?ewQFk+HC!)eW6~hq16Eky}|F5*rUFzIs?$%Ht2eZxnn`2b;H};q>B5@FgJ?GcO>C z2#EZS52dG7GpYw8O@J&=1dIMzQw%s#p1csU0ih=o3);IqqowDBgA{)Fj1y44FtW_c z>sgUFE{PjfEF3+uiL+Gg&p>z@d>}nF)_!2iPNG9%9rn3S`xPv2a9ClW&M*Bj)+<1v z^CD8s-XWd+nxTylk}L^Wz2yO5bvwdlD8|Dsb%YeRSBs27!nA4KS9!K-moQlkNFM^& zKE+aHhh(QBFuRy`LI)#6&y6!$U@nI*tpbkoeOp_6j75r6XL6C3(9l!Y1 zM&00%L$fT*h#FqsMY#&4Ch)6eKOv7S&P%!Ni0AUY-P()np{Lo8?qLW5y$P>!@l+zSB+V13 za!c1pFQTm^gwxe~CjieYlPa8ck+wYA78<{mB@e7ZR`T~cXK1CNbQ@x*zDZdwz#iJz zYE+sfap;n#DKOc;OatoJL1cROq1PDFnHi28Q77bjC#eV$h}H?4xi6w>_0 ztlZL%%E(@?b7<2K3@|N-_acI~MRS!Dd`%|OUT!f_2U~Zj)InMZy#ryj(8&)7kMEuQ zzTn^PW>6j?*%6qKL7no|FM0&qQM1(qIZ$9Ao&zZ)py5{1XvoSI(VPiN+eIsRc(lLZ zf4raV0O4WQMZZBp4fJ6{+x?;ZqzHrP>I}&KU_# z#UzEj7!WAcZ=Z&0ydKh*a0=2aaC17+iEw3mhl4!T8tG#g4g`7)vc;p}3r*T0=!Pae ze@}4aI+P6MPUgQGnu!!;fY4-WcXAKfcu_Q;Og5GiGodG6UUicvvXj0>hj}ULtgs)> z90_{tK~^UeF6_KM7J|7ArxmcOmK!jcKj-BStpbqVet9QUVoXwFDIIv@v$GDTB%$bdtkRZ#isTn zP@^tdsT@=po9dgaqac+G*GC~uGkvkUwWt0*LFO4IlNg;T?K6Jh8OB)$CDKS$x;D1> z!eRMyA{2mc99#!7*?R8Y%2*qWk3`$uTU#-JcPsnXB6rUcw?9CTGe~A1haD)j> zyIAf^NSGTRX1(q5XwiOtdJ9HZd$Yl7k91|xd*fx57^|#r#Cc?CuA~UzN?4XvU$*@e zwh{{3U492cw!LAEe66OVtwz+Nuhk3QNRYW*Cm=_3TQPY>9p*TPk9C1y!edY3ews?( zM*0gG?M&r8*GN8ZY?%$d_<~~0YR*b5Nd20~1R6<1-DAhBx^sqI!2=OFmFC>=+Kl6B2d!MCBktBo`~oI|hxTlv~m>H`in zv^7QSYU$28svPJ^M&jS@Po&Ihsl9aaE?8kS)W{)}3X*X(ceGA3)No5DqcIq8I|TYT zd7El@ycI8H1LAr>M!j6X=d&3F>hTbD|M624=neGL4gP00{eKQ zYU=3+B>pQ8_3wY)0c7*9%KX{Z?(YQ9zbb>4a{bGF{QDn~L2wWXhJJhi_kqEf798-> U^ca|df&cl;o;T~1Cx7ez16vx$;Q#;t literal 0 HcmV?d00001 diff --git a/rviz_default_plugins/test/reference_images/test_marker_with_marker_array_ref.png b/rviz_default_plugins/test/reference_images/test_marker_with_marker_array_ref.png new file mode 100644 index 0000000000000000000000000000000000000000..2ae914dc169aae7c5c2da951ff7e96be941355df GIT binary patch literal 26012 zcmeIbc~nzb(>|Js5KNdAk}yUPK?OpHfFd#k6Hsxa9grbR8c;MK3L*mS0TCtQfQSf+iVBE8E6%shiT(C)|GDd~@2>Ctt=qj?nq^M**}Hb_+D}zI zRlRNA9G>PV`X~$rqv`A8y#Ry3y@o%JNI3YLw07(F7))fiueYavVsJQ{$>dYjWDF)9 z_qQM3UZN=WQ7(tiCj9LODNP}G9LfGIOQH~vr2qNBlwJ(t>R12uuh0*hEeYH@_~gMq zKAAU4ti0GLNHWnBD|M)U{)nlN9{u~@AfFZck)&Dh$*I3Rt0^uSjg!R`D2J#1WAbh5 zX@2;>EeCnPW3LX?B!T9)^dFyaYzW-PcCy#&{;^bfAFGJ?x5Xn5h@2SAsr{^?^Z)on zvVerhpylm9R*FbV^G`wj{0$T1#gA`~WJhNHM}+7nlp{J+L^uNeSB{6JU zs-ma7QE-#~rv%a8W-^$F49@;zrHHWD|J-RhPOc$V#ug}dPX31&cY5&=83gnGp#>nU zsDJ7On;_7~$)4NEY*zk5jMgfRx$+N(Rdo^RL4EYhnJi zF#iW!Nti}~LXU>q8d60{{=}jO${3=Fgepg7Wlj!N^Po~kF$O1#tQWK?%y4Fji5)`& zQi+}ea$zx?_G66fX4(f!kDfpuo4X;A+fpYOROsX468bZw#AX4hQ%k&2Pfr$OI99*D zC#gWWk4?wj)4+bP4{7D`UUC=eXX#M=jAXvyY(gm&g$Ld6gg+TurOf?Jd5`@Q8+d%t zq3(Aa>n^2L2he!=uj|R;AAI{CW|^OrI_6UeWfxcS8gq2_Zsj)WjFN4Z_jxVHupF6X zEM}Pl6T=b44@7&9@%TZdZmww#@ju8pnz=da%tfP^7QQ9rmbrK*<5Xu}gq%W)imW#y zN$Od*unt;ZTvwNogS6iR#NU`J0%@sPOhwL~ z5RrwQLbAgepJI;3e-Rva%=7A0$JZ<)68W)1`t57I>=L*e@5q*7SCTNxgR=Q^7a@?QUc-qyu!L+_-Zl2NLNTRigu8%*Q4jhq zcAb5TT18x6{6m~zlsU&J^QA^2$%7bF?%=z3_F-xgDkw`EBhkt-APJD`@sCVqYZ z_%7&RvVPzf3$slZI2Rr6WMF9VUD!MJ*+kWo7gYFWH-Nfm2F6%jV*Gh}lFt*tiLI%a z!+m2}mk(`Wd%}pnAQNU46~l8y0RCdwMF(;?{P=4Ewi|qEs~Y7D`l&f)`nmA|Qch1YttWJ)=JH-Bvut$D*Z{#s&XluT)kzvnyYiWqwBhDwdv#dIL){LF^n%;$vlx9A33# z048JfoZASy&qv^_)mC_V0s~WNoCtHt@H>;EVR25`i^BH5pmL9V9nmdADQp%l-!P(C zjkU(5r~;LpKo^x=Jgk|mf$c0NAAan1`LW>e5vI@M$oS3}c?3-p+ol4V>tHgcy)fn7 zk@3IPkY)!f9!yBCZV~jO7`b?c3Z>r%8B8FETDUk_MJ9L2=dF~>B$0Db=JM03b*YAm zf}!wzaSxGI%?d_Vb$;D{UiF>s7X&(119rrT9@VfC6`)qN%xi6S3hD~ure9RJ{U=)II1{*Qo*PX$ zZs-z9xVmjE`wj}Fo9$Fc`5`RDiIB)WmdWMnrOc(imo7i9T0}a!Ion_&F&vD`CUdQe zp~E?syY}BRk1^y2X3wE!?zWLJ6sW5}fLGpwr($ZI=sRlM2%?yU{7E~qxWnjd`#{~n zTiFFeHE~26VL|1`SR<*r9|fCLuE!sGYsxUkez*bh68hZ%g{u{;M?JjaS*QbNB?n)Q zN6KI_rE`olpC2afVd~MEX8&!qHCBXjDm9g-ecb4%1|5!&G~|rZ*cEk6{_yMef%9}p zx29}$MMm~=)K$VJr73#$DFp#WumNBhyu_Q>W$EE0%y_BD!F6gVyXZ2HBML>1F$pxP zFb(qxndIyiIDEJNIs8{-c;%NrSSHV#o0cLMj7l)LZ={Av$cGs;-j&g}bvn8*vF?cUMm--urE@xXV%JJu^9Xghe0ZxDj05RgRph9?&vA zCfGdRl}W#X3Qw+br`zDv(>DD;f!xBgGP;ctHL|G3XDpyHy{c_|5& zAoj}aqhJ1O7XYSao;#CXsX8u5#0IhdpmKRX-G8w73c=oWY+pF0ek%K-K+Wf$KmM1a z6ri1=ww_OR7OYK~H@*nYvA~|gy$5T+8T}s-!T?LkA1=+#-F=N4o-CgY<~~4WWi$U9 z137>*E~Lo1rl8TU$EwIu8R&NmN%|l{X0z&Nq^8seyE_aif~~ zy$9p@R<(w6$o=-0Fya5WP5AS`z;FN0;(!1Ri=8e{+|2fb)0e6GRxRA_7a7ce{LG^u zc67?ct5M^)@ZNuyar79{p+2#4i_?1~2+TIH22rQ0?x|n@8$}(>C|)mQGts!Dz!X(p zZznJ%fQ&z{V(0!mikLB~UD)@x-DDH~w{2iiy6Cf5>0$aG4K5B)U6?IDbz+_^p!4Ih zZBcAOj}E%J2w2^pW*>h0Uk=SSPb1vp-btW29ACvg!z1qmo5DfYkpJkvORxVH?EXuj zzXu$Oy3Nwmcqe+J>_`K^qVcFlWE1oMQS=(STiK)XKzJl$_6irl4gQwx)m_~Jk@h7W z7e40q`LgNwnrq4t6<%J4O0xwZRP#6jJ&s1bHGkDsc37w19_8|;&bixioNjt<%9+Jj zm3UI^%?KHBEOC*4QewJT3_ygGbRETAVRirMrxZ;!3gs3>^EySd^JSyJVmaoySzE;X zlqsG&sh2Q&?V~A%+@U^X+Y!)#xCvs^P3W{rp()pYC0+;;oe(z}l3oj?)m<(%0t?CX z*P~m5u>k~n=8VXO~GDNU$E~Rhms1^BY zVMj>SkH(g_tY-COLsAHFVg7P26mKBuxUC?>Hmuosk=(vcmPtLq8!fgXw@+XFEQV;m zf%YVt9dH8X3KuzI*sp5% zYivia^2KGDP3+%<98bb7GG71n2IG5*Se#Kc{jhG$JPM0YelbC=o8oT8Sh+?Voe_Or z(RA%s?}AwNkZPems^i`Po9jea?a<0(4&<*^H^SpBH-7iN`-Ob#d)jq&b6W>2`6A!y zM8lx^(INXn?5mp)fXbyOT%uC$Q*UR#Jc*d*zj(chPKtO-S%CiBCmL&N%`dE>Pblfz zt2^^bDPhaqUtwT?@Ie0o1R9^lI)dE;c@*}YqlD@8@X#P&g`a+}g zJceF~^ism1eWQ5H#G=2)BhlE7*E{7gn8Uj-xwD)%l{+yYVyq zGiOa1oLw>Haz~_m9-u3_>N|RX4&giQrM;|94CVQcP!|k-pJiAtzc{WYk{zNFYlljI zZi84t8O(i0b9C~(>VvAzpXr>E&?j5xBnx*lz`-pTI zg-f9fafej?IYdDaRYrmd;5;W+ksmE;j?3;D!o-$W@89eIU;B9KOJh zp}-jFf2vV)v4yYqfj{D5V6+CwhbNt(idh-g4i03(>BZDC~*ks|WR5^8EZm@CY@` z9!Vr=f3B6|{jcHtktPMkxar4D#lRZ&i_F71Be<-O?(vx)-SL;k>9`CJ#-xrDPMsVm zdU3-{81R`DAd;5pjA&(jd@5U~uJ(R9M32W0d`~g5Q!Hv+CVUodW}C#FP5P>CIQ1Qk zDms@v#dI)VjJ0)Wk0?9JZKd!ZtFeX$3#uHZb*l$4&*pvKrn_~E;&_kSlCKtc{BwZ= zE6fX1NHvcnm3?AO9pDDbvmp|C%TB@mxJQgF?5&cT<~d%w8R)ZXNUeSIjEM;yDcSSG zWt&A`Y0XU2D31NTklw)^;x4!7(O#e3b94*VK*+m(B1gor&tXN#8>Y6cFA1TG3S8Z* z)A>@nMAP|?v|0DGS5<~NSqD2ZG5SdOcH%Bh1pl#JylIjyy||{QGnV^eiUohcCxv2w zZsF@euU0H?8ZSIgJ6!|u;IH_D0h%@(B-;*O@%yx;dfU5)f(?!fIGB~&h71BVun`p+lAQG zwlrG~cZW_OW8h%=t1p5c9o0}NZ``!+Y{cUsw;<|yyTec?MzKrNm_nIGp(H~j{@&9d zR#A?66zMC`J_NJ=6yw#s>Vio=*V@T}Jp)nH1FfF$CahU}$;&a)vq-3Z@oqzt!lJwCq8H_JFUuhrRm>zQ^jSgerTa*HMn42H@ZyiT`5+F>3& zuP~F!zce~SWN~9(XDGEk{%+5wb(ETO^j8wwMztiZUq21*u9loQ@ZHg6YmULmU}8ft zdGg#N8yor7w}htDNI)vnh36D%9bWEj7igOq1KJNFu(zpMPD=%bS7BREq}e)TQqL&y z?DWM;uTz3w)sj|LC#UyWdmfl}X#G&6VrY`BB$!xJO!hm&PjwCyT`^2tv0QFU-R-w~ zP3M?7IacKEtsTnwyCK#vZ|0lA%)+ysyGLdbiFU0%l!ra{S0>7aX(^1{`QP{D++0E7 zdQ-=Z$}+}{Izr9j|API^0+;AnJGXTluad%ZPuUr;(6#G3&3N%srtHj%;^u-HVY3=_ ztRlAHQkm$}HGci5;EBVgm50n8Uf6MO=E-us2?5i5S;ph2YrW~$nG3MYcQtz_J>>@# zD6PmQd3c#vjIFUQjwKcM%?ZOLJYaC@_LUnstT3V;c=UYjo_n5p?Y0|4gzF2`>osy$ zP-Y5a?u4!?G#9F?$2(N7J@j~7{Om#n1o!9FtLH>IrBoj|xJ_6aN0f~5wq9#y%s5-8 zP`Ho2tKiQ*nU}HLjkyi?(U$4sV7`64FHs;KdtE4+&iHx@d(7=ltWT)HMFs9bIksYo z!m2T$)w*%Y?`*w)p0(!bH_VaNatWQbOElk*jI= zn{o~bvOGDnjj)S*7rdCTU{`gn(@6RpJ+(zeDaq0tq28w3n3#A4t#jbgiqZt*eY)6b z2Qe;pN3herXg~6Fn(DwVbC?XlX1!O_)X+4PFwRT)rT)PwU8z%jmdu|%e2CQ+M$KY)RDTwK%$}EcZo?=j0w%(M)BoW{R5%@LDo8NGU?)AhyWzzk;sl%*RVi4Ear^VWHez>UY+-AAdR1mwdy}WkK1Xr^@wx5|XY?g9M+gt03b@of==v2kS5`C%v%+egkw>|xR z_EsU>g}s2%(KYYEA6#J_Q%)(-kd-7d6bGvdgxg>%QXlW&u5gMf+F5!o%*%t{AtbFUQmxCx=(iu-A0 zP7GN}^5-rE)`zeu@8=X@9*SFite)kiyF!G*6@ZQJj7pR}~U zT9()UsAR8$m}9?$r$&6`dD;8G%#S-xg>7ALW6)zWIC?9=_r^q$&Sp8euefz8`O zRX@ppDUfs4MD2gU;Mi}OuaDV83}V*Mro?Wp^U2SVe-0NIVA+0hIZ0nG-@aQbccD$J z@95U#Pj_4blCxV?Qrs*gu>(N80esg8GALgLXm1K?4_(mvo<|KOEOUxrZxU)=(TS(i zGP8B}ZV_)xNDsU_W6r+X@&1XUYD-TWe`NM1&}^%D%T?+z-GxAx)2JN_EheRM@jJ61 zb88rWE#Nh)=rjO&F2j5({XD?=lRB8&G%S+rtwf|nx#@Dr?DD3a%GeQQ+-O?@ zF$U<2r|&wR{!TkYdwzXKy>gNp3H79@`Qw7>g6NsNwJ&qPS$llLh$3qURs8nZUa97rfIrG+**fl4}Kta8=G_dt~1xOxk7^8tLy6|h%v=zfIPTm%b zMl_=Z)1_1VF{}VDBd&a5;7W8>Gsu_j33cmP^!QQpTw(s#e1 zJ}6`dC2y44A@CffY84Bvn7~-CcWIYpY9E`m#MOZ`j|UZ$mz%hDKy4P8Yh_X!2ZXN0s8lUafJdq%{bicl9L@s zR_ozp`dnF5?n?$U2}q!0W#OdexMRaTaZjpu@8s155g(T~-YE%e%Y?@t{8F!S*|h5! z9?!eFmHY2n*!%{!12_6{$VO-8PL`oC|Tsw?V{ZaWv3@L)4O| zUO^L{;xQUE%A0&S3-x`0z9U~1-Lm=^JF%frFcdj&TtC>g;^>Ozl_mYv%8i)L_qQDb z9)nzTz~cS-nw#I3f#$0+q??OoQ;}I290l2P^WO?*pOFU+vMvWEfX^+@s58aI7dakk zNL324a89-ZRra^2p<%OL$u-=}bHs$157@*IE6m>N{- zx^F_EyX*qQHy}{7$*Z%`aKEzH)Do3osBf+WHV2vgO4aN|-UfXIE0lwvUa*@tiE}G5R(KhR2r@ z!=k>Ora)DOf1A3k86L+#gFf1a`6h6_t-7pqyW2g+N_M)hYZ<8p_8UZ>b9gVj+EZ6I zr0Og6nOcLukJ)Byrio3GD4N|C7ucPSq4)1J4Laj$9rqdd$d+TuVYsYCDtfR5g8%1hFtrwA@ss$E_XcWjB=n+GaOz$C>NW^Go zb9c~?vJ{D*8ZLR*2TMY;yhgD&e|WN~<$ zJ@AcF7{4P1Z7H};w-M}cJMwqI#sv8VXStUs*k-qr>k;Jaf>qQ@w_|t=ZNwn5w%LGA zWdJ?LU3(`Whs9;}>vQuycJ^FXmWSRK|bI&#qjnWw==`I2|6q#w_i zcRIIlcMPEMw@GmNF82@k8Nf@hS<8~zBjQC5fCs&nD;rbS45_Qpu4V6M)ID&zP-I^U zquFhs4N%-cEBj$mPn5F0?Z_}ffJ6^h_GKYp~B`iL}~6#Mt1PF z)aUD4_xU+r?oJz#PHSs3rSwrgA8l1OVkJRulR!sAF$HEzdQGqFh?T3O6b}!7QZnsk zm-kcr7@MaynMqZRf>9;|=uPz0+b3JYg5Q(*A&l3k$^cfI;M5UweaRByDYP=`k-mM( z9< zCZWW45nP*E%QCsofinnP%~C_0I`18p8rAUnj^I1W@iN!Jv6l?8KxvQ@@WQpt?%t87 zsY-;$zsaH7i_cbO-SeKc7Zi7g2n%*@icIXA$PRhODmsYnh-3qZ=-GFpX!0!{Tr)=r z&M;|FEA@$qo8Ua=j^NvLbnzYxkB{5tbnesDJerBf$mcU+%7HGGIO(1m@k!6X7r}LB z)$6R65b39a8YqUiB@qPv8ddEgx@zzD*hx(tsLIBJdS;FNG@cF4N+>0ZfhG3mRO zwTOxexjRka!7jJi<9OcPf!>k=i4-_a=61r1LA|_$%UK3m=u?0#8g+_4z;*Y#9inSv z>$D18Z|@ICx(*2lMH6!uX4%EL96{HH)zrfsVjsEsPMQ{#=@MvuviI6A&oi*6rBF_#OqzN?yJ|FxqGucTI4vP#g#tTM-aR?c zV?|5Xg!h07hUaVp12C)=YkELA)Xc4bgsLhN1-tSxgFOA=;6S@s=A5Z0cYt~!$auOV zM)%va;nnO4t)n`8lT59#h$670!``#uSAUGhgd0ViywSui z{EZ0+GA9x64W$TIE2i5W-!e1ghZGLW#A4J?4SMlKyLT_#*q)ZYBEC5ia~@%bVQ@iS zUSV>%ihWTG2ThpWd6<_1SWNo!&;)B_cgzar{@P~_%2=bt&3~3d7K@jZOa`k{9`%eYMJW(g}4BsGd(s5!M zLlk*shuiPCT^+|NrB>;v34F zk7?91S#o@tkQtHPb*?pXm~J5f0SL~fonSC8cSj=0f0Ryz0jzk|fgZdq6s(QnS^ka2 zb?%wF1OC{yB^sAVR|JtY997Yw*5f!85ZTJD$^$4X4D+7B>^7{xT$u_=#9#OoG*L9mxXmeqTTm%o zq&h7YDiLOBAbC2MHqfuBu>cw=tA;aI=a|MEpqvJesnoS7%mxi2bP6h(>&GY{@$n1D zuU0@RhQqaTBik)xys6-A7Tngh;i`*pF{wG>w4LSyz}}@;U>aO(RE+m3at;ynUsvjmC=aa? z!1&uuT1O5&Pcl5=9N2504dr#zxGyl8P#)A9^->C0spe{s19Gc928jw`2ew_VVMif5 zS{GCbfZpVh@+L6X;f(`n7)^r;DR+pEM$k!u3Mn#~RjSF%B2hkFw?jN4KsVPl;2ZWk zP5nlVh7mv69VT}4FuO3z%S$R-Bjx+(Y_wv{`p~l-8kx>-?|$7`fS`DP)tQfAcnX zx+60>+_0mqWP0P2q*6QG_tTRmJOL5NIi3|d#h=0qGg_h^C(}U1ie{i|$CU=Hr_G| z1c@8M&Mba|>SgU=1eeOvZU{)`UXN@iB9q2I1@;Ym9N+Vqmw3%aQE8x=ziZIe*>Gq02=@o zM<%=IHf>Kbt%-$4DJ|l;sqt>FGbCVp)w_6Ppw5Cr|CQlUaN)AdRm$~XAwu6h86@<%hn3KU!68i-tg_p>#CRGL&ONt_#&P3C~<1MhO83MDrb+gZ9J=g(D zkN4t_827Wl^eTXz`#93uU=xD8C!00_K)youODRlXYlOp|UI&$0ik!1(4d&&AN%CGZ z)D2vYZXse1-$305)Mvn%w=(u2fuAm2op0)=odZUumsO-!N<;DQz5`WM?dGH%h<1(B z*zd(}2AlQxQH`B;^GY!EH~W+e$Y=;Ix5xlQOGz&@lY{*}Q0f}cP@IP8gF6Ul*N43k z%vL?)79yz6X^X&x$X5#54sApuO0ad0R(q$4@V2)*s3z%VO>P2spcZ(ZsL+bz5YW?8 zuZjJlT(^_sHmLqJx!@9&F+!X)m?`d>jK_N$6y7>;ZRh)v6J}3(T;sNzwLP31KW&#* zd{yHu<;}I`XuL-8dX4!V7hI6mz|JmA07L8W}=9r?^)S0mdiS3Kh!PWaa>mjF`Dy;npBVfJ1e z`o4maMJa7E47@;&o3VA#ar1}PoHOIUMP?ks1hA(QC-^iIaSnBA>c6zC;OlGM(V2az zoIY0YY>n@SqMC5Y=jRmJPu|@;Qtt|dpo;&y8#+$$eQU)h-1M=yc{w{h3@qKa84tY{ zEcK19o*De=0CkzjB)fX4_#3^rm&0vIH;5YZY@_FSoY-L7ZY`VL#bY3OXUv{G=F8&D z2TLu0Wrg^CH`K3Rq5e*xI`VcB8pDain6blG$6wF-D3>04$8mhDef!pzl`gwWA}p_d zWNW3rlUg7Y-QJVsZFbqbA@?9J(ee~K%{rC4A3gpIo%RM-LasnXoSkqTfL;Sqpwe4vf4k zEe$gnVzf^lqs6vc9T^zfp&feW{)mN6_=)u>ilD&R}|v zmvZY!>HI;npo6Z@8c%_Z5?n~r#VKNV{(>riWXxqcO_94Ae~bY;nTW%Nq8!Hc6hcf2>Bes9+(z-?nC z+=i9%F0UsdlengWlJj>m+WuIV{0R=CL@<7;$dzCXLH0APkPMN&JPRIjO=TCWbmIXF zb0uO%8W&YUID9D5hjK-F=Kc{&LrTgnJI-oR&c@m?%VZXIvdLrqh~R?(F8##1TqrWo zST!#u$CL8H)m@>w4EvT52E zQ7<&&GIi!Lqui&-G(G$vM{p7{35_KC#`IdRpy5lh zjs%g0Nn!ha8rA&tZdbhFXDxM~vd3NKd10MLu>31Rd?pkGR-<2NVe06j_u@c`*2qqivO2qs3h}4|kr8}h(tBK3^K>*} z?NQ4d1;Jg&%_gO?1M<^$i9NTAJ$H$_hCGS=DI>Sl<>Qa1#fr{+I%-JFq*S3>~bD5jR;Exs+_^O*s;&DMnGqDhKrwhfA6 z9QPTp)?{6`MOR102Id-Do}tBy#0Tf`@ekd%>q&JEAOS1&g} zsfr>){o^l_ye<8U@isG03bos67T7#$*eu83C5jNDq<}(6P>@=azu%qU6=lDofu4@vl2mCBYei z#}M11xOm=aj74K!*9m6Lc>Tg{3W2`*$7P}!)q*L^uQNS;reG!!`nQr(fjo9vmKc9K zBbotZZ>-iy%#I1;g!yVNoLZ;02wE0nfIKaXGU;@`*vM{6=;mphiFdRl6xYClp1TnZ zWac=JBKs|hU)r`1C0F_E04vI`gQ79hqScMdfAzA}=RT}$wmt;db0=}qav1X?X3cq_ z)M=??4e2Y!aDhU%;0uKUoQLnB?7&pr9hg$RQL>ZqQTq!^f9sx6h;5^8bq@Kyb+7#o zMNGk#&KYZDFKPxRnu?zIs}YIj=eg0fm8L=ZoIY?UDspIt1bzmc$rnlZl(C6BXB3IW zE9DKWb`GZ8;k6AOFRWE-6>dm5-F405+c-Z+U~z1IaY%V=tz%dNHkx z5XwSF_15Ba1C}~W)#L2)LCPf!q=kM4p+bElmvqg**L7|n7#af;*pF>*(%3gXNuj{3 zD*u}PiN4_kY00|x+BMD8jun?Nmu}390TF$0eX_HPH7n-p@AH;+5(pspD%A=eAj zD#V`oI$9Q2NExBj^n}J+)><<^h&6Z4^=T?t*%`YuLs$0Vh9Fl%8@sY~io)i3Im&dYm>puivP5JPA;-{cJqA)m3K#=wYZumo^}d`}wya z?I~H}^&$oMoJ|FHd?p^^4yi)cF94k!R0<|Q@expbx=AeEfef-czGmQNkVQnjC^}&c zHvFL3K0VQE@rv?s5~UY7I7CQ1)&*H_$J|g7rY+!fx;tg#4Oi9}+1;D&yn+Ago1Q@R zD2<>@Z26o5?D=3LD72bw`y-ah43IFTyBV_INkCu$%9rDP%&c+z)^l4wR%dAiXO^B) zKeb)-N{Y=^eob-EHK+8iATybYp)tQh+GW+EZ?0j73U7|Thm~m-nwnQ^pDrVg)pyxn zwzUDMN{4qb6vk>cobOpqfk?%{Me>wtV($d(EMxw7BbU*)w)xM`w7Ld)ku8diCRati z@-9;6;B5Yk87V8xh{?^7-eb!At9I%Zni3REf%1!F<^o0}=_8b`UfVv0PaE7X#x9U} zNM3N?YW-3FirvjqT(MMW;5>=M56XCFV4SsziKfTCnD&P7Yt&lTdL1BRtcY!a-)lcnea(fxk_%5SStl`ER;uHIFPyurl9l9g1T$f_ z(Q(IOqpf?K%%4LQF?rzGuS!QZu0P|mN#%MYAKXHXGN(`O0Vz3%gJQ^9s43h`cNxa@ z+Qw#gV3>of4qnpP`P8Ok!?Pm}_Jq?Su8_+pSf@ZFO?F~5mg~msSjtN{IA7Qgj@3M+M-U1B5{+o^nm> zs55z{-6?iCH*StwlCS1HG*IU2LmC<|U;QpOCkK;Ng)w4=R3+1*eKlK?yNv$t++Si0 ziE`p1IeCfNZ4dILtBOVB#Bw#aY2p~eF=KWyTrrMCo+>9e2=!0&Re7;#gq#&S$|xs0 zN7=Lr$yN7lIYJDvJaF+wcC2a#uBrC05yTZknkU@Fe&XbF6{BA5PL!xqDCLr0glT%W zy9{P)a$+#chao6&Effrcs(3K}B?$4IP9k>5)RZ_+qpZtoP%kJ^a9Si0Cl;QmI&R|k&r=)-yrUeCmH=J_L9E)&SZXNIE?n8uC8Wph=-X9+Lm!)q9N}eKIbwH!7_FbzhNlC+dVG*1JzY2NyBqQN)|Fbgi8o*cVK$)ByoXd zTlLJ1&foBN^mChqB@f_T4<)@crfkOs-mGlU)k4Vk>hpmCi&WZrWQq@;(**Xe6TL#? zJgXpaF*bb}!Qa0-D&dBE1cum}L3`6MKZfm&Mje}{BQ}-05K1tbWqolw#3P$=r#Fs1 zY%45zA|x*_*M7lCEU7~BOMevjAl;Cb!zmonnKwSPJ#E*G6g$hwfr9DnTWP~J%Q4H@ zA*zA@tTd*a&?$bsN*72?Nm`fqTt%~qXues~T=XQ#RfXyPuu`@*_RzzM=J;*>_J_1% zX`(KJTQ1Z?cqZbTt3kG}8FG#=Ris*^*@7TyG90_RvRTlamg=}xeSNu_qm|?V<`#m@ z*}xn93_&eOj?9Fu^VGw!>G6!JE|xXuf<-t=m{lC&=|vV z%0+m&wRqLR&5(}#OT#JfFpySA!zApwyIF}P+w(J4l~e9^8Jub|#N$&!g~VHe-6~}P zk~}h3=sf{Iy*aEot+eL5`VS%bfdOTnP^hJlu4d6~{lI8VSH*+CVt8chC@St}T%Hxb zZCdZao6h=5N1Cswn#`p7HzdSgX%s{u87f3AR2*sHqzxtgdO?R4z0|r6(>=^=U8InL z;Uq)Ht76C&BME=#5kPI#DSlQ=b|42z2H^cK$z^JeK14pE%QK*c1rY^WyKzZJ;neSS z-P7=zWC;_|e8swz2~v;AGZAGz1t3$2UPG!)P3JS!QXucfr7(-}Ln4S4c_x zd|lyiq~ek}1d2^;XTqvht0?TP@ar6g=v-jI9=d^k@Set+8jV%un7N5Y9~|7uCi0O2 z=^c46N6;^{g;xi(RERIB?DI;Dv@5$mmcf`Br`J*pvnV=<7H%OyBgm6BAXQF=IH<3; z4mmW7FK(=-t*(~3*JExOGK=T-W&yuxw*^S_j%wsfAdoGjc~#y89yO7NH7|8WS2f3w z?;^gIG3W|pyn$1s90na6CSyY+?)*k;C|0O&%0?!z&PMbOlc6 zH!n=({fu5u!{J@ODVC3-#%5kzOyVMpV}Fe`)$`u_6e2|YIG{lzTZHt?lEjco9llmw z_qfqaF73jZ1!5()1sl_PEwAB;9)0y2PuS4%XffqdgC($JNckWqG!9Oe3#Ze$+uOj@ zbiJvm7E!WAmN|xgUH<`t#$QMjn50??-XRh|n@E7~j&us^GR9R=o^C98Zg-XOop5oE zLh4@jUi8$W%lNlyTHOGH7V(O4DcmQzMf$QEb`44`AR7+H>@G;+vIHZ%U0T>>E3EEY zKW2<^%;*nFYe*$yj4;nV*N!POdA{M5mhjVEX~Z$reNDe<3#od4E_h>$_s!sBdO0(r=3Sysr^3MnMSm#u_fMLHu)`kNYv zGe3`IGmu6vKf8naXowff=DSS-)`uxifACK%C(Y1-%D92$>=WrTNSNpEKMMR+-GAIv zZCsuJf_5czI559bHGdCZuc9rpDKW4EXCNSI@p9a%R}C-6V> zXKINJ`B$-D-pbBRqj+KN5;r-}3;sTf$%M_<)>9#HFX4^GKiAC=RY1?QLfGU8888u3#3l`VgVCfg?39!`sU*?(1S=$K)eH$yM720Xz!%d)SzKeH)R#tzY*FT_T z6Q>jU!EG!X2O$T^%IFlRbqW9U_Gi1L;!WrvsHZoLVE*v;%sEueFNd9ylrK@wVpuf~ z@R+b|5(R{Cp^=dVBey!i=$l#70#qDqRqGfitVJF{$k(~KwVf^85bm!(TT}eh%p5Ij zXR2}yjG-aZ&6A2_za_EjR6MLd+>IwXm^o&ZxQfm3yZU{6ZsZtDVBX-tdS^j1Euk`(l%^(GP1=>5YF6B zUyZ{ee0PA>QPfPioP5yfvvbi(RSO8nxPm9K4H;q#?jV%fiJD9yYa6FWi)bTJ-yP&S z{H;u$sLBWR{IkM~%!9@v)Fu`jWnyz$}m3AhS$huTjks+R_4qN1EFT^l%EB zlx%(q53*(fqtKn%f=pEi+K94&#saiuKr6jpE^px_P%}F%$!3X0O&3wQ|D}7|J#Fkr zT)NV;Aa_rtc?&m3ZLW-_YRQ692O;D_N`=Z}BoE4JnFuSH4*k%gRK62Rl91V``nM^x zJ|e8gYdG+`IB&T5(1sZ@>1c!ldB7Mq@OT6%T8V4hI@jo%X;UaVlY1%+C<45XbczdP z)vt2`mo#_ZMVQRN&{z6d%hBc^*a<2#H3d%CF374wn>gQU_%AfMbAaHN?O)s94M54O zW@{y^Ntvp)#|d{dJwz6YR$qJER~@fXkT4WZz#bkQ4{0wrd{$5=T|6IE=zQ3wd62}? z=l;bYS1Drb|{)PI{B*ihXBNAW-!(@=&e?Yq| z0-F)d7&svABDa=7>R)zzJ9f98XdAY0LPxfwUUiZD)CHuN9oTLW+a@D4Xs$j`M&LzD zppv6f2V=lhxULjES3Ko$gsEv5<_+*@AW^FrQ=Zx6k%oDDb_rw z2oT@DFEGz4ZC9W6?Aye|Aaag)QNOgGNw-apO~bTVn=oH|B_12gh2D>3$vSo?bXoMc zT&18#40oEHmu-@_jT_(7`9h2N0t*c-D-*oOR4QhYmc0E+{J4kdHXW&IAcspZpm=`9 zOT<{C>YU(ah-crusl)HZ%S?a$;aIduwZc8VYAzk*Q#B&a7Acs=ZfJj2uJ)FX_@rzO z9~mN}tI;sxIE2|P$B&r?%|Erqq6ez`SiZ8>xu~w$3lmpZCjedpCKoDGB~mNsBW7+7 zZ0`xYg|}fxAXg6xtgn9twCG{h(M(Vow33CJ-}3XWgm4)KS~Jq`s+1d2x>GE|IPakt z*iB=b(jO@YhKg=4E*^q@1F0Bhnl$TH=24xmHWBRP3<{PDK_8{!XCtk@wiW_X76h%L z{@EP-->%kx5B_a~{%;%ffBWYD+c#guZU4y^1t|UR|7hW->xJG$fBujIE(a|8ufqKQ dL18eMaZyQ|R_`k@!oWYiGv;`o@#1g&zW}13XAuAZ literal 0 HcmV?d00001 diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_display_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_common_test.cpp similarity index 75% rename from rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_display_test.cpp rename to rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_common_test.cpp index 1222c9652..087334180 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_display_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_common_test.cpp @@ -40,8 +40,9 @@ #include "visualization_msgs/msg/marker.hpp" -#include "rviz_default_plugins/displays/marker/marker_display.hpp" -#include "rviz_default_plugins/displays/marker/markers/marker_factory.hpp" +#include "rviz_common/display.hpp" + +#include "rviz_default_plugins/displays/marker/marker_common.hpp" #include "rviz_default_plugins/displays/marker/markers/arrow_marker.hpp" #include "rviz_default_plugins/displays/marker/markers/shape_marker.hpp" #include "rviz_default_plugins/displays/marker/markers/line_strip_marker.hpp" @@ -58,21 +59,25 @@ using namespace ::testing; // NOLINT -class MarkerDisplayFixture : public DisplayTestFixture +class MarkerCommonFixture : public DisplayTestFixture { public: - MarkerDisplayFixture() + MarkerCommonFixture() { - factory_ = std::make_unique(); - factory_->initialize( - nullptr, context_.get(), scene_manager_->getRootSceneNode()->createChildSceneNode()); + display_ = std::make_unique(); + + auto scene_node = scene_manager_->getRootSceneNode()->createChildSceneNode(); + common_ = std::make_unique(display_.get()); + common_->initialize(context_.get(), scene_node); + } - display_ = std::make_unique( - std::move(factory_), context_.get()); + ~MarkerCommonFixture() override + { + common_.reset(); } - std::unique_ptr factory_; - std::unique_ptr display_; + std::unique_ptr common_; + std::unique_ptr display_; }; visualization_msgs::msg::Marker::SharedPtr createSharedPtrMessage( @@ -84,10 +89,10 @@ visualization_msgs::msg::Marker::SharedPtr createSharedPtrMessage( return std::make_shared(marker); } -TEST_F(MarkerDisplayFixture, processMessage_creates_correct_marker_on_add_type) { +TEST_F(MarkerCommonFixture, processMessage_creates_correct_marker_on_add_type) { mockValidTransform(); - display_->processMessage(createSharedPtrMessage( + common_->processMessage(createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::TEXT_VIEW_FACING)); auto text = rviz_rendering::findOneMovableText(scene_manager_->getRootSceneNode()); @@ -96,20 +101,20 @@ TEST_F(MarkerDisplayFixture, processMessage_creates_correct_marker_on_add_type) } -TEST_F(MarkerDisplayFixture, processMessage_deletes_correct_marker_on_delete_type) { +TEST_F(MarkerCommonFixture, processMessage_deletes_correct_marker_on_delete_type) { mockValidTransform(); - display_->processMessage(createSharedPtrMessage( + common_->processMessage(createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, 0)); - display_->processMessage(createSharedPtrMessage( + common_->processMessage(createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::POINTS, 1)); - display_->processMessage(createSharedPtrMessage( + common_->processMessage(createSharedPtrMessage( visualization_msgs::msg::Marker::DELETE, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, 0)); @@ -118,20 +123,20 @@ TEST_F(MarkerDisplayFixture, processMessage_deletes_correct_marker_on_delete_typ ASSERT_TRUE(rviz_rendering::findOnePointCloud(scene_manager_->getRootSceneNode())); } -TEST_F(MarkerDisplayFixture, processMessage_with_deleteall_deletes_all_markers) { +TEST_F(MarkerCommonFixture, processMessage_with_deleteall_deletes_all_markers) { mockValidTransform(); - display_->processMessage(createSharedPtrMessage( + common_->processMessage(createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, 0)); - display_->processMessage(createSharedPtrMessage( + common_->processMessage(createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::POINTS, 1)); - display_->processMessage(createSharedPtrMessage( + common_->processMessage(createSharedPtrMessage( visualization_msgs::msg::Marker::DELETEALL, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, 3)); @@ -140,47 +145,47 @@ TEST_F(MarkerDisplayFixture, processMessage_with_deleteall_deletes_all_markers) ASSERT_FALSE(rviz_rendering::findOnePointCloud(scene_manager_->getRootSceneNode())); } -TEST_F(MarkerDisplayFixture, proccesMessage_add_all_markers_correctly) { +TEST_F(MarkerCommonFixture, proccesMessage_add_all_markers_correctly) { mockValidTransform(); auto marker = createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::TEXT_VIEW_FACING); - display_->processMessage(marker); + common_->processMessage(marker); ASSERT_TRUE(rviz_rendering::findOneMovableText(scene_manager_->getRootSceneNode())); - display_->deleteAllMarkers(); + common_->deleteAllMarkers(); marker->type = visualization_msgs::msg::Marker::MESH_RESOURCE; - display_->processMessage(marker); + common_->processMessage(marker); ASSERT_TRUE(rviz_rendering::findEntityByMeshName( scene_manager_->getRootSceneNode(), marker->mesh_resource)); - display_->deleteAllMarkers(); + common_->deleteAllMarkers(); marker->type = visualization_msgs::msg::Marker::ARROW; - display_->processMessage(marker); + common_->processMessage(marker); ASSERT_TRUE(rviz_rendering::findOneArrow(scene_manager_->getRootSceneNode())); - display_->deleteAllMarkers(); + common_->deleteAllMarkers(); marker->type = visualization_msgs::msg::Marker::LINE_LIST; - display_->processMessage(marker); + common_->processMessage(marker); ASSERT_TRUE(rviz_rendering::findOneBillboardChain(scene_manager_->getRootSceneNode())); - display_->deleteAllMarkers(); + common_->deleteAllMarkers(); marker->type = visualization_msgs::msg::Marker::LINE_STRIP; - display_->processMessage(marker); + common_->processMessage(marker); ASSERT_TRUE(rviz_rendering::findOneBillboardChain(scene_manager_->getRootSceneNode())); - display_->deleteAllMarkers(); + common_->deleteAllMarkers(); marker->type = visualization_msgs::msg::Marker::CYLINDER; - display_->processMessage(marker); + common_->processMessage(marker); ASSERT_TRUE(rviz_rendering::findEntityByMeshName( scene_manager_->getRootSceneNode(), "rviz_cylinder.mesh")); - display_->deleteAllMarkers(); + common_->deleteAllMarkers(); geometry_msgs::msg::Point point; point.x = 0; @@ -188,23 +193,23 @@ TEST_F(MarkerDisplayFixture, proccesMessage_add_all_markers_correctly) { point.z = 0; marker->type = visualization_msgs::msg::Marker::TRIANGLE_LIST; marker->points.push_back(point); - display_->processMessage(marker); + common_->processMessage(marker); ASSERT_TRUE(rviz_rendering::findOneManualObject(scene_manager_->getRootSceneNode())); - display_->deleteAllMarkers(); + common_->deleteAllMarkers(); } -TEST_F(MarkerDisplayFixture, processMessage_adds_two_markers_of_same_type_if_ids_are_different) { +TEST_F(MarkerCommonFixture, processMessage_adds_two_markers_of_same_type_if_ids_are_different) { mockValidTransform(); - display_->processMessage(createSharedPtrMessage( + common_->processMessage(createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::ARROW, 0)); EXPECT_THAT(rviz_rendering::findAllArrows(scene_manager_->getRootSceneNode()), SizeIs(1)); - display_->processMessage(createSharedPtrMessage( + common_->processMessage(createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::ARROW, 1)); @@ -212,34 +217,34 @@ TEST_F(MarkerDisplayFixture, processMessage_adds_two_markers_of_same_type_if_ids EXPECT_THAT(rviz_rendering::findAllArrows(scene_manager_->getRootSceneNode()), SizeIs(2)); } -TEST_F(MarkerDisplayFixture, +TEST_F(MarkerCommonFixture, processMessage_adds_two_markers_of_same_type_if_namespaces_are_different) { mockValidTransform(); auto marker = createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::ARROW); - display_->processMessage(marker); + common_->processMessage(marker); EXPECT_THAT(rviz_rendering::findAllArrows(scene_manager_->getRootSceneNode()), SizeIs(1)); marker->ns = "new_ns"; - display_->processMessage(marker); + common_->processMessage(marker); EXPECT_THAT(rviz_rendering::findAllArrows(scene_manager_->getRootSceneNode()), SizeIs(2)); } -TEST_F(MarkerDisplayFixture, processMessage_does_not_create_new_marker_if_id_already_exists) { +TEST_F(MarkerCommonFixture, processMessage_does_not_create_new_marker_if_id_already_exists) { mockValidTransform(); - display_->processMessage(createSharedPtrMessage( + common_->processMessage(createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::ARROW, 0)); EXPECT_THAT(rviz_rendering::findAllArrows(scene_manager_->getRootSceneNode()), SizeIs(1)); - display_->processMessage(createSharedPtrMessage( + common_->processMessage(createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::ARROW, 0)); @@ -247,36 +252,36 @@ TEST_F(MarkerDisplayFixture, processMessage_does_not_create_new_marker_if_id_alr EXPECT_THAT(rviz_rendering::findAllArrows(scene_manager_->getRootSceneNode()), SizeIs(1)); } -TEST_F(MarkerDisplayFixture, processMessage_updates_modified_marker) { +TEST_F(MarkerCommonFixture, processMessage_updates_modified_marker) { mockValidTransform(); auto marker = createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::TEXT_VIEW_FACING); - display_->processMessage(marker); + common_->processMessage(marker); auto before_update = rviz_rendering::findOneMovableText(scene_manager_->getRootSceneNode()); ASSERT_TRUE(before_update); EXPECT_THAT(before_update->getCaption(), StrEq(marker->text)); marker->text = "New text"; - display_->processMessage(marker); + common_->processMessage(marker); auto after_update = rviz_rendering::findOneMovableText(scene_manager_->getRootSceneNode()); ASSERT_TRUE(after_update); EXPECT_THAT(after_update->getCaption(), StrEq("New text")); } -TEST_F(MarkerDisplayFixture, processMessage_using_modify_works_like_add) { +TEST_F(MarkerCommonFixture, processMessage_using_modify_works_like_add) { mockValidTransform(); - display_->processMessage(createSharedPtrMessage( + common_->processMessage(createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::ARROW, 0)); EXPECT_THAT(rviz_rendering::findAllArrows(scene_manager_->getRootSceneNode()), SizeIs(1)); - display_->processMessage(createSharedPtrMessage( + common_->processMessage(createSharedPtrMessage( visualization_msgs::msg::Marker::MODIFY, visualization_msgs::msg::Marker::ARROW, 0)); @@ -284,7 +289,7 @@ TEST_F(MarkerDisplayFixture, processMessage_using_modify_works_like_add) { EXPECT_THAT(rviz_rendering::findAllArrows(scene_manager_->getRootSceneNode()), SizeIs(1)); } -TEST_F(MarkerDisplayFixture, update_retransforms_frame_locked_messages) { +TEST_F(MarkerCommonFixture, update_retransforms_frame_locked_messages) { auto marker = createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::POINTS); marker->frame_locked = true; @@ -306,7 +311,7 @@ TEST_F(MarkerDisplayFixture, update_retransforms_frame_locked_messages) { SetArgReferee<4>(next_orientation), Return(true))); - display_->processMessage(marker); + common_->processMessage(marker); auto pointCloud = rviz_rendering::findOnePointCloud(scene_manager_->getRootSceneNode()); ASSERT_TRUE(pointCloud); @@ -314,14 +319,14 @@ TEST_F(MarkerDisplayFixture, update_retransforms_frame_locked_messages) { EXPECT_THAT(pointCloud->getParentSceneNode()->getOrientation(), QuaternionEq(starting_orientation)); - display_->update(0, 0); + common_->update(0, 0); EXPECT_THAT(pointCloud->getParentSceneNode()->getPosition(), Vector3Eq(next_position)); EXPECT_THAT(pointCloud->getParentSceneNode()->getOrientation(), QuaternionEq(next_orientation)); } -TEST_F(MarkerDisplayFixture, update_does_not_retransform_normal_messages) { +TEST_F(MarkerCommonFixture, update_does_not_retransform_normal_messages) { auto marker = createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::POINTS); marker->frame_locked = false; @@ -337,7 +342,7 @@ TEST_F(MarkerDisplayFixture, update_does_not_retransform_normal_messages) { Return(true) )); // Test will fail if this function is called repeatedly - display_->processMessage(marker); + common_->processMessage(marker); auto pointCloud = rviz_rendering::findOnePointCloud(scene_manager_->getRootSceneNode()); ASSERT_TRUE(pointCloud); @@ -345,81 +350,81 @@ TEST_F(MarkerDisplayFixture, update_does_not_retransform_normal_messages) { EXPECT_THAT(pointCloud->getParentSceneNode()->getOrientation(), QuaternionEq(starting_orientation)); - display_->update(0, 0); + common_->update(0, 0); EXPECT_THAT(pointCloud->getParentSceneNode()->getPosition(), Vector3Eq(starting_position)); EXPECT_THAT(pointCloud->getParentSceneNode()->getOrientation(), QuaternionEq(starting_orientation)); } -TEST_F(MarkerDisplayFixture, processMessage_adds_new_namespace_for_message) { +TEST_F(MarkerCommonFixture, processMessage_adds_new_namespace_for_message) { mockValidTransform(); - ASSERT_THAT(display_->childAt(3)->numChildren(), Eq(0)); + ASSERT_THAT(display_->childAt(0)->numChildren(), Eq(0)); auto marker = createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::POINTS); - display_->processMessage(marker); + common_->processMessage(marker); - ASSERT_THAT(display_->childAt(3)->childAt(0)->getName().toStdString(), StrEq("test_ns")); + ASSERT_THAT(display_->childAt(0)->childAt(0)->getName().toStdString(), StrEq("test_ns")); } -TEST_F(MarkerDisplayFixture, processMessage_does_not_add_new_namespace_if_already_present) { +TEST_F(MarkerCommonFixture, processMessage_does_not_add_new_namespace_if_already_present) { mockValidTransform(); - display_->processMessage(createSharedPtrMessage( + common_->processMessage(createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::POINTS)); - ASSERT_THAT(display_->childAt(3)->numChildren(), Eq(1)); - ASSERT_THAT(display_->childAt(3)->childAt(0)->getName().toStdString(), StrEq("test_ns")); + ASSERT_THAT(display_->childAt(0)->numChildren(), Eq(1)); + ASSERT_THAT(display_->childAt(0)->childAt(0)->getName().toStdString(), StrEq("test_ns")); - display_->processMessage(createSharedPtrMessage( + common_->processMessage(createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::TEXT_VIEW_FACING)); - ASSERT_THAT(display_->childAt(3)->numChildren(), Eq(1)); - ASSERT_THAT(display_->childAt(3)->childAt(0)->getName().toStdString(), StrEq("test_ns")); + ASSERT_THAT(display_->childAt(0)->numChildren(), Eq(1)); + ASSERT_THAT(display_->childAt(0)->childAt(0)->getName().toStdString(), StrEq("test_ns")); } -TEST_F(MarkerDisplayFixture, onEnableChanged_in_namespace_removes_all_markers_in_that_namespace) { +TEST_F(MarkerCommonFixture, onEnableChanged_in_namespace_removes_all_markers_in_that_namespace) { mockValidTransform(); auto marker = createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::POINTS); - display_->processMessage(marker); + common_->processMessage(marker); marker->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker->ns = "new_ns"; - display_->processMessage(marker); + common_->processMessage(marker); EXPECT_TRUE(rviz_rendering::findOnePointCloud(scene_manager_->getRootSceneNode())); EXPECT_TRUE(rviz_rendering::findOneMovableText(scene_manager_->getRootSceneNode())); auto namespace_property = dynamic_cast( - display_->childAt(3)->childAt(0)); + display_->childAt(0)->childAt(0)); namespace_property->setValue(false); EXPECT_FALSE(rviz_rendering::findOnePointCloud(scene_manager_->getRootSceneNode())); EXPECT_TRUE(rviz_rendering::findOneMovableText(scene_manager_->getRootSceneNode())); } -TEST_F(MarkerDisplayFixture, processMessage_does_not_add_message_with_disabled_namespace) { +TEST_F(MarkerCommonFixture, processMessage_does_not_add_message_with_disabled_namespace) { mockValidTransform(); auto marker = createSharedPtrMessage( visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::POINTS); // this is necessary to initialize namespace as we don't load a config - display_->processMessage(marker); + common_->processMessage(marker); EXPECT_TRUE(rviz_rendering::findOnePointCloud(scene_manager_->getRootSceneNode())); auto namespace_property = dynamic_cast( - display_->childAt(3)->childAt(0)); + display_->childAt(0)->childAt(0)); namespace_property->setValue(false); marker->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - display_->processMessage(marker); + common_->processMessage(marker); EXPECT_FALSE(rviz_rendering::findOnePointCloud(scene_manager_->getRootSceneNode())); EXPECT_FALSE(rviz_rendering::findOneMovableText(scene_manager_->getRootSceneNode())); diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_display_visual_test.cpp index 7c84d676b..5675e8247 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_display_visual_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_display_visual_test.cpp @@ -36,10 +36,11 @@ #include "rviz_visual_testing_framework/visual_test_publisher.hpp" #include "../../page_objects/marker_display_page_object.hpp" +#include "../../publishers/marker_array_publisher.hpp" #include "../../publishers/marker_publisher.hpp" TEST_F(VisualTestFixture, test_marker_with_three_marker_types) { - auto path_publisher = std::make_unique( + auto marker_publisher = std::make_unique( std::make_shared(), "marker_frame"); setCamPose(Ogre::Vector3(0, 0, 16)); @@ -51,3 +52,17 @@ TEST_F(VisualTestFixture, test_marker_with_three_marker_types) { assertMainWindowIdentity(); } + +TEST_F(VisualTestFixture, test_marker_with_marker_array) { + auto marker_publisher = std::make_unique( + std::make_shared(), "marker_array_frame"); + + setCamPose(Ogre::Vector3(0, 0, 16)); + setCamLookAt(Ogre::Vector3(0, 0, 0)); + + auto marker_display = addDisplay(); + marker_display->setTopic("/marker"); + wait(2000); + + assertMainWindowIdentity(); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.cpp index 297072977..17e0c025f 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.cpp @@ -36,5 +36,9 @@ MarkersTestFixture::MarkersTestFixture() { - marker_display_ = std::make_shared(); + display_ = std::make_unique(); + scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); + + marker_common_ = std::make_unique(display_.get()); + marker_common_->initialize(context_.get(), scene_node_); } diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.hpp b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.hpp index ca260807e..1f3db48c4 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.hpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.hpp @@ -60,13 +60,16 @@ class MarkersTestFixture : public DisplayTestFixture std::unique_ptr makeMarker() { return std::make_unique( - marker_display_.get(), + marker_common_.get(), context_.get(), - scene_manager_->getRootSceneNode()->createChildSceneNode()); + scene_node_); } - std::shared_ptr marker_display_; - std::shared_ptr marker_; + std::unique_ptr display_; + std::unique_ptr marker_common_; + Ogre::SceneNode * scene_node_; + + std::unique_ptr marker_; }; #endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__MARKER__MARKERS__MARKERS_TEST_FIXTURE_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/marker_array/marker_array_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/marker_array/marker_array_display_visual_test.cpp new file mode 100644 index 000000000..b381ce079 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/marker_array/marker_array_display_visual_test.cpp @@ -0,0 +1,53 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted (subject to the limitations in the disclaimer + * below) 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. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. 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 +#include + +#include "rviz_visual_testing_framework/visual_test_fixture.hpp" +#include "rviz_visual_testing_framework/visual_test_publisher.hpp" + +#include "../../page_objects/marker_array_display_page_object.hpp" +#include "../../publishers/marker_array_publisher.hpp" + +TEST_F(VisualTestFixture, test_marker_array_with_three_marker_types) { + auto marker_array_publisher = std::make_unique( + std::make_shared(), "marker_array_frame"); + + setCamPose(Ogre::Vector3(0, 0, 16)); + setCamLookAt(Ogre::Vector3(0, 0, 0)); + + auto marker_display = addDisplay(); + marker_display->setTopic("/marker_array"); + wait(2000); + + assertMainWindowIdentity(); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/marker_array_display_page_object.cpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/marker_array_display_page_object.cpp new file mode 100644 index 000000000..d624f42d2 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/marker_array_display_page_object.cpp @@ -0,0 +1,36 @@ +/* + * 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 OWNER 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 "marker_array_display_page_object.hpp" + +MarkerArrayDisplayPageObject::MarkerArrayDisplayPageObject() +: MarkerDisplayPageObject() +{ + display_name_ = "MarkerArray"; +} diff --git a/rviz/src/rviz/default_plugin/marker_array_display.h b/rviz_default_plugins/test/rviz_default_plugins/page_objects/marker_array_display_page_object.hpp similarity index 58% rename from rviz/src/rviz/default_plugin/marker_array_display.h rename to rviz_default_plugins/test/rviz_default_plugins/page_objects/marker_array_display_page_object.hpp index 22953a78b..d34f63966 100644 --- a/rviz/src/rviz/default_plugin/marker_array_display.h +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/marker_array_display_page_object.hpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2011, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -10,8 +10,8 @@ * * 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 Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from + * * 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" @@ -26,38 +26,16 @@ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RVIZ_MARKER_ARRAY_DISPLAY_H -#define RVIZ_MARKER_ARRAY_DISPLAY_H -#include "marker_display.h" +#ifndef RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__MARKER_ARRAY_DISPLAY_PAGE_OBJECT_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__MARKER_ARRAY_DISPLAY_PAGE_OBJECT_HPP_ -namespace rviz -{ +#include "marker_display_page_object.hpp" -/** - * @brief Display for an array of markers. The MarkerDisplay class handles - * MarkerArray messages. This is just a wrapper to let MarkerArray - * topics get selected in the topic browser. - */ -class MarkerArrayDisplay: public MarkerDisplay +class MarkerArrayDisplayPageObject : public MarkerDisplayPageObject { -Q_OBJECT public: - MarkerArrayDisplay(); - -protected: - /** @brief Overridden from MarkerDisplay. Subscribes to the marker - * array topic. */ - virtual void subscribe(); - - /** @brief Overridden from MarkerDisplay. Unsubscribes to the - * marker array topic. */ - virtual void unsubscribe(); - -private: - void handleMarkerArray( const visualization_msgs::MarkerArray::ConstPtr& array ); + MarkerArrayDisplayPageObject(); }; -} // end namespace rviz - -#endif // RVIZ_MARKER_ARRAY_DISPLAY_H +#endif // RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__MARKER_ARRAY_DISPLAY_PAGE_OBJECT_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/publishers/marker_array_publisher.hpp b/rviz_default_plugins/test/rviz_default_plugins/publishers/marker_array_publisher.hpp new file mode 100644 index 000000000..62a5d6b08 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/publishers/marker_array_publisher.hpp @@ -0,0 +1,99 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted (subject to the limitations in the disclaimer + * below) 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. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. 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__MARKER_ARRAY_PUBLISHER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PUBLISHERS__MARKER_ARRAY_PUBLISHER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/header.hpp" + +// TODO(greimela): Workaround for duplicate constant definition +// compare https://github.com/ros2/common_interfaces/issues/44 +#if defined(_WIN32) && defined(DELETE) +# undef DELETE +#endif +#include "visualization_msgs/msg/marker_array.hpp" +#include "marker_publisher.hpp" + +using namespace std::chrono_literals; // NOLINT + +namespace nodes +{ + +class MarkerArrayPublisher : public MarkerPublisher +{ +public: + MarkerArrayPublisher() + : MarkerPublisher("marker_array_publisher") + { + array_publisher_ = this->create_publisher("marker_array"); + } + +private: + void timer_callback() override + { + auto header = std_msgs::msg::Header(); + header.frame_id = "marker_array_frame"; + header.stamp = rclcpp::Clock().now(); + + std::string ns = "test_ns"; + int id = 0; + + auto first_sphere_marker = createSphereMarker(header, ns, ++id); + auto second_sphere_marker = createSphereMarker(header, ns, ++id); + auto cube_list_marker = createCubeListMarker(header, ns, ++id); + cube_list_marker.color.g = 0; + first_sphere_marker.color.g = 0; + first_sphere_marker.color.b = 1; + second_sphere_marker.color.g = 0; + second_sphere_marker.color.b = 1; + first_sphere_marker.pose.position.x = -2; + second_sphere_marker.pose.position.x = 2; + + auto marker_array = visualization_msgs::msg::MarkerArray(); + marker_array.markers = std::vector(); + marker_array.markers.emplace_back(first_sphere_marker); + marker_array.markers.emplace_back(second_sphere_marker); + marker_array.markers.emplace_back(createLineStripMarker(header, ns, ++id)); + marker_array.markers.emplace_back(cube_list_marker); + + array_publisher_->publish(marker_array); + } + + rclcpp::Publisher::SharedPtr array_publisher_; +}; + +} // namespace nodes + +#endif // RVIZ_DEFAULT_PLUGINS__PUBLISHERS__MARKER_ARRAY_PUBLISHER_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/publishers/marker_publisher.hpp b/rviz_default_plugins/test/rviz_default_plugins/publishers/marker_publisher.hpp index 646a1d17b..cc3806b3b 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/publishers/marker_publisher.hpp +++ b/rviz_default_plugins/test/rviz_default_plugins/publishers/marker_publisher.hpp @@ -60,26 +60,15 @@ class MarkerPublisher : public rclcpp::Node timer = this->create_wall_timer(200ms, std::bind(&MarkerPublisher::timer_callback, this)); } -private: - void timer_callback() + // Convenience constructor for by MarkerArrayPublisher + explicit MarkerPublisher(std::string node_name) + : Node(node_name) { - auto header = std_msgs::msg::Header(); - header.frame_id = "marker_frame"; - header.stamp = rclcpp::Clock().now(); - - std::string ns = "test_ns"; - int id = 0; - - auto sphere_marker = createSphereMarker(header, ns, ++id); - publisher->publish(sphere_marker); - - auto line_strip_marker = createLineStripMarker(header, ns, ++id); - publisher->publish(line_strip_marker); - - auto cube_list_marker = createCubeListMarker(header, ns, ++id); - publisher->publish(cube_list_marker); + timer = this->create_wall_timer(200ms, std::bind(&MarkerPublisher::timer_callback, this)); + publisher = this->create_publisher("marker"); } +protected: visualization_msgs::msg::Marker createSphereMarker( std_msgs::msg::Header header, const std::string & ns, int id) { @@ -203,6 +192,26 @@ class MarkerPublisher : public rclcpp::Node return marker; } +private: + virtual void timer_callback() + { + auto header = std_msgs::msg::Header(); + header.frame_id = "marker_frame"; + header.stamp = rclcpp::Clock().now(); + + std::string ns = "test_ns"; + int id = 0; + + auto sphere_marker = createSphereMarker(header, ns, ++id); + publisher->publish(sphere_marker); + + auto line_strip_marker = createLineStripMarker(header, ns, ++id); + publisher->publish(line_strip_marker); + + auto cube_list_marker = createCubeListMarker(header, ns, ++id); + publisher->publish(cube_list_marker); + } + rclcpp::TimerBase::SharedPtr timer; rclcpp::Publisher::SharedPtr publisher; };