From 8bde8dc7ade47909ae80590e8b7016f751361759 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 20 Jul 2022 13:54:54 -0700 Subject: [PATCH 1/8] =?UTF-8?q?=F0=9F=95=90=20Tock:=20Remove=20Fortress=20?= =?UTF-8?q?deprecations?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel --- include/gz/sim/EntityComponentManager.hh | 43 - include/gz/sim/Types.hh | 17 - include/gz/sim/components/Factory.hh | 59 - include/gz/sim/detail/ComponentStorageBase.hh | 55 - .../gz/sim/detail/EntityComponentManager.hh | 35 - src/EntityComponentManager.cc | 14 - src/EntityComponentManager_TEST.cc | 77 - src/gui/plugins/CMakeLists.txt | 1 - src/gui/plugins/plot_3d/Plot3D_TEST.cc | 3 - src/gui/plugins/scene3d/CMakeLists.txt | 7 - src/gui/plugins/scene3d/GzScene3D.qml | 134 - src/gui/plugins/scene3d/GzScene3D.qrc | 5 - src/gui/plugins/scene3d/Scene3D.cc | 4088 ----------------- src/gui/plugins/scene3d/Scene3D.hh | 943 ---- src/systems/hydrodynamics/Hydrodynamics.cc | 11 +- src/systems/hydrodynamics/Hydrodynamics.hh | 2 - test/worlds/log_record_resources.sdf | 3 - 17 files changed, 1 insertion(+), 5496 deletions(-) delete mode 100644 include/gz/sim/detail/ComponentStorageBase.hh delete mode 100644 src/gui/plugins/scene3d/CMakeLists.txt delete mode 100644 src/gui/plugins/scene3d/GzScene3D.qml delete mode 100644 src/gui/plugins/scene3d/GzScene3D.qrc delete mode 100644 src/gui/plugins/scene3d/Scene3D.cc delete mode 100644 src/gui/plugins/scene3d/Scene3D.hh diff --git a/include/gz/sim/EntityComponentManager.hh b/include/gz/sim/EntityComponentManager.hh index 08778673cf..e93c3b6c54 100644 --- a/include/gz/sim/EntityComponentManager.hh +++ b/include/gz/sim/EntityComponentManager.hh @@ -201,13 +201,6 @@ namespace gz /// \return True if the provided _typeId has been created. public: bool HasComponentType(const ComponentTypeId _typeId) const; - /// \brief Check whether an entity has a specific component. - /// \param[in] _entity The entity to check. - /// \param[in] _key The component to check. - /// \return True if the component key belongs to the entity. - public: bool GZ_DEPRECATED(6) EntityHasComponent(const Entity _entity, - const ComponentKey &_key) const; - /// \brief Check whether an entity has a specific component type. /// \param[in] _entity The entity to check. /// \param[in] _typeId Component type id to check. @@ -223,14 +216,6 @@ namespace gz public: bool EntityMatches(Entity _entity, const std::set &_types) const; - /// \brief Remove a component from an entity based on a key. - /// \param[in] _entity The entity. - /// \param[in] _key A key that uniquely identifies a component. - /// \return True if the entity and component existed and the component was - /// removed. - public: bool GZ_DEPRECATED(6) RemoveComponent( - const Entity _entity, const ComponentKey &_key); - /// \brief Remove a component from an entity based on a type id. /// \param[in] _entity The entity. /// \param[in] _typeId Component's type Id. @@ -280,22 +265,6 @@ namespace gz public: template ComponentTypeT *Component(const Entity _entity); - /// \brief Get a component based on a key. - /// \param[in] _key A key that uniquely identifies a component. - /// \return The component associated with the key, or nullptr if the - /// component could not be found. - public: template - const ComponentTypeT GZ_DEPRECATED(6) * Component( - const ComponentKey &_key) const; - - /// \brief Get a mutable component based on a key. - /// \param[in] _key A key that uniquely identifies a component. - /// \return The component associated with the key, or nullptr if the - /// component could not be found. - public: template - ComponentTypeT GZ_DEPRECATED(6) * Component( - const ComponentKey &_key); - /// \brief Get a mutable component assigned to an entity based on a /// component type. If the component doesn't exist, create it and /// initialize with the given default value. @@ -341,18 +310,6 @@ namespace gz public: std::unordered_set ComponentTypes( Entity _entity) const; - /// \brief The first component instance of the specified type. - /// This function is now deprecated, and will always return nullptr. - /// \return nullptr. - public: template - const ComponentTypeT GZ_DEPRECATED(6) * First() const; - - /// \brief The first component instance of the specified type. - /// This function is now deprecated, and will always return nullptr. - /// \return nullptr. - public: template - ComponentTypeT GZ_DEPRECATED(6) * First(); - /// \brief Get an entity which matches the value of all the given /// components. For example, the following will return the entity which /// has a name component equal to "name" and has a model component: diff --git a/include/gz/sim/Types.hh b/include/gz/sim/Types.hh index 869de5425f..da38b3a8b9 100644 --- a/include/gz/sim/Types.hh +++ b/include/gz/sim/Types.hh @@ -77,32 +77,15 @@ namespace gz OneTimeChange = 2 }; - /// \brief A unique identifier for a component instance. The uniqueness - /// of a ComponentId is scoped to the component's type. - /// \sa ComponentKey. - /// \deprecated Deprecated on version 6, removed on version 7. Use - /// ComponentTypeId + Entity instead. - using ComponentId = int; - /// \brief A unique identifier for a component type. A component type /// must be derived from `components::BaseComponent` and can contain plain /// data or something more complex like `gz::math::Pose3d`. using ComponentTypeId = uint64_t; - /// \brief A key that uniquely identifies, at the global scope, a component - /// instance - /// \note On version 6, the 2nd element was changed to the entity ID. - /// \deprecated Deprecated on version 6, removed on version 7. Use - /// ComponentTypeId + Entity instead. - using ComponentKey = std::pair; - /// \brief typedef for query callbacks using EntityQueryCallback = std::function; - /// \brief Id that indicates an invalid component. - static const ComponentId kComponentIdInvalid = -1; - /// \brief Id that indicates an invalid component type. static const ComponentTypeId kComponentTypeIdInvalid = UINT64_MAX; } diff --git a/include/gz/sim/components/Factory.hh b/include/gz/sim/components/Factory.hh index 28d5d46d5a..5ea38c00b4 100644 --- a/include/gz/sim/components/Factory.hh +++ b/include/gz/sim/components/Factory.hh @@ -27,7 +27,6 @@ #include #include #include -#include #include #include #include @@ -78,58 +77,10 @@ namespace components } }; - /// \brief A base class for an object responsible for creating storages. - class StorageDescriptorBase - { - /// \brief Constructor - public: GZ_DEPRECATED(6) StorageDescriptorBase() = default; - - /// \brief Destructor - public: virtual ~StorageDescriptorBase() = default; - - /// \brief Create an instance of a storage. - /// \return Pointer to a storage. - public: virtual std::unique_ptr Create() const = 0; - }; - - /// \brief A class for an object responsible for creating storages. - /// \tparam ComponentTypeT type of component that the storage will hold. - template - class StorageDescriptor - : public StorageDescriptorBase - { - /// \brief Constructor - public: GZ_DEPRECATED(6) StorageDescriptor() = default; - - /// \brief Create an instance of a storage that holds ComponentTypeT - /// components. - /// \return Pointer to a component. - public: std::unique_ptr Create() const override - { - return std::make_unique>(); - } - }; - /// \brief A factory that generates a component based on a string type. class Factory : public gz::common::SingletonT { - /// \brief Register a component so that the factory can create instances - /// of the component and its storage based on an ID. - /// \param[in] _type Type of component to register. - /// \param[in] _compDesc Object to manage the creation of ComponentTypeT - /// objects. - /// \param[in] _storageDesc Ignored. - /// \tparam ComponentTypeT Type of component to register. - /// \deprecated See function that doesn't accept a storage - public: template - void GZ_DEPRECATED(6) Register(const std::string &_type, - ComponentDescriptorBase *_compDesc, - StorageDescriptorBase * /*_storageDesc*/) - { - this->Register(_type, _compDesc); - } - /// \brief Register a component so that the factory can create instances /// of the component based on an ID. /// \param[in] _type Type of component to register. @@ -310,16 +261,6 @@ namespace components return comp; } - /// \brief Create a new instance of a component storage. - /// \param[in] _typeId Type of component which the storage will hold. - /// \return Always returns nullptr. - /// \deprecated Storages aren't necessary anymore. - public: std::unique_ptr GZ_DEPRECATED(6) NewStorage( - const ComponentTypeId & /*_typeId*/) - { - return nullptr; - } - /// \brief Get all the registered component types by ID. /// return Vector of component IDs. public: std::vector TypeIds() const diff --git a/include/gz/sim/detail/ComponentStorageBase.hh b/include/gz/sim/detail/ComponentStorageBase.hh deleted file mode 100644 index 34b757e90f..0000000000 --- a/include/gz/sim/detail/ComponentStorageBase.hh +++ /dev/null @@ -1,55 +0,0 @@ -/* - * Copyright (C) 2018 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ -#ifndef GZ_SIM_DETAIL_COMPONENTSTORAGEBASE_HH_ -#define GZ_SIM_DETAIL_COMPONENTSTORAGEBASE_HH_ - -#include "gz/sim/Export.hh" - -namespace gz -{ - namespace sim - { - // Inline bracket to help doxygen filtering. - inline namespace GZ_SIM_VERSION_NAMESPACE { - // - /// \brief All component instances of the same type are stored - /// sequentially in memory. This is a base class for storing components - /// of a particular type. - class GZ_SIM_HIDDEN ComponentStorageBase - { - /// \brief Constructor - public: GZ_DEPRECATED(6) ComponentStorageBase() = default; - - /// \brief Destructor - public: virtual ~ComponentStorageBase() = default; - }; - - /// \brief Templated implementation of component storage. - template - class GZ_SIM_HIDDEN ComponentStorage : public ComponentStorageBase - { - /// \brief Constructor - public: explicit GZ_DEPRECATED(6) ComponentStorage() - : ComponentStorageBase() - { - } - }; - } - } -} - -#endif diff --git a/include/gz/sim/detail/EntityComponentManager.hh b/include/gz/sim/detail/EntityComponentManager.hh index 9c5c14cf78..7eea6491ba 100644 --- a/include/gz/sim/detail/EntityComponentManager.hh +++ b/include/gz/sim/detail/EntityComponentManager.hh @@ -127,23 +127,6 @@ ComponentTypeT *EntityComponentManager::Component(const Entity _entity) this->ComponentImplementation(_entity, typeId)); } -////////////////////////////////////////////////// -template -const ComponentTypeT *EntityComponentManager::Component( - const ComponentKey &_key) const -{ - return static_cast( - this->ComponentImplementation(_key.second, _key.first)); -} - -////////////////////////////////////////////////// -template -ComponentTypeT *EntityComponentManager::Component(const ComponentKey &_key) -{ - return static_cast( - this->ComponentImplementation(_key.second, _key.first)); -} - ////////////////////////////////////////////////// template ComponentTypeT *EntityComponentManager::ComponentDefault(Entity _entity, @@ -186,24 +169,6 @@ bool EntityComponentManager::SetComponentData(const Entity _entity, return comp->SetData(_data, CompareData); } -////////////////////////////////////////////////// -template -const ComponentTypeT *EntityComponentManager::First() const -{ - gzwarn << "EntityComponentManager::First is now deprecated and will always " - << "return nullptr.\n"; - return nullptr; -} - -////////////////////////////////////////////////// -template -ComponentTypeT *EntityComponentManager::First() -{ - gzwarn << "EntityComponentManager::First is now deprecated and will always " - << "return nullptr.\n"; - return nullptr; -} - ////////////////////////////////////////////////// template Entity EntityComponentManager::EntityByComponents( diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 65f4a57119..0a68fc4849 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -878,20 +878,6 @@ bool EntityComponentManager::RemoveComponent( return true; } -///////////////////////////////////////////////// -bool EntityComponentManager::RemoveComponent( - const Entity _entity, const ComponentKey &_key) -{ - return this->RemoveComponent(_entity, _key.first); -} - -///////////////////////////////////////////////// -bool EntityComponentManager::EntityHasComponent(const Entity _entity, - const ComponentKey &_key) const -{ - return this->EntityHasComponentType(_entity, _key.first); -} - ///////////////////////////////////////////////// bool EntityComponentManager::EntityHasComponentType(const Entity _entity, const ComponentTypeId &_typeId) const diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 3a7e0c1175..ac79a3860f 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -22,7 +22,6 @@ #include #include #include -#include #include "gz/sim/components/CanonicalLink.hh" #include "gz/sim/components/ChildLinkName.hh" @@ -2943,82 +2942,6 @@ TEST_P(EntityComponentManagerFixture, EXPECT_EQ(18u, manager.EntityCount()); } -///////////////////////////////////////////////// -// Check that some widely used deprecated APIs still work -TEST_P(EntityComponentManagerFixture, - GZ_UTILS_TEST_DISABLED_ON_WIN32(Deprecated)) -{ - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - - // Fail to create component for inexistent entity - EXPECT_EQ(nullptr, manager.CreateComponent(789, - IntComponent(123))); - - // Create some entities - auto eInt = manager.CreateEntity(); - auto eDouble = manager.CreateEntity(); - auto eIntDouble = manager.CreateEntity(); - EXPECT_EQ(3u, manager.EntityCount()); - - // Add components and keep their unique ComponentKeys - EXPECT_NE(nullptr, manager.CreateComponent(eInt, - IntComponent(123))); - ComponentKey cIntEInt = {IntComponent::typeId, eInt}; - - EXPECT_NE(nullptr, manager.CreateComponent(eDouble, - DoubleComponent(0.123))); - ComponentKey cDoubleEDouble = {DoubleComponent::typeId, eDouble}; - - EXPECT_NE(nullptr, manager.CreateComponent(eIntDouble, - IntComponent(456))); - ComponentKey cIntEIntDouble = {IntComponent::typeId, eIntDouble}; - - EXPECT_NE(nullptr, manager.CreateComponent(eIntDouble, - DoubleComponent(0.456))); - ComponentKey cDoubleEIntDouble = {DoubleComponent::typeId, eIntDouble}; - - // Check entities have the components - EXPECT_TRUE(manager.EntityHasComponent(eInt, cIntEInt)); - EXPECT_EQ(1u, manager.ComponentTypes(eInt).size()); - EXPECT_EQ(IntComponent::typeId, *manager.ComponentTypes(eInt).begin()); - - EXPECT_TRUE(manager.EntityHasComponent(eDouble, cDoubleEDouble)); - EXPECT_EQ(1u, manager.ComponentTypes(eDouble).size()); - EXPECT_EQ(DoubleComponent::typeId, *manager.ComponentTypes(eDouble).begin()); - - EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); - EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); - EXPECT_EQ(2u, manager.ComponentTypes(eIntDouble).size()); - auto types = manager.ComponentTypes(eIntDouble); - EXPECT_NE(types.end(), types.find(IntComponent::typeId)); - EXPECT_NE(types.end(), types.find(DoubleComponent::typeId)); - - // Remove component by key - EXPECT_TRUE(manager.RemoveComponent(eInt, cIntEInt)); - EXPECT_FALSE(manager.EntityHasComponent(eInt, cIntEInt)); - EXPECT_TRUE(manager.ComponentTypes(eInt).empty()); - - // Remove component by type id - auto typeDouble = DoubleComponent::typeId; - - EXPECT_TRUE(manager.RemoveComponent(eDouble, typeDouble)); - EXPECT_FALSE(manager.EntityHasComponent(eDouble, cDoubleEDouble)); - EXPECT_TRUE(manager.ComponentTypes(eDouble).empty()); - - // Remove component by type - EXPECT_TRUE(manager.RemoveComponent(eIntDouble)); - EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); - EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); - EXPECT_EQ(1u, manager.ComponentTypes(eIntDouble).size()); - - EXPECT_TRUE(manager.RemoveComponent(eIntDouble)); - EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); - EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); - EXPECT_EQ(0u, manager.ComponentTypes(eIntDouble).size()); - - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION -} - ////////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, PinnedEntity) { diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 5e90801b21..a6f83e8b86 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -131,7 +131,6 @@ add_subdirectory(playback_scrubber) add_subdirectory(plot_3d) add_subdirectory(plotting) add_subdirectory(resource_spawner) -add_subdirectory(scene3d) add_subdirectory(select_entities) add_subdirectory(scene_manager) add_subdirectory(shapes) diff --git a/src/gui/plugins/plot_3d/Plot3D_TEST.cc b/src/gui/plugins/plot_3d/Plot3D_TEST.cc index a689be6e81..99ffb35577 100644 --- a/src/gui/plugins/plot_3d/Plot3D_TEST.cc +++ b/src/gui/plugins/plot_3d/Plot3D_TEST.cc @@ -29,7 +29,6 @@ #include #include #include -#include #include "gz/sim/components/Joint.hh" #include "gz/sim/components/JointAxis.hh" @@ -68,9 +67,7 @@ TEST_F(Plot3D, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) app->AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); // Create GUI runner to handle sim::gui plugins - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION auto runner = new sim::GuiRunner("test"); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION runner->setParent(gui::App()); // Add plugin diff --git a/src/gui/plugins/scene3d/CMakeLists.txt b/src/gui/plugins/scene3d/CMakeLists.txt deleted file mode 100644 index d8e7a52e23..0000000000 --- a/src/gui/plugins/scene3d/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -gz_add_gui_plugin(GzScene3D - SOURCES Scene3D.cc - QT_HEADERS Scene3D.hh - PRIVATE_LINK_LIBS - ${PROJECT_LIBRARY_TARGET_NAME}-rendering - gz-utils${GZ_UTILS_VER}::gz-utils${GZ_UTILS_VER} -) diff --git a/src/gui/plugins/scene3d/GzScene3D.qml b/src/gui/plugins/scene3d/GzScene3D.qml deleted file mode 100644 index cf90843327..0000000000 --- a/src/gui/plugins/scene3d/GzScene3D.qml +++ /dev/null @@ -1,134 +0,0 @@ -/* - * Copyright (C) 2018 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ -import GzSim 1.0 as GzSim -import QtGraphicalEffects 1.0 -import QtQuick 2.9 -import QtQuick.Controls 2.2 -import QtQuick.Dialogs 1.0 -import QtQuick.Layouts 1.3 -import RenderWindow 1.0 - -Rectangle { - Layout.minimumWidth: 200 - Layout.minimumHeight: 200 - anchors.fill: parent - - /** - * True to enable gamma correction - */ - property bool gammaCorrect: false - - /** - * Get mouse position on 3D widget - */ - MouseArea { - id: mouseArea - anchors.fill: parent - hoverEnabled: true - acceptedButtons: Qt.NoButton - visible: GzScene3D.loadingError.length == 0 - onEntered: { - GzScene3D.OnFocusWindow() - } - onPositionChanged: { - GzScene3D.OnHovered(mouseArea.mouseX, mouseArea.mouseY); - } - } - - RenderWindow { - id: renderWindow - objectName: "renderWindow" - anchors.fill: parent - visible: GzScene3D.loadingError.length == 0 - - /** - * Message to be displayed over the render window - */ - property string message: "" - - Connections { - target: renderWindow - onOpenContextMenu: entityContextMenu.open(_entity, "model", - mouseArea.mouseX, mouseArea.mouseY); - } - } - - /* - * Gamma correction for sRGB output. Enabled when engine is set to ogre2 - */ - GammaAdjust { - anchors.fill: renderWindow - source: renderWindow - gamma: 2.4 - enabled: gammaCorrect - visible: gammaCorrect - } - - onParentChanged: { - if (undefined === parent) - return; - - width = Qt.binding(function() {return parent.parent.width}) - height = Qt.binding(function() {return parent.parent.height}) - } - - GzSim.EntityContextMenu { - id: entityContextMenu - anchors.fill: parent - } - - // todo(anyone) replace this with snackbar notifications - Text { - text: renderWindow.message - } - - DropArea { - anchors.fill: renderWindow - - onDropped: { - GzScene3D.OnDropped(drop.text, drag.x, drag.y) - } - } - - // pop error - Connections { - target: GzScene3D - onPopupError: errorPopup.open() - } - - Dialog { - id: errorPopup - parent: ApplicationWindow.overlay - modal: true - focus: true - x: (parent.width - width) / 2 - y: (parent.height - height) / 2 - title: "Error" - Text { - text: GzScene3D.errorPopupText - } - standardButtons: Dialog.Ok - } - - Label { - anchors.fill: parent - anchors.margins: 10 - text: GzScene3D.loadingError - visible: (GzScene3D.loadingError.length > 0); - wrapMode: Text.WordWrap - } -} diff --git a/src/gui/plugins/scene3d/GzScene3D.qrc b/src/gui/plugins/scene3d/GzScene3D.qrc deleted file mode 100644 index 65b6bce0b7..0000000000 --- a/src/gui/plugins/scene3d/GzScene3D.qrc +++ /dev/null @@ -1,5 +0,0 @@ - - - GzScene3D.qml - - diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc deleted file mode 100644 index bbba0eedd6..0000000000 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ /dev/null @@ -1,4088 +0,0 @@ -/* - * Copyright (C) 2019 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#include "Scene3D.hh" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -#include "gz/sim/components/Name.hh" -#include "gz/sim/components/RenderEngineGuiPlugin.hh" -#include "gz/sim/components/World.hh" -#include "gz/sim/EntityComponentManager.hh" -#include "gz/sim/gui/GuiEvents.hh" -#include "gz/sim/rendering/RenderUtil.hh" - -/// \brief condition variable for lockstepping video recording -/// todo(anyone) avoid using a global condition variable when we support -/// multiple viewports in the future. -std::condition_variable g_renderCv; - -Q_DECLARE_METATYPE(std::string) -Q_DECLARE_METATYPE(gz::sim::RenderSync*) - -namespace gz -{ -namespace sim -{ -inline namespace GZ_SIM_VERSION_NAMESPACE { - /// \brief Helper to store selection requests to be handled in the render - /// thread by `GzRenderer::HandleEntitySelection`. - // SelectEntities - struct SelectionHelper - { - /// \brief Entity to be selected - Entity selectEntity{kNullEntity}; - - /// \brief Deselect all entities - bool deselectAll{false}; - - /// \brief True to send an event and notify all widgets - bool sendEvent{false}; - }; - - /// \brief Private data class for GzRenderer - class GzRendererPrivate - { - // -------------------------------------------------------------- - // InteractiveViewControl - - /// \brief Orbit view controller - public: rendering::OrbitViewController orbitViewControl; - - /// \brief Ortho view controller - public: rendering::OrthoViewController orthoViewControl; - - /// \brief Camera view controller - public: rendering::ViewController *viewControl{nullptr}; - - /// \brief View controller - public: std::string viewController{"orbit"}; - - /// \brief View control focus target - public: math::Vector3d target = math::Vector3d( - math::INF_D, math::INF_D, math::INF_D); - - // -------------------------------------------------------------- - // TransformControl - - /// \brief The xyz values by which to snap the object. - public: math::Vector3d xyzSnap = math::Vector3d::One; - - /// \brief The rpy values by which to snap the object. - public: math::Vector3d rpySnap = {45, 45, 45}; - - /// \brief The scale values by which to snap the object. - public: math::Vector3d scaleSnap = math::Vector3d::One; - - /// \brief Transform controller for models - public: rendering::TransformController transformControl; - - /// \brief Transform space: local or world - public: rendering::TransformSpace transformSpace = - rendering::TransformSpace::TS_LOCAL; - - /// \brief Transform mode: none, translation, rotation, or scale - public: rendering::TransformMode transformMode = - rendering::TransformMode::TM_NONE; - - /// \brief Name of service for setting entity pose - public: std::string poseCmdService; - - /// \brief Flag to indicate whether the x key is currently being pressed - public: bool xPressed = false; - - /// \brief Flag to indicate whether the y key is currently being pressed - public: bool yPressed = false; - - /// \brief Flag to indicate whether the z key is currently being pressed - public: bool zPressed = false; - - /// \brief The starting world pose of a clicked visual. - public: gz::math::Vector3d startWorldPos = math::Vector3d::Zero; - - /// \brief Flag to keep track of world pose setting used - /// for button translating. - public: bool isStartWorldPosSet = false; - - // -------------------------------------------------------------- - // VideoRecorder - - /// \brief True to record a video from the user camera - public: bool recordVideo = false; - - /// \brief Video encoding format - public: std::string recordVideoFormat; - - /// \brief Path to save the recorded video - public: std::string recordVideoSavePath; - - /// \brief Use sim time as timestamp during video recording - /// By default (false), video encoding is done using real time. - public: bool recordVideoUseSimTime = false; - - /// \brief Lockstep gui with ECM when recording - public: bool recordVideoLockstep = false; - - /// \brief Video recorder bitrate (bps) - public: unsigned int recordVideoBitrate = 2070000; - - /// \brief Previous camera update time during video recording - /// only used in lockstep mode and recording in sim time. - public: std::chrono::steady_clock::time_point recordVideoUpdateTime; - - /// \brief Start time of video recording - public: std::chrono::steady_clock::time_point recordStartTime; - - /// \brief Video recording statistics publisher - public: transport::Node::Publisher recorderStatsPub; - - /// \brief Image from user camera - public: rendering::Image cameraImage; - - /// \brief Video encoder - public: common::VideoEncoder videoEncoder; - - // -------------------------------------------------------------- - // CameraTracking - - /// \brief Target to move the user camera to - public: std::string moveToTarget; - - /// \brief Helper object to move user camera - public: gz::rendering::MoveToHelper moveToHelper; - - /// \brief Target to follow - public: std::string followTarget; - - /// \brief Wait for follow target - public: bool followTargetWait = false; - - /// \brief Offset of camera from target being followed - public: math::Vector3d followOffset = math::Vector3d(-5, 0, 3); - - /// \brief Flag to indicate the follow offset needs to be updated - public: bool followOffsetDirty = false; - - /// \brief Flag to indicate the follow offset has been updated - public: bool newFollowOffset = true; - - /// \brief Follow P gain - public: double followPGain = 0.01; - - /// \brief True follow the target at an offset that is in world frame, - /// false to follow in target's local frame - public: bool followWorldFrame = false; - - /// \brief The pose set from the move to pose service. - public: std::optional moveToPoseValue; - - /// \brief Last move to animation time - public: std::chrono::time_point prevMoveToTime; - - // -------------------------------------------------------------- - // VisualizationCapabilities - - /// \brief Target to view as transparent - public: std::string viewTransparentTarget; - - /// \brief Target to view center of mass - public: std::string viewCOMTarget; - - /// \brief Target to view inertia - public: std::string viewInertiaTarget; - - /// \brief Target to view joints - public: std::string viewJointsTarget; - - /// \brief Target to view wireframes - public: std::string viewWireframesTarget; - - /// \brief Target to view collisions - public: std::string viewCollisionsTarget; - - // -------------------------------------------------------------- - // SelectEntities - - /// \brief Helper object to select entities. Only the latest selection - /// request is kept. - public: SelectionHelper selectionHelper; - - // -------------------------------------------------------------- - // Spawn - - /// \brief Flag for indicating whether we are spawning or not. - public: bool isSpawning = false; - - /// \brief Flag for indicating whether the user is currently placing a - /// resource with the shapes plugin or not - public: bool isPlacing = false; - - /// \brief The SDF string of the resource to be used with plugins that spawn - /// entities. - public: std::string spawnSdfString; - - /// \brief Path of an SDF file, to be used with plugins that spawn entities. - public: std::string spawnSdfPath; - - /// \brief The pose of the spawn preview. - public: gz::math::Pose3d spawnPreviewPose = - gz::math::Pose3d::Zero; - - /// \brief The visual generated from the spawnSdfString / spawnSdfPath - public: rendering::NodePtr spawnPreview = nullptr; - - /// \brief A record of the ids currently used by the entity spawner - /// for easy deletion of visuals later - public: std::vector previewIds; - - /// \brief Name of service for creating entity - public: std::string createCmdService; - - // -------------------------------------------------------------- - // ViewAngle - - /// \brief Flag for indicating whether we are in view angle mode or not - public: bool viewAngle = false; - - /// \brief The pose set during a view angle button press that holds - /// the pose the camera should assume relative to the entit(y/ies). - /// The vector (0, 0, 0) indicates to return the camera back to the home - /// pose originally loaded from the sdf. - public: math::Vector3d viewAngleDirection = math::Vector3d::Zero; - - // -------------------------------------------------------------- - // Common to various plugins - - /// \brief Flag to indicate if mouse event is dirty - public: bool mouseDirty = false; - - /// \brief Flag to indicate if hover event is dirty - public: bool hoverDirty = false; - - /// \brief Mouse event - public: common::MouseEvent mouseEvent; - - /// \brief Key event - public: common::KeyEvent keyEvent; - - /// \brief Mouse move distance since last event. - public: math::Vector2d drag; - - /// \brief Mutex to protect mouse events - public: std::mutex mutex; - - /// \brief User camera - public: rendering::CameraPtr camera; - - /// \brief Atomic bool indicating whether the dropdown menu - /// is currently enabled or disabled. - public: std::atomic_bool dropdownMenuEnabled = true; - - /// \brief The currently hovered mouse position in screen coordinates - public: math::Vector2i mouseHoverPos = math::Vector2i::Zero; - - /// \brief Ray query for mouse clicks - public: rendering::RayQueryPtr rayQuery; - - /// \brief Rendering utility - public: RenderUtil renderUtil; - - /// \brief Transport node for making transform control requests - public: transport::Node node; - - /// \brief Where the mouse left off - used to continue translating - /// smoothly when switching axes through keybinding and clicking - /// Updated on an x, y, or z, press or release and a mouse press - public: math::Vector2i mousePressPos = math::Vector2i::Zero; - - /// \brief Flag to indicate whether the escape key has been released. - public: bool escapeReleased = false; - - /// \brief ID of thread where render calls can be made. - public: std::thread::id renderThreadId; - }; - - /// \brief Qt and Ogre rendering is happening in different threads - /// The original sample 'textureinthread' from Qt used a double-buffer - /// scheme so that the worker (Ogre) thread write to FBO A, while - /// Qt is displaying FBO B. - /// - /// However Qt's implementation doesn't handle all the edge cases - /// (like resizing a window), and also it increases our VRAM - /// consumption in multiple ways (since we have to double other - /// resources as well or re-architect certain parts of the code - /// to avoid it) - /// - /// Thus we just serialize both threads so that when Qt reaches - /// drawing preparation, it halts and Ogre worker thread starts rendering, - /// then resumes when Ogre is done. - /// - /// This code is admitedly more complicated than it should be - /// because Qt's synchronization using signals and slots causes - /// deadlocks when other means of synchronization are introduced. - /// The whole threaded loop should be rewritten. - /// - /// All RenderSync does is conceptually: - /// - /// \code - /// TextureNode::PrepareNode() - /// { - /// renderSync.WaitForWorkerThread(); // Qt thread - /// // WaitForQtThreadAndBlock(); - /// // Now worker thread begins executing what's between - /// // ReleaseQtThreadFromBlock(); - /// continue with qt code... - /// } - /// \endcode - /// - /// - /// For more info see - /// https://github.com/gazebosim/gz-rendering/issues/304 - class RenderSync - { - /// \brief Cond. variable to synchronize rendering on specific events - /// (e.g. texture resize) or for debugging (e.g. keep - /// all API calls sequential) - public: std::mutex mutex; - - /// \brief Cond. variable to synchronize rendering on specific events - /// (e.g. texture resize) or for debugging (e.g. keep - /// all API calls sequential) - public: std::condition_variable cv; - - public: enum class RenderStallState - { - /// Qt is stuck inside WaitForWorkerThread - /// Worker thread can proceed - WorkerCanProceed, - /// Qt is stuck inside WaitForWorkerThread - /// Worker thread is between WaitForQtThreadAndBlock - /// and ReleaseQtThreadFromBlock - WorkerIsProceeding, - /// Worker is stuck inside WaitForQtThreadAndBlock - /// Qt can proceed - QtCanProceed, - /// Do not block - ShuttingDown, - }; - - /// \brief See TextureNode::RenderSync::RenderStallState - public: RenderStallState renderStallState = - RenderStallState::QtCanProceed /*GUARDED_BY(sharedRenderMutex)*/; - - /// \brief Must be called from worker thread when we want to block - /// \param[in] lock Acquired lock. Must be based on this->mutex - public: void WaitForQtThreadAndBlock(std::unique_lock &_lock); - - /// \brief Must be called from worker thread when we are done - /// \param[in] lock Acquired lock. Must be based on this->mutex - public: void ReleaseQtThreadFromBlock(std::unique_lock &_lock); - - /// \brief Must be called from Qt thread periodically - public: void WaitForWorkerThread(); - - /// \brief Must be called from GUI thread when shutting down - public: void Shutdown(); - }; - - /// \brief Private data class for RenderWindowItem - class RenderWindowItemPrivate - { - /// \brief Keep latest mouse event - public: common::MouseEvent mouseEvent; - - /// \brief Render thread - public : RenderThread *renderThread = nullptr; - - /// \brief See RenderSync - public: RenderSync renderSync; - - //// \brief Set to true after the renderer is initialized - public: bool rendererInit = false; - - //// \brief List of threads - public: static QList threads; - }; - - /// \brief Private data class for Scene3D - class Scene3DPrivate - { - // -------------------------------------------------------------- - // TransformControl - - /// \brief Transform mode service - public: std::string transformModeService; - - // -------------------------------------------------------------- - // VideoRecorder - - /// \brief Record video service - public: std::string recordVideoService; - - /// \brief lockstep ECM updates with rendering - public: bool recordVideoLockstep = false; - - /// \brief True to indicate video recording in progress - public: bool recording = false; - - /// \brief mutex to protect the recording variable - public: std::mutex recordMutex; - - // -------------------------------------------------------------- - // CameraTracking - - /// \brief Move to service - public: std::string moveToService; - - /// \brief Follow service - public: std::string followService; - - /// \brief Follow offset service - public: std::string followOffsetService; - - /// \brief Move to pose service - public: std::string moveToPoseService; - - /// \brief Camera pose topic - public: std::string cameraPoseTopic; - - /// \brief Camera pose publisher - public: transport::Node::Publisher cameraPosePub; - - // -------------------------------------------------------------- - // VisualizationCapabilities - - /// \brief View transparent service - public: std::string viewTransparentService; - - /// \brief View center of mass service - public: std::string viewCOMService; - - /// \brief View inertia service - public: std::string viewInertiaService; - - /// \brief View joints service - public: std::string viewJointsService; - - /// \brief View wireframes service - public: std::string viewWireframesService; - - /// \brief View collisions service - public: std::string viewCollisionsService; - - // -------------------------------------------------------------- - // InteractiveViewControl - - /// \brief Camera view control service - public: std::string cameraViewControlService; - - // -------------------------------------------------------------- - // GzSceneManager - - /// \brief Rendering utility - public: RenderUtil *renderUtil = nullptr; - - // -------------------------------------------------------------- - // ViewAngle - - /// \brief View angle service - public: std::string viewAngleService; - - // -------------------------------------------------------------- - // Common to various plugins - - /// \brief Transport node - public: transport::Node node; - - /// \brief Name of the world - public: std::string worldName; - - /// \brief mutex to protect the render condition variable - /// Used when recording in lockstep mode. - public: std::mutex renderMutex; - - /// \brief Text for popup error message - public: QString errorPopupText; - }; -} -} -} - -using namespace gz; -using namespace sim; - -QList RenderWindowItemPrivate::threads; - -///////////////////////////////////////////////// -void RenderSync::WaitForQtThreadAndBlock(std::unique_lock &_lock) -{ - this->cv.wait(_lock, [this] - { return this->renderStallState == RenderStallState::WorkerCanProceed || - this->renderStallState == RenderStallState::ShuttingDown; }); - - this->renderStallState = RenderStallState::WorkerIsProceeding; -} - -///////////////////////////////////////////////// -void RenderSync::ReleaseQtThreadFromBlock(std::unique_lock &_lock) -{ - this->renderStallState = RenderStallState::QtCanProceed; - _lock.unlock(); - this->cv.notify_one(); -} - -///////////////////////////////////////////////// -void RenderSync::WaitForWorkerThread() -{ - std::unique_lock lock(this->mutex); - - // Wait until we're clear to go - this->cv.wait( lock, [this] - { - return this->renderStallState == RenderStallState::QtCanProceed || - this->renderStallState == RenderStallState::ShuttingDown; - } ); - - // Worker thread asked us to wait! - this->renderStallState = RenderStallState::WorkerCanProceed; - lock.unlock(); - // Wake up worker thread - this->cv.notify_one(); - lock.lock(); - - // Wait until we're clear to go - this->cv.wait( lock, [this] - { - return this->renderStallState == RenderStallState::QtCanProceed || - this->renderStallState == RenderStallState::ShuttingDown; - } ); -} - -///////////////////////////////////////////////// -void RenderSync::Shutdown() -{ - { - std::unique_lock lock(this->mutex); - - this->renderStallState = RenderStallState::ShuttingDown; - - lock.unlock(); - this->cv.notify_one(); - } -} - -///////////////////////////////////////////////// -GzRenderer::GzRenderer() - : dataPtr(new GzRendererPrivate) -{ - this->dataPtr->moveToHelper.SetInitCameraPose(this->cameraPose); - - // recorder stats topic - std::string recorderStatsTopic = "/gui/record_video/stats"; - this->dataPtr->recorderStatsPub = - this->dataPtr->node.Advertise(recorderStatsTopic); - gzmsg << "Video recorder stats topic advertised on [" - << recorderStatsTopic << "]" << std::endl; -} - - -///////////////////////////////////////////////// -GzRenderer::~GzRenderer() = default; - -//////////////////////////////////////////////// -RenderUtil *GzRenderer::RenderUtil() const -{ - return &this->dataPtr->renderUtil; -} - -///////////////////////////////////////////////// -void GzRenderer::Render(RenderSync *_renderSync) -{ - rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene(); - if (!scene) - { - gzwarn << "Scene is null. The render step will not occur in Scene3D." - << std::endl; - return; - } - - this->dataPtr->renderThreadId = std::this_thread::get_id(); - - GZ_PROFILE_THREAD_NAME("RenderThread"); - GZ_PROFILE("GzRenderer::Render"); - - std::unique_lock lock(_renderSync->mutex); - _renderSync->WaitForQtThreadAndBlock(lock); - - if (this->textureDirty) - { - // TODO(anyone) If SwapFromThread gets implemented, - // then we only need to lock when texture is dirty - // (but we still need to lock the whole routine if - // debugging from RenderDoc or if user is not willing - // to sacrifice VRAM) - // - // std::unique_lock lock(renderSync->mutex); - // _renderSync->WaitForQtThreadAndBlock(lock); - this->dataPtr->camera->SetImageWidth(this->textureSize.width()); - this->dataPtr->camera->SetImageHeight(this->textureSize.height()); - this->dataPtr->camera->SetAspectRatio(this->textureSize.width() / - static_cast(this->textureSize.height())); - // setting the size should cause the render texture to be rebuilt - { - GZ_PROFILE("GzRenderer::Render Pre-render camera"); - this->dataPtr->camera->Update(); - } - // mark mouse dirty to force update view projection in HandleMouseEvent - this->dataPtr->mouseDirty = true; - - this->textureDirty = false; - - // TODO(anyone) See SwapFromThread comments - // _renderSync->ReleaseQtThreadFromBlock(lock); - } - - // texture id could change so get the value in every render update - this->textureId = this->dataPtr->camera->RenderTextureGLId(); - - // update the scene - this->dataPtr->renderUtil.SetTransformActive( - this->dataPtr->transformControl.Active()); - this->dataPtr->renderUtil.Update(); - - // view control - this->HandleMouseEvent(); - - // Entity selection - this->HandleEntitySelection(); - - // reset follow mode if target node got removed - if (!this->dataPtr->followTarget.empty()) - { - rendering::NodePtr target = scene->NodeByName(this->dataPtr->followTarget); - if (!target && !this->dataPtr->followTargetWait) - { - this->dataPtr->camera->SetFollowTarget(nullptr); - this->dataPtr->camera->SetTrackTarget(nullptr); - this->dataPtr->followTarget.clear(); - emit FollowTargetChanged(std::string(), false); - } - } - - // check if recording is in lockstep mode and if it is using sim time - // if so, there is no need to update camera if sim time has not advanced - bool update = true; - if (this->dataPtr->recordVideoLockstep && - this->dataPtr->recordVideoUseSimTime && - this->dataPtr->videoEncoder.IsEncoding()) - { - std::chrono::steady_clock::time_point t = - std::chrono::steady_clock::time_point( - this->dataPtr->renderUtil.SimTime()); - if (t - this->dataPtr->recordVideoUpdateTime == std::chrono::seconds(0)) - update = false; - else - this->dataPtr->recordVideoUpdateTime = t; - } - - // update and render to texture - if (update) - { - GZ_PROFILE("GzRenderer::Render Update camera"); - this->dataPtr->camera->Update(); - } - - // record video is requested - { - GZ_PROFILE("GzRenderer::Render Record Video"); - if (this->dataPtr->recordVideo) - { - unsigned int width = this->dataPtr->camera->ImageWidth(); - unsigned int height = this->dataPtr->camera->ImageHeight(); - - if (this->dataPtr->cameraImage.Width() != width || - this->dataPtr->cameraImage.Height() != height) - { - this->dataPtr->cameraImage = this->dataPtr->camera->CreateImage(); - } - - // Video recorder is on. Add more frames to it - if (this->dataPtr->videoEncoder.IsEncoding()) - { - this->dataPtr->camera->Copy(this->dataPtr->cameraImage); - - std::chrono::steady_clock::time_point t = - std::chrono::steady_clock::now(); - if (this->dataPtr->recordVideoUseSimTime) - { - t = std::chrono::steady_clock::time_point( - this->dataPtr->renderUtil.SimTime()); - } - bool frameAdded = this->dataPtr->videoEncoder.AddFrame( - this->dataPtr->cameraImage.Data(), width, height, t); - - if (frameAdded) - { - // publish recorder stats - if (this->dataPtr->recordStartTime == - std::chrono::steady_clock::time_point( - std::chrono::duration(std::chrono::seconds(0)))) - { - // start time, i.e. time when first frame is added - this->dataPtr->recordStartTime = t; - } - - std::chrono::steady_clock::duration dt; - dt = t - this->dataPtr->recordStartTime; - int64_t sec, nsec; - std::tie(sec, nsec) = gz::math::durationToSecNsec(dt); - msgs::Time msg; - msg.set_sec(sec); - msg.set_nsec(nsec); - this->dataPtr->recorderStatsPub.Publish(msg); - } - } - // Video recorder is idle. Start recording. - else - { - if (this->dataPtr->recordVideoUseSimTime) - gzmsg << "Recording video using sim time." << std::endl; - if (this->dataPtr->recordVideoLockstep) - { - gzmsg << "Recording video in lockstep mode" << std::endl; - if (!this->dataPtr->recordVideoUseSimTime) - { - gzwarn << "It is recommended to set to true " - << "when recording video in lockstep mode." << std::endl; - } - } - gzmsg << "Recording video using bitrate: " - << this->dataPtr->recordVideoBitrate << std::endl; - this->dataPtr->videoEncoder.Start(this->dataPtr->recordVideoFormat, - this->dataPtr->recordVideoSavePath, width, height, 25, - this->dataPtr->recordVideoBitrate); - this->dataPtr->recordStartTime = std::chrono::steady_clock::time_point( - std::chrono::duration(std::chrono::seconds(0))); - } - } - else if (this->dataPtr->videoEncoder.IsEncoding()) - { - this->dataPtr->videoEncoder.Stop(); - } - } - - // Move To - { - GZ_PROFILE("GzRenderer::Render MoveTo"); - if (!this->dataPtr->moveToTarget.empty()) - { - if (this->dataPtr->moveToHelper.Idle()) - { - rendering::NodePtr target = scene->NodeByName( - this->dataPtr->moveToTarget); - if (target) - { - this->dataPtr->moveToHelper.MoveTo(this->dataPtr->camera, target, 0.5, - std::bind(&GzRenderer::OnMoveToComplete, this)); - this->dataPtr->prevMoveToTime = std::chrono::system_clock::now(); - } - else - { - gzerr << "Unable to move to target. Target: '" - << this->dataPtr->moveToTarget << "' not found" << std::endl; - this->dataPtr->moveToTarget.clear(); - } - } - else - { - auto now = std::chrono::system_clock::now(); - std::chrono::duration dt = now - this->dataPtr->prevMoveToTime; - this->dataPtr->moveToHelper.AddTime(dt.count()); - this->dataPtr->prevMoveToTime = now; - } - } - } - - // Move to pose - { - GZ_PROFILE("GzRenderer::Render MoveToPose"); - if (this->dataPtr->moveToPoseValue) - { - if (this->dataPtr->moveToHelper.Idle()) - { - this->dataPtr->moveToHelper.MoveTo(this->dataPtr->camera, - *(this->dataPtr->moveToPoseValue), - 0.5, std::bind(&GzRenderer::OnMoveToPoseComplete, this)); - this->dataPtr->prevMoveToTime = std::chrono::system_clock::now(); - } - else - { - auto now = std::chrono::system_clock::now(); - std::chrono::duration dt = now - this->dataPtr->prevMoveToTime; - this->dataPtr->moveToHelper.AddTime(dt.count()); - this->dataPtr->prevMoveToTime = now; - } - } - } - - // Follow - { - GZ_PROFILE("GzRenderer::Render Follow"); - if (!this->dataPtr->moveToTarget.empty()) - { - _renderSync->ReleaseQtThreadFromBlock(lock); - return; - } - rendering::NodePtr followTarget = this->dataPtr->camera->FollowTarget(); - if (!this->dataPtr->followTarget.empty()) - { - rendering::NodePtr target = scene->NodeByName( - this->dataPtr->followTarget); - if (target) - { - if (!followTarget || target != followTarget - || this->dataPtr->newFollowOffset) - { - this->dataPtr->camera->SetFollowTarget(target, - this->dataPtr->followOffset, - this->dataPtr->followWorldFrame); - this->dataPtr->camera->SetFollowPGain(this->dataPtr->followPGain); - - this->dataPtr->camera->SetTrackTarget(target); - // found target, no need to wait anymore - this->dataPtr->followTargetWait = false; - this->dataPtr->newFollowOffset = false; - } - else if (this->dataPtr->followOffsetDirty) - { - math::Vector3d offset = - this->dataPtr->camera->WorldPosition() - target->WorldPosition(); - if (!this->dataPtr->followWorldFrame) - { - offset = target->WorldRotation().RotateVectorReverse(offset); - } - this->dataPtr->camera->SetFollowOffset(offset); - this->dataPtr->followOffsetDirty = false; - } - } - else if (!this->dataPtr->followTargetWait) - { - gzerr << "Unable to follow target. Target: '" - << this->dataPtr->followTarget << "' not found" << std::endl; - this->dataPtr->followTarget.clear(); - } - } - else if (followTarget) - { - this->dataPtr->camera->SetFollowTarget(nullptr); - this->dataPtr->camera->SetTrackTarget(nullptr); - } - } - - // View Angle - { - GZ_PROFILE("GzRenderer::Render ViewAngle"); - if (this->dataPtr->viewAngle) - { - if (this->dataPtr->moveToHelper.Idle()) - { - const std::vector &selectedEntities = - this->dataPtr->renderUtil.SelectedEntities(); - - // Look at the origin if no entities are selected - math::Vector3d lookAt = math::Vector3d::Zero; - if (!selectedEntities.empty()) - { - for (const auto &entity : selectedEntities) - { - rendering::NodePtr node = - this->dataPtr->renderUtil.SceneManager().NodeById(entity); - - if (!node) - continue; - - math::Vector3d nodePos = node->WorldPose().Pos(); - lookAt += nodePos; - } - lookAt /= selectedEntities.size(); - } - - this->dataPtr->moveToHelper.LookDirection(this->dataPtr->camera, - this->dataPtr->viewAngleDirection, lookAt, - 0.5, std::bind(&GzRenderer::OnViewAngleComplete, this)); - this->dataPtr->prevMoveToTime = std::chrono::system_clock::now(); - } - else - { - auto now = std::chrono::system_clock::now(); - std::chrono::duration dt = now - this->dataPtr->prevMoveToTime; - this->dataPtr->moveToHelper.AddTime(dt.count()); - this->dataPtr->prevMoveToTime = now; - } - } - } - - // Shapes - { - GZ_PROFILE("GzRenderer::Render Shapes"); - if (this->dataPtr->isSpawning) - { - // Generate spawn preview - rendering::VisualPtr rootVis = scene->RootVisual(); - sdf::Root root; - if (!this->dataPtr->spawnSdfString.empty()) - { - root.LoadSdfString(this->dataPtr->spawnSdfString); - } - else if (!this->dataPtr->spawnSdfPath.empty()) - { - root.Load(this->dataPtr->spawnSdfPath); - } - else - { - gzwarn << "Failed to spawn: no SDF string or path" << std::endl; - } - this->dataPtr->isPlacing = this->GeneratePreview(root); - this->dataPtr->isSpawning = false; - } - } - - // Escape action, clear all selections and terminate any - // spawned previews if escape button is released - { - if (this->dataPtr->escapeReleased) - { - this->DeselectAllEntities(true); - this->TerminateSpawnPreview(); - this->dataPtr->escapeReleased = false; - } - } - - // View as transparent - { - GZ_PROFILE("GzRenderer::Render ViewTransparent"); - if (!this->dataPtr->viewTransparentTarget.empty()) - { - rendering::NodePtr targetNode = - scene->NodeByName(this->dataPtr->viewTransparentTarget); - auto targetVis = std::dynamic_pointer_cast(targetNode); - - if (targetVis && targetVis->HasUserData("gazebo-entity")) - { - Entity targetEntity = - std::get(targetVis->UserData("gazebo-entity")); - this->dataPtr->renderUtil.ViewTransparent(targetEntity); - } - else - { - gzerr << "Unable to find node name [" - << this->dataPtr->viewTransparentTarget - << "] to view as transparent" << std::endl; - } - - this->dataPtr->viewTransparentTarget.clear(); - } - } - - // View center of mass - { - GZ_PROFILE("GzRenderer::Render ViewCOM"); - if (!this->dataPtr->viewCOMTarget.empty()) - { - rendering::NodePtr targetNode = - scene->NodeByName(this->dataPtr->viewCOMTarget); - auto targetVis = std::dynamic_pointer_cast(targetNode); - - if (targetVis && targetVis->HasUserData("gazebo-entity")) - { - Entity targetEntity = - std::get(targetVis->UserData("gazebo-entity")); - this->dataPtr->renderUtil.ViewCOM(targetEntity); - } - else - { - gzerr << "Unable to find node name [" - << this->dataPtr->viewCOMTarget - << "] to view center of mass" << std::endl; - } - - this->dataPtr->viewCOMTarget.clear(); - } - } - - // View inertia - { - GZ_PROFILE("GzRenderer::Render ViewInertia"); - if (!this->dataPtr->viewInertiaTarget.empty()) - { - rendering::NodePtr targetNode = - scene->NodeByName(this->dataPtr->viewInertiaTarget); - auto targetVis = std::dynamic_pointer_cast(targetNode); - - if (targetVis && targetVis->HasUserData("gazebo-entity")) - { - Entity targetEntity = - std::get(targetVis->UserData("gazebo-entity")); - this->dataPtr->renderUtil.ViewInertia(targetEntity); - } - else - { - gzerr << "Unable to find node name [" - << this->dataPtr->viewInertiaTarget - << "] to view inertia" << std::endl; - } - - this->dataPtr->viewInertiaTarget.clear(); - } - } - - // View joints - { - GZ_PROFILE("GzRenderer::Render ViewJoints"); - if (!this->dataPtr->viewJointsTarget.empty()) - { - rendering::NodePtr targetNode = - scene->NodeByName(this->dataPtr->viewJointsTarget); - auto targetVis = std::dynamic_pointer_cast(targetNode); - - if (targetVis && targetVis->HasUserData("gazebo-entity")) - { - Entity targetEntity = - std::get(targetVis->UserData("gazebo-entity")); - this->dataPtr->renderUtil.ViewJoints(targetEntity); - } - else - { - gzerr << "Unable to find node name [" - << this->dataPtr->viewJointsTarget - << "] to view joints" << std::endl; - } - - this->dataPtr->viewJointsTarget.clear(); - } - } - - // View wireframes - { - GZ_PROFILE("GzRenderer::Render ViewWireframes"); - if (!this->dataPtr->viewWireframesTarget.empty()) - { - rendering::NodePtr targetNode = - scene->NodeByName(this->dataPtr->viewWireframesTarget); - auto targetVis = std::dynamic_pointer_cast(targetNode); - - if (targetVis && targetVis->HasUserData("gazebo-entity")) - { - Entity targetEntity = - std::get(targetVis->UserData("gazebo-entity")); - this->dataPtr->renderUtil.ViewWireframes(targetEntity); - } - else - { - gzerr << "Unable to find node name [" - << this->dataPtr->viewWireframesTarget - << "] to view wireframes" << std::endl; - } - - this->dataPtr->viewWireframesTarget.clear(); - } - } - - // View collisions - { - GZ_PROFILE("GzRenderer::Render ViewCollisions"); - if (!this->dataPtr->viewCollisionsTarget.empty()) - { - rendering::NodePtr targetNode = - scene->NodeByName(this->dataPtr->viewCollisionsTarget); - auto targetVis = std::dynamic_pointer_cast(targetNode); - - if (targetVis && targetVis->HasUserData("gazebo-entity")) - { - Entity targetEntity = - std::get(targetVis->UserData("gazebo-entity")); - this->dataPtr->renderUtil.ViewCollisions(targetEntity); - } - else - { - gzerr << "Unable to find node name [" - << this->dataPtr->viewCollisionsTarget - << "] to view collisions" << std::endl; - } - - this->dataPtr->viewCollisionsTarget.clear(); - } - } - - if (gz::gui::App()) - { - gz::gui::events::Render event; - gz::gui::App()->sendEvent( - gz::gui::App()->findChild(), - &event); - } - - // only has an effect in video recording lockstep mode - // this notifes ECM to continue updating the scene - g_renderCv.notify_one(); - - // TODO(anyone) implement a SwapFromThread for parallel command generation - // See https://github.com/gazebosim/gz-rendering/issues/304 - // if( bForcedSerialization ) - // this->dataPtr->camera->SwapFromThread(); - // else - // _renderSync->ReleaseQtThreadFromBlock(lock); - _renderSync->ReleaseQtThreadFromBlock(lock); -} - -///////////////////////////////////////////////// -bool GzRenderer::GeneratePreview(const sdf::Root &_sdf) -{ - // Terminate any pre-existing spawned entities - this->TerminateSpawnPreview(); - - if (nullptr == _sdf.Model() && nullptr == _sdf.Light()) - { - gzwarn << "Only model entities can be spawned at the moment." << std::endl; - this->TerminateSpawnPreview(); - return false; - } - - if (_sdf.Model()) - { - // Only preview first model - sdf::Model model = *(_sdf.Model()); - this->dataPtr->spawnPreviewPose = model.RawPose(); - model.SetName(gz::common::Uuid().String()); - Entity modelId = this->UniqueId(); - if (!modelId) - { - this->TerminateSpawnPreview(); - return false; - } - this->dataPtr->spawnPreview = - this->dataPtr->renderUtil.SceneManager().CreateModel( - modelId, model, - this->dataPtr->renderUtil.SceneManager().WorldId()); - - this->dataPtr->previewIds.push_back(modelId); - for (auto j = 0u; j < model.LinkCount(); j++) - { - sdf::Link link = *(model.LinkByIndex(j)); - link.SetName(gz::common::Uuid().String()); - Entity linkId = this->UniqueId(); - if (!linkId) - { - this->TerminateSpawnPreview(); - return false; - } - this->dataPtr->renderUtil.SceneManager().CreateLink( - linkId, link, modelId); - this->dataPtr->previewIds.push_back(linkId); - for (auto k = 0u; k < link.VisualCount(); k++) - { - sdf::Visual visual = *(link.VisualByIndex(k)); - visual.SetName(gz::common::Uuid().String()); - Entity visualId = this->UniqueId(); - if (!visualId) - { - this->TerminateSpawnPreview(); - return false; - } - this->dataPtr->renderUtil.SceneManager().CreateVisual( - visualId, visual, linkId); - this->dataPtr->previewIds.push_back(visualId); - } - } - } - else if (_sdf.Light()) - { - // Only preview first model - sdf::Light light = *(_sdf.Light()); - this->dataPtr->spawnPreviewPose = light.RawPose(); - light.SetName(gz::common::Uuid().String()); - Entity lightVisualId = this->UniqueId(); - if (!lightVisualId) - { - this->TerminateSpawnPreview(); - return false; - } - Entity lightId = this->UniqueId(); - if (!lightId) - { - this->TerminateSpawnPreview(); - return false; - } - this->dataPtr->spawnPreview = - this->dataPtr->renderUtil.SceneManager().CreateLight( - lightId, light, light.Name(), - this->dataPtr->renderUtil.SceneManager().WorldId()); - this->dataPtr->renderUtil.SceneManager().CreateLightVisual( - lightVisualId, light, light.Name(), lightId); - - this->dataPtr->previewIds.push_back(lightId); - this->dataPtr->previewIds.push_back(lightVisualId); - } - return true; -} - -///////////////////////////////////////////////// -void GzRenderer::TerminateSpawnPreview() -{ - for (auto _id : this->dataPtr->previewIds) - this->dataPtr->renderUtil.SceneManager().RemoveEntity(_id); - this->dataPtr->previewIds.clear(); - this->dataPtr->isPlacing = false; -} - -///////////////////////////////////////////////// -Entity GzRenderer::UniqueId() -{ - auto timeout = 100000u; - for (auto i = 0u; i < timeout; ++i) - { - Entity id = std::numeric_limits::max() - i; - if (!this->dataPtr->renderUtil.SceneManager().HasEntity(id)) - return id; - } - return kNullEntity; -} - -///////////////////////////////////////////////// -void GzRenderer::HandleMouseEvent() -{ - std::lock_guard lock(this->dataPtr->mutex); - this->BroadcastHoverPos(); - this->BroadcastLeftClick(); - this->BroadcastRightClick(); - this->HandleMouseContextMenu(); - this->HandleModelPlacement(); - this->HandleMouseTransformControl(); - this->HandleMouseViewControl(); -} - -///////////////////////////////////////////////// -void GzRenderer::BroadcastHoverPos() -{ - if (this->dataPtr->hoverDirty) - { - math::Vector3d pos = this->ScreenToScene(this->dataPtr->mouseHoverPos); - - gz::gui::events::HoverToScene hoverToSceneEvent(pos); - gz::gui::App()->sendEvent( - gz::gui::App()->findChild(), - &hoverToSceneEvent); - } -} - -///////////////////////////////////////////////// -void GzRenderer::BroadcastLeftClick() -{ - if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT && - this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE && - !this->dataPtr->mouseEvent.Dragging() && this->dataPtr->mouseDirty) - { - math::Vector3d pos = this->ScreenToScene(this->dataPtr->mouseEvent.Pos()); - - gz::gui::events::LeftClickToScene leftClickToSceneEvent(pos); - gz::gui::App()->sendEvent( - gz::gui::App()->findChild(), - &leftClickToSceneEvent); - } -} - -///////////////////////////////////////////////// -void GzRenderer::BroadcastRightClick() -{ - if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::RIGHT && - this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE && - !this->dataPtr->mouseEvent.Dragging() && this->dataPtr->mouseDirty) - { - // If the dropdown menu is disabled, quash the mouse event - if (!this->dataPtr->dropdownMenuEnabled) - this->dataPtr->mouseDirty = false; - - math::Vector3d pos = this->ScreenToScene(this->dataPtr->mouseEvent.Pos()); - - gz::gui::events::RightClickToScene rightClickToSceneEvent(pos); - gz::gui::App()->sendEvent( - gz::gui::App()->findChild(), - &rightClickToSceneEvent); - } -} - -///////////////////////////////////////////////// -void GzRenderer::HandleMouseContextMenu() -{ - if (!this->dataPtr->mouseDirty) - return; - - if (!this->dataPtr->mouseEvent.Dragging() && - this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE && - this->dataPtr->mouseEvent.Button() == common::MouseEvent::RIGHT) - { - math::Vector2i dt = - this->dataPtr->mouseEvent.PressPos() - this->dataPtr->mouseEvent.Pos(); - - // check for click with some tol for mouse movement - if (dt.Length() > 5.0) - return; - - rendering::VisualPtr visual = this->dataPtr->camera->Scene()->VisualAt( - this->dataPtr->camera, - this->dataPtr->mouseEvent.Pos()); - - if (!visual) - return; - - // get model visual - while (visual->HasParent() && visual->Parent() != - visual->Scene()->RootVisual()) - { - visual = std::dynamic_pointer_cast(visual->Parent()); - } - - emit ContextMenuRequested(visual->Name().c_str()); - this->dataPtr->mouseDirty = false; - } -} - -//////////////////////////////////////////////// -void GzRenderer::HandleKeyPress(QKeyEvent *_e) -{ - if (_e->isAutoRepeat()) - return; - - std::lock_guard lock(this->dataPtr->mutex); - - this->dataPtr->keyEvent.SetKey(_e->key()); - this->dataPtr->keyEvent.SetText(_e->text().toStdString()); - - this->dataPtr->keyEvent.SetControl( - (_e->modifiers() & Qt::ControlModifier)); - this->dataPtr->keyEvent.SetShift( - (_e->modifiers() & Qt::ShiftModifier)); - this->dataPtr->keyEvent.SetAlt( - (_e->modifiers() & Qt::AltModifier)); - - this->dataPtr->mouseEvent.SetControl(this->dataPtr->keyEvent.Control()); - this->dataPtr->mouseEvent.SetShift(this->dataPtr->keyEvent.Shift()); - this->dataPtr->mouseEvent.SetAlt(this->dataPtr->keyEvent.Alt()); - this->dataPtr->keyEvent.SetType(common::KeyEvent::PRESS); - - // Update the object and mouse to be placed at the current position - // only for x, y, and z key presses - if (_e->key() == Qt::Key_X || - _e->key() == Qt::Key_Y || - _e->key() == Qt::Key_Z || - _e->key() == Qt::Key_Shift) - { - this->dataPtr->transformControl.Start(); - this->dataPtr->mousePressPos = this->dataPtr->mouseEvent.Pos(); - } - - // fullscreen - if (_e->key() == Qt::Key_F11) - { - if (gz::gui::App()->findChild - ()->QuickWindow()->visibility() - == QWindow::FullScreen) - { - gz::gui::App()->findChild - ()->QuickWindow()->showNormal(); - } - else - { - gz::gui::App()->findChild - ()->QuickWindow()->showFullScreen(); - } - } - - switch (_e->key()) - { - case Qt::Key_X: - this->dataPtr->xPressed = true; - break; - case Qt::Key_Y: - this->dataPtr->yPressed = true; - break; - case Qt::Key_Z: - this->dataPtr->zPressed = true; - break; - default: - break; - } -} - -//////////////////////////////////////////////// -void GzRenderer::HandleKeyRelease(QKeyEvent *_e) -{ - if (_e->isAutoRepeat()) - return; - - std::lock_guard lock(this->dataPtr->mutex); - - this->dataPtr->keyEvent.SetKey(0); - - this->dataPtr->keyEvent.SetControl( - (_e->modifiers() & Qt::ControlModifier) - && (_e->key() != Qt::Key_Control)); - this->dataPtr->keyEvent.SetShift( - (_e->modifiers() & Qt::ShiftModifier) - && (_e->key() != Qt::Key_Shift)); - this->dataPtr->keyEvent.SetAlt( - (_e->modifiers() & Qt::AltModifier) - && (_e->key() != Qt::Key_Alt)); - - this->dataPtr->mouseEvent.SetControl(this->dataPtr->keyEvent.Control()); - this->dataPtr->mouseEvent.SetShift(this->dataPtr->keyEvent.Shift()); - this->dataPtr->mouseEvent.SetAlt(this->dataPtr->keyEvent.Alt()); - this->dataPtr->keyEvent.SetType(common::KeyEvent::RELEASE); - - // Update the object and mouse to be placed at the current position - // only for x, y, and z key presses - if (_e->key() == Qt::Key_X || - _e->key() == Qt::Key_Y || - _e->key() == Qt::Key_Z || - _e->key() == Qt::Key_Shift) - { - this->dataPtr->transformControl.Start(); - this->dataPtr->mousePressPos = this->dataPtr->mouseEvent.Pos(); - this->dataPtr->isStartWorldPosSet = false; - } - - switch (_e->key()) - { - case Qt::Key_X: - this->dataPtr->xPressed = false; - break; - case Qt::Key_Y: - this->dataPtr->yPressed = false; - break; - case Qt::Key_Z: - this->dataPtr->zPressed = false; - break; - case Qt::Key_Escape: - this->dataPtr->escapeReleased = true; - break; - default: - break; - } -} - -///////////////////////////////////////////////// -void GzRenderer::HandleModelPlacement() -{ - if (!this->dataPtr->isPlacing) - return; - - if (this->dataPtr->spawnPreview && this->dataPtr->hoverDirty) - { - math::Vector3d pos = this->ScreenToPlane(this->dataPtr->mouseHoverPos); - pos.Z(this->dataPtr->spawnPreview->WorldPosition().Z()); - this->dataPtr->spawnPreview->SetWorldPosition(pos); - this->dataPtr->hoverDirty = false; - } - if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT && - this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE && - !this->dataPtr->mouseEvent.Dragging() && this->dataPtr->mouseDirty) - { - // Delete the generated visuals - this->TerminateSpawnPreview(); - - math::Pose3d modelPose = this->dataPtr->spawnPreviewPose; - std::function cb = - [](const gz::msgs::Boolean &/*_rep*/, const bool _result) - { - if (!_result) - gzerr << "Error creating model" << std::endl; - }; - math::Vector3d pos = this->ScreenToPlane(this->dataPtr->mouseEvent.Pos()); - pos.Z(modelPose.Pos().Z()); - msgs::EntityFactory req; - if (!this->dataPtr->spawnSdfString.empty()) - { - req.set_sdf(this->dataPtr->spawnSdfString); - } - else if (!this->dataPtr->spawnSdfPath.empty()) - { - req.set_sdf_filename(this->dataPtr->spawnSdfPath); - } - else - { - gzwarn << "Failed to find SDF string or file path" << std::endl; - } - req.set_allow_renaming(true); - msgs::Set(req.mutable_pose(), math::Pose3d(pos, modelPose.Rot())); - - if (this->dataPtr->createCmdService.empty()) - { - this->dataPtr->createCmdService = "/world/" + this->worldName - + "/create"; - } - this->dataPtr->createCmdService = transport::TopicUtils::AsValidTopic( - this->dataPtr->createCmdService); - if (this->dataPtr->createCmdService.empty()) - { - gzerr << "Failed to create valid create command service for world [" - << this->worldName <<"]" << std::endl; - return; - } - - this->dataPtr->node.Request(this->dataPtr->createCmdService, req, cb); - this->dataPtr->isPlacing = false; - this->dataPtr->mouseDirty = false; - this->dataPtr->spawnSdfString.clear(); - this->dataPtr->spawnSdfPath.clear(); - } -} - -///////////////////////////////////////////////// -void GzRenderer::HandleEntitySelection() -{ - if (this->dataPtr->selectionHelper.deselectAll) - { - this->DeselectAllEntities(this->dataPtr->selectionHelper.sendEvent); - - this->dataPtr->selectionHelper = SelectionHelper(); - } - else if (this->dataPtr->selectionHelper.selectEntity != kNullEntity) - { - auto node = this->dataPtr->renderUtil.SceneManager().NodeById( - this->dataPtr->selectionHelper.selectEntity); - this->UpdateSelectedEntity(node, - this->dataPtr->selectionHelper.sendEvent); - - this->dataPtr->selectionHelper = SelectionHelper(); - } -} - -///////////////////////////////////////////////// -void GzRenderer::DeselectAllEntities(bool _sendEvent) -{ - if (this->dataPtr->renderThreadId != std::this_thread::get_id()) - { - gzwarn << "Making render calls from outside the render thread" - << std::endl; - } - - this->dataPtr->renderUtil.DeselectAllEntities(); - - if (_sendEvent) - { - gz::sim::gui::events::DeselectAllEntities deselectEvent; - gz::gui::App()->sendEvent( - gz::gui::App()->findChild(), - &deselectEvent); - } -} - -///////////////////////////////////////////////// -double GzRenderer::SnapValue( - double _coord, double _interval, double _sensitivity) const -{ - double snap = _interval * _sensitivity; - double rem = fmod(_coord, _interval); - double minInterval = _coord - rem; - - if (rem < 0) - { - minInterval -= _interval; - } - - double maxInterval = minInterval + _interval; - - if (_coord < (minInterval + snap)) - { - _coord = minInterval; - } - else if (_coord > (maxInterval - snap)) - { - _coord = maxInterval; - } - - return _coord; -} - -///////////////////////////////////////////////// -void GzRenderer::SnapPoint( - gz::math::Vector3d &_point, math::Vector3d &_snapVals, - double _sensitivity) const -{ - if (_snapVals.X() <= 0 || _snapVals.Y() <= 0 || _snapVals.Z() <= 0) - { - gzerr << "Interval distance must be greater than 0" - << std::endl; - return; - } - - if (_sensitivity < 0 || _sensitivity > 1.0) - { - gzerr << "Sensitivity must be between 0 and 1" << std::endl; - return; - } - - _point.X() = this->SnapValue(_point.X(), _snapVals.X(), _sensitivity); - _point.Y() = this->SnapValue(_point.Y(), _snapVals.Y(), _sensitivity); - _point.Z() = this->SnapValue(_point.Z(), _snapVals.Z(), _sensitivity); -} - -///////////////////////////////////////////////// -void GzRenderer::XYZConstraint(math::Vector3d &_axis) -{ - math::Vector3d translationAxis = math::Vector3d::Zero; - - if (this->dataPtr->xPressed) - { - translationAxis += math::Vector3d::UnitX; - } - - if (this->dataPtr->yPressed) - { - translationAxis += math::Vector3d::UnitY; - } - - if (this->dataPtr->zPressed) - { - translationAxis += math::Vector3d::UnitZ; - } - - if (translationAxis != math::Vector3d::Zero) - { - _axis = translationAxis; - } -} - -///////////////////////////////////////////////// -void GzRenderer::HandleMouseTransformControl() -{ - if (this->dataPtr->renderThreadId != std::this_thread::get_id()) - { - gzwarn << "Making render calls from outside the render thread" - << std::endl; - } - - // set transform configuration - this->dataPtr->transformControl.SetTransformMode( - this->dataPtr->transformMode); - - if (!this->dataPtr->transformControl.Camera()) - this->dataPtr->transformControl.SetCamera(this->dataPtr->camera); - - // stop and detach transform controller if mode is none or no entity is - // selected - if (this->dataPtr->transformMode == rendering::TransformMode::TM_NONE || - (this->dataPtr->transformControl.Node() && - this->dataPtr->renderUtil.SelectedEntities().empty())) - { - if (this->dataPtr->transformControl.Active()) - this->dataPtr->transformControl.Stop(); - - this->dataPtr->transformControl.Detach(); - } - else - { - // shift indicates world space transformation - this->dataPtr->transformSpace = (this->dataPtr->keyEvent.Shift()) ? - rendering::TransformSpace::TS_WORLD : - rendering::TransformSpace::TS_LOCAL; - this->dataPtr->transformControl.SetTransformSpace( - this->dataPtr->transformSpace); - } - - // update gizmo visual - this->dataPtr->transformControl.Update(); - - // check for mouse events - if (!this->dataPtr->mouseDirty) - return; - - // handle transform control - if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT) - { - if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::PRESS - && this->dataPtr->transformControl.Node()) - { - this->dataPtr->mousePressPos = this->dataPtr->mouseEvent.Pos(); - // get the visual at mouse position - rendering::VisualPtr visual = this->dataPtr->camera->VisualAt( - this->dataPtr->mouseEvent.PressPos()); - - if (visual) - { - // check if the visual is an axis in the gizmo visual - math::Vector3d axis = - this->dataPtr->transformControl.AxisById(visual->Id()); - if (axis != gz::math::Vector3d::Zero) - { - // start the transform process - this->dataPtr->transformControl.SetActiveAxis(axis); - this->dataPtr->transformControl.Start(); - this->dataPtr->mouseDirty = false; - } - else - return; - } - } - else if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE) - { - this->dataPtr->isStartWorldPosSet = false; - if (this->dataPtr->transformControl.Active()) - { - if (this->dataPtr->transformControl.Node()) - { - std::function cb = - [](const gz::msgs::Boolean &/*_rep*/, const bool _result) - { - if (!_result) - gzerr << "Error setting pose" << std::endl; - }; - rendering::NodePtr node = this->dataPtr->transformControl.Node(); - gz::msgs::Pose req; - req.set_name(node->Name()); - msgs::Set(req.mutable_position(), node->WorldPosition()); - msgs::Set(req.mutable_orientation(), node->WorldRotation()); - if (this->dataPtr->poseCmdService.empty()) - { - this->dataPtr->poseCmdService = "/world/" + this->worldName - + "/set_pose"; - } - this->dataPtr->poseCmdService = transport::TopicUtils::AsValidTopic( - this->dataPtr->poseCmdService); - if (this->dataPtr->poseCmdService.empty()) - { - gzerr << "Failed to create valid pose command service for world [" - << this->worldName <<"]" << std::endl; - return; - } - this->dataPtr->node.Request(this->dataPtr->poseCmdService, req, cb); - } - - this->dataPtr->transformControl.Stop(); - this->dataPtr->mouseDirty = false; - } - // Select entity - else if (!this->dataPtr->mouseEvent.Dragging()) - { - rendering::VisualPtr visual = this->dataPtr->camera->Scene()->VisualAt( - this->dataPtr->camera, - this->dataPtr->mouseEvent.Pos()); - - if (!visual) - { - // Hit the background, deselect all - if (!this->dataPtr->mouseEvent.Dragging()) - { - this->DeselectAllEntities(true); - } - return; - } - - // check if the visual is an axis in the gizmo visual - math::Vector3d axis = - this->dataPtr->transformControl.AxisById(visual->Id()); - if (axis == gz::math::Vector3d::Zero) - { - auto topVis = - this->dataPtr->renderUtil.SceneManager().TopLevelVisual(visual); - // TODO(anyone) Check plane geometry instead of hardcoded name! - if (topVis && topVis->Name() != "ground_plane") - { - // Highlight entity and notify other widgets - this->UpdateSelectedEntity(topVis, true); - - this->dataPtr->mouseDirty = false; - return; - } - // Don't deselect after dragging, user may be orbiting the camera - else if (!this->dataPtr->mouseEvent.Dragging()) - { - // Hit the ground, deselect all - this->DeselectAllEntities(true); - return; - } - } - } - } - } - - if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::MOVE - && this->dataPtr->transformControl.Active()) - { - // compute the the start and end mouse positions in normalized coordinates - auto imageWidth = static_cast(this->dataPtr->camera->ImageWidth()); - auto imageHeight = static_cast( - this->dataPtr->camera->ImageHeight()); - double nx = 2.0 * - this->dataPtr->mousePressPos.X() / - imageWidth - 1.0; - double ny = 1.0 - 2.0 * - this->dataPtr->mousePressPos.Y() / - imageHeight; - double nxEnd = 2.0 * this->dataPtr->mouseEvent.Pos().X() / - imageWidth - 1.0; - double nyEnd = 1.0 - 2.0 * this->dataPtr->mouseEvent.Pos().Y() / - imageHeight; - math::Vector2d start(nx, ny); - math::Vector2d end(nxEnd, nyEnd); - - // get the current active axis - math::Vector3d axis = this->dataPtr->transformControl.ActiveAxis(); - - // compute 3d transformation from 2d mouse movement - if (this->dataPtr->transformControl.Mode() == - rendering::TransformMode::TM_TRANSLATION) - { - Entity nodeId = this->dataPtr->renderUtil.SelectedEntities().front(); - rendering::NodePtr target = - this->dataPtr->renderUtil.SceneManager().NodeById(nodeId); - if (!target) - { - gzwarn << "Failed to find node with ID [" << nodeId << "]" - << std::endl; - return; - } - this->XYZConstraint(axis); - if (!this->dataPtr->isStartWorldPosSet) - { - this->dataPtr->isStartWorldPosSet = true; - this->dataPtr->startWorldPos = - target->WorldPosition(); - } - math::Vector3d distance = - this->dataPtr->transformControl.TranslationFrom2d(axis, start, end); - if (this->dataPtr->keyEvent.Control()) - { - // Translate to world frame for snapping - distance += this->dataPtr->startWorldPos; - math::Vector3d snapVals = this->XYZSnap(); - - // Constrain snap values to a minimum of 1e-4 - snapVals.X() = std::max(1e-4, snapVals.X()); - snapVals.Y() = std::max(1e-4, snapVals.Y()); - snapVals.Z() = std::max(1e-4, snapVals.Z()); - - SnapPoint(distance, snapVals); - - // Translate back to entity frame - distance -= this->dataPtr->startWorldPos; - distance *= axis; - } - this->dataPtr->transformControl.Translate(distance); - } - else if (this->dataPtr->transformControl.Mode() == - rendering::TransformMode::TM_ROTATION) - { - math::Quaterniond rotation = - this->dataPtr->transformControl.RotationFrom2d(axis, start, end); - - if (this->dataPtr->keyEvent.Control()) - { - math::Vector3d currentRot = rotation.Euler(); - math::Vector3d snapVals = this->RPYSnap(); - - if (snapVals.X() <= 1e-4) - { - snapVals.X() = GZ_PI/4; - } - else - { - snapVals.X() = GZ_DTOR(snapVals.X()); - } - if (snapVals.Y() <= 1e-4) - { - snapVals.Y() = GZ_PI/4; - } - else - { - snapVals.Y() = GZ_DTOR(snapVals.Y()); - } - if (snapVals.Z() <= 1e-4) - { - snapVals.Z() = GZ_PI/4; - } - else - { - snapVals.Z() = GZ_DTOR(snapVals.Z()); - } - - SnapPoint(currentRot, snapVals); - rotation = math::Quaterniond::EulerToQuaternion(currentRot); - } - this->dataPtr->transformControl.Rotate(rotation); - } - else if (this->dataPtr->transformControl.Mode() == - rendering::TransformMode::TM_SCALE) - { - this->XYZConstraint(axis); - // note: scaling is limited to local space - math::Vector3d scale = - this->dataPtr->transformControl.ScaleFrom2d(axis, start, end); - if (this->dataPtr->keyEvent.Control()) - { - math::Vector3d snapVals = this->ScaleSnap(); - - if (snapVals.X() <= 1e-4) - snapVals.X() = 0.1; - if (snapVals.Y() <= 1e-4) - snapVals.Y() = 0.1; - if (snapVals.Z() <= 1e-4) - snapVals.Z() = 0.1; - - SnapPoint(scale, snapVals); - } - this->dataPtr->transformControl.Scale(scale); - } - this->dataPtr->drag = 0; - this->dataPtr->mouseDirty = false; - } -} - - -///////////////////////////////////////////////// -void GzRenderer::HandleMouseViewControl() -{ - if (!this->dataPtr->mouseDirty) - return; - - if (this->dataPtr->renderThreadId != std::this_thread::get_id()) - { - gzwarn << "Making render calls from outside the render thread" - << std::endl; - } - - if (!this->dataPtr->followTarget.empty()) - this->dataPtr->camera->WorldPosition(); - - if (this->dataPtr->viewController == "ortho") - { - this->dataPtr->viewControl = &this->dataPtr->orthoViewControl; - } - else if (this->dataPtr->viewController == "orbit") - { - this->dataPtr->viewControl = &this->dataPtr->orbitViewControl; - } - else - { - gzerr << "Unknown view controller: " << this->dataPtr->viewController - << ". Defaulting to orbit view controller" << std::endl; - this->dataPtr->viewController = "orbit"; - this->dataPtr->viewControl = &this->dataPtr->orbitViewControl; - } - this->dataPtr->viewControl->SetCamera(this->dataPtr->camera); - - if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::SCROLL) - { - this->dataPtr->target = - this->ScreenToScene(this->dataPtr->mouseEvent.Pos()); - this->dataPtr->viewControl->SetTarget(this->dataPtr->target); - double distance = this->dataPtr->camera->WorldPosition().Distance( - this->dataPtr->target); - double amount = -this->dataPtr->drag.Y() * distance / 5.0; - this->dataPtr->viewControl->Zoom(amount); - } - else - { - if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::PRESS || - // the rendering thread may miss the press event due to - // race condition when doing a drag operation (press and move, where - // the move event overrides the press event before it is processed) - // so we double check to see if target is set or not - (this->dataPtr->mouseEvent.Type() == common::MouseEvent::MOVE && - this->dataPtr->mouseEvent.Dragging() && - std::isinf(this->dataPtr->target.X()))) - { - this->dataPtr->target = this->ScreenToScene( - this->dataPtr->mouseEvent.PressPos()); - this->dataPtr->viewControl->SetTarget(this->dataPtr->target); - } - // unset the target on release (by setting to inf) - else if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE) - { - this->dataPtr->target = gz::math::INF_D; - } - - // Pan with left button - if (this->dataPtr->mouseEvent.Buttons() & common::MouseEvent::LEFT) - { - if (Qt::ShiftModifier == QGuiApplication::queryKeyboardModifiers()) - this->dataPtr->viewControl->Orbit(this->dataPtr->drag); - else - this->dataPtr->viewControl->Pan(this->dataPtr->drag); - } - // Orbit with middle button - else if (this->dataPtr->mouseEvent.Buttons() & common::MouseEvent::MIDDLE) - { - this->dataPtr->viewControl->Orbit(this->dataPtr->drag); - } - else if (this->dataPtr->mouseEvent.Buttons() & common::MouseEvent::RIGHT) - { - double hfov = this->dataPtr->camera->HFOV().Radian(); - double vfov = 2.0f * atan(tan(hfov / 2.0f) / - this->dataPtr->camera->AspectRatio()); - double distance = this->dataPtr->camera->WorldPosition().Distance( - this->dataPtr->target); - double amount = ((-this->dataPtr->drag.Y() / - static_cast(this->dataPtr->camera->ImageHeight())) - * distance * tan(vfov/2.0) * 6.0); - this->dataPtr->viewControl->Zoom(amount); - } - } - this->dataPtr->drag = 0; - this->dataPtr->mouseDirty = false; - - - if (!this->dataPtr->followTarget.empty()) - this->dataPtr->followOffsetDirty = true; -} - -///////////////////////////////////////////////// -std::string GzRenderer::Initialize() -{ - if (this->initialized) - return std::string(); - - // Only one engine / scene / user camera is currently supported. - // Fail gracefully even before getting to renderUtil. - if (!rendering::loadedEngines().empty()) - { - return "Currently only one plugin providing a 3D scene is supported at a " - "time."; - } - - this->dataPtr->renderUtil.SetWinID(std::to_string( - gz::gui::App()->findChild()-> - QuickWindow()->winId())); - this->dataPtr->renderUtil.SetUseCurrentGLContext(true); - this->dataPtr->renderUtil.Init(); - - rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene(); - if (!scene) - return "Failed to create a 3D scene."; - - auto root = scene->RootVisual(); - - scene->SetCameraPassCountPerGpuFlush(6u); - - // Camera - this->dataPtr->camera = scene->CreateCamera(); - this->dataPtr->camera->SetUserData("user-camera", true); - root->AddChild(this->dataPtr->camera); - this->dataPtr->camera->SetLocalPose(this->cameraPose); - this->dataPtr->camera->SetImageWidth(this->textureSize.width()); - this->dataPtr->camera->SetImageHeight(this->textureSize.height()); - this->dataPtr->camera->SetAntiAliasing(8); - this->dataPtr->camera->SetHFOV(M_PI * 0.5); - this->dataPtr->camera->SetVisibilityMask(this->visibilityMask); - // setting the size and calling PreRender should cause the render texture to - // be rebuilt - this->dataPtr->camera->PreRender(); - this->textureId = this->dataPtr->camera->RenderTextureGLId(); - - // Ray Query - this->dataPtr->rayQuery = this->dataPtr->camera->Scene()->CreateRayQuery(); - - this->initialized = true; - return std::string(); -} - -///////////////////////////////////////////////// -void GzRenderer::Destroy() -{ - auto engine = rendering::engine(this->dataPtr->renderUtil.EngineName()); - if (!engine) - return; - auto scene = engine->SceneByName(this->dataPtr->renderUtil.SceneName()); - if (!scene) - return; - - scene->DestroySensor(this->dataPtr->camera); - - // If that was the last sensor, destroy scene - if (scene->SensorCount() == 0) - { - gzdbg << "Destroy scene [" << scene->Name() << "]" << std::endl; - engine->DestroyScene(scene); - - // TODO(anyone) If that was the last scene, terminate engine? - } -} - -///////////////////////////////////////////////// -void GzRenderer::SetXYZSnap(const math::Vector3d &_xyz) -{ - this->dataPtr->xyzSnap = _xyz; -} - -///////////////////////////////////////////////// -math::Vector3d GzRenderer::XYZSnap() const -{ - return this->dataPtr->xyzSnap; -} - -///////////////////////////////////////////////// -void GzRenderer::SetRPYSnap(const math::Vector3d &_rpy) -{ - this->dataPtr->rpySnap = _rpy; -} - -///////////////////////////////////////////////// -math::Vector3d GzRenderer::RPYSnap() const -{ - return this->dataPtr->rpySnap; -} - -///////////////////////////////////////////////// -void GzRenderer::SetScaleSnap(const math::Vector3d &_scale) -{ - this->dataPtr->scaleSnap = _scale; -} - -///////////////////////////////////////////////// -math::Vector3d GzRenderer::ScaleSnap() const -{ - return this->dataPtr->scaleSnap; -} -///////////////////////////////////////////////// -void GzRenderer::UpdateSelectedEntity(const rendering::NodePtr &_node, - bool _sendEvent) -{ - if (!_node) - return; - - if (this->dataPtr->renderThreadId != std::this_thread::get_id()) - { - gzwarn << "Making render calls from outside the render thread" - << std::endl; - } - - bool deselectedAll{false}; - - // Deselect all if control is not being held - if (!(QGuiApplication::keyboardModifiers() & Qt::ControlModifier) && - !this->dataPtr->renderUtil.SelectedEntities().empty()) - { - // Notify other widgets regardless of _sendEvent, because this is a new - // decision from this widget - this->DeselectAllEntities(true); - deselectedAll = true; - } - - // Attach control if in a transform mode - control is attached to: - // * latest selection - // * top-level nodes (model, light...) - if (this->dataPtr->transformMode != rendering::TransformMode::TM_NONE) - { - auto topNode = - this->dataPtr->renderUtil.SceneManager().TopLevelNode(_node); - if (topNode == _node) - { - this->dataPtr->transformControl.Attach(_node); - - // When attached, we want only one entity selected - // Notify other widgets regardless of _sendEvent, because this is a new - // decision from this widget - this->DeselectAllEntities(true); - deselectedAll = true; - } - else - { - this->dataPtr->transformControl.Detach(); - } - } - - // Select new entity - this->dataPtr->renderUtil.SetSelectedEntity(_node); - - // Notify other widgets of the currently selected entities - if (_sendEvent || deselectedAll) - { - gz::sim::gui::events::EntitiesSelected selectEvent( - this->dataPtr->renderUtil.SelectedEntities()); - gz::gui::App()->sendEvent( - gz::gui::App()->findChild(), - &selectEvent); - } -} - -///////////////////////////////////////////////// -void GzRenderer::SetTransformMode(const std::string &_mode) -{ - std::lock_guard lock(this->dataPtr->mutex); - if (_mode == "select") - this->dataPtr->transformMode = rendering::TransformMode::TM_NONE; - else if (_mode == "translate") - this->dataPtr->transformMode = rendering::TransformMode::TM_TRANSLATION; - else if (_mode == "rotate") - this->dataPtr->transformMode = rendering::TransformMode::TM_ROTATION; - else if (_mode == "scale") - this->dataPtr->transformMode = rendering::TransformMode::TM_SCALE; - else - gzerr << "Unknown transform mode: [" << _mode << "]" << std::endl; - - // Update selected entities if transform control is changed - if (!this->dataPtr->renderUtil.SelectedEntities().empty()) - { - Entity entity = this->dataPtr->renderUtil.SelectedEntities().back(); - this->dataPtr->selectionHelper = {entity, false, false}; - } -} - -///////////////////////////////////////////////// -void GzRenderer::SetModel(const std::string &_model) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->isSpawning = true; - this->dataPtr->spawnSdfString = _model; -} - -///////////////////////////////////////////////// -void GzRenderer::SetModelPath(const std::string &_filePath) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->isSpawning = true; - this->dataPtr->spawnSdfPath = _filePath; -} - -///////////////////////////////////////////////// -void GzRenderer::SetDropdownMenuEnabled(bool _enableDropdownMenu) -{ - this->dataPtr->dropdownMenuEnabled = _enableDropdownMenu; -} - -///////////////////////////////////////////////// -void GzRenderer::SetRecordVideo(bool _record, const std::string &_format, - const std::string &_savePath) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->recordVideo = _record; - this->dataPtr->recordVideoFormat = _format; - this->dataPtr->recordVideoSavePath = _savePath; -} - -///////////////////////////////////////////////// -void GzRenderer::SetRecordVideoUseSimTime(bool _useSimTime) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->recordVideoUseSimTime = _useSimTime; -} - -///////////////////////////////////////////////// -void GzRenderer::SetRecordVideoLockstep(bool _useSimTime) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->recordVideoLockstep = _useSimTime; -} - -///////////////////////////////////////////////// -void GzRenderer::SetRecordVideoBitrate(unsigned int _bitrate) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->recordVideoBitrate = _bitrate; -} - -///////////////////////////////////////////////// -void GzRenderer::SetMoveTo(const std::string &_target) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->moveToTarget = _target; -} - -///////////////////////////////////////////////// -void GzRenderer::SetFollowTarget(const std::string &_target, - bool _waitForTarget) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->followTarget = _target; - this->dataPtr->followTargetWait = _waitForTarget; -} - -///////////////////////////////////////////////// -void GzRenderer::SetViewAngle(const math::Vector3d &_direction) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->viewAngle = true; - this->dataPtr->viewAngleDirection = _direction; -} - -///////////////////////////////////////////////// -void GzRenderer::SetMoveToPose(const math::Pose3d &_pose) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->moveToPoseValue = _pose; -} - -///////////////////////////////////////////////// -void GzRenderer::SetViewTransparentTarget(const std::string &_target) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->viewTransparentTarget = _target; -} - -///////////////////////////////////////////////// -void GzRenderer::SetViewCOMTarget(const std::string &_target) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->viewCOMTarget = _target; -} - -///////////////////////////////////////////////// -void GzRenderer::SetViewInertiaTarget(const std::string &_target) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->viewInertiaTarget = _target; -} - -///////////////////////////////////////////////// -void GzRenderer::SetViewJointsTarget(const std::string &_target) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->viewJointsTarget = _target; -} - -///////////////////////////////////////////////// -void GzRenderer::SetViewWireframesTarget(const std::string &_target) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->viewWireframesTarget = _target; -} - -///////////////////////////////////////////////// -void GzRenderer::SetViewCollisionsTarget(const std::string &_target) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->viewCollisionsTarget = _target; -} - -///////////////////////////////////////////////// -void GzRenderer::SetViewController(const std::string &_controller) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->viewController = _controller; - - // mark mouse dirty to trigger HandleMouseEvent call and - // set up a new view controller - this->dataPtr->mouseDirty = true; -} - -///////////////////////////////////////////////// -void GzRenderer::SetFollowPGain(double _gain) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->followPGain = _gain; -} - -///////////////////////////////////////////////// -void GzRenderer::SetFollowWorldFrame(bool _worldFrame) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->followWorldFrame = _worldFrame; -} - -///////////////////////////////////////////////// -void GzRenderer::SetInitCameraPose(const math::Pose3d &_pose) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->moveToHelper.SetInitCameraPose(_pose); -} - -///////////////////////////////////////////////// -bool GzRenderer::FollowWorldFrame() const -{ - std::lock_guard lock(this->dataPtr->mutex); - return this->dataPtr->followWorldFrame; -} - -///////////////////////////////////////////////// -void GzRenderer::SetFollowOffset(const math::Vector3d &_offset) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->followOffset = _offset; - - if (!this->dataPtr->followTarget.empty()) - this->dataPtr->newFollowOffset = true; -} - -///////////////////////////////////////////////// -math::Vector3d GzRenderer::FollowOffset() const -{ - std::lock_guard lock(this->dataPtr->mutex); - return this->dataPtr->followOffset; -} - -///////////////////////////////////////////////// -std::string GzRenderer::FollowTarget() const -{ - std::lock_guard lock(this->dataPtr->mutex); - return this->dataPtr->followTarget; -} - -///////////////////////////////////////////////// -void GzRenderer::OnMoveToComplete() -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->moveToTarget.clear(); -} - -///////////////////////////////////////////////// -void GzRenderer::OnViewAngleComplete() -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->viewAngle = false; -} - -///////////////////////////////////////////////// -void GzRenderer::OnMoveToPoseComplete() -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->moveToPoseValue.reset(); -} - -///////////////////////////////////////////////// -void GzRenderer::NewHoverEvent(const math::Vector2i &_hoverPos) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->mouseHoverPos = _hoverPos; - this->dataPtr->hoverDirty = true; -} - -///////////////////////////////////////////////// -void GzRenderer::NewMouseEvent(const common::MouseEvent &_e, - const math::Vector2d &_drag) -{ - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->mouseEvent = _e; - this->dataPtr->drag += _drag; - this->dataPtr->mouseDirty = true; -} - -///////////////////////////////////////////////// -math::Vector3d GzRenderer::ScreenToPlane( - const math::Vector2i &_screenPos) const -{ - // Normalize point on the image - double width = this->dataPtr->camera->ImageWidth(); - double height = this->dataPtr->camera->ImageHeight(); - - double nx = 2.0 * _screenPos.X() / width - 1.0; - double ny = 1.0 - 2.0 * _screenPos.Y() / height; - - // Make a ray query - this->dataPtr->rayQuery->SetFromCamera( - this->dataPtr->camera, math::Vector2d(nx, ny)); - - gz::math::Planed plane(gz::math::Vector3d(0, 0, 1), 0); - - math::Vector3d origin = this->dataPtr->rayQuery->Origin(); - math::Vector3d direction = this->dataPtr->rayQuery->Direction(); - double distance = plane.Distance(origin, direction); - return origin + direction * distance; -} - -///////////////////////////////////////////////// -math::Pose3d GzRenderer::CameraPose() const -{ - if (this->dataPtr->camera) - return this->dataPtr->camera->WorldPose(); - return math::Pose3d::Zero; -} - -///////////////////////////////////////////////// -math::Vector3d GzRenderer::ScreenToScene( - const math::Vector2i &_screenPos) const -{ - // Normalize point on the image - double width = this->dataPtr->camera->ImageWidth(); - double height = this->dataPtr->camera->ImageHeight(); - - double nx = 2.0 * _screenPos.X() / width - 1.0; - double ny = 1.0 - 2.0 * _screenPos.Y() / height; - - // Make a ray query - this->dataPtr->rayQuery->SetFromCamera( - this->dataPtr->camera, math::Vector2d(nx, ny)); - - auto result = this->dataPtr->rayQuery->ClosestPoint(); - if (result) - return result.point; - - // Set point to be 10m away if no intersection found - return this->dataPtr->rayQuery->Origin() + - this->dataPtr->rayQuery->Direction() * 10; -} - -//////////////////////////////////////////////// -void GzRenderer::RequestSelectionChange(Entity _selectedEntity, - bool _deselectAll, bool _sendEvent) -{ - this->dataPtr->selectionHelper = {_selectedEntity, _deselectAll, _sendEvent}; -} - -///////////////////////////////////////////////// -RenderThread::RenderThread() -{ - RenderWindowItemPrivate::threads << this; - qRegisterMetaType(); - qRegisterMetaType("RenderSync*"); -} - -///////////////////////////////////////////////// -void RenderThread::SetErrorCb(std::function _cb) -{ - this->errorCb = _cb; -} - -///////////////////////////////////////////////// -void RenderThread::RenderNext(RenderSync *_renderSync) -{ - this->context->makeCurrent(this->surface); - - if (!this->gzRenderer.initialized) - { - // Initialize renderer - auto loadingError = this->gzRenderer.Initialize(); - if (!loadingError.empty()) - { - this->errorCb(QString::fromStdString(loadingError)); - return; - } - } - - // check if engine has been successfully initialized - if (!this->gzRenderer.initialized) - { - gzerr << "Unable to initialize renderer" << std::endl; - return; - } - - this->gzRenderer.Render(_renderSync); - - emit TextureReady(this->gzRenderer.textureId, this->gzRenderer.textureSize); -} - -///////////////////////////////////////////////// -void RenderThread::ShutDown() -{ - if (this->context && this->surface) - this->context->makeCurrent(this->surface); - - this->gzRenderer.Destroy(); - - if (this->context) - { - this->context->doneCurrent(); - delete this->context; - } - - // schedule this to be deleted only after we're done cleaning up - if (this->surface) - this->surface->deleteLater(); - - // Stop event processing, move the thread to GUI and make sure it is deleted. - this->exit(); - if (this->gzRenderer.initialized) - this->moveToThread(QGuiApplication::instance()->thread()); -} - - -///////////////////////////////////////////////// -void RenderThread::SizeChanged() -{ - auto item = qobject_cast(this->sender()); - if (!item) - { - gzerr << "Internal error, sender is not QQuickItem." << std::endl; - return; - } - - if (item->width() <= 0 || item->height() <= 0) - return; - - this->gzRenderer.textureSize = QSize(item->width(), item->height()); - this->gzRenderer.textureDirty = true; -} - -///////////////////////////////////////////////// -TextureNode::TextureNode(QQuickWindow *_window, RenderSync &_renderSync) - : renderSync(_renderSync), window(_window) -{ - // Our texture node must have a texture, so use the default 0 texture. -#if QT_VERSION < QT_VERSION_CHECK(5, 14, 0) - this->texture = this->window->createTextureFromId(0, QSize(1, 1)); -#else - void * nativeLayout; - this->texture = this->window->createTextureFromNativeObject( - QQuickWindow::NativeObjectTexture, &nativeLayout, 0, QSize(1, 1), - QQuickWindow::TextureIsOpaque); -#endif - this->setTexture(this->texture); -} - -///////////////////////////////////////////////// -TextureNode::~TextureNode() -{ - delete this->texture; -} - -///////////////////////////////////////////////// -void TextureNode::NewTexture(uint _id, const QSize &_size) -{ - this->mutex.lock(); - this->id = _id; - this->size = _size; - this->mutex.unlock(); - - // We cannot call QQuickWindow::update directly here, as this is only allowed - // from the rendering thread or GUI thread. - emit PendingNewTexture(); -} - -///////////////////////////////////////////////// -void TextureNode::PrepareNode() -{ - this->mutex.lock(); - uint newId = this->id; - QSize sz = this->size; - this->id = 0; - this->mutex.unlock(); - if (newId) - { - delete this->texture; - // note: include QQuickWindow::TextureHasAlphaChannel if the rendered - // content has alpha. -#if QT_VERSION < QT_VERSION_CHECK(5, 14, 0) - this->texture = this->window->createTextureFromId( - newId, sz, QQuickWindow::TextureIsOpaque); -#else - // TODO(anyone) Use createTextureFromNativeObject - // https://github.com/gazebosim/gz-gui/issues/113 -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#endif - this->texture = this->window->createTextureFromId( - newId, sz, QQuickWindow::TextureIsOpaque); -#ifndef _WIN32 -# pragma GCC diagnostic pop -#endif - -#endif - this->setTexture(this->texture); - - this->markDirty(DirtyMaterial); - - // This will notify the rendering thread that the texture is now being - // rendered and it can start rendering to the other one. - // emit TextureInUse(&this->renderSync); See comment below - } - - // NOTE: The original code from Qt samples only emitted when - // newId is not null. - // - // This is correct... for their case. - // However we need to synchronize the threads when resolution changes, - // and we're also currently doing everything in lockstep (i.e. both Qt - // and worker thread are serialized, - // see https://github.com/gazebosim/gz-rendering/issues/304 ) - // - // We need to emit even if newId == 0 because it's safe as long as both - // threads are forcefully serialized and otherwise we may get a - // deadlock (this func. called twice in a row with the worker thread still - // finishing the 1st iteration, may result in a deadlock for newer versions - // of Qt; as WaitForWorkerThread will be called with no corresponding - // WaitForQtThreadAndBlock as the worker thread thinks there are - // no more jobs to do. - // - // If we want these to run in worker thread and stay resolution-synchronized, - // we probably should use a different method of signals and slots - // to send work to the worker thread and get results back - emit TextureInUse(&this->renderSync); - - this->renderSync.WaitForWorkerThread(); -} - -///////////////////////////////////////////////// -RenderWindowItem::RenderWindowItem(QQuickItem *_parent) - : QQuickItem(_parent), dataPtr(new RenderWindowItemPrivate) -{ - // TODO(chapulina) Forward-porting #1294 from gz-sim3 to gz-sim5 - // is non-trivial since the plugin's internals have changed. Keeping this - // shortcut here for now, and revisiting later specifically for gz-sim5 - // onwards. - // See https://github.com/gazebosim/gz-sim/issues/1254 - static bool done{false}; - if (done) - { - return; - } - done = true; - - this->setAcceptedMouseButtons(Qt::AllButtons); - this->setFlag(ItemHasContents); - this->dataPtr->renderThread = new RenderThread(); -} - -///////////////////////////////////////////////// -RenderWindowItem::~RenderWindowItem() -{ - this->dataPtr->renderSync.Shutdown(); - QMetaObject::invokeMethod(this->dataPtr->renderThread, - "ShutDown", - Qt::QueuedConnection); - - this->dataPtr->renderThread->wait(); -} - -///////////////////////////////////////////////// -void RenderWindowItem::Ready() -{ - this->dataPtr->renderThread->surface = new QOffscreenSurface(); - this->dataPtr->renderThread->surface->setFormat( - this->dataPtr->renderThread->context->format()); - this->dataPtr->renderThread->surface->create(); - - this->dataPtr->renderThread->gzRenderer.textureSize = - QSize(std::max({this->width(), 1.0}), std::max({this->height(), 1.0})); - - this->connect(&this->dataPtr->renderThread->gzRenderer, - &GzRenderer::ContextMenuRequested, - this, &RenderWindowItem::OnContextMenuRequested, Qt::QueuedConnection); - - this->connect(&this->dataPtr->renderThread->gzRenderer, - &GzRenderer::FollowTargetChanged, - this, &RenderWindowItem::SetFollowTarget, Qt::QueuedConnection); - - this->dataPtr->renderThread->moveToThread(this->dataPtr->renderThread); - - this->connect(this, &QQuickItem::widthChanged, - this->dataPtr->renderThread, &RenderThread::SizeChanged); - this->connect(this, &QQuickItem::heightChanged, - this->dataPtr->renderThread, &RenderThread::SizeChanged); - - this->dataPtr->renderThread->start(); - this->update(); - - this->dataPtr->rendererInit = true; -} - -///////////////////////////////////////////////// -bool RenderWindowItem::RendererInitialized() const -{ - return this->dataPtr->rendererInit; -} - -///////////////////////////////////////////////// -QSGNode *RenderWindowItem::updatePaintNode(QSGNode *_node, - QQuickItem::UpdatePaintNodeData *) -{ - auto node = static_cast(_node); - - if (!this->dataPtr->renderThread->context) - { - QOpenGLContext *current = this->window()->openglContext(); - // Some GL implementations require that the currently bound context is - // made non-current before we set up sharing, so we doneCurrent here - // and makeCurrent down below while setting up our own context. - current->doneCurrent(); - - this->dataPtr->renderThread->context = new QOpenGLContext(); - if (this->RenderUtil()->EngineName() == "ogre2") - { - // Although it seems unbelievable, we can request another format for a - // shared context; it is needed because Qt selects by default a compat - // context which is much less likely to provide OpenGL 3.3 (only closed - // NVidia drivers). This means there will be mismatch between what is - // reported by QSG_INFO=1 and by OGRE. - auto surfaceFormat = QSurfaceFormat(); - surfaceFormat.setMajorVersion(3); - surfaceFormat.setMinorVersion(3); - surfaceFormat.setProfile(QSurfaceFormat::CoreProfile); - this->dataPtr->renderThread->context->setFormat(surfaceFormat); - } - else - { - this->dataPtr->renderThread->context->setFormat(current->format()); - } - this->dataPtr->renderThread->context->setShareContext(current); - this->dataPtr->renderThread->context->create(); - this->dataPtr->renderThread->context->moveToThread( - this->dataPtr->renderThread); - - current->makeCurrent(this->window()); - - QMetaObject::invokeMethod(this, "Ready"); - return nullptr; - } - - if (!node) - { - node = new TextureNode(this->window(), this->dataPtr->renderSync); - - // Set up connections to get the production of render texture in sync with - // vsync on the rendering thread. - // - // When a new texture is ready on the rendering thread, we use a direct - // connection to the texture node to let it know a new texture can be used. - // The node will then emit PendingNewTexture which we bind to - // QQuickWindow::update to schedule a redraw. - // - // When the scene graph starts rendering the next frame, the PrepareNode() - // function is used to update the node with the new texture. Once it - // completes, it emits TextureInUse() which we connect to the rendering - // thread's RenderNext() to have it start producing content into its render - // texture. - // - // This rendering pipeline is throttled by vsync on the scene graph - // rendering thread. - - this->connect(this->dataPtr->renderThread, &RenderThread::TextureReady, - node, &TextureNode::NewTexture, Qt::DirectConnection); - this->connect(node, &TextureNode::PendingNewTexture, this->window(), - &QQuickWindow::update, Qt::QueuedConnection); - this->connect(this->window(), &QQuickWindow::beforeRendering, node, - &TextureNode::PrepareNode, Qt::DirectConnection); - this->connect(node, &TextureNode::TextureInUse, this->dataPtr->renderThread, - &RenderThread::RenderNext, Qt::QueuedConnection); - - // Get the production of FBO textures started.. - QMetaObject::invokeMethod(this->dataPtr->renderThread, "RenderNext", - Qt::QueuedConnection, - Q_ARG( RenderSync*, &node->renderSync )); - } - - node->setRect(this->boundingRect()); - - return node; -} - -/////////////////////////////////////////////////// -void RenderWindowItem::OnContextMenuRequested(QString _entity) -{ - emit openContextMenu(std::move(_entity)); -} - -/////////////////////////////////////////////////// -math::Vector3d RenderWindowItem::ScreenToScene(const math::Vector2i &_screenPos) -{ - return this->dataPtr->renderThread->gzRenderer.ScreenToScene(_screenPos); -} - -//////////////////////////////////////////////// -RenderUtil *RenderWindowItem::RenderUtil() const -{ - return this->dataPtr->renderThread->gzRenderer.RenderUtil(); -} - -///////////////////////////////////////////////// -Scene3D::Scene3D() - : GuiSystem(), dataPtr(new Scene3DPrivate) -{ - gzwarn << "The GzScene3D plugin is deprecated on v6 and will be removed on " - << "v7. Use MinimalScene together with other plugins as needed." - << std::endl; - qmlRegisterType("RenderWindow", 1, 0, "RenderWindow"); -} - -///////////////////////////////////////////////// -Scene3D::~Scene3D() = default; - -///////////////////////////////////////////////// -void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) -{ - // TODO(chapulina) Forward-porting #1294 from gz-sim3 to gz-sim5 - // is non-trivial since the plugin's internals have changed. Keeping this - // shortcut here for now, and revisiting later specifically for gz-sim5 - // onwards. - // See https://github.com/gazebosim/gz-sim/issues/1254 - static bool done{false}; - if (done) - { - gzerr << "Only one Scene3D is supported per process at the moment." - << std::endl; - return; - } - done = true; - - auto renderWindow = this->PluginItem()->findChild(); - if (!renderWindow) - { - gzerr << "Unable to find Render Window item. " - << "Render window will not be created" << std::endl; - return; - } - renderWindow->SetErrorCb(std::bind(&Scene3D::SetLoadingError, this, - std::placeholders::_1)); - - if (this->title.empty()) - this->title = "3D Scene"; - - this->dataPtr->renderUtil = renderWindow->RenderUtil(); - - renderWindow->forceActiveFocus(); - - // Custom parameters - if (_pluginElem) - { - if (auto elem = _pluginElem->FirstChildElement("engine")) - { - std::string engineName = elem->GetText(); - if (!engineName.empty()) - { - this->dataPtr->renderUtil->SetEngineName(engineName); - - // there is a problem with displaying ogre2 render textures that are in - // sRGB format. Workaround for now is to apply gamma correction - // manually. There maybe a better way to solve the problem by making - // OpenGL calls.. - if (engineName == std::string("ogre2")) - this->PluginItem()->setProperty("gammaCorrect", true); - } - } - - if (auto elem = _pluginElem->FirstChildElement("scene")) - { - this->dataPtr->renderUtil->SetSceneName(elem->GetText()); - } - - if (auto elem = _pluginElem->FirstChildElement("ambient_light")) - { - math::Color ambient; - std::stringstream colorStr; - colorStr << std::string(elem->GetText()); - colorStr >> ambient; - this->dataPtr->renderUtil->SetAmbientLight(ambient); - } - - if (auto elem = _pluginElem->FirstChildElement("background_color")) - { - math::Color bgColor; - std::stringstream colorStr; - colorStr << std::string(elem->GetText()); - colorStr >> bgColor; - this->dataPtr->renderUtil->SetBackgroundColor(bgColor); - } - - if (auto elem = _pluginElem->FirstChildElement("sky")) - { - this->dataPtr->renderUtil->SetSkyEnabled(true); - if (!elem->NoChildren()) - gzwarn << "Child elements of are not supported yet" << std::endl; - } - - if (auto elem = _pluginElem->FirstChildElement("camera_pose")) - { - math::Pose3d pose; - std::stringstream poseStr; - poseStr << std::string(elem->GetText()); - poseStr >> pose; - renderWindow->SetInitCameraPose(pose); - renderWindow->SetCameraPose(pose); - } - - if (auto elem = _pluginElem->FirstChildElement("camera_follow")) - { - if (auto gainElem = elem->FirstChildElement("p_gain")) - { - double gain; - std::stringstream gainStr; - gainStr << std::string(gainElem->GetText()); - gainStr >> gain; - if (gain >= 0 && gain <= 1.0) - renderWindow->SetFollowPGain(gain); - else - gzerr << "Camera follow p gain outside of range [0, 1]" << std::endl; - } - - if (auto targetElem = elem->FirstChildElement("target")) - { - std::stringstream targetStr; - targetStr << std::string(targetElem->GetText()); - renderWindow->SetFollowTarget(targetStr.str(), true); - } - - if (auto worldFrameElem = elem->FirstChildElement("world_frame")) - { - std::string worldFrameStr = - common::lowercase(worldFrameElem->GetText()); - if (worldFrameStr == "true" || worldFrameStr == "1") - renderWindow->SetFollowWorldFrame(true); - else if (worldFrameStr == "false" || worldFrameStr == "0") - renderWindow->SetFollowWorldFrame(false); - else - { - gzerr << "Faild to parse value: " << worldFrameStr - << std::endl; - } - } - - if (auto offsetElem = elem->FirstChildElement("offset")) - { - math::Vector3d offset; - std::stringstream offsetStr; - offsetStr << std::string(offsetElem->GetText()); - offsetStr >> offset; - renderWindow->SetFollowOffset(offset); - } - } - - if (auto elem = _pluginElem->FirstChildElement("record_video")) - { - if (auto useSimTimeElem = elem->FirstChildElement("use_sim_time")) - { - bool useSimTime = false; - if (useSimTimeElem->QueryBoolText(&useSimTime) != tinyxml2::XML_SUCCESS) - { - gzerr << "Faild to parse value: " - << useSimTimeElem->GetText() << std::endl; - } - else - { - renderWindow->SetRecordVideoUseSimTime(useSimTime); - } - } - if (auto lockstepElem = elem->FirstChildElement("lockstep")) - { - bool lockstep = false; - if (lockstepElem->QueryBoolText(&lockstep) != tinyxml2::XML_SUCCESS) - { - gzerr << "Failed to parse value: " - << lockstepElem->GetText() << std::endl; - } - else - { - renderWindow->SetRecordVideoLockstep(lockstep); - } - } - if (auto bitrateElem = elem->FirstChildElement("bitrate")) - { - unsigned int bitrate = 0u; - std::stringstream bitrateStr; - bitrateStr << std::string(bitrateElem->GetText()); - bitrateStr >> bitrate; - if (bitrate > 0u) - { - renderWindow->SetRecordVideoBitrate(bitrate); - } - else - { - gzerr << "Video recorder bitrate must be larger than 0" - << std::endl; - } - } - } - - if (auto elem = _pluginElem->FirstChildElement("fullscreen")) - { - auto fullscreen = false; - elem->QueryBoolText(&fullscreen); - if (fullscreen) - { - gz::gui::App()->findChild - ()->QuickWindow()->showFullScreen(); - } - } - - if (auto elem = _pluginElem->FirstChildElement("visibility_mask")) - { - uint32_t visibilityMask = 0xFFFFFFFFu; - std::stringstream visibilityMaskStr; - visibilityMaskStr << std::string(elem->GetText()); - bool isHex = common::lowercase( - visibilityMaskStr.str()).compare(0, 2, "0x") == 0; - if (isHex) - visibilityMaskStr >> std::hex >> visibilityMask; - else - visibilityMaskStr >> visibilityMask; - renderWindow->SetVisibilityMask(visibilityMask); - } - } - - // transform mode - this->dataPtr->transformModeService = - "/gui/transform_mode"; - this->dataPtr->node.Advertise(this->dataPtr->transformModeService, - &Scene3D::OnTransformMode, this); - gzmsg << "Transform mode service on [" - << this->dataPtr->transformModeService << "]" << std::endl; - - // video recorder - this->dataPtr->recordVideoService = - "/gui/record_video"; - this->dataPtr->node.Advertise(this->dataPtr->recordVideoService, - &Scene3D::OnRecordVideo, this); - gzmsg << "Record video service on [" - << this->dataPtr->recordVideoService << "]" << std::endl; - - // move to - this->dataPtr->moveToService = "/gui/move_to"; - this->dataPtr->node.Advertise(this->dataPtr->moveToService, - &Scene3D::OnMoveTo, this); - gzmsg << "Move to service on [" - << this->dataPtr->moveToService << "]" << std::endl; - - // follow - this->dataPtr->followService = "/gui/follow"; - this->dataPtr->node.Advertise(this->dataPtr->followService, - &Scene3D::OnFollow, this); - gzmsg << "Follow service on [" - << this->dataPtr->followService << "]" << std::endl; - - // follow offset - this->dataPtr->followOffsetService = "/gui/follow/offset"; - this->dataPtr->node.Advertise(this->dataPtr->followOffsetService, - &Scene3D::OnFollowOffset, this); - gzmsg << "Follow offset service on [" - << this->dataPtr->followOffsetService << "]" << std::endl; - - // view angle - this->dataPtr->viewAngleService = - "/gui/view_angle"; - this->dataPtr->node.Advertise(this->dataPtr->viewAngleService, - &Scene3D::OnViewAngle, this); - gzmsg << "View angle service on [" - << this->dataPtr->viewAngleService << "]" << std::endl; - - // move to pose service - this->dataPtr->moveToPoseService = - "/gui/move_to/pose"; - this->dataPtr->node.Advertise(this->dataPtr->moveToPoseService, - &Scene3D::OnMoveToPose, this); - gzmsg << "Move to pose service on [" - << this->dataPtr->moveToPoseService << "]" << std::endl; - - // camera position topic - this->dataPtr->cameraPoseTopic = "/gui/camera/pose"; - this->dataPtr->cameraPosePub = - this->dataPtr->node.Advertise(this->dataPtr->cameraPoseTopic); - gzmsg << "Camera pose topic advertised on [" - << this->dataPtr->cameraPoseTopic << "]" << std::endl; - - // view as transparent service - this->dataPtr->viewTransparentService = "/gui/view/transparent"; - this->dataPtr->node.Advertise(this->dataPtr->viewTransparentService, - &Scene3D::OnViewTransparent, this); - gzmsg << "View as transparent service on [" - << this->dataPtr->viewTransparentService << "]" << std::endl; - - // view center of mass service - this->dataPtr->viewCOMService = "/gui/view/com"; - this->dataPtr->node.Advertise(this->dataPtr->viewCOMService, - &Scene3D::OnViewCOM, this); - gzmsg << "View center of mass service on [" - << this->dataPtr->viewCOMService << "]" << std::endl; - - // view inertia service - this->dataPtr->viewInertiaService = "/gui/view/inertia"; - this->dataPtr->node.Advertise(this->dataPtr->viewInertiaService, - &Scene3D::OnViewInertia, this); - gzmsg << "View inertia service on [" - << this->dataPtr->viewInertiaService << "]" << std::endl; - - // view joints service - this->dataPtr->viewJointsService = "/gui/view/joints"; - this->dataPtr->node.Advertise(this->dataPtr->viewJointsService, - &Scene3D::OnViewJoints, this); - gzmsg << "View joints service on [" - << this->dataPtr->viewJointsService << "]" << std::endl; - - // view wireframes service - this->dataPtr->viewWireframesService = "/gui/view/wireframes"; - this->dataPtr->node.Advertise(this->dataPtr->viewWireframesService, - &Scene3D::OnViewWireframes, this); - gzmsg << "View wireframes service on [" - << this->dataPtr->viewWireframesService << "]" << std::endl; - - // view collisions service - this->dataPtr->viewCollisionsService = "/gui/view/collisions"; - this->dataPtr->node.Advertise(this->dataPtr->viewCollisionsService, - &Scene3D::OnViewCollisions, this); - gzmsg << "View collisions service on [" - << this->dataPtr->viewCollisionsService << "]" << std::endl; - - // camera view control mode - this->dataPtr->cameraViewControlService = "/gui/camera/view_control"; - this->dataPtr->node.Advertise(this->dataPtr->cameraViewControlService, - &Scene3D::OnViewControl, this); - gzmsg << "Camera view controller topic advertised on [" - << this->dataPtr->cameraViewControlService << "]" << std::endl; - - gz::gui::App()->findChild< - gz::gui::MainWindow *>()->QuickWindow()->installEventFilter(this); - gz::gui::App()->findChild< - gz::gui::MainWindow *>()->installEventFilter(this); -} - -////////////////////////////////////////////////// -void Scene3D::Update(const UpdateInfo &_info, - EntityComponentManager &_ecm) -{ - if (nullptr == this->dataPtr->renderUtil) - return; - - GZ_PROFILE("Scene3D::Update"); - auto renderWindow = this->PluginItem()->findChild(); - if (this->dataPtr->worldName.empty()) - { - // TODO(anyone) Only one scene is supported for now - Entity worldEntity; - _ecm.Each( - [&](const Entity &_entity, - const components::World * /* _world */ , - const components::Name *_name)->bool - { - this->dataPtr->worldName = _name->Data(); - worldEntity = _entity; - return true; - }); - - if (!this->dataPtr->worldName.empty()) - { - renderWindow->SetWorldName(this->dataPtr->worldName); - auto renderEngineGuiComp = - _ecm.Component(worldEntity); - if (renderEngineGuiComp && !renderEngineGuiComp->Data().empty()) - { - this->dataPtr->renderUtil->SetEngineName(renderEngineGuiComp->Data()); - } - else - { - gzdbg << "RenderEngineGuiPlugin component not found, " - "render engine won't be set from the ECM " << std::endl; - } - } - } - - if (this->dataPtr->cameraPosePub.HasConnections()) - { - msgs::Pose poseMsg = msgs::Convert(renderWindow->CameraPose()); - this->dataPtr->cameraPosePub.Publish(poseMsg); - } - this->dataPtr->renderUtil->UpdateECM(_info, _ecm); - this->dataPtr->renderUtil->UpdateFromECM(_info, _ecm); - - // check if video recording is enabled and if we need to lock step - // ECM updates with GUI rendering during video recording - std::unique_lock lock(this->dataPtr->recordMutex); - if (this->dataPtr->recording && this->dataPtr->recordVideoLockstep && - renderWindow->RendererInitialized()) - { - std::unique_lock lock2(this->dataPtr->renderMutex); - g_renderCv.wait(lock2); - } -} - -///////////////////////////////////////////////// -bool Scene3D::OnTransformMode(const msgs::StringMsg &_msg, - msgs::Boolean &_res) -{ - auto renderWindow = this->PluginItem()->findChild(); - renderWindow->SetTransformMode(_msg.data()); - - _res.set_data(true); - return true; -} - -///////////////////////////////////////////////// -bool Scene3D::OnRecordVideo(const msgs::VideoRecord &_msg, - msgs::Boolean &_res) -{ - auto renderWindow = this->PluginItem()->findChild(); - - bool record = _msg.start() && !_msg.stop(); - renderWindow->SetRecordVideo(record, _msg.format(), _msg.save_filename()); - - _res.set_data(true); - - std::unique_lock lock(this->dataPtr->recordMutex); - this->dataPtr->recording = record; - return true; -} - -///////////////////////////////////////////////// -bool Scene3D::OnMoveTo(const msgs::StringMsg &_msg, - msgs::Boolean &_res) -{ - auto renderWindow = this->PluginItem()->findChild(); - - renderWindow->SetMoveTo(_msg.data()); - - _res.set_data(true); - return true; -} - -///////////////////////////////////////////////// -bool Scene3D::OnFollow(const msgs::StringMsg &_msg, - msgs::Boolean &_res) -{ - auto renderWindow = this->PluginItem()->findChild(); - - renderWindow->SetFollowTarget(_msg.data()); - - _res.set_data(true); - return true; -} - -///////////////////////////////////////////////// -bool Scene3D::OnFollowOffset(const msgs::Vector3d &_msg, - msgs::Boolean &_res) -{ - auto renderWindow = this->PluginItem()->findChild(); - - math::Vector3d offset = msgs::Convert(_msg); - renderWindow->SetFollowOffset(offset); - - _res.set_data(true); - return true; -} - -///////////////////////////////////////////////// -bool Scene3D::OnViewAngle(const msgs::Vector3d &_msg, - msgs::Boolean &_res) -{ - auto renderWindow = this->PluginItem()->findChild(); - - renderWindow->SetViewAngle(msgs::Convert(_msg)); - - _res.set_data(true); - return true; -} - -///////////////////////////////////////////////// -bool Scene3D::OnMoveToPose(const msgs::GUICamera &_msg, msgs::Boolean &_res) -{ - auto renderWindow = this->PluginItem()->findChild(); - - math::Pose3d pose = msgs::Convert(_msg.pose()); - - // If there is no orientation in the message, then set a Rot value in the - // math::Pose3d object to infinite. This will prevent the orientation from - // being used when positioning the camera. - // See the MoveToHelper::MoveTo function - if (!_msg.pose().has_orientation()) - pose.Rot().X() = math::INF_D; - - // If there is no position in the message, then set a Pos value in the - // math::Pose3d object to infinite. This will prevent the orientation from - // being used when positioning the camera. - // See the MoveToHelper::MoveTo function - if (!_msg.pose().has_position()) - pose.Pos().X() = math::INF_D; - - renderWindow->SetMoveToPose(pose); - - _res.set_data(true); - return true; -} - -///////////////////////////////////////////////// -bool Scene3D::OnViewTransparent(const msgs::StringMsg &_msg, - msgs::Boolean &_res) -{ - auto renderWindow = this->PluginItem()->findChild(); - - renderWindow->SetViewTransparentTarget(_msg.data()); - - _res.set_data(true); - return true; -} - -///////////////////////////////////////////////// -bool Scene3D::OnViewCOM(const msgs::StringMsg &_msg, - msgs::Boolean &_res) -{ - auto renderWindow = this->PluginItem()->findChild(); - - renderWindow->SetViewCOMTarget(_msg.data()); - - _res.set_data(true); - return true; -} - -///////////////////////////////////////////////// -bool Scene3D::OnViewInertia(const msgs::StringMsg &_msg, - msgs::Boolean &_res) -{ - auto renderWindow = this->PluginItem()->findChild(); - - renderWindow->SetViewInertiaTarget(_msg.data()); - - _res.set_data(true); - return true; -} - -///////////////////////////////////////////////// -bool Scene3D::OnViewJoints(const msgs::StringMsg &_msg, - msgs::Boolean &_res) -{ - auto renderWindow = this->PluginItem()->findChild(); - - renderWindow->SetViewJointsTarget(_msg.data()); - - _res.set_data(true); - return true; -} - -///////////////////////////////////////////////// -bool Scene3D::OnViewWireframes(const msgs::StringMsg &_msg, - msgs::Boolean &_res) -{ - auto renderWindow = this->PluginItem()->findChild(); - - renderWindow->SetViewWireframesTarget(_msg.data()); - - _res.set_data(true); - return true; -} - -///////////////////////////////////////////////// -bool Scene3D::OnViewCollisions(const msgs::StringMsg &_msg, - msgs::Boolean &_res) -{ - auto renderWindow = this->PluginItem()->findChild(); - - renderWindow->SetViewCollisionsTarget(_msg.data()); - - _res.set_data(true); - return true; -} - -///////////////////////////////////////////////// -bool Scene3D::OnViewControl(const msgs::StringMsg &_msg, - msgs::Boolean &_res) -{ - auto renderWindow = this->PluginItem()->findChild(); - - renderWindow->SetViewController(_msg.data()); - - _res.set_data(true); - return true; -} - - -///////////////////////////////////////////////// -void Scene3D::OnHovered(int _mouseX, int _mouseY) -{ - auto renderWindow = this->PluginItem()->findChild(); - renderWindow->OnHovered({_mouseX, _mouseY}); -} - -///////////////////////////////////////////////// -void Scene3D::OnDropped(const QString &_drop, int _mouseX, int _mouseY) -{ - if (_drop.toStdString().empty()) - { - this->SetErrorPopupText("Dropped empty entity URI."); - return; - } - - std::function cb = - [](const gz::msgs::Boolean &_res, const bool _result) - { - if (!_result || !_res.data()) - gzerr << "Error creating dropped entity." << std::endl; - }; - - auto renderWindow = this->PluginItem()->findChild(); - math::Vector3d pos = renderWindow->ScreenToScene({_mouseX, _mouseY}); - - msgs::EntityFactory req; - std::string dropStr = _drop.toStdString(); - if (QUrl(_drop).isLocalFile()) - { - // mesh to sdf model - common::rtrim(dropStr); - - if (!common::MeshManager::Instance()->IsValidFilename(dropStr)) - { - QString errTxt = QString::fromStdString("Invalid URI: " + dropStr + - "\nOnly Fuel URLs or mesh file types DAE, OBJ, and STL are supported."); - this->SetErrorPopupText(errTxt); - return; - } - - // Fixes whitespace - dropStr = common::replaceAll(dropStr, "%20", " "); - - std::string filename = common::basename(dropStr); - std::vector splitName = common::split(filename, "."); - - std::string sdf = "" - "" - "" - "" - "" - "" - "" - "" + dropStr + "" - "" - "" - "" - "" - "" - "" - "" + dropStr + "" - "" - "" - "" - "" - "" - ""; - - req.set_sdf(sdf); - } - else - { - // model from fuel - req.set_sdf_filename(dropStr); - } - - req.set_allow_renaming(true); - msgs::Set(req.mutable_pose(), - math::Pose3d(pos.X(), pos.Y(), pos.Z(), 1, 0, 0, 0)); - - this->dataPtr->node.Request("/world/" + this->dataPtr->worldName + "/create", - req, cb); -} - -///////////////////////////////////////////////// -QString Scene3D::ErrorPopupText() const -{ - return this->dataPtr->errorPopupText; -} - -///////////////////////////////////////////////// -void Scene3D::SetErrorPopupText(const QString &_errorTxt) -{ - this->dataPtr->errorPopupText = _errorTxt; - this->ErrorPopupTextChanged(); - this->popupError(); -} - -///////////////////////////////////////////////// -void Scene3D::OnFocusWindow() -{ - auto renderWindow = this->PluginItem()->findChild(); - renderWindow->forceActiveFocus(); -} - -///////////////////////////////////////////////// -QString Scene3D::LoadingError() const -{ - return this->loadingError; -} - -///////////////////////////////////////////////// -void Scene3D::SetLoadingError(const QString &_loadingError) -{ - this->loadingError = _loadingError; - this->LoadingErrorChanged(); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetXYZSnap(const math::Vector3d &_xyz) -{ - this->dataPtr->renderThread->gzRenderer.SetXYZSnap(_xyz); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetRPYSnap(const math::Vector3d &_rpy) -{ - this->dataPtr->renderThread->gzRenderer.SetRPYSnap(_rpy); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetScaleSnap(const math::Vector3d &_scale) -{ - this->dataPtr->renderThread->gzRenderer.SetScaleSnap(_scale); -} - -///////////////////////////////////////////////// -bool Scene3D::eventFilter(QObject *_obj, QEvent *_event) -{ - if (_event->type() == QEvent::KeyPress) - { - QKeyEvent *keyEvent = static_cast(_event); - if (keyEvent) - { - auto renderWindow = this->PluginItem()->findChild(); - renderWindow->HandleKeyPress(keyEvent); - } - } - else if (_event->type() == QEvent::KeyRelease) - { - QKeyEvent *keyEvent = static_cast(_event); - if (keyEvent) - { - auto renderWindow = this->PluginItem()->findChild(); - renderWindow->HandleKeyRelease(keyEvent); - } - } - else if (_event->type() == - gz::sim::gui::events::EntitiesSelected::kType) - { - auto selectedEvent = - reinterpret_cast( - _event); - if (selectedEvent) - { - for (const auto &entity : selectedEvent->Data()) - { - // If the event is from the user, update render util state - if (!selectedEvent->FromUser()) - continue; - - auto node = this->dataPtr->renderUtil->SceneManager().NodeById(entity); - - if (nullptr == node) - { - // If an unknown entity has been selected, and control is not pressed, - // deselect all known selected entities - if (!(QGuiApplication::keyboardModifiers() & Qt::ControlModifier)) - { - auto renderWindow = - this->PluginItem()->findChild(); - renderWindow->DeselectAllEntities(false); - } - continue; - } - - auto renderWindow = this->PluginItem()->findChild(); - renderWindow->UpdateSelectedEntity(entity, false); - } - } - } - else if (_event->type() == - gz::sim::gui::events::DeselectAllEntities::kType) - { - auto deselectEvent = - reinterpret_cast( - _event); - - // If the event is from the user, update render util state - if (deselectEvent && deselectEvent->FromUser()) - { - auto renderWindow = this->PluginItem()->findChild(); - renderWindow->DeselectAllEntities(false); - } - } - else if (_event->type() == - gz::gui::events::SnapIntervals::kType) - { - auto snapEvent = reinterpret_cast( - _event); - if (snapEvent) - { - auto renderWindow = this->PluginItem()->findChild(); - renderWindow->SetXYZSnap(snapEvent->Position()); - renderWindow->SetRPYSnap(snapEvent->Rotation()); - renderWindow->SetScaleSnap(snapEvent->Scale()); - } - } - else if (_event->type() == - gz::gui::events::SpawnFromDescription::kType) - { - auto spawnPreviewEvent = reinterpret_cast< - gz::gui::events::SpawnFromDescription *>(_event); - if (spawnPreviewEvent) - { - auto renderWindow = this->PluginItem()->findChild(); - renderWindow->SetModel(spawnPreviewEvent->Description()); - } - } - else if (_event->type() == gz::gui::events::SpawnFromPath::kType) - { - auto spawnPreviewPathEvent = - reinterpret_cast(_event); - if (spawnPreviewPathEvent) - { - auto renderWindow = this->PluginItem()->findChild(); - renderWindow->SetModelPath(spawnPreviewPathEvent->FilePath()); - } - } - else if (_event->type() == - gz::gui::events::DropdownMenuEnabled::kType) - { - auto dropdownMenuEnabledEvent = - reinterpret_cast(_event); - if (dropdownMenuEnabledEvent) - { - auto renderWindow = this->PluginItem()->findChild(); - renderWindow->SetDropdownMenuEnabled( - dropdownMenuEnabledEvent->MenuEnabled()); - } - } - - // Standard event processing - return QObject::eventFilter(_obj, _event); -} - -///////////////////////////////////////////////// -void RenderWindowItem::UpdateSelectedEntity(Entity _entity, - bool _sendEvent) -{ - this->dataPtr->renderThread->gzRenderer.RequestSelectionChange( - _entity, false, _sendEvent); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetTransformMode(const std::string &_mode) -{ - this->dataPtr->renderThread->gzRenderer.SetTransformMode(_mode); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetModel(const std::string &_model) -{ - this->dataPtr->renderThread->gzRenderer.SetModel(_model); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetModelPath(const std::string &_filePath) -{ - this->dataPtr->renderThread->gzRenderer.SetModelPath(_filePath); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetDropdownMenuEnabled(bool _enableDropdownMenu) -{ - this->dataPtr->renderThread->gzRenderer.SetDropdownMenuEnabled( - _enableDropdownMenu); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetRecordVideo(bool _record, const std::string &_format, - const std::string &_savePath) -{ - this->dataPtr->renderThread->gzRenderer.SetRecordVideo(_record, _format, - _savePath); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetMoveTo(const std::string &_target) -{ - this->dataPtr->renderThread->gzRenderer.SetMoveTo(_target); -} - -///////////////////////////////////////////////// -void RenderWindowItem::DeselectAllEntities(bool _sendEvent) -{ - this->dataPtr->renderThread->gzRenderer.RequestSelectionChange( - kNullEntity, true, _sendEvent); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetFollowTarget(const std::string &_target, - bool _waitForTarget) -{ - this->setProperty("message", _target.empty() ? "" : - "Press Escape to exit Follow mode"); - this->dataPtr->renderThread->gzRenderer.SetFollowTarget(_target, - _waitForTarget); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetViewAngle(const math::Vector3d &_direction) -{ - this->dataPtr->renderThread->gzRenderer.SetViewAngle(_direction); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetMoveToPose(const math::Pose3d &_pose) -{ - this->dataPtr->renderThread->gzRenderer.SetMoveToPose(_pose); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetViewTransparentTarget(const std::string &_target) -{ - this->dataPtr->renderThread->gzRenderer.SetViewTransparentTarget(_target); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetViewCOMTarget(const std::string &_target) -{ - this->dataPtr->renderThread->gzRenderer.SetViewCOMTarget(_target); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetViewInertiaTarget(const std::string &_target) -{ - this->dataPtr->renderThread->gzRenderer.SetViewInertiaTarget(_target); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetViewJointsTarget(const std::string &_target) -{ - this->dataPtr->renderThread->gzRenderer.SetViewJointsTarget(_target); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetViewWireframesTarget(const std::string &_target) -{ - this->dataPtr->renderThread->gzRenderer.SetViewWireframesTarget(_target); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetViewCollisionsTarget(const std::string &_target) -{ - this->dataPtr->renderThread->gzRenderer.SetViewCollisionsTarget(_target); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetViewController(const std::string &_controller) -{ - this->dataPtr->renderThread->gzRenderer.SetViewController(_controller); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetFollowPGain(double _gain) -{ - this->dataPtr->renderThread->gzRenderer.SetFollowPGain(_gain); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetFollowWorldFrame(bool _worldFrame) -{ - this->dataPtr->renderThread->gzRenderer.SetFollowWorldFrame(_worldFrame); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetFollowOffset(const math::Vector3d &_offset) -{ - this->dataPtr->renderThread->gzRenderer.SetFollowOffset(_offset); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetCameraPose(const math::Pose3d &_pose) -{ - this->dataPtr->renderThread->gzRenderer.cameraPose = _pose; -} - -///////////////////////////////////////////////// -math::Pose3d RenderWindowItem::CameraPose() const -{ - if (this->dataPtr->renderThread) - return this->dataPtr->renderThread->gzRenderer.CameraPose(); - return math::Pose3d::Zero; -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetInitCameraPose(const math::Pose3d &_pose) -{ - this->dataPtr->renderThread->gzRenderer.SetInitCameraPose(_pose); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetWorldName(const std::string &_name) -{ - this->dataPtr->renderThread->gzRenderer.worldName = _name; -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetRecordVideoUseSimTime(bool _useSimTime) -{ - this->dataPtr->renderThread->gzRenderer.SetRecordVideoUseSimTime( - _useSimTime); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetRecordVideoLockstep(bool _lockstep) -{ - this->dataPtr->renderThread->gzRenderer.SetRecordVideoLockstep( - _lockstep); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetRecordVideoBitrate(unsigned int _bitrate) -{ - this->dataPtr->renderThread->gzRenderer.SetRecordVideoBitrate( - _bitrate); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetVisibilityMask(uint32_t _mask) -{ - this->dataPtr->renderThread->gzRenderer.visibilityMask = _mask; -} - -///////////////////////////////////////////////// -void RenderWindowItem::OnHovered(const gz::math::Vector2i &_hoverPos) -{ - this->dataPtr->renderThread->gzRenderer.NewHoverEvent(_hoverPos); -} - -///////////////////////////////////////////////// -void RenderWindowItem::SetErrorCb(std::function _cb) -{ - this->dataPtr->renderThread->SetErrorCb(_cb); -} - -///////////////////////////////////////////////// -void RenderWindowItem::mousePressEvent(QMouseEvent *_e) -{ - this->forceActiveFocus(); - - auto event = gz::gui::convert(*_e); - event.SetPressPos(event.Pos()); - this->dataPtr->mouseEvent = event; - this->dataPtr->mouseEvent.SetType(common::MouseEvent::PRESS); - - this->dataPtr->renderThread->gzRenderer.NewMouseEvent( - this->dataPtr->mouseEvent); -} - -//////////////////////////////////////////////// -void RenderWindowItem::mouseReleaseEvent(QMouseEvent *_e) -{ - auto event = gz::gui::convert(*_e); - event.SetPressPos(this->dataPtr->mouseEvent.PressPos()); - - // A release at the end of a drag - if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::MOVE) - event.SetDragging(this->dataPtr->mouseEvent.Dragging()); - - this->dataPtr->mouseEvent = event; - this->dataPtr->mouseEvent.SetType(common::MouseEvent::RELEASE); - - this->dataPtr->renderThread->gzRenderer.NewMouseEvent( - this->dataPtr->mouseEvent); -} - -//////////////////////////////////////////////// -void RenderWindowItem::mouseMoveEvent(QMouseEvent *_e) -{ - auto event = gz::gui::convert(*_e); - - if (!event.Dragging()) - return; - - event.SetPressPos(this->dataPtr->mouseEvent.PressPos()); - - auto dragInt = event.Pos() - this->dataPtr->mouseEvent.Pos(); - auto dragDistance = math::Vector2d(dragInt.X(), dragInt.Y()); - - this->dataPtr->mouseEvent = event; - this->dataPtr->mouseEvent.SetType(common::MouseEvent::MOVE); - this->dataPtr->renderThread->gzRenderer.NewMouseEvent( - this->dataPtr->mouseEvent, dragDistance); -} - -//////////////////////////////////////////////// -void RenderWindowItem::wheelEvent(QWheelEvent *_e) -{ - this->forceActiveFocus(); - - this->dataPtr->mouseEvent.SetType(common::MouseEvent::SCROLL); -#if QT_VERSION < QT_VERSION_CHECK(5, 14, 0) - this->dataPtr->mouseEvent.SetPos(_e->x(), _e->y()); -#else - this->dataPtr->mouseEvent.SetPos(_e->position().x(), _e->position().y()); -#endif - double scroll = (_e->angleDelta().y() > 0) ? -1.0 : 1.0; - this->dataPtr->renderThread->gzRenderer.NewMouseEvent( - this->dataPtr->mouseEvent, math::Vector2d(scroll, scroll)); -} - -//////////////////////////////////////////////// -void RenderWindowItem::HandleKeyPress(QKeyEvent *_e) -{ - this->dataPtr->renderThread->gzRenderer.HandleKeyPress(_e); -} - -//////////////////////////////////////////////// -void RenderWindowItem::HandleKeyRelease(QKeyEvent *_e) -{ - this->dataPtr->renderThread->gzRenderer.HandleKeyRelease(_e); - - if (_e->key() == Qt::Key_Escape) - { - if (!this->dataPtr->renderThread->gzRenderer.FollowTarget().empty()) - { - this->SetFollowTarget(std::string()); - this->setProperty("message", ""); - - _e->accept(); - } - } -} - -/////////////////////////////////////////////////// -// void Scene3D::resizeEvent(QResizeEvent *_e) -// { -// if (this->dataPtr->renderWindow) -// { -// this->dataPtr->renderWindow->OnResize(_e->size().width(), -// _e->size().height()); -// } -// -// if (this->dataPtr->camera) -// { -// this->dataPtr->camera->SetAspectRatio( -// static_cast(this->width()) / this->height()); -// this->dataPtr->camera->SetHFOV(M_PI * 0.5); -// } -// } -// - -// Register this plugin -GZ_ADD_PLUGIN(gz::sim::Scene3D, - gz::gui::Plugin) diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh deleted file mode 100644 index e750d881e1..0000000000 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ /dev/null @@ -1,943 +0,0 @@ -/* - * Copyright (C) 2019 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#ifndef GZ_SIM_GUI_SCENE3D_HH_ -#define GZ_SIM_GUI_SCENE3D_HH_ - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include - -#include - -#include - -#include "gz/gui/qt.h" - - -namespace gz -{ -namespace sim -{ -// Inline bracket to help doxygen filtering. -inline namespace GZ_SIM_VERSION_NAMESPACE { - class GzRendererPrivate; - class RenderWindowItemPrivate; - class Scene3DPrivate; - class RenderUtil; - - /// \brief Creates a Gazebo Rendering scene and user camera. - /// It is possible to orbit the camera around the scene with - /// the mouse. Use other plugins to manage objects in the scene. - /// - /// Only one plugin displaying a Gazebo Rendering scene can be used at a - /// time. - /// - /// ## Configuration - /// - /// * \ : Optional render engine name, defaults to 'ogre'. - /// * \ : Optional scene name, defaults to 'scene'. The plugin will - /// create a scene with this name if there isn't one yet. If - /// there is already one, a new camera is added to it. - /// * \ : Optional color for ambient light, defaults to - /// (0.3, 0.3, 0.3, 1.0) - /// * \ : Optional background color, defaults to - /// (0.3, 0.3, 0.3, 1.0) - /// * \ : Optional starting pose for the camera, defaults to - /// (0, 0, 5, 0, 0, 0) - /// * \ : - /// * \ : Camera follow movement p gain. - /// * \ : Target to follow. - /// * \ : Optional starting the window in fullscreen. - class Scene3D : public gz::sim::GuiSystem - { - Q_OBJECT - - /// \brief Text for popup error - Q_PROPERTY( - QString errorPopupText - READ ErrorPopupText - WRITE SetErrorPopupText - NOTIFY ErrorPopupTextChanged - ) - - /// \brief Loading error message - Q_PROPERTY( - QString loadingError - READ LoadingError - WRITE SetLoadingError - NOTIFY LoadingErrorChanged - ) - - /// \brief Constructor - public: Scene3D(); - - /// \brief Destructor - public: ~Scene3D() override; - - // Documentation inherited - public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; - - // Documentation inherited - public: void Update(const UpdateInfo &_info, - EntityComponentManager &_ecm) override; - - /// \brief Callback when receives a drop event. - /// \param[in] _drop Dropped string. - /// \param[in] _mouseX x coordinate of mouse position. - /// \param[in] _mouseY y coordinate of mouse position. - public slots: void OnDropped(const QString &_drop, - int _mouseX, int _mouseY); - - /// \brief Callback when the mouse hovers to a new position. - /// \param[in] _mouseX x coordinate of the hovered mouse position. - /// \param[in] _mouseY y coordinate of the hovered mouse position. - public slots: void OnHovered(int _mouseX, int _mouseY); - - /// \brief Callback when the mouse enters the render window to - /// focus the window for mouse/key events - public slots: void OnFocusWindow(); - - // Documentation inherited - protected: bool eventFilter(QObject *_obj, QEvent *_event) override; - - /// \brief Callback for a transform mode request - /// \param[in] _msg Request message to set a new transform mode - /// \param[in] _res Response data - /// \return True if the request is received - private: bool OnTransformMode(const msgs::StringMsg &_msg, - msgs::Boolean &_res); - - /// \brief Callback for a record video request - /// \param[in] _msg Request message to enable/disable video recording. - /// \param[in] _res Response data - /// \return True if the request is received - private: bool OnRecordVideo(const msgs::VideoRecord &_msg, - msgs::Boolean &_res); - - /// \brief Callback for a move to request - /// \param[in] _msg Request message to set the target to move to. - /// \param[in] _res Response data - /// \return True if the request is received - private: bool OnMoveTo(const msgs::StringMsg &_msg, - msgs::Boolean &_res); - - /// \brief Callback for a follow request - /// \param[in] _msg Request message to set the target to follow. - /// \param[in] _res Response data - /// \return True if the request is received - private: bool OnFollow(const msgs::StringMsg &_msg, - msgs::Boolean &_res); - - /// \brief Callback for a follow offset request - /// \param[in] _msg Request message to set the camera's follow offset. - /// \param[in] _res Response data - /// \return True if the request is received - private: bool OnFollowOffset(const msgs::Vector3d &_msg, - msgs::Boolean &_res); - - /// \brief Callback for a view angle request - /// \param[in] _msg Request message to set the camera to. - /// \param[in] _res Response data - /// \return True if the request is received - private: bool OnViewAngle(const msgs::Vector3d &_msg, - msgs::Boolean &_res); - - /// \brief Callback for a move to pose request. - /// \param[in] _msg GUICamera request message. - /// \param[in] _res Response data - /// \return True if the request is received - private: bool OnMoveToPose(const msgs::GUICamera &_msg, - msgs::Boolean &_res); - - /// \brief Callback for view as transparent request - /// \param[in] _msg Request message to set the target to view as - /// transparent - /// \param[in] _res Response data - /// \return True if the request is received - private: bool OnViewTransparent(const msgs::StringMsg &_msg, - msgs::Boolean &_res); - - /// \brief Callback for view center of mass request - /// \param[in] _msg Request message to set the target to view center of - /// mass - /// \param[in] _res Response data - /// \return True if the request is received - private: bool OnViewCOM(const msgs::StringMsg &_msg, - msgs::Boolean &_res); - - /// \brief Callback for view inertia request - /// \param[in] _msg Request message to set the target to view inertia - /// \param[in] _res Response data - /// \return True if the request is received - private: bool OnViewInertia(const msgs::StringMsg &_msg, - msgs::Boolean &_res); - - /// \brief Callback for view joints request - /// \param[in] _msg Request message to set the target to view joints - /// \param[in] _res Response data - /// \return True if the request is received - private: bool OnViewJoints(const msgs::StringMsg &_msg, - msgs::Boolean &_res); - - /// \brief Callback for view wireframes request - /// \param[in] _msg Request message to set the target to view wireframes - /// \param[in] _res Response data - /// \return True if the request is received - private: bool OnViewWireframes(const msgs::StringMsg &_msg, - msgs::Boolean &_res); - - /// \brief Callback for view collisions request - /// \param[in] _msg Request message to set the target to view collisions - /// \param[in] _res Response data - /// \return True if the request is received - private: bool OnViewCollisions(const msgs::StringMsg &_msg, - msgs::Boolean &_res); - - /// \brief Get the text for the popup error message - /// \return The error text - public: Q_INVOKABLE QString ErrorPopupText() const; - - /// \brief Set the text for the popup error message - /// \param[in] _errorTxt The error text - public: Q_INVOKABLE void SetErrorPopupText(const QString &_errorTxt); - - /// \brief Notify the popup error text has changed - signals: void ErrorPopupTextChanged(); - - /// \brief Notify that an error has occurred (opens popup) - /// Note that the function name needs to start with lowercase in order for - /// the connection to work on the QML side - signals: void popupError(); - - /// \brief Callback for camera view controller request - /// \param[in] _msg Request message to set the camera view controller - /// \param[in] _res Response data - /// \return True if the request is received - private: bool OnViewControl(const msgs::StringMsg &_msg, - msgs::Boolean &_res); - - /// \brief Get the loading error string. - /// \return String explaining the loading error. If empty, there's no error. - public: Q_INVOKABLE QString LoadingError() const; - - /// \brief Set the loading error message. - /// \param[in] _loadingError Error message. - public: Q_INVOKABLE void SetLoadingError(const QString &_loadingError); - - /// \brief Notify that loading error has changed - signals: void LoadingErrorChanged(); - - /// \brief Loading error message - public: QString loadingError; - - /// \internal - /// \brief Pointer to private data. - private: std::unique_ptr dataPtr; - }; - - class RenderSync; - - /// \brief gz-rendering renderer. - /// All gz-rendering calls should be performed inside this class as it makes - /// sure that opengl calls in the underlying render engine do not interfere - /// with QtQuick's opengl render operations. The main Render function will - /// render to an offscreen texture and notify via signal and slots when it's - /// ready to be displayed. - class GzRenderer : public QObject - { - Q_OBJECT - - /// \brief Constructor - public: GzRenderer(); - - /// \brief Destructor - public: ~GzRenderer() override; - - /// \brief Main render function - /// \param[in] _renderSync RenderSync to safely - /// synchronize Qt and worker thread (this) - public: void Render(RenderSync *_renderSync); - - /// \brief Initialize the render engine - /// \return Error message if initialization failed. If empty, no errors - /// occurred. - public: std::string Initialize(); - - /// \brief Destroy camera associated with this renderer - public: void Destroy(); - - /// \brief Set the renderer - public: class RenderUtil *RenderUtil() const; - - /// \brief Set the transform mode - /// \param[in] _mode New transform mode to set to - public: void SetTransformMode(const std::string &_mode); - - /// \brief Set the model to hover over the scene. - /// \param[in] _model Sdf string of the model to load in for the user. - public: void SetModel(const std::string &_model); - - /// \brief Set the path to the model to hover over the scene. - /// \param[in] _filePath Sdf path of the model to load in for the user. - public: void SetModelPath(const std::string &_filePath); - - /// \brief Set if the dropdown menu is enabled or disabled. - /// \param[in] _enableDropdownMenu The boolean to enable or disable - /// the dropdown menu - public: void SetDropdownMenuEnabled(bool _enableDropdownMenu); - - /// \brief Set whether to record video - /// \param[in] _record True to start video recording, false to stop. - /// \param[in] _format Video encoding format: "mp4", "ogv" - /// \param[in] _savePath Path to save the recorded video. - public: void SetRecordVideo(bool _record, const std::string &_format, - const std::string &_savePath); - - /// \brief Set whether to record video using sim time as timestamp - /// \param[in] _useSimTime True record video using sim time - public: void SetRecordVideoUseSimTime(bool _useSimTime); - - /// \brief Set whether to record video in lockstep mode - /// \param[in] _lockstep True to record video in lockstep mode - public: void SetRecordVideoLockstep(bool _lockstep); - - /// \brief Set video recorder bitrate in bps - /// \param[in] _bitrate Bit rate to set to - public: void SetRecordVideoBitrate(unsigned int _bitrate); - - /// \brief Move the user camera to move to the speficied target - /// \param[in] _target Target to move the camera to - public: void SetMoveTo(const std::string &_target); - - /// \brief Move the user camera to follow the speficied target - /// \param[in] _target Target to follow - /// \param[in] _waitForTarget True to continuously look for the target - /// to follow. A typical use case is when following a target that is not - /// present on startup but spawned later into simulation - public: void SetFollowTarget(const std::string &_target, - bool _waitForTarget = false); - - /// \brief Set the viewing angle of the camera - /// \param[in] _direction The pose to assume relative to the entit(y/ies). - /// (0, 0, 0) indicates to return the camera back to the home pose - /// originally loaded in from the sdf - public: void SetViewAngle(const math::Vector3d &_direction); - - /// \brief Set the world pose of the camera - /// \param[in] _pose The world pose to set the camera to. - public: void SetMoveToPose(const math::Pose3d &_pose); - - /// \brief View the specified target as transparent - /// \param[in] _target Target to view as transparent - public: void SetViewTransparentTarget(const std::string &_target); - - /// \brief View center of mass of the specified target - /// \param[in] _target Target to view center of mass - public: void SetViewCOMTarget(const std::string &_target); - - /// \brief View inertia of the specified target - /// \param[in] _target Target to view inertia - public: void SetViewInertiaTarget(const std::string &_target); - - /// \brief View joints of the specified target - /// \param[in] _target Target to view joints - public: void SetViewJointsTarget(const std::string &_target); - - /// \brief View wireframes of the specified target - /// \param[in] _target Target to view wireframes - public: void SetViewWireframesTarget(const std::string &_target); - - /// \brief View collisions of the specified target - /// \param[in] _target Target to view collisions - public: void SetViewCollisionsTarget(const std::string &_target); - - /// \brief Set camera view controller - /// \param[in] _viewController. Values are "orbit", and "ortho" - public: void SetViewController(const std::string &_viewController); - - /// \brief Set the p gain for the camera follow movement - /// \param[in] _gain Camera follow p gain. - public: void SetFollowPGain(double _gain); - - /// \brief True to set the camera to follow the target in world frame, - /// false to follow in target's local frame - /// \param[in] _worldFrame True to use the world frame. - public: void SetFollowWorldFrame(bool _worldFrame); - - /// \brief Set the camera follow offset position - /// \param[in] _offset Camera follow offset position. - public: void SetFollowOffset(const math::Vector3d &_offset); - - /// \brief Get the target which the user camera is following - /// \return Target being followed - public: std::string FollowTarget() const; - - /// \brief Get whether the camera is following the entity in world frame. - /// \return True if follow mode is in world frame, false if local frame - public: bool FollowWorldFrame() const; - - /// \brief Get the camera follow offset position - /// \return Camera follow offset position. - public: math::Vector3d FollowOffset() const; - - /// \brief Set the initial user camera pose - /// \param[in] _pose Pose to set the camera to - public: void SetInitCameraPose(const math::Pose3d &_pose); - - /// \brief New mouse event triggered - /// \param[in] _e New mouse event - /// \param[in] _drag Mouse move distance - public: void NewMouseEvent(const common::MouseEvent &_e, - const math::Vector2d &_drag = math::Vector2d::Zero); - - /// \brief New hover event triggered. - /// \param[in] _hoverPos Mouse hover screen position - public: void NewHoverEvent(const math::Vector2i &_hoverPos); - - /// \brief Handle key press event for snapping - /// \param[in] _e The key event to process. - public: void HandleKeyPress(QKeyEvent *_e); - - /// \brief Handle key release event for snapping - /// \param[in] _e The key event to process. - public: void HandleKeyRelease(QKeyEvent *_e); - - /// \brief Set the XYZ snap values. - /// \param[in] _xyz The XYZ snap values - public: void SetXYZSnap(const math::Vector3d &_xyz); - - /// \brief Get the XYZ snap values. - /// \return XYZ snapping values as a Vector3d - public: math::Vector3d XYZSnap() const; - - /// \brief Set the RPY snap values. - /// \param[in] _rpy The RPY snap values - public: void SetRPYSnap(const math::Vector3d &_rpy); - - /// \brief Get the RPY snap values. - /// \return RPY snapping values as a Vector3d - public: math::Vector3d RPYSnap() const; - - /// \brief Set the scale snap values. - /// \param[in] _scale The scale snap values - public: void SetScaleSnap(const math::Vector3d &_scale); - - /// \brief Get the scale snap values. - /// \return Scale snapping values as a Vector3d - public: math::Vector3d ScaleSnap() const; - - /// \brief Snaps a point at intervals of a fixed distance. Currently used - /// to give a snapping behavior when moving models with a mouse. - /// \param[in] _point Input point to snap. - /// \param[in] _snapVals The snapping values to use for each corresponding - /// coordinate in _point - /// \param[in] _sensitivity Sensitivity of a point snapping, in terms of a - /// percentage of the interval. - public: void SnapPoint( - math::Vector3d &_point, - math::Vector3d &_snapVals, double _sensitivity = 0.4) const; - - /// \brief Request entity selection. This queues the selection to be handled - /// later in the render thread. - /// \param[in] _selectedEntity Entity to be selected, or `kNullEntity`. - /// \param[in] _deselectAll True if all entities should be deselected. - /// \param[in] _sendEvent True to emit an event to other widgets. - public: void RequestSelectionChange(Entity _selectedEntity, - bool _deselectAll, bool _sendEvent); - - /// \brief Snaps a value at intervals of a fixed distance. Currently used - /// to give a snapping behavior when moving models with a mouse. - /// \param[in] _coord Input coordinate point. - /// \param[in] _interval Fixed distance interval at which the point is - /// snapped. - /// \param[in] _sensitivity Sensitivity of a point snapping, in terms of a - /// percentage of the interval. - /// \return Snapped coordinate point. - private: double SnapValue( - double _coord, double _interval, double _sensitivity) const; - - /// \brief Constraints the passed in axis to the currently selected axes. - /// \param[in] _axis The axis to constrain. - private: void XYZConstraint(math::Vector3d &_axis); - - /// \brief Handle mouse events - private: void HandleMouseEvent(); - - /// \brief Handle mouse event for context menu - private: void HandleMouseContextMenu(); - - /// \brief Handle mouse event for view control - private: void HandleMouseViewControl(); - - /// \brief Handle mouse event for transform control - private: void HandleMouseTransformControl(); - - /// \brief Handle entity selection requests - private: void HandleEntitySelection(); - - /// \brief Handle model placement requests - private: void HandleModelPlacement(); - - /// \brief Broadcasts the currently hovered 3d scene location. - private: void BroadcastHoverPos(); - - /// \brief Broadcasts a left click within the scene - private: void BroadcastLeftClick(); - - /// \brief Broadcasts a right click within the scene - private: void BroadcastRightClick(); - - /// \brief Generate a unique entity id. - /// \return The unique entity id - private: Entity UniqueId(); - - /// \brief Generate a preview of a resource. - /// \param[in] _sdf The SDF to be previewed. - /// \return True on success, false if failure - public: bool GeneratePreview(const sdf::Root &_sdf); - - /// \brief Delete the visuals generated while an entity is being spawned. - public: void TerminateSpawnPreview(); - - /// \brief Retrieve the point on a plane at z = 0 in the 3D scene hit by a - /// ray cast from the given 2D screen coordinates. - /// \param[in] _screenPos 2D coordinates on the screen, in pixels. - /// \return 3D coordinates of a point in the 3D scene. - public: math::Vector3d ScreenToPlane(const math::Vector2i &_screenPos) - const; - - /// \brief Retrieve the first point on a surface in the 3D scene hit by a - /// ray cast from the given 2D screen coordinates. - /// \param[in] _screenPos 2D coordinates on the screen, in pixels. - /// \return 3D coordinates of a point in the 3D scene. - public: math::Vector3d ScreenToScene(const math::Vector2i &_screenPos) - const; - - /// \brief Get the current camera pose. - /// \return Pose of the camera. - public: math::Pose3d CameraPose() const; - - /// \brief Callback when a move to animation is complete - private: void OnMoveToComplete(); - - /// \brief Callback when a view angle animation is complete - private: void OnViewAngleComplete(); - - /// \brief Callback when a move to pose animation is complete - private: void OnMoveToPoseComplete(); - - /// \brief Process a node's selection - /// \param[in] _node The node to be selected. - /// \param[in] _sendEvent True to notify other widgets. This should be true - /// when the selection is initiated from this plugin. - private: void UpdateSelectedEntity(const rendering::NodePtr &_node, - bool _sendEvent); - - /// \brief Deselect all entities. - /// \param[in] _sendEvent True to notify other widgets. This should be true - /// when the deselection is initiated from this plugin. - private: void DeselectAllEntities(bool _sendEvent); - - /// \brief Signal fired when context menu event is triggered - signals: void ContextMenuRequested(QString _entity); - - /// \brief When fired, the follow target changed. May not be fired for - /// every target change. - /// \param[in] _target Target to follow - /// \param[in] _waitForTarget True to continuously look for the target - signals: void FollowTargetChanged(const std::string &_target, - bool _waitForTarget); - - /// \brief Render texture id - /// Values is constantly constantly cycled/swapped/changed - /// from a worker thread - /// Don't read this directly - public: GLuint textureId; - - /// \brief Initial Camera pose - public: math::Pose3d cameraPose = math::Pose3d(0, 0, 2, 0, 0.4, 0); - - /// \brief Name of the world - public: std::string worldName; - - /// \brief Camera visibility mask - public: uint32_t visibilityMask = 0xFFFFFFFFu; - - /// \brief True if engine has been initialized; - public: bool initialized = false; - - /// \brief Render texture size - public: QSize textureSize = QSize(1024, 1024); - - /// \brief Flag to indicate texture size has changed. - public: bool textureDirty = false; - - /// \internal - /// \brief Pointer to private data. - private: std::unique_ptr dataPtr; - }; - - /// \brief Rendering thread - class RenderThread : public QThread - { - Q_OBJECT - - /// \brief Constructor - public: RenderThread(); - - /// \brief Render the next frame - /// \param[in] _renderSync RenderSync to safely - /// synchronize Qt and worker thread (this) - public slots: void RenderNext(RenderSync *renderSync); - - /// \brief Shutdown the thread and the render engine - public slots: void ShutDown(); - - /// \brief Slot called to update render texture size - public slots: void SizeChanged(); - - /// \brief Signal to indicate that a frame has been rendered and ready - /// to be displayed - /// \param[in] _id GLuid of the opengl texture - /// \param[in] _size Size of the texture - signals: void TextureReady(uint _id, const QSize &_size); - - /// \brief Set a callback to be called in case there are errors. - /// \param[in] _cb Error callback - public: void SetErrorCb(std::function _cb); - - /// \brief Function to be called if there are errors. - public: std::function errorCb; - - /// \brief Offscreen surface to render to - public: QOffscreenSurface *surface = nullptr; - - /// \brief OpenGL context to be passed to the render engine - public: QOpenGLContext *context = nullptr; - - /// \brief gz-rendering renderer - public: GzRenderer gzRenderer; - }; - - - /// \brief A QQUickItem that manages the render window - class RenderWindowItem : public QQuickItem - { - Q_OBJECT - - /// \brief Constructor - /// \param[in] _parent Parent item - public: explicit RenderWindowItem(QQuickItem *_parent = nullptr); - - /// \brief Destructor - public: ~RenderWindowItem() override; - - /// \brief Set the renderer - public: class RenderUtil *RenderUtil() const; - - /// \brief Set the initial user camera pose - /// \param[in] _pose Pose to set the camera to - public: void SetCameraPose(const math::Pose3d &_pose); - - /// \brief Get the user camera pose. - /// \return Pose of the user camera. - public: math::Pose3d CameraPose() const; - - /// \brief Set the initial user camera pose - /// \param[in] _pose Pose to set the camera to - public: void SetInitCameraPose(const math::Pose3d &_pose); - - /// \brief Set the user camera visibility mask - /// \param[in] _mask Visibility mask to set to - public: void SetVisibilityMask(uint32_t _mask); - - /// \brief Set the transform mode - /// \param[in] _mode New transform mode to set to - public: void SetTransformMode(const std::string &_mode); - - /// \brief Set the model to hover. - /// \param[in] _model Sdf string of the model to load in for the user. - public: void SetModel(const std::string &_model); - - /// \brief Set the path of the model to hover. - /// \param[in] _filePath File path of the model to load in for the user. - public: void SetModelPath(const std::string &_filePath); - - /// \brief Set if the dropdown menu is enabled or disabled. - /// \param[in] _enableDropdownMenu The boolean to enable or disable - /// the menu - public: void SetDropdownMenuEnabled(bool _enableDropdownMenu); - - /// \brief Set whether to record video - /// \param[in] _record True to start video recording, false to stop. - /// \param[in] _format Video encoding format: "mp4", "ogv" - /// \param[in] _savePath Path to save the recorded video. - public: void SetRecordVideo(bool _record, const std::string &_format, - const std::string &_savePath); - - /// \brief Set whether to record video using sim time as timestamp - /// \param[in] _useSimTime True record video using sim time - public: void SetRecordVideoUseSimTime(bool _useSimTime); - - /// \brief Set whether to record video in lockstep mode - /// \param[in] _lockstep True to record video in lockstep mode - public: void SetRecordVideoLockstep(bool _lockstep); - - /// \brief Set video recorder bitrate in bps - /// \param[in] _bitrate Bit rate to set to - public: void SetRecordVideoBitrate(unsigned int _bitrate); - - /// \brief Move the user camera to move to the specified target - /// \param[in] _target Target to move the camera to - public: void SetMoveTo(const std::string &_target); - - /// \brief Move the user camera to follow the specified target - /// \param[in] _target Target to follow - /// \param[in] _waitForTarget True to continuously look for the target - /// to follow. A typical use case is follow a target that is not present - /// on startup but spawned later into simulation - public Q_SLOTS: void SetFollowTarget(const std::string &_target, - bool _waitForTarget = false); - - /// \brief Set the viewing angle of the camera - /// \param[in] _direction The pose to assume relative to the entit(y/ies). - /// (0, 0, 0) indicates to return the camera back to the home pose - /// originally loaded in from the sdf - public: void SetViewAngle(const math::Vector3d &_direction); - - /// \brief Set the pose of the camera - /// \param[in] _pose The new camera pose in the world frame. - public: void SetMoveToPose(const math::Pose3d &_pose); - - /// \brief View the specified target as transparent - /// \param[in] _target Target to view as transparent - public: void SetViewTransparentTarget(const std::string &_target); - - /// \brief View center of mass of the specified target - /// \param[in] _target Target to view center of mass - public: void SetViewCOMTarget(const std::string &_target); - - /// \brief View inertia of the specified target - /// \param[in] _target Target to view inertia - public: void SetViewInertiaTarget(const std::string &_target); - - /// \brief View joints of the specified target - /// \param[in] _target Target to view joints - public: void SetViewJointsTarget(const std::string &_target); - - /// \brief View wireframes of the specified target - /// \param[in] _target Target to view wireframes - public: void SetViewWireframesTarget(const std::string &_target); - - /// \brief View collisions of the specified target - /// \param[in] _target Target to view collisions - public: void SetViewCollisionsTarget(const std::string &_target); - - /// \brief Set camera view controller - /// \param[in] _viewController. Values are "orbit", and "ortho" - public: void SetViewController(const std::string &_viewController); - - /// \brief Set the p gain for the camera follow movement - /// \param[in] _gain Camera follow p gain. - public: void SetFollowPGain(double _gain); - - /// \brief True to set the camera to follow the target in world frame, - /// false to follow in target's local frame - /// \param[in] _worldFrame True to use the world frame. - public: void SetFollowWorldFrame(bool _worldFrame); - - /// \brief Set the camera follow offset position - /// \param[in] _offset Camera follow offset position. - public: void SetFollowOffset(const math::Vector3d &_offset); - - /// \brief Set the world name - /// \param[in] _name Name of the world to set to. - public: void SetWorldName(const std::string &_name); - - /// \brief An update function to apply the rules of selection to the - /// passed in node. The rules are as follows: - /// - If control is held, append the node to the selected entity list. - /// - If control is not held, deselect all other entities. - /// - If the user is currently in a transform mode, attach the control - /// to the latest selected node and deselect all others. - /// Note that this function emits events to update other widgets. - /// \param[in] _entity The entity to select. - /// \param[in] _sendEvent True to notify other widgets. This should be true - /// when the selection is initiated from this plugin. - public: void UpdateSelectedEntity(Entity _entity, - bool _sendEvent); - - /// \brief Deselect all the currently selected entities within - /// the RenderUtil class. - /// \param[in] _sendEvent True to notify other widgets. This should be true - /// when the deselection is initiated from this plugin. - public: void DeselectAllEntities(bool _sendEvent); - - /// \brief Set the XYZ snap values from the user input. - /// \param[in] _xyz The XYZ snap values - public: void SetXYZSnap(const math::Vector3d &_xyz); - - /// \brief Set the RPY snap values from the user input. - /// \param[in] _rpy The RPY snap values - public: void SetRPYSnap(const math::Vector3d &_rpy); - - /// \brief Set the scale snap values from the user input. - /// \param[in] _scale The scale snap values - public: void SetScaleSnap(const math::Vector3d &_scale); - - /// \brief Retrieve the first point on a surface in the 3D scene hit by a - /// ray cast from the given 2D screen coordinates. - /// \param[in] _screenPos 2D coordinates on the screen, in pixels. - /// \return 3D coordinates of a point in the 3D scene. - public: math::Vector3d ScreenToScene(const math::Vector2i &_screenPos); - - /// \brief Called when the mouse hovers to a new position. - /// \param[in] _hoverPos 2D coordinates of the hovered mouse position on - /// the render window. - public: void OnHovered(const gz::math::Vector2i &_hoverPos); - - /// \brief Get whether the renderer is initialized. The renderer is - /// initialized when the context is created and the render thread is - /// started. - /// \return True if the renderer is initialized. - public: bool RendererInitialized() const; - - /// \brief Slot called when thread is ready to be started - public Q_SLOTS: void Ready(); - - /// \brief Handle key press event for snapping - /// \param[in] _e The key event to process. - public: void HandleKeyPress(QKeyEvent *_e); - - /// \brief Handle key release event for snapping - /// \param[in] _e The key event to process. - public: void HandleKeyRelease(QKeyEvent *_e); - - // Documentation inherited - protected: void mousePressEvent(QMouseEvent *_e) override; - - // Documentation inherited - protected: void mouseReleaseEvent(QMouseEvent *_e) override; - - // Documentation inherited - protected: void mouseMoveEvent(QMouseEvent *_e) override; - - // Documentation inherited - protected: void wheelEvent(QWheelEvent *_e) override; - - /// \brief Overrides the paint event to render the render engine - /// camera view - /// \param[in] _oldNode The node passed in previous updatePaintNode - /// function. It represents the visual representation of the item. - /// \param[in] _data The node transformation data. - /// \return Updated node. - private: QSGNode *updatePaintNode(QSGNode *_oldNode, - QQuickItem::UpdatePaintNodeData *_data) override; - - /// \brief Signal fired to open context menu - /// Note that the function name needs to start with lowercase in order for - /// the connection to work on the QML side - /// \param[in] _entity Scoped name of entity. - signals: void openContextMenu(QString _entity); // NOLINT - - /// \brief Qt callback when context menu request is received - /// \param[in] _entity Scoped name of entity. - public slots: void OnContextMenuRequested(QString _entity); - - /// \brief Set a callback to be called in case there are errors. - /// \param[in] _cb Error callback - public: void SetErrorCb(std::function _cb); - - /// \internal - /// \brief Pointer to private data. - private: std::unique_ptr dataPtr; - }; - - /// \brief Texture node for displaying the render texture from gz-renderer - class TextureNode : public QObject, public QSGSimpleTextureNode - { - Q_OBJECT - - /// \brief Constructor - /// \param[in] _window Parent window - /// \param[in] _renderSync RenderSync to safely - /// synchronize Qt (this) and worker thread - public: explicit TextureNode(QQuickWindow *_window, - RenderSync &_renderSync); - - /// \brief Destructor - public: ~TextureNode() override; - - /// \brief This function gets called on the FBO rendering thread and will - /// store the texture id and size and schedule an update on the window. - /// \param[in] _id OpenGL render texture Id - /// \param[in] _size Texture size - public slots: void NewTexture(uint _id, const QSize &_size); - - /// \brief Before the scene graph starts to render, we update to the - /// pending texture - public slots: void PrepareNode(); - - /// \brief Signal emitted when the texture is being rendered and renderer - /// can start rendering next frame - /// \param[in] _renderSync RenderSync to send to the worker thread - signals: void TextureInUse(RenderSync *_renderSync); - - /// \brief Signal emitted when a new texture is ready to trigger window - /// update - signals: void PendingNewTexture(); - - /// \brief OpenGL texture id - public: uint id = 0; - - /// \brief Texture size - public: QSize size = QSize(0, 0); - - /// \brief Mutex to protect the texture variables - public: QMutex mutex; - - /// \brief See RenderSync - public: RenderSync &renderSync; - - /// \brief Qt's scene graph texture - public: QSGTexture *texture = nullptr; - - /// \brief Qt quick window - public: QQuickWindow *window = nullptr; - }; -} -} -} - -#endif diff --git a/src/systems/hydrodynamics/Hydrodynamics.cc b/src/systems/hydrodynamics/Hydrodynamics.cc index 6ed85bb9f8..96e2333620 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.cc +++ b/src/systems/hydrodynamics/Hydrodynamics.cc @@ -203,16 +203,7 @@ void Hydrodynamics::Configure( gz::sim::EventManager &/*_eventMgr*/ ) { - if (_sdf->HasElement("waterDensity")) - { - gzwarn << - " parameter is deprecated and will be removed Gazebo G.\n" - << "\tPlease update your SDF to use instead."; - } - - this->dataPtr->waterDensity = SdfParamDouble(_sdf, "waterDensity", - SdfParamDouble(_sdf, "water_density", 998) - ); + this->dataPtr->waterDensity = SdfParamDouble(_sdf, "water_density", 998); this->dataPtr->paramXdotU = SdfParamDouble(_sdf, "xDotU" , 5); this->dataPtr->paramYdotV = SdfParamDouble(_sdf, "yDotV" , 5); this->dataPtr->paramZdotW = SdfParamDouble(_sdf, "zDotW" , 0.1); diff --git a/src/systems/hydrodynamics/Hydrodynamics.hh b/src/systems/hydrodynamics/Hydrodynamics.hh index 4514fc3412..ddb736bcaf 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.hh +++ b/src/systems/hydrodynamics/Hydrodynamics.hh @@ -63,8 +63,6 @@ namespace systems /// * - Quadratic damping, 2nd order, yaw component [kg/m^2] /// * - Linear damping, 1st order, yaw component [kg/m] /// Additionally the system also supports the following parameters: - /// * - The density of the fluid its moving in. - /// Defaults to 998kgm^-3. [kgm^-3, deprecated] /// * - The density of the fluid its moving in. /// Defaults to 998kgm^-3. [kgm^-3] /// * - The link of the model that is being subject to diff --git a/test/worlds/log_record_resources.sdf b/test/worlds/log_record_resources.sdf index 68cb7c7ceb..c95e80ca59 100644 --- a/test/worlds/log_record_resources.sdf +++ b/test/worlds/log_record_resources.sdf @@ -24,9 +24,6 @@ - - 1 0 From a314d66c8625b39b4b3f41bf1d531142b89f5dab Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 20 Jul 2022 14:11:48 -0700 Subject: [PATCH 2/8] remove legacy and more scene3d Signed-off-by: Louise Poubel --- .../rendering_plugins/RenderingGuiPlugin.hh | 4 +- .../log_video_recorder/log_video_recorder.sdf | 3 - examples/worlds/conveyor.sdf | 84 +++++++----- examples/worlds/minimal_scene.sdf | 9 -- examples/worlds/spherical_coordinates.sdf | 3 - examples/worlds/tracked_vehicle_simple.sdf | 65 ++++++++-- examples/worlds/tunnel.sdf | 3 - examples/worlds/video_record_dbl_pendulum.sdf | 6 - src/gui/gui.config | 3 - src/gui/plugins/align_tool/AlignTool.cc | 2 +- .../plugins/scene_manager/GzSceneManager.hh | 2 +- .../plugins/select_entities/SelectEntities.hh | 2 +- .../transform_control/TransformControl.cc | 121 +++--------------- .../plugins/video_recorder/VideoRecorder.cc | 68 ---------- src/gui/plugins/view_angle/ViewAngle.cc | 61 +-------- src/gui/plugins/view_angle/ViewAngle.hh | 1 - .../plugins/visualize_lidar/VisualizeLidar.cc | 2 +- test/worlds/conveyor.sdf | 96 -------------- test/worlds/revolute_joint_equilibrium.sdf | 41 ------ tutorials/gui_config.md | 20 ++- tutorials/rendering_plugins.md | 2 +- tutorials/server_config.md | 40 +++++- tutorials/video_recorder.md | 24 ++-- 23 files changed, 198 insertions(+), 464 deletions(-) diff --git a/examples/plugin/rendering_plugins/RenderingGuiPlugin.hh b/examples/plugin/rendering_plugins/RenderingGuiPlugin.hh index d5b12185fe..e3cd676b67 100644 --- a/examples/plugin/rendering_plugins/RenderingGuiPlugin.hh +++ b/examples/plugin/rendering_plugins/RenderingGuiPlugin.hh @@ -23,8 +23,8 @@ #include /// \brief Example of a GUI plugin that uses Gazebo Rendering. -/// This plugin works with either Gazebo GUI's Scene3D or Gazebo Sim's -/// Scene3D. +/// This plugin works with Gazebo GUI's MinimalScene or any plugin providing +/// similar functionality. class RenderingGuiPlugin : public gz::gui::Plugin { Q_OBJECT diff --git a/examples/scripts/log_video_recorder/log_video_recorder.sdf b/examples/scripts/log_video_recorder/log_video_recorder.sdf index 5aa3f1a89b..e464fc4fe1 100644 --- a/examples/scripts/log_video_recorder/log_video_recorder.sdf +++ b/examples/scripts/log_video_recorder/log_video_recorder.sdf @@ -128,9 +128,6 @@ true 4000000 - - - false diff --git a/examples/worlds/conveyor.sdf b/examples/worlds/conveyor.sdf index 85c1e69223..958a7ab960 100644 --- a/examples/worlds/conveyor.sdf +++ b/examples/worlds/conveyor.sdf @@ -221,43 +221,57 @@ - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 + + + + 3D View + false + docked + - floating - - - - - + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + - true - true - true - - + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + diff --git a/examples/worlds/minimal_scene.sdf b/examples/worlds/minimal_scene.sdf index eee488cd11..764d493880 100644 --- a/examples/worlds/minimal_scene.sdf +++ b/examples/worlds/minimal_scene.sdf @@ -214,9 +214,6 @@ Features: false #777777 - - - false @@ -251,9 +248,6 @@ Features: true 4000000 - - - false @@ -275,9 +269,6 @@ Features: docked_collapsed - - - false diff --git a/examples/worlds/spherical_coordinates.sdf b/examples/worlds/spherical_coordinates.sdf index 9f11a5fdcd..d9a9a1ba45 100644 --- a/examples/worlds/spherical_coordinates.sdf +++ b/examples/worlds/spherical_coordinates.sdf @@ -213,9 +213,6 @@ gz service -s /world/spherical_coordinates/set_spherical_coordinates \ false #777777 - - - false diff --git a/examples/worlds/tracked_vehicle_simple.sdf b/examples/worlds/tracked_vehicle_simple.sdf index f1f4ac1b4d..ec0a9316d0 100644 --- a/examples/worlds/tracked_vehicle_simple.sdf +++ b/examples/worlds/tracked_vehicle_simple.sdf @@ -1723,18 +1723,57 @@ - - - 3D View - 0 - docked - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + World control @@ -1819,4 +1858,4 @@ 6e-06 2.3e-05 -4.2e-05 - \ No newline at end of file + diff --git a/examples/worlds/tunnel.sdf b/examples/worlds/tunnel.sdf index 23b72f88af..223ef8fef4 100644 --- a/examples/worlds/tunnel.sdf +++ b/examples/worlds/tunnel.sdf @@ -89,9 +89,6 @@ false #666666 - - - false diff --git a/examples/worlds/video_record_dbl_pendulum.sdf b/examples/worlds/video_record_dbl_pendulum.sdf index 542e5cadf3..4912f50232 100644 --- a/examples/worlds/video_record_dbl_pendulum.sdf +++ b/examples/worlds/video_record_dbl_pendulum.sdf @@ -134,9 +134,6 @@ false #777777 - - - false @@ -225,9 +222,6 @@ true 4000000 - - - false diff --git a/src/gui/gui.config b/src/gui/gui.config index d226e8937b..e9e9e4e873 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -203,9 +203,6 @@ false #777777 - - - false diff --git a/src/gui/plugins/align_tool/AlignTool.cc b/src/gui/plugins/align_tool/AlignTool.cc index d509e74af3..4e6677cc46 100644 --- a/src/gui/plugins/align_tool/AlignTool.cc +++ b/src/gui/plugins/align_tool/AlignTool.cc @@ -552,7 +552,7 @@ bool AlignTool::eventFilter(QObject *_obj, QEvent *_event) { if (_event->type() == gz::gui::events::Render::kType) { - // This event is called in Scene3d's RenderThread, so it's safe to make + // This event is called in the RenderThread, so it's safe to make // rendering calls here std::lock_guard lock(this->dataPtr->mutex); diff --git a/src/gui/plugins/scene_manager/GzSceneManager.hh b/src/gui/plugins/scene_manager/GzSceneManager.hh index 0124d1d725..aeb07260de 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.hh +++ b/src/gui/plugins/scene_manager/GzSceneManager.hh @@ -33,7 +33,7 @@ inline namespace GZ_SIM_VERSION_NAMESPACE { /// \brief Updates a 3D scene based on information coming from the ECM. /// This plugin doesn't instantiate a new 3D scene. Instead, it relies on /// another plugin being loaded alongside it that will create and paint the - /// scene to the window, such as `gz::gui::plugins::Scene3D`. + /// scene to the window, such as `gz::gui::plugins::MinimalScene`. /// /// Only one GzSceneManager can be used at a time. class GzSceneManager : public GuiSystem diff --git a/src/gui/plugins/select_entities/SelectEntities.hh b/src/gui/plugins/select_entities/SelectEntities.hh index f2a0cf88f0..8b5c7ec769 100644 --- a/src/gui/plugins/select_entities/SelectEntities.hh +++ b/src/gui/plugins/select_entities/SelectEntities.hh @@ -31,7 +31,7 @@ namespace gui class SelectEntitiesPrivate; /// \brief This plugin is in charge of selecting and deselecting the entities - /// from the Scene3D and emit the corresponding events. + /// from the 3D scene and emit the corresponding events. class SelectEntities : public gz::gui::Plugin { Q_OBJECT diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index 16f58a13b4..11c37e9d2a 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -95,13 +95,6 @@ namespace gz::sim // TODO(anyone): check on mutex usage public: std::mutex mutex; - /// \brief Transform control service name - /// Only used when in legacy mode, where this plugin requests a - /// transport service provided by `GzScene3D`. - /// The new behaviour is that this plugin performs the entire transform - /// operation. - public: std::string service{"/gui/transform_mode"}; - /// \brief Flag for if the snapping values should be set to the grid. public: bool snapToGrid{false}; @@ -175,10 +168,6 @@ namespace gz::sim /// \brief Block orbit public: bool blockOrbit = false; - - /// \brief Enable legacy features for plugin to work with GzScene3D. - /// Disable them to work with the new MinimalScene plugin. - public: bool legacy{false}; }; } @@ -196,30 +185,11 @@ TransformControl::TransformControl() TransformControl::~TransformControl() = default; ///////////////////////////////////////////////// -void TransformControl::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +void TransformControl::LoadConfig(const tinyxml2::XMLElement *) { if (this->title.empty()) this->title = "Transform control"; - if (_pluginElem) - { - if (auto elem = _pluginElem->FirstChildElement("legacy")) - { - elem->QueryBoolText(&this->dataPtr->legacy); - } - } - - if (this->dataPtr->legacy) - { - gzdbg << "Legacy mode is enabled; this plugin must be used with " - << "GzScene3D." << std::endl; - } - else - { - gzdbg << "Legacy mode is disabled; this plugin must be used with " - << "MinimalScene." << std::endl; - } - gz::gui::App()->findChild ()->installEventFilter(this); gz::gui::App()->findChild @@ -236,17 +206,6 @@ void TransformControl::OnSnapUpdate( this->dataPtr->rpySnapVals = math::Vector3d(_roll, _pitch, _yaw); this->dataPtr->scaleSnapVals = math::Vector3d(_scaleX, _scaleY, _scaleZ); - // Emit event to GzScene3D in legacy mode - if (this->dataPtr->legacy) - { - gz::gui::events::SnapIntervals event( - this->dataPtr->xyzSnapVals, - this->dataPtr->rpySnapVals, - this->dataPtr->scaleSnapVals); - gz::gui::App()->sendEvent( - gz::gui::App()->findChild(), &event); - } - this->newSnapValues(); } @@ -255,42 +214,24 @@ void TransformControl::OnMode(const QString &_mode) { auto modeStr = _mode.toStdString(); - // Legacy behaviour: send request to GzScene3D - if (this->dataPtr->legacy) - { - std::function cb = - [](const gz::msgs::Boolean &/*_rep*/, const bool _result) - { - if (!_result) - gzerr << "Error setting transform mode" << std::endl; - }; - - gz::msgs::StringMsg req; - req.set_data(modeStr); - this->dataPtr->node.Request(this->dataPtr->service, req, cb); - } - // New behaviour: handle the transform control locally + std::lock_guard lock(this->dataPtr->mutex); + if (modeStr == "select") + this->dataPtr->transformMode = rendering::TransformMode::TM_NONE; + else if (modeStr == "translate") + this->dataPtr->transformMode = rendering::TransformMode::TM_TRANSLATION; + else if (modeStr == "rotate") + this->dataPtr->transformMode = rendering::TransformMode::TM_ROTATION; + else if (modeStr == "scale") + this->dataPtr->transformMode = rendering::TransformMode::TM_SCALE; else - { - std::lock_guard lock(this->dataPtr->mutex); - if (modeStr == "select") - this->dataPtr->transformMode = rendering::TransformMode::TM_NONE; - else if (modeStr == "translate") - this->dataPtr->transformMode = rendering::TransformMode::TM_TRANSLATION; - else if (modeStr == "rotate") - this->dataPtr->transformMode = rendering::TransformMode::TM_ROTATION; - else if (modeStr == "scale") - this->dataPtr->transformMode = rendering::TransformMode::TM_SCALE; - else - gzerr << "Unknown transform mode: [" << modeStr << "]" << std::endl; - - gz::sim::gui::events::TransformControlModeActive - transformControlModeActive(this->dataPtr->transformMode); - gz::gui::App()->sendEvent( - gz::gui::App()->findChild(), - &transformControlModeActive); - this->dataPtr->mouseDirty = true; - } + gzerr << "Unknown transform mode: [" << modeStr << "]" << std::endl; + + gz::sim::gui::events::TransformControlModeActive + transformControlModeActive(this->dataPtr->transformMode); + gz::gui::App()->sendEvent( + gz::gui::App()->findChild(), + &transformControlModeActive); + this->dataPtr->mouseDirty = true; } ///////////////////////////////////////////////// @@ -349,7 +290,7 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) { if (_event->type() == gz::gui::events::Render::kType) { - // This event is called in Scene3d's RenderThread, so it's safe to make + // This event is called in the RenderThread, so it's safe to make // rendering calls here if (this->dataPtr->snapToGrid) { @@ -425,30 +366,6 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) } } - if (this->dataPtr->legacy) - { - if (_event->type() == QEvent::KeyPress) - { - QKeyEvent *keyEvent = static_cast(_event); - if (keyEvent->key() == Qt::Key_T) - { - this->activateTranslate(); - } - else if (keyEvent->key() == Qt::Key_R) - { - this->activateRotate(); - } - } - else if (_event->type() == QEvent::KeyRelease) - { - QKeyEvent *keyEvent = static_cast(_event); - if (keyEvent->key() == Qt::Key_Escape) - { - this->activateSelect(); - } - } - } - return QObject::eventFilter(_obj, _event); } diff --git a/src/gui/plugins/video_recorder/VideoRecorder.cc b/src/gui/plugins/video_recorder/VideoRecorder.cc index 0ba4fe5ab8..beb4e6f379 100644 --- a/src/gui/plugins/video_recorder/VideoRecorder.cc +++ b/src/gui/plugins/video_recorder/VideoRecorder.cc @@ -92,12 +92,6 @@ namespace gz::sim /// \brief Record stats topic name public: std::string recorderStatsTopic = "/gui/record_video/stats"; - /// \brief Record video service - /// Only used when in legacy mode, where this plugin requests a - /// transport service provided by `GzScene3D`. - /// The new behaviour is that this plugin performs the entire operation. - public: std::string service = "/gui/record_video"; - /// \brief True to indicate video recording in progress public: bool recording = false; @@ -114,10 +108,6 @@ namespace gz::sim /// \brief Filename of the recorded video public: std::string filename; - - /// \brief Enable legacy features for plugin to work with GzScene3D. - /// Disable them to work with the new MinimalScene plugin. - public: bool legacy{false}; }; } @@ -127,11 +117,6 @@ using namespace sim; ///////////////////////////////////////////////// void VideoRecorderPrivate::Initialize() { - // Don't setup rendering or transport in legacy mode, GzScene3D takes care of - // that - if (this->legacy) - return; - // Already initialized if (this->scene) return; @@ -170,10 +155,6 @@ void VideoRecorderPrivate::Initialize() ///////////////////////////////////////////////// void VideoRecorderPrivate::OnRender() { - // Don't render in legacy mode, GzScene3D takes care of that - if (this->legacy) - return; - this->Initialize(); // record video is requested @@ -272,10 +253,6 @@ VideoRecorder::~VideoRecorder() = default; void VideoRecorder::Update(const UpdateInfo &_info, EntityComponentManager & /*_ecm*/) { - // Don't lockstep in legacy mode, GzScene3D takes care of that - if (this->dataPtr->legacy) - return; - this->dataPtr->simTime = _info.simTime; // check if video recording is enabled and if we need to lock step @@ -342,22 +319,6 @@ void VideoRecorder::LoadConfig(const tinyxml2::XMLElement * _pluginElem) } } } - - if (auto elem = _pluginElem->FirstChildElement("legacy")) - { - elem->QueryBoolText(&this->dataPtr->legacy); - } - } - - if (this->dataPtr->legacy) - { - gzdbg << "Legacy mode is enabled; this plugin must be used with " - << "GzScene3D." << std::endl; - } - else - { - gzdbg << "Legacy mode is disabled; this plugin must be used with " - << "MinimalScene." << std::endl; } gz::gui::App()->findChild< @@ -383,21 +344,6 @@ void VideoRecorder::OnStart(const QString &_format) this->dataPtr->filename = "ign_recording." + this->dataPtr->format; this->dataPtr->recordVideo = true; this->dataPtr->recording = true; - - if (this->dataPtr->legacy) - { - std::function cb = - [](const gz::msgs::Boolean &/*_rep*/, const bool _result) - { - if (!_result) - gzerr << "Error sending video record start request" << std::endl; - }; - gz::msgs::VideoRecord req; - req.set_start(this->dataPtr->recordVideo); - req.set_format(this->dataPtr->format); - req.set_save_filename(this->dataPtr->filename); - this->dataPtr->node.Request(this->dataPtr->service, req, cb); - } } ///////////////////////////////////////////////// @@ -405,20 +351,6 @@ void VideoRecorder::OnStop() { this->dataPtr->recordVideo = false; this->dataPtr->recording = false; - - if (this->dataPtr->legacy) - { - std::function cb = - [](const gz::msgs::Boolean &/*_rep*/, const bool _result) - { - if (!_result) - gzerr << "Error sending video record stop request" << std::endl; - }; - - gz::msgs::VideoRecord req; - req.set_stop(true); - this->dataPtr->node.Request(this->dataPtr->service, req, cb); - } } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/view_angle/ViewAngle.cc b/src/gui/plugins/view_angle/ViewAngle.cc index 5751042d4a..352a3763c3 100644 --- a/src/gui/plugins/view_angle/ViewAngle.cc +++ b/src/gui/plugins/view_angle/ViewAngle.cc @@ -101,10 +101,6 @@ namespace gz::sim /// \brief The pose set from the move to pose service. public: std::optional moveToPoseValue; - - /// \brief Enable legacy features for plugin to work with GzScene3D. - /// Disable them to work with the new MinimalScene plugin. - public: bool legacy{false}; }; } @@ -121,19 +117,11 @@ ViewAngle::ViewAngle() ViewAngle::~ViewAngle() = default; ///////////////////////////////////////////////// -void ViewAngle::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +void ViewAngle::LoadConfig(const tinyxml2::XMLElement *) { if (this->title.empty()) this->title = "View Angle"; - if (_pluginElem) - { - if (auto elem = _pluginElem->FirstChildElement("legacy")) - { - elem->QueryBoolText(&this->dataPtr->legacy); - } - } - // For view angle requests this->dataPtr->viewAngleService = "/gui/view_angle"; @@ -197,29 +185,8 @@ bool ViewAngle::eventFilter(QObject *_obj, QEvent *_event) ///////////////////////////////////////////////// void ViewAngle::OnAngleMode(int _x, int _y, int _z) { - // Legacy mode: request view angle from GzScene3d - if (this->dataPtr->legacy) - { - std::function cb = - [](const msgs::Boolean &/*_rep*/, const bool _result) - { - if (!_result) - gzerr << "Error setting view angle mode" << std::endl; - }; - - msgs::Vector3d req; - req.set_x(_x); - req.set_y(_y); - req.set_z(_z); - - this->dataPtr->node.Request(this->dataPtr->viewAngleService, req, cb); - } - // New behaviour: handle camera movement here - else - { - this->dataPtr->viewingAngle = true; - this->dataPtr->viewAngleDirection = math::Vector3d(_x, _y, _z); - } + this->dataPtr->viewingAngle = true; + this->dataPtr->viewAngleDirection = math::Vector3d(_x, _y, _z); } ///////////////////////////////////////////////// @@ -265,27 +232,7 @@ void ViewAngle::SetCamPose(double _x, double _y, double _z, double _roll, double _pitch, double _yaw) { this->dataPtr->camPose.Set(_x, _y, _z, _roll, _pitch, _yaw); - - // Legacy mode: request view angle from GzScene3d - if (this->dataPtr->legacy) - { - std::function cb = - [](const msgs::Boolean &/*_rep*/, const bool _result) - { - if (!_result) - gzerr << "Error sending move camera to pose request" << std::endl; - }; - - msgs::GUICamera req; - msgs::Set(req.mutable_pose(), this->dataPtr->camPose); - - this->dataPtr->node.Request(this->dataPtr->moveToPoseService, req, cb); - } - // New behaviour: handle camera movement here - else - { - this->dataPtr->moveToPoseValue = {_x, _y, _z, _roll, _pitch, _yaw}; - } + this->dataPtr->moveToPoseValue = {_x, _y, _z, _roll, _pitch, _yaw}; } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/view_angle/ViewAngle.hh b/src/gui/plugins/view_angle/ViewAngle.hh index 4a1b2f23b8..931dcd2ec3 100644 --- a/src/gui/plugins/view_angle/ViewAngle.hh +++ b/src/gui/plugins/view_angle/ViewAngle.hh @@ -34,7 +34,6 @@ namespace sim /// /// ## Configuration /// \ : Set the service to receive view angle requests. - /// \ : Set to true to use with GzScene3D, false to use with /// MinimalScene. Defaults to true. class ViewAngle : public gz::gui::Plugin { diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc index 6f4cbfbb90..d7f5ad8050 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc @@ -223,7 +223,7 @@ bool VisualizeLidar::eventFilter(QObject *_obj, QEvent *_event) { if (_event->type() == gz::gui::events::Render::kType) { - // This event is called in Scene3d's RenderThread, so it's safe to make + // This event is called in the RenderThread, so it's safe to make // rendering calls here std::lock_guard lock(this->dataPtr->serviceMutex); diff --git a/test/worlds/conveyor.sdf b/test/worlds/conveyor.sdf index 21d0ade2a9..ddb90b4559 100644 --- a/test/worlds/conveyor.sdf +++ b/test/worlds/conveyor.sdf @@ -186,101 +186,5 @@ - - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - - - - - - Transform control - - - - - false - 230 - 50 - floating - false - #666666 - - - - - - - - - - - false - 200 - 50 - floating - false - #666666 - - - diff --git a/test/worlds/revolute_joint_equilibrium.sdf b/test/worlds/revolute_joint_equilibrium.sdf index 9a471fcfe7..7cc65cd501 100644 --- a/test/worlds/revolute_joint_equilibrium.sdf +++ b/test/worlds/revolute_joint_equilibrium.sdf @@ -14,47 +14,6 @@ name="gz::sim::systems::SceneBroadcaster"> - - - - - - 3D View - false - false - 0 - - - - - - - ogre - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -1 0 1 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - - - - true - true - true - - - true diff --git a/tutorials/gui_config.md b/tutorials/gui_config.md index a8ff5e98a2..e43960f3a0 100644 --- a/tutorials/gui_config.md +++ b/tutorials/gui_config.md @@ -87,7 +87,7 @@ favorite editor and save this file as `fuel_preview.sdf`: - + 3D View false @@ -100,6 +100,24 @@ favorite editor and save this file as `fuel_preview.sdf`: 0.4 0.6 1.0 8.3 7 7.8 0 0.5 -2.4 + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + diff --git a/tutorials/rendering_plugins.md b/tutorials/rendering_plugins.md index e62160544f..d41d8137a8 100644 --- a/tutorials/rendering_plugins.md +++ b/tutorials/rendering_plugins.md @@ -20,7 +20,7 @@ The server-side scene will only be created when using the scene that shows what the sensors see. The client-side scene will only be created when using the -`gz::sim::Scene3D` GUI system plugin on the client. This is the +`gz::gui::plugins::MinimalScene` GUI system plugin on the client. This is the scene that shows what the user sees. For the user to see what the sensors see, they need to use other GUI plugins diff --git a/tutorials/server_config.md b/tutorials/server_config.md index 08a649d946..3a0760eead 100644 --- a/tutorials/server_config.md +++ b/tutorials/server_config.md @@ -97,7 +97,7 @@ favorite editor and save this file as `fuel_preview.sdf`: - + 3D View false @@ -110,6 +110,24 @@ favorite editor and save this file as `fuel_preview.sdf`: 0.4 0.6 1.0 8.3 7 7.8 0 0.5 -2.4 + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + @@ -159,7 +177,7 @@ Let's start by saving this simple world with a camera sensor as - + 3D View false @@ -172,6 +190,24 @@ Let's start by saving this simple world with a camera sensor as 0.4 0.6 1.0 8.3 7 7.8 0 0.5 -2.4 + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + diff --git a/tutorials/video_recorder.md b/tutorials/video_recorder.md index aee32d2bfd..ffd06943fb 100644 --- a/tutorials/video_recorder.md +++ b/tutorials/video_recorder.md @@ -44,30 +44,26 @@ launched Gazebo with a world file that does not have GUI configurations, you will need to specify the settings in `$HOME/.gz/sim/<#>/gui.config`. -Recall that videos are recorded from the 3D scene, we will to set the video -configurations in the 3D scene plugin. Here is an example of the -Scene 3D plugin with custom video recorder settings: +Here is an example of the video recorder plugin's settings: ```xml - + - 3D View - false - docked + false + 300 + 50 + 50 + 50 + floating + false + #777777 - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - 6 0 6 0 0.5 3.14 - true true 4000000 - ``` From e5398a08ec6ec5ad486202db3ccdc5f9bed8cebc Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 9 Aug 2022 14:22:07 -0700 Subject: [PATCH 3/8] indentation Signed-off-by: Louise Poubel --- examples/worlds/tracked_vehicle_simple.sdf | 98 +++++++++++----------- 1 file changed, 49 insertions(+), 49 deletions(-) diff --git a/examples/worlds/tracked_vehicle_simple.sdf b/examples/worlds/tracked_vehicle_simple.sdf index ec0a9316d0..9e7c6f9e8d 100644 --- a/examples/worlds/tracked_vehicle_simple.sdf +++ b/examples/worlds/tracked_vehicle_simple.sdf @@ -1723,57 +1723,57 @@ - - - - 3D View - false - docked - + + + + 3D View + false + docked + - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + - - - - floating - 5 - 5 - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + World control From 63d11091487931ae2e253fb3ba6afafb05c4016f Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 9 Aug 2022 15:40:22 -0700 Subject: [PATCH 4/8] Redirect GzScene3d to MinimalScene Signed-off-by: Louise Poubel --- examples/worlds/deprecated_ignition.sdf | 19 ++++++++++++ src/gui/Gui.cc | 40 ++++++++++++++++++++++++- 2 files changed, 58 insertions(+), 1 deletion(-) diff --git a/examples/worlds/deprecated_ignition.sdf b/examples/worlds/deprecated_ignition.sdf index 1c05f479fd..50dfe997a1 100644 --- a/examples/worlds/deprecated_ignition.sdf +++ b/examples/worlds/deprecated_ignition.sdf @@ -2,6 +2,9 @@ @@ -18,6 +21,22 @@ name="ignition::gazebo::systems::SceneBroadcaster"> + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + true 0 0 10 0 0 0 diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index 041c121fbb..9cf90ee4ba 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -239,7 +239,45 @@ std::unique_ptr createGui( for (int p = 0; p < res.plugin_size(); ++p) { const auto &plugin = res.plugin(p); - const auto &fileName = plugin.filename(); + auto fileName = plugin.filename(); + + // Redirect GzScene3D to MinimalScene for backwards compatibility, + // with warnings + if (fileName == "GzScene3D") + { + std::vector extras{"GzSceneManager", + "InteractiveViewControl", + "CameraTracking", + "MarkerManager", + "SelectEntities", + "EntityContextMenuPlugin", + "Spawn", + "VisualizationCapabilities"}; + + std::string msg{"The [GzScene3D] GUI plugin has been removed since Garden. " + "Loading the following plugins instead:\n"}; + + for (auto extra : extras) + { + msg += "* " + extra + "\n"; + + auto newPlugin = res.add_plugin(); + newPlugin->set_filename(extra); + newPlugin->set_innerxml(std::string( + "" + " floating" + " 5" + " 5" + " false" + " false" + "")); + } + + gzwarn << msg; + + fileName = "MinimalScene"; + } + std::string pluginStr = "" + plugin.innerxml() + ""; From 1c77db95d18fbdc152a627414fc2b6cda81d5a8e Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 9 Aug 2022 15:40:58 -0700 Subject: [PATCH 5/8] indentation Signed-off-by: Louise Poubel --- examples/worlds/conveyor.sdf | 98 ++++++++++++++++++------------------ 1 file changed, 49 insertions(+), 49 deletions(-) diff --git a/examples/worlds/conveyor.sdf b/examples/worlds/conveyor.sdf index 958a7ab960..dfdaf222af 100644 --- a/examples/worlds/conveyor.sdf +++ b/examples/worlds/conveyor.sdf @@ -221,57 +221,57 @@ - - - - 3D View - false - docked - + + + + 3D View + false + docked + - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + - - - - floating - 5 - 5 - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + From 465aedc99c6159480690523d76e55c7606241abe Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 9 Aug 2022 15:43:49 -0700 Subject: [PATCH 6/8] indentation Signed-off-by: Louise Poubel --- examples/worlds/conveyor.sdf | 98 ++++++++++++++++++------------------ 1 file changed, 49 insertions(+), 49 deletions(-) diff --git a/examples/worlds/conveyor.sdf b/examples/worlds/conveyor.sdf index dfdaf222af..940dd4af3f 100644 --- a/examples/worlds/conveyor.sdf +++ b/examples/worlds/conveyor.sdf @@ -221,57 +221,57 @@ - - - - 3D View - false - docked - + + + + 3D View + false + docked + - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + - - - - floating - 5 - 5 - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + From 8ddc01f7aba54c4561b00e051fbdd832ea8fdd7e Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 9 Aug 2022 15:45:32 -0700 Subject: [PATCH 7/8] indentation Signed-off-by: Louise Poubel --- examples/worlds/tracked_vehicle_simple.sdf | 98 +++++++++++----------- 1 file changed, 49 insertions(+), 49 deletions(-) diff --git a/examples/worlds/tracked_vehicle_simple.sdf b/examples/worlds/tracked_vehicle_simple.sdf index 9e7c6f9e8d..c69ad0628c 100644 --- a/examples/worlds/tracked_vehicle_simple.sdf +++ b/examples/worlds/tracked_vehicle_simple.sdf @@ -1723,57 +1723,57 @@ - - - - 3D View - false - docked - + + + + 3D View + false + docked + - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + - - - - floating - 5 - 5 - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + World control From 8fbd6dd5c706d9698cad47958dda4f7613a61d21 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 10 Aug 2022 10:21:46 -0700 Subject: [PATCH 8/8] linter Signed-off-by: Louise Poubel --- src/gui/Gui.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index 9cf90ee4ba..0a433fc7b1 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -254,7 +254,8 @@ std::unique_ptr createGui( "Spawn", "VisualizationCapabilities"}; - std::string msg{"The [GzScene3D] GUI plugin has been removed since Garden. " + std::string msg{ + "The [GzScene3D] GUI plugin has been removed since Garden. " "Loading the following plugins instead:\n"}; for (auto extra : extras)