diff --git a/rviz_common/CMakeLists.txt b/rviz_common/CMakeLists.txt index e285ace1d..c67d68290 100644 --- a/rviz_common/CMakeLists.txt +++ b/rviz_common/CMakeLists.txt @@ -40,7 +40,7 @@ find_package(rviz_assimp_vendor REQUIRED) find_package(rviz_rendering REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) -find_package(tinyxml_vendor REQUIRED) +find_package(tinyxml2_vendor REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) @@ -48,7 +48,7 @@ find_package(message_filters REQUIRED) find_package(urdf REQUIRED) find_package(yaml_cpp_vendor REQUIRED) -find_package(TinyXML REQUIRED) # provided by tinyxml_vendor +find_package(TinyXML2 REQUIRED) # provided by tinyxml_vendor # Copy env_config.hpp so that env_config.cpp can find it # TODO(jsquare): Get rid of copy hpp file @@ -227,7 +227,6 @@ target_include_directories(rviz_common PUBLIC $ $ - ${TinyXML_INCLUDE_DIRS} ) target_link_libraries(rviz_common @@ -236,7 +235,6 @@ target_link_libraries(rviz_common rviz_ogre_vendor::OgreOverlay rviz_rendering::rviz_rendering Qt5::Widgets - ${TinyXML_LIBRARIES} ) ament_target_dependencies(rviz_common @@ -252,6 +250,7 @@ ament_target_dependencies(rviz_common tf2_geometry_msgs tf2_ros message_filters + tinyxml2_vendor urdf yaml_cpp_vendor ) @@ -330,6 +329,17 @@ if(BUILD_TESTING) qt5_wrap_cpp(rviz_common_test_moc_files test/mock_property_change_receiver.hpp) qt5_wrap_cpp(rviz_common_test_frame_manager_moc src/rviz_common/frame_manager.hpp) + + ament_add_gmock(display_factory_test + test/display_factory_test.cpp + src/rviz_common/display_factory.cpp + ${SKIP_DISPLAY_TESTS}) + if(TARGET display_factory_test) + target_compile_definitions(display_factory_test PUBLIC + -D_TEST_PLUGIN_DESCRIPTIONS="${CMAKE_CURRENT_SOURCE_DIR}") + target_link_libraries(display_factory_test rviz_common Qt5::Widgets) + endif() + ament_add_gmock(frame_manager_test test/frame_manager_test.cpp src/rviz_common/frame_manager.cpp diff --git a/rviz_common/package.xml b/rviz_common/package.xml index 82a856136..cff84ba60 100644 --- a/rviz_common/package.xml +++ b/rviz_common/package.xml @@ -40,7 +40,7 @@ tf2_geometry_msgs tf2_ros message_filters - tinyxml_vendor + tinyxml2_vendor urdf yaml_cpp_vendor diff --git a/rviz_common/src/rviz_common/display_factory.cpp b/rviz_common/src/rviz_common/display_factory.cpp index 348de990d..54721c468 100644 --- a/rviz_common/src/rviz_common/display_factory.cpp +++ b/rviz_common/src/rviz_common/display_factory.cpp @@ -32,8 +32,7 @@ #include -// TODO(wjwwood): replace with tinyxml2? implicit dependency? -#include // NOLINT: cpplint is unable to handle the include order here +#include // NOLINT: cpplint is unable to handle the include order here #include "rviz_common/display_group.hpp" #include "rviz_common/logging.hpp" @@ -77,71 +76,22 @@ QSet DisplayFactory::getMessageTypes(const QString & class_id) if (!xml_file.isEmpty()) { RVIZ_COMMON_LOG_DEBUG_STREAM("Parsing " << xml_file.toStdString()); - TiXmlDocument document; - document.LoadFile(xml_file.toStdString()); - TiXmlElement * config = document.RootElement(); - if (config == nullptr) { - RVIZ_COMMON_LOG_ERROR_STREAM( - "Skipping XML Document \"" << xml_file.toStdString() << "\" which had no Root Element. " - "This likely means the XML is malformed or missing."); - return QSet(); - } - if (config->ValueStr() != "library" && - config->ValueStr() != "class_libraries") + tinyxml2::XMLDocument document; + document.LoadFile(xml_file.toUtf8().constData()); + tinyxml2::XMLElement * config = document.RootElement(); + if (!hasRootNode(config, xml_file.toStdString()) || + !hasLibraryRoot(config, xml_file.toStdString())) { - RVIZ_COMMON_LOG_ERROR_STREAM( - "The XML document \"" << xml_file.toStdString() << - "\" given to add must have either \"library\" or " - "\"class_libraries\" as the root tag"); return QSet(); } // Step into the filter list if necessary - if (config->ValueStr() == "class_libraries") { + if (config->Value() == std::string("class_libraries")) { config = config->FirstChildElement("library"); } - TiXmlElement * library = config; + tinyxml2::XMLElement * library = config; while (library) { - TiXmlElement * class_element = library->FirstChildElement("class"); - while (class_element) { - std::string derived_class; - if (class_element->Attribute("type")) { - derived_class = class_element->Attribute("type"); - } - std::string current_class_id; - if (class_element->Attribute("name")) { - current_class_id = class_element->Attribute("name"); - RVIZ_COMMON_LOG_DEBUG_STREAM( - "XML file specifies lookup name (i.e. magic name) = " << current_class_id); - } else { - RVIZ_COMMON_LOG_DEBUG_STREAM( - "XML file has no lookup name (i.e. magic name) for class " << derived_class << - ", assuming class_id == real class name."); - current_class_id = derived_class; - } - - QSet message_types; - TiXmlElement * message_type = class_element->FirstChildElement("message_type"); - - while (message_type) { - if (message_type->GetText()) { - const char * message_type_str = message_type->GetText(); - RVIZ_COMMON_LOG_DEBUG_STREAM( - current_class_id << " supports message type " << message_type_str); -#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0) - message_types.insert(QString::fromAscii(message_type_str)); -#else - message_types.insert(QString(message_type_str)); -#endif - } - message_type = message_type->NextSiblingElement("message_type"); - } - - message_type_cache_[QString::fromStdString(current_class_id)] = message_types; - - // step to next class_element - class_element = class_element->NextSiblingElement("class"); - } + fillCacheForAllClassElements(library); library = library->NextSiblingElement("library"); } } @@ -154,4 +104,85 @@ QSet DisplayFactory::getMessageTypes(const QString & class_id) return QSet(); } +bool DisplayFactory::hasRootNode(tinyxml2::XMLElement * root_element, const std::string & xml_file) +{ + if (root_element == nullptr) { + RVIZ_COMMON_LOG_ERROR_STREAM( + "Skipping XML Document \"" << xml_file << "\" which had no Root Element. " + "This likely means the XML is malformed or missing."); + return false; + } + return true; +} + +bool +DisplayFactory::hasLibraryRoot(tinyxml2::XMLElement * root_element, const std::string & xml_file) +{ + if (root_element->Value() != std::string("library") && + root_element->Value() != std::string("class_libraries")) + { + RVIZ_COMMON_LOG_ERROR_STREAM( + "The XML document \"" << xml_file << + "\" given to add must have either \"library\" or " + "\"class_libraries\" as the root tag"); + return false; + } + return true; +} + +void DisplayFactory::fillCacheForAllClassElements(tinyxml2::XMLElement * library) +{ + tinyxml2::XMLElement * class_element = library->FirstChildElement("class"); + while (class_element) { + const std::string derived_class = lookupDerivedClass(class_element); + const std::string current_class_id = lookupClassId(class_element, derived_class); + QSet message_types = parseMessageTypes(class_element, current_class_id); + + message_type_cache_[QString::fromStdString(current_class_id)] = message_types; + + class_element = class_element->NextSiblingElement("class"); + } +} + +QSet DisplayFactory::parseMessageTypes( + tinyxml2::XMLElement * class_element, const std::string & current_class_id) const +{ + QSet message_types; + + const tinyxml2::XMLElement * message_type = class_element->FirstChildElement("message_type"); + while (message_type) { + if (message_type->GetText()) { + const char * message_type_str = message_type->GetText(); + RVIZ_COMMON_LOG_DEBUG_STREAM( + current_class_id << " supports message type " << message_type_str); + message_types.insert(QString(message_type_str)); + } + message_type = message_type->NextSiblingElement("message_type"); + } + return message_types; +} + +std::string DisplayFactory::lookupDerivedClass(const tinyxml2::XMLElement * class_element) const +{ + if (class_element->Attribute("type")) { + return class_element->Attribute("type"); + } + return ""; +} + +std::string DisplayFactory::lookupClassId( + const tinyxml2::XMLElement * class_element, const std::string & derived_class) const +{ + if (class_element->Attribute("name")) { + RVIZ_COMMON_LOG_DEBUG_STREAM( + "XML file specifies lookup name (i.e. magic name) = " << class_element->Attribute("name")); + return class_element->Attribute("name"); + } else { + RVIZ_COMMON_LOG_DEBUG_STREAM( + "XML file has no lookup name (i.e. magic name) for class " << derived_class << + ", assuming class_id == real class name."); + return derived_class; + } +} + } // namespace rviz_common diff --git a/rviz_common/src/rviz_common/display_factory.hpp b/rviz_common/src/rviz_common/display_factory.hpp index dfaa16ebb..30693000a 100644 --- a/rviz_common/src/rviz_common/display_factory.hpp +++ b/rviz_common/src/rviz_common/display_factory.hpp @@ -31,9 +31,11 @@ #ifndef RVIZ_COMMON__DISPLAY_FACTORY_HPP_ #define RVIZ_COMMON__DISPLAY_FACTORY_HPP_ -#include -#include -#include +#include + +#include // NOLINT: cpplint cannot handle include order here +#include // NOLINT: cpplint cannot handle include order here +#include // NOLINT: cpplint cannot handle include order here #include "rviz_common/factory/pluginlib_factory.hpp" #include "rviz_common/display.hpp" @@ -54,6 +56,16 @@ class DisplayFactory : public PluginlibFactory Display * makeRaw(const QString & class_id, QString * error_return = nullptr) override; QMap> message_type_cache_; + +private: + bool hasRootNode(tinyxml2::XMLElement * root_element, const std::string & xml_file); + bool hasLibraryRoot(tinyxml2::XMLElement * root_element, const std::string & xml_file); + void fillCacheForAllClassElements(tinyxml2::XMLElement * library); + QSet parseMessageTypes( + tinyxml2::XMLElement * class_element, const std::string & current_class_id) const; + std::string lookupClassId( + const tinyxml2::XMLElement * class_element, const std::string & derived_class) const; + std::string lookupDerivedClass(const tinyxml2::XMLElement * class_element) const; }; } // namespace rviz_common diff --git a/rviz_common/test/display_factory_test.cpp b/rviz_common/test/display_factory_test.cpp new file mode 100644 index 000000000..5ae0c209a --- /dev/null +++ b/rviz_common/test/display_factory_test.cpp @@ -0,0 +1,86 @@ +/* + * Copyright (c) 2019, Martin Idel + * 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 +#include + +#include // NOLINT: cpplint cannot handle include order here + +#include "../src/rviz_common/display_factory.hpp" + +using namespace ::testing; // NOLINT use namespace testing to not clutter tests + +class TestDisplayFactory : public rviz_common::DisplayFactory +{ +public: + explicit TestDisplayFactory(QString path_suffix = "/test/resources/plugins_description.xml") + : rviz_common::DisplayFactory(), path_suffix_(std::move(path_suffix)) + {} + + // overwrite getPluginManifestPath to get our own test resource. This method is called in the + // real method we want to test which uses tinyxml to parse the plugin manifest. + QString getPluginManifestPath(const QString & class_id) const override + { + (void) class_id; + const auto path_prefix = QString(_TEST_PLUGIN_DESCRIPTIONS); + return path_prefix + path_suffix_; + } + +private: + QString path_suffix_; +}; + +TEST(TestDisplayFactory, getMessageTypes_finds_sensor_msgs_for_fake_camera_display) { + auto display_factory = std::make_unique(); + const QSet message_types = display_factory->getMessageTypes("rviz_common_test/Camera"); + ASSERT_THAT(message_types, Contains("sensor_msgs/msg/Image")); + ASSERT_THAT(message_types, Contains("sensor_msgs/msg/CompressedImage")); +} + +TEST(TestDisplayFactory, getMessageTypes_finds_no_messages_for_fake_grid_display) { + auto display_factory = std::make_unique(); + const QSet message_types = display_factory->getMessageTypes("rviz_common_test/Grid"); + ASSERT_THAT(message_types, IsEmpty()); +} + +TEST(TestDisplayFactory, getMessageTypes_finds_no_messages_for_missing_display) { + auto display_factory = std::make_unique(); + const QSet message_types = display_factory->getMessageTypes( + "rviz_common_test/MissingDisplay"); + ASSERT_THAT(message_types, IsEmpty()); +} + +TEST(TestDisplayFactory, getMessageTypes_finds_no_messages_for_xml_with_wrong_root) { + auto display_factory = + std::make_unique("/test/resources/broken_plugins.xml"); + const QSet message_types = display_factory->getMessageTypes("rviz_common_test/Camera"); + ASSERT_THAT(message_types, IsEmpty()); +} diff --git a/rviz_common/test/resources/broken_plugins.xml b/rviz_common/test/resources/broken_plugins.xml new file mode 100644 index 000000000..bcb7cc4a9 --- /dev/null +++ b/rviz_common/test/resources/broken_plugins.xml @@ -0,0 +1,13 @@ + + + + Displays an image from a camera, with the visualized world rendered behind it. <a href="http://www.ros.org/wiki/rviz/DisplayTypes/Camera">More Information</a>. + + sensor_msgs/msg/Image + sensor_msgs/msg/CompressedImage + + diff --git a/rviz_common/test/resources/plugins_description.xml b/rviz_common/test/resources/plugins_description.xml new file mode 100644 index 000000000..82cb29499 --- /dev/null +++ b/rviz_common/test/resources/plugins_description.xml @@ -0,0 +1,23 @@ + + + + Displays an image from a camera, with the visualized world rendered behind it. <a href="http://www.ros.org/wiki/rviz/DisplayTypes/Camera">More Information</a>. + + sensor_msgs/msg/Image + sensor_msgs/msg/CompressedImage + + + + + Displays a grid along the ground plane, centered at the origin of the target frame of reference. + + + diff --git a/rviz_default_plugins/package.xml b/rviz_default_plugins/package.xml index e21524dfa..94d8a2836 100644 --- a/rviz_default_plugins/package.xml +++ b/rviz_default_plugins/package.xml @@ -41,7 +41,6 @@ tf2 tf2_geometry_msgs tf2_ros - tinyxml_vendor urdf visualization_msgs