From 3a7d09ca8335289662a6eedf3267a99c0a18c1c0 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 12 May 2021 14:08:26 -0700 Subject: [PATCH 01/27] Add scene manager GUI plugin that works with ign-gui's Scene3D Signed-off-by: Louise Poubel --- .../ignition/gazebo/rendering/RenderUtil.hh | 4 + src/gui/gui.config | 16 ++- src/gui/plugins/CMakeLists.txt | 1 + src/gui/plugins/scene_manager/CMakeLists.txt | 7 + .../plugins/scene_manager/GzSceneManager.cc | 135 ++++++++++++++++++ .../plugins/scene_manager/GzSceneManager.hh | 66 +++++++++ .../plugins/scene_manager/GzSceneManager.qml | 28 ++++ .../plugins/scene_manager/GzSceneManager.qrc | 5 + src/rendering/RenderUtil.cc | 12 ++ 9 files changed, 273 insertions(+), 1 deletion(-) create mode 100644 src/gui/plugins/scene_manager/CMakeLists.txt create mode 100644 src/gui/plugins/scene_manager/GzSceneManager.cc create mode 100644 src/gui/plugins/scene_manager/GzSceneManager.hh create mode 100644 src/gui/plugins/scene_manager/GzSceneManager.qml create mode 100644 src/gui/plugins/scene_manager/GzSceneManager.qrc diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 8f21ff3625..daecee7422 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -92,6 +92,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return Name of the rendering scene. public: std::string SceneName() const; + /// \brief Set the scene to use. + /// \param[in] _scene Pointer to the scene. + public: void SetScene(const rendering::ScenePtr &_scene); + /// \brief Set background color of render window /// \param[in] _color Color of render window background public: void SetBackgroundColor(const math::Color &_color); diff --git a/src/gui/gui.config b/src/gui/gui.config index 072f631f63..bb2b9f7e42 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -26,7 +26,7 @@ - + 3D View false @@ -40,6 +40,20 @@ 6 0 6 0 0.5 3.14 + + + + + + + false + 5 + 5 + floating + false + + + diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 6d7d140fe7..7df38dfb35 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -119,6 +119,7 @@ add_subdirectory(playback_scrubber) add_subdirectory(plotting) add_subdirectory(resource_spawner) add_subdirectory(scene3d) +add_subdirectory(scene_manager) add_subdirectory(shapes) add_subdirectory(tape_measure) add_subdirectory(transform_control) diff --git a/src/gui/plugins/scene_manager/CMakeLists.txt b/src/gui/plugins/scene_manager/CMakeLists.txt new file mode 100644 index 0000000000..6e5e884b94 --- /dev/null +++ b/src/gui/plugins/scene_manager/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_gui_plugin(GzSceneManager + SOURCES GzSceneManager.cc + QT_HEADERS GzSceneManager.hh + PRIVATE_LINK_LIBS + ${PROJECT_LIBRARY_TARGET_NAME}-rendering + ignition-utils${IGN_UTILS_VER}::ignition-utils${IGN_UTILS_VER} +) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc new file mode 100644 index 0000000000..ee3a204a29 --- /dev/null +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -0,0 +1,135 @@ +/* + * Copyright (C) 2021 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 "GzSceneManager.hh" + +#include + +#include + +#include +#include + +#include +#include +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/rendering/RenderUtil.hh" + +namespace ignition +{ +namespace gazebo +{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + /// \brief Private data class for GzSceneManager + class GzSceneManagerPrivate + { + /// \brief Update the 3D scene based on the latest state of the ECM. + public: void OnRender(); + + //// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene; + + /// \brief Name of the world + public: std::string worldName; + + /// \brief Rendering utility + public: RenderUtil renderUtil; + }; +} +} +} + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +GzSceneManager::GzSceneManager() + : GuiSystem(), dataPtr(std::make_unique) +{ +} + +///////////////////////////////////////////////// +GzSceneManager::~GzSceneManager() = default; + +///////////////////////////////////////////////// +void GzSceneManager::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +{ + if (this->title.empty()) + this->title = "Scene Manager"; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); +} + +////////////////////////////////////////////////// +void GzSceneManager::Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("GzSceneManager::Update"); + 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 false; + }); + } + + this->dataPtr->renderUtil.UpdateECM(_info, _ecm); + this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); +} + +///////////////////////////////////////////////// +bool GzSceneManager::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == gui::events::Render::kType) + { + this->dataPtr->OnRender(); + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void GzSceneManagerPrivate::OnRender() +{ + if (nullptr == this->scene) + { + this->scene = rendering::someInitializedScene(); + if (nullptr == this->scene) + return; + + this->renderUtil.SetScene(this->scene); + } + + this->renderUtil.Update(); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::GzSceneManager, + ignition::gui::Plugin) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.hh b/src/gui/plugins/scene_manager/GzSceneManager.hh new file mode 100644 index 0000000000..ab0d5f1be0 --- /dev/null +++ b/src/gui/plugins/scene_manager/GzSceneManager.hh @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2021 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 IGNITION_GAZEBO_GUI_GZSCENEMANAGER_HH_ +#define IGNITION_GAZEBO_GUI_GZSCENEMANAGER_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + class GzSceneManagerPrivate; + + /// \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 `ignition::gui::plugins::Scene3D`. + class GzSceneManager : public GuiSystem + { + Q_OBJECT + + /// \brief Constructor + public: GzSceneManager(); + + /// \brief Destructor + public: ~GzSceneManager() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) + override; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + // Documentation inherited + private: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} +} + +#endif diff --git a/src/gui/plugins/scene_manager/GzSceneManager.qml b/src/gui/plugins/scene_manager/GzSceneManager.qml new file mode 100644 index 0000000000..873da30014 --- /dev/null +++ b/src/gui/plugins/scene_manager/GzSceneManager.qml @@ -0,0 +1,28 @@ +/* + * Copyright (C) 2021 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 QtQuick 2.0 +import QtQuick.Controls 2.0 +import QtQuick.Layouts 1.3 + +// TODO: remove invisible rectangle, see +// https://github.com/ignitionrobotics/ign-gui/issues/220 +Rectangle { + visible: false + Layout.minimumWidth: 100 + Layout.minimumHeight: 100 +} diff --git a/src/gui/plugins/scene_manager/GzSceneManager.qrc b/src/gui/plugins/scene_manager/GzSceneManager.qrc new file mode 100644 index 0000000000..b28c13c3b4 --- /dev/null +++ b/src/gui/plugins/scene_manager/GzSceneManager.qrc @@ -0,0 +1,5 @@ + + + GzSceneManager.qml + + diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 382a48ae86..65a076b52e 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1827,6 +1827,10 @@ void RenderUtilPrivate::RemoveRenderingEntities( ///////////////////////////////////////////////// void RenderUtil::Init() { + // Already initialized + if (nullptr != this->dataPtr->scene) + return; + ignition::common::SystemPaths pluginPath; pluginPath.SetPluginPathEnv(kRenderPluginPathEnv); rendering::setPluginPaths(pluginPath.PluginPaths()); @@ -1927,6 +1931,14 @@ void RenderUtil::SetSceneName(const std::string &_name) this->dataPtr->sceneName = _name; } +///////////////////////////////////////////////// +void RenderUtil::SetScene(const rendering::ScenePtr &_scene) +{ + this->dataPtr->scene = _scene; + this->dataPtr->sceneManager.SetScene(_scene); + this->dataPtr->engine = _scene == nullptr ? nullptr : _scene->Engine(); +} + ///////////////////////////////////////////////// std::string RenderUtil::SceneName() const { From 80161fb7fe13c8c1dabcbe4c7af8bafa38d59549 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 12 May 2021 14:29:17 -0700 Subject: [PATCH 02/27] cleanup Signed-off-by: Louise Poubel --- .../plugins/scene_manager/GzSceneManager.cc | 21 ++----------------- 1 file changed, 2 insertions(+), 19 deletions(-) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index ee3a204a29..7491687688 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -47,9 +47,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { //// \brief Pointer to the rendering scene public: rendering::ScenePtr scene; - /// \brief Name of the world - public: std::string worldName; - /// \brief Rendering utility public: RenderUtil renderUtil; }; @@ -62,7 +59,7 @@ using namespace gazebo; ///////////////////////////////////////////////// GzSceneManager::GzSceneManager() - : GuiSystem(), dataPtr(std::make_unique) + : GuiSystem(), dataPtr(std::make_unique()) { } @@ -70,7 +67,7 @@ GzSceneManager::GzSceneManager() GzSceneManager::~GzSceneManager() = default; ///////////////////////////////////////////////// -void GzSceneManager::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +void GzSceneManager::LoadConfig(const tinyxml2::XMLElement *) { if (this->title.empty()) this->title = "Scene Manager"; @@ -84,20 +81,6 @@ void GzSceneManager::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { IGN_PROFILE("GzSceneManager::Update"); - 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 false; - }); - } this->dataPtr->renderUtil.UpdateECM(_info, _ecm); this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); From 22454e95cebaed483292aa6c85940a622b378e34 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 9 Jun 2021 14:49:00 +0200 Subject: [PATCH 03/27] Added tranform control and select entities GUI plugins Signed-off-by: ahcorde --- include/ignition/gazebo/gui/GuiEvents.hh | 21 + src/gui/gui.config | 42 + src/gui/plugins/CMakeLists.txt | 2 + .../plugins/scene_manager/GzSceneManager.cc | 17 +- .../plugins/select_entities/CMakeLists.txt | 11 + .../plugins/select_entities/SelectEntities.cc | 348 +++++++++ .../plugins/select_entities/SelectEntities.hh | 61 ++ .../select_entities/SelectEntities.qml | 28 + .../select_entities/SelectEntities.qrc | 5 + .../transform_control_logic/CMakeLists.txt | 11 + .../TransformControlLogic.cc | 724 ++++++++++++++++++ .../TransformControlLogic.hh | 61 ++ .../TransformControlLogic.qml | 28 + .../TransformControlLogic.qrc | 5 + 14 files changed, 1361 insertions(+), 3 deletions(-) create mode 100644 src/gui/plugins/select_entities/CMakeLists.txt create mode 100644 src/gui/plugins/select_entities/SelectEntities.cc create mode 100644 src/gui/plugins/select_entities/SelectEntities.hh create mode 100644 src/gui/plugins/select_entities/SelectEntities.qml create mode 100644 src/gui/plugins/select_entities/SelectEntities.qrc create mode 100644 src/gui/plugins/transform_control_logic/CMakeLists.txt create mode 100644 src/gui/plugins/transform_control_logic/TransformControlLogic.cc create mode 100644 src/gui/plugins/transform_control_logic/TransformControlLogic.hh create mode 100644 src/gui/plugins/transform_control_logic/TransformControlLogic.qml create mode 100644 src/gui/plugins/transform_control_logic/TransformControlLogic.qrc diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 20002bd6b1..dbf637f7cd 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -210,6 +210,27 @@ namespace events /// \brief The path of SDF file to be previewed. std::string filePath; }; + + class TransformControlMode : public QEvent + { + /// \brief Constructor + /// \param[in] _filePath The path to an SDF file. + public: explicit TransformControlMode(const bool _tranformModeActive) + : QEvent(kType), tranformModeActive(_tranformModeActive) + { + } + + /// \brief Unique type for this event. + static const QEvent::Type kType = QEvent::Type(QEvent::User + 6); + + public: bool TransformControl() + { + return this->tranformModeActive; + } + + private: bool tranformModeActive; + }; + } // namespace events } } // namespace gui diff --git a/src/gui/gui.config b/src/gui/gui.config index bb2b9f7e42..910b6ccb5c 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -54,6 +54,48 @@ + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + + false + 5 + 5 + floating + false + + + diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 7df38dfb35..30bea754bb 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -119,10 +119,12 @@ add_subdirectory(playback_scrubber) add_subdirectory(plotting) add_subdirectory(resource_spawner) add_subdirectory(scene3d) +add_subdirectory(select_entities) add_subdirectory(scene_manager) add_subdirectory(shapes) add_subdirectory(tape_measure) add_subdirectory(transform_control) +add_subdirectory(transform_control_logic) add_subdirectory(video_recorder) add_subdirectory(view_angle) add_subdirectory(visualize_lidar) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 7491687688..4d034ee354 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -49,6 +49,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Rendering utility public: RenderUtil renderUtil; + + public: bool blockUpdate = false; }; } } @@ -82,8 +84,11 @@ void GzSceneManager::Update(const UpdateInfo &_info, { IGN_PROFILE("GzSceneManager::Update"); - this->dataPtr->renderUtil.UpdateECM(_info, _ecm); - this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + if (!this->dataPtr->blockUpdate) + { + this->dataPtr->renderUtil.UpdateECM(_info, _ecm); + this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + } } ///////////////////////////////////////////////// @@ -93,6 +98,12 @@ bool GzSceneManager::eventFilter(QObject *_obj, QEvent *_event) { this->dataPtr->OnRender(); } + else if (_event->type() == ignition::gui::events::BlockOrbit::kType) + { + auto blockOrbit = reinterpret_cast( + _event); + this->dataPtr->blockUpdate = blockOrbit->Block(); + } // Standard event processing return QObject::eventFilter(_obj, _event); @@ -103,7 +114,7 @@ void GzSceneManagerPrivate::OnRender() { if (nullptr == this->scene) { - this->scene = rendering::someInitializedScene(); + this->scene = rendering::sceneFromFirstRenderEngine(); if (nullptr == this->scene) return; diff --git a/src/gui/plugins/select_entities/CMakeLists.txt b/src/gui/plugins/select_entities/CMakeLists.txt new file mode 100644 index 0000000000..e8d86d7d98 --- /dev/null +++ b/src/gui/plugins/select_entities/CMakeLists.txt @@ -0,0 +1,11 @@ +gz_add_gui_plugin(SelectEntities + SOURCES + SelectEntities.cc + QT_HEADERS + SelectEntities.hh + TEST_SOURCES + # CameraControllerManager_TEST.cc + PUBLIC_LINK_LIBS + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + ${PROJECT_LIBRARY_TARGET_NAME}-rendering +) diff --git a/src/gui/plugins/select_entities/SelectEntities.cc b/src/gui/plugins/select_entities/SelectEntities.cc new file mode 100644 index 0000000000..499ff7e479 --- /dev/null +++ b/src/gui/plugins/select_entities/SelectEntities.cc @@ -0,0 +1,348 @@ +/* + * Copyright (C) 2021 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 +#include +#include +#include +#include +#include +#include +#include +#include "ignition/rendering/Camera.hh" + +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/rendering/RenderUtil.hh" + +#include "SelectEntities.hh" + +namespace ignition +{ +namespace gazebo +{ +/// \brief Helper to store selection requests to be handled in the render +/// thread by `IgnRenderer::HandleEntitySelection`. +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 SelectEntities +class ignition::gazebo::plugins::SelectEntitiesPrivate +{ + public: void Initialize(); + + public: void HandleEntitySelection(); + + public: void UpdateSelectedEntity(const rendering::VisualPtr &_visual, + bool _sendEvent); + + public: void HighlightNode(const rendering::VisualPtr &_visual); + + public: void SetSelectedEntity(const rendering::VisualPtr &_visual); + + public: void DeselectAllEntities(); + + public: void LowlightNode(const rendering::VisualPtr &_visual); + + /// \brief Helper object to select entities. Only the latest selection + /// request is kept. + public: SelectionHelper selectionHelper; + + /// \brief Currently selected entities, organized by order of selection. + public: std::vector selectedEntities; + + //// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene = nullptr; + + /// \brief A map of entity ids and wire boxes + public: std::unordered_map wireBoxes; + + public: ignition::common::MouseEvent mouseEvent; + + public: bool mouseDirty = false; + + /// \brief User camera + public: rendering::CameraPtr camera = nullptr; + + public: bool transformControlActive = false; +}; + +using namespace ignition; +using namespace gazebo; +using namespace plugins; + +///////////////////////////////////////////////// +void SelectEntitiesPrivate::HandleEntitySelection() +{ + if (!mouseDirty) + return; + + this->mouseDirty = false; + + rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + if (!visual) + { + this->DeselectAllEntities(); + return; + } + + this->selectionHelper.selectEntity = std::get(visual->UserData("gazebo-entity")); + + if (this->selectionHelper.deselectAll) + { + this->DeselectAllEntities(); + + this->selectionHelper = SelectionHelper(); + } + else if (this->selectionHelper.selectEntity != kNullEntity) + { + this->UpdateSelectedEntity(visual, + this->selectionHelper.sendEvent); + + this->selectionHelper = SelectionHelper(); + } +} + +//////////////////////////////////////////////// +void SelectEntitiesPrivate::LowlightNode(const rendering::VisualPtr &_visual) +{ + Entity entityId = kNullEntity; + if (_visual) + entityId = std::get(_visual->UserData("gazebo-entity")); + if (this->wireBoxes.find(entityId) != this->wireBoxes.end()) + { + ignition::rendering::WireBoxPtr wireBox = + this->wireBoxes[entityId]; + auto visParent = wireBox->Parent(); + if (visParent) + visParent->SetVisible(false); + } +} + +//////////////////////////////////////////////// +void SelectEntitiesPrivate::HighlightNode(const rendering::VisualPtr &_visual) +{ + Entity entityId = kNullEntity; + + if (_visual) + entityId = std::get(_visual->UserData("gazebo-entity")); + + // If the entity is not found in the existing map, create a wire box + auto wireBoxIt = this->wireBoxes.find(entityId); + if (wireBoxIt == this->wireBoxes.end()) + { + auto white = this->scene->Material("highlight_material"); + if (!white) + { + white = this->scene->CreateMaterial("highlight_material"); + white->SetAmbient(1.0, 1.0, 1.0); + white->SetDiffuse(1.0, 1.0, 1.0); + white->SetSpecular(1.0, 1.0, 1.0); + white->SetEmissive(1.0, 1.0, 1.0); + } + + ignition::rendering::WireBoxPtr wireBox = + this->scene->CreateWireBox(); + ignition::math::AxisAlignedBox aabb = _visual->LocalBoundingBox(); + wireBox->SetBox(aabb); + + // Create visual and add wire box + ignition::rendering::VisualPtr wireBoxVis = + this->scene->CreateVisual(); + wireBoxVis->SetInheritScale(false); + wireBoxVis->AddGeometry(wireBox); + wireBoxVis->SetMaterial(white, false); + _visual->AddChild(wireBoxVis); + + // Add wire box to map for setting visibility + this->wireBoxes.insert( + std::pair(entityId, wireBox)); + } + else + { + ignition::rendering::WireBoxPtr wireBox = wireBoxIt->second; + ignition::math::AxisAlignedBox aabb = _visual->LocalBoundingBox(); + wireBox->SetBox(aabb); + auto visParent = wireBox->Parent(); + if (visParent) + visParent->SetVisible(true); + } +} + +///////////////////////////////////////////////// +void SelectEntitiesPrivate::SetSelectedEntity(const rendering::VisualPtr &_visual) +{ + Entity entityId = kNullEntity; + + if (_visual) + entityId = std::get(_visual->UserData("gazebo-entity")); + + if (entityId == kNullEntity) + return; + + this->selectedEntities.push_back(_visual->Id()); + this->HighlightNode(_visual); + ignition::gazebo::gui::events::EntitiesSelected entitiesSelected( + this->selectedEntities, true); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &entitiesSelected); +} + +///////////////////////////////////////////////// +void SelectEntitiesPrivate::DeselectAllEntities() +{ + if (this->scene) + { + for (const auto &entity : this->selectedEntities) + { + auto node = this->scene->VisualById(entity); + auto vis = std::dynamic_pointer_cast(node); + this->LowlightNode(vis); + } + this->selectedEntities.clear(); + + ignition::gazebo::gui::events::DeselectAllEntities deselectEvent(true); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &deselectEvent); + } +} + +///////////////////////////////////////////////// +void SelectEntitiesPrivate::UpdateSelectedEntity(const rendering::VisualPtr &_visual, + bool _sendEvent) +{ + + bool deselectedAll{false}; + std::cerr << "transformControlActive " << transformControlActive << '\n'; + + // Deselect all if control is not being held + if ((!(QGuiApplication::keyboardModifiers() & Qt::ControlModifier) && + !this->selectedEntities.empty()) || this->transformControlActive) + { + // Notify other widgets regardless of _sendEvent, because this is a new + // decision from this widget + this->DeselectAllEntities(); + deselectedAll = true; + } + + // Select new entity + this->SetSelectedEntity(_visual); + + // Notify other widgets of the currently selected entities + if (_sendEvent || deselectedAll) + { + ignition::gazebo::gui::events::EntitiesSelected selectEvent( + this->selectedEntities); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &selectEvent); + } +} + +///////////////////////////////////////////////// +void SelectEntitiesPrivate::Initialize() +{ + if (nullptr == this->scene) + { + this->scene = rendering::sceneFromFirstRenderEngine(); + if (nullptr == this->scene) + return; + + this->camera = std::dynamic_pointer_cast(this->scene->SensorByName("Scene3DCamera")); + if (!this->camera) + { + ignerr << "Camera is not available" << std::endl; + return; + } + } +} + +///////////////////////////////////////////////// +SelectEntities::SelectEntities() + : GuiSystem(), dataPtr(new SelectEntitiesPrivate) +{ +} + +///////////////////////////////////////////////// +SelectEntities::~SelectEntities() +{ +} + +///////////////////////////////////////////////// +void SelectEntities::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Select entities"; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->QuickWindow()->installEventFilter(this); + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); +} + +///////////////////////////////////////////////// +void SelectEntities::Update(const UpdateInfo &/* _info */, + EntityComponentManager &_ecm) +{ +} + +///////////////////////////////////////////////// +bool SelectEntities::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == ignition::gui::events::LeftClickOnScene::kType) + { + ignition::gui::events::LeftClickOnScene *_e = + static_cast(_event); + this->dataPtr->mouseEvent = _e->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == ignition::gui::events::Render::kType) + { + this->dataPtr->Initialize(); + this->dataPtr->HandleEntitySelection(); + } + else if (_event->type() == ignition::gazebo::gui::events::TransformControlMode::kType) + { + auto transformControlMode = reinterpret_cast( + _event); + this->dataPtr->transformControlActive = transformControlMode->TransformControl(); + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::plugins::SelectEntities, + ignition::gui::Plugin) diff --git a/src/gui/plugins/select_entities/SelectEntities.hh b/src/gui/plugins/select_entities/SelectEntities.hh new file mode 100644 index 0000000000..64df430f17 --- /dev/null +++ b/src/gui/plugins/select_entities/SelectEntities.hh @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2021 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 IGNITION_GUI_PLUGINS_CAMERACONTROLLERMANAGER_HH_ +#define IGNITION_GUI_PLUGINS_CAMERACONTROLLERMANAGER_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +namespace plugins +{ + class SelectEntitiesPrivate; + + class SelectEntities : public ignition::gazebo::GuiSystem + { + Q_OBJECT + + /// \brief Constructor + public: SelectEntities(); + + /// \brief Destructor + public: virtual ~SelectEntities(); + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + // Documentation inherited + public: virtual void LoadConfig(const tinyxml2::XMLElement *_pluginElem) + override; + + // Documentation inherited + private: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} +} +#endif diff --git a/src/gui/plugins/select_entities/SelectEntities.qml b/src/gui/plugins/select_entities/SelectEntities.qml new file mode 100644 index 0000000000..873da30014 --- /dev/null +++ b/src/gui/plugins/select_entities/SelectEntities.qml @@ -0,0 +1,28 @@ +/* + * Copyright (C) 2021 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 QtQuick 2.0 +import QtQuick.Controls 2.0 +import QtQuick.Layouts 1.3 + +// TODO: remove invisible rectangle, see +// https://github.com/ignitionrobotics/ign-gui/issues/220 +Rectangle { + visible: false + Layout.minimumWidth: 100 + Layout.minimumHeight: 100 +} diff --git a/src/gui/plugins/select_entities/SelectEntities.qrc b/src/gui/plugins/select_entities/SelectEntities.qrc new file mode 100644 index 0000000000..d303772321 --- /dev/null +++ b/src/gui/plugins/select_entities/SelectEntities.qrc @@ -0,0 +1,5 @@ + + + SelectEntities.qml + + diff --git a/src/gui/plugins/transform_control_logic/CMakeLists.txt b/src/gui/plugins/transform_control_logic/CMakeLists.txt new file mode 100644 index 0000000000..51845a57b1 --- /dev/null +++ b/src/gui/plugins/transform_control_logic/CMakeLists.txt @@ -0,0 +1,11 @@ +gz_add_gui_plugin(TransformControlLogic + SOURCES + TransformControlLogic.cc + QT_HEADERS + TransformControlLogic.hh + TEST_SOURCES + # CameraControllerManager_TEST.cc + PUBLIC_LINK_LIBS + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + ${PROJECT_LIBRARY_TARGET_NAME}-rendering +) diff --git a/src/gui/plugins/transform_control_logic/TransformControlLogic.cc b/src/gui/plugins/transform_control_logic/TransformControlLogic.cc new file mode 100644 index 0000000000..89ab17e557 --- /dev/null +++ b/src/gui/plugins/transform_control_logic/TransformControlLogic.cc @@ -0,0 +1,724 @@ +/* + * Copyright (C) 2021 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 + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + +#include "ignition/gazebo/gui/GuiEvents.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/World.hh" + +#include "TransformControlLogic.hh" + +/// \brief Private data class for TransformControlLogic +class ignition::gazebo::plugins::TransformControlLogicPrivate +{ + /// \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 + public: bool OnTransformMode(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + + public: void HandleTransform(); + + /// \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( + ignition::math::Vector3d &_point, math::Vector3d &_snapVals, + double _sensitivity = 0.4) const; + + /// \brief Constraints the passed in axis to the currently selected axes. + /// \param[in] _axis The axis to constrain. + public: void XYZConstraint(math::Vector3d &_axis); + + public: double SnapValue( + double _coord, double _interval, double _sensitivity) const; + + /// \brief Set the XYZ snap values. + /// \param[in] _xyz The XYZ snap values + public: void SetXYZSnap(const math::Vector3d &_xyz); + + /// \brief Set the RPY snap values. + /// \param[in] _rpy The RPY snap values + public: void SetRPYSnap(const math::Vector3d &_rpy); + + /// \brief Set the scale snap values. + /// \param[in] _scale The scale snap values + public: void SetScaleSnap(const math::Vector3d &_scale); + + ///////////////////////////////////////////////// + rendering::NodePtr TopLevelNode( + const rendering::NodePtr &_node) const; + + /// \brief Whether the transform gizmo is being dragged. + public: bool transformActive{false}; + + /// \brief Transform mode service + public: std::string transformModeService; + + /// \brief Transport node + public: transport::Node node; + + /// \brief Name of service for setting entity pose + public: std::string poseCmdService; + + /// \brief Name of the world + public: std::string worldName; + + /// \brief Transform mode: none, translation, rotation, or scale + public: rendering::TransformMode transformMode = + rendering::TransformMode::TM_NONE; + + /// \brief Transform space: local or world + public: rendering::TransformSpace transformSpace = + rendering::TransformSpace::TS_LOCAL; + + /// \brief Currently selected entities, organized by order of selection. + public: std::vector selectedEntities; + + /// \brief Transform controller for models + public: rendering::TransformController transformControl; + + public: ignition::common::MouseEvent mouseEvent; + + public: bool mouseDirty = false; + + /// \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 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 keep track of world pose setting used + /// for button translating. + public: bool isStartWorldPosSet = false; + + /// \brief The starting world pose of a clicked visual. + public: ignition::math::Vector3d startWorldPos = math::Vector3d::Zero; + + public: ignition::common::KeyEvent keyEvent; + + public: std::mutex mutex; + + //// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene = nullptr; + + /// \brief User camera + public: rendering::CameraPtr camera = nullptr; + + /// \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; + + public: bool blockOrbit = false; +}; + +using namespace ignition; +using namespace gazebo; +using namespace plugins; + +void TransformControlLogicPrivate::HandleTransform() +{ + if (nullptr == this->scene) + { + this->scene = rendering::sceneFromFirstRenderEngine(); + if (nullptr == this->scene) + return; + + this->camera = std::dynamic_pointer_cast(this->scene->SensorByName("Scene3DCamera")); + if (!this->camera) + { + ignerr << "TransformControlLogic Camera is not available" << std::endl; + return; + } + + if (!this->transformControl.Camera()) + this->transformControl.SetCamera(this->camera); + } + + // set transform configuration + this->transformControl.SetTransformMode(this->transformMode); + + // stop and detach transform controller if mode is none or no entity is + // selected + if (this->transformMode == rendering::TransformMode::TM_NONE || + (this->transformControl.Node() && + this->selectedEntities.empty())) + { + if (this->transformControl.Active()) + this->transformControl.Stop(); + + this->transformControl.Detach(); + } + else + { + // shift indicates world space transformation + this->transformSpace = (this->keyEvent.Shift()) ? + rendering::TransformSpace::TS_WORLD : + rendering::TransformSpace::TS_LOCAL; + this->transformControl.SetTransformSpace( + this->transformSpace); + } + + // update gizmo visual + this->transformControl.Update(); + + // check for mouse events + if (!this->mouseDirty) + return; + + // handle transform control + if (this->mouseEvent.Button() == ignition::common::MouseEvent::LEFT) + { + if (this->mouseEvent.Type() == ignition::common::MouseEvent::PRESS + && this->transformControl.Node()) + { + this->mousePressPos = this->mouseEvent.Pos(); + + // get the visual at mouse position + rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + if (visual) + { + // check if the visual is an axis in the gizmo visual + math::Vector3d axis = + this->transformControl.AxisById(visual->Id()); + if (axis != ignition::math::Vector3d::Zero) + { + this->blockOrbit = true; + // start the transform process + this->transformControl.SetActiveAxis(axis); + this->transformControl.Start(); + this->mouseDirty = false; + } + else + { + this->blockOrbit = false; + return; + } + } + } + else if (this->mouseEvent.Type() == ignition::common::MouseEvent::RELEASE) + { + this->blockOrbit = false; + + this->isStartWorldPosSet = false; + if (this->transformControl.Active()) + { + if (this->transformControl.Node()) + { + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error setting pose" << std::endl; + }; + rendering::NodePtr nodeTmp = this->transformControl.Node(); + auto topVisual = std::dynamic_pointer_cast(nodeTmp); + ignition::msgs::Pose req; + req.set_name(topVisual->Name()); + std::cerr << "TransformControlLogic Moving " << nodeTmp->Name() << " " << topVisual->Name() << '\n'; + msgs::Set(req.mutable_position(), nodeTmp->WorldPosition()); + msgs::Set(req.mutable_orientation(), nodeTmp->WorldRotation()); + if (this->poseCmdService.empty()) + { + this->poseCmdService = "/world/" + this->worldName + + "/set_pose"; + } + this->poseCmdService = transport::TopicUtils::AsValidTopic( + this->poseCmdService); + if (this->poseCmdService.empty()) + { + ignerr << "Failed to create valid pose command service for world [" + << this->worldName <<"]" << std::endl; + return; + } + this->node.Request(this->poseCmdService, req, cb); + } + + this->transformControl.Stop(); + this->mouseDirty = false; + } + // Select entity + else if (!this->mouseEvent.Dragging()) + { + rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + if (!visual) + { + return; + } + + // check if the visual is an axis in the gizmo visual + math::Vector3d axis = this->transformControl.AxisById(visual->Id()); + if (axis == ignition::math::Vector3d::Zero) + { + auto topNode = this->TopLevelNode(visual); + if (!topNode) + { + return; + } + + auto topVis = std::dynamic_pointer_cast(topNode); + // TODO(anyone) Check plane geometry instead of hardcoded name! + if (topVis && topVis->Name() != "ground_plane") + { + // // Highlight entity and notify other widgets + + // Attach control if in a transform mode - control is attached to: + // * latest selection + // * top-level nodes (model, light...) + if (this->transformMode != rendering::TransformMode::TM_NONE) + { + std::cerr << "TransformControlLogic Trying to attach!" << '\n'; + rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + + auto topNode = this->TopLevelNode(visual); + auto topVisual = std::dynamic_pointer_cast(topNode); + + if (topNode == topVisual) + { + this->transformControl.Attach(topVisual); + } + else + { + this->transformControl.Detach(); + } + } + + this->mouseDirty = false; + return; + } + } + } + } + } + if (this->mouseEvent.Type() == common::MouseEvent::MOVE + && this->transformControl.Active()) + { + std::cerr << "TransformControlLogicPrivate MOVE" << '\n'; + + this->blockOrbit = true; + // compute the the start and end mouse positions in normalized coordinates + auto imageWidth = static_cast(this->camera->ImageWidth()); + auto imageHeight = static_cast( + this->camera->ImageHeight()); + double nx = 2.0 * this->mousePressPos.X() / imageWidth - 1.0; + double ny = 1.0 - 2.0 * this->mousePressPos.Y() / imageHeight; + double nxEnd = 2.0 * this->mouseEvent.Pos().X() / imageWidth - 1.0; + double nyEnd = 1.0 - 2.0 * this->mouseEvent.Pos().Y() / imageHeight; + math::Vector2d start(nx, ny); + math::Vector2d end(nxEnd, nyEnd); + + // get the current active axis + math::Vector3d axis = this->transformControl.ActiveAxis(); + + // compute 3d transformation from 2d mouse movement + if (this->transformControl.Mode() == + rendering::TransformMode::TM_TRANSLATION) + { + Entity nodeId = this->selectedEntities.front(); + rendering::NodePtr target = this->scene->VisualById(nodeId); + if (!target) + { + ignwarn << "Failed to find node with ID [" << nodeId << "]" + << std::endl; + return; + } + this->XYZConstraint(axis); + if (!this->isStartWorldPosSet) + { + this->isStartWorldPosSet = true; + this->startWorldPos = target->WorldPosition(); + } + ignition::math::Vector3d worldPos = target->WorldPosition(); + math::Vector3d distance = + this->transformControl.TranslationFrom2d(axis, start, end); + if (this->keyEvent.Control()) + { + // Translate to world frame for snapping + distance += this->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()); + + this->SnapPoint(distance, snapVals); + + // Translate back to entity frame + distance -= this->startWorldPos; + distance *= axis; + } + this->transformControl.Translate(distance); + std::cerr << "distance " << distance << " " << startWorldPos << '\n'; + } + else if (this->transformControl.Mode() == + rendering::TransformMode::TM_ROTATION) + { + math::Quaterniond rotation = + this->transformControl.RotationFrom2d(axis, start, end); + + if (this->keyEvent.Control()) + { + math::Vector3d currentRot = rotation.Euler(); + math::Vector3d snapVals = this->rpySnap; + + if (snapVals.X() <= 1e-4) + { + snapVals.X() = IGN_PI/4; + } + else + { + snapVals.X() = IGN_DTOR(snapVals.X()); + } + if (snapVals.Y() <= 1e-4) + { + snapVals.Y() = IGN_PI/4; + } + else + { + snapVals.Y() = IGN_DTOR(snapVals.Y()); + } + if (snapVals.Z() <= 1e-4) + { + snapVals.Z() = IGN_PI/4; + } + else + { + snapVals.Z() = IGN_DTOR(snapVals.Z()); + } + + this->SnapPoint(currentRot, snapVals); + rotation = math::Quaterniond::EulerToQuaternion(currentRot); + } + this->transformControl.Rotate(rotation); + } + else if (this->transformControl.Mode() == + rendering::TransformMode::TM_SCALE) + { + this->XYZConstraint(axis); + // note: scaling is limited to local space + math::Vector3d scale = + this->transformControl.ScaleFrom2d(axis, start, end); + if (this->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; + + this->SnapPoint(scale, snapVals); + } + this->transformControl.Scale(scale); + } + this->mouseDirty = false; + } + + ignition::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &blockOrbitEvent); +} + +///////////////////////////////////////////////// +rendering::NodePtr TransformControlLogicPrivate::TopLevelNode( + const rendering::NodePtr &_node) const +{ + if (!this->scene) + return rendering::NodePtr(); + + rendering::NodePtr rootNode = this->scene->RootVisual(); + + rendering::NodePtr nodeTmp = _node; + while (nodeTmp && nodeTmp->Parent() != rootNode) + { + nodeTmp = + std::dynamic_pointer_cast(nodeTmp->Parent()); + } + + return nodeTmp; +} + +///////////////////////////////////////////////// +void TransformControlLogicPrivate::SetXYZSnap(const math::Vector3d &_xyz) +{ + this->xyzSnap = _xyz; +} + +///////////////////////////////////////////////// +void TransformControlLogicPrivate::SetRPYSnap(const math::Vector3d &_rpy) +{ + this->rpySnap = _rpy; +} + +///////////////////////////////////////////////// +void TransformControlLogicPrivate::SetScaleSnap(const math::Vector3d &_scale) +{ + this->scaleSnap = _scale; +} + +///////////////////////////////////////////////// +double TransformControlLogicPrivate::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 TransformControlLogicPrivate::XYZConstraint(math::Vector3d &_axis) +{ + math::Vector3d translationAxis = math::Vector3d::Zero; + + if (this->xPressed) + { + translationAxis += math::Vector3d::UnitX; + } + + if (this->yPressed) + { + translationAxis += math::Vector3d::UnitY; + } + + if (this->zPressed) + { + translationAxis += math::Vector3d::UnitZ; + } + + if (translationAxis != math::Vector3d::Zero) + { + _axis = translationAxis; + } +} + +///////////////////////////////////////////////// +void TransformControlLogicPrivate::SnapPoint( + ignition::math::Vector3d &_point, math::Vector3d &_snapVals, + double _sensitivity) const +{ + if (_snapVals.X() <= 0 || _snapVals.Y() <= 0 || _snapVals.Z() <= 0) + { + ignerr << "Interval distance must be greater than 0" + << std::endl; + return; + } + + if (_sensitivity < 0 || _sensitivity > 1.0) + { + ignerr << "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); +} + +bool TransformControlLogicPrivate::OnTransformMode(const msgs::StringMsg &_msg, + msgs::Boolean &_res) +{ + std::string _mode = _msg.data(); + std::lock_guard lock(this->mutex); + if (_mode == "select") + this->transformMode = rendering::TransformMode::TM_NONE; + else if (_mode == "translate") + this->transformMode = rendering::TransformMode::TM_TRANSLATION; + else if (_mode == "rotate") + this->transformMode = rendering::TransformMode::TM_ROTATION; + else if (_mode == "scale") + this->transformMode = rendering::TransformMode::TM_SCALE; + else + ignerr << "Unknown transform mode: [" << _mode << "]" << std::endl; + + ignition::gazebo::gui::events::TransformControlMode transformControlMode(this->transformMode); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &transformControlMode); + + this->HandleTransform(); + + _res.set_data(true); + return true; +} + +///////////////////////////////////////////////// +TransformControlLogic::TransformControlLogic() + : GuiSystem(), dataPtr(new TransformControlLogicPrivate) +{ +} + +///////////////////////////////////////////////// +TransformControlLogic::~TransformControlLogic() +{ +} + +///////////////////////////////////////////////// +void TransformControlLogic::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Select entities"; + + // transform mode + this->dataPtr->transformModeService = + "/gui/transform_mode"; + this->dataPtr->node.Advertise(this->dataPtr->transformModeService, + &TransformControlLogicPrivate::OnTransformMode, this->dataPtr.get()); + ignmsg << "Transform mode service on [" + << this->dataPtr->transformModeService << "]" << std::endl; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); +} + +///////////////////////////////////////////////// +void TransformControlLogic::Update(const UpdateInfo &/* _info */, + EntityComponentManager &_ecm) +{ + if (this->dataPtr->worldName.empty()) + { + 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; + }); + } +} + +///////////////////////////////////////////////// +bool TransformControlLogic::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == ignition::gazebo::gui::events::EntitiesSelected::kType) + { + ignition::gazebo::gui::events::EntitiesSelected *_e = + static_cast(_event); + this->dataPtr->selectedEntities = _e->Data(); + this->dataPtr->HandleTransform(); + } + if (_event->type() == ignition::gui::events::Render::kType) + { + if (this->dataPtr->transformControl.Active()) + this->dataPtr->mouseDirty = true; + this->dataPtr->HandleTransform(); + } + else if (_event->type() == ignition::gazebo::gui::events::DeselectAllEntities::kType) + { + this->dataPtr->selectedEntities.clear(); + } + else if (_event->type() == ignition::gui::events::LeftClickOnScene::kType) + { + ignition::gui::events::LeftClickOnScene *_e = + static_cast(_event); + this->dataPtr->mouseEvent = _e->Mouse(); + this->dataPtr->mouseDirty = true; + this->dataPtr->HandleTransform(); + } + else if (_event->type() == ignition::gui::events::KeyPress::kType) + { + ignition::gui::events::KeyPress *_e = + static_cast(_event); + this->dataPtr->keyEvent = _e->Key(); + } + else if (_event->type() == + ignition::gui::events::SnapIntervals::kType) + { + auto snapEvent = reinterpret_cast( + _event); + if (snapEvent) + { + this->dataPtr->SetXYZSnap(snapEvent->Position()); + this->dataPtr->SetRPYSnap(snapEvent->Rotation()); + this->dataPtr->SetScaleSnap(snapEvent->Scale()); + } + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::plugins::TransformControlLogic, + ignition::gui::Plugin) diff --git a/src/gui/plugins/transform_control_logic/TransformControlLogic.hh b/src/gui/plugins/transform_control_logic/TransformControlLogic.hh new file mode 100644 index 0000000000..3a795d23cc --- /dev/null +++ b/src/gui/plugins/transform_control_logic/TransformControlLogic.hh @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2021 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 IGNITION_GUI_PLUGINS_CAMERACONTROLLERMANAGER_HH_ +#define IGNITION_GUI_PLUGINS_CAMERACONTROLLERMANAGER_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +namespace plugins +{ + class TransformControlLogicPrivate; + + class TransformControlLogic : public ignition::gazebo::GuiSystem + { + Q_OBJECT + + /// \brief Constructor + public: TransformControlLogic(); + + /// \brief Destructor + public: virtual ~TransformControlLogic(); + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + // Documentation inherited + public: virtual void LoadConfig(const tinyxml2::XMLElement *_pluginElem) + override; + + // Documentation inherited + private: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} +} +#endif diff --git a/src/gui/plugins/transform_control_logic/TransformControlLogic.qml b/src/gui/plugins/transform_control_logic/TransformControlLogic.qml new file mode 100644 index 0000000000..873da30014 --- /dev/null +++ b/src/gui/plugins/transform_control_logic/TransformControlLogic.qml @@ -0,0 +1,28 @@ +/* + * Copyright (C) 2021 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 QtQuick 2.0 +import QtQuick.Controls 2.0 +import QtQuick.Layouts 1.3 + +// TODO: remove invisible rectangle, see +// https://github.com/ignitionrobotics/ign-gui/issues/220 +Rectangle { + visible: false + Layout.minimumWidth: 100 + Layout.minimumHeight: 100 +} diff --git a/src/gui/plugins/transform_control_logic/TransformControlLogic.qrc b/src/gui/plugins/transform_control_logic/TransformControlLogic.qrc new file mode 100644 index 0000000000..9920375f35 --- /dev/null +++ b/src/gui/plugins/transform_control_logic/TransformControlLogic.qrc @@ -0,0 +1,5 @@ + + + TransformControlLogic.qml + + From 4465595bc523f0517d651a6b977093128c26c489 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 18 Jun 2021 13:41:47 +0200 Subject: [PATCH 04/27] Used new plugin Signed-off-by: ahcorde --- src/gui/gui.config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gui/gui.config b/src/gui/gui.config index 910b6ccb5c..e02c5be030 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -26,7 +26,7 @@ - + 3D View false From eda24b60a6f400a1890517b544835772d2209121 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 18 Jun 2021 15:01:03 +0200 Subject: [PATCH 05/27] Fix Key event class name Signed-off-by: ahcorde --- .../transform_control_logic/TransformControlLogic.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/gui/plugins/transform_control_logic/TransformControlLogic.cc b/src/gui/plugins/transform_control_logic/TransformControlLogic.cc index 89ab17e557..028d76b2ec 100644 --- a/src/gui/plugins/transform_control_logic/TransformControlLogic.cc +++ b/src/gui/plugins/transform_control_logic/TransformControlLogic.cc @@ -696,10 +696,10 @@ bool TransformControlLogic::eventFilter(QObject *_obj, QEvent *_event) this->dataPtr->mouseDirty = true; this->dataPtr->HandleTransform(); } - else if (_event->type() == ignition::gui::events::KeyPress::kType) + else if (_event->type() == ignition::gui::events::KeyPressOnScene::kType) { - ignition::gui::events::KeyPress *_e = - static_cast(_event); + ignition::gui::events::KeyPressOnScene *_e = + static_cast(_event); this->dataPtr->keyEvent = _e->Key(); } else if (_event->type() == From 5f94fe2e910d1355fb2f6675f7eeb6ab572b4543 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 21 Jun 2021 12:46:15 -0700 Subject: [PATCH 06/27] Update to latest Signed-off-by: Louise Poubel --- src/gui/gui.config | 2 +- src/gui/plugins/scene_manager/GzSceneManager.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/gui/gui.config b/src/gui/gui.config index bb2b9f7e42..67f4de172e 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -26,7 +26,7 @@ - + 3D View false diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 7491687688..61c86a21d2 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -103,7 +103,7 @@ void GzSceneManagerPrivate::OnRender() { if (nullptr == this->scene) { - this->scene = rendering::someInitializedScene(); + this->scene = rendering::sceneFromFirstRenderEngine(); if (nullptr == this->scene) return; From b7922fca5b91a7a864ad95894b7ba6ab9a5ef43a Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 22 Jun 2021 19:12:06 +0200 Subject: [PATCH 07/27] Some fixes to select entities plugin Signed-off-by: ahcorde --- .../plugins/select_entities/SelectEntities.cc | 160 +++++++++++++++--- .../plugins/select_entities/SelectEntities.hh | 4 - 2 files changed, 134 insertions(+), 30 deletions(-) diff --git a/src/gui/plugins/select_entities/SelectEntities.cc b/src/gui/plugins/select_entities/SelectEntities.cc index 499ff7e479..ec0a17d7ae 100644 --- a/src/gui/plugins/select_entities/SelectEntities.cc +++ b/src/gui/plugins/select_entities/SelectEntities.cc @@ -15,6 +15,8 @@ * */ +#include // std::find + #include #include #include @@ -70,6 +72,9 @@ class ignition::gazebo::plugins::SelectEntitiesPrivate public: void LowlightNode(const rendering::VisualPtr &_visual); + public: rendering::NodePtr TopLevelNode( + const rendering::NodePtr &_node); + /// \brief Helper object to select entities. Only the latest selection /// request is kept. public: SelectionHelper selectionHelper; @@ -77,19 +82,32 @@ class ignition::gazebo::plugins::SelectEntitiesPrivate /// \brief Currently selected entities, organized by order of selection. public: std::vector selectedEntities; + /// \brief Currently selected entities, organized by order of selection. + public: std::vector selectedEntitiesID; + + /// \brief New entities received from other plugins. + public: std::vector selectedEntitiesIDNew; + //// \brief Pointer to the rendering scene public: rendering::ScenePtr scene = nullptr; /// \brief A map of entity ids and wire boxes - public: std::unordered_map wireBoxes; + public: std::unordered_map wireBoxes; + /// \brief MouseEvent public: ignition::common::MouseEvent mouseEvent; + /// \brief is the mouse modify ? public: bool mouseDirty = false; + /// \brief selected entities from other plugins (for example: entityTree) + public: bool recievedSelectedEntities = false; + /// \brief User camera public: rendering::CameraPtr camera = nullptr; + /// \brief is transform control active ? public: bool transformControlActive = false; }; @@ -100,6 +118,32 @@ using namespace plugins; ///////////////////////////////////////////////// void SelectEntitiesPrivate::HandleEntitySelection() { + if (this->recievedSelectedEntities) + { + if (!(QGuiApplication::keyboardModifiers() & Qt::ControlModifier)) + { + this->DeselectAllEntities(); + } + + for (unsigned int i = 0; i < this->selectedEntitiesIDNew.size(); i++) + { + auto visualToHighLight = this->scene->VisualById( + this->selectedEntitiesIDNew[i]); + this->selectedEntitiesID.push_back(this->selectedEntitiesIDNew[i]); + this->selectedEntities.push_back( + std::get(visualToHighLight->UserData("gazebo-entity"))); + this->HighlightNode(visualToHighLight); + ignition::gazebo::gui::events::EntitiesSelected selectEvent( + this->selectedEntities); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &selectEvent); + } + this->recievedSelectedEntities = false; + this->selectionHelper = SelectionHelper(); + this->selectedEntitiesIDNew.clear(); + } + if (!mouseDirty) return; @@ -115,7 +159,8 @@ void SelectEntitiesPrivate::HandleEntitySelection() return; } - this->selectionHelper.selectEntity = std::get(visual->UserData("gazebo-entity")); + this->selectionHelper.selectEntity = + std::get(visual->UserData("gazebo-entity")); if (this->selectionHelper.deselectAll) { @@ -199,20 +244,47 @@ void SelectEntitiesPrivate::HighlightNode(const rendering::VisualPtr &_visual) } ///////////////////////////////////////////////// -void SelectEntitiesPrivate::SetSelectedEntity(const rendering::VisualPtr &_visual) +rendering::NodePtr SelectEntitiesPrivate::TopLevelNode( + const rendering::NodePtr &_node) +{ + if (!this->scene) + return rendering::NodePtr(); + + rendering::NodePtr rootNode = this->scene->RootVisual(); + + rendering::NodePtr nodeTmp = _node; + while (nodeTmp && nodeTmp->Parent() != rootNode) + { + nodeTmp = + std::dynamic_pointer_cast(nodeTmp->Parent()); + } + + return nodeTmp; +} + +///////////////////////////////////////////////// +void SelectEntitiesPrivate::SetSelectedEntity( + const rendering::VisualPtr &_visual) { Entity entityId = kNullEntity; - if (_visual) - entityId = std::get(_visual->UserData("gazebo-entity")); + auto topLevelNode = this->TopLevelNode(_visual); + auto topLevelVisual = std::dynamic_pointer_cast( + topLevelNode); + + if (topLevelVisual) + { + entityId = std::get(topLevelVisual->UserData("gazebo-entity")); + } if (entityId == kNullEntity) return; - this->selectedEntities.push_back(_visual->Id()); + this->selectedEntities.push_back(entityId); + this->selectedEntitiesID.push_back(_visual->Id()); this->HighlightNode(_visual); ignition::gazebo::gui::events::EntitiesSelected entitiesSelected( - this->selectedEntities, true); + this->selectedEntities); ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &entitiesSelected); @@ -223,13 +295,14 @@ void SelectEntitiesPrivate::DeselectAllEntities() { if (this->scene) { - for (const auto &entity : this->selectedEntities) + for (const auto &entity : this->selectedEntitiesID) { auto node = this->scene->VisualById(entity); auto vis = std::dynamic_pointer_cast(node); this->LowlightNode(vis); } this->selectedEntities.clear(); + this->selectedEntitiesID.clear(); ignition::gazebo::gui::events::DeselectAllEntities deselectEvent(true); ignition::gui::App()->sendEvent( @@ -239,16 +312,14 @@ void SelectEntitiesPrivate::DeselectAllEntities() } ///////////////////////////////////////////////// -void SelectEntitiesPrivate::UpdateSelectedEntity(const rendering::VisualPtr &_visual, - bool _sendEvent) +void SelectEntitiesPrivate::UpdateSelectedEntity( + const rendering::VisualPtr &_visual, bool _sendEvent) { - bool deselectedAll{false}; - std::cerr << "transformControlActive " << transformControlActive << '\n'; // Deselect all if control is not being held if ((!(QGuiApplication::keyboardModifiers() & Qt::ControlModifier) && - !this->selectedEntities.empty()) || this->transformControlActive) + !this->selectedEntitiesID.empty()) || this->transformControlActive) { // Notify other widgets regardless of _sendEvent, because this is a new // decision from this widget @@ -279,7 +350,8 @@ void SelectEntitiesPrivate::Initialize() if (nullptr == this->scene) return; - this->camera = std::dynamic_pointer_cast(this->scene->SensorByName("Scene3DCamera")); + this->camera = std::dynamic_pointer_cast( + this->scene->SensorByName("Scene3DCamera")); if (!this->camera) { ignerr << "Camera is not available" << std::endl; @@ -305,38 +377,74 @@ void SelectEntities::LoadConfig(const tinyxml2::XMLElement *) if (this->title.empty()) this->title = "Select entities"; - ignition::gui::App()->findChild< - ignition::gui::MainWindow *>()->QuickWindow()->installEventFilter(this); ignition::gui::App()->findChild< ignition::gui::MainWindow *>()->installEventFilter(this); } -///////////////////////////////////////////////// -void SelectEntities::Update(const UpdateInfo &/* _info */, - EntityComponentManager &_ecm) -{ -} - ///////////////////////////////////////////////// bool SelectEntities::eventFilter(QObject *_obj, QEvent *_event) { + // std::cerr << "_obj " << _obj << " this " << this << '\n'; if (_event->type() == ignition::gui::events::LeftClickOnScene::kType) { ignition::gui::events::LeftClickOnScene *_e = static_cast(_event); this->dataPtr->mouseEvent = _e->Mouse(); - this->dataPtr->mouseDirty = true; + // handle transform control + if (this->dataPtr->mouseEvent.Button() == + ignition::common::MouseEvent::LEFT) + { + if (this->dataPtr->mouseEvent.Type() == + ignition::common::MouseEvent::PRESS) + { + this->dataPtr->mouseDirty = true; + } + } } else if (_event->type() == ignition::gui::events::Render::kType) { this->dataPtr->Initialize(); this->dataPtr->HandleEntitySelection(); } - else if (_event->type() == ignition::gazebo::gui::events::TransformControlMode::kType) + else if (_event->type() == + ignition::gazebo::gui::events::TransformControlMode::kType) { - auto transformControlMode = reinterpret_cast( + auto transformControlMode = + reinterpret_cast( _event); - this->dataPtr->transformControlActive = transformControlMode->TransformControl(); + this->dataPtr->transformControlActive = + transformControlMode->TransformControl(); + } + else if (_event->type() == + ignition::gazebo::gui::events::EntitiesSelected::kType) + { + auto selectedEvent = + reinterpret_cast(_event); + if (selectedEvent && !selectedEvent->Data().empty() && + selectedEvent->FromUser()) + { + for (const auto &entity : selectedEvent->Data()) + { + for (unsigned int i = 0; i < this->dataPtr->scene->VisualCount(); i++) + { + auto visual = this->dataPtr->scene->VisualByIndex(i); + auto entityId = static_cast( + std::get(visual->UserData("gazebo-entity"))); + if (entityId == entity) + { + this->dataPtr->selectedEntitiesIDNew.push_back(visual->Id()); + this->dataPtr->recievedSelectedEntities = true; + break; + } + } + } + } + } + else if (_event->type() == + ignition::gazebo::gui::events::DeselectAllEntities::kType) + { + this->dataPtr->selectedEntitiesID.clear(); + this->dataPtr->selectedEntities.clear(); } // Standard event processing diff --git a/src/gui/plugins/select_entities/SelectEntities.hh b/src/gui/plugins/select_entities/SelectEntities.hh index 64df430f17..9a3561ff8e 100644 --- a/src/gui/plugins/select_entities/SelectEntities.hh +++ b/src/gui/plugins/select_entities/SelectEntities.hh @@ -40,10 +40,6 @@ namespace plugins /// \brief Destructor public: virtual ~SelectEntities(); - // Documentation inherited - public: void Update(const UpdateInfo &_info, - EntityComponentManager &_ecm) override; - // Documentation inherited public: virtual void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; From 3493fa060fe8d0de53d8615da00f297128199894 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 22 Jun 2021 19:53:52 +0200 Subject: [PATCH 08/27] Fixed transform control and linters Signed-off-by: ahcorde --- .../plugins/select_entities/SelectEntities.cc | 5 +- .../TransformControlLogic.cc | 46 ++++++++++++------- 2 files changed, 34 insertions(+), 17 deletions(-) diff --git a/src/gui/plugins/select_entities/SelectEntities.cc b/src/gui/plugins/select_entities/SelectEntities.cc index ec0a17d7ae..c767072d27 100644 --- a/src/gui/plugins/select_entities/SelectEntities.cc +++ b/src/gui/plugins/select_entities/SelectEntities.cc @@ -15,7 +15,10 @@ * */ -#include // std::find +#include +#include +#include +#include #include #include diff --git a/src/gui/plugins/transform_control_logic/TransformControlLogic.cc b/src/gui/plugins/transform_control_logic/TransformControlLogic.cc index 028d76b2ec..8beea6c507 100644 --- a/src/gui/plugins/transform_control_logic/TransformControlLogic.cc +++ b/src/gui/plugins/transform_control_logic/TransformControlLogic.cc @@ -14,7 +14,10 @@ * limitations under the License. * */ +#include #include +#include +#include #include #include @@ -172,7 +175,8 @@ void TransformControlLogicPrivate::HandleTransform() if (nullptr == this->scene) return; - this->camera = std::dynamic_pointer_cast(this->scene->SensorByName("Scene3DCamera")); + this->camera = std::dynamic_pointer_cast( + this->scene->SensorByName("Scene3DCamera")); if (!this->camera) { ignerr << "TransformControlLogic Camera is not available" << std::endl; @@ -263,10 +267,10 @@ void TransformControlLogicPrivate::HandleTransform() ignerr << "Error setting pose" << std::endl; }; rendering::NodePtr nodeTmp = this->transformControl.Node(); - auto topVisual = std::dynamic_pointer_cast(nodeTmp); + auto topVisual = std::dynamic_pointer_cast( + nodeTmp); ignition::msgs::Pose req; req.set_name(topVisual->Name()); - std::cerr << "TransformControlLogic Moving " << nodeTmp->Name() << " " << topVisual->Name() << '\n'; msgs::Set(req.mutable_position(), nodeTmp->WorldPosition()); msgs::Set(req.mutable_orientation(), nodeTmp->WorldRotation()); if (this->poseCmdService.empty()) @@ -321,18 +325,17 @@ void TransformControlLogicPrivate::HandleTransform() // * top-level nodes (model, light...) if (this->transformMode != rendering::TransformMode::TM_NONE) { - std::cerr << "TransformControlLogic Trying to attach!" << '\n'; - rendering::VisualPtr visual = this->scene->VisualAt( + rendering::VisualPtr clickedVisual = this->scene->VisualAt( this->camera, this->mouseEvent.Pos()); + auto topClickedNode = this->TopLevelNode(clickedVisual); + auto topClickedVisual = + std::dynamic_pointer_cast(topClickedNode); - auto topNode = this->TopLevelNode(visual); - auto topVisual = std::dynamic_pointer_cast(topNode); - - if (topNode == topVisual) + if (topClickedNode == topClickedVisual) { - this->transformControl.Attach(topVisual); + this->transformControl.Attach(topClickedVisual); } else { @@ -350,8 +353,6 @@ void TransformControlLogicPrivate::HandleTransform() if (this->mouseEvent.Type() == common::MouseEvent::MOVE && this->transformControl.Active()) { - std::cerr << "TransformControlLogicPrivate MOVE" << '\n'; - this->blockOrbit = true; // compute the the start and end mouse positions in normalized coordinates auto imageWidth = static_cast(this->camera->ImageWidth()); @@ -372,7 +373,19 @@ void TransformControlLogicPrivate::HandleTransform() rendering::TransformMode::TM_TRANSLATION) { Entity nodeId = this->selectedEntities.front(); - rendering::NodePtr target = this->scene->VisualById(nodeId); + rendering::NodePtr target; + for (unsigned int i = 0; i < this->scene->VisualCount(); i++) + { + auto visual = this->scene->VisualByIndex(i); + auto entityId = static_cast( + std::get(visual->UserData("gazebo-entity"))); + if (entityId == nodeId) + { + target = std::dynamic_pointer_cast( + this->scene->VisualById(visual->Id())); + break; + } + } if (!target) { ignwarn << "Failed to find node with ID [" << nodeId << "]" @@ -406,7 +419,6 @@ void TransformControlLogicPrivate::HandleTransform() distance *= axis; } this->transformControl.Translate(distance); - std::cerr << "distance " << distance << " " << startWorldPos << '\n'; } else if (this->transformControl.Mode() == rendering::TransformMode::TM_ROTATION) @@ -609,7 +621,8 @@ bool TransformControlLogicPrivate::OnTransformMode(const msgs::StringMsg &_msg, else ignerr << "Unknown transform mode: [" << _mode << "]" << std::endl; - ignition::gazebo::gui::events::TransformControlMode transformControlMode(this->transformMode); + ignition::gazebo::gui::events::TransformControlMode transformControlMode( + this->transformMode); ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &transformControlMode); @@ -684,7 +697,8 @@ bool TransformControlLogic::eventFilter(QObject *_obj, QEvent *_event) this->dataPtr->mouseDirty = true; this->dataPtr->HandleTransform(); } - else if (_event->type() == ignition::gazebo::gui::events::DeselectAllEntities::kType) + else if (_event->type() == + ignition::gazebo::gui::events::DeselectAllEntities::kType) { this->dataPtr->selectedEntities.clear(); } From 6bb0bcec8d7b5f4eb418374cc9d28db416c983de Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 1 Jul 2021 17:15:06 +0200 Subject: [PATCH 09/27] Added feedback Signed-off-by: ahcorde --- include/ignition/gazebo/gui/GuiEvents.hh | 2 +- .../plugins/select_entities/SelectEntities.cc | 18 ++++++++---- .../plugins/select_entities/SelectEntities.hh | 2 ++ .../TransformControlLogic.cc | 29 ++++++++++++++----- .../TransformControlLogic.hh | 2 ++ 5 files changed, 39 insertions(+), 14 deletions(-) diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 89ed386366..376af7a27e 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -164,7 +164,7 @@ namespace events class TransformControlMode : public QEvent { /// \brief Constructor - /// \param[in] _filePath The path to an SDF file. + /// \param[in] _tranformModeActive is the transform control mode active public: explicit TransformControlMode(const bool _tranformModeActive) : QEvent(kType), tranformModeActive(_tranformModeActive) { diff --git a/src/gui/plugins/select_entities/SelectEntities.cc b/src/gui/plugins/select_entities/SelectEntities.cc index c767072d27..09907d467d 100644 --- a/src/gui/plugins/select_entities/SelectEntities.cc +++ b/src/gui/plugins/select_entities/SelectEntities.cc @@ -353,12 +353,20 @@ void SelectEntitiesPrivate::Initialize() if (nullptr == this->scene) return; - this->camera = std::dynamic_pointer_cast( - this->scene->SensorByName("Scene3DCamera")); - if (!this->camera) + for (unsigned int i = 0; i < this->scene->NodeCount(); ++i) { - ignerr << "Camera is not available" << std::endl; - return; + auto cam = std::dynamic_pointer_cast( + this->scene->NodeByIndex(i)); + if (cam) + { + if (cam->Name().find("scene::Camera") != std::string::npos) + { + this->camera = cam; + igndbg << "InteractiveViewControl plugin is moving camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } } } } diff --git a/src/gui/plugins/select_entities/SelectEntities.hh b/src/gui/plugins/select_entities/SelectEntities.hh index 9a3561ff8e..6dc81661f2 100644 --- a/src/gui/plugins/select_entities/SelectEntities.hh +++ b/src/gui/plugins/select_entities/SelectEntities.hh @@ -30,6 +30,8 @@ namespace plugins { class SelectEntitiesPrivate; + /// \brief This plugin is in charge of selecting and deselecting the entities + /// from the Scene3D and emit the corresponding events. class SelectEntities : public ignition::gazebo::GuiSystem { Q_OBJECT diff --git a/src/gui/plugins/transform_control_logic/TransformControlLogic.cc b/src/gui/plugins/transform_control_logic/TransformControlLogic.cc index 8beea6c507..28c41fc862 100644 --- a/src/gui/plugins/transform_control_logic/TransformControlLogic.cc +++ b/src/gui/plugins/transform_control_logic/TransformControlLogic.cc @@ -51,6 +51,7 @@ class ignition::gazebo::plugins::TransformControlLogicPrivate public: bool OnTransformMode(const msgs::StringMsg &_msg, msgs::Boolean &_res); + /// \brief The method handle the logic of the transform control. public: void HandleTransform(); /// \brief Snaps a point at intervals of a fixed distance. Currently used @@ -83,7 +84,11 @@ class ignition::gazebo::plugins::TransformControlLogicPrivate /// \param[in] _scale The scale snap values public: void SetScaleSnap(const math::Vector3d &_scale); - ///////////////////////////////////////////////// + /// \brief Get the top level node for the given node, which + /// is the ancestor which is a direct child to the root visual. + /// Usually, this will be a model or a light. + /// \param[in] _node Child node + /// \return Top level node containining this node rendering::NodePtr TopLevelNode( const rendering::NodePtr &_node) const; @@ -146,10 +151,10 @@ class ignition::gazebo::plugins::TransformControlLogicPrivate public: std::mutex mutex; //// \brief Pointer to the rendering scene - public: rendering::ScenePtr scene = nullptr; + public: rendering::ScenePtr scene{nullptr}; /// \brief User camera - public: rendering::CameraPtr camera = nullptr; + public: rendering::CameraPtr camera{nullptr}; /// \brief The xyz values by which to snap the object. public: math::Vector3d xyzSnap = math::Vector3d::One; @@ -175,12 +180,20 @@ void TransformControlLogicPrivate::HandleTransform() if (nullptr == this->scene) return; - this->camera = std::dynamic_pointer_cast( - this->scene->SensorByName("Scene3DCamera")); - if (!this->camera) + for (unsigned int i = 0; i < this->scene->NodeCount(); ++i) { - ignerr << "TransformControlLogic Camera is not available" << std::endl; - return; + auto cam = std::dynamic_pointer_cast( + this->scene->NodeByIndex(i)); + if (cam) + { + if (cam->Name().find("scene::Camera") != std::string::npos) + { + this->camera = cam; + igndbg << "InteractiveViewControl plugin is moving camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } } if (!this->transformControl.Camera()) diff --git a/src/gui/plugins/transform_control_logic/TransformControlLogic.hh b/src/gui/plugins/transform_control_logic/TransformControlLogic.hh index 3a795d23cc..56d61f5791 100644 --- a/src/gui/plugins/transform_control_logic/TransformControlLogic.hh +++ b/src/gui/plugins/transform_control_logic/TransformControlLogic.hh @@ -30,6 +30,8 @@ namespace plugins { class TransformControlLogicPrivate; + /// \brief This plugins is in charge of handling the transform control logic. + /// This plugin emits event to block the Interactive View Plugin. class TransformControlLogic : public ignition::gazebo::GuiSystem { Q_OBJECT From e00ee2e421fe8391bbd5626df35d934518c57356 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 1 Jul 2021 17:46:52 +0200 Subject: [PATCH 10/27] make linters happy Signed-off-by: ahcorde --- src/gui/plugins/select_entities/SelectEntities.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/gui/plugins/select_entities/SelectEntities.cc b/src/gui/plugins/select_entities/SelectEntities.cc index 09907d467d..a40fd2d3ff 100644 --- a/src/gui/plugins/select_entities/SelectEntities.cc +++ b/src/gui/plugins/select_entities/SelectEntities.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include #include From fbceade64d8e2530f8cb31054134e716ef1ccca6 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 9 Aug 2021 15:08:37 -0700 Subject: [PATCH 11/27] Don't make it official yet Signed-off-by: Louise Poubel --- examples/worlds/minimal_scene.sdf | 452 ++++++++++++++++++++++++++++++ src/gui/gui.config | 16 +- 2 files changed, 453 insertions(+), 15 deletions(-) create mode 100644 examples/worlds/minimal_scene.sdf diff --git a/examples/worlds/minimal_scene.sdf b/examples/worlds/minimal_scene.sdf new file mode 100644 index 0000000000..6720d2aa85 --- /dev/null +++ b/examples/worlds/minimal_scene.sdf @@ -0,0 +1,452 @@ + + + + + + + + + + + 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 + + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/buoyancy/control + /world/buoyancy/stats + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/buoyancy/stats + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + + + + false + 250 + 50 + 50 + 50 + floating + false + #777777 + + + + + + + false + docked + + + + + + + false + docked + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 -1.5 0.5 0 0 0 + + + + 0.1458 + 0 + 0 + 0.1458 + 0 + 0.125 + + 1.0 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 1.5 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + 0 -3.0 0.5 0 0 0 + + + + 0.074154 + 0 + 0 + 0.074154 + 0 + 0.018769 + + 1.0 + + + + + 0.2 + 0.6 + + + + + + + 0.2 + 0.6 + + + + 1 1 0 1 + 1 1 0 1 + 1 1 0 1 + + + + + + + 0 3.0 0.5 0 0 0 + + + + 0.068 + 0 + 0 + 0.058 + 0 + 0.026 + + 1.0 + + + + + 0.2 0.3 0.5 + + + + + + + 0.2 0.3 0.5 + + + + 1 0 1 1 + 1 0 1 1 + 1 0 1 1 + + + + + + diff --git a/src/gui/gui.config b/src/gui/gui.config index 71c16f48f3..137c55e648 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -27,7 +27,7 @@ - + 3D View false @@ -41,20 +41,6 @@ 6 0 6 0 0.5 3.14 - - - - - - - false - 5 - 5 - floating - false - - - From 21e1b7b9c923742e200e22ea0cfe651c824d80d1 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 9 Aug 2021 15:22:13 -0700 Subject: [PATCH 12/27] alphabetize Signed-off-by: Louise Poubel --- src/gui/plugins/scene_manager/GzSceneManager.cc | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 61c86a21d2..891f9920b4 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -18,19 +18,16 @@ #include "GzSceneManager.hh" #include - +#include +#include #include - #include #include - -#include -#include #include +#include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/World.hh" -#include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/rendering/RenderUtil.hh" namespace ignition From a74a4a6300c914cba7d01348842c465973cdb085 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 9 Aug 2021 18:01:33 -0700 Subject: [PATCH 13/27] clean up select entities Signed-off-by: Louise Poubel --- examples/worlds/minimal_scene.sdf | 2 +- include/ignition/gazebo/gui/GuiEvents.hh | 62 --------- .../plugins/select_entities/SelectEntities.cc | 126 +++++++++++------- .../plugins/select_entities/SelectEntities.hh | 10 +- .../TransformControlLogic.cc | 54 +++----- .../TransformControlLogic.hh | 4 - 6 files changed, 103 insertions(+), 155 deletions(-) diff --git a/examples/worlds/minimal_scene.sdf b/examples/worlds/minimal_scene.sdf index 19a0b55469..e09803aa37 100644 --- a/examples/worlds/minimal_scene.sdf +++ b/examples/worlds/minimal_scene.sdf @@ -11,13 +11,13 @@ Features: * Markers * Tape measure * Grid config +* Select entities Missing for parity with GzScene3D: * Spawn entities through GUI * Context menu * Transform controls -* Select entities * Record video * View angles * View collisions, wireframe, transparent, CoM, etc diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 376af7a27e..eb2950e13c 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -99,68 +99,6 @@ namespace events private: bool fromUser{false}; }; - /// \brief Event called in the render thread of a 3D scene. - /// It's safe to make rendering calls in this event's callback. - class IGN_DEPRECATED(5) Render : public QEvent - { - public: Render() - : QEvent(kType) - { - } - /// \brief Unique type for this event. - static const QEvent::Type kType = QEvent::Type(QEvent::User + 3); - }; - - /// \brief Event called to spawn a preview model. - /// Used by plugins that spawn models. - class IGN_DEPRECATED(5) SpawnPreviewModel : public QEvent - { - /// \brief Constructor - /// \param[in] _modelSdfString The model's SDF file as a string. - public: explicit SpawnPreviewModel(const std::string &_modelSdfString) - : QEvent(kType), modelSdfString(_modelSdfString) - { - } - - /// \brief Unique type for this event. - static const QEvent::Type kType = QEvent::Type(QEvent::User + 4); - - /// \brief Get the sdf string of the model. - /// \return The model sdf string - public: std::string ModelSdfString() const - { - return this->modelSdfString; - } - - /// \brief The sdf string of the model to be previewed. - std::string modelSdfString; - }; - - /// \brief Event called to spawn a preview resource, which takes the path - /// to the SDF file. Used by plugins that spawn resources. - class IGN_DEPRECATED(5) SpawnPreviewPath : public QEvent - { - /// \brief Constructor - /// \param[in] _filePath The path to an SDF file. - public: explicit SpawnPreviewPath(const std::string &_filePath) - : QEvent(kType), filePath(_filePath) - { - } - - /// \brief Unique type for this event. - static const QEvent::Type kType = QEvent::Type(QEvent::User + 5); - - /// \brief Get the path of the SDF file. - /// \return The file path. - public: std::string FilePath() const - { - return this->filePath; - } - - /// \brief The path of SDF file to be previewed. - std::string filePath; - }; - class TransformControlMode : public QEvent { /// \brief Constructor diff --git a/src/gui/plugins/select_entities/SelectEntities.cc b/src/gui/plugins/select_entities/SelectEntities.cc index a40fd2d3ff..32363feee1 100644 --- a/src/gui/plugins/select_entities/SelectEntities.cc +++ b/src/gui/plugins/select_entities/SelectEntities.cc @@ -42,6 +42,8 @@ namespace ignition { namespace gazebo { +namespace gui +{ /// \brief Helper to store selection requests to be handled in the render /// thread by `IgnRenderer::HandleEntitySelection`. struct SelectionHelper @@ -57,25 +59,42 @@ struct SelectionHelper }; } } +} /// \brief Private data class for SelectEntities -class ignition::gazebo::plugins::SelectEntitiesPrivate +class ignition::gazebo::gui::SelectEntitiesPrivate { + /// \brief Initialize the plugin, attaching to a camera. public: void Initialize(); + /// \brief Handle entity selection in the render thread. public: void HandleEntitySelection(); + /// \brief Select new entity + /// \param[in] _visual Visual that was clicked + /// \param[in] _sendEvent True to send an event and notify other widgets public: void UpdateSelectedEntity(const rendering::VisualPtr &_visual, bool _sendEvent); + /// \brief Highlight a selected rendering node + /// \param[in] _visual Node to be highlighted public: void HighlightNode(const rendering::VisualPtr &_visual); + /// \brief Remove highlight from a rendering node that's no longer selected + /// \param[in] _visual Node to be lowlighted + public: void LowlightNode(const rendering::VisualPtr &_visual); + + /// \brief Select the entity for the given visual + /// \param[in] _visual Visual to select public: void SetSelectedEntity(const rendering::VisualPtr &_visual); + /// \brief Deselect all selected entities. public: void DeselectAllEntities(); - public: void LowlightNode(const rendering::VisualPtr &_visual); - + /// \brief Get the ancestor of a given node which is a direct child of the + /// world. + /// \param[in] _node Node to get ancestor of + /// \return Top level node. public: rendering::NodePtr TopLevelNode( const rendering::NodePtr &_node); @@ -84,13 +103,16 @@ class ignition::gazebo::plugins::SelectEntitiesPrivate public: SelectionHelper selectionHelper; /// \brief Currently selected entities, organized by order of selection. + /// These are ign-gazebo IDs public: std::vector selectedEntities; /// \brief Currently selected entities, organized by order of selection. - public: std::vector selectedEntitiesID; + /// These are ign-rendering IDs + public: std::vector selectedEntitiesID; /// \brief New entities received from other plugins. - public: std::vector selectedEntitiesIDNew; + /// These are ign-rendering IDs + public: std::vector selectedEntitiesIDNew; //// \brief Pointer to the rendering scene public: rendering::ScenePtr scene = nullptr; @@ -106,7 +128,7 @@ class ignition::gazebo::plugins::SelectEntitiesPrivate public: bool mouseDirty = false; /// \brief selected entities from other plugins (for example: entityTree) - public: bool recievedSelectedEntities = false; + public: bool receivedSelectedEntities = false; /// \brief User camera public: rendering::CameraPtr camera = nullptr; @@ -117,12 +139,12 @@ class ignition::gazebo::plugins::SelectEntitiesPrivate using namespace ignition; using namespace gazebo; -using namespace plugins; +using namespace gazebo::gui; ///////////////////////////////////////////////// void SelectEntitiesPrivate::HandleEntitySelection() { - if (this->recievedSelectedEntities) + if (this->receivedSelectedEntities) { if (!(QGuiApplication::keyboardModifiers() & Qt::ControlModifier)) { @@ -133,17 +155,27 @@ void SelectEntitiesPrivate::HandleEntitySelection() { auto visualToHighLight = this->scene->VisualById( this->selectedEntitiesIDNew[i]); + + if (nullptr == visualToHighLight) + { + ignerr << "Failed to get visual with ID [" + << this->selectedEntitiesIDNew[i] << "]" << std::endl; + continue; + } + this->selectedEntitiesID.push_back(this->selectedEntitiesIDNew[i]); this->selectedEntities.push_back( std::get(visualToHighLight->UserData("gazebo-entity"))); + this->HighlightNode(visualToHighLight); + ignition::gazebo::gui::events::EntitiesSelected selectEvent( this->selectedEntities); ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &selectEvent); } - this->recievedSelectedEntities = false; + this->receivedSelectedEntities = false; this->selectionHelper = SelectionHelper(); this->selectedEntitiesIDNew.clear(); } @@ -174,8 +206,7 @@ void SelectEntitiesPrivate::HandleEntitySelection() } else if (this->selectionHelper.selectEntity != kNullEntity) { - this->UpdateSelectedEntity(visual, - this->selectionHelper.sendEvent); + this->UpdateSelectedEntity(visual, this->selectionHelper.sendEvent); this->selectionHelper = SelectionHelper(); } @@ -189,8 +220,7 @@ void SelectEntitiesPrivate::LowlightNode(const rendering::VisualPtr &_visual) entityId = std::get(_visual->UserData("gazebo-entity")); if (this->wireBoxes.find(entityId) != this->wireBoxes.end()) { - ignition::rendering::WireBoxPtr wireBox = - this->wireBoxes[entityId]; + ignition::rendering::WireBoxPtr wireBox = this->wireBoxes[entityId]; auto visParent = wireBox->Parent(); if (visParent) visParent->SetVisible(false); @@ -200,10 +230,13 @@ void SelectEntitiesPrivate::LowlightNode(const rendering::VisualPtr &_visual) //////////////////////////////////////////////// void SelectEntitiesPrivate::HighlightNode(const rendering::VisualPtr &_visual) { - Entity entityId = kNullEntity; + if (nullptr == _visual) + { + ignerr << "Failed to highlight null visual." << std::endl; + return; + } - if (_visual) - entityId = std::get(_visual->UserData("gazebo-entity")); + auto entityId = std::get(_visual->UserData("gazebo-entity")); // If the entity is not found in the existing map, create a wire box auto wireBoxIt = this->wireBoxes.find(entityId); @@ -219,14 +252,12 @@ void SelectEntitiesPrivate::HighlightNode(const rendering::VisualPtr &_visual) white->SetEmissive(1.0, 1.0, 1.0); } - ignition::rendering::WireBoxPtr wireBox = - this->scene->CreateWireBox(); + ignition::rendering::WireBoxPtr wireBox = this->scene->CreateWireBox(); ignition::math::AxisAlignedBox aabb = _visual->LocalBoundingBox(); wireBox->SetBox(aabb); // Create visual and add wire box - ignition::rendering::VisualPtr wireBoxVis = - this->scene->CreateVisual(); + ignition::rendering::VisualPtr wireBoxVis = this->scene->CreateVisual(); wireBoxVis->SetInheritScale(false); wireBoxVis->AddGeometry(wireBox); wireBoxVis->SetMaterial(white, false); @@ -270,6 +301,12 @@ rendering::NodePtr SelectEntitiesPrivate::TopLevelNode( void SelectEntitiesPrivate::SetSelectedEntity( const rendering::VisualPtr &_visual) { + if (nullptr == _visual) + { + ignerr << "Failed to select null visual" << std::endl; + return; + } + Entity entityId = kNullEntity; auto topLevelNode = this->TopLevelNode(_visual); @@ -297,22 +334,22 @@ void SelectEntitiesPrivate::SetSelectedEntity( ///////////////////////////////////////////////// void SelectEntitiesPrivate::DeselectAllEntities() { - if (this->scene) - { - for (const auto &entity : this->selectedEntitiesID) - { - auto node = this->scene->VisualById(entity); - auto vis = std::dynamic_pointer_cast(node); - this->LowlightNode(vis); - } - this->selectedEntities.clear(); - this->selectedEntitiesID.clear(); + if (nullptr == this->scene) + return; - ignition::gazebo::gui::events::DeselectAllEntities deselectEvent(true); - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), - &deselectEvent); + for (const auto &entity : this->selectedEntitiesID) + { + auto node = this->scene->VisualById(entity); + auto vis = std::dynamic_pointer_cast(node); + this->LowlightNode(vis); } + this->selectedEntities.clear(); + this->selectedEntitiesID.clear(); + + ignition::gazebo::gui::events::DeselectAllEntities deselectEvent(true); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &deselectEvent); } ///////////////////////////////////////////////// @@ -374,14 +411,12 @@ void SelectEntitiesPrivate::Initialize() ///////////////////////////////////////////////// SelectEntities::SelectEntities() - : GuiSystem(), dataPtr(new SelectEntitiesPrivate) + : dataPtr(std::make_unique()) { } ///////////////////////////////////////////////// -SelectEntities::~SelectEntities() -{ -} +SelectEntities::~SelectEntities() = default; ///////////////////////////////////////////////// void SelectEntities::LoadConfig(const tinyxml2::XMLElement *) @@ -396,21 +431,16 @@ void SelectEntities::LoadConfig(const tinyxml2::XMLElement *) ///////////////////////////////////////////////// bool SelectEntities::eventFilter(QObject *_obj, QEvent *_event) { - // std::cerr << "_obj " << _obj << " this " << this << '\n'; if (_event->type() == ignition::gui::events::LeftClickOnScene::kType) { ignition::gui::events::LeftClickOnScene *_e = static_cast(_event); this->dataPtr->mouseEvent = _e->Mouse(); // handle transform control - if (this->dataPtr->mouseEvent.Button() == - ignition::common::MouseEvent::LEFT) + if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT && + this->dataPtr->mouseEvent.Type() == common::MouseEvent::PRESS) { - if (this->dataPtr->mouseEvent.Type() == - ignition::common::MouseEvent::PRESS) - { - this->dataPtr->mouseDirty = true; - } + this->dataPtr->mouseDirty = true; } } else if (_event->type() == ignition::gui::events::Render::kType) @@ -445,7 +475,7 @@ bool SelectEntities::eventFilter(QObject *_obj, QEvent *_event) if (entityId == entity) { this->dataPtr->selectedEntitiesIDNew.push_back(visual->Id()); - this->dataPtr->recievedSelectedEntities = true; + this->dataPtr->receivedSelectedEntities = true; break; } } @@ -464,5 +494,5 @@ bool SelectEntities::eventFilter(QObject *_obj, QEvent *_event) } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::plugins::SelectEntities, +IGNITION_ADD_PLUGIN(ignition::gazebo::gui::SelectEntities, ignition::gui::Plugin) diff --git a/src/gui/plugins/select_entities/SelectEntities.hh b/src/gui/plugins/select_entities/SelectEntities.hh index 6dc81661f2..cdbd3a172c 100644 --- a/src/gui/plugins/select_entities/SelectEntities.hh +++ b/src/gui/plugins/select_entities/SelectEntities.hh @@ -15,24 +15,24 @@ * */ -#ifndef IGNITION_GUI_PLUGINS_CAMERACONTROLLERMANAGER_HH_ -#define IGNITION_GUI_PLUGINS_CAMERACONTROLLERMANAGER_HH_ +#ifndef IGNITION_GAZEBO_GUI_SELECTENTITIES_HH_ +#define IGNITION_GAZEBO_GUI_SELECTENTITIES_HH_ #include -#include +#include namespace ignition { namespace gazebo { -namespace plugins +namespace gui { class SelectEntitiesPrivate; /// \brief This plugin is in charge of selecting and deselecting the entities /// from the Scene3D and emit the corresponding events. - class SelectEntities : public ignition::gazebo::GuiSystem + class SelectEntities : public ignition::gui::Plugin { Q_OBJECT diff --git a/src/gui/plugins/transform_control_logic/TransformControlLogic.cc b/src/gui/plugins/transform_control_logic/TransformControlLogic.cc index 28c41fc862..cfa6bba26d 100644 --- a/src/gui/plugins/transform_control_logic/TransformControlLogic.cc +++ b/src/gui/plugins/transform_control_logic/TransformControlLogic.cc @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -96,7 +97,7 @@ class ignition::gazebo::plugins::TransformControlLogicPrivate public: bool transformActive{false}; /// \brief Transform mode service - public: std::string transformModeService; + public: std::string transformModeService{"/gui/transform_mode"}; /// \brief Transport node public: transport::Node node; @@ -104,9 +105,6 @@ class ignition::gazebo::plugins::TransformControlLogicPrivate /// \brief Name of service for setting entity pose public: std::string poseCmdService; - /// \brief Name of the world - public: std::string worldName; - /// \brief Transform mode: none, translation, rotation, or scale public: rendering::TransformMode transformMode = rendering::TransformMode::TM_NONE; @@ -286,18 +284,25 @@ void TransformControlLogicPrivate::HandleTransform() req.set_name(topVisual->Name()); msgs::Set(req.mutable_position(), nodeTmp->WorldPosition()); msgs::Set(req.mutable_orientation(), nodeTmp->WorldRotation()); + + // First time, create the service if (this->poseCmdService.empty()) { - this->poseCmdService = "/world/" + this->worldName - + "/set_pose"; - } - this->poseCmdService = transport::TopicUtils::AsValidTopic( - this->poseCmdService); - if (this->poseCmdService.empty()) - { - ignerr << "Failed to create valid pose command service for world [" - << this->worldName <<"]" << std::endl; - return; + std::string worldName; + auto worldNames = ignition::gui::worldNames(); + if (!worldNames.empty()) + worldName = worldNames[0].toStdString(); + + this->poseCmdService = "/world/" + worldName + "/set_pose"; + + this->poseCmdService = transport::TopicUtils::AsValidTopic( + this->poseCmdService); + if (this->poseCmdService.empty()) + { + ignerr << "Failed to create valid pose command service for world [" + << worldName <<"]" << std::endl; + return; + } } this->node.Request(this->poseCmdService, req, cb); } @@ -664,8 +669,6 @@ void TransformControlLogic::LoadConfig(const tinyxml2::XMLElement *) this->title = "Select entities"; // transform mode - this->dataPtr->transformModeService = - "/gui/transform_mode"; this->dataPtr->node.Advertise(this->dataPtr->transformModeService, &TransformControlLogicPrivate::OnTransformMode, this->dataPtr.get()); ignmsg << "Transform mode service on [" @@ -675,25 +678,6 @@ void TransformControlLogic::LoadConfig(const tinyxml2::XMLElement *) ignition::gui::MainWindow *>()->installEventFilter(this); } -///////////////////////////////////////////////// -void TransformControlLogic::Update(const UpdateInfo &/* _info */, - EntityComponentManager &_ecm) -{ - if (this->dataPtr->worldName.empty()) - { - 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; - }); - } -} - ///////////////////////////////////////////////// bool TransformControlLogic::eventFilter(QObject *_obj, QEvent *_event) { diff --git a/src/gui/plugins/transform_control_logic/TransformControlLogic.hh b/src/gui/plugins/transform_control_logic/TransformControlLogic.hh index 56d61f5791..7e20ad77c6 100644 --- a/src/gui/plugins/transform_control_logic/TransformControlLogic.hh +++ b/src/gui/plugins/transform_control_logic/TransformControlLogic.hh @@ -42,10 +42,6 @@ namespace plugins /// \brief Destructor public: virtual ~TransformControlLogic(); - // Documentation inherited - public: void Update(const UpdateInfo &_info, - EntityComponentManager &_ecm) override; - // Documentation inherited public: virtual void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; From 357f626ed2ee24c1c78cd4831df734f9b9b8acd8 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 10 Aug 2021 09:17:20 -0700 Subject: [PATCH 14/27] Start combining TransformControls Signed-off-by: Louise Poubel --- examples/worlds/minimal_scene.sdf | 18 +- src/gui/plugins/CMakeLists.txt | 1 - .../transform_control/TransformControl.cc | 655 +++++++++++++++++- 3 files changed, 646 insertions(+), 28 deletions(-) diff --git a/examples/worlds/minimal_scene.sdf b/examples/worlds/minimal_scene.sdf index e09803aa37..1eee1a7a82 100644 --- a/examples/worlds/minimal_scene.sdf +++ b/examples/worlds/minimal_scene.sdf @@ -12,12 +12,12 @@ Features: * Tape measure * Grid config * Select entities +* Transform controls Missing for parity with GzScene3D: * Spawn entities through GUI * Context menu -* Transform controls * Record video * View angles * View collisions, wireframe, transparent, CoM, etc @@ -97,19 +97,6 @@ Missing for parity with GzScene3D: false - - - - - - - false - 5 - 5 - floating - false - - @@ -213,6 +200,9 @@ Missing for parity with GzScene3D: false #777777 + + + false diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 3a97a4c81f..2b0489a16c 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -133,7 +133,6 @@ add_subdirectory(select_entities) add_subdirectory(scene_manager) add_subdirectory(shapes) add_subdirectory(transform_control) -add_subdirectory(transform_control_logic) add_subdirectory(video_recorder) add_subdirectory(view_angle) add_subdirectory(visualize_contacts) diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index c273ec43ea..5ad96ea9bc 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -21,11 +21,16 @@ #include #include +#include #include +#include #include +#include +#include #include #include +#include #include #include #include @@ -34,24 +39,62 @@ #include #include #include +#include #include #include #include #include "ignition/gazebo/gui/GuiEvents.hh" +// TODO: snap not working +// TODO: select -> press T: no transform + namespace ignition::gazebo { class TransformControlPrivate { + /// \brief Perform transformations in the render thread. + public: void HandleTransform(); + + /// \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( + ignition::math::Vector3d &_point, math::Vector3d &_snapVals, + double _sensitivity = 0.4) const; + + /// \brief Constraints the passed in axis to the currently selected axes. + /// \param[in] _axis The axis to constrain. + public: void XYZConstraint(math::Vector3d &_axis); + + public: double SnapValue( + double _coord, double _interval, double _sensitivity) const; + + /// \brief Get the top level node for the given node, which + /// is the ancestor which is a direct child to the root visual. + /// Usually, this will be a model or a light. + /// \param[in] _node Child node + /// \return Top level node containining this node + rendering::NodePtr TopLevelNode( + const rendering::NodePtr &_node) const; + /// \brief Ignition communication node. public: transport::Node node; /// \brief Mutex to protect mode +// TODO: check on mutex usage public: std::mutex mutex; /// \brief Transform control service name - public: std::string service; + /// \brief Only used when in legacy mode, where this plugin requested 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}; @@ -67,6 +110,69 @@ namespace ignition::gazebo /// \brief The scale snap values held for snap to grid. public: math::Vector3d scaleSnapVals{1.0, 1.0, 1.0}; + + /// \brief Transform mode: none, translation, rotation, or scale + public: rendering::TransformMode transformMode = + rendering::TransformMode::TM_NONE; + + /// \brief Transform space: local or world + public: rendering::TransformSpace transformSpace = + rendering::TransformSpace::TS_LOCAL; + + /// \brief Transform controller for models + public: rendering::TransformController transformControl; + + /// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene{nullptr}; + + /// \brief User camera + public: rendering::CameraPtr camera{nullptr}; + + /// \brief True if there are new mouse events to process. + public: bool mouseDirty{false}; + + /// \brief Whether the transform gizmo is being dragged. + public: bool transformActive{false}; + + /// \brief Name of service for setting entity pose + public: std::string poseCmdService; + + /// \brief Currently selected entities, organized by order of selection. + public: std::vector selectedEntities; + + public: ignition::common::MouseEvent mouseEvent; + + /// \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 Flag to indicate whether the escape key has been released. + public: bool escapeReleased = false; + + /// \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 keep track of world pose setting used + /// for button translating. + public: bool isStartWorldPosSet = false; + + /// \brief The starting world pose of a clicked visual. + public: ignition::math::Vector3d startWorldPos = math::Vector3d::Zero; + + public: ignition::common::KeyEvent keyEvent; + + 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{true}; }; } @@ -84,13 +190,29 @@ TransformControl::TransformControl() TransformControl::~TransformControl() = default; ///////////////////////////////////////////////// -void TransformControl::LoadConfig(const tinyxml2::XMLElement *) +void TransformControl::LoadConfig(const tinyxml2::XMLElement *_pluginElem) { if (this->title.empty()) this->title = "Transform control"; - // For transform requests - this->dataPtr->service = "/gui/transform_mode"; + if (_pluginElem) + { + if (auto elem = _pluginElem->FirstChildElement("legacy")) + { + elem->QueryBoolText(&this->dataPtr->legacy); + } + } + + if (this->dataPtr->legacy) + { + igndbg << "Legacy mode is enabled; this plugin must be used with " + << "GzScene3D." << std::endl; + } + else + { + igndbg << "Legacy mode is disabled; this plugin must be used with " + << "MinimalScene." << std::endl; + } ignition::gui::App()->findChild ()->installEventFilter(this); @@ -108,12 +230,16 @@ void TransformControl::OnSnapUpdate( this->dataPtr->rpySnapVals = math::Vector3d(_roll, _pitch, _yaw); this->dataPtr->scaleSnapVals = math::Vector3d(_scaleX, _scaleY, _scaleZ); - ignition::gui::events::SnapIntervals event( - this->dataPtr->xyzSnapVals, - this->dataPtr->rpySnapVals, - this->dataPtr->scaleSnapVals); - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), &event); + // Emit event to GzScene3D in legacy mode + if (this->dataPtr->legacy) + { + ignition::gui::events::SnapIntervals event( + this->dataPtr->xyzSnapVals, + this->dataPtr->rpySnapVals, + this->dataPtr->scaleSnapVals); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), &event); + } this->newSnapValues(); } @@ -128,9 +254,36 @@ void TransformControl::OnMode(const QString &_mode) ignerr << "Error setting transform mode" << std::endl; }; - ignition::msgs::StringMsg req; - req.set_data(_mode.toStdString()); - this->dataPtr->node.Request(this->dataPtr->service, req, cb); + auto modeStr = _mode.toStdString(); + + // Legacy behaviour: send request to GzScene3D + if (this->dataPtr->legacy) + { + ignition::msgs::StringMsg req; + req.set_data(modeStr); + this->dataPtr->node.Request(this->dataPtr->service, req, cb); + } + // New behaviour: handle the transform control locally + 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 + ignerr << "Unknown transform mode: [" << modeStr << "]" << std::endl; + + ignition::gazebo::gui::events::TransformControlMode transformControlMode( + this->dataPtr->transformMode); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &transformControlMode); + } } ///////////////////////////////////////////////// @@ -196,6 +349,7 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) this->SnapToGrid(); this->dataPtr->snapToGrid = false; } + this->dataPtr->HandleTransform(); } else if (_event->type() == QEvent::KeyPress) { @@ -217,6 +371,36 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) this->activateSelect(); } } + else if (_event->type() == ignition::gazebo::gui::events::EntitiesSelected::kType) + { + ignition::gazebo::gui::events::EntitiesSelected *_e = + static_cast(_event); + this->dataPtr->selectedEntities = _e->Data(); + } + if (_event->type() == ignition::gui::events::Render::kType) + { + if (this->dataPtr->transformControl.Active()) + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == + ignition::gazebo::gui::events::DeselectAllEntities::kType) + { + this->dataPtr->selectedEntities.clear(); + } + else if (_event->type() == ignition::gui::events::LeftClickOnScene::kType) + { + ignition::gui::events::LeftClickOnScene *_e = + static_cast(_event); + this->dataPtr->mouseEvent = _e->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == ignition::gui::events::KeyPressOnScene::kType) + { + ignition::gui::events::KeyPressOnScene *_e = + static_cast(_event); + this->dataPtr->keyEvent = _e->Key(); +ignerr << "EVENT " << this->dataPtr->keyEvent.Control() << std::endl; + } return QObject::eventFilter(_obj, _event); } @@ -275,6 +459,451 @@ double TransformControl::scaleZSnap() return this->dataPtr->scaleSnapVals[2]; } +///////////////////////////////////////////////// +void TransformControlPrivate::HandleTransform() +{ + if (nullptr == this->scene) + { + this->scene = rendering::sceneFromFirstRenderEngine(); + if (nullptr == this->scene) + return; + + for (unsigned int i = 0; i < this->scene->NodeCount(); ++i) + { + auto cam = std::dynamic_pointer_cast( + this->scene->NodeByIndex(i)); + if (cam) + { + if (cam->Name().find("scene::Camera") != std::string::npos) + { + this->camera = cam; + igndbg << "TransformControl plugin is using camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } + } + + if (!this->transformControl.Camera()) + this->transformControl.SetCamera(this->camera); + } + + // Escape action, clear all selections + if (this->escapeReleased) + { + this->selectedEntities.clear(); + this->escapeReleased = false; + } + + // set transform configuration + this->transformControl.SetTransformMode(this->transformMode); + + // stop and detach transform controller if mode is none or no entity is + // selected + if (this->transformMode == rendering::TransformMode::TM_NONE || + (this->transformControl.Node() && + this->selectedEntities.empty())) + { + if (this->transformControl.Active()) + this->transformControl.Stop(); + + this->transformControl.Detach(); + } + else + { + // shift indicates world space transformation + this->transformSpace = (this->keyEvent.Shift()) ? + rendering::TransformSpace::TS_WORLD : + rendering::TransformSpace::TS_LOCAL; + this->transformControl.SetTransformSpace( + this->transformSpace); + } + + // update gizmo visual + this->transformControl.Update(); + + // check for mouse events + if (!this->mouseDirty) + return; + + // handle mouse movements + if (this->mouseEvent.Button() == ignition::common::MouseEvent::LEFT) + { + if (this->mouseEvent.Type() == ignition::common::MouseEvent::PRESS + && this->transformControl.Node()) + { + this->mousePressPos = this->mouseEvent.Pos(); + + // get the visual at mouse position + rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + if (visual) + { + // check if the visual is an axis in the gizmo visual + math::Vector3d axis = + this->transformControl.AxisById(visual->Id()); + if (axis != ignition::math::Vector3d::Zero) + { + this->blockOrbit = true; + // start the transform process + this->transformControl.SetActiveAxis(axis); + this->transformControl.Start(); + this->mouseDirty = false; + } + else + { + this->blockOrbit = false; + return; + } + } + } + else if (this->mouseEvent.Type() == ignition::common::MouseEvent::RELEASE) + { + this->blockOrbit = false; + + this->isStartWorldPosSet = false; + if (this->transformControl.Active()) + { + if (this->transformControl.Node()) + { + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error setting pose" << std::endl; + }; + rendering::NodePtr nodeTmp = this->transformControl.Node(); + auto topVisual = std::dynamic_pointer_cast( + nodeTmp); + ignition::msgs::Pose req; + req.set_name(topVisual->Name()); + msgs::Set(req.mutable_position(), nodeTmp->WorldPosition()); + msgs::Set(req.mutable_orientation(), nodeTmp->WorldRotation()); + + // First time, create the service + if (this->poseCmdService.empty()) + { + std::string worldName; + auto worldNames = ignition::gui::worldNames(); + if (!worldNames.empty()) + worldName = worldNames[0].toStdString(); + + this->poseCmdService = "/world/" + worldName + "/set_pose"; + + this->poseCmdService = transport::TopicUtils::AsValidTopic( + this->poseCmdService); + if (this->poseCmdService.empty()) + { + ignerr << "Failed to create valid pose command service for world [" + << worldName <<"]" << std::endl; + return; + } + } + this->node.Request(this->poseCmdService, req, cb); + } + + this->transformControl.Stop(); + this->mouseDirty = false; + } + // Select entity + else if (!this->mouseEvent.Dragging()) + { + rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + if (!visual) + { + return; + } + + // check if the visual is an axis in the gizmo visual + math::Vector3d axis = this->transformControl.AxisById(visual->Id()); + if (axis == ignition::math::Vector3d::Zero) + { + auto topNode = this->TopLevelNode(visual); + if (!topNode) + { + return; + } + + auto topVis = std::dynamic_pointer_cast(topNode); + // TODO(anyone) Check plane geometry instead of hardcoded name! + if (topVis && topVis->Name() != "ground_plane") + { + // // Highlight entity and notify other widgets + + // Attach control if in a transform mode - control is attached to: + // * latest selection + // * top-level nodes (model, light...) + if (this->transformMode != rendering::TransformMode::TM_NONE) + { + rendering::VisualPtr clickedVisual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + auto topClickedNode = this->TopLevelNode(clickedVisual); + auto topClickedVisual = + std::dynamic_pointer_cast(topClickedNode); + + if (topClickedNode == topClickedVisual) + { + this->transformControl.Attach(topClickedVisual); + } + else + { + this->transformControl.Detach(); + } + } + + this->mouseDirty = false; + return; + } + } + } + } + } + if (this->mouseEvent.Type() == common::MouseEvent::MOVE + && this->transformControl.Active()) + { + this->blockOrbit = true; + // compute the the start and end mouse positions in normalized coordinates + auto imageWidth = static_cast(this->camera->ImageWidth()); + auto imageHeight = static_cast( + this->camera->ImageHeight()); + double nx = 2.0 * this->mousePressPos.X() / imageWidth - 1.0; + double ny = 1.0 - 2.0 * this->mousePressPos.Y() / imageHeight; + double nxEnd = 2.0 * this->mouseEvent.Pos().X() / imageWidth - 1.0; + double nyEnd = 1.0 - 2.0 * this->mouseEvent.Pos().Y() / imageHeight; + math::Vector2d start(nx, ny); + math::Vector2d end(nxEnd, nyEnd); + + // get the current active axis + math::Vector3d axis = this->transformControl.ActiveAxis(); + + // compute 3d transformation from 2d mouse movement + if (this->transformControl.Mode() == + rendering::TransformMode::TM_TRANSLATION) + { + Entity nodeId = this->selectedEntities.front(); + rendering::NodePtr target; + for (unsigned int i = 0; i < this->scene->VisualCount(); i++) + { + auto visual = this->scene->VisualByIndex(i); + auto entityId = static_cast( + std::get(visual->UserData("gazebo-entity"))); + if (entityId == nodeId) + { + target = std::dynamic_pointer_cast( + this->scene->VisualById(visual->Id())); + break; + } + } + if (!target) + { + ignwarn << "Failed to find node with ID [" << nodeId << "]" + << std::endl; + return; + } + this->XYZConstraint(axis); + if (!this->isStartWorldPosSet) + { + this->isStartWorldPosSet = true; + this->startWorldPos = target->WorldPosition(); + } + ignition::math::Vector3d worldPos = target->WorldPosition(); + math::Vector3d distance = + this->transformControl.TranslationFrom2d(axis, start, end); +ignerr << "CONTROL " << this->keyEvent.Control() << std::endl; + if (this->keyEvent.Control()) + { +ignerr << "SNAP" << std::endl; + // Translate to world frame for snapping + distance += this->startWorldPos; + math::Vector3d snapVals = this->xyzSnapVals; + + // 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()); + + this->SnapPoint(distance, snapVals); + + // Translate back to entity frame + distance -= this->startWorldPos; + distance *= axis; + } + this->transformControl.Translate(distance); + } + else if (this->transformControl.Mode() == + rendering::TransformMode::TM_ROTATION) + { + math::Quaterniond rotation = + this->transformControl.RotationFrom2d(axis, start, end); + + if (this->keyEvent.Control()) + { + math::Vector3d currentRot = rotation.Euler(); + math::Vector3d snapVals = this->rpySnapVals; + + if (snapVals.X() <= 1e-4) + { + snapVals.X() = IGN_PI/4; + } + else + { + snapVals.X() = IGN_DTOR(snapVals.X()); + } + if (snapVals.Y() <= 1e-4) + { + snapVals.Y() = IGN_PI/4; + } + else + { + snapVals.Y() = IGN_DTOR(snapVals.Y()); + } + if (snapVals.Z() <= 1e-4) + { + snapVals.Z() = IGN_PI/4; + } + else + { + snapVals.Z() = IGN_DTOR(snapVals.Z()); + } + + this->SnapPoint(currentRot, snapVals); + rotation = math::Quaterniond::EulerToQuaternion(currentRot); + } + this->transformControl.Rotate(rotation); + } + else if (this->transformControl.Mode() == + rendering::TransformMode::TM_SCALE) + { + this->XYZConstraint(axis); + // note: scaling is limited to local space + math::Vector3d scale = + this->transformControl.ScaleFrom2d(axis, start, end); + if (this->keyEvent.Control()) + { + math::Vector3d snapVals = this->scaleSnapVals; + + 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; + + this->SnapPoint(scale, snapVals); + } + this->transformControl.Scale(scale); + } + this->mouseDirty = false; + } + + ignition::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &blockOrbitEvent); +} + +///////////////////////////////////////////////// +rendering::NodePtr TransformControlPrivate::TopLevelNode( + const rendering::NodePtr &_node) const +{ + if (!this->scene) + return rendering::NodePtr(); + + rendering::NodePtr rootNode = this->scene->RootVisual(); + + rendering::NodePtr nodeTmp = _node; + while (nodeTmp && nodeTmp->Parent() != rootNode) + { + nodeTmp = + std::dynamic_pointer_cast(nodeTmp->Parent()); + } + + return nodeTmp; +} + +///////////////////////////////////////////////// +double TransformControlPrivate::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 TransformControlPrivate::XYZConstraint(math::Vector3d &_axis) +{ + math::Vector3d translationAxis = math::Vector3d::Zero; + + if (this->xPressed) + { + translationAxis += math::Vector3d::UnitX; + } + + if (this->yPressed) + { + translationAxis += math::Vector3d::UnitY; + } + + if (this->zPressed) + { + translationAxis += math::Vector3d::UnitZ; + } + + if (translationAxis != math::Vector3d::Zero) + { + _axis = translationAxis; + } +} + +///////////////////////////////////////////////// +void TransformControlPrivate::SnapPoint( + ignition::math::Vector3d &_point, math::Vector3d &_snapVals, + double _sensitivity) const +{ + if (_snapVals.X() <= 0 || _snapVals.Y() <= 0 || _snapVals.Z() <= 0) + { + ignerr << "Interval distance must be greater than 0" + << std::endl; + return; + } + + if (_sensitivity < 0 || _sensitivity > 1.0) + { + ignerr << "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); +} + // Register this plugin IGNITION_ADD_PLUGIN(ignition::gazebo::TransformControl, ignition::gui::Plugin) From 209cb68ce8f9731e17904ed6c223c7ae7f6015ba Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 11 Aug 2021 23:05:39 +0200 Subject: [PATCH 15/27] improvements Signed-off-by: ahcorde --- src/gui/plugins/scene3d/Scene3D.cc | 1 + .../plugins/select_entities/SelectEntities.cc | 63 +++++++++++++++---- .../transform_control/TransformControl.cc | 40 ++++++------ src/rendering/SceneManager.cc | 1 - 4 files changed, 69 insertions(+), 36 deletions(-) diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index f64b711aae..8452e5c7be 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -2033,6 +2033,7 @@ void IgnRenderer::Initialize() // 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()); diff --git a/src/gui/plugins/select_entities/SelectEntities.cc b/src/gui/plugins/select_entities/SelectEntities.cc index 32363feee1..bbb4b4c6a6 100644 --- a/src/gui/plugins/select_entities/SelectEntities.cc +++ b/src/gui/plugins/select_entities/SelectEntities.cc @@ -164,8 +164,14 @@ void SelectEntitiesPrivate::HandleEntitySelection() } this->selectedEntitiesID.push_back(this->selectedEntitiesIDNew[i]); - this->selectedEntities.push_back( - std::get(visualToHighLight->UserData("gazebo-entity"))); + + unsigned int entityId = kNullEntity; + try{ + entityId = std::get(visualToHighLight->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &e){} + + this->selectedEntities.push_back(entityId); this->HighlightNode(visualToHighLight); @@ -195,8 +201,13 @@ void SelectEntitiesPrivate::HandleEntitySelection() return; } - this->selectionHelper.selectEntity = - std::get(visual->UserData("gazebo-entity")); + unsigned int entityId = kNullEntity; + try{ + entityId = std::get(visual->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &e) {} + + this->selectionHelper.selectEntity = entityId; if (this->selectionHelper.deselectAll) { @@ -217,7 +228,12 @@ void SelectEntitiesPrivate::LowlightNode(const rendering::VisualPtr &_visual) { Entity entityId = kNullEntity; if (_visual) - entityId = std::get(_visual->UserData("gazebo-entity")); + { + try{ + entityId = std::get(_visual->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &e){} + } if (this->wireBoxes.find(entityId) != this->wireBoxes.end()) { ignition::rendering::WireBoxPtr wireBox = this->wireBoxes[entityId]; @@ -236,7 +252,11 @@ void SelectEntitiesPrivate::HighlightNode(const rendering::VisualPtr &_visual) return; } - auto entityId = std::get(_visual->UserData("gazebo-entity")); + int entityId = kNullEntity; + try{ + entityId = std::get(_visual->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &e) {} // If the entity is not found in the existing map, create a wire box auto wireBoxIt = this->wireBoxes.find(entityId); @@ -315,7 +335,10 @@ void SelectEntitiesPrivate::SetSelectedEntity( if (topLevelVisual) { - entityId = std::get(topLevelVisual->UserData("gazebo-entity")); + try{ + entityId = std::get(topLevelVisual->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &e) {} } if (entityId == kNullEntity) @@ -391,21 +414,27 @@ void SelectEntitiesPrivate::Initialize() if (nullptr == this->scene) return; - for (unsigned int i = 0; i < this->scene->NodeCount(); ++i) + for (unsigned int i = 0; i < scene->NodeCount(); ++i) { auto cam = std::dynamic_pointer_cast( - this->scene->NodeByIndex(i)); + scene->NodeByIndex(i)); if (cam) { - if (cam->Name().find("scene::Camera") != std::string::npos) + if (std::get(cam->UserData("user-camera"))) { this->camera = cam; - igndbg << "InteractiveViewControl plugin is moving camera [" + igndbg << "TransformControl plugin is using camera [" << this->camera->Name() << "]" << std::endl; break; } } } + + if (!this->camera) + { + ignerr << "TransformControl camera is not available" << std::endl; + return; + } } } @@ -470,8 +499,16 @@ bool SelectEntities::eventFilter(QObject *_obj, QEvent *_event) for (unsigned int i = 0; i < this->dataPtr->scene->VisualCount(); i++) { auto visual = this->dataPtr->scene->VisualByIndex(i); - auto entityId = static_cast( - std::get(visual->UserData("gazebo-entity"))); + + unsigned int entityId = kNullEntity; + try{ + entityId = std::get(visual->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &e) + { + ignerr << "Error eventFilter" << std::endl; + } + if (entityId == entity) { this->dataPtr->selectedEntitiesIDNew.push_back(visual->Id()); diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index 5ad96ea9bc..0179e97094 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -151,9 +151,6 @@ namespace ignition::gazebo /// \brief Flag to indicate whether the z key is currently being pressed public: bool zPressed = false; - /// \brief Flag to indicate whether the escape key has been released. - public: bool escapeReleased = false; - /// \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 @@ -168,6 +165,7 @@ namespace ignition::gazebo public: ignition::common::KeyEvent keyEvent; + /// \brief Block orbit public: bool blockOrbit = false; /// \brief Enable legacy features for plugin to work with GzScene3D. @@ -283,6 +281,7 @@ void TransformControl::OnMode(const QString &_mode) ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &transformControlMode); + this->dataPtr->mouseDirty = true; } } @@ -349,6 +348,8 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) this->SnapToGrid(); this->dataPtr->snapToGrid = false; } + if (this->dataPtr->transformControl.Active()) + this->dataPtr->mouseDirty = true; this->dataPtr->HandleTransform(); } else if (_event->type() == QEvent::KeyPress) @@ -373,19 +374,20 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) } else if (_event->type() == ignition::gazebo::gui::events::EntitiesSelected::kType) { - ignition::gazebo::gui::events::EntitiesSelected *_e = - static_cast(_event); - this->dataPtr->selectedEntities = _e->Data(); - } - if (_event->type() == ignition::gui::events::Render::kType) - { - if (this->dataPtr->transformControl.Active()) - this->dataPtr->mouseDirty = true; + if (!this->dataPtr->blockOrbit) + { + ignition::gazebo::gui::events::EntitiesSelected *_e = + static_cast(_event); + this->dataPtr->selectedEntities = _e->Data(); + } } else if (_event->type() == ignition::gazebo::gui::events::DeselectAllEntities::kType) { - this->dataPtr->selectedEntities.clear(); + if (!this->dataPtr->blockOrbit) + { + this->dataPtr->selectedEntities.clear(); + } } else if (_event->type() == ignition::gui::events::LeftClickOnScene::kType) { @@ -399,7 +401,6 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) ignition::gui::events::KeyPressOnScene *_e = static_cast(_event); this->dataPtr->keyEvent = _e->Key(); -ignerr << "EVENT " << this->dataPtr->keyEvent.Control() << std::endl; } return QObject::eventFilter(_obj, _event); @@ -466,7 +467,9 @@ void TransformControlPrivate::HandleTransform() { this->scene = rendering::sceneFromFirstRenderEngine(); if (nullptr == this->scene) + { return; + } for (unsigned int i = 0; i < this->scene->NodeCount(); ++i) { @@ -474,7 +477,7 @@ void TransformControlPrivate::HandleTransform() this->scene->NodeByIndex(i)); if (cam) { - if (cam->Name().find("scene::Camera") != std::string::npos) + if (std::get(cam->UserData("user-camera"))) { this->camera = cam; igndbg << "TransformControl plugin is using camera [" @@ -488,13 +491,6 @@ void TransformControlPrivate::HandleTransform() this->transformControl.SetCamera(this->camera); } - // Escape action, clear all selections - if (this->escapeReleased) - { - this->selectedEntities.clear(); - this->escapeReleased = false; - } - // set transform configuration this->transformControl.SetTransformMode(this->transformMode); @@ -633,7 +629,7 @@ void TransformControlPrivate::HandleTransform() // TODO(anyone) Check plane geometry instead of hardcoded name! if (topVis && topVis->Name() != "ground_plane") { - // // Highlight entity and notify other widgets + // Highlight entity and notify other widgets // Attach control if in a transform mode - control is attached to: // * latest selection diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index a9c7e356a2..ccef1a239e 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -237,7 +237,6 @@ rendering::VisualPtr SceneManager::CreateLink(Entity _id, linkVis->SetUserData("gazebo-entity", static_cast(_id)); linkVis->SetUserData("pause-update", static_cast(0)); linkVis->SetLocalPose(_link.RawPose()); - linkVis->SetUserData("gazebo-entity", static_cast(_id)); this->dataPtr->visuals[_id] = linkVis; if (parent) From cbf5c00f3162ece926322f251637d1900c6151a2 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 11 Aug 2021 23:15:05 +0200 Subject: [PATCH 16/27] make linters happy Signed-off-by: ahcorde --- .../plugins/transform_control/TransformControl.cc | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index 0179e97094..e09da773c9 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -46,9 +47,6 @@ #include "ignition/gazebo/gui/GuiEvents.hh" -// TODO: snap not working -// TODO: select -> press T: no transform - namespace ignition::gazebo { class TransformControlPrivate @@ -86,7 +84,7 @@ namespace ignition::gazebo public: transport::Node node; /// \brief Mutex to protect mode -// TODO: check on mutex usage + // TODO(anyone): check on mutex usage public: std::mutex mutex; /// \brief Transform control service name @@ -372,7 +370,8 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) this->activateSelect(); } } - else if (_event->type() == ignition::gazebo::gui::events::EntitiesSelected::kType) + else if (_event->type() == + ignition::gazebo::gui::events::EntitiesSelected::kType) { if (!this->dataPtr->blockOrbit) { @@ -592,8 +591,8 @@ void TransformControlPrivate::HandleTransform() this->poseCmdService); if (this->poseCmdService.empty()) { - ignerr << "Failed to create valid pose command service for world [" - << worldName <<"]" << std::endl; + ignerr << "Failed to create valid pose command service " << + << "for world [" << worldName << "]" << std::endl; return; } } From 52829271435f672a092e27542233b39dbb8bd6fe Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 11 Aug 2021 23:15:53 +0200 Subject: [PATCH 17/27] Remove transform control logic plugin Signed-off-by: ahcorde --- .../transform_control_logic/CMakeLists.txt | 11 - .../TransformControlLogic.cc | 735 ------------------ .../TransformControlLogic.hh | 59 -- .../TransformControlLogic.qml | 28 - .../TransformControlLogic.qrc | 5 - 5 files changed, 838 deletions(-) delete mode 100644 src/gui/plugins/transform_control_logic/CMakeLists.txt delete mode 100644 src/gui/plugins/transform_control_logic/TransformControlLogic.cc delete mode 100644 src/gui/plugins/transform_control_logic/TransformControlLogic.hh delete mode 100644 src/gui/plugins/transform_control_logic/TransformControlLogic.qml delete mode 100644 src/gui/plugins/transform_control_logic/TransformControlLogic.qrc diff --git a/src/gui/plugins/transform_control_logic/CMakeLists.txt b/src/gui/plugins/transform_control_logic/CMakeLists.txt deleted file mode 100644 index 51845a57b1..0000000000 --- a/src/gui/plugins/transform_control_logic/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -gz_add_gui_plugin(TransformControlLogic - SOURCES - TransformControlLogic.cc - QT_HEADERS - TransformControlLogic.hh - TEST_SOURCES - # CameraControllerManager_TEST.cc - PUBLIC_LINK_LIBS - ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} - ${PROJECT_LIBRARY_TARGET_NAME}-rendering -) diff --git a/src/gui/plugins/transform_control_logic/TransformControlLogic.cc b/src/gui/plugins/transform_control_logic/TransformControlLogic.cc deleted file mode 100644 index cfa6bba26d..0000000000 --- a/src/gui/plugins/transform_control_logic/TransformControlLogic.cc +++ /dev/null @@ -1,735 +0,0 @@ -/* - * Copyright (C) 2021 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 -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include - -#include -#include - -#include "ignition/gazebo/gui/GuiEvents.hh" -#include "ignition/gazebo/components/Name.hh" -#include "ignition/gazebo/components/World.hh" - -#include "TransformControlLogic.hh" - -/// \brief Private data class for TransformControlLogic -class ignition::gazebo::plugins::TransformControlLogicPrivate -{ - /// \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 - public: bool OnTransformMode(const msgs::StringMsg &_msg, - msgs::Boolean &_res); - - /// \brief The method handle the logic of the transform control. - public: void HandleTransform(); - - /// \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( - ignition::math::Vector3d &_point, math::Vector3d &_snapVals, - double _sensitivity = 0.4) const; - - /// \brief Constraints the passed in axis to the currently selected axes. - /// \param[in] _axis The axis to constrain. - public: void XYZConstraint(math::Vector3d &_axis); - - public: double SnapValue( - double _coord, double _interval, double _sensitivity) const; - - /// \brief Set the XYZ snap values. - /// \param[in] _xyz The XYZ snap values - public: void SetXYZSnap(const math::Vector3d &_xyz); - - /// \brief Set the RPY snap values. - /// \param[in] _rpy The RPY snap values - public: void SetRPYSnap(const math::Vector3d &_rpy); - - /// \brief Set the scale snap values. - /// \param[in] _scale The scale snap values - public: void SetScaleSnap(const math::Vector3d &_scale); - - /// \brief Get the top level node for the given node, which - /// is the ancestor which is a direct child to the root visual. - /// Usually, this will be a model or a light. - /// \param[in] _node Child node - /// \return Top level node containining this node - rendering::NodePtr TopLevelNode( - const rendering::NodePtr &_node) const; - - /// \brief Whether the transform gizmo is being dragged. - public: bool transformActive{false}; - - /// \brief Transform mode service - public: std::string transformModeService{"/gui/transform_mode"}; - - /// \brief Transport node - public: transport::Node node; - - /// \brief Name of service for setting entity pose - public: std::string poseCmdService; - - /// \brief Transform mode: none, translation, rotation, or scale - public: rendering::TransformMode transformMode = - rendering::TransformMode::TM_NONE; - - /// \brief Transform space: local or world - public: rendering::TransformSpace transformSpace = - rendering::TransformSpace::TS_LOCAL; - - /// \brief Currently selected entities, organized by order of selection. - public: std::vector selectedEntities; - - /// \brief Transform controller for models - public: rendering::TransformController transformControl; - - public: ignition::common::MouseEvent mouseEvent; - - public: bool mouseDirty = false; - - /// \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 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 keep track of world pose setting used - /// for button translating. - public: bool isStartWorldPosSet = false; - - /// \brief The starting world pose of a clicked visual. - public: ignition::math::Vector3d startWorldPos = math::Vector3d::Zero; - - public: ignition::common::KeyEvent keyEvent; - - public: std::mutex mutex; - - //// \brief Pointer to the rendering scene - public: rendering::ScenePtr scene{nullptr}; - - /// \brief User camera - public: rendering::CameraPtr camera{nullptr}; - - /// \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; - - public: bool blockOrbit = false; -}; - -using namespace ignition; -using namespace gazebo; -using namespace plugins; - -void TransformControlLogicPrivate::HandleTransform() -{ - if (nullptr == this->scene) - { - this->scene = rendering::sceneFromFirstRenderEngine(); - if (nullptr == this->scene) - return; - - for (unsigned int i = 0; i < this->scene->NodeCount(); ++i) - { - auto cam = std::dynamic_pointer_cast( - this->scene->NodeByIndex(i)); - if (cam) - { - if (cam->Name().find("scene::Camera") != std::string::npos) - { - this->camera = cam; - igndbg << "InteractiveViewControl plugin is moving camera [" - << this->camera->Name() << "]" << std::endl; - break; - } - } - } - - if (!this->transformControl.Camera()) - this->transformControl.SetCamera(this->camera); - } - - // set transform configuration - this->transformControl.SetTransformMode(this->transformMode); - - // stop and detach transform controller if mode is none or no entity is - // selected - if (this->transformMode == rendering::TransformMode::TM_NONE || - (this->transformControl.Node() && - this->selectedEntities.empty())) - { - if (this->transformControl.Active()) - this->transformControl.Stop(); - - this->transformControl.Detach(); - } - else - { - // shift indicates world space transformation - this->transformSpace = (this->keyEvent.Shift()) ? - rendering::TransformSpace::TS_WORLD : - rendering::TransformSpace::TS_LOCAL; - this->transformControl.SetTransformSpace( - this->transformSpace); - } - - // update gizmo visual - this->transformControl.Update(); - - // check for mouse events - if (!this->mouseDirty) - return; - - // handle transform control - if (this->mouseEvent.Button() == ignition::common::MouseEvent::LEFT) - { - if (this->mouseEvent.Type() == ignition::common::MouseEvent::PRESS - && this->transformControl.Node()) - { - this->mousePressPos = this->mouseEvent.Pos(); - - // get the visual at mouse position - rendering::VisualPtr visual = this->scene->VisualAt( - this->camera, - this->mouseEvent.Pos()); - - if (visual) - { - // check if the visual is an axis in the gizmo visual - math::Vector3d axis = - this->transformControl.AxisById(visual->Id()); - if (axis != ignition::math::Vector3d::Zero) - { - this->blockOrbit = true; - // start the transform process - this->transformControl.SetActiveAxis(axis); - this->transformControl.Start(); - this->mouseDirty = false; - } - else - { - this->blockOrbit = false; - return; - } - } - } - else if (this->mouseEvent.Type() == ignition::common::MouseEvent::RELEASE) - { - this->blockOrbit = false; - - this->isStartWorldPosSet = false; - if (this->transformControl.Active()) - { - if (this->transformControl.Node()) - { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) - { - if (!_result) - ignerr << "Error setting pose" << std::endl; - }; - rendering::NodePtr nodeTmp = this->transformControl.Node(); - auto topVisual = std::dynamic_pointer_cast( - nodeTmp); - ignition::msgs::Pose req; - req.set_name(topVisual->Name()); - msgs::Set(req.mutable_position(), nodeTmp->WorldPosition()); - msgs::Set(req.mutable_orientation(), nodeTmp->WorldRotation()); - - // First time, create the service - if (this->poseCmdService.empty()) - { - std::string worldName; - auto worldNames = ignition::gui::worldNames(); - if (!worldNames.empty()) - worldName = worldNames[0].toStdString(); - - this->poseCmdService = "/world/" + worldName + "/set_pose"; - - this->poseCmdService = transport::TopicUtils::AsValidTopic( - this->poseCmdService); - if (this->poseCmdService.empty()) - { - ignerr << "Failed to create valid pose command service for world [" - << worldName <<"]" << std::endl; - return; - } - } - this->node.Request(this->poseCmdService, req, cb); - } - - this->transformControl.Stop(); - this->mouseDirty = false; - } - // Select entity - else if (!this->mouseEvent.Dragging()) - { - rendering::VisualPtr visual = this->scene->VisualAt( - this->camera, - this->mouseEvent.Pos()); - - if (!visual) - { - return; - } - - // check if the visual is an axis in the gizmo visual - math::Vector3d axis = this->transformControl.AxisById(visual->Id()); - if (axis == ignition::math::Vector3d::Zero) - { - auto topNode = this->TopLevelNode(visual); - if (!topNode) - { - return; - } - - auto topVis = std::dynamic_pointer_cast(topNode); - // TODO(anyone) Check plane geometry instead of hardcoded name! - if (topVis && topVis->Name() != "ground_plane") - { - // // Highlight entity and notify other widgets - - // Attach control if in a transform mode - control is attached to: - // * latest selection - // * top-level nodes (model, light...) - if (this->transformMode != rendering::TransformMode::TM_NONE) - { - rendering::VisualPtr clickedVisual = this->scene->VisualAt( - this->camera, - this->mouseEvent.Pos()); - - auto topClickedNode = this->TopLevelNode(clickedVisual); - auto topClickedVisual = - std::dynamic_pointer_cast(topClickedNode); - - if (topClickedNode == topClickedVisual) - { - this->transformControl.Attach(topClickedVisual); - } - else - { - this->transformControl.Detach(); - } - } - - this->mouseDirty = false; - return; - } - } - } - } - } - if (this->mouseEvent.Type() == common::MouseEvent::MOVE - && this->transformControl.Active()) - { - this->blockOrbit = true; - // compute the the start and end mouse positions in normalized coordinates - auto imageWidth = static_cast(this->camera->ImageWidth()); - auto imageHeight = static_cast( - this->camera->ImageHeight()); - double nx = 2.0 * this->mousePressPos.X() / imageWidth - 1.0; - double ny = 1.0 - 2.0 * this->mousePressPos.Y() / imageHeight; - double nxEnd = 2.0 * this->mouseEvent.Pos().X() / imageWidth - 1.0; - double nyEnd = 1.0 - 2.0 * this->mouseEvent.Pos().Y() / imageHeight; - math::Vector2d start(nx, ny); - math::Vector2d end(nxEnd, nyEnd); - - // get the current active axis - math::Vector3d axis = this->transformControl.ActiveAxis(); - - // compute 3d transformation from 2d mouse movement - if (this->transformControl.Mode() == - rendering::TransformMode::TM_TRANSLATION) - { - Entity nodeId = this->selectedEntities.front(); - rendering::NodePtr target; - for (unsigned int i = 0; i < this->scene->VisualCount(); i++) - { - auto visual = this->scene->VisualByIndex(i); - auto entityId = static_cast( - std::get(visual->UserData("gazebo-entity"))); - if (entityId == nodeId) - { - target = std::dynamic_pointer_cast( - this->scene->VisualById(visual->Id())); - break; - } - } - if (!target) - { - ignwarn << "Failed to find node with ID [" << nodeId << "]" - << std::endl; - return; - } - this->XYZConstraint(axis); - if (!this->isStartWorldPosSet) - { - this->isStartWorldPosSet = true; - this->startWorldPos = target->WorldPosition(); - } - ignition::math::Vector3d worldPos = target->WorldPosition(); - math::Vector3d distance = - this->transformControl.TranslationFrom2d(axis, start, end); - if (this->keyEvent.Control()) - { - // Translate to world frame for snapping - distance += this->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()); - - this->SnapPoint(distance, snapVals); - - // Translate back to entity frame - distance -= this->startWorldPos; - distance *= axis; - } - this->transformControl.Translate(distance); - } - else if (this->transformControl.Mode() == - rendering::TransformMode::TM_ROTATION) - { - math::Quaterniond rotation = - this->transformControl.RotationFrom2d(axis, start, end); - - if (this->keyEvent.Control()) - { - math::Vector3d currentRot = rotation.Euler(); - math::Vector3d snapVals = this->rpySnap; - - if (snapVals.X() <= 1e-4) - { - snapVals.X() = IGN_PI/4; - } - else - { - snapVals.X() = IGN_DTOR(snapVals.X()); - } - if (snapVals.Y() <= 1e-4) - { - snapVals.Y() = IGN_PI/4; - } - else - { - snapVals.Y() = IGN_DTOR(snapVals.Y()); - } - if (snapVals.Z() <= 1e-4) - { - snapVals.Z() = IGN_PI/4; - } - else - { - snapVals.Z() = IGN_DTOR(snapVals.Z()); - } - - this->SnapPoint(currentRot, snapVals); - rotation = math::Quaterniond::EulerToQuaternion(currentRot); - } - this->transformControl.Rotate(rotation); - } - else if (this->transformControl.Mode() == - rendering::TransformMode::TM_SCALE) - { - this->XYZConstraint(axis); - // note: scaling is limited to local space - math::Vector3d scale = - this->transformControl.ScaleFrom2d(axis, start, end); - if (this->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; - - this->SnapPoint(scale, snapVals); - } - this->transformControl.Scale(scale); - } - this->mouseDirty = false; - } - - ignition::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), - &blockOrbitEvent); -} - -///////////////////////////////////////////////// -rendering::NodePtr TransformControlLogicPrivate::TopLevelNode( - const rendering::NodePtr &_node) const -{ - if (!this->scene) - return rendering::NodePtr(); - - rendering::NodePtr rootNode = this->scene->RootVisual(); - - rendering::NodePtr nodeTmp = _node; - while (nodeTmp && nodeTmp->Parent() != rootNode) - { - nodeTmp = - std::dynamic_pointer_cast(nodeTmp->Parent()); - } - - return nodeTmp; -} - -///////////////////////////////////////////////// -void TransformControlLogicPrivate::SetXYZSnap(const math::Vector3d &_xyz) -{ - this->xyzSnap = _xyz; -} - -///////////////////////////////////////////////// -void TransformControlLogicPrivate::SetRPYSnap(const math::Vector3d &_rpy) -{ - this->rpySnap = _rpy; -} - -///////////////////////////////////////////////// -void TransformControlLogicPrivate::SetScaleSnap(const math::Vector3d &_scale) -{ - this->scaleSnap = _scale; -} - -///////////////////////////////////////////////// -double TransformControlLogicPrivate::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 TransformControlLogicPrivate::XYZConstraint(math::Vector3d &_axis) -{ - math::Vector3d translationAxis = math::Vector3d::Zero; - - if (this->xPressed) - { - translationAxis += math::Vector3d::UnitX; - } - - if (this->yPressed) - { - translationAxis += math::Vector3d::UnitY; - } - - if (this->zPressed) - { - translationAxis += math::Vector3d::UnitZ; - } - - if (translationAxis != math::Vector3d::Zero) - { - _axis = translationAxis; - } -} - -///////////////////////////////////////////////// -void TransformControlLogicPrivate::SnapPoint( - ignition::math::Vector3d &_point, math::Vector3d &_snapVals, - double _sensitivity) const -{ - if (_snapVals.X() <= 0 || _snapVals.Y() <= 0 || _snapVals.Z() <= 0) - { - ignerr << "Interval distance must be greater than 0" - << std::endl; - return; - } - - if (_sensitivity < 0 || _sensitivity > 1.0) - { - ignerr << "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); -} - -bool TransformControlLogicPrivate::OnTransformMode(const msgs::StringMsg &_msg, - msgs::Boolean &_res) -{ - std::string _mode = _msg.data(); - std::lock_guard lock(this->mutex); - if (_mode == "select") - this->transformMode = rendering::TransformMode::TM_NONE; - else if (_mode == "translate") - this->transformMode = rendering::TransformMode::TM_TRANSLATION; - else if (_mode == "rotate") - this->transformMode = rendering::TransformMode::TM_ROTATION; - else if (_mode == "scale") - this->transformMode = rendering::TransformMode::TM_SCALE; - else - ignerr << "Unknown transform mode: [" << _mode << "]" << std::endl; - - ignition::gazebo::gui::events::TransformControlMode transformControlMode( - this->transformMode); - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), - &transformControlMode); - - this->HandleTransform(); - - _res.set_data(true); - return true; -} - -///////////////////////////////////////////////// -TransformControlLogic::TransformControlLogic() - : GuiSystem(), dataPtr(new TransformControlLogicPrivate) -{ -} - -///////////////////////////////////////////////// -TransformControlLogic::~TransformControlLogic() -{ -} - -///////////////////////////////////////////////// -void TransformControlLogic::LoadConfig(const tinyxml2::XMLElement *) -{ - if (this->title.empty()) - this->title = "Select entities"; - - // transform mode - this->dataPtr->node.Advertise(this->dataPtr->transformModeService, - &TransformControlLogicPrivate::OnTransformMode, this->dataPtr.get()); - ignmsg << "Transform mode service on [" - << this->dataPtr->transformModeService << "]" << std::endl; - - ignition::gui::App()->findChild< - ignition::gui::MainWindow *>()->installEventFilter(this); -} - -///////////////////////////////////////////////// -bool TransformControlLogic::eventFilter(QObject *_obj, QEvent *_event) -{ - if (_event->type() == ignition::gazebo::gui::events::EntitiesSelected::kType) - { - ignition::gazebo::gui::events::EntitiesSelected *_e = - static_cast(_event); - this->dataPtr->selectedEntities = _e->Data(); - this->dataPtr->HandleTransform(); - } - if (_event->type() == ignition::gui::events::Render::kType) - { - if (this->dataPtr->transformControl.Active()) - this->dataPtr->mouseDirty = true; - this->dataPtr->HandleTransform(); - } - else if (_event->type() == - ignition::gazebo::gui::events::DeselectAllEntities::kType) - { - this->dataPtr->selectedEntities.clear(); - } - else if (_event->type() == ignition::gui::events::LeftClickOnScene::kType) - { - ignition::gui::events::LeftClickOnScene *_e = - static_cast(_event); - this->dataPtr->mouseEvent = _e->Mouse(); - this->dataPtr->mouseDirty = true; - this->dataPtr->HandleTransform(); - } - else if (_event->type() == ignition::gui::events::KeyPressOnScene::kType) - { - ignition::gui::events::KeyPressOnScene *_e = - static_cast(_event); - this->dataPtr->keyEvent = _e->Key(); - } - else if (_event->type() == - ignition::gui::events::SnapIntervals::kType) - { - auto snapEvent = reinterpret_cast( - _event); - if (snapEvent) - { - this->dataPtr->SetXYZSnap(snapEvent->Position()); - this->dataPtr->SetRPYSnap(snapEvent->Rotation()); - this->dataPtr->SetScaleSnap(snapEvent->Scale()); - } - } - - // Standard event processing - return QObject::eventFilter(_obj, _event); -} - -// Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::plugins::TransformControlLogic, - ignition::gui::Plugin) diff --git a/src/gui/plugins/transform_control_logic/TransformControlLogic.hh b/src/gui/plugins/transform_control_logic/TransformControlLogic.hh deleted file mode 100644 index 7e20ad77c6..0000000000 --- a/src/gui/plugins/transform_control_logic/TransformControlLogic.hh +++ /dev/null @@ -1,59 +0,0 @@ -/* - * Copyright (C) 2021 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 IGNITION_GUI_PLUGINS_CAMERACONTROLLERMANAGER_HH_ -#define IGNITION_GUI_PLUGINS_CAMERACONTROLLERMANAGER_HH_ - -#include - -#include - -namespace ignition -{ -namespace gazebo -{ -namespace plugins -{ - class TransformControlLogicPrivate; - - /// \brief This plugins is in charge of handling the transform control logic. - /// This plugin emits event to block the Interactive View Plugin. - class TransformControlLogic : public ignition::gazebo::GuiSystem - { - Q_OBJECT - - /// \brief Constructor - public: TransformControlLogic(); - - /// \brief Destructor - public: virtual ~TransformControlLogic(); - - // Documentation inherited - public: virtual void LoadConfig(const tinyxml2::XMLElement *_pluginElem) - override; - - // Documentation inherited - private: bool eventFilter(QObject *_obj, QEvent *_event) override; - - /// \internal - /// \brief Pointer to private data. - private: std::unique_ptr dataPtr; - }; -} -} -} -#endif diff --git a/src/gui/plugins/transform_control_logic/TransformControlLogic.qml b/src/gui/plugins/transform_control_logic/TransformControlLogic.qml deleted file mode 100644 index 873da30014..0000000000 --- a/src/gui/plugins/transform_control_logic/TransformControlLogic.qml +++ /dev/null @@ -1,28 +0,0 @@ -/* - * Copyright (C) 2021 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 QtQuick 2.0 -import QtQuick.Controls 2.0 -import QtQuick.Layouts 1.3 - -// TODO: remove invisible rectangle, see -// https://github.com/ignitionrobotics/ign-gui/issues/220 -Rectangle { - visible: false - Layout.minimumWidth: 100 - Layout.minimumHeight: 100 -} diff --git a/src/gui/plugins/transform_control_logic/TransformControlLogic.qrc b/src/gui/plugins/transform_control_logic/TransformControlLogic.qrc deleted file mode 100644 index 9920375f35..0000000000 --- a/src/gui/plugins/transform_control_logic/TransformControlLogic.qrc +++ /dev/null @@ -1,5 +0,0 @@ - - - TransformControlLogic.qml - - From ee7784cc2efe0013fd33bfc917926845e6932157 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 12 Aug 2021 12:34:45 +0200 Subject: [PATCH 18/27] Fixed compilation error Signed-off-by: ahcorde --- src/gui/plugins/transform_control/TransformControl.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index e09da773c9..c3cd854ae5 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -591,7 +591,7 @@ void TransformControlPrivate::HandleTransform() this->poseCmdService); if (this->poseCmdService.empty()) { - ignerr << "Failed to create valid pose command service " << + ignerr << "Failed to create valid pose command service " << "for world [" << worldName << "]" << std::endl; return; } From 73b9e4e0635c2ecc339e848e715179ed9b31c6ee Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 12 Aug 2021 12:55:52 +0200 Subject: [PATCH 19/27] Remove traces Signed-off-by: ahcorde --- src/gui/plugins/transform_control/TransformControl.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index c3cd854ae5..146282c1ef 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -711,10 +711,8 @@ void TransformControlPrivate::HandleTransform() ignition::math::Vector3d worldPos = target->WorldPosition(); math::Vector3d distance = this->transformControl.TranslationFrom2d(axis, start, end); -ignerr << "CONTROL " << this->keyEvent.Control() << std::endl; if (this->keyEvent.Control()) { -ignerr << "SNAP" << std::endl; // Translate to world frame for snapping distance += this->startWorldPos; math::Vector3d snapVals = this->xyzSnapVals; From caf08a9ab4a08260bde250004d736a78f12fa499 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 12 Aug 2021 18:08:49 -0700 Subject: [PATCH 20/27] Documentation and style Signed-off-by: Louise Poubel --- include/ignition/gazebo/gui/GuiEvents.hh | 4 ++ .../plugins/select_entities/SelectEntities.cc | 44 ++++++++++++++----- .../transform_control/TransformControl.cc | 32 +++++++++----- 3 files changed, 57 insertions(+), 23 deletions(-) diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index eb2950e13c..9695bc9c37 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -99,6 +99,8 @@ namespace events private: bool fromUser{false}; }; + /// \brief True if a transform control is currently active (translate / + /// rotate / scale). False if we're in selection mode. class TransformControlMode : public QEvent { /// \brief Constructor @@ -111,11 +113,13 @@ namespace events /// \brief Unique type for this event. static const QEvent::Type kType = QEvent::Type(QEvent::User + 6); + /// \brief Get the event's value. public: bool TransformControl() { return this->tranformModeActive; } + /// \brief True if a transform mode is active. private: bool tranformModeActive; }; } // namespace events diff --git a/src/gui/plugins/select_entities/SelectEntities.cc b/src/gui/plugins/select_entities/SelectEntities.cc index bbb4b4c6a6..be3421278f 100644 --- a/src/gui/plugins/select_entities/SelectEntities.cc +++ b/src/gui/plugins/select_entities/SelectEntities.cc @@ -166,10 +166,14 @@ void SelectEntitiesPrivate::HandleEntitySelection() this->selectedEntitiesID.push_back(this->selectedEntitiesIDNew[i]); unsigned int entityId = kNullEntity; - try{ + try + { entityId = std::get(visualToHighLight->UserData("gazebo-entity")); } - catch(std::bad_variant_access &e){} + catch(std::bad_variant_access &_e) + { + // It's ok to get here + } this->selectedEntities.push_back(entityId); @@ -202,10 +206,14 @@ void SelectEntitiesPrivate::HandleEntitySelection() } unsigned int entityId = kNullEntity; - try{ + try + { entityId = std::get(visual->UserData("gazebo-entity")); } - catch(std::bad_variant_access &e) {} + catch(std::bad_variant_access &e) + { + // It's ok to get here + } this->selectionHelper.selectEntity = entityId; @@ -229,10 +237,14 @@ void SelectEntitiesPrivate::LowlightNode(const rendering::VisualPtr &_visual) Entity entityId = kNullEntity; if (_visual) { - try{ + try + { entityId = std::get(_visual->UserData("gazebo-entity")); } - catch(std::bad_variant_access &e){} + catch(std::bad_variant_access &) + { + // It's ok to get here + } } if (this->wireBoxes.find(entityId) != this->wireBoxes.end()) { @@ -253,10 +265,14 @@ void SelectEntitiesPrivate::HighlightNode(const rendering::VisualPtr &_visual) } int entityId = kNullEntity; - try{ + try + { entityId = std::get(_visual->UserData("gazebo-entity")); } - catch(std::bad_variant_access &e) {} + catch(std::bad_variant_access &) + { + // It's ok to get here + } // If the entity is not found in the existing map, create a wire box auto wireBoxIt = this->wireBoxes.find(entityId); @@ -335,10 +351,14 @@ void SelectEntitiesPrivate::SetSelectedEntity( if (topLevelVisual) { - try{ + try + { entityId = std::get(topLevelVisual->UserData("gazebo-entity")); } - catch(std::bad_variant_access &e) {} + catch(std::bad_variant_access &) + { + // It's ok to get here + } } if (entityId == kNullEntity) @@ -504,9 +524,9 @@ bool SelectEntities::eventFilter(QObject *_obj, QEvent *_event) try{ entityId = std::get(visual->UserData("gazebo-entity")); } - catch(std::bad_variant_access &e) + catch(std::bad_variant_access &) { - ignerr << "Error eventFilter" << std::endl; + // It's ok to get here } if (entityId == entity) diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index 146282c1ef..5433b8d472 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -66,9 +66,17 @@ namespace ignition::gazebo double _sensitivity = 0.4) const; /// \brief Constraints the passed in axis to the currently selected axes. - /// \param[in] _axis The axis to constrain. + /// \param[in, out] _axis The axis to constrain. public: void XYZConstraint(math::Vector3d &_axis); + /// \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. public: double SnapValue( double _coord, double _interval, double _sensitivity) const; @@ -88,7 +96,7 @@ namespace ignition::gazebo public: std::mutex mutex; /// \brief Transform control service name - /// \brief Only used when in legacy mode, where this plugin requested a + /// 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. @@ -138,8 +146,12 @@ namespace ignition::gazebo /// \brief Currently selected entities, organized by order of selection. public: std::vector selectedEntities; + /// \brief Holds the latest mouse event public: ignition::common::MouseEvent mouseEvent; + /// \brief Holds the latest key event + public: ignition::common::KeyEvent keyEvent; + /// \brief Flag to indicate whether the x key is currently being pressed public: bool xPressed = false; @@ -161,8 +173,6 @@ namespace ignition::gazebo /// \brief The starting world pose of a clicked visual. public: ignition::math::Vector3d startWorldPos = math::Vector3d::Zero; - public: ignition::common::KeyEvent keyEvent; - /// \brief Block orbit public: bool blockOrbit = false; @@ -243,18 +253,18 @@ void TransformControl::OnSnapUpdate( ///////////////////////////////////////////////// void TransformControl::OnMode(const QString &_mode) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) - { - if (!_result) - ignerr << "Error setting transform mode" << std::endl; - }; - auto modeStr = _mode.toStdString(); // Legacy behaviour: send request to GzScene3D if (this->dataPtr->legacy) { + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error setting transform mode" << std::endl; + }; + ignition::msgs::StringMsg req; req.set_data(modeStr); this->dataPtr->node.Request(this->dataPtr->service, req, cb); From e6e3e160f35011c59e568c6b9410032450888e80 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 13 Aug 2021 10:24:48 +0200 Subject: [PATCH 21/27] Fixed keyRelease event Signed-off-by: ahcorde --- .../transform_control/TransformControl.cc | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index 5433b8d472..ee8eb80963 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -372,14 +372,6 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) this->activateRotate(); } } - else if (_event->type() == QEvent::KeyRelease) - { - QKeyEvent *keyEvent = static_cast(_event); - if (keyEvent->key() == Qt::Key_Escape) - { - this->activateSelect(); - } - } else if (_event->type() == ignition::gazebo::gui::events::EntitiesSelected::kType) { @@ -411,6 +403,16 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) static_cast(_event); this->dataPtr->keyEvent = _e->Key(); } + else if (_event->type() == ignition::gui::events::KeyReleaseOnScene::kType) + { + ignition::gui::events::KeyReleaseOnScene *_e = + static_cast(_event); + this->dataPtr->keyEvent = _e->Key(); + if (this->dataPtr->keyEvent.Key() == Qt::Key_Escape) + { + this->activateSelect(); + } + } return QObject::eventFilter(_obj, _event); } From ae18badad9d3cb590798ada9737c985500470922 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 13 Aug 2021 10:54:28 +0200 Subject: [PATCH 22/27] Fixed frozen scene Signed-off-by: ahcorde --- .../plugins/scene_manager/GzSceneManager.cc | 7 ++---- .../transform_control/TransformControl.cc | 22 ++++++++++++++++++- 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index d23b448453..77d29d36ff 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -81,11 +81,8 @@ void GzSceneManager::Update(const UpdateInfo &_info, { IGN_PROFILE("GzSceneManager::Update"); - if (!this->dataPtr->blockUpdate) - { - this->dataPtr->renderUtil.UpdateECM(_info, _ecm); - this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); - } + this->dataPtr->renderUtil.UpdateECM(_info, _ecm); + this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index ee8eb80963..44685cdabd 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -511,6 +511,10 @@ void TransformControlPrivate::HandleTransform() (this->transformControl.Node() && this->selectedEntities.empty())) { + if (this->transformControl.Node()) + this->transformControl.Node()->SetUserData( + "pause-update", static_cast(0)); + if (this->transformControl.Active()) this->transformControl.Stop(); @@ -557,6 +561,9 @@ void TransformControlPrivate::HandleTransform() // start the transform process this->transformControl.SetActiveAxis(axis); this->transformControl.Start(); + if (this->transformControl.Node()) + this->transformControl.Node()->SetUserData( + "pause-update", static_cast(1)); this->mouseDirty = false; } else @@ -576,8 +583,13 @@ void TransformControlPrivate::HandleTransform() if (this->transformControl.Node()) { std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + [this](const ignition::msgs::Boolean &/*_rep*/, const bool _result) { + if (this->transformControl.Node()) + { + this->transformControl.Node()->SetUserData( + "pause-update", static_cast(0)); + } if (!_result) ignerr << "Error setting pose" << std::endl; }; @@ -658,10 +670,14 @@ void TransformControlPrivate::HandleTransform() if (topClickedNode == topClickedVisual) { this->transformControl.Attach(topClickedVisual); + topClickedVisual->SetUserData( + "pause-update", static_cast(1)); } else { this->transformControl.Detach(); + topClickedVisual->SetUserData( + "pause-update", static_cast(0)); } } @@ -675,6 +691,10 @@ void TransformControlPrivate::HandleTransform() if (this->mouseEvent.Type() == common::MouseEvent::MOVE && this->transformControl.Active()) { + if (this->transformControl.Node()) + this->transformControl.Node()->SetUserData( + "pause-update", static_cast(1)); + this->blockOrbit = true; // compute the the start and end mouse positions in normalized coordinates auto imageWidth = static_cast(this->camera->ImageWidth()); From 0a754f4e0edc0b2f94381cbd36562484fe467f2c Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 13 Aug 2021 11:12:47 +0200 Subject: [PATCH 23/27] Fix seg fault Signed-off-by: ahcorde --- .../transform_control/TransformControl.cc | 71 +++++++++++++++---- 1 file changed, 57 insertions(+), 14 deletions(-) diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index 44685cdabd..89ed5f3af7 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -512,8 +512,14 @@ void TransformControlPrivate::HandleTransform() this->selectedEntities.empty())) { if (this->transformControl.Node()) - this->transformControl.Node()->SetUserData( - "pause-update", static_cast(0)); + try { + this->transformControl.Node()->SetUserData( + "pause-update", static_cast(0)); + } + catch (std::bad_variant_access &) + { + // It's ok to get here + } if (this->transformControl.Active()) this->transformControl.Stop(); @@ -562,8 +568,14 @@ void TransformControlPrivate::HandleTransform() this->transformControl.SetActiveAxis(axis); this->transformControl.Start(); if (this->transformControl.Node()) - this->transformControl.Node()->SetUserData( - "pause-update", static_cast(1)); + try { + this->transformControl.Node()->SetUserData( + "pause-update", static_cast(1)); + } + catch (std::bad_variant_access &) + { + // It's ok to get here + } this->mouseDirty = false; } else @@ -587,8 +599,14 @@ void TransformControlPrivate::HandleTransform() { if (this->transformControl.Node()) { - this->transformControl.Node()->SetUserData( - "pause-update", static_cast(0)); + try { + this->transformControl.Node()->SetUserData( + "pause-update", static_cast(0)); + } + catch (std::bad_variant_access &) + { + // It's ok to get here + } } if (!_result) ignerr << "Error setting pose" << std::endl; @@ -670,14 +688,26 @@ void TransformControlPrivate::HandleTransform() if (topClickedNode == topClickedVisual) { this->transformControl.Attach(topClickedVisual); - topClickedVisual->SetUserData( - "pause-update", static_cast(1)); + try { + topClickedVisual->SetUserData( + "pause-update", static_cast(1)); + } + catch (std::bad_variant_access &) + { + // It's ok to get here + } } else { this->transformControl.Detach(); - topClickedVisual->SetUserData( - "pause-update", static_cast(0)); + try { + topClickedVisual->SetUserData( + "pause-update", static_cast(0)); + } + catch (std::bad_variant_access &) + { + // It's ok to get here + } } } @@ -691,9 +721,15 @@ void TransformControlPrivate::HandleTransform() if (this->mouseEvent.Type() == common::MouseEvent::MOVE && this->transformControl.Active()) { - if (this->transformControl.Node()) - this->transformControl.Node()->SetUserData( - "pause-update", static_cast(1)); + if (this->transformControl.Node()){ + try { + this->transformControl.Node()->SetUserData( + "pause-update", static_cast(1)); + } catch (std::bad_variant_access &) + { + // It's ok to get here + } + } this->blockOrbit = true; // compute the the start and end mouse positions in normalized coordinates @@ -719,8 +755,15 @@ void TransformControlPrivate::HandleTransform() for (unsigned int i = 0; i < this->scene->VisualCount(); i++) { auto visual = this->scene->VisualByIndex(i); - auto entityId = static_cast( + auto entityId = kNullEntity; + try { + entityId = static_cast( std::get(visual->UserData("gazebo-entity"))); + } + catch (std::bad_variant_access &) + { + // It's ok to get here + } if (entityId == nodeId) { target = std::dynamic_pointer_cast( From 4cec6b2bf1b26da8246aa9c6520581641c00bd89 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 13 Aug 2021 11:13:23 +0200 Subject: [PATCH 24/27] Renamed TransformControlModeActive to TransformControlModeActive Signed-off-by: ahcorde --- include/ignition/gazebo/gui/GuiEvents.hh | 6 +++--- src/gui/plugins/select_entities/SelectEntities.cc | 6 +++--- src/gui/plugins/transform_control/TransformControl.cc | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 9695bc9c37..c68c61cfa8 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -101,11 +101,11 @@ namespace events /// \brief True if a transform control is currently active (translate / /// rotate / scale). False if we're in selection mode. - class TransformControlMode : public QEvent + class TransformControlModeActive : public QEvent { /// \brief Constructor /// \param[in] _tranformModeActive is the transform control mode active - public: explicit TransformControlMode(const bool _tranformModeActive) + public: explicit TransformControlModeActive(const bool _tranformModeActive) : QEvent(kType), tranformModeActive(_tranformModeActive) { } @@ -114,7 +114,7 @@ namespace events static const QEvent::Type kType = QEvent::Type(QEvent::User + 6); /// \brief Get the event's value. - public: bool TransformControl() + public: bool TransformControlActive() { return this->tranformModeActive; } diff --git a/src/gui/plugins/select_entities/SelectEntities.cc b/src/gui/plugins/select_entities/SelectEntities.cc index be3421278f..1ce1b1f554 100644 --- a/src/gui/plugins/select_entities/SelectEntities.cc +++ b/src/gui/plugins/select_entities/SelectEntities.cc @@ -498,13 +498,13 @@ bool SelectEntities::eventFilter(QObject *_obj, QEvent *_event) this->dataPtr->HandleEntitySelection(); } else if (_event->type() == - ignition::gazebo::gui::events::TransformControlMode::kType) + ignition::gazebo::gui::events::TransformControlModeActive::kType) { auto transformControlMode = - reinterpret_cast( + reinterpret_cast( _event); this->dataPtr->transformControlActive = - transformControlMode->TransformControl(); + transformControlMode->TransformControlActive(); } else if (_event->type() == ignition::gazebo::gui::events::EntitiesSelected::kType) diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index 89ed5f3af7..ec52df6ca0 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -284,11 +284,11 @@ void TransformControl::OnMode(const QString &_mode) else ignerr << "Unknown transform mode: [" << modeStr << "]" << std::endl; - ignition::gazebo::gui::events::TransformControlMode transformControlMode( - this->dataPtr->transformMode); + ignition::gazebo::gui::events::TransformControlModeActive + transformControlModeActive(this->dataPtr->transformMode); ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), - &transformControlMode); + &transformControlModeActive); this->dataPtr->mouseDirty = true; } } From 4279b67de409be06ecede869e8db7460500799ec Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 13 Aug 2021 11:56:22 +0200 Subject: [PATCH 25/27] make linters happy Signed-off-by: ahcorde --- src/gui/plugins/select_entities/SelectEntities.cc | 2 +- src/gui/plugins/transform_control/TransformControl.cc | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/gui/plugins/select_entities/SelectEntities.cc b/src/gui/plugins/select_entities/SelectEntities.cc index 1ce1b1f554..9d218832bf 100644 --- a/src/gui/plugins/select_entities/SelectEntities.cc +++ b/src/gui/plugins/select_entities/SelectEntities.cc @@ -501,7 +501,7 @@ bool SelectEntities::eventFilter(QObject *_obj, QEvent *_event) ignition::gazebo::gui::events::TransformControlModeActive::kType) { auto transformControlMode = - reinterpret_cast( + reinterpret_cast( _event); this->dataPtr->transformControlActive = transformControlMode->TransformControlActive(); diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index ec52df6ca0..ce5d187e1f 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -512,6 +512,7 @@ void TransformControlPrivate::HandleTransform() this->selectedEntities.empty())) { if (this->transformControl.Node()) + { try { this->transformControl.Node()->SetUserData( "pause-update", static_cast(0)); @@ -520,6 +521,7 @@ void TransformControlPrivate::HandleTransform() { // It's ok to get here } + } if (this->transformControl.Active()) this->transformControl.Stop(); @@ -567,7 +569,7 @@ void TransformControlPrivate::HandleTransform() // start the transform process this->transformControl.SetActiveAxis(axis); this->transformControl.Start(); - if (this->transformControl.Node()) + if (this->transformControl.Node()){ try { this->transformControl.Node()->SetUserData( "pause-update", static_cast(1)); @@ -576,6 +578,7 @@ void TransformControlPrivate::HandleTransform() { // It's ok to get here } + } this->mouseDirty = false; } else @@ -595,7 +598,7 @@ void TransformControlPrivate::HandleTransform() if (this->transformControl.Node()) { std::function cb = - [this](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + [this](const ignition::msgs::Boolean &/*_rep*/, const bool _result) { if (this->transformControl.Node()) { From 6ab0c3da4ed582245d17180344d34f166a3d78e7 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 13 Aug 2021 22:10:19 +0200 Subject: [PATCH 26/27] Fixed key event for legacy mode Signed-off-by: ahcorde --- .../transform_control/TransformControl.cc | 45 ++++++++++++++----- 1 file changed, 33 insertions(+), 12 deletions(-) diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index ce5d187e1f..d6ccb485fe 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -360,18 +360,6 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) this->dataPtr->mouseDirty = true; this->dataPtr->HandleTransform(); } - else 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() == ignition::gazebo::gui::events::EntitiesSelected::kType) { @@ -402,6 +390,15 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) ignition::gui::events::KeyPressOnScene *_e = static_cast(_event); this->dataPtr->keyEvent = _e->Key(); + + if (this->dataPtr->keyEvent.Key() == Qt::Key_T) + { + this->activateTranslate(); + } + else if (this->dataPtr->keyEvent.Key() == Qt::Key_R) + { + this->activateRotate(); + } } else if (_event->type() == ignition::gui::events::KeyReleaseOnScene::kType) { @@ -414,6 +411,30 @@ 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); } From eb51a6c95f38ec25a3c4623a9a8f9cf53dfcb694 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 13 Aug 2021 16:07:50 -0700 Subject: [PATCH 27/27] revert unused blockUpdate Signed-off-by: Louise Poubel --- src/gui/plugins/scene_manager/GzSceneManager.cc | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 77d29d36ff..f7ec59fbac 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -46,8 +46,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Rendering utility public: RenderUtil renderUtil; - - public: bool blockUpdate = false; }; } } @@ -92,12 +90,6 @@ bool GzSceneManager::eventFilter(QObject *_obj, QEvent *_event) { this->dataPtr->OnRender(); } - else if (_event->type() == ignition::gui::events::BlockOrbit::kType) - { - auto blockOrbit = reinterpret_cast( - _event); - this->dataPtr->blockUpdate = blockOrbit->Block(); - } // Standard event processing return QObject::eventFilter(_obj, _event);