diff --git a/README.md b/README.md index 3d8b8a5a1..4b8febf7d 100644 --- a/README.md +++ b/README.md @@ -58,8 +58,24 @@ In case you wished to see those features in RViz for ROS 2, feel free to add a p Make sure to read the developer guide below and the migration guide. ### New features +#### Pluggable transformation library -None, yet. +In RViz for ROS 1 the frames transformation library used is tf2 (detailed information about it can be found [here](http://wiki.ros.org/tf2)). +In RViz for ROS 2 the frames transformation library is now pluggable, meaning that different transformation library plugins can be loaded and changed dynamically in the gui. +Developers can create and use their own plugins to provide custom transformation behavior. + +Two plugins are bundled with RViz: +- a plugin for tf2 (`TFFrameTransformer`, in `rviz_default_plugins`), which provides the standard tf2 functionality and which is used as a default +- a trivial plugin (`IdentityFrameTransformer`, in `rviz_common`), which always performs identity transforms. + This plugin is used by default if the tf2 plugin is not available and no other valid plugin is specified. + +As anticipated, in order for the user to choose the plugin to use, RViz provides a dedicated panel: the `Transformation` panel. + +Note: Not all transformation plugins are necessarily compatible with all RViz displays (e.g. some of the default displays, like the TF display, can only work with tf2). +In order to take this possibility into account, the `TransformerGuard` class is provided. +Adding it to a display ensures that the display will be disabled and won't function in case the wrong transformer is used. + +More detailed information on how to write a transformation plugin and on how to handle transformation specific displays can be found in the [plugin development guide](docs/plugin_development.md). ## Developer Guide @@ -72,7 +88,7 @@ https://github.com/ros2/ros2/wiki/Installation #### Building RViz in a separate workspace -When developing for RViz, it can be beneficial to build it in a separate workspace. +When developing for RViz, it can be beneficial to build it in a separate workspace. **Note:** When building the current ros2 branch from source, the latest ROS 2 release for all dependencies might not be sufficient: it could be necessary to build the ROS 2 master branch. Make sure to have a source build of ROS 2 available (see installation procedure above). @@ -122,6 +138,7 @@ Plugins can extend RViz at different extension points: - Displays - Panels - Tools +- Frames transformation library - View Controllers More information on writing plugins can be found in the [plugin development guide](docs/plugin_development.md). diff --git a/docs/plugin_development.md b/docs/plugin_development.md index 242862ce1..faba06642 100644 --- a/docs/plugin_development.md +++ b/docs/plugin_development.md @@ -9,12 +9,13 @@ This is intended as a guide for people wishing to develop custom plugins. Plugins can extend RViz at different extension points: -| plugin type | base type | -| --------------- | ----------------------------- | -| Display | `rviz_common::Display` | -| Panel | `rviz_common::Panel` | -| Tool | `rviz_common::Tool` | -| View Controller | `rviz_common::ViewController` | +| plugin type | base type | +| ----------------------------- | ----------------------------------------------- | +| Display | `rviz_common::Display` | +| Panel | `rviz_common::Panel` | +| Tool | `rviz_common::Tool` | +| Frames transformation library | `rviz_common::transformation::FrameTransformer` | +| View Controller | `rviz_common::ViewController` | Every plugin must be of the corresponding base type in order to be recognized by RViz. Refer to the documentation of the relevant base class for a detailed API description. @@ -22,8 +23,8 @@ Refer to the documentation of the relevant base class for a detailed API descrip ### Plugin Basics * In order to write your own plugin, set up a regular ROS 2 workspace (see the [official documentation]("https://github.com/ros2/ros2/wiki/Colcon-Tutorial") for further help). -* You will need to link your program against `rviz_common` and probably also against `rviz_rendering` and `rviz_default_plugins`. -* To let the plugin loader find your plugin, you will need to invoke the `PLUGINLIB_EXPORT_CLASS` macro. +* You will need to link your program against `rviz_common` and probably also against `rviz_rendering` and `rviz_default_plugins`. +* To let the plugin loader find your plugin, you will need to invoke the `PLUGINLIB_EXPORT_CLASS` macro. For instance, if your plugin is called "myplugin::Plugin" with base class `rviz_common::Dispay`, you will need to include the following into your `.cpp` file: ``` #include @@ -36,7 +37,7 @@ target_compile_definitions(rviz_default_plugins PUBLIC "PLUGINLIB__DISABLE_BOOST # Causes the visibility macros to use dllexport rather than dllimport (for Windows, when your plugin should be used as library) target_compile_definitions(rviz_default_plugins PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") ``` -* You need to write a `plugin_description.xml` file which contains the information necessary for the pluginlib to load your plugin at runtime. +* You need to write a `plugin_description.xml` file which contains the information necessary for the pluginlib to load your plugin at runtime. See `rviz_default_plugins/plugins_description.xml` for an example (the syntax is the same as for the old RViz) * Export the plugin description file via ``` @@ -50,7 +51,7 @@ This should make sure that your plugins are found at runtime #### Writing a display without ros subscription -When writing a display without subscription to ros, derive from `rviz_common::Display`. +When writing a display without subscription to ros, derive from `rviz_common::Display`. * The Display class provides an `Ogre::SceneNode`, which can be used to add visual objects. * It provides convenience methods to set and delete Status messages @@ -58,7 +59,7 @@ When writing a display without subscription to ros, derive from `rviz_common::Di #### Writing a display with subscription -When writing a display for a topic type, derive from `rviz_common::RosTopicDisplay`. +When writing a display for a topic type, derive from `rviz_common::RosTopicDisplay`. The RosTopicDisplay is templated on the message type of this display: * RosTopicDisplay derives from Display and thereby contains all of the functionality described in the previous section @@ -68,7 +69,7 @@ The RosTopicDisplay is templated on the message type of this display: #### Providing Ogre media files -It is possible to add your own meshes, scripts, etc. to RViz. +It is possible to add your own meshes, scripts, etc. to RViz. To make them available at runtime, use the cmake macro `register_rviz_ogre_media_exports` defined in `rviz_rendering` For instance, if you want to register a folder `test_folder/scripts` which resides in your project's source folder, write ``` @@ -76,6 +77,27 @@ register_rviz_ogre_media_exports(DIRECTORIES "test_folder/scripts") ``` *Note:* If you want to export folder hierarchies, each folder needs to be exported separately. Folders within exported folders are not automatically available. +#### Writing a display which can only work with one transformation plugin + +Some of the default displays (e.g. TF display or LaserScan display) can only work with tf2 as transformation framework. +Similarly, as a developer, you may have written your own transformation plugin which cannot work with every display, or your own display which cannot work with every transformation plugin. +The class `rviz_default_plugins::transformation::TransformerGuard` helps you to handle these possibilities. + +As explained below, every transformation plugin implements the base class `rviz_common::transformation::FrameTransformer`. +The `TransformerGuard` class is templated on the type of this implementation and will make sure that the display by which it is owned will be disabled if the transformer currently in use is of a different type. + +When writing a display that is supposed to be working only with a specific transformation plugin, you can add a templated instance of `TransformerGuard` as a display member. +The constructor of this class takes two argument: a raw pointer to the display that owns it, and the name of the transformer it is supposed to work with (needed to correctly set the error status). +Ideally the `TransformerGuard` object is initialized in the constructor of the display. + +In order to make the `TransformerGuard` object work correctly, you need to call the method `TransformerGuard::initialize(rviz_common::DisplayContext * context)` inside the display `onInitialize()` method, passing as parameter the `context_` member of the display. +Doing this will make sure that every time the current transformer plugin is of the wrong type, the display will be disabled and an appropriate error message will be shown in the display status. + +In addition to this, the `TransformerGuard` class also provides the `checkTransformer()` method, which returns `true` if the currently usd transformer is of the allowed type, and `false` otherwise. +This method can be used by the display every time you want it to behave differently according to what kind of transformation plugin is used at the moment. + +For a concrete example of how all this is done, you can have a look, for example, at the TF or the LaserScan display, in `rviz_default_plugins` which, as said, only work with tf2. + ### Writing a panel plugin To write a custom panel, derive from `rviz_common::Panel`. @@ -89,10 +111,14 @@ To write a custom panel, derive from `rviz_common::Panel`. ### Writing a view controller plugin -* In order to write a custom view controller which should be independent of tf frames, derive from `rviz_common::ViewController` +* In order to write a custom view controller which should be independent of tf frames, derive from `rviz_common::ViewController` * If your view controller should be able to track tf frames in a scene, derive from `rviz_common::FramePositionTrackingViewController`, which already contains convenience functionality for tracking target frames * If your custom view controller orbits a focal point, it might also be beneficial to derive from `rviz_default_plugins::OrbitViewController` +### Writing a transformation library plugin + +- To write a transformer plugin, you must implement the `rviz_common::transformation::FrameTransformer` class (refer to the API documentation contained in the header file). +- If for your plugin you also need extra functionality, or you want to offer direct access to some parts of the library itself, you can also implement a subclass of `rviz_common::transformation::TransformerLibraryConnector` (see, for example, `rviz_default_plugins::transformation::TFWrapper`). ## Overview of RViz API @@ -101,7 +127,7 @@ To write a custom panel, derive from `rviz_common::Panel`. The `rviz_rendering` package is supposed to contain all functionality referring to rendering: * Visuals and objects to be added to the scene graph such as arrows, shapes or text objects in the subfolder `objects` (many of those objects were ported from the folder `ogre_helpers`) -* The render window, including functions exposing some of its internals (RenderWindowOgreAdapter). +* The render window, including functions exposing some of its internals (RenderWindowOgreAdapter). If possible, refrain from using the RenderWindowOgreAdapter, as it might be deprecated and deleted in the future * Convenience functions to work with materials (`material_manager.hpp`) or other ogre related functionality (e.g. ray tracing in `viewport_projection_finder.hpp`) * Convenience classes for testing, allowing to set up a working ogre environment and helpers for scene graph introspection @@ -114,20 +140,20 @@ The `rviz_common` package contains the bulk of RViz useful for doing plugin deve * Main entry points for plugin development - base classes for panels, view controllers, displays and tools * Convenience classes to work with properties in the various views (such as the display panel) located in `rviz_common/properties` * Main classes for selectable types, the SelectionHandlers (located in `rviz_common/interaction`) -* Access points to ROS 2. - Currently, RViz uses only one node, which can be accessed via `ros_integration`. +* Access points to ROS 2. + Currently, RViz uses only one node, which can be accessed via `ros_integration`. In the future, further changes may be necessary to fully abstract ROS 2 access into `ros_integration` ### rviz_default_plugins -The `rviz_default_plugins` contains all plugins (view controllers, tools, displays and in the future, panels) shipped with RViz (most of them ported from the `default_plugins` folder of RViz). +The `rviz_default_plugins` contains all plugins (view controllers, tools, displays and in the future, panels) shipped with RViz (most of them ported from the `default_plugins` folder of RViz). * When developing simple plugins it is not necessary to use anything in this package. * When developing more complex plugins similar to existing plugins it might be beneficial to use or even derive from classes contained in this package to simplify your development. ### rviz_visual_testing_framework -The `rviz_visual_testing_framework` contains the backbone to writing visual tests for plugins. +The `rviz_visual_testing_framework` contains the backbone to writing visual tests for plugins. It will only ever be necessary to use this package as a test dependency if you want to write automated screenshot tests. Please see the documentation in the package for further help. diff --git a/rviz2/package.xml b/rviz2/package.xml index 5e981e757..0a725f996 100644 --- a/rviz2/package.xml +++ b/rviz2/package.xml @@ -17,12 +17,12 @@ https://github.com/ros2/rviz/issues ament_cmake - + qtbase5-dev rviz_default_plugins - + rviz_common rviz_ogre_vendor diff --git a/rviz_common/CMakeLists.txt b/rviz_common/CMakeLists.txt index e96bcd99e..a8308bdca 100644 --- a/rviz_common/CMakeLists.txt +++ b/rviz_common/CMakeLists.txt @@ -88,6 +88,7 @@ set(rviz_common_headers_to_moc src/rviz_common/displays_panel.hpp src/rviz_common/failed_panel.hpp src/rviz_common/frame_manager.hpp + include/rviz_common/transformation/frame_transformer.hpp include/rviz_common/frame_manager_iface.hpp src/rviz_common/loading_dialog.hpp src/rviz_common/new_object_dialog.hpp @@ -104,6 +105,8 @@ set(rviz_common_headers_to_moc include/rviz_common/properties/editable_enum_property.hpp include/rviz_common/properties/float_edit.hpp include/rviz_common/properties/float_property.hpp + include/rviz_common/properties/grouped_checkbox_property.hpp + include/rviz_common/properties/grouped_checkbox_property_group.hpp include/rviz_common/properties/int_property.hpp include/rviz_common/properties/line_edit_with_button.hpp include/rviz_common/properties/property.hpp @@ -125,9 +128,13 @@ set(rviz_common_headers_to_moc include/rviz_common/interaction/selection_renderer.hpp src/rviz_common/splash_screen.hpp # src/rviz_common/time_panel.hpp + src/rviz_common/transformation/identity_frame_transformer.hpp + include/rviz_common/transformation/structs.hpp include/rviz_common/tool.hpp src/rviz_common/tool_manager.hpp src/rviz_common/tool_properties_panel.hpp + include/rviz_common/transformation/transformation_manager.hpp + src/rviz_common/transformation_panel.hpp include/rviz_common/view_controller.hpp include/rviz_common/view_manager.hpp src/rviz_common/views_panel.hpp @@ -176,6 +183,8 @@ set(rviz_common_source_files src/rviz_common/properties/file_picker_property.cpp src/rviz_common/properties/float_edit.cpp src/rviz_common/properties/float_property.cpp + src/rviz_common/properties/grouped_checkbox_property.cpp + src/rviz_common/properties/grouped_checkbox_property_group.cpp src/rviz_common/properties/int_property.cpp src/rviz_common/properties/line_edit_with_button.cpp src/rviz_common/properties/parse_color.cpp @@ -206,9 +215,14 @@ set(rviz_common_source_files src/rviz_common/interaction/view_picker.cpp src/rviz_common/splash_screen.cpp # src/rviz_common/time_panel.cpp + src/rviz_common/transformation/identity_frame_transformer.cpp + src/rviz_common/transformation/structs.cpp + src/rviz_common/transformation/ros_helpers/ros_conversion_helpers.cpp src/rviz_common/tool_manager.cpp src/rviz_common/tool_properties_panel.cpp src/rviz_common/tool.cpp + src/rviz_common/transformation_panel.cpp + src/rviz_common/transformation/transformation_manager.cpp src/rviz_common/uniform_string_stream.cpp src/rviz_common/view_controller.cpp src/rviz_common/view_manager.cpp @@ -326,8 +340,21 @@ if(BUILD_TESTING) ament_lint_cmake() ament_uncrustify() + ament_add_gmock(frame_manager_test + test/frame_manager_test.cpp + src/rviz_common/frame_manager.cpp + ) + if(TARGET frame_manager_test) + target_link_libraries(frame_manager_test + rviz_common + rviz_ogre_vendor::OgreMain + rviz_ogre_vendor::OgreOverlay + ) + endif() + ament_add_gtest(rviz_common_config_test - test/config_test.cpp) + test/config_test.cpp + ) if(TARGET rviz_common_config_test) target_link_libraries(rviz_common_config_test rviz_common @@ -337,7 +364,8 @@ if(BUILD_TESTING) endif() ament_add_gtest(rviz_common_uniform_string_stream_test - test/uniform_string_stream_test.cpp) + test/uniform_string_stream_test.cpp + ) if(TARGET rviz_common_uniform_string_stream_test) target_link_libraries(rviz_common_uniform_string_stream_test rviz_common @@ -348,23 +376,38 @@ if(BUILD_TESTING) ament_add_gtest(rviz_common_property_test test/property_test.cpp - test/mock_property_change_receiver.cpp) + test/mock_property_change_receiver.cpp + ) if(TARGET rviz_common_property_test) target_link_libraries(rviz_common_property_test rviz_common) endif() - ament_add_gmock(rviz_common_visualizer_app_test - test/visualizer_app_test.cpp) + ament_add_gmock(rviz_common_visualizer_app_test test/visualizer_app_test.cpp) if(TARGET rviz_common_visualizer_app_test) target_link_libraries(rviz_common_visualizer_app_test rviz_common) endif() ament_add_gmock(rviz_common_ros_node_abstraction_test - test/ros_node_abstraction_test.cpp) + test/ros_node_abstraction_test.cpp + ) if(TARGET rviz_common_ros_node_abstraction_test) target_link_libraries(rviz_common_ros_node_abstraction_test rviz_common) endif() + ament_add_gmock(identity_transformer_test + test/transformation/identity_frame_transformer_test.cpp + ) + if(TARGET identity_transformer_test) + target_link_libraries(identity_transformer_test rviz_common) + endif() + + ament_add_gmock(ros_conversion_helpers_test + test/transformation/ros_conversion_helpers_test.cpp + ) + if(TARGET ros_conversion_helpers_test) + target_link_libraries(ros_conversion_helpers_test rviz_common) + endif() + ament_add_gmock(selection_manager_test test/interaction/selection_manager_test.cpp test/interaction/mock_selection_renderer.hpp @@ -375,7 +418,8 @@ if(BUILD_TESTING) target_link_libraries(selection_manager_test rviz_common rviz_ogre_vendor::OgreMain - rviz_ogre_vendor::OgreOverlay) + rviz_ogre_vendor::OgreOverlay + ) endif() ament_add_gmock(selection_handler_test @@ -387,7 +431,8 @@ if(BUILD_TESTING) target_link_libraries(selection_handler_test rviz_common rviz_ogre_vendor::OgreMain - rviz_ogre_vendor::OgreOverlay) + rviz_ogre_vendor::OgreOverlay + ) endif() ament_add_gtest(rviz_common_display_test @@ -396,11 +441,8 @@ if(BUILD_TESTING) test/mock_display_group.cpp ${SKIP_DISPLAY_TESTS}) if(TARGET rviz_common_display_test) - target_link_libraries(rviz_common_display_test - rviz_common - Qt5::Widgets) - ament_target_dependencies(rviz_common_display_test - rviz_yaml_cpp_vendor) + target_link_libraries(rviz_common_display_test rviz_common Qt5::Widgets) + ament_target_dependencies(rviz_common_display_test rviz_yaml_cpp_vendor) endif() endif() diff --git a/rviz_common/icons/classes/Transformation.png b/rviz_common/icons/classes/Transformation.png new file mode 100644 index 000000000..e5a44f553 Binary files /dev/null and b/rviz_common/icons/classes/Transformation.png differ diff --git a/rviz_common/include/rviz_common/display_context.hpp b/rviz_common/include/rviz_common/display_context.hpp index b475f9101..8dd6459f3 100644 --- a/rviz_common/include/rviz_common/display_context.hpp +++ b/rviz_common/include/rviz_common/display_context.hpp @@ -70,6 +70,13 @@ class ViewPickerIface; } // namespace interaction +namespace transformation +{ + +class TransformationManager; + +} // namespace transformation + class BitAllocator; class DisplayFactory; class DisplayGroup; @@ -181,6 +188,10 @@ class RVIZ_COMMON_PUBLIC DisplayContext : public QObject ViewManager * getViewManager() const = 0; + virtual + transformation::TransformationManager * + getTransformationManager() = 0; + /// Return the root DisplayGroup. virtual DisplayGroup * diff --git a/rviz_common/include/rviz_common/factory/class_id_recording_factory.hpp b/rviz_common/include/rviz_common/factory/class_id_recording_factory.hpp index 3ab3eda15..fcb9acf37 100644 --- a/rviz_common/include/rviz_common/factory/class_id_recording_factory.hpp +++ b/rviz_common/include/rviz_common/factory/class_id_recording_factory.hpp @@ -63,7 +63,7 @@ class ClassIdRecordingFactory : public Factory Type * obj = makeRaw(class_id, error_return); if (obj != nullptr) { obj->setClassId(class_id); - obj->setDescription(getClassDescription(class_id)); + obj->setDescription(getPluginInfo(class_id).description); } return obj; } diff --git a/rviz_common/include/rviz_common/factory/factory.hpp b/rviz_common/include/rviz_common/factory/factory.hpp index 18e865352..9af59b8bf 100644 --- a/rviz_common/include/rviz_common/factory/factory.hpp +++ b/rviz_common/include/rviz_common/factory/factory.hpp @@ -1,6 +1,7 @@ /* * Copyright (c) 2012, Willow Garage, Inc. * Copyright (c) 2017, Open Source Robotics Foundation, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -31,13 +32,36 @@ #ifndef RVIZ_COMMON__FACTORY__FACTORY_HPP_ #define RVIZ_COMMON__FACTORY__FACTORY_HPP_ -#include -#include -#include +#include +#include + +#include // NOLINT +#include // NOLINT +#include // NOLINT namespace rviz_common { +/// Struct to bundle the information available for a plugin +struct PluginInfo +{ + QString id; + QString name; + QString package; + QString description; + QIcon icon; + + friend bool operator==(const PluginInfo & lhs, const PluginInfo & rhs) + { + return lhs.id == rhs.id; + } + + friend bool operator<(const PluginInfo & lhs, const PluginInfo & rhs) + { + return lhs.id < rhs.id; + } +}; + /// Abstract base class representing a plugin load-able class factory. /** * The class represents the ability to get a list of class IDs and the ability @@ -50,11 +74,8 @@ class Factory public: virtual ~Factory() {} - virtual QStringList getDeclaredClassIds() = 0; - virtual QString getClassDescription(const QString & class_id) const = 0; - virtual QString getClassName(const QString & class_id) const = 0; - virtual QString getClassPackage(const QString & class_id) const = 0; - virtual QIcon getIcon(const QString & class_id) const = 0; + virtual std::vector getDeclaredPlugins() = 0; + virtual PluginInfo getPluginInfo(const QString & class_id) const = 0; }; } // namespace rviz_common diff --git a/rviz_common/include/rviz_common/factory/pluginlib_factory.hpp b/rviz_common/include/rviz_common/factory/pluginlib_factory.hpp index 9063e1c0c..7c6841ebd 100644 --- a/rviz_common/include/rviz_common/factory/pluginlib_factory.hpp +++ b/rviz_common/include/rviz_common/factory/pluginlib_factory.hpp @@ -31,20 +31,21 @@ #ifndef RVIZ_COMMON__FACTORY__PLUGINLIB_FACTORY_HPP_ #define RVIZ_COMMON__FACTORY__PLUGINLIB_FACTORY_HPP_ -#include -#include -#include - #include #include +#include // NOLINT +#include // NOLINT +#include // NOLINT + #ifndef Q_MOC_RUN -#include +#include "pluginlib/class_loader.hpp" #endif -#include "rviz_common/logging.hpp" -#include "./class_id_recording_factory.hpp" +#include "rviz_common/factory/factory.hpp" +#include "rviz_common/factory/class_id_recording_factory.hpp" #include "rviz_common/load_resource.hpp" +#include "rviz_common/logging.hpp" namespace rviz_common { @@ -74,45 +75,39 @@ class PluginlibFactory : public ClassIdRecordingFactory delete class_loader_; } - QStringList getDeclaredClassIds() override + std::vector getDeclaredPlugins() override { - QStringList ids; + std::vector plugins; std::vector std_ids = class_loader_->getDeclaredClasses(); - for (size_t i = 0; i < std_ids.size(); i++) { - ids.push_back(QString::fromStdString(std_ids[i])); + for (const auto & id : std_ids) { + plugins.emplace_back(getPluginInfo(QString::fromStdString(id))); } typename QHash::const_iterator iter; - for (iter = built_ins_.begin(); iter != built_ins_.end(); iter++) { - ids.push_back(iter.key()); - } - return ids; - } - - QString getClassDescription(const QString & class_id) const override - { - typename QHash::const_iterator iter = built_ins_.find(class_id); - if (iter != built_ins_.end()) { - return iter->description_; + for (iter = built_ins_.begin(); iter != built_ins_.end(); ++iter) { + plugins.emplace_back(getPluginInfo(iter->class_id_)); } - return QString::fromStdString(class_loader_->getClassDescription(class_id.toStdString())); + return plugins; } - QString getClassName(const QString & class_id) const override + PluginInfo getPluginInfo(const QString & class_id) const override { + PluginInfo info; typename QHash::const_iterator iter = built_ins_.find(class_id); if (iter != built_ins_.end()) { - return iter->name_; + info.id = iter->class_id_; + info.name = iter->name_; + info.package = iter->package_; + info.description = iter->description_; + info.icon = getIcon(info); + return info; } - return QString::fromStdString(class_loader_->getName(class_id.toStdString())); - } - - QString getClassPackage(const QString & class_id) const override - { - typename QHash::const_iterator iter = built_ins_.find(class_id); - if (iter != built_ins_.end()) { - return iter->package_; - } - return QString::fromStdString(class_loader_->getClassPackage(class_id.toStdString())); + auto class_id_std = class_id.toStdString(); + info.id = class_id; + info.name = QString::fromStdString(class_loader_->getName(class_id_std)); + info.package = QString::fromStdString(class_loader_->getClassPackage(class_id_std)); + info.description = QString::fromStdString(class_loader_->getClassDescription(class_id_std)); + info.icon = getIcon(info); + return info; } virtual QString getPluginManifestPath(const QString & class_id) const @@ -124,20 +119,6 @@ class PluginlibFactory : public ClassIdRecordingFactory return QString::fromStdString(class_loader_->getPluginManifestPath(class_id.toStdString())); } - QIcon getIcon(const QString & class_id) const override - { - QString package = getClassPackage(class_id); - QString class_name = getClassName(class_id); - QIcon icon = loadPixmap("package://" + package + "/icons/classes/" + class_name + ".svg"); - if (icon.isNull()) { - icon = loadPixmap("package://" + package + "/icons/classes/" + class_name + ".png"); - if (icon.isNull()) { - icon = loadPixmap("package://rviz_common/icons/default_class_icon.png"); - } - } - return icon; - } - virtual void addBuiltInClass( const QString & package, const QString & name, const QString & description, std::function factory_function) @@ -187,6 +168,25 @@ class PluginlibFactory : public ClassIdRecordingFactory } } + virtual QIcon getIcon(const PluginInfo & info) const + { + auto default_icon_path = "package://rviz_common/icons/default_class_icon.png"; + + if (info.package.isEmpty() || info.name.isEmpty()) { + return loadPixmap(default_icon_path); + } + + auto base_path = "package://" + info.package + "/icons/classes/" + info.name; + QIcon icon = loadPixmap(base_path + ".svg"); + if (icon.isNull()) { + icon = loadPixmap(base_path + ".png"); + if (icon.isNull()) { + icon = loadPixmap(default_icon_path); + } + } + return icon; + } + private: pluginlib::ClassLoader * class_loader_; QHash built_ins_; diff --git a/rviz_common/include/rviz_common/frame_manager_iface.hpp b/rviz_common/include/rviz_common/frame_manager_iface.hpp index 20bd9d020..ee8a2ee8e 100644 --- a/rviz_common/include/rviz_common/frame_manager_iface.hpp +++ b/rviz_common/include/rviz_common/frame_manager_iface.hpp @@ -35,10 +35,20 @@ #include #include #include +#include + +#ifdef __clang__ +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wkeyword-macro" +#endif #include #include +#ifdef __clang__ +#pragma clang diagnostic pop +#endif + #include // NOLINT: cpplint is unable to handle the include order here #include "geometry_msgs/msg/pose.hpp" @@ -49,6 +59,7 @@ // #include "tf2_ros/message_filter.h" #include "rviz_common/visibility_control.hpp" +#include "rviz_common/transformation/frame_transformer.hpp" namespace tf2_ros { @@ -85,25 +96,39 @@ class RVIZ_COMMON_PUBLIC FrameManagerIface : public QObject * The fixed frame serves as the reference for all getTransform() and * transform() functions in FrameManager. */ - virtual void setFixedFrame(const std::string & frame) = 0; + virtual + void + setFixedFrame(const std::string & frame) = 0; /// Enable/disable pause mode. - virtual void setPause(bool pause) = 0; + virtual + void + setPause(bool pause) = 0; /// Get pause state. - virtual bool getPause() = 0; + virtual + bool + getPause() = 0; /// Set synchronization mode (off/exact/approximate). - virtual void setSyncMode(SyncMode mode) = 0; + virtual + void + setSyncMode(SyncMode mode) = 0; /// Get the synchronization mode. - virtual SyncMode getSyncMode() = 0; + virtual + SyncMode + getSyncMode() = 0; /// Synchronize with given time. - virtual void syncTime(rclcpp::Time time) = 0; + virtual + void + syncTime(rclcpp::Time time) = 0; /// Get current time, depending on the sync mode. - virtual rclcpp::Time getTime() = 0; + virtual + rclcpp::Time + getTime() = 0; /// Return the pose for a header, relative to the fixed frame, in Ogre classes. /** @@ -115,8 +140,11 @@ class RVIZ_COMMON_PUBLIC FrameManagerIface : public QObject * \return true on success, false on failure. */ template - bool getTransform( - const Header & header, Ogre::Vector3 & position, Ogre::Quaternion & orientation) + bool + getTransform( + const Header & header, + Ogre::Vector3 & position, + Ogre::Quaternion & orientation) { return getTransform(header.frame_id, header.stamp, position, orientation); } @@ -129,8 +157,12 @@ class RVIZ_COMMON_PUBLIC FrameManagerIface : public QObject * fixed frame. * \return true on success, false on failure. */ - virtual bool getTransform( - const std::string & frame, Ogre::Vector3 & position, Ogre::Quaternion & orientation) = 0; + virtual + bool + getTransform( + const std::string & frame, + Ogre::Vector3 & position, + Ogre::Quaternion & orientation) = 0; /// Return the pose for a frame relative to the fixed frame, in Ogre classes, at a given time. /** @@ -141,7 +173,9 @@ class RVIZ_COMMON_PUBLIC FrameManagerIface : public QObject * fixed frame. * \return true on success, false on failure. */ - virtual bool getTransform( + virtual + bool + getTransform( const std::string & frame, rclcpp::Time time, Ogre::Vector3 & position, @@ -157,7 +191,8 @@ class RVIZ_COMMON_PUBLIC FrameManagerIface : public QObject * \return true on success, false on failure. */ template - bool transform( + bool + transform( const Header & header, const geometry_msgs::msg::Pose & pose, Ogre::Vector3 & position, @@ -177,7 +212,9 @@ class RVIZ_COMMON_PUBLIC FrameManagerIface : public QObject * frame. * \return true on success, false on failure. */ - virtual bool transform( + virtual + bool + transform( const std::string & frame, rclcpp::Time time, const geometry_msgs::msg::Pose & pose, @@ -185,7 +222,9 @@ class RVIZ_COMMON_PUBLIC FrameManagerIface : public QObject Ogre::Quaternion & orientation) = 0; /// Clear the internal cache. - virtual void update() = 0; + virtual + void + update() = 0; /// Check to see if a frame exists in the tf::TransformListener. /** @@ -194,14 +233,18 @@ class RVIZ_COMMON_PUBLIC FrameManagerIface : public QObject * stored here. * \return true if the frame does not exist, false if it does exist. */ - virtual bool frameHasProblems(const std::string & frame, std::string & error) = 0; + virtual + bool + frameHasProblems(const std::string & frame, std::string & error) = 0; /// Check to see if a transform is known between a given frame and the fixed frame. /** * \param[in] frame The name of the frame to check. * \param[out] error If the transform is not known, an error message is stored here. * \return true if the transform is not known, false if it is. */ - virtual bool transformHasProblems(const std::string & frame, std::string & error) = 0; + virtual + bool + transformHasProblems(const std::string & frame, std::string & error) = 0; /// Check to see if a transform is known between a given frame and the fixed frame. /** @@ -209,8 +252,9 @@ class RVIZ_COMMON_PUBLIC FrameManagerIface : public QObject * \param[in] time The time at which the transform is desired. * \param[out] error If the transform is not known, an error message is stored here. * \return true if the transform is not known, false if it is. */ - virtual bool transformHasProblems( - const std::string & frame, rclcpp::Time time, std::string & error) = 0; + virtual + bool + transformHasProblems(const std::string & frame, rclcpp::Time time, std::string & error) = 0; #if 0 /// Connect a tf::MessageFilter's callbacks to success and failure handler functions. @@ -224,8 +268,10 @@ class RVIZ_COMMON_PUBLIC FrameManagerIface : public QObject * \param display The Display using the filter. */ template - void registerFilterForTransformStatusCheck( - tf2_ros::MessageFilter * filter, Display * display) override + void + registerFilterForTransformStatusCheck( + tf2_ros::MessageFilter * filter, + Display * display) override { filter->registerCallback(boost::bind(&FrameManager::messageCallback, this, _1, display)); filter->registerFailureCallback(boost::bind(&FrameManager::failureCallback, this, _1, _2, @@ -234,16 +280,19 @@ class RVIZ_COMMON_PUBLIC FrameManagerIface : public QObject #endif /// Return the current fixed frame name. - virtual const std::string & getFixedFrame() = 0; + virtual + const std::string & + getFixedFrame() = 0; - /// Return the tf::TransformListener used to receive transform data. - virtual tf2_ros::TransformListener * getTFClient() = 0; + /// Return a weak pointer to the internal transformation object. + virtual + transformation::TransformationLibraryConnector::WeakPtr + getConnector() = 0; - /// Return a shared pointer to the tf2_ros::TransformListener used to receive transform data. - virtual const std::shared_ptr & getTFClientPtr() = 0; - - /// Return a shared pointer to the tf2_ros::Buffer object. - virtual const std::shared_ptr & getTFBufferPtr() = 0; + /// Return a shared pointer to the transformer object. + virtual + std::shared_ptr + getTransformer() = 0; // TODO(wjwwood): figure out how to replace FilgerFailureReason here #if 0 @@ -257,15 +306,27 @@ class RVIZ_COMMON_PUBLIC FrameManagerIface : public QObject * * Once a problem has been detected with a given frame or transform, * call this to get an error message describing the problem. */ - virtual std::string discoverFailureReason( + virtual + std::string + discoverFailureReason( const std::string & frame_id, const rclcpp::Time & stamp, const std::string & caller_id, tf::FilterFailureReason reason) = 0; #endif + virtual + std::vector + getAllFrameNames() = 0; + +public Q_SLOTS: + virtual + void + setTransformerPlugin(std::shared_ptr transformer) = 0; + Q_SIGNALS: - void fixedFrameChanged(); + void + fixedFrameChanged(); }; } // namespace rviz_common diff --git a/rviz_common/include/rviz_common/properties/grouped_checkbox_property.hpp b/rviz_common/include/rviz_common/properties/grouped_checkbox_property.hpp new file mode 100644 index 000000000..3800100e8 --- /dev/null +++ b/rviz_common/include/rviz_common/properties/grouped_checkbox_property.hpp @@ -0,0 +1,90 @@ +/* + * 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_COMMON__PROPERTIES__GROUPED_CHECKBOX_PROPERTY_HPP_ +#define RVIZ_COMMON__PROPERTIES__GROUPED_CHECKBOX_PROPERTY_HPP_ + +#include + +#include "rviz_common/properties/bool_property.hpp" + +namespace rviz_common +{ +namespace properties +{ +class GroupedCheckboxPropertyGroup; + +/// Checkbox property that is grouped together with other checkbox properties. +/** + * Behaves like radio buttons (exclusive selection, no manual deselect). + */ +class GroupedCheckboxProperty : public BoolProperty +{ +public: + explicit + GroupedCheckboxProperty( + std::shared_ptr group, + const QString & name = QString(), + bool default_value = false, + const QString & description = QString(), + Property * parent = nullptr, + const char * changed_slot = nullptr, + QObject * receiver = nullptr); + + /// An override for Property::setValue that is a noop. + /** + * To set the value correctly, use setBoolValue. + * + * This is a workaround for the issue where sometimes, clicking on the + * GroupedCheckboxPropertyGroup does not send the clicked() event of + * QTreeView but only the clicked() event of this property, resulting + * in weird behaviour. + * + * \param new_value IGNORED, radio buttons can generally not be deselected + * manually. + */ + bool + setValue(const QVariant & new_value) override; + + /// Set the value of the underlying property without triggering the group. + bool + setRawValue(const QVariant & new_value); + + /// Notify the group so that all other GroupedCheckboxProperty instances are disabled. + void + checkPropertyInGroup(); + +private: + std::shared_ptr group_; +}; + +} // namespace properties +} // namespace rviz_common + +#endif // RVIZ_COMMON__PROPERTIES__GROUPED_CHECKBOX_PROPERTY_HPP_ diff --git a/rviz_common/include/rviz_common/properties/grouped_checkbox_property_group.hpp b/rviz_common/include/rviz_common/properties/grouped_checkbox_property_group.hpp new file mode 100644 index 000000000..94dddd31e --- /dev/null +++ b/rviz_common/include/rviz_common/properties/grouped_checkbox_property_group.hpp @@ -0,0 +1,62 @@ +/* + * 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_COMMON__PROPERTIES__GROUPED_CHECKBOX_PROPERTY_GROUP_HPP_ +#define RVIZ_COMMON__PROPERTIES__GROUPED_CHECKBOX_PROPERTY_GROUP_HPP_ + +#include + +#include "rviz_common/properties/grouped_checkbox_property.hpp" + +namespace rviz_common +{ +namespace properties +{ + +/// Groups multiple GroupedCheckboxProperties together. +class GroupedCheckboxPropertyGroup +{ +public: + void + addProperty(GroupedCheckboxProperty * property); + + GroupedCheckboxProperty * + getChecked(); + + void + setChecked(GroupedCheckboxProperty * property_to_check); + +private: + std::vector properties_; +}; + +} // namespace properties +} // namespace rviz_common + +#endif // RVIZ_COMMON__PROPERTIES__GROUPED_CHECKBOX_PROPERTY_GROUP_HPP_ diff --git a/rviz_common/include/rviz_common/transformation/frame_transformer.hpp b/rviz_common/include/rviz_common/transformation/frame_transformer.hpp new file mode 100644 index 000000000..57cd429b9 --- /dev/null +++ b/rviz_common/include/rviz_common/transformation/frame_transformer.hpp @@ -0,0 +1,212 @@ +/* + * 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. + */ + +#ifndef RVIZ_COMMON__TRANSFORMATION__FRAME_TRANSFORMER_HPP_ +#define RVIZ_COMMON__TRANSFORMATION__FRAME_TRANSFORMER_HPP_ + +#include +#include +#include + +#include // NOLINT + +#include "rviz_common/ros_integration/ros_node_abstraction.hpp" +#include "rviz_common/visibility_control.hpp" + +#include "rviz_common/transformation/structs.hpp" + +namespace rviz_common +{ +namespace transformation +{ + +class FrameTransformerException : public std::runtime_error +{ +public: + RVIZ_COMMON_PUBLIC + explicit FrameTransformerException(const char * error_message) + : std::runtime_error(error_message) {} + + RVIZ_COMMON_PUBLIC + ~FrameTransformerException() noexcept override = default; +}; + +/// Class from which the plugin specific implementation of FrameTransformer should inherit. +class RVIZ_COMMON_PUBLIC TransformationLibraryConnector +{ +public: + virtual ~TransformationLibraryConnector() = default; + + using WeakPtr = std::weak_ptr; +}; + + +class RVIZ_COMMON_PUBLIC FrameTransformer +{ +public: + virtual ~FrameTransformer() = default; + + /// The pluginlib needs a no-parameters constructor. + /** + * The initialization of a FrameTransformer object is therefore delegated + * to this method. + */ + virtual + void + initialize( + ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node, + rclcpp::Clock::SharedPtr clock) = 0; + + /// Method thought to reset the internal implementation object. + virtual + void + clear() = 0; + + /** \returns A std::string vector containing all the available frame ids */ + virtual std::vector getAllFrameNames() = 0; + + /// Transform a PoseStamped into a given target frame. + /** + * \param pose_in The pose to be transformed + * \param target_frame The frame into which to transform + * \returns The transformed PoseStamped + */ + virtual + transformation::PoseStamped + transform( + // NOLINT (this is not std::transform) + const transformation::PoseStamped & pose_in, + const std::string & target_frame) = 0; + + /// Checks if a transformation between two frames is available. + /** + * \param target_frame The target frame of the transformation + * \param source_frame The source frame of the transformation + * \returns True if a transformation between the two frames is available + */ + virtual + bool + transformIsAvailable( + const std::string & target_frame, + const std::string & source_frame) = 0; + + /// Checks that a given transformation can be performed. + /** + * \param target_frame The target frame of the transformation + * \param source_frame The source frame of the transformation + * \param time The time of the transformation + * \param error An out string in which an error (of generated) message is saved + * \returns True if the given transformation has some problem and cannot be performed + */ + virtual + bool + transformHasProblems( + const std::string & source_frame, + const std::string & target_frame, + const rclcpp::Time & time, + std::string & error) = 0; + + /// Checks that a given frame exists and can be used. + /** + * \param frame The frame to check + * \param error An out string in which an error message (if generated) is saved + * \returns True if the given frame has some problem + */ + virtual + bool + frameHasProblems(const std::string & frame, std::string & error) = 0; + + /// A getter for the internal implementation object. + virtual + TransformationLibraryConnector::WeakPtr + getConnector() = 0; + + // TODO(botteroa-si): This method can be needed when having displays working with tf_filter. + // Reenable once tf_filter is ported. +#if 0 + /// Waits until a transformation between two given frames is available, then performs an action. + /** + * \param target_frame The target frame of the transformation + * \param source_frame The source frame of the transformation + * \param time The time of the transformation + * \param timeout A timeout duration after which the waiting will be stopped + * \param callback The function to be called if the transform becomes available before the + * timeout + */ + virtual + void + waitForValidTransform( + std::string target_frame, + std::string source_frame, + rclcpp::Time time, + rclcpp::Duration timeout, + std::function callback) = 0; +#endif + + /// Return the class id set by the PluginlibFactory. + virtual + QString + getClassId() const + { + return class_id_; + } + + /// Used by PluginlibFactory to store the class id. + virtual + void + setClassId(const QString & class_id) + { + class_id_ = class_id; + } + + /// Return the description set by the PluginlibFactory. + virtual + QString + getDescription() const + { + return description_; + } + + /// Used by PluginlibFactory to store the description. + virtual + void + setDescription(const QString & description) + { + description_ = description; + } + +private: + QString class_id_; + QString description_; +}; + +} // namespace transformation +} // namespace rviz_common + +#endif // RVIZ_COMMON__TRANSFORMATION__FRAME_TRANSFORMER_HPP_ diff --git a/rviz_common/include/rviz_common/transformation/ros_helpers/ros_conversion_helpers.hpp b/rviz_common/include/rviz_common/transformation/ros_helpers/ros_conversion_helpers.hpp new file mode 100644 index 000000000..f8f7c2033 --- /dev/null +++ b/rviz_common/include/rviz_common/transformation/ros_helpers/ros_conversion_helpers.hpp @@ -0,0 +1,105 @@ +/* + * 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. + */ + +#ifndef RVIZ_COMMON__TRANSFORMATION__ROS_HELPERS__ROS_CONVERSION_HELPERS_HPP_ +#define RVIZ_COMMON__TRANSFORMATION__ROS_HELPERS__ROS_CONVERSION_HELPERS_HPP_ + +#include + +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "std_msgs/msg/header.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include "rviz_common/visibility_control.hpp" + +namespace rviz_common +{ +namespace transformation +{ +struct Header; +struct Time; +struct Point; +struct Quaternion; +struct PoseStamped; +struct TransformStamped; + +namespace ros_helpers +{ + +RVIZ_COMMON_PUBLIC +rviz_common::transformation::Point +fromRosPoint(geometry_msgs::msg::Point ros_point); + +RVIZ_COMMON_PUBLIC +rviz_common::transformation::Quaternion +fromRosQuaternion( + geometry_msgs::msg::Quaternion ros_quaternion); + +RVIZ_COMMON_PUBLIC +rviz_common::transformation::Point +fromRosVector3(geometry_msgs::msg::Vector3 ros_vector); + +RVIZ_COMMON_PUBLIC +rviz_common::transformation::PoseStamped +fromRosPoseStamped(geometry_msgs::msg::PoseStamped ros_pose); + +RVIZ_COMMON_PUBLIC +rviz_common::transformation::TransformStamped +fromRosTransformStamped(geometry_msgs::msg::TransformStamped ros_transform); + +RVIZ_COMMON_PUBLIC +std_msgs::msg::Header +toRosHeader(rviz_common::transformation::Time time_stamp, const std::string & frame_id); + +RVIZ_COMMON_PUBLIC +geometry_msgs::msg::Point +toRosPoint(rviz_common::transformation::Point point); + +RVIZ_COMMON_PUBLIC +geometry_msgs::msg::Vector3 +toRosVector3(rviz_common::transformation::Point point); + +RVIZ_COMMON_PUBLIC +geometry_msgs::msg::Quaternion +toRosQuaternion(rviz_common::transformation::Quaternion ros_quaternion); + +RVIZ_COMMON_PUBLIC +geometry_msgs::msg::PoseStamped +toRosPoseStamped(rviz_common::transformation::PoseStamped pose_stamped); + +} // namespace ros_helpers +} // namespace transformation +} // namespace rviz_common + +#endif // RVIZ_COMMON__TRANSFORMATION__ROS_HELPERS__ROS_CONVERSION_HELPERS_HPP_ diff --git a/rviz_common/include/rviz_common/transformation/structs.hpp b/rviz_common/include/rviz_common/transformation/structs.hpp new file mode 100644 index 000000000..1000af485 --- /dev/null +++ b/rviz_common/include/rviz_common/transformation/structs.hpp @@ -0,0 +1,111 @@ +/* + * 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. + */ + +#ifndef RVIZ_COMMON__TRANSFORMATION__STRUCTS_HPP_ +#define RVIZ_COMMON__TRANSFORMATION__STRUCTS_HPP_ + +#include + +#include "rviz_common/visibility_control.hpp" + +namespace rviz_common +{ +namespace transformation +{ + +struct RVIZ_COMMON_PUBLIC Time +{ + Time(); + Time(int32_t sec, uint32_t nanosec); + + int32_t seconds; + uint32_t nanoseconds; +}; + +struct RVIZ_COMMON_PUBLIC Point +{ + Point(); + Point(double x_co, double y_co, double z_co); + + double x; + double y; + double z; +}; + +struct RVIZ_COMMON_PUBLIC Quaternion +{ + Quaternion(); + Quaternion(double w_co, double x_co, double y_co, double z_co); + + double w; + double x; + double y; + double z; +}; + +struct PoseStamped +{ +public: + RVIZ_COMMON_PUBLIC + PoseStamped(); + + RVIZ_COMMON_PUBLIC + PoseStamped(Time time, std::string frame, Point position_vector, Quaternion orientation_quat); + + Time time_stamp; + std::string frame_id; + Point position; + Quaternion orientation; +}; + +struct TransformStamped +{ +public: + RVIZ_COMMON_PUBLIC + TransformStamped(); + + RVIZ_COMMON_PUBLIC + TransformStamped( + Time time, + std::string parent_frame, + std::string child_frame, + Point translation_vector, + Quaternion rotation_quat); + + Time time_stamp; + std::string parent_frame_id; + std::string child_frame_id; + Point translation; + Quaternion rotation; +}; + +} // namespace transformation +} // namespace rviz_common + +#endif // RVIZ_COMMON__TRANSFORMATION__STRUCTS_HPP_ diff --git a/rviz_common/include/rviz_common/transformation/transformation_manager.hpp b/rviz_common/include/rviz_common/transformation/transformation_manager.hpp new file mode 100644 index 000000000..26a5150b8 --- /dev/null +++ b/rviz_common/include/rviz_common/transformation/transformation_manager.hpp @@ -0,0 +1,105 @@ +/* + * 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_COMMON__TRANSFORMATION__TRANSFORMATION_MANAGER_HPP_ +#define RVIZ_COMMON__TRANSFORMATION__TRANSFORMATION_MANAGER_HPP_ + +#include +#include +#include + +#include // NOLINT +#include // NOLINT + +#include "rviz_common/config.hpp" +#include "rviz_common/factory/factory.hpp" +#include "rviz_common/factory/pluginlib_factory.hpp" +#include "rviz_common/transformation/frame_transformer.hpp" +#include "rviz_common/visibility_control.hpp" + + +namespace rviz_common +{ +namespace transformation +{ + +class RVIZ_COMMON_PUBLIC TransformationManager : public QObject +{ + Q_OBJECT + +public: + explicit TransformationManager( + ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node, + rclcpp::Clock::SharedPtr clock); + + /// Load configuration from a Config object. + void + load(const Config & config); + + /// Save configuration to a Config object. + void + save(Config config) const; + + std::vector + getAvailableTransformers() const; + + std::shared_ptr + getCurrentTransformer() const; + + PluginInfo + getCurrentTransformerInfo() const; + + void + setTransformer(const PluginInfo & plugin_info); + +Q_SIGNALS: + /// Emitted when the current transformer changes. + void + configChanged(); + + /// Emitted when the current transformer changes. + void + transformerChanged( + std::shared_ptr new_transformer); + +private: + std::unique_ptr> factory_; + + std::shared_ptr current_transformer_; + + ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node_; + rclcpp::Clock::SharedPtr clock_; +}; + +} // namespace transformation +} // namespace rviz_common + +#endif // RVIZ_COMMON__TRANSFORMATION__TRANSFORMATION_MANAGER_HPP_ diff --git a/rviz_common/src/rviz_common/add_display_dialog.cpp b/rviz_common/src/rviz_common/add_display_dialog.cpp index e8c31e5b1..40514e748 100644 --- a/rviz_common/src/rviz_common/add_display_dialog.cpp +++ b/rviz_common/src/rviz_common/add_display_dialog.cpp @@ -117,7 +117,7 @@ bool isSubtopic(const std::string & base, const std::string & topic) std::string query = topic; // Both checks are required, otherwise the loop does not terminate when adding 'by topic' - while (query != "" && query != "/") { + while (!query.empty() && query != "/") { if (query == base) { return true; } @@ -148,7 +148,7 @@ void getPluginGroups( std::map> topic_names_and_types = rviz_ros_node.lock()->get_topic_names_and_types(); - for (const auto map_pair : topic_names_and_types) { + for (const auto & map_pair : topic_names_and_types) { QString topic = QString::fromStdString(map_pair.first); if (map_pair.second.empty()) { throw std::runtime_error("topic '" + map_pair.first + "' unexpectedly has not types."); @@ -161,12 +161,12 @@ void getPluginGroups( for (const auto & topic_type_name : map_pair.second) { ss << " '" << topic_type_name << "'"; } - RVIZ_COMMON_LOG_WARNING(ss.str().c_str()); + RVIZ_COMMON_LOG_WARNING(ss.str()); } QString datatype = QString::fromStdString(map_pair.second[0]); if (datatype_plugins.contains(datatype)) { - if (groups->size() == 0 || + if (groups->empty() || !isSubtopic(groups->back().base_topic.toStdString(), topic.toStdString())) { @@ -182,10 +182,7 @@ void getPluginGroups( topic_suffix = topic.right(topic.size() - group.base_topic.size() - 1); } - const QList & plugin_names = - datatype_plugins.values(datatype); - for (int i = 0; i < plugin_names.size(); ++i) { - const QString & name = plugin_names[i]; + for (const auto & name : datatype_plugins.values(datatype)) { PluginGroup::Info & info = group.plugins[name]; info.topic_suffixes.append(topic_suffix); info.datatypes.append(datatype); @@ -220,18 +217,18 @@ AddDisplayDialog::AddDisplayDialog( setObjectName("AddDisplayDialog"); // Display Type group - QGroupBox * type_box = new QGroupBox("Create visualization"); + auto type_box = new QGroupBox("Create visualization"); type_box->setObjectName("AddDisplayDialog/Visualization_Typebox"); - QLabel * description_label = new QLabel("Description:"); + auto description_label = new QLabel("Description:"); description_ = new QTextBrowser; description_->setMaximumHeight(100); description_->setOpenExternalLinks(true); - DisplayTypeTree * display_tree = new DisplayTypeTree; + auto display_tree = new DisplayTypeTree; display_tree->fillTree(factory); - TopicDisplayWidget * topic_widget = new TopicDisplayWidget(rviz_ros_node); + auto topic_widget = new TopicDisplayWidget(rviz_ros_node); topic_widget->fill(factory); tab_widget_ = new QTabWidget; @@ -239,7 +236,7 @@ AddDisplayDialog::AddDisplayDialog( display_tab_ = tab_widget_->addTab(display_tree, tr("By display type")); topic_tab_ = tab_widget_->addTab(topic_widget, tr("By topic")); - QVBoxLayout * type_layout = new QVBoxLayout; + auto type_layout = new QVBoxLayout; type_layout->addWidget(tab_widget_); type_layout->addWidget(description_label); type_layout->addWidget(description_); @@ -251,7 +248,7 @@ AddDisplayDialog::AddDisplayDialog( if (display_name_output_) { name_box = new QGroupBox("Display Name"); name_editor_ = new QLineEdit; - QVBoxLayout * name_layout = new QVBoxLayout; + auto name_layout = new QVBoxLayout; name_layout->addWidget(name_editor_); name_box->setLayout(name_layout); } @@ -261,7 +258,7 @@ AddDisplayDialog::AddDisplayDialog( new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel, Qt::Horizontal); button_box_->setObjectName("AddDisplayDialog/ButtonBox"); - QVBoxLayout * main_layout = new QVBoxLayout; + auto main_layout = new QVBoxLayout; main_layout->addWidget(type_box); if (display_name_output_) { main_layout->addWidget(name_box); @@ -297,7 +294,7 @@ AddDisplayDialog::AddDisplayDialog( QSize AddDisplayDialog::sizeHint() const { - return QSize(500, 660); + return {500, 660}; } void AddDisplayDialog::onTabChanged(int index) @@ -320,7 +317,7 @@ void AddDisplayDialog::onTopicSelected(SelectionData * data) void AddDisplayDialog::updateDisplay() { - SelectionData * data = NULL; + SelectionData * data = nullptr; if (tab_widget_->currentIndex() == topic_tab_) { data = &topic_data_; } else if (tab_widget_->currentIndex() == display_tab_) { @@ -404,7 +401,7 @@ void DisplayTypeTree::onCurrentItemChanged( Q_UNUSED(prev); // If display is selected, populate selection data. Otherwise, clear data. SelectionData sd; - if (curr->parent() != NULL) { + if (curr->parent() != nullptr) { // Leave topic and datatype blank sd.whats_this = curr->whatsThis(0); sd.lookup_name = curr->data(0, Qt::UserRole).toString(); @@ -417,40 +414,34 @@ void DisplayTypeTree::fillTree(Factory * factory) { QIcon default_package_icon = loadPixmap("package://rviz_common/icons/default_package_icon.png"); - QStringList classes = factory->getDeclaredClassIds(); - classes.sort(); + auto plugins = factory->getDeclaredPlugins(); + std::sort(plugins.begin(), plugins.end()); // Map from package names to the corresponding top-level tree widget items. std::map package_items; - for (int i = 0; i < classes.size(); i++) { - QString lookup_name = classes[i]; - QString package = factory->getClassPackage(lookup_name); - QString description = factory->getClassDescription(lookup_name); - QString name = factory->getClassName(lookup_name); - + for (const auto & plugin : plugins) { QTreeWidgetItem * package_item; - std::map::iterator mi; - mi = package_items.find(package); - if (mi == package_items.end()) { + auto package_item_entry = package_items.find(plugin.package); + if (package_item_entry == package_items.end()) { package_item = new QTreeWidgetItem(this); - package_item->setText(0, package); + package_item->setText(0, plugin.package); package_item->setIcon(0, default_package_icon); package_item->setExpanded(true); - package_items[package] = package_item; + package_items[plugin.package] = package_item; } else { - package_item = (*mi).second; + package_item = (*package_item_entry).second; } - QTreeWidgetItem * class_item = new QTreeWidgetItem(package_item); + auto class_item = new QTreeWidgetItem(package_item); - class_item->setIcon(0, factory->getIcon(lookup_name)); + class_item->setIcon(0, plugin.icon); - class_item->setText(0, name); - class_item->setWhatsThis(0, description); + class_item->setText(0, plugin.name); + class_item->setWhatsThis(0, plugin.description); // Store the lookup name for each class in the UserRole of the item. - class_item->setData(0, Qt::UserRole, lookup_name); + class_item->setData(0, Qt::UserRole, plugin.id); } } @@ -472,7 +463,7 @@ TopicDisplayWidget::TopicDisplayWidget( enable_hidden_box_ = new QCheckBox("Show unvisualizable topics"); enable_hidden_box_->setCheckState(Qt::Unchecked); - QVBoxLayout * layout = new QVBoxLayout; + auto layout = new QVBoxLayout; layout->setContentsMargins(QMargins(0, 0, 0, 0)); layout->addWidget(tree_); @@ -505,8 +496,8 @@ void TopicDisplayWidget::onCurrentItemChanged(QTreeWidgetItem * curr) sd.lookup_name = curr->data(0, Qt::UserRole).toString(); sd.display_name = curr->text(0); - QComboBox * combo = qobject_cast(tree_->itemWidget(curr, 1)); - if (combo != NULL) { + auto combo = qobject_cast(tree_->itemWidget(curr, 1)); + if (combo) { QString combo_text = combo->currentText(); if (combo_text != "raw") { sd.topic += "/" + combo_text; @@ -547,24 +538,26 @@ void TopicDisplayWidget::fill(DisplayFactory * factory) for (pg_it = groups.begin(); pg_it < groups.end(); ++pg_it) { const PluginGroup & pg = *pg_it; - QTreeWidgetItem * item = insertItem(pg.base_topic, false); + auto item = insertItem(pg.base_topic, false); item->setData(0, Qt::UserRole, pg.base_topic); QMap::const_iterator it; for (it = pg.plugins.begin(); it != pg.plugins.end(); ++it) { - const QString plugin_name = it.key(); + const QString & plugin_id = it.key(); const PluginGroup::Info & info = it.value(); - QTreeWidgetItem * row = new QTreeWidgetItem(item); + auto row = new QTreeWidgetItem(item); + + auto plugin_info = factory->getPluginInfo(plugin_id); - row->setText(0, factory->getClassName(plugin_name)); - row->setIcon(0, factory->getIcon(plugin_name)); - row->setWhatsThis(0, factory->getClassDescription(plugin_name)); - row->setData(0, Qt::UserRole, plugin_name); + row->setText(0, plugin_info.name); + row->setIcon(0, plugin_info.icon); + row->setWhatsThis(0, plugin_info.description); + row->setData(0, Qt::UserRole, plugin_id); row->setData(1, Qt::UserRole, info.datatypes[0]); // *INDENT-OFF* - uncrustify cannot deal with commas here if (info.topic_suffixes.size() > 1) { - EmbeddableComboBox * box = new EmbeddableComboBox(row, 1); + auto box = new EmbeddableComboBox(row, 1); connect(box, SIGNAL(itemClicked(QTreeWidgetItem *, int)), this, SLOT(onComboBoxClicked(QTreeWidgetItem *))); for (int i = 0; i < info.topic_suffixes.size(); ++i) { @@ -589,17 +582,12 @@ void TopicDisplayWidget::fill(DisplayFactory * factory) void TopicDisplayWidget::findPlugins(DisplayFactory * factory) { // Build map from topic type to plugin by instantiating every plugin we have. - QStringList lookup_names = factory->getDeclaredClassIds(); - - QStringList::iterator it; - for (it = lookup_names.begin(); it != lookup_names.end(); ++it) { - const QString & lookup_name = *it; - // ROS_INFO("Class: %s", lookup_name.toStdString().c_str()); + auto plugins = factory->getDeclaredPlugins(); - QSet topic_types = factory->getMessageTypes(lookup_name); + for (const auto & plugin : plugins) { + QSet topic_types = factory->getMessageTypes(plugin.id); Q_FOREACH (QString topic_type, topic_types) { - // ROS_INFO("Type: %s", topic_type.toStdString().c_str()); - datatype_plugins_.insertMulti(topic_type, lookup_name); + datatype_plugins_.insertMulti(topic_type, plugin.id); } } } @@ -625,7 +613,7 @@ QTreeWidgetItem * TopicDisplayWidget::insertItem( } // If no match, create a new child. if (!match) { - QTreeWidgetItem * new_child = new QTreeWidgetItem(current); + auto new_child = new QTreeWidgetItem(current); // Only expand first few levels of the tree new_child->setExpanded(3 > part_ind); new_child->setText(0, part); diff --git a/rviz_common/src/rviz_common/display_factory.cpp b/rviz_common/src/rviz_common/display_factory.cpp index ab486f0a6..348de990d 100644 --- a/rviz_common/src/rviz_common/display_factory.cpp +++ b/rviz_common/src/rviz_common/display_factory.cpp @@ -35,7 +35,7 @@ // TODO(wjwwood): replace with tinyxml2? implicit dependency? #include // NOLINT: cpplint is unable to handle the include order here -#include "include/rviz_common/display_group.hpp" +#include "rviz_common/display_group.hpp" #include "rviz_common/logging.hpp" namespace rviz_common @@ -56,7 +56,7 @@ Display * DisplayFactory::makeRaw(const QString & class_id, QString * error_retu { Display * display = PluginlibFactory::makeRaw(class_id, error_return); if (display) { - display->setIcon(getIcon(class_id)); + display->setIcon(getPluginInfo(class_id).icon); } return display; } diff --git a/rviz_common/src/rviz_common/frame_manager.cpp b/rviz_common/src/rviz_common/frame_manager.cpp index 7f7490cad..6f75b2dd1 100644 --- a/rviz_common/src/rviz_common/frame_manager.cpp +++ b/rviz_common/src/rviz_common/frame_manager.cpp @@ -35,31 +35,26 @@ #include #include #include +#include #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "rclcpp/clock.hpp" #include "std_msgs/msg/float32.hpp" -#include "tf2/buffer_core.h" -#include "tf2/LinearMath/Quaternion.h" -#include "tf2/LinearMath/Vector3.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#include "tf2_ros/transform_listener.h" #include "rviz_common/display.hpp" #include "rviz_common/logging.hpp" #include "rviz_common/msg_conversions.hpp" #include "rviz_common/properties/property.hpp" +#include "rviz_common/transformation/ros_helpers/ros_conversion_helpers.hpp" namespace rviz_common { FrameManager::FrameManager( - std::shared_ptr tf, - std::shared_ptr buffer, - rclcpp::Clock::SharedPtr clock) -: tf_(tf), buffer_(buffer), sync_time_(0), clock_(clock) + rclcpp::Clock::SharedPtr clock, std::shared_ptr transformer) +: transformer_(transformer), sync_time_(0), clock_(clock) { setSyncMode(SyncOff); setPause(false); @@ -173,24 +168,8 @@ bool FrameManager::adjustTime(const std::string & frame, rclcpp::Time & time) case SyncApprox: { // try to get the time from the latest available transformation - try { - auto lastAvailableTransform = - buffer_->lookupTransform(fixed_frame_, frame, tf2::TimePointZero); - if (lastAvailableTransform.header.stamp.nanosec > sync_time_.nanoseconds()) { - time = sync_time_; - } - } catch (const tf2::LookupException & exception) { - RVIZ_COMMON_LOG_ERROR_STREAM(exception.what()); - return false; - } catch (const tf2::ConnectivityException & exception) { - RVIZ_COMMON_LOG_ERROR_STREAM(exception.what()); - return false; - } catch (const tf2::ExtrapolationException & exception) { - RVIZ_COMMON_LOG_ERROR_STREAM(exception.what()); - return false; - } catch (const tf2::InvalidArgumentException & exception) { - RVIZ_COMMON_LOG_ERROR_STREAM(exception.what()); - return false; + if (transformer_->transformIsAvailable(fixed_frame_, frame)) { + time = sync_time_; } } break; @@ -265,7 +244,7 @@ bool FrameManager::transform( pose_in.header.frame_id = pose_in.header.frame_id.substr(1); } pose_in.pose = pose_msg; - geometry_msgs::msg::PoseStamped pose_out; + transformation::PoseStamped out_pose; // TODO(wjwwood): figure out where the `/` is coming from and remove it // also consider warning the user in the GUI about this... @@ -274,20 +253,13 @@ bool FrameManager::transform( stripped_fixed_frame = stripped_fixed_frame.substr(1); } - // convert pose into new frame + geometry_msgs::msg::PoseStamped pose_out; try { - buffer_->transform(pose_in, pose_out, stripped_fixed_frame); - } catch (const tf2::LookupException & exception) { - RVIZ_COMMON_LOG_ERROR_STREAM(exception.what()); - return false; - } catch (const tf2::ConnectivityException & exception) { - RVIZ_COMMON_LOG_ERROR_STREAM(exception.what()); - return false; - } catch (const tf2::ExtrapolationException & exception) { - RVIZ_COMMON_LOG_ERROR_STREAM(exception.what()); - return false; - } catch (const tf2::InvalidArgumentException & exception) { - RVIZ_COMMON_LOG_ERROR_STREAM(exception.what()); + pose_out = transformation::ros_helpers::toRosPoseStamped( + transformer_->transform( + transformation::ros_helpers::fromRosPoseStamped(pose_in), stripped_fixed_frame)); + } catch (const transformation::FrameTransformerException & exception) { + (void) exception; return false; } @@ -296,19 +268,9 @@ bool FrameManager::transform( return true; } -bool FrameManager::frameHasProblems( - const std::string & frame, - std::string & error) +bool FrameManager::frameHasProblems(const std::string & frame, std::string & error) { - if (!buffer_->_frameExists(frame)) { - error = "Frame [" + frame + "] does not exist"; - if (frame == fixed_frame_) { - error = "Fixed " + error; - } - return true; - } - - return false; + return transformer_->frameHasProblems(frame, error); } bool FrameManager::transformHasProblems( @@ -320,31 +282,7 @@ bool FrameManager::transformHasProblems( return false; } - std::string tf_error; - tf2::TimePoint tf2_time(std::chrono::nanoseconds(time.nanoseconds())); - bool transform_succeeded = buffer_->canTransform(fixed_frame_, frame, tf2_time, &tf_error); - if (transform_succeeded) { - return false; - } - - bool ok = true; - ok = ok && !frameHasProblems(fixed_frame_, error); - ok = ok && !frameHasProblems(frame, error); - - if (ok) { - std::stringstream ss; - ss << "No transform to fixed frame [" << fixed_frame_ << "]. TF error: [" << tf_error << "]"; - error = ss.str(); - ok = false; - } - - { - std::stringstream ss; - ss << "For frame [" << frame << "]: " << error; - error = ss.str(); - } - - return !ok; + return transformer_->transformHasProblems(frame, fixed_frame_, time, error); } const std::string & FrameManager::getFixedFrame() @@ -352,16 +290,6 @@ const std::string & FrameManager::getFixedFrame() return fixed_frame_; } -tf2_ros::TransformListener * FrameManager::getTFClient() -{ - return tf_.get(); -} - -const std::shared_ptr & FrameManager::getTFClientPtr() -{ - return tf_; -} - std::string getTransformStatusName(const std::string & caller_id) { std::stringstream ss; @@ -404,6 +332,38 @@ void FrameManager::messageArrived( display->setStatusStd(StatusProperty::Ok, getTransformStatusName(caller_id), "Transform OK"); } +transformation::TransformationLibraryConnector::WeakPtr FrameManager::getConnector() +{ + return transformer_->getConnector(); +} + +std::shared_ptr FrameManager::getTransformer() +{ + return transformer_; +} + +std::vector FrameManager::getAllFrameNames() +{ + return transformer_->getAllFrameNames(); +} + +void FrameManager::clear() +{ + transformer_->clear(); +} + +bool FrameManager::anyTransformationDataAvailable() +{ + auto frames = transformer_->getAllFrameNames(); + return !frames.empty(); +} + +void FrameManager::setTransformerPlugin( + std::shared_ptr transformer) +{ + transformer_ = transformer; +} + #if 0 void FrameManager::messageFailed( const std::string & frame_id, @@ -419,9 +379,4 @@ void FrameManager::messageFailed( } #endif -const std::shared_ptr & FrameManager::getTFBufferPtr() -{ - return buffer_; -} - } // namespace rviz_common diff --git a/rviz_common/src/rviz_common/frame_manager.hpp b/rviz_common/src/rviz_common/frame_manager.hpp index 679244594..2f02f3229 100644 --- a/rviz_common/src/rviz_common/frame_manager.hpp +++ b/rviz_common/src/rviz_common/frame_manager.hpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include @@ -45,19 +46,9 @@ #include "rclcpp/clock.hpp" #include "rclcpp/time.hpp" -// TODO(wjwwood): reenable this when message_filters is ported. -// #include "tf2_ros/message_filter.h" - #include "rviz_common/visibility_control.hpp" #include "rviz_common/frame_manager_iface.hpp" - -namespace tf2_ros -{ - -class Buffer; -class TransformListener; - -} // namespace tf2_ros +#include "rviz_common/transformation/frame_transformer.hpp" namespace rviz_common { @@ -80,10 +71,7 @@ class FrameManager : public FrameManagerIface * else because of thread safety). */ FrameManager( - std::shared_ptr tf, - std::shared_ptr buffer, - rclcpp::Clock::SharedPtr clock - ); + rclcpp::Clock::SharedPtr clock, std::shared_ptr transformer); /// Destructor. /** @@ -198,14 +186,11 @@ class FrameManager : public FrameManagerIface /// Return the current fixed frame name. const std::string & getFixedFrame() override; - /// Return the tf::TransformListener used to receive transform data. - tf2_ros::TransformListener * getTFClient() override; + /// Return a weak pointer to the internal transformation object. + transformation::TransformationLibraryConnector::WeakPtr getConnector() override; - /// Return a shared pointer to the tf2_ros::TransformListener used to receive transform data. - const std::shared_ptr & getTFClientPtr() override; - - /// Return a shared pointer to the tf2_ros::Buffer object. - const std::shared_ptr & getTFBufferPtr() override; + /// Return a shared pointer to the transformer object. + std::shared_ptr getTransformer() override; // TODO(wjwwood): figure out how to replace FilgerFailureReason here #if 0 @@ -226,6 +211,16 @@ class FrameManager : public FrameManagerIface tf::FilterFailureReason reason) override; #endif + std::vector getAllFrameNames() override; + + virtual void clear(); + + virtual bool anyTransformationDataAvailable(); + +public Q_SLOTS: + void setTransformerPlugin( + std::shared_ptr transformer) override; + private: bool adjustTime(const std::string & frame, rclcpp::Time & time); @@ -292,8 +287,7 @@ class FrameManager : public FrameManagerIface std::mutex cache_mutex_; M_Cache cache_; - std::shared_ptr tf_; - std::shared_ptr buffer_; + std::shared_ptr transformer_; std::string fixed_frame_; bool pause_; diff --git a/rviz_common/src/rviz_common/new_object_dialog.cpp b/rviz_common/src/rviz_common/new_object_dialog.cpp index a22598494..2b7448993 100644 --- a/rviz_common/src/rviz_common/new_object_dialog.cpp +++ b/rviz_common/src/rviz_common/new_object_dialog.cpp @@ -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 @@ -29,6 +30,7 @@ #include "./new_object_dialog.hpp" +#include #include #include // NOLINT: cpplint is unable to handle the include order here @@ -63,18 +65,18 @@ NewObjectDialog::NewObjectDialog( //***** Layout // Display Type group - QGroupBox * type_box = new QGroupBox(object_type + " Type"); + auto type_box = new QGroupBox(object_type + " Type"); - QTreeWidget * tree = new QTreeWidget; + auto tree = new QTreeWidget; tree->setHeaderHidden(true); fillTree(tree); - QLabel * description_label = new QLabel("Description:"); + auto description_label = new QLabel("Description:"); description_ = new QTextBrowser; description_->setMaximumHeight(100); description_->setOpenExternalLinks(true); - QVBoxLayout * type_layout = new QVBoxLayout; + auto type_layout = new QVBoxLayout; type_layout->addWidget(tree); type_layout->addWidget(description_label); type_layout->addWidget(description_); @@ -86,7 +88,7 @@ NewObjectDialog::NewObjectDialog( if (display_name_output_) { name_box = new QGroupBox(object_type + " Name"); name_editor_ = new QLineEdit; - QVBoxLayout * name_layout = new QVBoxLayout; + auto name_layout = new QVBoxLayout; name_layout->addWidget(name_editor_); name_box->setLayout(name_layout); } @@ -95,7 +97,7 @@ NewObjectDialog::NewObjectDialog( button_box_ = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel, Qt::Horizontal); - QVBoxLayout * main_layout = new QVBoxLayout; + auto main_layout = new QVBoxLayout; main_layout->addWidget(type_box); if (display_name_output_) { main_layout->addWidget(name_box); @@ -114,8 +116,7 @@ NewObjectDialog::NewObjectDialog( // *INDENT-ON* if (display_name_output_) { - connect(name_editor_, SIGNAL(textEdited(const QString&)), - this, SLOT(onNameChanged())); + connect(name_editor_, SIGNAL(textEdited(const QString&)), this, SLOT(onNameChanged())); } } @@ -128,41 +129,36 @@ void NewObjectDialog::fillTree(QTreeWidget * tree) { QIcon default_package_icon = loadPixmap("package://rviz_common/icons/default_package_icon.png"); - QStringList classes = factory_->getDeclaredClassIds(); - classes.sort(); + auto plugins = factory_->getDeclaredPlugins(); + std::sort(plugins.begin(), plugins.end()); // Map from package names to the corresponding top-level tree widget items. std::map package_items; - for (int i = 0; i < classes.size(); i++) { - QString lookup_name = classes[i]; - QString package = factory_->getClassPackage(lookup_name); - QString description = factory_->getClassDescription(lookup_name); - QString name = factory_->getClassName(lookup_name); - + for (const auto & plugin : plugins) { QTreeWidgetItem * package_item; std::map::iterator mi; - mi = package_items.find(package); + mi = package_items.find(plugin.package); if (mi == package_items.end()) { package_item = new QTreeWidgetItem(tree); - package_item->setText(0, package); + package_item->setText(0, plugin.package); package_item->setIcon(0, default_package_icon); package_item->setExpanded(true); - package_items[package] = package_item; + package_items[plugin.package] = package_item; } else { package_item = (*mi).second; } - QTreeWidgetItem * class_item = new QTreeWidgetItem(package_item); + auto class_item = new QTreeWidgetItem(package_item); - class_item->setIcon(0, factory_->getIcon(lookup_name)); + class_item->setIcon(0, plugin.icon); - class_item->setText(0, name); - class_item->setWhatsThis(0, description); + class_item->setText(0, plugin.name); + class_item->setWhatsThis(0, plugin.description); // Store the lookup name for each class in the UserRole of the item. - class_item->setData(0, Qt::UserRole, lookup_name); - class_item->setDisabled(disallowed_class_lookup_names_.contains(lookup_name)); + class_item->setData(0, Qt::UserRole, plugin.id); + class_item->setDisabled(disallowed_class_lookup_names_.contains(plugin.id)); } } diff --git a/rviz_common/src/rviz_common/panel_factory.cpp b/rviz_common/src/rviz_common/panel_factory.cpp index 508d120f5..98b3179fa 100644 --- a/rviz_common/src/rviz_common/panel_factory.cpp +++ b/rviz_common/src/rviz_common/panel_factory.cpp @@ -39,6 +39,7 @@ #include "./selection_panel.hpp" // #include "./time_panel.hpp" #include "./tool_properties_panel.hpp" +#include "./transformation_panel.hpp" #include "./views_panel.hpp" #include "./visualization_manager.hpp" #include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" @@ -50,6 +51,7 @@ static Panel * newHelpPanel() {return new HelpPanel();} static Panel * newSelectionPanel() {return new SelectionPanel();} // static Panel * newTimePanel() {return new TimePanel();} static Panel * newToolPropertiesPanel() {return new ToolPropertiesPanel();} +static Panel * newTransformationPanel() {return new TransformationPanel();} static Panel * newViewsPanel() {return new ViewsPanel();} PanelFactory::PanelFactory( @@ -70,6 +72,8 @@ PanelFactory::PanelFactory( // "Show the current time", &newTimePanel); addBuiltInClass("rviz_common", "Tool Properties", "Show and edit properties of tools", &newToolPropertiesPanel); + addBuiltInClass("rviz_common", "Transformation", + "Choose the transformation plugin", &newTransformationPanel); addBuiltInClass("rviz_common", "Views", "Show and edit viewpoints", &newViewsPanel); } diff --git a/rviz_common/src/rviz_common/properties/grouped_checkbox_property.cpp b/rviz_common/src/rviz_common/properties/grouped_checkbox_property.cpp new file mode 100644 index 000000000..018a1e003 --- /dev/null +++ b/rviz_common/src/rviz_common/properties/grouped_checkbox_property.cpp @@ -0,0 +1,71 @@ +/* + * 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_common/properties/grouped_checkbox_property.hpp" +#include "rviz_common/properties/grouped_checkbox_property_group.hpp" + +namespace rviz_common +{ +namespace properties +{ + +GroupedCheckboxProperty::GroupedCheckboxProperty( + std::shared_ptr group, + const QString & name, + bool default_value, + const QString & description, + Property * parent, + const char * changed_slot, + QObject * receiver +) +: BoolProperty(name, default_value, description, parent, changed_slot, receiver), group_(group) +{ + group->addProperty(this); +} + +bool GroupedCheckboxProperty::setValue(const QVariant & new_value) +{ + Q_UNUSED(new_value); + return true; +} + +bool GroupedCheckboxProperty::setRawValue(const QVariant & new_value) +{ + return Property::setValue(new_value); +} + +void GroupedCheckboxProperty::checkPropertyInGroup() +{ + group_->setChecked(this); +} + +} // namespace properties +} // namespace rviz_common diff --git a/rviz_common/src/rviz_common/properties/grouped_checkbox_property_group.cpp b/rviz_common/src/rviz_common/properties/grouped_checkbox_property_group.cpp new file mode 100644 index 000000000..8a1480fb2 --- /dev/null +++ b/rviz_common/src/rviz_common/properties/grouped_checkbox_property_group.cpp @@ -0,0 +1,63 @@ +/* + * 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 "rviz_common/properties/grouped_checkbox_property.hpp" +#include "rviz_common/properties/grouped_checkbox_property_group.hpp" + +namespace rviz_common +{ +namespace properties +{ + +void GroupedCheckboxPropertyGroup::addProperty(GroupedCheckboxProperty * property) +{ + properties_.push_back(property); +} + +void GroupedCheckboxPropertyGroup::setChecked(GroupedCheckboxProperty * property_to_check) +{ + for (const auto & property : properties_) { + if (property) { + property->setRawValue(property == property_to_check); + } + } +} + +GroupedCheckboxProperty * GroupedCheckboxPropertyGroup::getChecked() +{ + for (const auto & property : properties_) { + if (property && property->getValue().toBool()) { + return property; + } + } + return nullptr; +} + +} // namespace properties +} // namespace rviz_common diff --git a/rviz_common/src/rviz_common/properties/tf_frame_property.cpp b/rviz_common/src/rviz_common/properties/tf_frame_property.cpp index 357eb6929..68bd210fb 100644 --- a/rviz_common/src/rviz_common/properties/tf_frame_property.cpp +++ b/rviz_common/src/rviz_common/properties/tf_frame_property.cpp @@ -55,7 +55,7 @@ TfFrameProperty::TfFrameProperty( const char * changed_slot, QObject * receiver) : EditableEnumProperty(name, default_value, description, parent, changed_slot, receiver), - frame_manager_(NULL), + frame_manager_(nullptr), include_fixed_frame_string_(include_fixed_frame_string) { // Parent class EditableEnumProperty has requestOptions() signal. @@ -95,8 +95,7 @@ FrameManagerIface * TfFrameProperty::getFrameManager() const void TfFrameProperty::fillFrameList() { - std::vector std_frames; - frame_manager_->getTFBufferPtr()->_getFrameStrings(std_frames); + std::vector std_frames = frame_manager_->getAllFrameNames(); std::sort(std_frames.begin(), std_frames.end() ); clearOptions(); diff --git a/rviz_common/src/rviz_common/tool_manager.cpp b/rviz_common/src/rviz_common/tool_manager.cpp index 489ad60dd..ccb4f7b64 100644 --- a/rviz_common/src/rviz_common/tool_manager.cpp +++ b/rviz_common/src/rviz_common/tool_manager.cpp @@ -75,7 +75,7 @@ ToolManager::~ToolManager() void ToolManager::initialize() { // get a list of available tool plugin class ids - QStringList class_ids = factory_->getDeclaredClassIds(); + auto plugins = factory_->getDeclaredPlugins(); // define a list of preferred tool names (they will be listed first in the toolbar) std::vector preferred_tool_names = { "rviz_default_plugins/MoveCamera", @@ -86,9 +86,9 @@ void ToolManager::initialize() }; // attempt to load each preferred tool in order for (const auto & preferred_tool_name : preferred_tool_names) { - for (const auto & class_id : class_ids) { - if (class_id.toStdString() == preferred_tool_name) { - addTool(class_id); + for (const auto & plugin : plugins) { + if (plugin.name.toStdString() == preferred_tool_name) { + addTool(plugin); } } } @@ -112,7 +112,7 @@ void ToolManager::load(const Config & config) QString class_id; if (tool_config.mapGetString("Class", &class_id)) { - Tool * tool = addTool(class_id); + Tool * tool = addTool(factory_->getPluginInfo(class_id)); tool->load(tool_config); } } @@ -225,18 +225,23 @@ void ToolManager::closeTool() } Tool * ToolManager::addTool(const QString & class_id) +{ + return addTool(factory_->getPluginInfo(class_id)); +} + +Tool * ToolManager::addTool(const PluginInfo & tool_plugin) { QString error; bool failed = false; - Tool * tool = factory_->make(class_id, &error); + Tool * tool = factory_->make(tool_plugin.id, &error); if (!tool) { - tool = new FailedTool(class_id, error); + tool = new FailedTool(tool_plugin.id, error); failed = true; } tools_.append(tool); - tool->setName(addSpaceToCamelCase(factory_->getClassName(class_id))); - tool->setIcon(factory_->getIcon(class_id)); + tool->setName(addSpaceToCamelCase(tool_plugin.name)); + tool->setIcon(tool_plugin.icon); tool->initialize(context_); if (tool->getShortcutKey() != '\0') { diff --git a/rviz_common/src/rviz_common/tool_manager.hpp b/rviz_common/src/rviz_common/tool_manager.hpp index 496b6c55f..cb1711706 100644 --- a/rviz_common/src/rviz_common/tool_manager.hpp +++ b/rviz_common/src/rviz_common/tool_manager.hpp @@ -76,8 +76,11 @@ class ToolManager : public QObject /// Get the property tree model. rviz_common::properties::PropertyTreeModel * getPropertyModel() const; - /// Create a tool by class lookup name, add it to the list, and return it. - Tool * addTool(const QString & tool_class_lookup_name); + /// Create a tool by class id, add it to the list, and return it. + Tool * addTool(const QString & class_id); + + /// Create a tool by plugin info, add it to the list, and return it. + Tool * addTool(const PluginInfo & tool_plugin); /// Return the tool currently in use. /** diff --git a/rviz_common/src/rviz_common/transformation/identity_frame_transformer.cpp b/rviz_common/src/rviz_common/transformation/identity_frame_transformer.cpp new file mode 100644 index 000000000..e5ff5dc73 --- /dev/null +++ b/rviz_common/src/rviz_common/transformation/identity_frame_transformer.cpp @@ -0,0 +1,112 @@ +/* + * 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 "./identity_frame_transformer.hpp" + +#include +#include +#include + +namespace rviz_common +{ +namespace transformation +{ + +void IdentityFrameTransformer::initialize( + ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node, rclcpp::Clock::SharedPtr clock) +{ + (void) rviz_ros_node; + (void) clock; +} + +void IdentityFrameTransformer::clear() {} + +std::vector IdentityFrameTransformer::getAllFrameNames() +{ + return {""}; +} + +transformation::PoseStamped IdentityFrameTransformer::transform( + const transformation::PoseStamped & pose_in, const std::string & target_frame) +{ + (void) target_frame; + transformation::PoseStamped pose_out = pose_in; + + if (quaternionIsValid(pose_out.orientation)) { + return pose_out; + } + pose_out.orientation.w = 1; + return pose_out; +} + +bool IdentityFrameTransformer::transformIsAvailable( + const std::string & target_frame, const std::string & source_frame) +{ + (void) target_frame; + (void) source_frame; + + return true; +} + +bool IdentityFrameTransformer::transformHasProblems( + const std::string & source_frame, + const std::string & target_frame, + const rclcpp::Time & time, + std::string & error) +{ + (void) source_frame; + (void) target_frame; + (void) time; + (void) error; + + return false; +} + +bool IdentityFrameTransformer::frameHasProblems(const std::string & frame, std::string & error) +{ + (void) frame; + (void) error; + + return false; +} + +TransformationLibraryConnector::WeakPtr IdentityFrameTransformer::getConnector() +{ + return std::weak_ptr(); +} + +bool IdentityFrameTransformer::quaternionIsValid(transformation::Quaternion quaternion) +{ + return quaternion.w + quaternion.x + quaternion.y + quaternion.z != 0; +} + +} // namespace transformation +} // namespace rviz_common diff --git a/rviz_common/src/rviz_common/transformation/identity_frame_transformer.hpp b/rviz_common/src/rviz_common/transformation/identity_frame_transformer.hpp new file mode 100644 index 000000000..8869a496e --- /dev/null +++ b/rviz_common/src/rviz_common/transformation/identity_frame_transformer.hpp @@ -0,0 +1,109 @@ +/* + * 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_COMMON__TRANSFORMATION__IDENTITY_FRAME_TRANSFORMER_HPP_ +#define RVIZ_COMMON__TRANSFORMATION__IDENTITY_FRAME_TRANSFORMER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "rviz_common/transformation/frame_transformer.hpp" +#include "rviz_common/transformation/structs.hpp" +#include "rviz_common/visibility_control.hpp" + +namespace rviz_common +{ +namespace transformation +{ + +/// A trivial subclass of FrameTransformer, that always uses the identity transform. +class RVIZ_COMMON_PUBLIC IdentityFrameTransformer : public FrameTransformer +{ +public: + IdentityFrameTransformer() = default; + ~IdentityFrameTransformer() override = default; + + void + initialize( + ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node, + rclcpp::Clock::SharedPtr clock) override; + + void + clear() override; + + std::vector + getAllFrameNames() override; + + transformation::PoseStamped + transform( + // NOLINT (this is not std::transform) + const transformation::PoseStamped & pose_in, + const std::string & target_frame) override; + + bool + transformIsAvailable( + const std::string & target_frame, + const std::string & source_frame) override; + + bool + transformHasProblems( + const std::string & source_frame, + const std::string & target_frame, + const rclcpp::Time & time, + std::string & error) override; + + bool + frameHasProblems(const std::string & frame, std::string & error) override; + + TransformationLibraryConnector::WeakPtr + getConnector() override; + +#if 0 + void + waitForValidTransform( + std::string target_frame, + std::string source_frame, + rclcpp::Time time, + rclcpp::Duration timeout, + std::function callback) override; +#endif + +private: + bool + quaternionIsValid(transformation::Quaternion quaternion); +}; + +} // namespace transformation +} // namespace rviz_common + +#endif // RVIZ_COMMON__TRANSFORMATION__IDENTITY_FRAME_TRANSFORMER_HPP_ diff --git a/rviz_common/src/rviz_common/transformation/ros_helpers/ros_conversion_helpers.cpp b/rviz_common/src/rviz_common/transformation/ros_helpers/ros_conversion_helpers.cpp new file mode 100644 index 000000000..b4167bf75 --- /dev/null +++ b/rviz_common/src/rviz_common/transformation/ros_helpers/ros_conversion_helpers.cpp @@ -0,0 +1,158 @@ +/* + * 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 "rviz_common/transformation/ros_helpers/ros_conversion_helpers.hpp" + +#include + +#include "rviz_common/transformation/structs.hpp" + +namespace rviz_common +{ +namespace transformation +{ +namespace ros_helpers +{ + +rviz_common::transformation::Point fromRosPoint(geometry_msgs::msg::Point ros_point) +{ + Point point; + point.x = ros_point.x; + point.y = ros_point.y; + point.z = ros_point.z; + + return point; +} + +rviz_common::transformation::Quaternion fromRosQuaternion( + geometry_msgs::msg::Quaternion ros_quaternion) +{ + Quaternion quaternion; + quaternion.w = ros_quaternion.w; + quaternion.x = ros_quaternion.x; + quaternion.y = ros_quaternion.y; + quaternion.z = ros_quaternion.z; + + return quaternion; +} + +rviz_common::transformation::Point fromRosVector3(geometry_msgs::msg::Vector3 ros_vector) +{ + Point point; + point.x = ros_vector.x; + point.y = ros_vector.y; + point.z = ros_vector.z; + + return point; +} + +rviz_common::transformation::PoseStamped fromRosPoseStamped( + geometry_msgs::msg::PoseStamped ros_pose) +{ + rviz_common::transformation::PoseStamped pose_stamped; + pose_stamped.time_stamp.seconds = ros_pose.header.stamp.sec; + pose_stamped.time_stamp.nanoseconds = ros_pose.header.stamp.nanosec; + pose_stamped.frame_id = ros_pose.header.frame_id; + pose_stamped.position = fromRosPoint(ros_pose.pose.position); + pose_stamped.orientation = fromRosQuaternion(ros_pose.pose.orientation); + + return pose_stamped; +} + +rviz_common::transformation::TransformStamped fromRosTransformStamped( + geometry_msgs::msg::TransformStamped ros_transform) +{ + rviz_common::transformation::TransformStamped transform_stamped; + transform_stamped.time_stamp.seconds = ros_transform.header.stamp.sec; + transform_stamped.time_stamp.nanoseconds = ros_transform.header.stamp.nanosec; + transform_stamped.parent_frame_id = ros_transform.header.frame_id; + transform_stamped.child_frame_id = ros_transform.child_frame_id; + transform_stamped.translation = fromRosVector3(ros_transform.transform.translation); + transform_stamped.rotation = fromRosQuaternion(ros_transform.transform.rotation); + + return transform_stamped; +} + +std_msgs::msg::Header toRosHeader( + rviz_common::transformation::Time time_stamp, const std::string & frame_id) +{ + std_msgs::msg::Header ros_header; + ros_header.stamp.sec = time_stamp.seconds; + ros_header.stamp.nanosec = time_stamp.nanoseconds; + ros_header.frame_id = frame_id; + + return ros_header; +} + +geometry_msgs::msg::Point toRosPoint(rviz_common::transformation::Point point) +{ + geometry_msgs::msg::Point ros_point; + ros_point.x = point.x; + ros_point.y = point.y; + ros_point.z = point.z; + + return ros_point; +} +geometry_msgs::msg::Vector3 toRosVector3(rviz_common::transformation::Point point) +{ + geometry_msgs::msg::Vector3 ros_vector; + ros_vector.x = point.x; + ros_vector.y = point.y; + ros_vector.z = point.z; + + return ros_vector; +} + + +geometry_msgs::msg::Quaternion toRosQuaternion( + rviz_common::transformation::Quaternion quaternion) +{ + geometry_msgs::msg::Quaternion ros_quaternion; + ros_quaternion.w = quaternion.w; + ros_quaternion.x = quaternion.x; + ros_quaternion.y = quaternion.y; + ros_quaternion.z = quaternion.z; + + return ros_quaternion; +} + +geometry_msgs::msg::PoseStamped toRosPoseStamped( + rviz_common::transformation::PoseStamped pose_stamped) +{ + geometry_msgs::msg::PoseStamped ros_pose; + ros_pose.header = toRosHeader(pose_stamped.time_stamp, pose_stamped.frame_id); + ros_pose.pose.position = toRosPoint(pose_stamped.position); + ros_pose.pose.orientation = toRosQuaternion(pose_stamped.orientation); + + return ros_pose; +} + +} // namespace ros_helpers +} // namespace transformation +} // namespace rviz_common diff --git a/rviz_common/src/rviz_common/transformation/structs.cpp b/rviz_common/src/rviz_common/transformation/structs.cpp new file mode 100644 index 000000000..0fe0462a9 --- /dev/null +++ b/rviz_common/src/rviz_common/transformation/structs.cpp @@ -0,0 +1,86 @@ +/* + * 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 "rviz_common/transformation/structs.hpp" + +#include + +namespace rviz_common +{ +namespace transformation +{ + +Time::Time() +: seconds(0), nanoseconds(0) {} + +Time::Time(int32_t sec, uint32_t nanosec) +: seconds(sec), nanoseconds(nanosec) +{} + +Point::Point() +: x(0), y(0), z(0) {} + +Point::Point(double x, double y, double z) +: x(x), y(y), z(z) +{} + +Quaternion::Quaternion() +: w(1), x(0), y(0), z(0) {} + +Quaternion::Quaternion(double w, double x, double y, double z) +: w(w), x(x), y(y), z(z) +{} + +PoseStamped::PoseStamped() +: time_stamp(), frame_id(""), position(), orientation() {} + +PoseStamped::PoseStamped( + Time time, std::string frame, Point position_vector, Quaternion orientation_quat) +: time_stamp(time), frame_id(frame), position(position_vector), orientation(orientation_quat) +{} + +TransformStamped::TransformStamped() +: time_stamp(), parent_frame_id(""), child_frame_id(""), translation(), rotation() +{} + +TransformStamped::TransformStamped( + Time time, + std::string parent_frame, + std::string child_frame, + Point translation_vector, + Quaternion rotation_quat) +: time_stamp(time), + parent_frame_id(parent_frame), + child_frame_id(child_frame), + translation(translation_vector), + rotation(rotation_quat) +{} + +} // namespace transformation +} // namespace rviz_common diff --git a/rviz_common/src/rviz_common/transformation/transformation_manager.cpp b/rviz_common/src/rviz_common/transformation/transformation_manager.cpp new file mode 100644 index 000000000..274a05059 --- /dev/null +++ b/rviz_common/src/rviz_common/transformation/transformation_manager.cpp @@ -0,0 +1,111 @@ +/* + * 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 "rviz_common/transformation/transformation_manager.hpp" + +#include +#include +#include + +#include "rviz_common/factory/pluginlib_factory.hpp" +#include "./identity_frame_transformer.hpp" + +namespace rviz_common +{ +namespace transformation +{ + +TransformationManager::TransformationManager( + ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node, + rclcpp::Clock::SharedPtr clock) +: rviz_ros_node_(rviz_ros_node), clock_(clock) +{ + factory_ = std::make_unique>( + "rviz_common", "rviz_common::transformation::FrameTransformer"); + factory_->addBuiltInClass( + "rviz_common", + "Identity", + "A trivial FrameTransformer implementation", + []() -> FrameTransformer * {return new IdentityFrameTransformer();}); + + for (const auto & transformer : getAvailableTransformers()) { + if (transformer.id == "rviz_default_plugins/TF") { + setTransformer(transformer); + return; + } + } + setTransformer(factory_->getPluginInfo("rviz_common/Identity")); +} + +void TransformationManager::load(const Config & config) +{ + Config current_config = config.mapGetChild("Current"); + QString class_id; + if (current_config.mapGetString("Class", &class_id)) { + setTransformer(factory_->getPluginInfo(class_id)); + } +} + +void TransformationManager::save(Config config) const +{ + Config current_config = config.mapMakeChild("Current"); + current_config.mapSetValue("Class", getCurrentTransformerInfo().id); +} + +std::vector TransformationManager::getAvailableTransformers() const +{ + return factory_->getDeclaredPlugins(); +} + +std::shared_ptr TransformationManager::getCurrentTransformer() const +{ + return current_transformer_; +} + +PluginInfo TransformationManager::getCurrentTransformerInfo() const +{ + return factory_->getPluginInfo(current_transformer_->getClassId()); +} + +void TransformationManager::setTransformer(const PluginInfo & plugin_info) +{ + auto new_transformer = std::shared_ptr(factory_->make(plugin_info.id)); + if (new_transformer) { + current_transformer_ = new_transformer; + current_transformer_->initialize(rviz_ros_node_, clock_); + + Q_EMIT transformerChanged(current_transformer_); + Q_EMIT configChanged(); + } +} + +} // namespace transformation +} // namespace rviz_common diff --git a/rviz_common/src/rviz_common/transformation_panel.cpp b/rviz_common/src/rviz_common/transformation_panel.cpp new file mode 100644 index 000000000..79bca1dcd --- /dev/null +++ b/rviz_common/src/rviz_common/transformation_panel.cpp @@ -0,0 +1,171 @@ +/* + * 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 "transformation_panel.hpp" + +#include +#include +#include +#include + +#include // NOLINT +#include // NOLINT +#include // NOLINT +#include // NOLINT + +#include "rviz_common/display_context.hpp" +#include "rviz_common/properties/grouped_checkbox_property.hpp" +#include "rviz_common/properties/grouped_checkbox_property_group.hpp" +#include "rviz_common/properties/property.hpp" +#include "rviz_common/properties/property_tree_widget.hpp" +#include "rviz_common/transformation/transformation_manager.hpp" + +namespace rviz_common +{ + +TransformationPanel::TransformationPanel(QWidget * parent) +: Panel(parent), + checkbox_property_group_(std::make_shared()), + transformation_manager_(nullptr) +{ + auto layout = new QVBoxLayout(); + layout->setContentsMargins(0, 0, 0, 0); + layout->addWidget(initializeTreeWidget()); + layout->addLayout(initializeBottomButtonRow()); + layout->addStretch(1); + setLayout(layout); +} + +properties::PropertyTreeWidget * TransformationPanel::initializeTreeWidget() +{ + root_property_ = new properties::Property(); + tree_model_ = new properties::PropertyTreeModel(root_property_); + tree_widget_ = new properties::PropertyTreeWidget(); + tree_widget_->setSelectionMode(QTreeView::NoSelection); + tree_widget_->setFocusPolicy(Qt::NoFocus); + tree_widget_->setModel(tree_model_); + connect( + tree_widget_, + SIGNAL(clicked(const QModelIndex&)), + this, + SLOT(onItemClicked(const QModelIndex&))); + + return tree_widget_; +} + +QHBoxLayout * TransformationPanel::initializeBottomButtonRow() +{ + save_button_ = new QPushButton("Save"); + + connect(save_button_, SIGNAL(clicked()), this, SLOT(onSaveClicked())); + + auto button_layout = new QHBoxLayout(); + button_layout->addWidget(save_button_); + return button_layout; +} + +void TransformationPanel::onInitialize() +{ + transformation_manager_ = getDisplayContext()->getTransformationManager(); + + auto available_transformers = transformation_manager_->getAvailableTransformers(); + for (const auto & transformer_info : available_transformers) { + createProperty(transformer_info); + } + + updateButtonState(); +} + +void TransformationPanel::createProperty(const PluginInfo & transformer_info) +{ + properties::Property * package_property = getOrCreatePackageProperty(transformer_info.package); + + auto transformer_property = new properties::GroupedCheckboxProperty( + checkbox_property_group_, transformer_info.name, false, QString(), package_property); + transformer_property_infos_.insert(std::make_pair(transformer_property, transformer_info)); + + if (isCurrentTransformerProperty(transformer_property)) { + transformer_property->checkPropertyInGroup(); + } +} + +properties::Property * TransformationPanel::getOrCreatePackageProperty(const QString & package) +{ + auto package_property_entry = package_properties_.find(package); + if (package_property_entry != package_properties_.end()) { + return package_property_entry->second; + } else { + auto package_property = new properties::Property(package, QString(), QString(), root_property_); + + package_property->setReadOnly(true); + package_property->expand(); + + package_properties_.insert(std::make_pair(package, package_property)); + + return package_property; + } +} + +void TransformationPanel::onSaveClicked() +{ + auto property = checkbox_property_group_->getChecked(); + if (property) { + transformation_manager_->setTransformer(transformer_property_infos_[property]); + updateButtonState(); + } +} + +void TransformationPanel::onItemClicked(const QModelIndex & index) +{ + auto property = dynamic_cast(tree_model_->getProp(index)); + if (property) { + property->checkPropertyInGroup(); + } + updateButtonState(); +} + +void TransformationPanel::updateButtonState() +{ + save_button_->setEnabled(checkedPropertyIsNotCurrentTransformer()); +} + +bool TransformationPanel::checkedPropertyIsNotCurrentTransformer() +{ + auto checked_property = checkbox_property_group_->getChecked(); + return !(checked_property && isCurrentTransformerProperty(checked_property)); +} + +bool TransformationPanel::isCurrentTransformerProperty( + properties::GroupedCheckboxProperty * property) +{ + auto transformer_info = transformation_manager_->getCurrentTransformerInfo(); + return transformer_property_infos_[property] == transformer_info; +} + +} // namespace rviz_common diff --git a/rviz_common/src/rviz_common/transformation_panel.hpp b/rviz_common/src/rviz_common/transformation_panel.hpp new file mode 100644 index 000000000..08d9ce64c --- /dev/null +++ b/rviz_common/src/rviz_common/transformation_panel.hpp @@ -0,0 +1,98 @@ +/* + * 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. + */ + +#ifndef RVIZ_COMMON__TRANSFORMATION_PANEL_HPP_ +#define RVIZ_COMMON__TRANSFORMATION_PANEL_HPP_ + +#include +#include +#include +#include + +#include // NOLINT +#include // NOLINT + +#include "rviz_common/panel.hpp" +#include "rviz_common/properties/grouped_checkbox_property_group.hpp" +#include "rviz_common/transformation/transformation_manager.hpp" + +namespace rviz_common +{ +namespace properties +{ +class Property; +class StringProperty; +class PropertyTreeWidget; +} + +class DisplayContext; + +/// A panel to choose the transformation plugin. +class TransformationPanel : public Panel +{ + Q_OBJECT + +public: + explicit TransformationPanel(QWidget * parent = 0); + ~TransformationPanel() override = default; + + void onInitialize() override; + +private Q_SLOTS: + void onSaveClicked(); + void onItemClicked(const QModelIndex & index); + +private: + void updateButtonState(); + bool isCurrentTransformerProperty(properties::GroupedCheckboxProperty * property); + bool checkedPropertyIsNotCurrentTransformer(); + + properties::PropertyTreeWidget * initializeTreeWidget(); + QHBoxLayout * initializeBottomButtonRow(); + void createProperty(const PluginInfo & transformer_info); + properties::Property * getOrCreatePackageProperty(const QString & package); + + properties::Property * root_property_; + properties::PropertyTreeModel * tree_model_; + properties::PropertyTreeWidget * tree_widget_; + + QPushButton * save_button_; + + std::shared_ptr checkbox_property_group_; + + transformation::TransformationManager * transformation_manager_; + + std::map transformer_property_infos_; + + std::map package_properties_; +}; + +} // namespace rviz_common + +#endif // RVIZ_COMMON__TRANSFORMATION_PANEL_HPP_ diff --git a/rviz_common/src/rviz_common/view_manager.cpp b/rviz_common/src/rviz_common/view_manager.cpp index 53d6f0db5..20265c316 100644 --- a/rviz_common/src/rviz_common/view_manager.cpp +++ b/rviz_common/src/rviz_common/view_manager.cpp @@ -105,7 +105,11 @@ ViewController * ViewManager::create(const QString & class_id) QStringList ViewManager::getDeclaredClassIdsFromFactory() { - return impl_->factory->getDeclaredClassIds(); + QStringList class_ids; + for (const auto & plugin : impl_->factory->getDeclaredPlugins()) { + class_ids.append(plugin.id); + } + return class_ids; } ViewController * ViewManager::getCurrent() const @@ -171,7 +175,7 @@ void ViewManager::copyCurrentToList() ViewController * current = getCurrent(); if (current) { ViewController * new_copy = copy(current); - new_copy->setName(impl_->factory->getClassName(new_copy->getClassId())); + new_copy->setName(impl_->factory->getPluginInfo(new_copy->getClassId()).name); impl_->root_property->addChild(new_copy); } } diff --git a/rviz_common/src/rviz_common/visualization_frame.cpp b/rviz_common/src/rviz_common/visualization_frame.cpp index 87aad0c7b..36188db63 100644 --- a/rviz_common/src/rviz_common/visualization_frame.cpp +++ b/rviz_common/src/rviz_common/visualization_frame.cpp @@ -326,12 +326,7 @@ void VisualizationFrame::initialize( render_panel_->getRenderWindow()->initialize(); auto clock = std::make_shared(RCL_ROS_TIME); - auto buffer = std::make_shared(clock); - buffer->setUsingDedicatedThread(true); - auto tf_listener = std::make_shared( - *buffer, rviz_ros_node.lock()->get_raw_node(), false); - manager_ = new VisualizationManager( - render_panel_, rviz_ros_node, this, tf_listener, buffer, clock); + manager_ = new VisualizationManager(render_panel_, rviz_ros_node, this, clock); manager_->setHelpPath(help_path_); panel_factory_ = new PanelFactory(rviz_ros_node_, manager_); @@ -1241,7 +1236,7 @@ QDockWidget * VisualizationFrame::addPanelByName( record.panel->initialize(manager_); - record.dock->setIcon(panel_factory_->getIcon(class_id)); + record.dock->setIcon(panel_factory_->getPluginInfo(class_id).icon); return record.dock; } diff --git a/rviz_common/src/rviz_common/visualization_manager.cpp b/rviz_common/src/rviz_common/visualization_manager.cpp index 2144fadbb..9024b1638 100644 --- a/rviz_common/src/rviz_common/visualization_manager.cpp +++ b/rviz_common/src/rviz_common/visualization_manager.cpp @@ -150,10 +150,7 @@ class VisualizationManagerPrivate VisualizationManager::VisualizationManager( RenderPanel * render_panel, ros_integration::RosNodeAbstractionIface::WeakPtr ros_node_abstraction, - WindowManagerInterface * wm, - std::shared_ptr tf, - std::shared_ptr buffer, - rclcpp::Clock::SharedPtr clock) + WindowManagerInterface * wm, rclcpp::Clock::SharedPtr clock) : ogre_root_(Ogre::Root::getSingletonPtr()), update_timer_(0), shutting_down_(false), @@ -172,7 +169,15 @@ VisualizationManager::VisualizationManager( // (and thus initialized later be default): default_visibility_bit_ = visibility_bit_allocator_.allocBit(); - frame_manager_ = new FrameManager(tf, buffer, clock); + transformation_manager_ = new transformation::TransformationManager(rviz_ros_node_, clock_); + connect(transformation_manager_, SIGNAL(configChanged()), this, SIGNAL(configChanged())); + + frame_manager_ = new FrameManager(clock, transformation_manager_->getCurrentTransformer()); + connect( + transformation_manager_, + SIGNAL(transformerChanged(std::shared_ptr)), + frame_manager_, + SLOT(setTransformerPlugin(std::shared_ptr))); // TODO(wjwwood): is this needed? #if 0 @@ -455,14 +460,10 @@ void VisualizationManager::updateTime() void VisualizationManager::updateFrames() { - typedef std::vector V_string; - V_string frames; - frame_manager_->getTFBufferPtr()->_getFrameStrings(frames); - // Check the fixed frame to see if it's ok std::string error; if (frame_manager_->frameHasProblems(getFixedFrame().toStdString(), error)) { - if (frames.empty()) { + if (!frame_manager_->anyTransformationDataAvailable()) { // fixed_prop->setToWarn(); std::stringstream ss; ss << "No tf data. Actual error: " << error; @@ -493,7 +494,7 @@ RenderPanel * VisualizationManager::getRenderPanel() const void VisualizationManager::resetTime() { root_display_group_->reset(); - frame_manager_->getTFBufferPtr()->clear(); + frame_manager_->clear(); ros_time_begin_ = rclcpp::Time(0, 0, clock_->get_clock_type()); wall_clock_begin_ = std::chrono::system_clock::time_point(); @@ -529,6 +530,11 @@ ViewManager * VisualizationManager::getViewManager() const return view_manager_; } +transformation::TransformationManager * VisualizationManager::getTransformationManager() +{ + return transformation_manager_; +} + void VisualizationManager::addDisplay(Display * display, bool enabled) { root_display_group_->addDisplay(display); @@ -559,6 +565,9 @@ void VisualizationManager::load(const Config & config) emitStatusUpdate("Creating views"); view_manager_->load(config.mapGetChild("Views")); + emitStatusUpdate("Loading transformation"); + transformation_manager_->load(config.mapGetChild("Transformation")); + startUpdate(); } @@ -567,6 +576,7 @@ void VisualizationManager::save(Config config) const root_display_group_->save(config); tool_manager_->save(config.mapMakeChild("Tools")); view_manager_->save(config.mapMakeChild("Views")); + transformation_manager_->save(config.mapMakeChild("Transformation")); } Display * VisualizationManager::createDisplay( diff --git a/rviz_common/src/rviz_common/visualization_manager.hpp b/rviz_common/src/rviz_common/visualization_manager.hpp index 7a0716484..6c323b429 100644 --- a/rviz_common/src/rviz_common/visualization_manager.hpp +++ b/rviz_common/src/rviz_common/visualization_manager.hpp @@ -44,6 +44,7 @@ #include "rviz_common/display_context.hpp" #include "rviz_common/frame_manager_iface.hpp" #include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" +#include "rviz_common/transformation/transformation_manager.hpp" class QTimer; @@ -102,11 +103,7 @@ class VisualizationManager : public DisplayContext explicit VisualizationManager( RenderPanel * render_panel, ros_integration::RosNodeAbstractionIface::WeakPtr ros_node_abstraction, - WindowManagerInterface * wm = 0, - std::shared_ptr tf = nullptr, - std::shared_ptr buffer = nullptr, - rclcpp::Clock::SharedPtr clock = nullptr - ); + WindowManagerInterface * wm, rclcpp::Clock::SharedPtr clock); /// Destructor. /** @@ -235,6 +232,9 @@ class VisualizationManager : public DisplayContext /// Return a pointer to the ViewManager. ViewManager * getViewManager() const override; + /// Return a pointer to the TransformationManager + rviz_common::transformation::TransformationManager * getTransformationManager() override; + /// Lock a mutex to delay calls to Ogre::Root::renderOneFrame(). void lockRender() override; @@ -400,6 +400,7 @@ private Q_SLOTS: QString help_path_; rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node_; + rviz_common::transformation::TransformationManager * transformation_manager_; }; } // namespace rviz_common diff --git a/rviz_common/test/frame_manager_test.cpp b/rviz_common/test/frame_manager_test.cpp new file mode 100644 index 000000000..967ac5020 --- /dev/null +++ b/rviz_common/test/frame_manager_test.cpp @@ -0,0 +1,108 @@ +/* + * 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 + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "../src/rviz_common/frame_manager.hpp" +#include "mock_frame_transformer.hpp" + +using namespace ::testing; // NOLINT + +class FrameManagerTestFixture : public testing::Test +{ +public: + FrameManagerTestFixture() + { + clock_ = std::make_shared(RCL_ROS_TIME); + frame_transformer_ = std::make_shared(); + frame_manager_ = std::make_unique(clock_, frame_transformer_); + } + + std::unique_ptr frame_manager_; + std::shared_ptr frame_transformer_; + std::shared_ptr clock_; +}; + + +TEST_F(FrameManagerTestFixture, transform_uses_transform_of_transformer) { + rviz_common::transformation::PoseStamped dummy_pose; + EXPECT_CALL(*frame_transformer_, transform(_, _)).WillOnce(Return(dummy_pose)); + + geometry_msgs::msg::Pose pose; + Ogre::Vector3 position; + Ogre::Quaternion orientation; + frame_manager_->transform( + "any_frame", rclcpp::Time(0, 0, clock_->get_clock_type()), pose, position, orientation); +} + +TEST_F(FrameManagerTestFixture, lastAvailableTransform_used_when_using_sync_approx) { + rviz_common::transformation::PoseStamped dummy_pose; + EXPECT_CALL(*frame_transformer_, transformIsAvailable(_, _)).WillOnce(Return(true)); + EXPECT_CALL(*frame_transformer_, transform(_, _)).WillOnce(Return(dummy_pose)); // NOLINT + + frame_manager_->setSyncMode(rviz_common::FrameManager::SyncApprox); + geometry_msgs::msg::Pose pose; + Ogre::Vector3 position; + Ogre::Quaternion orientation; + frame_manager_->transform( + "any_frame", rclcpp::Time(0, 0, clock_->get_clock_type()), pose, position, orientation); +} + +TEST_F(FrameManagerTestFixture, getAllFrameNames_uses_transformer_method) { + std::vector frame_names = {"test_frame"}; + EXPECT_CALL(*frame_transformer_, getAllFrameNames()).WillOnce(Return(frame_names)); + + auto frames = frame_manager_->getAllFrameNames(); + ASSERT_THAT(frames, SizeIs(1)); + EXPECT_THAT(frames[0], Eq(frame_names[0])); +} + +TEST_F(FrameManagerTestFixture, transformHasProblems_uses_transformer_method) { + std::string source_frame_name = "test_frame"; + EXPECT_CALL( + *frame_transformer_, transformHasProblems(source_frame_name, _, _, _)).WillOnce(Return(false)); + + std::string error; + EXPECT_FALSE(frame_manager_->transformHasProblems( + source_frame_name, rclcpp::Time(0, 0, clock_->get_clock_type()), error)); +} + +TEST_F(FrameManagerTestFixture, frameHasProblems_uses_transformer_method) { + std::string frame_name = "test_frame"; + EXPECT_CALL(*frame_transformer_, frameHasProblems(frame_name, _)).WillOnce(Return(true)); + + std::string error; + EXPECT_TRUE(frame_manager_->frameHasProblems(frame_name, error)); +} diff --git a/rviz_common/test/mock_display_context.hpp b/rviz_common/test/mock_display_context.hpp index b4bba8430..f8de43f1c 100644 --- a/rviz_common/test/mock_display_context.hpp +++ b/rviz_common/test/mock_display_context.hpp @@ -67,6 +67,7 @@ class MockDisplayContext : public rviz_common::DisplayContext MOCK_CONST_METHOD0(getToolManager, rviz_common::ToolManager * ()); MOCK_CONST_METHOD0(getViewManager, rviz_common::ViewManager * ()); + MOCK_METHOD0(getTransformationManager, rviz_common::transformation::TransformationManager * ()); MOCK_CONST_METHOD0(getRootDisplayGroup, rviz_common::DisplayGroup * ()); MOCK_CONST_METHOD0(getDefaultVisibilityBit, uint32_t()); diff --git a/rviz_common/test/mock_frame_transformer.hpp b/rviz_common/test/mock_frame_transformer.hpp new file mode 100644 index 000000000..9b82aeeac --- /dev/null +++ b/rviz_common/test/mock_frame_transformer.hpp @@ -0,0 +1,64 @@ +/* + * 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. + */ + +#ifndef MOCK_FRAME_TRANSFORMER_HPP_ +#define MOCK_FRAME_TRANSFORMER_HPP_ + +#include + +#include +#include +#include + +#include "rviz_common/transformation/frame_transformer.hpp" +#include "rviz_common/ros_integration/ros_node_abstraction.hpp" + +class MockFrameTransformer : public rviz_common::transformation::FrameTransformer +{ +public: + MOCK_METHOD2(initialize, void( + rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node, + rclcpp::Clock::SharedPtr clock)); + MOCK_METHOD0(clear, void()); + MOCK_METHOD0(getAllFrameNames, std::vector()); + MOCK_METHOD2(transform, rviz_common::transformation::PoseStamped( + const rviz_common::transformation::PoseStamped & pose_in, const std::string & frame)); + MOCK_METHOD2(transformIsAvailable, bool( + const std::string & target_frame, const std::string & source_frame)); + MOCK_METHOD4(transformHasProblems, bool( + const std::string & frame, + const std::string & fixed_frame, + const rclcpp::Time & time, + std::string & error)); + MOCK_METHOD2(frameHasProblems, bool(const std::string & frame, std::string & error)); + MOCK_METHOD0(getConnector, + rviz_common::transformation::TransformationLibraryConnector::WeakPtr()); +}; + +#endif // MOCK_FRAME_TRANSFORMER_HPP_ diff --git a/rviz_common/test/transformation/identity_frame_transformer_test.cpp b/rviz_common/test/transformation/identity_frame_transformer_test.cpp new file mode 100644 index 000000000..ae6a3eee0 --- /dev/null +++ b/rviz_common/test/transformation/identity_frame_transformer_test.cpp @@ -0,0 +1,103 @@ +/* + * 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 +#include + +#include +#include + +#include "src/rviz_common/transformation/identity_frame_transformer.hpp" +#include "transformation_test_helpers.hpp" + +using namespace testing; // NOLINT + +class IdentityTransformerFixture : public testing::Test +{ +public: + IdentityTransformerFixture() + { + transformer_ = std::make_shared(); + } + + std::shared_ptr transformer_; +}; + + +TEST_F(IdentityTransformerFixture, getAllFrameNames_returns_a_vector_containing_an_empty_string) { + auto frame_names = transformer_->getAllFrameNames(); + + ASSERT_THAT(frame_names, SizeIs(1)); + EXPECT_THAT(frame_names[0], Eq("")); +} + +TEST_F(IdentityTransformerFixture, transform_returns_the_input_pose_if_orientation_is_valid) { + auto pose_stamped = createRvizCommonPoseStamped( + 1, 3, "test", createRvizCommonPoint(0, 3, 6), createRvizCommonQuaternion(0, 1, 0, 0)); + auto transformed_pose = transformer_->transform(pose_stamped, "any_frame"); + + EXPECT_THAT(transformed_pose.time_stamp.seconds, Eq(1)); + EXPECT_THAT(transformed_pose.time_stamp.nanoseconds, Eq(static_cast(3))); + EXPECT_THAT(transformed_pose.frame_id, Eq("test")); + EXPECT_THAT(transformed_pose.position, PointEq(createRvizCommonPoint(0, 3, 6))); + EXPECT_THAT(transformed_pose.orientation, QuaternionEq(createRvizCommonQuaternion(0, 1, 0, 0))); +} + +TEST_F( + IdentityTransformerFixture, + transform_returns_the_input_pose_with_trivial_orientation_if_original_orientation_is_not_valid) { + auto pose_stamped = createRvizCommonPoseStamped( + 1, 3, "test", createRvizCommonPoint(1, 3, 6), createRvizCommonQuaternion(0, 0, 0, 0)); + auto transformed_pose = transformer_->transform(pose_stamped, "any_frame"); + + EXPECT_THAT(transformed_pose.time_stamp.seconds, Eq(1)); + EXPECT_THAT(transformed_pose.time_stamp.nanoseconds, Eq(static_cast(3))); + EXPECT_THAT(transformed_pose.frame_id, Eq("test")); + EXPECT_THAT(transformed_pose.position, PointEq(createRvizCommonPoint(1, 3, 6))); + EXPECT_THAT(transformed_pose.orientation, QuaternionEq(createRvizCommonQuaternion(1, 0, 0, 0))); +} + +TEST_F(IdentityTransformerFixture, transformationIsAvailable_returns_true) { + EXPECT_TRUE(transformer_->transformIsAvailable("any_target", "any_source")); +} + +TEST_F(IdentityTransformerFixture, transformHasProblesm_returns_false) { + std::string error = ""; + + EXPECT_FALSE(transformer_->transformHasProblems( + "any_target", "any_source", rclcpp::Clock().now(), error)); + EXPECT_THAT(error, Eq("")); +} + +TEST_F(IdentityTransformerFixture, frameHasProblesm_returns_false) { + std::string error = ""; + + EXPECT_FALSE(transformer_->frameHasProblems("any_frame", error)); + EXPECT_THAT(error, Eq("")); +} diff --git a/rviz_common/test/transformation/ros_conversion_helpers_test.cpp b/rviz_common/test/transformation/ros_conversion_helpers_test.cpp new file mode 100644 index 000000000..a990f2bc2 --- /dev/null +++ b/rviz_common/test/transformation/ros_conversion_helpers_test.cpp @@ -0,0 +1,166 @@ +/* + * 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 +#include + +#include + +#include "rviz_common/transformation/ros_helpers/ros_conversion_helpers.hpp" +#include "transformation_test_helpers.hpp" + +using namespace ::testing; // NOLINT + +TEST(ros_conversion_helpers, fromRosPoint_converts_ros_point_to_rviz_common_transformation_point) { + auto ros_point = createRosPoint(0, 1, 3); + auto rviz_common_point = rviz_common::transformation::ros_helpers::fromRosPoint(ros_point); + + EXPECT_THAT(rviz_common_point, PointEq(ros_point)); +} + +TEST( + ros_conversion_helpers, + fromRosQuaternion_converts_ros_quaternion_to_rviz_common_transformation_quaternion) { + auto ros_quaternion = createRosQuaternion(0.707, 0, 0.707, 0); + auto rviz_common_quaternion = + rviz_common::transformation::ros_helpers::fromRosQuaternion(ros_quaternion); + + EXPECT_THAT(rviz_common_quaternion, QuaternionEq(ros_quaternion)); +} + +TEST( + ros_conversion_helpers, fromRosVector3_converts_ros_vector3_to_rviz_common_transformation_point) { + auto ros_vector = createRosVector3(2, 1, 3); + auto rviz_common_point = rviz_common::transformation::ros_helpers::fromRosVector3(ros_vector); + + EXPECT_THAT(rviz_common_point, PointEq(ros_vector)); +} + +TEST(ros_conversion_helpers, toRosPoint_converts_rviz_common_transformation_point_to_ros_point) { + auto rviz_common_point = createRvizCommonPoint(4, 1, 3); + auto ros_point = rviz_common::transformation::ros_helpers::toRosPoint(rviz_common_point); + + EXPECT_THAT(ros_point, PointEq(rviz_common_point)); +} + +TEST( + ros_conversion_helpers, toRosVector3_converts_rviz_common_transformation_point_to_ros_vector3) { + auto rviz_common_point = createRvizCommonPoint(4, 1, 0); + auto ros_vector3 = rviz_common::transformation::ros_helpers::toRosVector3(rviz_common_point); + + EXPECT_THAT(ros_vector3, PointEq(rviz_common_point)); +} + +TEST( + ros_conversion_helpers, + toRosQuaternion_converts_rviz_common_transformation_quaternion_to_ros_quaternion) { + auto rviz_common_quaternion = createRvizCommonQuaternion(0.707, 0, 0.707, 0); + auto ros_quaternion = + rviz_common::transformation::ros_helpers::toRosQuaternion(rviz_common_quaternion); + + EXPECT_THAT(ros_quaternion, QuaternionEq(rviz_common_quaternion)); +} + +TEST(ros_conversion_helpers, toRosHeader_creates_a_ros_header_from_time_and_frame_id) { + rviz_common::transformation::Time rviz_common_time_stamp(2, 100); + std::string frame_id = "test_frame"; + auto ros_header = rviz_common::transformation::ros_helpers::toRosHeader( + rviz_common_time_stamp, frame_id); + + EXPECT_THAT(ros_header.stamp.sec, Eq(2)); + EXPECT_THAT(ros_header.stamp.nanosec, Eq(static_cast(100))); + EXPECT_THAT(ros_header.frame_id, Eq(frame_id)); +} + +TEST( + ros_conversion_helpers, + toRosPoseStamped_creates_a_ros_pose_stamped_from_an_rviz_common_transformation_pose_stamped) { + rviz_common::transformation::PoseStamped rviz_common_pose_stamped; + rviz_common_pose_stamped.time_stamp = rviz_common::transformation::Time(2, 10); + rviz_common_pose_stamped.frame_id = "pose_stamped_frame"; + rviz_common_pose_stamped.position = createRvizCommonPoint(1, 2, 3); + rviz_common_pose_stamped.orientation = createRvizCommonQuaternion(0, 1, 0, 0); + + auto ros_pose_stamped = rviz_common::transformation::ros_helpers::toRosPoseStamped( + rviz_common_pose_stamped); + + EXPECT_THAT(ros_pose_stamped.header.frame_id, Eq(rviz_common_pose_stamped.frame_id)); + EXPECT_THAT(ros_pose_stamped.pose.position, PointEq(rviz_common_pose_stamped.position)); + EXPECT_THAT(ros_pose_stamped.header.stamp.sec, Eq(rviz_common_pose_stamped.time_stamp.seconds)); + EXPECT_THAT( + ros_pose_stamped.pose.orientation, QuaternionEq(rviz_common_pose_stamped.orientation)); + EXPECT_THAT( + ros_pose_stamped.header.stamp.nanosec, Eq(rviz_common_pose_stamped.time_stamp.nanoseconds)); +} + +TEST( + ros_conversion_helpers, fromRosPoseStamped_creates_an_rviz_common_pose_stamped_from_a_ros_one) { + std_msgs::msg::Header header; + header.stamp.sec = 10; + header.stamp.nanosec = 20; + header.frame_id = "pose_frame"; + auto ros_pose = createRosPoseStamped( + header, createRosPoint(3, 2, 1), createRosQuaternion(0, 0, 1, 0)); + auto rviz_common_pose_stamped = + rviz_common::transformation::ros_helpers::fromRosPoseStamped(ros_pose); + + EXPECT_THAT( + rviz_common_pose_stamped.time_stamp.seconds, Eq(ros_pose.header.stamp.sec)); + EXPECT_THAT( + rviz_common_pose_stamped.time_stamp.nanoseconds, Eq(ros_pose.header.stamp.nanosec)); + EXPECT_THAT(rviz_common_pose_stamped.frame_id, Eq(ros_pose.header.frame_id)); + EXPECT_THAT(rviz_common_pose_stamped.position, PointEq(ros_pose.pose.position)); + EXPECT_THAT(rviz_common_pose_stamped.orientation, QuaternionEq(ros_pose.pose.orientation)); +} + +TEST( + ros_conversion_helpers, + fromRostransformStamped_creates_an_rviz_common_transform_stamped_from_a_ros_one) { + std_msgs::msg::Header header; + header.stamp.sec = 10; + header.stamp.nanosec = 20; + header.frame_id = "parent_frame"; + auto ros_transform = createRosTransformStamped( + header, "child_frame", createRosVector3(3, 2, 1), createRosQuaternion(0, 0, 1, 0)); + auto rviz_common_transform_stamped = + rviz_common::transformation::ros_helpers::fromRosTransformStamped(ros_transform); + + EXPECT_THAT( + rviz_common_transform_stamped.time_stamp.nanoseconds, Eq(ros_transform.header.stamp.nanosec)); + EXPECT_THAT( + rviz_common_transform_stamped.time_stamp.seconds, Eq(ros_transform.header.stamp.sec)); + EXPECT_THAT( + rviz_common_transform_stamped.child_frame_id, Eq(ros_transform.child_frame_id)); + EXPECT_THAT( + rviz_common_transform_stamped.translation, PointEq(ros_transform.transform.translation)); + EXPECT_THAT( + rviz_common_transform_stamped.rotation, QuaternionEq(ros_transform.transform.rotation)); + EXPECT_THAT( + rviz_common_transform_stamped.parent_frame_id, Eq(ros_transform.header.frame_id)); +} diff --git a/rviz_common/test/transformation/transformation_test_helpers.hpp b/rviz_common/test/transformation/transformation_test_helpers.hpp new file mode 100644 index 000000000..b383d460a --- /dev/null +++ b/rviz_common/test/transformation/transformation_test_helpers.hpp @@ -0,0 +1,153 @@ +/* + * 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. + */ + +#ifndef TRANSFORMATION__TRANSFORMATION_TEST_HELPERS_HPP_ +#define TRANSFORMATION__TRANSFORMATION_TEST_HELPERS_HPP_ + +#include + +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "std_msgs/msg/header.hpp" + +#include "rviz_common/transformation/structs.hpp" + +MATCHER_P(PointEq, expected, "") { + return expected.x == arg.x && expected.y == arg.y && expected.z == arg.z; +} + +MATCHER_P(QuaternionEq, expected, "") { + return expected.w == arg.w && + expected.x == arg.x && + expected.y == arg.y && + expected.z == arg.z; +} + +geometry_msgs::msg::Point createRosPoint(double x, double y, double z) +{ + geometry_msgs::msg::Point point; + point.x = x; + point.y = y; + point.z = z; + + return point; +} + +geometry_msgs::msg::Vector3 createRosVector3(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 vector; + vector.x = x; + vector.y = y; + vector.z = z; + + return vector; +} + +geometry_msgs::msg::Quaternion createRosQuaternion(double w, double x, double y, double z) +{ + geometry_msgs::msg::Quaternion quaternion; + quaternion.w = w; + quaternion.x = x; + quaternion.y = y; + quaternion.z = z; + + return quaternion; +} + +geometry_msgs::msg::PoseStamped createRosPoseStamped( + std_msgs::msg::Header header, + geometry_msgs::msg::Point position, + geometry_msgs::msg::Quaternion orientation) +{ + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header = header; + pose_stamped.pose.position = position; + pose_stamped.pose.orientation = orientation; + + return pose_stamped; +} + +geometry_msgs::msg::TransformStamped createRosTransformStamped( + std_msgs::msg::Header header, + std::string child_frame_id, + geometry_msgs::msg::Vector3 translation, + geometry_msgs::msg::Quaternion rotation) +{ + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header = header; + transform_stamped.child_frame_id = child_frame_id; + transform_stamped.transform.translation = translation; + transform_stamped.transform.rotation = rotation; + + return transform_stamped; +} + +rviz_common::transformation::Point createRvizCommonPoint(double x, double y, double z) +{ + rviz_common::transformation::Point point; + point.x = x; + point.y = y; + point.z = z; + + return point; +} + +rviz_common::transformation::Quaternion createRvizCommonQuaternion( + double w, double x, double y, double z) +{ + rviz_common::transformation::Quaternion quaternion; + quaternion.w = w; + quaternion.x = x; + quaternion.y = y; + quaternion.z = z; + + return quaternion; +} + +rviz_common::transformation::PoseStamped createRvizCommonPoseStamped( + int32_t sec, + uint32_t nanosec, + std::string frame, + rviz_common::transformation::Point pose, + rviz_common::transformation::Quaternion orient) +{ + rviz_common::transformation::PoseStamped pose_stamped; + pose_stamped.time_stamp.seconds = sec; + pose_stamped.time_stamp.nanoseconds = nanosec; + pose_stamped.frame_id = frame; + pose_stamped.position = pose; + pose_stamped.orientation = orient; + + return pose_stamped; +} + +#endif // TRANSFORMATION__TRANSFORMATION_TEST_HELPERS_HPP_ diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index 6494d7a89..330216232 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -83,6 +83,7 @@ set(rviz_default_plugins_headers_to_moc 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/transformation/transformer_guard.hpp include/rviz_default_plugins/displays/path/path_display.hpp include/rviz_default_plugins/displays/point/point_stamped_display.hpp include/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp @@ -112,6 +113,8 @@ set(rviz_default_plugins_headers_to_moc include/rviz_default_plugins/tools/pose/pose_tool.hpp include/rviz_default_plugins/tools/pose_estimate/initial_pose_tool.hpp include/rviz_default_plugins/tools/point/point_tool.hpp + include/rviz_default_plugins/transformation/tf_frame_transformer.hpp + include/rviz_default_plugins/transformation/tf_wrapper.hpp include/rviz_default_plugins/view_controllers/orbit/orbit_view_controller.hpp include/rviz_default_plugins/view_controllers/ortho/fixed_orientation_ortho_view_controller.hpp include/rviz_default_plugins/view_controllers/xy_orbit/xy_orbit_view_controller.hpp @@ -180,6 +183,8 @@ set(rviz_default_plugins_source_files src/rviz_default_plugins/tools/pose_estimate/initial_pose_tool.cpp src/rviz_default_plugins/tools/point/point_tool.cpp src/rviz_default_plugins/tools/select/selection_tool.cpp + src/rviz_default_plugins/transformation/tf_frame_transformer.cpp + src/rviz_default_plugins/transformation/tf_wrapper.cpp src/rviz_default_plugins/view_controllers/follower/third_person_follower_view_controller.cpp src/rviz_default_plugins/view_controllers/fps/fps_view_controller.cpp src/rviz_default_plugins/view_controllers/orbit/orbit_view_controller.cpp @@ -565,6 +570,23 @@ if(BUILD_TESTING) target_link_libraries(xy_orbit_view_controller_test ${TEST_LINK_LIBRARIES}) endif() + ament_add_gmock(frame_transformer_tf_test + test/rviz_default_plugins/transformation/frame_transformer_tf_test.cpp + ${SKIP_DISPLAY_TESTS}) + if(TARGET frame_transformer_tf_test) + target_include_directories(frame_transformer_tf_test PUBLIC ${TEST_INCLUDE_DIRS}) + target_link_libraries(frame_transformer_tf_test ${TEST_LINK_LIBRARIES}) + endif() + + ament_add_gmock(transformer_guard_test + test/rviz_default_plugins/transformation/transformer_guard_test.cpp + test/rviz_default_plugins/displays/display_test_fixture.cpp + ${SKIP_DISPLAY_TESTS}) + if(TARGET transformer_guard_test) + target_include_directories(transformer_guard_test PUBLIC ${TEST_INCLUDE_DIRS}) + target_link_libraries(transformer_guard_test ${TEST_LINK_LIBRARIES}) + endif() + ament_add_gtest(camera_display_visual_test test/rviz_default_plugins/displays/camera/camera_display_visual_test.cpp test/rviz_default_plugins/publishers/camera_info_publisher.hpp diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp index 98844b2e2..4b24fc4cd 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp @@ -32,16 +32,19 @@ #include +#include //NOLINT + #include "sensor_msgs/msg/laser_scan.hpp" +#include "laser_geometry/laser_geometry.hpp" + #include "rviz_common/ros_topic_display.hpp" +#include "rviz_common/transformation/frame_transformer.hpp" +#include "rviz_default_plugins/transformation/transformer_guard.hpp" +#include "rviz_default_plugins/transformation/tf_wrapper.hpp" +#include "rviz_default_plugins/transformation/tf_frame_transformer.hpp" #include "rviz_default_plugins/visibility_control.hpp" -namespace laser_geometry -{ -class LaserProjection; -} // namespace laser_geometry - namespace rviz_common { class QueueSizeProperty; @@ -55,13 +58,14 @@ namespace rviz_default_plugins class PointCloudCommon; namespace displays { - /** @brief Visualizes a laser scan, received as a sensor_msgs::LaserScan. */ // TODO(botteroa-si): This display originally extended the MessageFilterDisplay. Revisit when // available class RVIZ_DEFAULT_PLUGINS_PUBLIC LaserScanDisplay : public rviz_common::RosTopicDisplay { + Q_OBJECT + public: LaserScanDisplay(); ~LaserScanDisplay() override = default; @@ -81,6 +85,10 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC LaserScanDisplay : public std::unique_ptr point_cloud_common_; std::unique_ptr queue_size_property_; std::unique_ptr projector_; + +private: + std::unique_ptr> transformer_guard_; }; } // namespace displays diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/robot_model/robot_model_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/robot_model/robot_model_display.hpp index ce79037b6..e7cac502f 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/robot_model/robot_model_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/robot_model/robot_model_display.hpp @@ -40,6 +40,8 @@ #include "rviz_common/ros_topic_display.hpp" +#include "rviz_default_plugins/transformation/transformer_guard.hpp" +#include "rviz_default_plugins/transformation/tf_frame_transformer.hpp" #include "rviz_default_plugins/visibility_control.hpp" namespace Ogre @@ -138,6 +140,9 @@ private Q_SLOTS: rviz_common::properties::FilePickerProperty * description_file_property_; rviz_common::properties::FloatProperty * alpha_property_; rviz_common::properties::StringProperty * tf_prefix_property_; + + std::unique_ptr> transformer_guard_; }; } // namespace displays diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/tf/tf_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/tf/tf_display.hpp index 3a11ccdf0..035fc0fdf 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/tf/tf_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/tf/tf_display.hpp @@ -48,6 +48,8 @@ #include "rviz_common/interaction/forwards.hpp" #include "rviz_common/display.hpp" +#include "rviz_default_plugins/transformation/transformer_guard.hpp" +#include "rviz_default_plugins/transformation/tf_frame_transformer.hpp" #include "rviz_default_plugins/visibility_control.hpp" namespace Ogre @@ -149,6 +151,9 @@ private Q_SLOTS: bool changing_single_frame_enabled_state_; + std::unique_ptr> transformer_guard_; + void updateRelativePositionAndOrientation( const FrameInfo * frame, std::shared_ptr tf_buffer) const; diff --git a/rviz_default_plugins/include/rviz_default_plugins/transformation/tf_frame_transformer.hpp b/rviz_default_plugins/include/rviz_default_plugins/transformation/tf_frame_transformer.hpp new file mode 100644 index 000000000..07b6f866d --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/transformation/tf_frame_transformer.hpp @@ -0,0 +1,110 @@ +/* + * 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. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__TRANSFORMATION__TF_FRAME_TRANSFORMER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__TRANSFORMATION__TF_FRAME_TRANSFORMER_HPP_ + +#include +#include +#include + +#include +#include + +#include "rviz_common/transformation/frame_transformer.hpp" +#include "rviz_common/ros_integration/ros_node_abstraction.hpp" +#include "rviz_default_plugins/transformation/tf_wrapper.hpp" + +#include "rviz_default_plugins/visibility_control.hpp" + +namespace rviz_default_plugins +{ +namespace transformation +{ +class TFFrameTransformer : public rviz_common::transformation::FrameTransformer +{ +public: + RVIZ_DEFAULT_PLUGINS_PUBLIC + TFFrameTransformer(); + + RVIZ_DEFAULT_PLUGINS_PUBLIC + explicit TFFrameTransformer(std::shared_ptr wrapper); + + ~TFFrameTransformer() override = default; + + RVIZ_DEFAULT_PLUGINS_PUBLIC + void + initialize( + rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node, + rclcpp::Clock::SharedPtr clock) override; + + RVIZ_DEFAULT_PLUGINS_PUBLIC + void + clear() override; + + RVIZ_DEFAULT_PLUGINS_PUBLIC + std::vector + getAllFrameNames() override; + + RVIZ_DEFAULT_PLUGINS_PUBLIC + rviz_common::transformation::PoseStamped + transform( + // NOLINT (this is not std::transform) + const rviz_common::transformation::PoseStamped & pose_in, + const std::string & target_frame) override; + + RVIZ_DEFAULT_PLUGINS_PUBLIC + bool + transformIsAvailable( + const std::string & target_frame, + const std::string & source_frame) override; + + RVIZ_DEFAULT_PLUGINS_PUBLIC + bool + transformHasProblems( + const std::string & source_frame, + const std::string & target_frame, + const rclcpp::Time & time, + std::string & error) override; + + RVIZ_DEFAULT_PLUGINS_PUBLIC + bool + frameHasProblems(const std::string & frame, std::string & error) override; + + RVIZ_DEFAULT_PLUGINS_PUBLIC + rviz_common::transformation::TransformationLibraryConnector::WeakPtr + getConnector() override; + +private: + std::shared_ptr tf_wrapper_; +}; +} // namespace transformation +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__TRANSFORMATION__TF_FRAME_TRANSFORMER_HPP_ diff --git a/rviz_default_plugins/include/rviz_default_plugins/transformation/tf_wrapper.hpp b/rviz_default_plugins/include/rviz_default_plugins/transformation/tf_wrapper.hpp new file mode 100644 index 000000000..bec947a44 --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/transformation/tf_wrapper.hpp @@ -0,0 +1,114 @@ +/* + * 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. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__TRANSFORMATION__TF_WRAPPER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__TRANSFORMATION__TF_WRAPPER_HPP_ + +#include +#include +#include + +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" + +#include "rviz_common/transformation/frame_transformer.hpp" +#include "rviz_default_plugins/visibility_control.hpp" + +namespace rviz_default_plugins +{ +namespace transformation +{ +class TFWrapper + : public rviz_common::transformation::TransformationLibraryConnector +{ +public: + RVIZ_DEFAULT_PLUGINS_PUBLIC + TFWrapper(); + + RVIZ_DEFAULT_PLUGINS_PUBLIC + ~TFWrapper() override = default; + + RVIZ_DEFAULT_PLUGINS_PUBLIC + void transform( + const geometry_msgs::msg::PoseStamped & pose_in, + geometry_msgs::msg::PoseStamped & pose_out, + const std::string & frame); + + RVIZ_DEFAULT_PLUGINS_PUBLIC + geometry_msgs::msg::TransformStamped + lookupTransform( + const std::string & target_frame, + const std::string & source_frame, + const tf2::TimePoint & time); + + RVIZ_DEFAULT_PLUGINS_PUBLIC + bool + canTransform( + const std::string & fixed_frame, + const std::string & frame, + tf2::TimePoint time, + std::string & error); + + RVIZ_DEFAULT_PLUGINS_PUBLIC + std::vector + getFrameStrings(); + + RVIZ_DEFAULT_PLUGINS_PUBLIC + std::shared_ptr + getBuffer(); + + RVIZ_DEFAULT_PLUGINS_PUBLIC + bool + frameExists(const std::string & frame); + + RVIZ_DEFAULT_PLUGINS_PUBLIC + void + initialize( + rclcpp::Clock::SharedPtr clock, + rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node, + bool using_dedicated_thread); + + RVIZ_DEFAULT_PLUGINS_PUBLIC + void + clear(); + + RVIZ_DEFAULT_PLUGINS_PUBLIC + void + initializeBuffer(rclcpp::Clock::SharedPtr clock, bool using_dedicated_thread); + +private: + std::shared_ptr buffer_; + std::shared_ptr tf_listener_; +}; + +} // namespace transformation +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__TRANSFORMATION__TF_WRAPPER_HPP_ diff --git a/rviz_default_plugins/include/rviz_default_plugins/transformation/transformer_guard.hpp b/rviz_default_plugins/include/rviz_default_plugins/transformation/transformer_guard.hpp new file mode 100644 index 000000000..3cc2b4ecd --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/transformation/transformer_guard.hpp @@ -0,0 +1,205 @@ +/* + * 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. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__TRANSFORMATION__TRANSFORMER_GUARD_HPP_ +#define RVIZ_DEFAULT_PLUGINS__TRANSFORMATION__TRANSFORMER_GUARD_HPP_ + +#include +#include + +#include // NOLINT +#include //NOLINT + +#include "rviz_common/display.hpp" +#include "rviz_common/display_context.hpp" +#include "rviz_common/transformation/frame_transformer.hpp" +#include "rviz_common/transformation/transformation_manager.hpp" +#include "rviz_default_plugins/transformation/tf_wrapper.hpp" +#include "rviz_default_plugins/visibility_control.hpp" + +namespace rviz_default_plugins +{ +namespace transformation +{ + +/// Helper class for TransformerGuard. +/** + * This is needed because Qt's moc and c++ templates do not work nicely + * together (i.e. a Q_OBJECT may not be a template class). + * Not intended to be used directly, use TransformerGuard instead. + */ +class RVIZ_DEFAULT_PLUGINS_PUBLIC _TransformerGuard : public QObject +{ + Q_OBJECT + +public: + _TransformerGuard( + rviz_common::Display * display, + const std::string & transformer_name) + : display_(display), + allowed_transformer_name_(transformer_name), + using_allowed_transformer_(true), + display_disabled_by_user_(false), + context_(nullptr) + {} + + void + initialize(rviz_common::DisplayContext * context) + { + context_ = context; + connect( + context_->getTransformationManager(), + SIGNAL(transformerChanged(std::shared_ptr)), + this, + SLOT(transformerChanged(std::shared_ptr))); + connect(display_, SIGNAL(changed()), this, SLOT(displayEnabledChanged())); + + if (!checkTransformer()) { + using_allowed_transformer_ = false; + Q_EMIT (display_->changed()); + } + } + + virtual + bool + checkTransformer() = 0; + +protected Q_SLOTS: + virtual + void + transformerChanged( + std::shared_ptr new_transformer) = 0; + virtual + void + displayEnabledChanged() = 0; + +protected: + rviz_common::Display * display_; + std::string allowed_transformer_name_; + bool using_allowed_transformer_; + bool display_disabled_by_user_; + rviz_common::DisplayContext * context_; +}; + +/// Convenience class for displays that can only work with a specific transformer. +/** + * For example, some displays may only work with the tf2 backed transformer. + * It helps handling the change of the transformer. + */ +template +class TransformerGuard : public _TransformerGuard +{ +public: + // The transformer_name is only needed to set the error status. + TransformerGuard( + rviz_common::Display * display, + const std::string & transformer_name) + : _TransformerGuard(display, transformer_name) + {} + + ~TransformerGuard() override = default; + + bool + checkTransformer() override + { + return isAllowedTransformer(context_->getFrameManager()->getTransformer()); + } + + void + updateDisplayAccordingToTransformerType( + std::shared_ptr transformer) + { + using_allowed_transformer_ = isAllowedTransformer(transformer); + + if (!using_allowed_transformer_) { + display_disabled_by_user_ = !display_->isEnabled(); + disableDisplayAndSetErrorStatus(); + } else { + enableDisplayAndDeleteErrorStatus(); + } + } + +private: + void + transformerChanged( + std::shared_ptr new_transformer) override + { + if (using_allowed_transformer_ != isAllowedTransformer(new_transformer)) { + updateDisplayAccordingToTransformerType(context_->getFrameManager()->getTransformer()); + } + } + + void + displayEnabledChanged() override + { + if (!using_allowed_transformer_) { + disableDisplayAndSetErrorStatus(); + } + } + + virtual + bool + isAllowedTransformer( + std::shared_ptr transformer) + { + auto derived_transformer = std::dynamic_pointer_cast(transformer); + return static_cast(derived_transformer); + } + + void + setErrorStatus() + { + display_->setStatus( + rviz_common::properties::StatusProperty::Error, + "Transformer", + QString::fromStdString( + "The display works only with " + allowed_transformer_name_ + " Transformer")); + } + + void + enableDisplayAndDeleteErrorStatus() + { + display_->deleteStatusStd("Transformer"); + if (!display_disabled_by_user_) { + display_->setEnabled(true); + } + } + + void + disableDisplayAndSetErrorStatus() + { + display_->setEnabled(false); + setErrorStatus(); + } +}; + +} // namespace transformation +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__TRANSFORMATION__TRANSFORMER_GUARD_HPP_ diff --git a/rviz_default_plugins/plugins_description.xml b/rviz_default_plugins/plugins_description.xml index fe2c1db14..1d58a70fd 100644 --- a/rviz_default_plugins/plugins_description.xml +++ b/rviz_default_plugins/plugins_description.xml @@ -281,6 +281,18 @@ + + + + + Uses TF2 for frame transformations + + + #include -#include "laser_geometry/laser_geometry.hpp" #include "tf2_ros/buffer.h" -#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" #include "rviz_common/properties/int_property.hpp" #include "rviz_common/properties/queue_size_property.hpp" +#include "rviz_common/transformation/transformation_manager.hpp" #include "rviz_common/validate_floats.hpp" +#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" +#include "rviz_default_plugins/transformation/tf_wrapper.hpp" namespace rviz_default_plugins { @@ -48,13 +49,17 @@ namespace displays LaserScanDisplay::LaserScanDisplay() : point_cloud_common_(std::make_unique(this)), queue_size_property_(std::make_unique(this, 10)), - projector_(std::make_unique()) + projector_(std::make_unique()), + transformer_guard_( + std::make_unique>(this, "TF")) {} void LaserScanDisplay::onInitialize() { RTDClass::onInitialize(); point_cloud_common_->initialize(context_, scene_node_); + transformer_guard_->initialize(context_); } void LaserScanDisplay::processMessage(sensor_msgs::msg::LaserScan::ConstSharedPtr scan) @@ -69,29 +74,34 @@ void LaserScanDisplay::processMessage(sensor_msgs::msg::LaserScan::ConstSharedPt // } auto cloud = std::make_shared(); - - try { - auto buffer = context_->getFrameManager()->getTFBufferPtr(); - projector_->transformLaserScanToPointCloud( - fixed_frame_.toStdString(), - *scan, - *cloud, - *buffer, - -1, - laser_geometry::channel_option::Intensity); - } catch (tf2::TransformException & exception) { - setMissingTransformToFixedFrame(scan->header.frame_id); - RVIZ_COMMON_LOG_ERROR(exception.what()); - return; + auto tf_wrapper = std::dynamic_pointer_cast( + context_->getFrameManager()->getConnector().lock()); + + if (tf_wrapper) { + try { + projector_->transformLaserScanToPointCloud( + fixed_frame_.toStdString(), + *scan, + *cloud, + *tf_wrapper->getBuffer(), + -1, + laser_geometry::channel_option::Intensity); + } catch (tf2::TransformException & exception) { + setMissingTransformToFixedFrame(scan->header.frame_id); + RVIZ_COMMON_LOG_ERROR(exception.what()); + return; + } + setTransformOk(); + + point_cloud_common_->addMessage(cloud); } - setTransformOk(); - - point_cloud_common_->addMessage(cloud); } void LaserScanDisplay::update(float wall_dt, float ros_dt) { - point_cloud_common_->update(wall_dt, ros_dt); + if (transformer_guard_->checkTransformer()) { + point_cloud_common_->update(wall_dt, ros_dt); + } } void LaserScanDisplay::reset() 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 1e929ea99..6a7a9349c 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 @@ -78,7 +78,11 @@ void PointsMarker::onNewMessage( Ogre::Vector3 pose, scale; Ogre::Quaternion orientation; - transformAndSetVisibility(new_message, pose, scale, orientation); + if (!transform(new_message, pose, orientation, scale)) { // NOLINT: is super class method + scene_node_->setVisible(false); + return; + } + scene_node_->setVisible(true); setPosition(pose); setOrientation(orientation); @@ -94,19 +98,6 @@ void PointsMarker::onNewMessage( addPointsFromMessage(new_message); } -void PointsMarker::transformAndSetVisibility( - const MarkerConstSharedPtr & new_message, - Ogre::Vector3 & position, - Ogre::Vector3 & scale, - Ogre::Quaternion & orientation) -{ - if (!transform(new_message, position, orientation, scale)) { // NOLINT: is super class method - scene_node_->setVisible(false); - return; - } - scene_node_->setVisible(true); -} - void PointsMarker::setRenderModeAndDimensions( const MarkerConstSharedPtr & new_message, Ogre::Vector3 & scale) { diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp index c8ce16d36..7fc62b2fa 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp @@ -165,17 +165,16 @@ void PointCloudCommon::initialize( void PointCloudCommon::loadTransformers() { - QStringList classes = transformer_factory_->getDeclaredClassIds(); - for (auto const & class_id : classes) { - std::string name = transformer_factory_->getClassName(class_id).toStdString(); - - if (transformers_.count(name) > 0) { - RVIZ_COMMON_LOG_ERROR_STREAM("Transformer type " << name << " is already loaded."); + auto plugins = transformer_factory_->getDeclaredPlugins(); + for (auto const & plugin : plugins) { + auto plugin_name_std = plugin.name.toStdString(); + if (transformers_.count(plugin_name_std) > 0) { + RVIZ_COMMON_LOG_ERROR_STREAM("Transformer type " << plugin_name_std << " is already loaded."); continue; } - PointCloudTransformerPtr trans(transformer_factory_->make(class_id)); - loadTransformer(trans, name, class_id.toStdString()); + PointCloudTransformerPtr trans(transformer_factory_->make(plugin.id)); + loadTransformer(trans, plugin_name_std, plugin.id.toStdString()); } } diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/robot_model/robot_model_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/robot_model/robot_model_display.cpp index fd99f8dbc..0d4d01f4d 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/robot_model/robot_model_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/robot_model/robot_model_display.cpp @@ -81,7 +81,10 @@ enum DescriptionSource RobotModelDisplay::RobotModelDisplay() : has_new_transforms_(false), - time_since_last_transform_(0.0f) + time_since_last_transform_(0.0f), + transformer_guard_( + std::make_unique>(this, "TF")) { visual_enabled_property_ = new Property("Visual Enabled", true, "Whether to display the visual representation of the robot.", @@ -136,6 +139,8 @@ void RobotModelDisplay::onInitialize() updateCollisionVisible(); updateAlpha(); updatePropertyVisibility(); + + transformer_guard_->initialize(context_); } void RobotModelDisplay::updateAlpha() @@ -194,6 +199,10 @@ void RobotModelDisplay::updateTfPrefix() void RobotModelDisplay::load_urdf() { + if (!transformer_guard_->checkTransformer()) { + return; + } + if (description_source_property_->getOptionInt() == DescriptionSource::FILE && !description_file_property_->getString().isEmpty()) { @@ -269,6 +278,10 @@ void RobotModelDisplay::onDisable() void RobotModelDisplay::update(float wall_dt, float ros_dt) { + if (!transformer_guard_->checkTransformer()) { + return; + } + (void) ros_dt; time_since_last_transform_ += wall_dt; float rate = update_rate_property_->getFloat(); diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp index 614b6eca4..d52c242b4 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp @@ -42,6 +42,7 @@ #include #include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" #include "rviz_rendering/objects/arrow.hpp" #include "rviz_rendering/objects/axes.hpp" @@ -58,8 +59,9 @@ #include "rviz_common/interaction/forwards.hpp" #include "rviz_common/interaction/selection_manager.hpp" #include "rviz_common/uniform_string_stream.hpp" -#include "include/rviz_default_plugins/displays/tf/frame_info.hpp" -#include "include/rviz_default_plugins/displays/tf/frame_selection_handler.hpp" +#include "rviz_default_plugins/displays/tf/frame_info.hpp" +#include "rviz_default_plugins/displays/tf/frame_selection_handler.hpp" +#include "rviz_default_plugins/transformation/tf_wrapper.hpp" using rviz_common::interaction::SelectionHandler; using rviz_common::properties::BoolProperty; @@ -82,7 +84,10 @@ namespace displays TFDisplay::TFDisplay() : update_timer_(0.0f), - changing_single_frame_enabled_state_(false) + changing_single_frame_enabled_state_(false), + transformer_guard_( + std::make_unique>(this, "TF")) { show_names_property_ = new BoolProperty( "Show Names", @@ -162,6 +167,8 @@ void TFDisplay::onInitialize() names_node_ = root_node_->createChildSceneNode(); arrows_node_ = root_node_->createChildSceneNode(); axes_node_ = root_node_->createChildSceneNode(); + + transformer_guard_->initialize(context_); } void TFDisplay::load(const rviz_common::Config & config) @@ -262,6 +269,10 @@ void TFDisplay::allEnabledChanged() void TFDisplay::update(float wall_dt, float ros_dt) { + if (!transformer_guard_->checkTransformer()) { + return; + } + (void) ros_dt; update_timer_ += wall_dt; float update_rate = update_rate_property_->getFloat(); @@ -275,8 +286,7 @@ void TFDisplay::update(float wall_dt, float ros_dt) void TFDisplay::updateFrames() { typedef std::vector V_string; - V_string frames; - context_->getFrameManager()->getTFBufferPtr()->_getFrameStrings(frames); + V_string frames = context_->getFrameManager()->getAllFrameNames(); std::sort(frames.begin(), frames.end()); S_FrameInfo current_frames = createOrUpdateFrames(frames); @@ -405,77 +415,82 @@ FrameInfo * TFDisplay::createFrame(const std::string & frame) void TFDisplay::updateFrame(FrameInfo * frame) { - std::shared_ptr tf_buffer = context_->getFrameManager()->getTFBufferPtr(); - - // Check last received time so we can grey out/fade out frames that have stopped being published - tf2::TimePoint latest_time; - - std::string stripped_fixed_frame = fixed_frame_.toStdString(); - if (stripped_fixed_frame[0] == '/') { - stripped_fixed_frame = stripped_fixed_frame.substr(1); - } - try { - tf_buffer->_getLatestCommonTime( - tf_buffer->_validateFrameId("get_latest_common_time", stripped_fixed_frame), - tf_buffer->_validateFrameId("get_latest_common_time", frame->name_), - latest_time, - nullptr); - } catch (const tf2::LookupException & e) { - logTransformationException(stripped_fixed_frame, frame->name_, e.what()); - return; - } - - frame->setLastUpdate(latest_time); + auto tf_wrapper = std::dynamic_pointer_cast( + context_->getFrameManager()->getConnector().lock()); - double age = tf2::durationToSec(tf2::get_now() - frame->last_update_); - double frame_timeout = frame_timeout_property_->getFloat(); - if (age > frame_timeout) { - frame->setVisible(false); - return; - } - frame->updateColorForAge(age, frame_timeout); + if (tf_wrapper) { + std::shared_ptr tf_buffer = tf_wrapper->getBuffer(); - setStatusStd(StatusProperty::Ok, frame->name_, "Transform OK"); + // Check last received time so we can grey out/fade out frames that have stopped being published + tf2::TimePoint latest_time; - Ogre::Vector3 position(0, 0, 0); - Ogre::Quaternion orientation(1.0, 0.0, 0.0, 0.0); - if (!context_->getFrameManager()->getTransform(frame->name_, position, orientation)) { - rviz_common::UniformStringStream ss; - ss << "No transform from [" << frame->name_ << "] to [" << fixed_frame_.toStdString() << "]"; - setStatusStd(StatusProperty::Warn, frame->name_, ss.str()); - frame->setVisible(false); - return; - } + std::string stripped_fixed_frame = fixed_frame_.toStdString(); + if (stripped_fixed_frame[0] == '/') { + stripped_fixed_frame = stripped_fixed_frame.substr(1); + } + try { + tf_buffer->_getLatestCommonTime( + tf_buffer->_validateFrameId("get_latest_common_time", stripped_fixed_frame), + tf_buffer->_validateFrameId("get_latest_common_time", frame->name_), + latest_time, + nullptr); + } catch (const tf2::LookupException & e) { + logTransformationException(stripped_fixed_frame, frame->name_, e.what()); + return; + } - frame->updatePositionAndOrientation(position, orientation, scale_property_->getFloat()); - frame->setNamesVisible(show_names_property_->getBool()); - frame->setAxesVisible(show_axes_property_->getBool()); + frame->setLastUpdate(latest_time); - std::string old_parent = frame->parent_; - frame->parent_.clear(); - bool has_parent = tf_buffer->_getParent(frame->name_, tf2::TimePointZero, frame->parent_); - if (has_parent) { - if (hasNoTreePropertyOrParentChanged(frame, old_parent)) { - updateParentTreeProperty(frame); + double age = tf2::durationToSec(tf2::get_now() - frame->last_update_); + double frame_timeout = frame_timeout_property_->getFloat(); + if (age > frame_timeout) { + frame->setVisible(false); + return; + } + frame->updateColorForAge(age, frame_timeout); + + setStatusStd(StatusProperty::Ok, frame->name_, "Transform OK"); + + Ogre::Vector3 position(0, 0, 0); + Ogre::Quaternion orientation(1.0, 0.0, 0.0, 0.0); + if (!context_->getFrameManager()->getTransform(frame->name_, position, orientation)) { + rviz_common::UniformStringStream ss; + ss << "No transform from [" << frame->name_ << "] to [" << fixed_frame_.toStdString() << "]"; + setStatusStd(StatusProperty::Warn, frame->name_, ss.str()); + frame->setVisible(false); + return; } - updateRelativePositionAndOrientation(frame, tf_buffer); - - if (show_arrows_property_->getBool()) { - updateParentArrowIfTransformExists(frame, position); + frame->updatePositionAndOrientation(position, orientation, scale_property_->getFloat()); + frame->setNamesVisible(show_names_property_->getBool()); + frame->setAxesVisible(show_axes_property_->getBool()); + + std::string old_parent = frame->parent_; + frame->parent_.clear(); + bool has_parent = tf_buffer->_getParent(frame->name_, tf2::TimePointZero, frame->parent_); + if (has_parent) { + if (hasNoTreePropertyOrParentChanged(frame, old_parent)) { + updateParentTreeProperty(frame); + } + + updateRelativePositionAndOrientation(frame, tf_buffer); + + if (show_arrows_property_->getBool()) { + updateParentArrowIfTransformExists(frame, position); + } else { + frame->setParentArrowVisible(false); + } } else { + if (hasNoTreePropertyOrParentChanged(frame, old_parent)) { + frame->updateTreeProperty(tree_category_); + } + frame->setParentArrowVisible(false); } - } else { - if (hasNoTreePropertyOrParentChanged(frame, old_parent)) { - frame->updateTreeProperty(tree_category_); - } - frame->setParentArrowVisible(false); + frame->parent_property_->setStdString(frame->parent_); + frame->selection_handler_->setParentName(frame->parent_); } - - frame->parent_property_->setStdString(frame->parent_); - frame->selection_handler_->setParentName(frame->parent_); } void TFDisplay::updateParentTreeProperty(FrameInfo * frame) const diff --git a/rviz_default_plugins/src/rviz_default_plugins/transformation/tf_frame_transformer.cpp b/rviz_default_plugins/src/rviz_default_plugins/transformation/tf_frame_transformer.cpp new file mode 100644 index 000000000..d39f9e2e5 --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/transformation/tf_frame_transformer.cpp @@ -0,0 +1,170 @@ +/* + * 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 "rviz_default_plugins/transformation/tf_frame_transformer.hpp" + +#include +#include +#include + +#include "rviz_common/logging.hpp" +#include "rviz_common/transformation/frame_transformer.hpp" +#include "rviz_common/transformation/ros_helpers/ros_conversion_helpers.hpp" + +namespace rviz_default_plugins +{ +namespace transformation +{ + +TFFrameTransformer::TFFrameTransformer() +: tf_wrapper_(std::make_shared()) +{} + +TFFrameTransformer::TFFrameTransformer(std::shared_ptr wrapper) +: tf_wrapper_(wrapper) +{} + +rviz_common::transformation::PoseStamped TFFrameTransformer::transform( + const rviz_common::transformation::PoseStamped & pose_in, const std::string & target_frame) +{ + geometry_msgs::msg::PoseStamped out_pose; + geometry_msgs::msg::PoseStamped in_pose = + rviz_common::transformation::ros_helpers::toRosPoseStamped(pose_in); + try { + tf_wrapper_->transform(in_pose, out_pose, target_frame); + return rviz_common::transformation::ros_helpers::fromRosPoseStamped(out_pose); + } catch (const tf2::LookupException & exception) { + std::string prefix = "[tf2::LookupException]: "; + throw rviz_common::transformation::FrameTransformerException( + std::string(prefix + exception.what()).c_str()); + } catch (const tf2::ConnectivityException & exception) { + std::string prefix = "[tf2::ConnectivityException]: "; + throw rviz_common::transformation::FrameTransformerException( + std::string(prefix + exception.what()).c_str()); + } catch (const tf2::ExtrapolationException & exception) { + std::string prefix = "[tf2::ExtrapolationException]: "; + throw rviz_common::transformation::FrameTransformerException( + std::string(prefix + exception.what()).c_str()); + } catch (const tf2::InvalidArgumentException & exception) { + std::string prefix = "[tf2::InvalidArgumentException]: "; + throw rviz_common::transformation::FrameTransformerException( + std::string(prefix + exception.what()).c_str()); + } +} + +bool TFFrameTransformer::transformIsAvailable( + const std::string & target_frame, const std::string & source_frame) +{ + try { + tf_wrapper_->lookupTransform(target_frame, source_frame, tf2::TimePointZero); + return true; + } catch (const tf2::LookupException & exception) { + (void) exception; + return false; + } catch (const tf2::ConnectivityException & exception) { + (void) exception; + return false; + } catch (const tf2::ExtrapolationException & exception) { + (void) exception; + return false; + } catch (const tf2::InvalidArgumentException & exception) { + (void) exception; + return false; + } +} + +bool TFFrameTransformer::transformHasProblems( + const std::string & source_frame, + const std::string & target_frame, + const rclcpp::Time & time, + std::string & error) +{ + std::string tf_error; + tf2::TimePoint tf2_time(std::chrono::nanoseconds(time.nanoseconds())); + bool transform_succeeded = tf_wrapper_->canTransform( + target_frame, source_frame, tf2_time, tf_error); + if (transform_succeeded) { + return false; + } + + bool fixed_frame_ok = !frameHasProblems(target_frame, error); + bool ok = fixed_frame_ok && !frameHasProblems(source_frame, error); + + if (ok) { + error = "No transform to fixed frame [" + target_frame + "]. TF error: [" + tf_error + "]"; + return true; + } + + error = fixed_frame_ok ? + "For frame [" + source_frame + "]: " + error : + "For frame [" + source_frame + "]: Fixed " + error; + + return true; +} + +bool TFFrameTransformer::frameHasProblems(const std::string & frame, std::string & error) +{ + if (!tf_wrapper_->frameExists(frame)) { + error = "Frame [" + frame + "] does not exist"; + return true; + } + + return false; +} + +rviz_common::transformation::TransformationLibraryConnector::WeakPtr +TFFrameTransformer::getConnector() +{ + return tf_wrapper_; +} + +void TFFrameTransformer::initialize( + rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node, + rclcpp::Clock::SharedPtr clock) +{ + tf_wrapper_->initialize(clock, rviz_ros_node, true); +} + +void TFFrameTransformer::clear() +{ + tf_wrapper_->clear(); +} + +std::vector TFFrameTransformer::getAllFrameNames() +{ + return tf_wrapper_->getFrameStrings(); +} + +} // namespace transformation +} // namespace rviz_default_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS( + rviz_default_plugins::transformation::TFFrameTransformer, + rviz_common::transformation::FrameTransformer) diff --git a/rviz_default_plugins/src/rviz_default_plugins/transformation/tf_wrapper.cpp b/rviz_default_plugins/src/rviz_default_plugins/transformation/tf_wrapper.cpp new file mode 100644 index 000000000..c92953544 --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/transformation/tf_wrapper.cpp @@ -0,0 +1,109 @@ +/* + * 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 "rviz_default_plugins/transformation/tf_wrapper.hpp" + +#include +#include +#include + +namespace rviz_default_plugins +{ +namespace transformation +{ + +TFWrapper::TFWrapper() +: buffer_(nullptr), tf_listener_(nullptr) +{} + +void TFWrapper::transform( + const geometry_msgs::msg::PoseStamped & pose_in, + geometry_msgs::msg::PoseStamped & pose_out, + const std::string & frame) +{ + buffer_->transform(pose_in, pose_out, frame); +} + +geometry_msgs::msg::TransformStamped TFWrapper::lookupTransform( + const std::string & target_frame, + const std::string & source_frame, + const tf2::TimePoint & time) +{ + return buffer_->lookupTransform(target_frame, source_frame, time); +} + +bool TFWrapper::canTransform( + const std::string & fixed_frame, + const std::string & frame, + tf2::TimePoint time, + std::string & error) +{ + return buffer_->canTransform(fixed_frame, frame, time, &error); +} + +std::vector TFWrapper::getFrameStrings() +{ + std::vector frames; + buffer_->_getFrameStrings(frames); + return frames; +} + +bool TFWrapper::frameExists(const std::string & frame) +{ + return buffer_->_frameExists(frame); +} + +std::shared_ptr TFWrapper::getBuffer() +{ + return buffer_; +} + +void TFWrapper::initialize( + rclcpp::Clock::SharedPtr clock, + rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node, + bool using_dedicated_thread) +{ + initializeBuffer(clock, using_dedicated_thread); + tf_listener_ = std::make_shared( + *buffer_, rviz_ros_node.lock()->get_raw_node(), false); +} + +void TFWrapper::initializeBuffer(rclcpp::Clock::SharedPtr clock, bool using_dedicated_thread) +{ + buffer_ = std::make_shared(clock); + buffer_->setUsingDedicatedThread(using_dedicated_thread); +} + +void TFWrapper::clear() +{ + buffer_->clear(); +} + +} // namespace transformation +} // namespace rviz_default_plugins diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_common_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_common_test.cpp index 47fad96c8..218820c37 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_common_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_common_test.cpp @@ -229,3 +229,10 @@ TEST_F(PointCloudCommonTestFixture, auto point_clouds = rviz_rendering::findAllPointClouds(scene_manager_->getRootSceneNode()); ASSERT_THAT(point_clouds.size(), Eq(0u)); } + +int main(int argc, char ** argv) +{ + QApplication app(argc, argv); + InitGoogleMock(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/mock_display_context.hpp b/rviz_default_plugins/test/rviz_default_plugins/mock_display_context.hpp index 5a144f357..4deff1d18 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/mock_display_context.hpp +++ b/rviz_default_plugins/test/rviz_default_plugins/mock_display_context.hpp @@ -53,6 +53,7 @@ class MockDisplayContext : public rviz_common::DisplayContext getSelectionManager, std::shared_ptr()); MOCK_CONST_METHOD0(getViewPicker, std::shared_ptr()); MOCK_CONST_METHOD0(getFrameManager, rviz_common::FrameManagerIface * ()); + MOCK_METHOD0(getTransformationManager, rviz_common::transformation::TransformationManager * ()); MOCK_CONST_METHOD0(getFixedFrame, QString()); MOCK_CONST_METHOD0(getFrameCount, uint64_t()); MOCK_CONST_METHOD0(getDisplayFactory, rviz_common::DisplayFactory * ()); diff --git a/rviz_default_plugins/test/rviz_default_plugins/mock_frame_manager.hpp b/rviz_default_plugins/test/rviz_default_plugins/mock_frame_manager.hpp index 91c90d25c..a3afd597a 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/mock_frame_manager.hpp +++ b/rviz_default_plugins/test/rviz_default_plugins/mock_frame_manager.hpp @@ -36,8 +36,10 @@ #include #include +#include #include "rviz_common/frame_manager_iface.hpp" +#include "rviz_common/transformation/frame_transformer.hpp" class MockFrameManager : public rviz_common::FrameManagerIface { @@ -64,9 +66,13 @@ class MockFrameManager : public rviz_common::FrameManagerIface MOCK_METHOD2(transformHasProblems, bool(const std::string &, std::string &)); MOCK_METHOD3(transformHasProblems, bool(const std::string &, rclcpp::Time, std::string &)); MOCK_METHOD0(getFixedFrame, const std::string & ()); - MOCK_METHOD0(getTFClient, tf2_ros::TransformListener * ()); - MOCK_METHOD0(getTFClientPtr, const std::shared_ptr&()); - MOCK_METHOD0(getTFBufferPtr, const std::shared_ptr&()); + MOCK_METHOD0(getConnector, + rviz_common::transformation::TransformationLibraryConnector::WeakPtr()); + MOCK_METHOD0(getTransformer, std::shared_ptr()); + MOCK_METHOD0(getAllFrameNames, std::vector()); + MOCK_METHOD1( + setTransformerPlugin, + void(std::shared_ptr transformer)); MOCK_METHOD0(fixedFrameChanged, void()); }; diff --git a/rviz_default_plugins/test/rviz_default_plugins/transformation/frame_transformer_tf_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/transformation/frame_transformer_tf_test.cpp new file mode 100644 index 000000000..3a1f7f36c --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/transformation/frame_transformer_tf_test.cpp @@ -0,0 +1,146 @@ +/* + * 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 + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/point.hpp" + +#include "rviz_default_plugins/transformation/tf_frame_transformer.hpp" + +class FrameTransformerTfFixture : public testing::Test +{ +public: + FrameTransformerTfFixture() + { + auto clock = std::make_shared(RCL_ROS_TIME); + tf_wrapper_ = std::make_shared(); + tf_wrapper_->initializeBuffer(clock, false); + tf_transformer_ = std::make_unique( + tf_wrapper_); + } + + geometry_msgs::msg::TransformStamped getTransformStamped( + std::string frame = "frame", std::string fixed_frame = "fixed_frame") + { + geometry_msgs::msg::TransformStamped transform_stamped; + std_msgs::msg::Header header; + header.frame_id = fixed_frame; + geometry_msgs::msg::Transform transform; + transform.rotation = geometry_msgs::msg::Quaternion(); + transform.rotation.w = 1; + transform.translation = geometry_msgs::msg::Vector3(); + + transform_stamped.header = header; + transform_stamped.child_frame_id = frame; + transform_stamped.transform = transform; + + return transform_stamped; + } + + geometry_msgs::msg::PoseStamped getPoseStamped() + { + std_msgs::msg::Header header; + header.frame_id = "pose_frame"; + geometry_msgs::msg::Pose pose; + pose.position = geometry_msgs::msg::Point(); + pose.orientation = geometry_msgs::msg::Quaternion(); + pose.orientation.w = 1; + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header = header; + pose_stamped.pose = pose; + + return pose_stamped; + } + + std::shared_ptr tf_wrapper_; + std::unique_ptr tf_transformer_; +}; + +TEST_F(FrameTransformerTfFixture, frameHasProblems_returns_true_if_frame_does_not_exist) { + std::string error; + EXPECT_TRUE(tf_transformer_->frameHasProblems("any_frame", error)); +} + +TEST_F(FrameTransformerTfFixture, frameHasProblems_returns_false_if_frame_does_exist) { + tf_wrapper_->getBuffer()->setTransform(getTransformStamped(), "test", true); + + std::string error; + EXPECT_FALSE(tf_transformer_->frameHasProblems("fixed_frame", error)); + EXPECT_FALSE(tf_transformer_->frameHasProblems("frame", error)); +} + +TEST_F(FrameTransformerTfFixture, transformHasProblems_returns_true_if_frame_does_not_exist) { + tf_wrapper_->getBuffer()->setTransform(getTransformStamped(), "test", true); + + std::string error; + std::string expected_error = "For frame [another_frame]: Frame [another_frame] does not exist"; + EXPECT_TRUE(tf_transformer_->transformHasProblems( + "another_frame", "fixed_frame", rclcpp::Clock().now(), error)); + EXPECT_THAT(error, testing::StrEq(expected_error)); +} + +TEST_F(FrameTransformerTfFixture, transformHasProblems_returns_true_if_fixed_frame_does_not_exist) { + tf_wrapper_->getBuffer()->setTransform(getTransformStamped(), "test", true); + + std::string error; + std::string expected_error = + "For frame [frame]: Fixed Frame [another_fixed_frame] does not exist"; + EXPECT_TRUE(tf_transformer_->transformHasProblems( + "frame", "another_fixed_frame", rclcpp::Clock().now(), error)); + EXPECT_THAT(error, testing::StrEq(expected_error)); +} + +TEST_F(FrameTransformerTfFixture, transformHasProblems_returns_true_if_transform_does_not_exist) { + tf_wrapper_->getBuffer()->setTransform(getTransformStamped(), "test", true); + tf_wrapper_->getBuffer()->setTransform(getTransformStamped("third_frame", + "fourth_frame"), "test", true); + + std::string error; + std::string partial_expected_error = "No transform to fixed frame"; + EXPECT_TRUE( + tf_transformer_->transformHasProblems( + "frame", "fourth_frame", rclcpp::Clock().now(), error)); + EXPECT_THAT(error, testing::HasSubstr(partial_expected_error)); + EXPECT_THAT(error, testing::HasSubstr("fourth_frame")); + EXPECT_THAT(error, testing::HasSubstr("frame")); +} + +TEST_F(FrameTransformerTfFixture, transformHasProblems_returns_false_if_transform_exists) { + tf_wrapper_->getBuffer()->setTransform(getTransformStamped(), "test", true); + + std::string error; + EXPECT_FALSE(tf_transformer_->transformHasProblems( + "frame", "fixed_frame", rclcpp::Clock().now(), error)); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/transformation/mock_frame_transformer.hpp b/rviz_default_plugins/test/rviz_default_plugins/transformation/mock_frame_transformer.hpp new file mode 100644 index 000000000..6af598ea1 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/transformation/mock_frame_transformer.hpp @@ -0,0 +1,62 @@ +/* + * 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__TRANSFORMATION__MOCK_FRAME_TRANSFORMER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__TRANSFORMATION__MOCK_FRAME_TRANSFORMER_HPP_ + +#include + +#include +#include +#include + +#include "rviz_common/transformation/frame_transformer.hpp" + +class MockFrameTransformer : public rviz_common::transformation::FrameTransformer +{ +public: + MOCK_METHOD2(initialize, void( + rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr, rclcpp::Clock::SharedPtr)); + MOCK_METHOD0(clear, void()); + MOCK_METHOD0(getAllFrameNames, std::vector()); + MOCK_METHOD2( + transform, rviz_common::transformation::PoseStamped( + const rviz_common::transformation::PoseStamped &, const std::string &)); + MOCK_METHOD2(transformIsAvailable, bool(const std::string &, const std::string &)); + MOCK_METHOD4( + transformHasProblems, + bool(const std::string &, const std::string &, const rclcpp::Time &, std::string &)); + MOCK_METHOD2(frameHasProblems, bool(const std::string &, std::string &)); + MOCK_METHOD0(getConnector, + rviz_common::transformation::TransformationLibraryConnector::WeakPtr()); +}; + +#endif // RVIZ_DEFAULT_PLUGINS__TRANSFORMATION__MOCK_FRAME_TRANSFORMER_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/transformation/transformer_guard_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/transformation/transformer_guard_test.cpp new file mode 100644 index 000000000..2aee8ac81 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/transformation/transformer_guard_test.cpp @@ -0,0 +1,160 @@ +/* + * 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 // NOLINT + +#include "tf2_ros/buffer.h" + +#include "rviz_common/display.hpp" +#include "rviz_common/transformation/frame_transformer.hpp" + +#include "rviz_default_plugins/transformation/transformer_guard.hpp" +#include "rviz_default_plugins/transformation/tf_frame_transformer.hpp" +#include "rviz_default_plugins/transformation/tf_wrapper.hpp" + +#include "../displays/display_test_fixture.hpp" +#include "../mock_display_context.hpp" +#include "./mock_frame_transformer.hpp" + +using namespace ::testing; // NOLINT + +class TransformerGuardTestFixture : public DisplayTestFixture +{ +public: + TransformerGuardTestFixture() + { + display_ = std::make_shared(); + display_->initialize(context_.get()); + base_transformer_ = std::make_shared(); + auto clock = std::make_shared(RCL_ROS_TIME); + auto tf_wrapper = std::make_shared(); + tf_wrapper->initializeBuffer(clock, false); + tf_transformer_ = std::make_shared( + tf_wrapper); + + EXPECT_CALL(*context_, getFrameManager()).WillRepeatedly(Return(frame_manager_.get())); + EXPECT_CALL(*frame_manager_, getTransformer()).WillOnce(Return(base_transformer_)); + + transformer_guard_ = std::make_unique< + rviz_default_plugins::transformation::TransformerGuard< + rviz_default_plugins::transformation::TFFrameTransformer>>(display_.get(), "TF"); + transformer_guard_->initialize(context_.get()); + + display_->setEnabled(true); + } + + void setWrongTransformer() + { + transformer_guard_->updateDisplayAccordingToTransformerType(base_transformer_); + } + + void setCorrectTransformer() + { + transformer_guard_->updateDisplayAccordingToTransformerType(tf_transformer_); + } + + void setCorectTransformerAndEnableDisplay() + { + setCorrectTransformer(); + display_->setEnabled(true); + } + + std::shared_ptr display_; + std::unique_ptr> transformer_guard_; + std::shared_ptr base_transformer_; + std::shared_ptr tf_transformer_; +}; + +TEST_F(TransformerGuardTestFixture, initialize_makes_display_disabled_if_transformer_is_wrong) { + EXPECT_FALSE(display_->isEnabled()); +} + +TEST_F( + TransformerGuardTestFixture, + updateDisplayAccordingToTransformerType_disables_display_on_wrong_transformer) { + setCorectTransformerAndEnableDisplay(); + + setWrongTransformer(); + + EXPECT_FALSE(display_->isEnabled()); +} + +TEST_F( + TransformerGuardTestFixture, + updateDisplayAccordingToTransformerType_enables_display_on_correct_transformer) { + setCorectTransformerAndEnableDisplay(); + + setWrongTransformer(); + setCorrectTransformer(); + + EXPECT_TRUE(display_->isEnabled()); +} + +TEST_F( + TransformerGuardTestFixture, + updateDisplayAccordingToTransformerType_keeps_display_disabled_if_it_was_disabled_by_user) { + setCorectTransformerAndEnableDisplay(); + display_->setEnabled(false); + + setWrongTransformer(); + setCorrectTransformer(); + + EXPECT_FALSE(display_->isEnabled()); +} + +TEST_F( + TransformerGuardTestFixture, + usingAllowedTransformer_returns_true_if_current_transformer_is_correct_one) { + EXPECT_CALL(*frame_manager_, getTransformer()).WillOnce(Return(tf_transformer_)); + + EXPECT_TRUE(transformer_guard_->checkTransformer()); +} + +TEST_F( + TransformerGuardTestFixture, + usingAllowedTransformer_returns_false_if_current_transformer_is_a_wrong_one) { + EXPECT_CALL(*frame_manager_, getTransformer()).WillOnce(Return(base_transformer_)); + + EXPECT_FALSE(transformer_guard_->checkTransformer()); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + QApplication app(argc, argv); + InitGoogleMock(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/rviz_visual_testing_framework/visual_tests_default_config.rviz b/rviz_visual_testing_framework/visual_tests_default_config.rviz index 2ae1aab1f..044ed4c7d 100644 --- a/rviz_visual_testing_framework/visual_tests_default_config.rviz +++ b/rviz_visual_testing_framework/visual_tests_default_config.rviz @@ -24,6 +24,9 @@ Visualization Manager: Name: root Tools: - Class: rviz_default_plugins/MoveCamera + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: diff --git a/rviz_visual_testing_framework/visual_tests_test_image_config.rviz b/rviz_visual_testing_framework/visual_tests_test_image_config.rviz index f03c72331..b0dbc796b 100644 --- a/rviz_visual_testing_framework/visual_tests_test_image_config.rviz +++ b/rviz_visual_testing_framework/visual_tests_test_image_config.rviz @@ -24,6 +24,9 @@ Visualization Manager: Name: root Tools: - Class: rviz_default_plugins/MoveCamera + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: