diff --git a/examples/worlds/minimal_scene.sdf b/examples/worlds/minimal_scene.sdf index 8dc69f2bbf..1eee1a7a82 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 +* Transform controls 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 @@ -97,6 +97,19 @@ Missing for parity with GzScene3D: false + + + + + + + false + 5 + 5 + floating + false + + @@ -187,6 +200,9 @@ Missing for parity with GzScene3D: false #777777 + + + false diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index d206f728e2..c68c61cfa8 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -98,6 +98,30 @@ namespace events /// \brief Whether the event was generated by the user, private: bool fromUser{false}; }; + + /// \brief True if a transform control is currently active (translate / + /// rotate / scale). False if we're in selection mode. + class TransformControlModeActive : public QEvent + { + /// \brief Constructor + /// \param[in] _tranformModeActive is the transform control mode active + public: explicit TransformControlModeActive(const bool _tranformModeActive) + : QEvent(kType), tranformModeActive(_tranformModeActive) + { + } + + /// \brief Unique type for this event. + static const QEvent::Type kType = QEvent::Type(QEvent::User + 6); + + /// \brief Get the event's value. + public: bool TransformControlActive() + { + return this->tranformModeActive; + } + + /// \brief True if a transform mode is active. + private: bool tranformModeActive; + }; } // namespace events } } // namespace gui diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index c8d7ab74f4..2b0489a16c 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -129,6 +129,7 @@ add_subdirectory(plot_3d) add_subdirectory(plotting) add_subdirectory(resource_spawner) add_subdirectory(scene3d) +add_subdirectory(select_entities) add_subdirectory(scene_manager) add_subdirectory(shapes) add_subdirectory(transform_control) diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index d3fd784124..b9faac6b54 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -2037,6 +2037,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/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..9d218832bf --- /dev/null +++ b/src/gui/plugins/select_entities/SelectEntities.cc @@ -0,0 +1,555 @@ +/* + * 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/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 +{ +namespace gui +{ +/// \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::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(); + + /// \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); + + /// \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. + /// These are ign-gazebo IDs + public: std::vector selectedEntities; + + /// \brief Currently selected entities, organized by order of selection. + /// These are ign-rendering IDs + public: std::vector selectedEntitiesID; + + /// \brief New entities received from other plugins. + /// These are ign-rendering IDs + 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; + + /// \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 receivedSelectedEntities = false; + + /// \brief User camera + public: rendering::CameraPtr camera = nullptr; + + /// \brief is transform control active ? + public: bool transformControlActive = false; +}; + +using namespace ignition; +using namespace gazebo; +using namespace gazebo::gui; + +///////////////////////////////////////////////// +void SelectEntitiesPrivate::HandleEntitySelection() +{ + if (this->receivedSelectedEntities) + { + 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]); + + if (nullptr == visualToHighLight) + { + ignerr << "Failed to get visual with ID [" + << this->selectedEntitiesIDNew[i] << "]" << std::endl; + continue; + } + + this->selectedEntitiesID.push_back(this->selectedEntitiesIDNew[i]); + + unsigned int entityId = kNullEntity; + try + { + entityId = std::get(visualToHighLight->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &_e) + { + // It's ok to get here + } + + this->selectedEntities.push_back(entityId); + + this->HighlightNode(visualToHighLight); + + ignition::gazebo::gui::events::EntitiesSelected selectEvent( + this->selectedEntities); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &selectEvent); + } + this->receivedSelectedEntities = false; + this->selectionHelper = SelectionHelper(); + this->selectedEntitiesIDNew.clear(); + } + + if (!mouseDirty) + return; + + this->mouseDirty = false; + + rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + if (!visual) + { + this->DeselectAllEntities(); + return; + } + + unsigned int entityId = kNullEntity; + try + { + entityId = std::get(visual->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &e) + { + // It's ok to get here + } + + this->selectionHelper.selectEntity = entityId; + + 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) + { + try + { + entityId = std::get(_visual->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &) + { + // It's ok to get here + } + } + 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) +{ + if (nullptr == _visual) + { + ignerr << "Failed to highlight null visual." << std::endl; + return; + } + + int entityId = kNullEntity; + try + { + entityId = std::get(_visual->UserData("gazebo-entity")); + } + 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); + 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); + } +} + +///////////////////////////////////////////////// +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) +{ + if (nullptr == _visual) + { + ignerr << "Failed to select null visual" << std::endl; + return; + } + + Entity entityId = kNullEntity; + + auto topLevelNode = this->TopLevelNode(_visual); + auto topLevelVisual = std::dynamic_pointer_cast( + topLevelNode); + + if (topLevelVisual) + { + try + { + entityId = std::get(topLevelVisual->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &) + { + // It's ok to get here + } + } + + if (entityId == kNullEntity) + return; + + this->selectedEntities.push_back(entityId); + this->selectedEntitiesID.push_back(_visual->Id()); + this->HighlightNode(_visual); + ignition::gazebo::gui::events::EntitiesSelected entitiesSelected( + this->selectedEntities); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &entitiesSelected); +} + +///////////////////////////////////////////////// +void SelectEntitiesPrivate::DeselectAllEntities() +{ + if (nullptr == this->scene) + return; + + 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); +} + +///////////////////////////////////////////////// +void SelectEntitiesPrivate::UpdateSelectedEntity( + const rendering::VisualPtr &_visual, bool _sendEvent) +{ + bool deselectedAll{false}; + + // Deselect all if control is not being held + if ((!(QGuiApplication::keyboardModifiers() & Qt::ControlModifier) && + !this->selectedEntitiesID.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; + + for (unsigned int i = 0; i < scene->NodeCount(); ++i) + { + auto cam = std::dynamic_pointer_cast( + scene->NodeByIndex(i)); + if (cam) + { + if (std::get(cam->UserData("user-camera"))) + { + this->camera = cam; + igndbg << "TransformControl plugin is using camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } + } + + if (!this->camera) + { + ignerr << "TransformControl camera is not available" << std::endl; + return; + } + } +} + +///////////////////////////////////////////////// +SelectEntities::SelectEntities() + : dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +SelectEntities::~SelectEntities() = default; + +///////////////////////////////////////////////// +void SelectEntities::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Select entities"; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); +} + +///////////////////////////////////////////////// +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(); + // handle transform control + if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT && + this->dataPtr->mouseEvent.Type() == 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::TransformControlModeActive::kType) + { + auto transformControlMode = + reinterpret_cast( + _event); + this->dataPtr->transformControlActive = + transformControlMode->TransformControlActive(); + } + 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); + + unsigned int entityId = kNullEntity; + try{ + entityId = std::get(visual->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &) + { + // It's ok to get here + } + + if (entityId == entity) + { + this->dataPtr->selectedEntitiesIDNew.push_back(visual->Id()); + this->dataPtr->receivedSelectedEntities = true; + break; + } + } + } + } + } + else if (_event->type() == + ignition::gazebo::gui::events::DeselectAllEntities::kType) + { + this->dataPtr->selectedEntitiesID.clear(); + this->dataPtr->selectedEntities.clear(); + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +// Register this plugin +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 new file mode 100644 index 0000000000..cdbd3a172c --- /dev/null +++ b/src/gui/plugins/select_entities/SelectEntities.hh @@ -0,0 +1,59 @@ +/* + * 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_SELECTENTITIES_HH_ +#define IGNITION_GAZEBO_GUI_SELECTENTITIES_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +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::gui::Plugin + { + Q_OBJECT + + /// \brief Constructor + public: SelectEntities(); + + /// \brief Destructor + public: virtual ~SelectEntities(); + + // 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/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index c273ec43ea..d6ccb485fe 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -20,12 +20,18 @@ #include #include +#include #include +#include #include +#include #include +#include +#include #include #include +#include #include #include #include @@ -34,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -44,14 +51,56 @@ 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, 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; + + /// \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(anyone): check on mutex usage public: std::mutex mutex; /// \brief Transform control service name - public: std::string service; + /// Only used when in legacy mode, where this plugin requests a + /// transport service provided by `GzScene3D`. + /// The new behaviour is that this plugin performs the entire 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 +116,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; + + /// \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; + + /// \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; + + /// \brief Block orbit + public: bool blockOrbit = false; + + /// \brief Enable legacy features for plugin to work with GzScene3D. + /// Disable them to work with the new MinimalScene plugin. + public: bool legacy{true}; }; } @@ -84,13 +196,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 +236,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(); } @@ -121,16 +253,44 @@ void TransformControl::OnSnapUpdate( ///////////////////////////////////////////////// void TransformControl::OnMode(const QString &_mode) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + auto modeStr = _mode.toStdString(); + + // Legacy behaviour: send request to GzScene3D + if (this->dataPtr->legacy) { - if (!_result) - ignerr << "Error setting transform mode" << std::endl; - }; + 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(_mode.toStdString()); - this->dataPtr->node.Request(this->dataPtr->service, req, cb); + 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::TransformControlModeActive + transformControlModeActive(this->dataPtr->transformMode); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &transformControlModeActive); + this->dataPtr->mouseDirty = true; + } } ///////////////////////////////////////////////// @@ -196,28 +356,85 @@ 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() == + ignition::gazebo::gui::events::EntitiesSelected::kType) + { + 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) + { + if (!this->dataPtr->blockOrbit) + { + 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() == QEvent::KeyPress) + else if (_event->type() == ignition::gui::events::KeyPressOnScene::kType) { - QKeyEvent *keyEvent = static_cast(_event); - if (keyEvent->key() == Qt::Key_T) + 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 (keyEvent->key() == Qt::Key_R) + else if (this->dataPtr->keyEvent.Key() == Qt::Key_R) { this->activateRotate(); } } - else if (_event->type() == QEvent::KeyRelease) + else if (_event->type() == ignition::gui::events::KeyReleaseOnScene::kType) { - QKeyEvent *keyEvent = static_cast(_event); - if (keyEvent->key() == Qt::Key_Escape) + ignition::gui::events::KeyReleaseOnScene *_e = + static_cast(_event); + this->dataPtr->keyEvent = _e->Key(); + if (this->dataPtr->keyEvent.Key() == Qt::Key_Escape) { this->activateSelect(); } } + 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); } @@ -275,6 +492,510 @@ 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 (std::get(cam->UserData("user-camera"))) + { + this->camera = cam; + igndbg << "TransformControl plugin is using 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.Node()) + { + 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(); + + 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(); + 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->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 = + [this](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (this->transformControl.Node()) + { + 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; + }; + 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); + try { + topClickedVisual->SetUserData( + "pause-update", static_cast(1)); + } + catch (std::bad_variant_access &) + { + // It's ok to get here + } + } + else + { + this->transformControl.Detach(); + try { + topClickedVisual->SetUserData( + "pause-update", static_cast(0)); + } + catch (std::bad_variant_access &) + { + // It's ok to get here + } + } + } + + this->mouseDirty = false; + return; + } + } + } + } + } + if (this->mouseEvent.Type() == common::MouseEvent::MOVE + && this->transformControl.Active()) + { + 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 + 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 = 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( + 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->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) 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)