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 000000000..5b3ca3096 Binary files /dev/null and b/rviz_default_plugins/test/reference_images/test_marker_array_with_three_marker_types_ref.png differ 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 000000000..2ae914dc1 Binary files /dev/null and b/rviz_default_plugins/test/reference_images/test_marker_with_marker_array_ref.png differ 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; };