From fc9ee3c72e1077a93cd7aeb77d2e9f216900a82c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 6 Oct 2021 17:50:52 -0700 Subject: [PATCH 01/25] add an add entity button to component inspector. Currently only enabled for models Signed-off-by: Ian Chen --- .../ComponentInspector.qml | 105 ++++++++++++++++++ 1 file changed, 105 insertions(+) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 7a416788a6..fab1cb91cc 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -22,6 +22,7 @@ import QtQuick.Layouts 1.3 import QtQuick.Controls.Styles 1.4 import IgnGazebo 1.0 as IgnGazebo + Rectangle { id: componentInspector color: lightGrey @@ -189,6 +190,27 @@ Rectangle { } } + ToolButton { + id: addButton + checkable: false + text: "Add entity" + visible: entityType == "model" + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "qrc:/Gazebo/images/plus.png" + sourceSize.width: 18; + sourceSize.height: 18; + } + ToolTip.text: "Add entity" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + onClicked: { + addLinkMenu.open() + } + } + Label { id: entityLabel text: 'Entity ' + ComponentInspector.entity @@ -200,6 +222,89 @@ Rectangle { } } + ListModel { + id: linkItems + ListElement { + text: "Box" + type: "Link" + } + ListElement { + text: "Cylinder" + type: "Link" + } + ListElement { + text: "Empty" + type: "Link" + } + ListElement { + text: "Mesh" + type: "Link" + } + ListElement { + text: "Sphere" + type: "Link" + } + ListElement { + text: "Ball" + type: "Light" + } + ListElement { + text: "Continuous" + type: "Light" + } + ListElement { + text: "Fixed" + type: "Light" + } + ListElement { + text: "Prismatic" + type: "Light" + } + ListElement { + text: "Revolute" + type: "Light" + } + ListElement { + text: "Universal" + type: "Light" + } + } + // The delegate for each section header + Component { + id: sectionHeading + Rectangle { + height: childrenRect.height + + Text { + text: section + font.pointSize: 10 + padding: 5 + } + } + } + + Menu { + id: addLinkMenu + ListView { + id: addLinkMenuListView + height: addLinkMenu.height + delegate: ItemDelegate { + width: parent.width + text: model.text + highlighted: ListView.isCurrentItem + onClicked: { + addLinkMenu.close() + } + } + model: linkItems + section.property: "type" + section.criteria: ViewSection.FullString + section.delegate: sectionHeading + } + // MenuItem { text: "Box" onTriggered: {} } + } + + ListView { anchors.top: header.bottom anchors.bottom: parent.bottom From 12fa0da41604ec7e734e696a95619b1f2fdb96b2 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 11 Oct 2021 13:20:12 -0700 Subject: [PATCH 02/25] add model editor gui plugin that inserts visuals to the scene in the render thread Signed-off-by: Ian Chen --- include/ignition/gazebo/gui/GuiEvents.hh | 38 +++ src/gui/plugins/CMakeLists.txt | 1 + .../component_inspector/ComponentInspector.cc | 12 + .../component_inspector/ComponentInspector.hh | 5 + .../ComponentInspector.qml | 1 + src/gui/plugins/model_editor/CMakeLists.txt | 7 + src/gui/plugins/model_editor/ModelEditor.cc | 277 ++++++++++++++++++ src/gui/plugins/model_editor/ModelEditor.hh | 65 ++++ src/gui/plugins/model_editor/ModelEditor.qml | 28 ++ src/gui/plugins/model_editor/ModelEditor.qrc | 5 + 10 files changed, 439 insertions(+) create mode 100644 src/gui/plugins/model_editor/CMakeLists.txt create mode 100644 src/gui/plugins/model_editor/ModelEditor.cc create mode 100644 src/gui/plugins/model_editor/ModelEditor.hh create mode 100644 src/gui/plugins/model_editor/ModelEditor.qml create mode 100644 src/gui/plugins/model_editor/ModelEditor.qrc diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index c68c61cfa8..46985d818d 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -122,6 +122,44 @@ namespace events /// \brief True if a transform mode is active. private: bool tranformModeActive; }; + + /// \brief Event that notifies an entity is to be added to the model editor + class ModelEditorAddEntity : public QEvent + { + /// \brief Constructor + /// \param[in] _tranformModeActive is the transform control mode active + public: explicit ModelEditorAddEntity(QString _entity, QString _type, + QString _parent) : QEvent(kType), entity(_entity), type(_type), + parent(_parent) + { + } + + /// \brief Get the entity to add + public: QString Entity() const + { + return this->entity; + } + + /// \brief Get the entity type + public: QString EntityType() const + { + return this->type; + } + + /// \brief Get the parent entity to add the entity to + public: QString ParentEntity() const + { + return this->parent; + } + + /// \brief Unique type for this event. + static const QEvent::Type kType = QEvent::Type(QEvent::User + 7); + + private: QString entity; + private: QString type; + private: QString parent; + }; + } // namespace events } } // namespace gui diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index d74c63cca3..bbcafc434d 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -121,6 +121,7 @@ add_subdirectory(modules) add_subdirectory(align_tool) add_subdirectory(banana_for_scale) add_subdirectory(component_inspector) +add_subdirectory(model_editor) add_subdirectory(entity_context_menu) add_subdirectory(entity_tree) add_subdirectory(joint_position_controller) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 158dde677d..2e97db2ea5 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -1026,6 +1026,18 @@ bool ComponentInspector::NestedModel() const return this->dataPtr->nestedModel; } +///////////////////////////////////////////////// +void ComponentInspector::OnAddEntity(QString _entity, QString _type) +{ + // currently just assumes parent is the model + // todo(anyone) support adding visuals / collisions / sensors to links + ignition::gazebo::gui::events::ModelEditorAddEntity addEntityEvent( + _entity, _type, QString(this->dataPtr->entityName.c_str())); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &addEntityEvent); +} + // Register this plugin IGNITION_ADD_PLUGIN(ignition::gazebo::ComponentInspector, ignition::gui::Plugin) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 18b0ef7d66..8a1dea92cc 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -331,6 +331,11 @@ namespace gazebo /// \brief Notify that paused has changed. signals: void PausedChanged(); + /// \brief Callback in Qt thread when an entity is to be added + /// \param[in] _entity Entity to add, e.g. box, sphere, cylinder, etc + /// \param[in] _type Entity type, e.g. link, visual, collision, etc + public: Q_INVOKABLE void OnAddEntity(QString _entity, QString _type); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index fab1cb91cc..6a30ab2028 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -293,6 +293,7 @@ Rectangle { text: model.text highlighted: ListView.isCurrentItem onClicked: { + ComponentInspector.OnAddEntity(model.text, "link"); addLinkMenu.close() } } diff --git a/src/gui/plugins/model_editor/CMakeLists.txt b/src/gui/plugins/model_editor/CMakeLists.txt new file mode 100644 index 0000000000..39f351015e --- /dev/null +++ b/src/gui/plugins/model_editor/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_gui_plugin(ModelEditor + SOURCES ModelEditor.cc + QT_HEADERS ModelEditor.hh + PUBLIC_LINK_LIBS + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + ${PROJECT_LIBRARY_TARGET_NAME}-rendering +) diff --git a/src/gui/plugins/model_editor/ModelEditor.cc b/src/gui/plugins/model_editor/ModelEditor.cc new file mode 100644 index 0000000000..274e39c118 --- /dev/null +++ b/src/gui/plugins/model_editor/ModelEditor.cc @@ -0,0 +1,277 @@ +/* + * 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 "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" + +#include "ModelEditor.hh" + +namespace ignition::gazebo +{ + class ModelEditorPrivate + { + public: void Initialize(); + + public: void HandleAddEntity(const std::string &_entity, + const std::string &_type, const std::string &_parent); + + /// \brief Generate a unique entity id. + /// \return The unique entity id + // public: Entity UniqueId() const; + + //// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene = nullptr; + + /// \brief Entity to add to the model editor + public: std::string entityToAdd; + + /// \brief Type of entity to add + public: std::string entityType; + + /// \brief Parent entity to add the entity to + public: std::string parentEntity; + + /// \brief True if there is an entity to be added to the editor + public: bool addEntityDirty = false; + + /// \brief Scene manager +// public: SceneManager sceneManager; + + /// \brief A record of the ids in the editor + /// for easy deletion of visuals later + public: std::vector entityIds; + }; +} + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +ModelEditor::ModelEditor() + : GuiSystem(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +ModelEditor::~ModelEditor() = default; + +///////////////////////////////////////////////// +void ModelEditor::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Model editor"; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); +} + +////////////////////////////////////////////////// +void ModelEditor::Update(const UpdateInfo &, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("ModelEditor::Update"); +} + +///////////////////////////////////////////////// +bool ModelEditor::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == gazebo::gui::events::ModelEditorAddEntity::kType) + { + auto event = reinterpret_cast(_event); + if (event) + { + this->dataPtr->entityToAdd = event->Entity().toStdString(); + this->dataPtr->entityType = event->EntityType().toStdString(); + this->dataPtr->parentEntity = event->ParentEntity().toStdString(); + this->dataPtr->addEntityDirty = true; + + std::cerr << "model editor add entity event " << event->EntityType().toStdString() << " " << this->dataPtr->entityToAdd.toStdString() << std::endl; + } + } + else if (_event->type() == ignition::gui::events::Render::kType) + { + this->dataPtr->Initialize(); + if (this->dataPtr->addEntityDirty) + { + this->dataPtr->HandleAddEntity(this->dataPtr->entityToAdd, + this->dataPtr->entityType, + this->dataPtr->parentEntity); + this->dataPtr->addEntityDirty = false; + } + } + + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void ModelEditorPrivate::Initialize() +{ + if (nullptr == this->scene) + { + this->scene = rendering::sceneFromFirstRenderEngine(); + if (nullptr == this->scene) + { + return; + } +// this->sceneManager.SetScene(this->scene); + } +} + +///////////////////////////////////////////////// +//Entity ModelEditorPrivate::UniqueId() +//{ +// auto timeout = 100000u; +// for (auto i = 0u; i < timeout; ++i) +// { +// Entity id = std::numeric_limits::max() - i; +// if (!this->sceneManager.HasEntity(id)) +// return id; +// } +// return kNullEntity; +//} + +///////////////////////////////////////////////// +void ModelEditorPrivate::HandleAddEntity(const std::string &_entityToAdd, + const std::string &_type, const std::string &_parentEntity) +{ + std::string type = common::lowercase(_type); + std::string entity = common::lowercase(_entityToAdd); + if (type == "link") + { + auto model = this->scene->VisualByName(parentEntity); + if (!model) + { + ignerr << "Unable to add link to model. " + << "Parent entity: '" << parentEntity << "' not found. " + << std::endl; + } + + auto link = this->scene->CreateVisual(); + auto visual = this->scene->CreateVisual(); + + rendering::GeometryPtr geom; + if (entity == "box") + geom = this->scene->CreateBox(); + else if (entity == "cylinder") + geom = this->scene->CreateCylinder(); + else if (entity == "sphere") + geom = this->scene->CreateSphere(); + + visual->AddGeometry(geom); + link->AddChild(visual); + model->AddChild(link); + } +/* + std::string entitySdfString = std::string( + "" + "" + "" + "0 0 0.5 0 0 0" + "" + "" + ""); + if (_entity == "box") + { + entitySdfString += "" + "1 1 1" + "" + } + else if (_entity == "cylinder") + { + entitySdfString += "" + "0.5" + "1.0" + "" + } + else if (_entity == "sphere") + { + entitySdfString += "" + "0.5" + "" + } + entitySdfString += + "" + "" + "" + "" + ""; + + + sdf::Root root; + root.LoadSdfString(entitySdfString); + + // create model + Entity modelId = this->UniqueId(); + if (!modelId) + { + ignerr << "unable to generate unique Id" << std::endl; + return; + } + + sdf::Model model = *(root.Model()); +// model.SetName(common::Uuid().String()); +// auto model = this->sceneManager.CreateModel( +// modelId, model, this->sceneManager.WorldId()); +// this->entityIds.push_back(modelId); + + // create link + sdf::Link link = *(model.LinkByIndex(0)); + link.SetName(common::Uuid().String()); + Entity linkId = this->UniqueId(); + if (!linkId) + { + ignerr << "unable to generate unique Id" << std::endl; + return; + } + this->sceneManager.CreateLink(linkId, link, modelId); + this->entityIds.push_back(linkId); + + // create visual + sdf::Visual visual = *(link.VisualByIndex(0)); + visual.SetName(common::Uuid().String()); + Entity visualId = this->UniqueId(); + if (!visualId) + { + ignerr << "unable to generate unique Id" << std::endl; + return; + } + + this->sceneManager.CreateVisual(visualId, visual, linkId); + this->entityIds.push_back(visualId); +*/ + +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::ModelEditor, + ignition::gui::Plugin) diff --git a/src/gui/plugins/model_editor/ModelEditor.hh b/src/gui/plugins/model_editor/ModelEditor.hh new file mode 100644 index 0000000000..94fcde7245 --- /dev/null +++ b/src/gui/plugins/model_editor/ModelEditor.hh @@ -0,0 +1,65 @@ +/* + * 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_MODELEDITOR_HH_ +#define IGNITION_GAZEBO_GUI_MODELEDITOR_HH_ + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ + class ModelEditorPrivate; + + /// \brief + /// + /// ## Configuration + /// None + class ModelEditor : public gazebo::GuiSystem + { + Q_OBJECT + + public: ModelEditor(); + public: ~ModelEditor() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + public: void Update(const UpdateInfo &, EntityComponentManager &) override; + + // Documentation inherited + protected: 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/model_editor/ModelEditor.qml b/src/gui/plugins/model_editor/ModelEditor.qml new file mode 100644 index 0000000000..c5a85ffd29 --- /dev/null +++ b/src/gui/plugins/model_editor/ModelEditor.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.9 +import QtQuick.Controls 1.4 +import QtQuick.Controls 2.2 +import QtQuick.Controls.Material 2.1 +import QtQuick.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 +import IgnGazebo 1.0 as IgnGazebo + + +Rectangle { + id: modelEditor +} diff --git a/src/gui/plugins/model_editor/ModelEditor.qrc b/src/gui/plugins/model_editor/ModelEditor.qrc new file mode 100644 index 0000000000..bc76232cfa --- /dev/null +++ b/src/gui/plugins/model_editor/ModelEditor.qrc @@ -0,0 +1,5 @@ + + + ModelEditor.qml + + From 5364f5136d023a7699a6528bf27cf0819bdfa5f4 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 11 Oct 2021 20:28:42 -0700 Subject: [PATCH 03/25] write to ECM Signed-off-by: Ian Chen --- src/gui/plugins/model_editor/ModelEditor.cc | 292 ++++++++++++-------- 1 file changed, 177 insertions(+), 115 deletions(-) diff --git a/src/gui/plugins/model_editor/ModelEditor.cc b/src/gui/plugins/model_editor/ModelEditor.cc index 274e39c118..262acb5b0c 100644 --- a/src/gui/plugins/model_editor/ModelEditor.cc +++ b/src/gui/plugins/model_editor/ModelEditor.cc @@ -28,8 +28,17 @@ #include #include +#include +#include + +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/SdfEntityCreator.hh" + #include "ignition/gazebo/gui/GuiEvents.hh" #include "ModelEditor.hh" @@ -40,8 +49,16 @@ namespace ignition::gazebo { public: void Initialize(); - public: void HandleAddEntity(const std::string &_entity, - const std::string &_type, const std::string &_parent); + public: void HandleAddEntity(const std::string &_geomOrLightType, + const std::string &_entityType, const std::string &/*_parent*/); + + public: std::string GeomSDFString( + const std::string &_geomType, + const math::Vector3d &_size = math::Vector3d::One) const; + + public: std::string LinkSDFString( + const std::string &_geomType, + const math::Vector3d &_size = math::Vector3d::One) const; /// \brief Generate a unique entity id. /// \return The unique entity id @@ -51,7 +68,7 @@ namespace ignition::gazebo public: rendering::ScenePtr scene = nullptr; /// \brief Entity to add to the model editor - public: std::string entityToAdd; + public: std::string geomOrLightType; /// \brief Type of entity to add public: std::string entityType; @@ -59,15 +76,21 @@ namespace ignition::gazebo /// \brief Parent entity to add the entity to public: std::string parentEntity; - /// \brief True if there is an entity to be added to the editor - public: bool addEntityDirty = false; - - /// \brief Scene manager -// public: SceneManager sceneManager; + /// \brief Entity Creator API. + public: std::unique_ptr entityCreator{nullptr}; /// \brief A record of the ids in the editor /// for easy deletion of visuals later public: std::vector entityIds; + + /// \brief Mutex to protect the entity sdf list + public: std::mutex mutex; + + /// \brief Links to add to the ECM + public: std::vector linksToAdd; + + /// \brief Event Manager + public: EventManager eventMgr; }; } @@ -98,6 +121,53 @@ void ModelEditor::Update(const UpdateInfo &, EntityComponentManager &_ecm) { IGN_PROFILE("ModelEditor::Update"); + + if (!this->dataPtr->entityCreator) + { + // create entities in ECM on the GUI side. + // Note we have to start the entity id at an offset so it does not conflict + // with the ones on the server. The log playback starts at max / 2 + // On the gui side, we will start entity id at an offset of max / 4 + // todo(anyone) set a better entity create offset + _ecm.SetEntityCreateOffset(math::MAX_I64 / 4); + this->dataPtr->entityCreator = std::make_unique( + _ecm, this->dataPtr->eventMgr); + } + + + std::lock_guard lock(this->dataPtr->mutex); + // add link entities to the ECM + for (auto & linkSdf : this->dataPtr->linksToAdd) + { + Entity parent = _ecm.EntityByComponents( + components::Model(), components::Name(this->dataPtr->parentEntity)); + if (parent == kNullEntity) + { + ignerr << "Unable to find " << this->dataPtr->parentEntity + << " in the ECM. " << std::endl; + continue; + } + + // generate link name + std::string linkName = "link"; + Entity linkEnt = _ecm.EntityByComponents( + components::Link(), components::ParentEntity(parent), + components::Name(linkName)); + int64_t counter = 0; + while (linkEnt) + { + linkName = std::string("link") + "_" + std::to_string(++counter); + _ecm.EntityByComponents( + components::Link(), components::ParentEntity(parent), + components::Name(linkName)); + } + + std::cerr << "creating entity in ecm " << linkName << std::endl; + linkSdf.SetName(linkName); + auto entity = this->dataPtr->entityCreator->CreateEntities(&linkSdf); + this->dataPtr->entityCreator->SetParent(entity, parent); + } + this->dataPtr->linksToAdd.clear(); } ///////////////////////////////////////////////// @@ -108,24 +178,26 @@ bool ModelEditor::eventFilter(QObject *_obj, QEvent *_event) auto event = reinterpret_cast(_event); if (event) { - this->dataPtr->entityToAdd = event->Entity().toStdString(); + this->dataPtr->geomOrLightType = event->Entity().toStdString(); this->dataPtr->entityType = event->EntityType().toStdString(); this->dataPtr->parentEntity = event->ParentEntity().toStdString(); - this->dataPtr->addEntityDirty = true; - std::cerr << "model editor add entity event " << event->EntityType().toStdString() << " " << this->dataPtr->entityToAdd.toStdString() << std::endl; + std::cerr << "model editor add entity event " << event->EntityType().toStdString() + << " " << this->dataPtr->geomOrLightType << std::endl; + + this->dataPtr->HandleAddEntity(this->dataPtr->geomOrLightType, + this->dataPtr->entityType, + this->dataPtr->parentEntity); + } } else if (_event->type() == ignition::gui::events::Render::kType) { + // initialize rendering this->dataPtr->Initialize(); - if (this->dataPtr->addEntityDirty) - { - this->dataPtr->HandleAddEntity(this->dataPtr->entityToAdd, - this->dataPtr->entityType, - this->dataPtr->parentEntity); - this->dataPtr->addEntityDirty = false; - } + + // do something in rendering thread + } @@ -160,116 +232,106 @@ void ModelEditorPrivate::Initialize() // return kNullEntity; //} + ///////////////////////////////////////////////// -void ModelEditorPrivate::HandleAddEntity(const std::string &_entityToAdd, - const std::string &_type, const std::string &_parentEntity) +std::string ModelEditorPrivate::GeomSDFString( + const std::string &_geomType, + const math::Vector3d &_size) const { - std::string type = common::lowercase(_type); - std::string entity = common::lowercase(_entityToAdd); - if (type == "link") - { - auto model = this->scene->VisualByName(parentEntity); - if (!model) - { - ignerr << "Unable to add link to model. " - << "Parent entity: '" << parentEntity << "' not found. " - << std::endl; - } - - auto link = this->scene->CreateVisual(); - auto visual = this->scene->CreateVisual(); - - rendering::GeometryPtr geom; - if (entity == "box") - geom = this->scene->CreateBox(); - else if (entity == "cylinder") - geom = this->scene->CreateCylinder(); - else if (entity == "sphere") - geom = this->scene->CreateSphere(); - - visual->AddGeometry(geom); - link->AddChild(visual); - model->AddChild(link); - } -/* - std::string entitySdfString = std::string( - "" - "" - "" - "0 0 0.5 0 0 0" - "" - "" - ""); - if (_entity == "box") + std::stringstream geomStr; + geomStr << ""; + if (_geomType == "box") { - entitySdfString += "" - "1 1 1" - "" + geomStr + << "" + << " " << _size << "" + << ""; } - else if (_entity == "cylinder") + else if (_geomType == "sphere") { - entitySdfString += "" - "0.5" - "1.0" - "" + geomStr + << "" + << " " << _size.X() * 0.5 << "" + << ""; } - else if (_entity == "sphere") + else if (_geomType == "cylinder") { - entitySdfString += "" - "0.5" - "" + geomStr + << "" + << " " << _size.X() * 0.5 << "" + << " " << _size.Z() << "" + << ""; } - entitySdfString += - "" - "" - "" - "" - ""; - + geomStr << ""; + return geomStr.str(); +} - sdf::Root root; - root.LoadSdfString(entitySdfString); +///////////////////////////////////////////////// +std::string ModelEditorPrivate::LinkSDFString( + const std::string &_geomType, + const math::Vector3d &_size) const +{ + std::string geomStr = this->GeomSDFString(_geomType, _size); + std::stringstream linkStr; + linkStr + << "" + << " " + << " " + << geomStr + << " " + << " " + << geomStr + << " " + << " " + << ""; + + return linkStr.str(); +} - // create model - Entity modelId = this->UniqueId(); - if (!modelId) - { - ignerr << "unable to generate unique Id" << std::endl; - return; - } - sdf::Model model = *(root.Model()); -// model.SetName(common::Uuid().String()); -// auto model = this->sceneManager.CreateModel( -// modelId, model, this->sceneManager.WorldId()); -// this->entityIds.push_back(modelId); - - // create link - sdf::Link link = *(model.LinkByIndex(0)); - link.SetName(common::Uuid().String()); - Entity linkId = this->UniqueId(); - if (!linkId) - { - ignerr << "unable to generate unique Id" << std::endl; - return; - } - this->sceneManager.CreateLink(linkId, link, modelId); - this->entityIds.push_back(linkId); - - // create visual - sdf::Visual visual = *(link.VisualByIndex(0)); - visual.SetName(common::Uuid().String()); - Entity visualId = this->UniqueId(); - if (!visualId) +///////////////////////////////////////////////// +void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, + const std::string &_type, const std::string &/*_parentEntity*/) +{ + std::string entType = common::lowercase(_type); + std::string geomLightType = common::lowercase(_geomOrLightType); + if (entType == "link") { - ignerr << "unable to generate unique Id" << std::endl; - return; + // auto model = this->scene->VisualByName(parentEntity); + // if (!model) + // { + // ignerr << "Unable to add link to model. " + // << "Parent entity: '" << parentEntity << "' not found. " + // << std::endl; + // } + + // auto link = this->scene->CreateVisual(); + // auto visual = this->scene->CreateVisual(); + + // rendering::GeometryPtr geom; + // if (entity == "box") + // geom = this->scene->CreateBox(); + // else if (entity == "cylinder") + // geom = this->scene->CreateCylinder(); + // else if (entity == "sphere") + // geom = this->scene->CreateSphere(); + + // visual->AddGeometry(geom); + // link->AddChild(visual); + // model->AddChild(link); + + // create an sdf::Link to it can be added to the ECM throught the + // CreateEntities call + sdf::ElementPtr linkElem(new sdf::Element); + sdf::initFile("link.sdf", linkElem); + sdf::readString(this->LinkSDFString(geomLightType), linkElem); + // std::cerr << this->LinkSDFString("new_entity", geomLightType) << std::endl; + sdf::Link linkSdf; + linkSdf.Load(linkElem); + + std::lock_guard lock(this->mutex); + this->linksToAdd.push_back(linkSdf); } - - this->sceneManager.CreateVisual(visualId, visual, linkId); - this->entityIds.push_back(visualId); -*/ - } // Register this plugin From cdd9e5096ebaf57cd603a877d926a149e58dbc1c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 14 Oct 2021 18:11:36 -0700 Subject: [PATCH 04/25] support adding light links Signed-off-by: Ian Chen --- .../ComponentInspector.qml | 24 ++- src/gui/plugins/model_editor/ModelEditor.cc | 175 ++++++++++++++---- 2 files changed, 158 insertions(+), 41 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 6a30ab2028..518086fc5f 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -245,28 +245,40 @@ Rectangle { type: "Link" } ListElement { - text: "Ball" + text: "Directional" type: "Light" } ListElement { - text: "Continuous" + text: "Spot" type: "Light" } ListElement { - text: "Fixed" + text: "Point" type: "Light" + } + ListElement { + text: "Ball" + type: "Joint" + } + ListElement { + text: "Continuous" + type: "Joint" + } + ListElement { + text: "Fixed" + type: "Joint" } ListElement { text: "Prismatic" - type: "Light" + type: "Joint" } ListElement { text: "Revolute" - type: "Light" + type: "Joint" } ListElement { text: "Universal" - type: "Light" + type: "Joint" } } // The delegate for each section header diff --git a/src/gui/plugins/model_editor/ModelEditor.cc b/src/gui/plugins/model_editor/ModelEditor.cc index 262acb5b0c..132cbfc6ef 100644 --- a/src/gui/plugins/model_editor/ModelEditor.cc +++ b/src/gui/plugins/model_editor/ModelEditor.cc @@ -53,12 +53,14 @@ namespace ignition::gazebo const std::string &_entityType, const std::string &/*_parent*/); public: std::string GeomSDFString( - const std::string &_geomType, - const math::Vector3d &_size = math::Vector3d::One) const; + const std::string &_geomType) const; + + public: std::string LightSDFString( + const std::string &_geomType) const; + public: std::string LinkSDFString( - const std::string &_geomType, - const math::Vector3d &_size = math::Vector3d::One) const; + const std::string &_geomType) const; /// \brief Generate a unique entity id. /// \return The unique entity id @@ -148,7 +150,7 @@ void ModelEditor::Update(const UpdateInfo &, continue; } - // generate link name + // generate unique link name std::string linkName = "link"; Entity linkEnt = _ecm.EntityByComponents( components::Link(), components::ParentEntity(parent), @@ -157,7 +159,7 @@ void ModelEditor::Update(const UpdateInfo &, while (linkEnt) { linkName = std::string("link") + "_" + std::to_string(++counter); - _ecm.EntityByComponents( + linkEnt = _ecm.EntityByComponents( components::Link(), components::ParentEntity(parent), components::Name(linkName)); } @@ -232,58 +234,155 @@ void ModelEditorPrivate::Initialize() // return kNullEntity; //} +///////////////////////////////////////////////// +std::string ModelEditorPrivate::LightSDFString( + const std::string &_lightType) const +{ + std::stringstream lightStr; + lightStr << ""; + + if (_lightType == "directional") + { + lightStr + << "false" + << "1.0 1.0 1.0 1" + << "0.5 0.5 0.5 1"; + } + else if (_lightType == "spot") + { + lightStr + << "false" + << "1.0 1.0 1.0 1" + << "0.5 0.5 0.5 1" + << "" + << "4" + << "0.2" + << "0.5" + << "0.01" + << "" + << "0 0 -1" + << "" + << "0.1" + << "0.5" + << "0.8" + << ""; + } + else if (_lightType == "point") + { + lightStr + << "false" + << "1.0 1.0 1.0 1" + << "0.5 0.5 0.5 1" + << "" + << "4" + << "0.2" + << "0.5" + << "0.01" + << ""; + } + else + { + ignwarn << "Light type not supported: " << _lightType << std::endl; + return std::string(); + } + + lightStr << ""; + return lightStr.str(); +} ///////////////////////////////////////////////// std::string ModelEditorPrivate::GeomSDFString( - const std::string &_geomType, - const math::Vector3d &_size) const + const std::string &_geomType) const { + math::Vector3d size = math::Vector3d::One; std::stringstream geomStr; geomStr << ""; if (_geomType == "box") { geomStr << "" - << " " << _size << "" + << " " << size << "" << ""; } else if (_geomType == "sphere") { geomStr << "" - << " " << _size.X() * 0.5 << "" + << " " << size.X() * 0.5 << "" << ""; } else if (_geomType == "cylinder") { geomStr << "" - << " " << _size.X() * 0.5 << "" - << " " << _size.Z() << "" + << " " << size.X() * 0.5 << "" + << " " << size.Z() << "" << ""; } + else if (_geomType == "capsule") + { + geomStr + << "" + << " " << size.X() * 0.5 << "" + << " " << size.Z() << "" + << ""; + } + else if (_geomType == "ellipsoid") + { + geomStr + << "" + << " " << size.X() * 0.5 << "" + << ""; + } + else + { + ignwarn << "Geometry type not supported: " << _geomType << std::endl; + return std::string(); + } + + geomStr << ""; return geomStr.str(); } ///////////////////////////////////////////////// std::string ModelEditorPrivate::LinkSDFString( - const std::string &_geomType, - const math::Vector3d &_size) const + const std::string &_geomOrLightType) const { - std::string geomStr = this->GeomSDFString(_geomType, _size); + std::stringstream linkStr; - linkStr - << "" - << " " - << " " - << geomStr - << " " - << " " - << geomStr - << " " - << " " - << ""; + if (_geomOrLightType == "empty") + { + linkStr << ""; + return linkStr.str(); + } + + std::string geomOrLightStr; + if (_geomOrLightType == "spot" || geomOrLightType == "directional" || + geomOrLightType == "point") + { + geomOrLightStr = this->LightSDFString(_geomOrLightType); + linkStr + << "" + << geomOrLightStr + << ""; + } + else + { + geomOrLightStr = this->GeomSDFString(_geomOrLightType); + linkStr + << "" + << " " + << geomOrLightStr + << " " + << " " + << geomOrLightStr + << " " + << ""; + } + + if (geomOrLightStr.empty()) + return std::string(); return linkStr.str(); } @@ -322,15 +421,21 @@ void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, // create an sdf::Link to it can be added to the ECM throught the // CreateEntities call - sdf::ElementPtr linkElem(new sdf::Element); - sdf::initFile("link.sdf", linkElem); - sdf::readString(this->LinkSDFString(geomLightType), linkElem); - // std::cerr << this->LinkSDFString("new_entity", geomLightType) << std::endl; - sdf::Link linkSdf; - linkSdf.Load(linkElem); - - std::lock_guard lock(this->mutex); - this->linksToAdd.push_back(linkSdf); + std::string linkSDFStr = this->LinkSDFString(geomLightType); + if (!linkSDFStr.empty()) + { + linkSDFStr = std::string("" + + linkSDFStr + ""; + + sdf::ElementPtr linkElem(new sdf::Element); + sdf::initFile("link.sdf", linkElem); + sdf::readString(linkSDFStr, linkElem); + sdf::Link linkSdf; + linkSdf.Load(linkElem); + + std::lock_guard lock(this->mutex); + this->linksToAdd.push_back(linkSdf); + } } } From 8f7714cc382593b1896dc8954fc0e30273362403 Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Fri, 22 Oct 2021 15:26:50 -0600 Subject: [PATCH 05/25] notify other GUI plugins of added/removed entities via GUI events Signed-off-by: Ashton Larkin --- include/ignition/gazebo/gui/GuiEvents.hh | 36 +++++++++++ src/gui/plugins/entity_tree/EntityTree.cc | 76 +++++++++++++++++++++++ 2 files changed, 112 insertions(+) diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index c68c61cfa8..5838fd7900 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -18,6 +18,7 @@ #define IGNITION_GAZEBO_GUI_GUIEVENTS_HH_ #include +#include #include #include #include @@ -99,6 +100,41 @@ namespace events private: bool fromUser{false}; }; + /// \brief Event that contains newly created and removed entities + class AddedRemovedEntities : public QEvent + { + /// \brief Constructor + /// \param[in] _newEntities Set of newly created entities + /// \param[in] _removedEntities Set of recently removed entities + public: AddedRemovedEntities(const std::set _newEntities, + const std::set &_removedEntities) + : QEvent(kType), newEntities(_newEntities), + removedEntities(_removedEntities) + { + } + + /// \brief Get the set of newly created entities + public: const std::set &NewEntities() const + { + return this->newEntities; + } + + /// \brief Get the set of recently removed entities + public: const std::set &RemovedEntities() const + { + return this->removedEntities; + } + + /// \brief Unique type for this event. + static const QEvent::Type kType = QEvent::Type(QEvent::User + 3); + + /// \brief Set of newly created entities + private: std::set newEntities; + + /// \brief Set of recently removed entities + private: std::set removedEntities; + }; + /// \brief True if a transform control is currently active (translate / /// rotate / scale). False if we're in selection mode. class TransformControlModeActive : public QEvent diff --git a/src/gui/plugins/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index 79de5dfb7f..1f915d9f2d 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -18,6 +18,8 @@ #include "EntityTree.hh" #include +#include +#include #include #include @@ -54,6 +56,16 @@ namespace ignition::gazebo /// \brief World entity public: Entity worldEntity{kNullEntity}; + + /// \brief List of new entities from a gui event + public: std::set newEntities; + + /// \brief List of removed entities from a gui event + public: std::set removedEntities; + + /// \brief Mutex to protect gui event and system upate call race conditions + /// for newEntities and removedEntities + public: std::mutex newRemovedEntityMutex; }; } @@ -376,6 +388,55 @@ void EntityTree::Update(const UpdateInfo &, EntityComponentManager &_ecm) Q_ARG(unsigned int, _entity)); return true; }); + + { + // update the entity tree with new/removed entities from gui events + std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); + + for (auto entity : this->dataPtr->newEntities) + { + // make sure the entity to be added has a name and parent + auto nameComp = _ecm.Component(entity); + if (!nameComp) + { + ignerr << "Could not add entity [" << entity << "] to the entity tree " + << "because it does not have a name component.\n"; + continue; + } + auto parentComp = _ecm.Component(entity); + if (!parentComp) + { + ignerr << "Could not add entity [" << entity << "] to the entity tree " + << "because it does not have a parent entity component.\n"; + continue; + } + + // World children are top-level + auto parentEntity = parentComp->Data(); + if (this->dataPtr->worldEntity != kNullEntity && + parentEntity == this->dataPtr->worldEntity) + { + parentEntity = kNullEntity; + } + + QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", + Qt::QueuedConnection, + Q_ARG(unsigned int, entity), + Q_ARG(QString, QString::fromStdString(nameComp->Data())), + Q_ARG(unsigned int, parentEntity), + Q_ARG(QString, entityType(entity, _ecm))); + } + + for (auto entity : this->dataPtr->removedEntities) + { + QMetaObject::invokeMethod(&this->dataPtr->treeModel, "RemoveEntity", + Qt::QueuedConnection, + Q_ARG(unsigned int, entity)); + } + + this->dataPtr->newEntities.clear(); + this->dataPtr->removedEntities.clear(); + } } ///////////////////////////////////////////////// @@ -428,6 +489,21 @@ bool EntityTree::eventFilter(QObject *_obj, QEvent *_event) Qt::QueuedConnection); } } + else if (_event->type() == + ignition::gazebo::gui::events::AddedRemovedEntities::kType) + { + std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); + auto addedRemovedEvent = + reinterpret_cast(_event); + if (addedRemovedEvent) + { + for (auto entity : addedRemovedEvent->NewEntities()) + this->dataPtr->newEntities.insert(entity); + + for (auto entity : addedRemovedEvent->RemovedEntities()) + this->dataPtr->removedEntities.insert(entity); + } + } // Standard event processing return QObject::eventFilter(_obj, _event); From 3b967a36e23bffad366a7bb8c9075004c7523afc Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Mon, 25 Oct 2021 09:09:36 -0600 Subject: [PATCH 06/25] use const ref for constructor input params Signed-off-by: Ashton Larkin --- include/ignition/gazebo/gui/GuiEvents.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 5838fd7900..aa22dbf0e2 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -106,7 +106,7 @@ namespace events /// \brief Constructor /// \param[in] _newEntities Set of newly created entities /// \param[in] _removedEntities Set of recently removed entities - public: AddedRemovedEntities(const std::set _newEntities, + public: AddedRemovedEntities(const std::set &_newEntities, const std::set &_removedEntities) : QEvent(kType), newEntities(_newEntities), removedEntities(_removedEntities) From afd5b8c496e75bf1c9f12fc781804c3cd2be8178 Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Mon, 25 Oct 2021 09:48:15 -0600 Subject: [PATCH 07/25] guarantee 64 bit entity IDs with gazebo::Entity instead of unsigned int Signed-off-by: Ashton Larkin --- src/gui/plugins/entity_tree/EntityTree.cc | 28 +++++++++++------------ src/gui/plugins/entity_tree/EntityTree.hh | 15 ++++++------ 2 files changed, 22 insertions(+), 21 deletions(-) diff --git a/src/gui/plugins/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index 1f915d9f2d..eb752688f4 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -115,8 +115,8 @@ TreeModel::TreeModel() : QStandardItemModel() } ///////////////////////////////////////////////// -void TreeModel::AddEntity(unsigned int _entity, const QString &_entityName, - unsigned int _parentEntity, const QString &_type) +void TreeModel::AddEntity(Entity _entity, const QString &_entityName, + Entity _parentEntity, const QString &_type) { IGN_PROFILE_THREAD_NAME("Qt thread"); IGN_PROFILE("TreeModel::AddEntity"); @@ -177,7 +177,7 @@ void TreeModel::AddEntity(unsigned int _entity, const QString &_entityName, } ///////////////////////////////////////////////// -void TreeModel::RemoveEntity(unsigned int _entity) +void TreeModel::RemoveEntity(Entity _entity) { IGN_PROFILE("TreeModel::RemoveEntity"); QStandardItem *item{nullptr}; @@ -259,7 +259,7 @@ QString TreeModel::ScopedName(const QModelIndex &_index) const } ///////////////////////////////////////////////// -unsigned int TreeModel::EntityId(const QModelIndex &_index) const +Entity TreeModel::EntityId(const QModelIndex &_index) const { Entity entity{kNullEntity}; QStandardItem *item = this->itemFromIndex(_index); @@ -341,9 +341,9 @@ void EntityTree::Update(const UpdateInfo &, EntityComponentManager &_ecm) QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", Qt::QueuedConnection, - Q_ARG(unsigned int, _entity), + Q_ARG(Entity, _entity), Q_ARG(QString, QString::fromStdString(_name->Data())), - Q_ARG(unsigned int, parentEntity), + Q_ARG(Entity, parentEntity), Q_ARG(QString, entityType(_entity, _ecm))); return true; }); @@ -371,9 +371,9 @@ void EntityTree::Update(const UpdateInfo &, EntityComponentManager &_ecm) QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", Qt::QueuedConnection, - Q_ARG(unsigned int, _entity), + Q_ARG(Entity, _entity), Q_ARG(QString, QString::fromStdString(_name->Data())), - Q_ARG(unsigned int, parentEntity), + Q_ARG(Entity, parentEntity), Q_ARG(QString, entityType(_entity, _ecm))); return true; }); @@ -385,7 +385,7 @@ void EntityTree::Update(const UpdateInfo &, EntityComponentManager &_ecm) { QMetaObject::invokeMethod(&this->dataPtr->treeModel, "RemoveEntity", Qt::QueuedConnection, - Q_ARG(unsigned int, _entity)); + Q_ARG(Entity, _entity)); return true; }); @@ -421,9 +421,9 @@ void EntityTree::Update(const UpdateInfo &, EntityComponentManager &_ecm) QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", Qt::QueuedConnection, - Q_ARG(unsigned int, entity), + Q_ARG(Entity, entity), Q_ARG(QString, QString::fromStdString(nameComp->Data())), - Q_ARG(unsigned int, parentEntity), + Q_ARG(Entity, parentEntity), Q_ARG(QString, entityType(entity, _ecm))); } @@ -431,7 +431,7 @@ void EntityTree::Update(const UpdateInfo &, EntityComponentManager &_ecm) { QMetaObject::invokeMethod(&this->dataPtr->treeModel, "RemoveEntity", Qt::QueuedConnection, - Q_ARG(unsigned int, entity)); + Q_ARG(Entity, entity)); } this->dataPtr->newEntities.clear(); @@ -440,7 +440,7 @@ void EntityTree::Update(const UpdateInfo &, EntityComponentManager &_ecm) } ///////////////////////////////////////////////// -void EntityTree::OnEntitySelectedFromQml(unsigned int _entity) +void EntityTree::OnEntitySelectedFromQml(Entity _entity) { std::vector entitySet {_entity}; gui::events::EntitiesSelected event(entitySet, true); @@ -474,7 +474,7 @@ bool EntityTree::eventFilter(QObject *_obj, QEvent *_event) QMetaObject::invokeMethod(this->PluginItem(), "onEntitySelectedFromCpp", Qt::QueuedConnection, Q_ARG(QVariant, - QVariant(static_cast(entity)))); + QVariant(static_cast(entity)))); } } } diff --git a/src/gui/plugins/entity_tree/EntityTree.hh b/src/gui/plugins/entity_tree/EntityTree.hh index 4f882de9f7..092332913a 100644 --- a/src/gui/plugins/entity_tree/EntityTree.hh +++ b/src/gui/plugins/entity_tree/EntityTree.hh @@ -22,6 +22,7 @@ #include #include +#include #include namespace ignition @@ -50,14 +51,14 @@ namespace gazebo /// \param[in] _parentEntity Parent entity. By default, kNullEntity, which /// means it's a root entity. /// \param[in] _type Entity type - public slots: void AddEntity(unsigned int _entity, + public slots: void AddEntity(Entity _entity, const QString &_entityName, - unsigned int _parentEntity = kNullEntity, + Entity _parentEntity = kNullEntity, const QString &_type = QString()); /// \brief Remove an entity from the tree. /// \param[in] _entity Entity to be removed - public slots: void RemoveEntity(unsigned int _entity); + public slots: void RemoveEntity(Entity _entity); /// \brief Get the entity type of a tree item at specified index /// \param[in] _index Model index @@ -72,7 +73,7 @@ namespace gazebo /// \brief Get the entity ID of a tree item at specified index /// \param[in] _index Model index /// \return Entity ID - public: Q_INVOKABLE unsigned int EntityId(const QModelIndex &_index) const; + public: Q_INVOKABLE Entity EntityId(const QModelIndex &_index) const; /// \brief Keep track of which item corresponds to which entity. private: std::map entityItems; @@ -82,14 +83,14 @@ namespace gazebo { /// \brief Entity ID // cppcheck-suppress unusedStructMember - unsigned int entity; + Entity entity; /// \brief Entity name QString name; /// \brief Parent ID // cppcheck-suppress unusedStructMember - unsigned int parentEntity; + Entity parentEntity; /// \brief Entity type QString type; @@ -123,7 +124,7 @@ namespace gazebo /// \brief Callback when an entity has been selected. This should be /// called from QML. /// \param[in] _entity Entity being selected. - public: Q_INVOKABLE void OnEntitySelectedFromQml(unsigned int _entity); + public: Q_INVOKABLE void OnEntitySelectedFromQml(Entity _entity); /// \brief Callback when all entities have been deselected. /// This should be called from QML. From 3031b3c3938f6c8d64cedd317d862c89fc346942 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 25 Oct 2021 14:32:36 -0700 Subject: [PATCH 08/25] testing makr as new entity func Signed-off-by: Ian Chen --- .../ignition/gazebo/EntityComponentManager.hh | 9 +++- src/EntityComponentManager.cc | 14 ++++++ src/gui/plugins/entity_tree/EntityTree.cc | 4 +- src/gui/plugins/model_editor/ModelEditor.cc | 35 +++++++++++++- .../plugins/scene_manager/GzSceneManager.cc | 48 ++++++++++++++++++- src/rendering/RenderUtil.cc | 2 + 6 files changed, 107 insertions(+), 5 deletions(-) diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index a98c2b854b..cf957b57c1 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -669,6 +669,13 @@ namespace ignition /// \param[in] _offset Offset value. public: void SetEntityCreateOffset(uint64_t _offset); + /// \internal + /// \brief Mark whether or not an entity is a newly created entity. + /// + /// \param[in] _entity Entity id to mark as new + /// \return True to mark it as new, false to mark it as not new + public: void MarkEntityAsNew(const Entity _entity, bool _new = true); + /// \brief Clear the list of newly added entities so that a call to /// EachAdded after this will have no entities to iterate. This function /// is protected to facilitate testing. @@ -692,7 +699,7 @@ namespace ignition /// call to ClearNewlyCreatedEntities /// \param[in] _entity Entity id to check. /// \return True if the Entity is new. - private: bool IsNewEntity(const Entity _entity) const; + public: bool IsNewEntity(const Entity _entity) const; /// \brief Get whether an Entity has been marked to be removed. /// \param[in] _entity Entity id to check. diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 1b261294d0..bdb60329e1 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -803,6 +803,20 @@ bool EntityComponentManager::EntityHasComponentType(const Entity _entity, return comp != nullptr; } +///////////////////////////////////////////////// +void EntityComponentManager::MarkEntityAsNew(const Entity _entity, bool _new) +{ + std::lock_guard lock(this->dataPtr->entityCreatedMutex); + if (_new) + { + this->dataPtr->newlyCreatedEntities.insert(_entity); + } + else + { + this->dataPtr->newlyCreatedEntities.erase(_entity); + } +} + ///////////////////////////////////////////////// bool EntityComponentManager::IsNewEntity(const Entity _entity) const { diff --git a/src/gui/plugins/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index 1f915d9f2d..e1d3a4318e 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -122,6 +122,8 @@ void TreeModel::AddEntity(unsigned int _entity, const QString &_entityName, IGN_PROFILE("TreeModel::AddEntity"); QStandardItem *parentItem{nullptr}; + std::cerr << "add entity " << _entity << " " << _entityName.toStdString() << std::endl; + // Root if (_parentEntity == kNullEntity) { @@ -392,9 +394,9 @@ void EntityTree::Update(const UpdateInfo &, EntityComponentManager &_ecm) { // update the entity tree with new/removed entities from gui events std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); - for (auto entity : this->dataPtr->newEntities) { + std::cerr << "entity tree ent: " << entity << std::endl; // make sure the entity to be added has a name and parent auto nameComp = _ecm.Component(entity); if (!nameComp) diff --git a/src/gui/plugins/model_editor/ModelEditor.cc b/src/gui/plugins/model_editor/ModelEditor.cc index 132cbfc6ef..d5cf4155aa 100644 --- a/src/gui/plugins/model_editor/ModelEditor.cc +++ b/src/gui/plugins/model_editor/ModelEditor.cc @@ -131,7 +131,7 @@ void ModelEditor::Update(const UpdateInfo &, // with the ones on the server. The log playback starts at max / 2 // On the gui side, we will start entity id at an offset of max / 4 // todo(anyone) set a better entity create offset - _ecm.SetEntityCreateOffset(math::MAX_I64 / 4); +// _ecm.SetEntityCreateOffset(math::MAX_I64 / 4); this->dataPtr->entityCreator = std::make_unique( _ecm, this->dataPtr->eventMgr); } @@ -139,6 +139,7 @@ void ModelEditor::Update(const UpdateInfo &, std::lock_guard lock(this->dataPtr->mutex); // add link entities to the ECM + std::set newEntities; for (auto & linkSdf : this->dataPtr->linksToAdd) { Entity parent = _ecm.EntityByComponents( @@ -164,11 +165,41 @@ void ModelEditor::Update(const UpdateInfo &, components::Name(linkName)); } - std::cerr << "creating entity in ecm " << linkName << std::endl; linkSdf.SetName(linkName); auto entity = this->dataPtr->entityCreator->CreateEntities(&linkSdf); this->dataPtr->entityCreator->SetParent(entity, parent); + + // traverse the tree and add all new entities created by the entity creator + // to the set + std::list entities; + entities.push_back(entity); + while (!entities.empty()) + { + Entity ent = entities.front(); + entities.pop_front(); + + // add new entity created + newEntities.insert(ent); + + auto childEntities = _ecm.EntitiesByComponents( + components::ParentEntity(ent)); + for (const auto &child : childEntities) + entities.push_back(child); + } } + + for (const auto & ent: newEntities) + std::cerr << "creating entity in ecm " << ent << std::endl; + + // use tmp AddedRemovedEntities event to update other gui plugins + // note this event will be removed in Ignition Garden + std::set removedEntities; + ignition::gazebo::gui::events::AddedRemovedEntities event( + newEntities, removedEntities); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &event); + this->dataPtr->linksToAdd.clear(); } diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index f7ec59fbac..00dc54e5cb 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -28,6 +28,7 @@ #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" #include "ignition/gazebo/rendering/RenderUtil.hh" namespace ignition @@ -46,6 +47,16 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Rendering utility public: RenderUtil renderUtil; + + /// \brief List of new entities from a gui event + public: std::set newEntities; + + /// \brief List of removed entities from a gui event + public: std::set removedEntities; + + /// \brief Mutex to protect gui event and system upate call race conditions + /// for newEntities and removedEntities + public: std::mutex newRemovedEntityMutex; }; } } @@ -80,16 +91,51 @@ void GzSceneManager::Update(const UpdateInfo &_info, IGN_PROFILE("GzSceneManager::Update"); this->dataPtr->renderUtil.UpdateECM(_info, _ecm); + + std::set tmpNewEntities; + std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); + { + for (const auto &ent : this->dataPtr->newEntities) + { + if (!_ecm.IsNewEntity(ent)) + _ecm.MarkEntityAsNew(ent, true); + else + tmpNewEntities.insert(ent); + } + this->dataPtr->newEntities.clear(); + } + this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + + for (const auto &ent : tmpNewEntities) + { + _ecm.MarkEntityAsNew(ent, false); + } + } ///////////////////////////////////////////////// bool GzSceneManager::eventFilter(QObject *_obj, QEvent *_event) { - if (_event->type() == gui::events::Render::kType) + if (_event->type() == ignition::gui::events::Render::kType) { this->dataPtr->OnRender(); } + else if (_event->type() == + ignition::gazebo::gui::events::AddedRemovedEntities::kType) + { + std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); + auto addedRemovedEvent = + reinterpret_cast(_event); + if (addedRemovedEvent) + { + for (auto entity : addedRemovedEvent->NewEntities()) + this->dataPtr->newEntities.insert(entity); + + for (auto entity : addedRemovedEvent->RemovedEntities()) + this->dataPtr->removedEntities.insert(entity); + } + } // Standard event processing return QObject::eventFilter(_obj, _event); diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 1ea5640e79..1b082deb68 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1499,6 +1499,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::Pose *_pose, const components::ParentEntity *_parent)->bool { + std::cerr << "new link first udpate " << _entity << " " << _name->Data() << std::endl; sdf::Link link; link.SetName(_name->Data()); link.SetRawPose(_pose->Data()); @@ -1816,6 +1817,7 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::Pose *_pose, const components::ParentEntity *_parent)->bool { + std::cerr << "got new link " << _entity << " " << _name->Data() << std::endl; sdf::Link link; link.SetName(_name->Data()); link.SetRawPose(_pose->Data()); From 399416cf272a9b3ea8e1336dfa723d84c6a74b7d Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 25 Oct 2021 14:33:42 -0700 Subject: [PATCH 09/25] rm printouts Signed-off-by: Ian Chen --- src/gui/plugins/entity_tree/EntityTree.cc | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/gui/plugins/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index e1d3a4318e..d0370e57be 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -122,8 +122,6 @@ void TreeModel::AddEntity(unsigned int _entity, const QString &_entityName, IGN_PROFILE("TreeModel::AddEntity"); QStandardItem *parentItem{nullptr}; - std::cerr << "add entity " << _entity << " " << _entityName.toStdString() << std::endl; - // Root if (_parentEntity == kNullEntity) { @@ -396,7 +394,6 @@ void EntityTree::Update(const UpdateInfo &, EntityComponentManager &_ecm) std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); for (auto entity : this->dataPtr->newEntities) { - std::cerr << "entity tree ent: " << entity << std::endl; // make sure the entity to be added has a name and parent auto nameComp = _ecm.Component(entity); if (!nameComp) From 79518bf8155219ab6d36908de27b4237ceffd1df Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 25 Oct 2021 14:47:05 -0700 Subject: [PATCH 10/25] register type Signed-off-by: Ian Chen --- src/gui/plugins/entity_tree/EntityTree.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/gui/plugins/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index e7ffd1bff5..d639f4e426 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -112,6 +112,7 @@ QString entityType(Entity _entity, ///////////////////////////////////////////////// TreeModel::TreeModel() : QStandardItemModel() { + qRegisterMetaType("Entity"); } ///////////////////////////////////////////////// From 413a075c0d1d14b932ab1330f0126f391bcbb2bc Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 25 Oct 2021 19:12:21 -0700 Subject: [PATCH 11/25] refactor render util Signed-off-by: Ian Chen --- .../ignition/gazebo/EntityComponentManager.hh | 2 +- include/ignition/gazebo/detail/BaseView.hh | 7 + .../ignition/gazebo/rendering/RenderUtil.hh | 8 + src/BaseView.cc | 21 + src/EntityComponentManager.cc | 30 +- .../plugins/scene_manager/GzSceneManager.cc | 26 +- src/rendering/RenderUtil.cc | 437 ++++++++++++------ 7 files changed, 373 insertions(+), 158 deletions(-) diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index cf957b57c1..c4f9607881 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -674,7 +674,7 @@ namespace ignition /// /// \param[in] _entity Entity id to mark as new /// \return True to mark it as new, false to mark it as not new - public: void MarkEntityAsNew(const Entity _entity, bool _new = true); + // public: void MarkEntityAsNew(const Entity _entity, bool _new = true); /// \brief Clear the list of newly added entities so that a call to /// EachAdded after this will have no entities to iterate. This function diff --git a/include/ignition/gazebo/detail/BaseView.hh b/include/ignition/gazebo/detail/BaseView.hh index eea6755316..be9463e983 100644 --- a/include/ignition/gazebo/detail/BaseView.hh +++ b/include/ignition/gazebo/detail/BaseView.hh @@ -99,6 +99,13 @@ class IGNITION_GAZEBO_VISIBLE BaseView /// \sa HasEntity, IsEntityMarkedForAddition public: bool MarkEntityToAdd(const Entity _entity, bool _new = false); + /// \internal + /// \brief Mark whether or not an entity is a newly created entity. + /// + /// \param[in] _entity Entity id to mark as new + /// \return True to mark it as new, false to mark it as not new + // public: bool MarkEntityAsNew(const Entity _entity, bool _new = false); + /// \brief See if the view requires a particular component type /// \param[in] _typeId The component type /// \return true if the view requires components of type _typeId, false diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 3d0361d955..01d34cded0 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -76,6 +76,14 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void UpdateFromECM(const UpdateInfo &_info, const EntityComponentManager &_ecm); + /// \brief Helper function to create visuals for new entities created in + /// ECM. This function is intended to be used by other GUI plugins when + /// new entities are created on the GUI side. + /// \param[in] _ecm Const reference to the entity component manager + /// \param[in] _entities Entities to create visuals for. + public: void CreateVisualsForEntities(const EntityComponentManager &_ecm, + const std::set &_entities); + /// \brief Set the rendering engine to use /// \param[in] _engineName Name of the rendering engine. public: void SetEngineName(const std::string &_engineName); diff --git a/src/BaseView.cc b/src/BaseView.cc index 23e78ee607..8aeb95633b 100644 --- a/src/BaseView.cc +++ b/src/BaseView.cc @@ -45,6 +45,27 @@ bool BaseView::MarkEntityToAdd(const Entity _entity, bool _new) return true; } +// ////////////////////////////////////////////////// +// bool BaseView::MarkEntityAsNew(const Entity _entity, bool _new) +// { +// return this->MarkEntityToAdd(_entity, _new); +// // if (_new) +// // this->newEntities.insert(_entity); +// // else +// // this->newEntities.erase(_entity); +// +// // this->MarkEntityToAdd(_entity, _new); +// // this->toAddEntities[_entity] = _new; +// +// // auto it = toAddEntities.find(_entity); +// // if (it != toAddEntities.end()) +// // it->second = _new; +// +// +// +// return true; +// } + ////////////////////////////////////////////////// bool BaseView::RequiresComponent(const ComponentTypeId _typeId) const { diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index bdb60329e1..387adbfd89 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -804,18 +804,24 @@ bool EntityComponentManager::EntityHasComponentType(const Entity _entity, } ///////////////////////////////////////////////// -void EntityComponentManager::MarkEntityAsNew(const Entity _entity, bool _new) -{ - std::lock_guard lock(this->dataPtr->entityCreatedMutex); - if (_new) - { - this->dataPtr->newlyCreatedEntities.insert(_entity); - } - else - { - this->dataPtr->newlyCreatedEntities.erase(_entity); - } -} +// void EntityComponentManager::MarkEntityAsNew(const Entity _entity, bool _new) +// { +// std::lock_guard lock(this->dataPtr->entityCreatedMutex); +// if (_new) +// { +// this->dataPtr->newlyCreatedEntities.insert(_entity); +// } +// else +// { +// this->dataPtr->newlyCreatedEntities.erase(_entity); +// } +// +// for (auto &viewPair : this->dataPtr->views) +// { +// auto &view = viewPair.second.first; +// view->MarkEntityAsNew(_entity, _new); +// } +// } ///////////////////////////////////////////////// bool EntityComponentManager::IsNewEntity(const Entity _entity) const diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 00dc54e5cb..eb9274797f 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -92,25 +92,27 @@ void GzSceneManager::Update(const UpdateInfo &_info, this->dataPtr->renderUtil.UpdateECM(_info, _ecm); - std::set tmpNewEntities; +// std::set tmpNewEntities; std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); { - for (const auto &ent : this->dataPtr->newEntities) - { - if (!_ecm.IsNewEntity(ent)) - _ecm.MarkEntityAsNew(ent, true); - else - tmpNewEntities.insert(ent); - } + this->dataPtr->renderUtil.CreateVisualsForEntities(_ecm, + this->dataPtr->newEntities); + // for (const auto &ent : this->dataPtr->newEntities) + // { + // if (!_ecm.IsNewEntity(ent)) + // _ecm.MarkEntityAsNew(ent, true); + // else + // tmpNewEntities.insert(ent); + // } this->dataPtr->newEntities.clear(); } this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); - for (const auto &ent : tmpNewEntities) - { - _ecm.MarkEntityAsNew(ent, false); - } +// for (const auto &ent : tmpNewEntities) +// { +// _ecm.MarkEntityAsNew(ent, false); +// } } diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 1b082deb68..1322d642e6 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -145,6 +145,40 @@ class ignition::gazebo::RenderUtilPrivate const sdf::Sensor &_sdfData, Entity _parent, const std::string &_topicSuffix); + /// \brief Helper function to create a visual for a link entity + /// \param[in] _ecm The entity-component manager + /// \param[in] _entity Entity to create the visual for + /// \param[in] _name Name component + /// \param[in] _pose Pose component + /// \param[in] _parent ParentEntity component + public: void CreateLink( + const EntityComponentManager &_ecm, + const Entity &_entity, + const components::Name *_name, + const components::Pose *_pose, + const components::ParentEntity *_parent); + + /// \brief Helper function to create a visual for a visual entity + /// \param[in] _ecm The entity-component manager + /// \param[in] _entity Entity to create the visual for + /// \param[in] _name Name component + /// \param[in] _pose Pose component + /// \param[in] _geom Geometry component + /// \param[in] _castShadows CastShadows component + /// \param[in] _transparency Transparency component + /// \param[in] _visibilityFlags VisibilityFlags component + /// \param[in] _parent ParentEntity component + public: void CreateVisual( + const EntityComponentManager &_ecm, + const Entity &_entity, + const components::Name *_name, + const components::Pose *_pose, + const components::Geometry *_geom, + const components::CastShadows *_castShadows, + const components::Transparency *_transparency, + const components::VisibilityFlags *_visibilityFlags, + const components::ParentEntity *_parent); + /// \brief Total time elapsed in simulation. This will not increase while /// paused. public: std::chrono::steady_clock::duration simTime{0}; @@ -1500,16 +1534,20 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::ParentEntity *_parent)->bool { std::cerr << "new link first udpate " << _entity << " " << _name->Data() << std::endl; - sdf::Link link; - link.SetName(_name->Data()); - link.SetRawPose(_pose->Data()); - this->newLinks.push_back( - std::make_tuple(_entity, link, _parent->Data())); - // used for collsions - this->modelToLinkEntities[_parent->Data()].push_back(_entity); - // used for joints - this->matchLinksWithEntities[_parent->Data()][_name->Data()] = - _entity; + + this->CreateLink(_ecm, _entity, _name, _pose, _parent); + + // sdf::Link link; + // link.SetName(_name->Data()); + // link.SetRawPose(_pose->Data()); + // this->newLinks.push_back( + // std::make_tuple(_entity, link, _parent->Data())); + // // used for collsions + // this->modelToLinkEntities[_parent->Data()].push_back(_entity); + // // used for joints + // this->matchLinksWithEntities[_parent->Data()][_name->Data()] = + // _entity; + return true; }); @@ -1530,62 +1568,65 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::VisibilityFlags *_visibilityFlags, const components::ParentEntity *_parent)->bool { - sdf::Visual visual; - visual.SetName(_name->Data()); - visual.SetRawPose(_pose->Data()); - visual.SetGeom(_geom->Data()); - visual.SetCastShadows(_castShadows->Data()); - visual.SetTransparency(_transparency->Data()); - visual.SetVisibilityFlags(_visibilityFlags->Data()); - - // Optional components - auto material = _ecm.Component(_entity); - if (material != nullptr) - { - visual.SetMaterial(material->Data()); - } - - auto laserRetro = _ecm.Component(_entity); - if (laserRetro != nullptr) - { - visual.SetLaserRetro(laserRetro->Data()); - } - - // set label - auto label = _ecm.Component(_entity); - if (label != nullptr) - { - this->entityLabel[_entity] = label->Data(); - } - - if (auto temp = _ecm.Component(_entity)) - { - // get the uniform temperature for the entity - this->entityTemp[_entity] = std::make_tuple - (temp->Data().Kelvin(), 0.0, ""); - } - else - { - // entity doesn't have a uniform temperature. Check if it has - // a heat signature with an associated temperature range - auto heatSignature = - _ecm.Component(_entity); - auto tempRange = - _ecm.Component(_entity); - if (heatSignature && tempRange) - { - this->entityTemp[_entity] = - std::make_tuple( - tempRange->Data().min.Kelvin(), - tempRange->Data().max.Kelvin(), - std::string(heatSignature->Data())); - } - } - - this->newVisuals.push_back( - std::make_tuple(_entity, visual, _parent->Data())); - - this->linkToVisualEntities[_parent->Data()].push_back(_entity); + this->CreateVisual(_ecm, _entity, _name, _pose, _geom, _castShadows, + _transparency, _visibilityFlags, _parent); + + // sdf::Visual visual; + // visual.SetName(_name->Data()); + // visual.SetRawPose(_pose->Data()); + // visual.SetGeom(_geom->Data()); + // visual.SetCastShadows(_castShadows->Data()); + // visual.SetTransparency(_transparency->Data()); + // visual.SetVisibilityFlags(_visibilityFlags->Data()); + + // // Optional components + // auto material = _ecm.Component(_entity); + // if (material != nullptr) + // { + // visual.SetMaterial(material->Data()); + // } + + // auto laserRetro = _ecm.Component(_entity); + // if (laserRetro != nullptr) + // { + // visual.SetLaserRetro(laserRetro->Data()); + // } + + // // set label + // auto label = _ecm.Component(_entity); + // if (label != nullptr) + // { + // this->entityLabel[_entity] = label->Data(); + // } + + // if (auto temp = _ecm.Component(_entity)) + // { + // // get the uniform temperature for the entity + // this->entityTemp[_entity] = std::make_tuple + // (temp->Data().Kelvin(), 0.0, ""); + // } + // else + // { + // // entity doesn't have a uniform temperature. Check if it has + // // a heat signature with an associated temperature range + // auto heatSignature = + // _ecm.Component(_entity); + // auto tempRange = + // _ecm.Component(_entity); + // if (heatSignature && tempRange) + // { + // this->entityTemp[_entity] = + // std::make_tuple( + // tempRange->Data().min.Kelvin(), + // tempRange->Data().max.Kelvin(), + // std::string(heatSignature->Data())); + // } + // } + + // this->newVisuals.push_back( + // std::make_tuple(_entity, visual, _parent->Data())); + + // this->linkToVisualEntities[_parent->Data()].push_back(_entity); return true; }); @@ -1817,17 +1858,18 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::Pose *_pose, const components::ParentEntity *_parent)->bool { - std::cerr << "got new link " << _entity << " " << _name->Data() << std::endl; - sdf::Link link; - link.SetName(_name->Data()); - link.SetRawPose(_pose->Data()); - this->newLinks.push_back( - std::make_tuple(_entity, link, _parent->Data())); - // used for collsions - this->modelToLinkEntities[_parent->Data()].push_back(_entity); - // used for joints - this->matchLinksWithEntities[_parent->Data()][_name->Data()] = - _entity; + // std::cerr << "got new link " << _entity << " " << _name->Data() << std::endl; + // sdf::Link link; + // link.SetName(_name->Data()); + // link.SetRawPose(_pose->Data()); + // this->newLinks.push_back( + // std::make_tuple(_entity, link, _parent->Data())); + // // used for collsions + // this->modelToLinkEntities[_parent->Data()].push_back(_entity); + // // used for joints + // this->matchLinksWithEntities[_parent->Data()][_name->Data()] = + // _entity; + this->CreateLink(_ecm, _entity, _name, _pose, _parent); return true; }); @@ -1848,62 +1890,65 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::VisibilityFlags *_visibilityFlags, const components::ParentEntity *_parent)->bool { - sdf::Visual visual; - visual.SetName(_name->Data()); - visual.SetRawPose(_pose->Data()); - visual.SetGeom(_geom->Data()); - visual.SetCastShadows(_castShadows->Data()); - visual.SetTransparency(_transparency->Data()); - visual.SetVisibilityFlags(_visibilityFlags->Data()); - - // Optional components - auto material = _ecm.Component(_entity); - if (material != nullptr) - { - visual.SetMaterial(material->Data()); - } - - auto laserRetro = _ecm.Component(_entity); - if (laserRetro != nullptr) - { - visual.SetLaserRetro(laserRetro->Data()); - } - - // set label - auto label = _ecm.Component(_entity); - if (label != nullptr) - { - this->entityLabel[_entity] = label->Data(); - } - - if (auto temp = _ecm.Component(_entity)) - { - // get the uniform temperature for the entity - this->entityTemp[_entity] = std::make_tuple - (temp->Data().Kelvin(), 0.0, ""); - } - else - { - // entity doesn't have a uniform temperature. Check if it has - // a heat signature with an associated temperature range - auto heatSignature = - _ecm.Component(_entity); - auto tempRange = - _ecm.Component(_entity); - if (heatSignature && tempRange) - { - this->entityTemp[_entity] = - std::make_tuple( - tempRange->Data().min.Kelvin(), - tempRange->Data().max.Kelvin(), - std::string(heatSignature->Data())); - } - } - - this->newVisuals.push_back( - std::make_tuple(_entity, visual, _parent->Data())); - - this->linkToVisualEntities[_parent->Data()].push_back(_entity); + this->CreateVisual(_ecm, _entity, _name, _pose, _geom, _castShadows, + _transparency, _visibilityFlags, _parent); + + // sdf::Visual visual; + // visual.SetName(_name->Data()); + // visual.SetRawPose(_pose->Data()); + // visual.SetGeom(_geom->Data()); + // visual.SetCastShadows(_castShadows->Data()); + // visual.SetTransparency(_transparency->Data()); + // visual.SetVisibilityFlags(_visibilityFlags->Data()); + + // // Optional components + // auto material = _ecm.Component(_entity); + // if (material != nullptr) + // { + // visual.SetMaterial(material->Data()); + // } + + // auto laserRetro = _ecm.Component(_entity); + // if (laserRetro != nullptr) + // { + // visual.SetLaserRetro(laserRetro->Data()); + // } + + // // set label + // auto label = _ecm.Component(_entity); + // if (label != nullptr) + // { + // this->entityLabel[_entity] = label->Data(); + // } + + // if (auto temp = _ecm.Component(_entity)) + // { + // // get the uniform temperature for the entity + // this->entityTemp[_entity] = std::make_tuple + // (temp->Data().Kelvin(), 0.0, ""); + // } + // else + // { + // // entity doesn't have a uniform temperature. Check if it has + // // a heat signature with an associated temperature range + // auto heatSignature = + // _ecm.Component(_entity); + // auto tempRange = + // _ecm.Component(_entity); + // if (heatSignature && tempRange) + // { + // this->entityTemp[_entity] = + // std::make_tuple( + // tempRange->Data().min.Kelvin(), + // tempRange->Data().max.Kelvin(), + // std::string(heatSignature->Data())); + // } + // } + + // this->newVisuals.push_back( + // std::make_tuple(_entity, visual, _parent->Data())); + + // this->linkToVisualEntities[_parent->Data()].push_back(_entity); return true; }); @@ -3437,3 +3482,129 @@ void RenderUtil::ViewCollisions(const Entity &_entity) } } } + +///////////////////////////////////////////////// +void RenderUtil::CreateVisualsForEntities( + const EntityComponentManager &_ecm, + const std::set &_entities) +{ + for (auto const &ent : _entities) + { + auto linkComp = _ecm.Component(ent); + if (linkComp) + { + this->dataPtr->CreateLink(_ecm, ent, + _ecm.Component(ent), + _ecm.Component(ent), + _ecm.Component(ent)); + continue; + } + + auto visualComp = _ecm.Component(ent); + if (visualComp) + { + this->dataPtr->CreateVisual(_ecm, ent, + _ecm.Component(ent), + _ecm.Component(ent), + _ecm.Component(ent), + _ecm.Component(ent), + _ecm.Component(ent), + _ecm.Component(ent), + _ecm.Component(ent)); + continue; + } + } +} + +///////////////////////////////////////////////// +void RenderUtilPrivate::CreateLink( + const EntityComponentManager &/*_ecm*/, + const Entity &_entity, + const components::Name *_name, + const components::Pose *_pose, + const components::ParentEntity *_parent) +{ + std::cerr << "create link " << _entity << " " << _name << std::endl; + sdf::Link link; + link.SetName(_name->Data()); + link.SetRawPose(_pose->Data()); + this->newLinks.push_back( + std::make_tuple(_entity, link, _parent->Data())); + // used for collsions + this->modelToLinkEntities[_parent->Data()].push_back(_entity); + // used for joints + this->matchLinksWithEntities[_parent->Data()][_name->Data()] = + _entity; +} + +///////////////////////////////////////////////// +void RenderUtilPrivate::CreateVisual( + const EntityComponentManager &_ecm, + const Entity &_entity, + const components::Name *_name, + const components::Pose *_pose, + const components::Geometry *_geom, + const components::CastShadows *_castShadows, + const components::Transparency *_transparency, + const components::VisibilityFlags *_visibilityFlags, + const components::ParentEntity *_parent) +{ + std::cerr << "create visual " << _entity << " " << _name << std::endl; + sdf::Visual visual; + visual.SetName(_name->Data()); + visual.SetRawPose(_pose->Data()); + visual.SetGeom(_geom->Data()); + visual.SetCastShadows(_castShadows->Data()); + visual.SetTransparency(_transparency->Data()); + visual.SetVisibilityFlags(_visibilityFlags->Data()); + + // Optional components + auto material = _ecm.Component(_entity); + if (material != nullptr) + { + visual.SetMaterial(material->Data()); + } + + auto laserRetro = _ecm.Component(_entity); + if (laserRetro != nullptr) + { + visual.SetLaserRetro(laserRetro->Data()); + } + + // set label + auto label = _ecm.Component(_entity); + if (label != nullptr) + { + this->entityLabel[_entity] = label->Data(); + } + + if (auto temp = _ecm.Component(_entity)) + { + // get the uniform temperature for the entity + this->entityTemp[_entity] = std::make_tuple + (temp->Data().Kelvin(), 0.0, ""); + } + else + { + // entity doesn't have a uniform temperature. Check if it has + // a heat signature with an associated temperature range + auto heatSignature = + _ecm.Component(_entity); + auto tempRange = + _ecm.Component(_entity); + if (heatSignature && tempRange) + { + this->entityTemp[_entity] = + std::make_tuple( + tempRange->Data().min.Kelvin(), + tempRange->Data().max.Kelvin(), + std::string(heatSignature->Data())); + } + } + + this->newVisuals.push_back( + std::make_tuple(_entity, visual, _parent->Data())); + + this->linkToVisualEntities[_parent->Data()].push_back(_entity); +} + From 4c81d382ceaf8e1dfd03609c529ad8558731d74b Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 2 Nov 2021 16:05:40 -0700 Subject: [PATCH 12/25] workaround for avoiding crash on exit Signed-off-by: Ian Chen --- src/gui/plugins/model_editor/ModelEditor.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/gui/plugins/model_editor/ModelEditor.cc b/src/gui/plugins/model_editor/ModelEditor.cc index d5cf4155aa..0ebb4bf62e 100644 --- a/src/gui/plugins/model_editor/ModelEditor.cc +++ b/src/gui/plugins/model_editor/ModelEditor.cc @@ -154,14 +154,14 @@ void ModelEditor::Update(const UpdateInfo &, // generate unique link name std::string linkName = "link"; Entity linkEnt = _ecm.EntityByComponents( - components::Link(), components::ParentEntity(parent), + /*components::Link(),*/ components::ParentEntity(parent), components::Name(linkName)); int64_t counter = 0; while (linkEnt) { linkName = std::string("link") + "_" + std::to_string(++counter); linkEnt = _ecm.EntityByComponents( - components::Link(), components::ParentEntity(parent), + /*components::Link(),*/ components::ParentEntity(parent), components::Name(linkName)); } From 44f785937ec6e9556fc14f48f21a1f6ad8eec18b Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 4 Nov 2021 13:14:21 -0700 Subject: [PATCH 13/25] refactor, comment out unused menu items Signed-off-by: Ian Chen --- .../ComponentInspector.qml | 59 ++-- src/gui/plugins/model_editor/ModelEditor.cc | 286 +++++++----------- src/rendering/RenderUtil.cc | 15 - 3 files changed, 138 insertions(+), 222 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index da262d0030..3259c348d0 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -245,10 +245,6 @@ Rectangle { text: "Empty" type: "Link" } - ListElement { - text: "Mesh" - type: "Link" - } ListElement { text: "Sphere" type: "Link" @@ -265,30 +261,36 @@ Rectangle { text: "Point" type: "Light" } - ListElement { - text: "Ball" - type: "Joint" - } - ListElement { - text: "Continuous" - type: "Joint" - } - ListElement { - text: "Fixed" - type: "Joint" - } - ListElement { - text: "Prismatic" - type: "Joint" - } - ListElement { - text: "Revolute" - type: "Joint" - } - ListElement { - text: "Universal" - type: "Joint" - } + + // \todo Uncomment the following items once they are supported + // ListElement { + // text: "Mesh" + // type: "Link" + // } + // ListElement { + // text: "Ball" + // type: "Joint" + // } + // ListElement { + // text: "Continuous" + // type: "Joint" + // } + // ListElement { + // text: "Fixed" + // type: "Joint" + // } + // ListElement { + // text: "Prismatic" + // type: "Joint" + // } + // ListElement { + // text: "Revolute" + // type: "Joint" + // } + // ListElement { + // text: "Universal" + // type: "Joint" + // } } // The delegate for each section header Component { @@ -323,7 +325,6 @@ Rectangle { section.criteria: ViewSection.FullString section.delegate: sectionHeading } - // MenuItem { text: "Box" onTriggered: {} } } diff --git a/src/gui/plugins/model_editor/ModelEditor.cc b/src/gui/plugins/model_editor/ModelEditor.cc index 0ebb4bf62e..b8bf80c8e0 100644 --- a/src/gui/plugins/model_editor/ModelEditor.cc +++ b/src/gui/plugins/model_editor/ModelEditor.cc @@ -16,23 +16,17 @@ */ #include -#include +#include #include #include #include #include #include #include -#include - -#include -#include #include #include - -#include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" @@ -45,38 +39,41 @@ namespace ignition::gazebo { - class ModelEditorPrivate + class EntityToAdd { - public: void Initialize(); + /// \brief Entity to add to the model editor + public: std::string geomOrLightType; + + /// \brief Type of entity to add + public: std::string entityType; + /// \brief Name of parent entity to add the entity to + public: std::string parentName; + }; + + class ModelEditorPrivate + { + /// \brief Handle entity addition + /// \param[in] _geomOrLightType Geometry or light type, e.g. sphere, directional, etc + /// \param[in] _entityType Type of entity: link, visual, collision, etc + /// \param[in] _parentName Name of parent entity public: void HandleAddEntity(const std::string &_geomOrLightType, - const std::string &_entityType, const std::string &/*_parent*/); + const std::string &_entityType, const std::string &_parentName); + /// \brief Get a SDF string of a geometry + /// \param[in] _geomType Type of geometry public: std::string GeomSDFString( const std::string &_geomType) const; + /// \brief Get a SDF string of a light + /// \param[in] _lightType Type of light public: std::string LightSDFString( - const std::string &_geomType) const; - + const std::string &_lightType) const; + /// \brief Get a SDF string of a link + /// \param[in] _geomOrLightType Type of light or geometry public: std::string LinkSDFString( - const std::string &_geomType) const; - - /// \brief Generate a unique entity id. - /// \return The unique entity id - // public: Entity UniqueId() const; - - //// \brief Pointer to the rendering scene - public: rendering::ScenePtr scene = nullptr; - - /// \brief Entity to add to the model editor - public: std::string geomOrLightType; - - /// \brief Type of entity to add - public: std::string entityType; - - /// \brief Parent entity to add the entity to - public: std::string parentEntity; + const std::string &_geomOrLightType) const; /// \brief Entity Creator API. public: std::unique_ptr entityCreator{nullptr}; @@ -88,8 +85,9 @@ namespace ignition::gazebo /// \brief Mutex to protect the entity sdf list public: std::mutex mutex; - /// \brief Links to add to the ECM - public: std::vector linksToAdd; + /// \brief A map of links to add to the ECM and the parent entity names + // public: std::vector> linksToAdd; + public: std::vector entitiesToAdd; /// \brief Event Manager public: EventManager eventMgr; @@ -126,71 +124,84 @@ void ModelEditor::Update(const UpdateInfo &, if (!this->dataPtr->entityCreator) { - // create entities in ECM on the GUI side. - // Note we have to start the entity id at an offset so it does not conflict - // with the ones on the server. The log playback starts at max / 2 - // On the gui side, we will start entity id at an offset of max / 4 - // todo(anyone) set a better entity create offset -// _ecm.SetEntityCreateOffset(math::MAX_I64 / 4); this->dataPtr->entityCreator = std::make_unique( _ecm, this->dataPtr->eventMgr); } - std::lock_guard lock(this->dataPtr->mutex); // add link entities to the ECM std::set newEntities; - for (auto & linkSdf : this->dataPtr->linksToAdd) + for (auto & eta: this->dataPtr->entitiesToAdd) { - Entity parent = _ecm.EntityByComponents( - components::Model(), components::Name(this->dataPtr->parentEntity)); - if (parent == kNullEntity) + sdf::Link linkSdf; + if (eta.entityType == "link") { - ignerr << "Unable to find " << this->dataPtr->parentEntity - << " in the ECM. " << std::endl; - continue; - } - - // generate unique link name - std::string linkName = "link"; - Entity linkEnt = _ecm.EntityByComponents( - /*components::Link(),*/ components::ParentEntity(parent), - components::Name(linkName)); - int64_t counter = 0; - while (linkEnt) - { - linkName = std::string("link") + "_" + std::to_string(++counter); - linkEnt = _ecm.EntityByComponents( - /*components::Link(),*/ components::ParentEntity(parent), - components::Name(linkName)); - } - - linkSdf.SetName(linkName); - auto entity = this->dataPtr->entityCreator->CreateEntities(&linkSdf); - this->dataPtr->entityCreator->SetParent(entity, parent); - - // traverse the tree and add all new entities created by the entity creator - // to the set - std::list entities; - entities.push_back(entity); - while (!entities.empty()) - { - Entity ent = entities.front(); - entities.pop_front(); - - // add new entity created - newEntities.insert(ent); - - auto childEntities = _ecm.EntitiesByComponents( - components::ParentEntity(ent)); - for (const auto &child : childEntities) - entities.push_back(child); + // create an sdf::Link to it can be added to the ECM throught the + // CreateEntities call + std::string linkSDFStr = this->dataPtr->LinkSDFString(eta.geomOrLightType); + if (!linkSDFStr.empty()) + { + linkSDFStr = std::string("" + + linkSDFStr + ""; + + sdf::ElementPtr linkElem(new sdf::Element); + sdf::initFile("link.sdf", linkElem); + sdf::readString(linkSDFStr, linkElem); + linkSdf.Load(linkElem); + } + else + { + continue; + } + Entity parent = _ecm.EntityByComponents( + components::Model(), components::Name(eta.parentName)); + if (parent == kNullEntity) + { + ignerr << "Unable to find " << eta.parentName << " in the ECM. " + << std::endl; + continue; + } + + // generate unique link name + // note passing components::Link() as arg to EntityByComponents causes + // a crash on exit, see issue #1158 + std::string linkName = "link"; + Entity linkEnt = _ecm.EntityByComponents( + /*components::Link(),*/ components::ParentEntity(parent), + components::Name(linkName)); + int64_t counter = 0; + while (linkEnt) + { + linkName = std::string("link") + "_" + std::to_string(++counter); + linkEnt = _ecm.EntityByComponents( + /*components::Link(),*/ components::ParentEntity(parent), + components::Name(linkName)); + } + + linkSdf.SetName(linkName); + auto entity = this->dataPtr->entityCreator->CreateEntities(&linkSdf); + this->dataPtr->entityCreator->SetParent(entity, parent); + + // traverse the tree and add all new entities created by the entity creator + // to the set + std::list entities; + entities.push_back(entity); + while (!entities.empty()) + { + Entity ent = entities.front(); + entities.pop_front(); + + // add new entity created + newEntities.insert(ent); + + auto childEntities = _ecm.EntitiesByComponents( + components::ParentEntity(ent)); + for (const auto &child : childEntities) + entities.push_back(child); + } } } - for (const auto & ent: newEntities) - std::cerr << "creating entity in ecm " << ent << std::endl; - // use tmp AddedRemovedEntities event to update other gui plugins // note this event will be removed in Ignition Garden std::set removedEntities; @@ -200,7 +211,7 @@ void ModelEditor::Update(const UpdateInfo &, ignition::gui::App()->findChild(), &event); - this->dataPtr->linksToAdd.clear(); + this->dataPtr->entitiesToAdd.clear(); } ///////////////////////////////////////////////// @@ -211,60 +222,16 @@ bool ModelEditor::eventFilter(QObject *_obj, QEvent *_event) auto event = reinterpret_cast(_event); if (event) { - this->dataPtr->geomOrLightType = event->Entity().toStdString(); - this->dataPtr->entityType = event->EntityType().toStdString(); - this->dataPtr->parentEntity = event->ParentEntity().toStdString(); - - std::cerr << "model editor add entity event " << event->EntityType().toStdString() - << " " << this->dataPtr->geomOrLightType << std::endl; - - this->dataPtr->HandleAddEntity(this->dataPtr->geomOrLightType, - this->dataPtr->entityType, - this->dataPtr->parentEntity); - + this->dataPtr->HandleAddEntity(event->Entity().toStdString(), + event->EntityType().toStdString(), + event->ParentEntity().toStdString()); } } - else if (_event->type() == ignition::gui::events::Render::kType) - { - // initialize rendering - this->dataPtr->Initialize(); - - // do something in rendering thread - - } - // Standard event processing return QObject::eventFilter(_obj, _event); } -///////////////////////////////////////////////// -void ModelEditorPrivate::Initialize() -{ - if (nullptr == this->scene) - { - this->scene = rendering::sceneFromFirstRenderEngine(); - if (nullptr == this->scene) - { - return; - } -// this->sceneManager.SetScene(this->scene); - } -} - -///////////////////////////////////////////////// -//Entity ModelEditorPrivate::UniqueId() -//{ -// auto timeout = 100000u; -// for (auto i = 0u; i < timeout; ++i) -// { -// Entity id = std::numeric_limits::max() - i; -// if (!this->sceneManager.HasEntity(id)) -// return id; -// } -// return kNullEntity; -//} - ///////////////////////////////////////////////// std::string ModelEditorPrivate::LightSDFString( const std::string &_lightType) const @@ -389,8 +356,8 @@ std::string ModelEditorPrivate::LinkSDFString( } std::string geomOrLightStr; - if (_geomOrLightType == "spot" || geomOrLightType == "directional" || - geomOrLightType == "point") + if (_geomOrLightType == "spot" || _geomOrLightType == "directional" || + _geomOrLightType == "point") { geomOrLightStr = this->LightSDFString(_geomOrLightType); linkStr @@ -418,56 +385,19 @@ std::string ModelEditorPrivate::LinkSDFString( return linkStr.str(); } - ///////////////////////////////////////////////// void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, - const std::string &_type, const std::string &/*_parentEntity*/) + const std::string &_type, const std::string &_parentName) { + std::lock_guard lock(this->mutex); std::string entType = common::lowercase(_type); std::string geomLightType = common::lowercase(_geomOrLightType); - if (entType == "link") - { - // auto model = this->scene->VisualByName(parentEntity); - // if (!model) - // { - // ignerr << "Unable to add link to model. " - // << "Parent entity: '" << parentEntity << "' not found. " - // << std::endl; - // } - - // auto link = this->scene->CreateVisual(); - // auto visual = this->scene->CreateVisual(); - - // rendering::GeometryPtr geom; - // if (entity == "box") - // geom = this->scene->CreateBox(); - // else if (entity == "cylinder") - // geom = this->scene->CreateCylinder(); - // else if (entity == "sphere") - // geom = this->scene->CreateSphere(); - - // visual->AddGeometry(geom); - // link->AddChild(visual); - // model->AddChild(link); - - // create an sdf::Link to it can be added to the ECM throught the - // CreateEntities call - std::string linkSDFStr = this->LinkSDFString(geomLightType); - if (!linkSDFStr.empty()) - { - linkSDFStr = std::string("" + - linkSDFStr + ""; - sdf::ElementPtr linkElem(new sdf::Element); - sdf::initFile("link.sdf", linkElem); - sdf::readString(linkSDFStr, linkElem); - sdf::Link linkSdf; - linkSdf.Load(linkElem); - - std::lock_guard lock(this->mutex); - this->linksToAdd.push_back(linkSdf); - } - } + EntityToAdd eta; + eta.entityType = entType; + eta.geomOrLightType = geomLightType; + eta.parentName = _parentName; + this->entitiesToAdd.push_back(eta); } // Register this plugin diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 1322d642e6..d607b7cfa1 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1533,8 +1533,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::Pose *_pose, const components::ParentEntity *_parent)->bool { - std::cerr << "new link first udpate " << _entity << " " << _name->Data() << std::endl; - this->CreateLink(_ecm, _entity, _name, _pose, _parent); // sdf::Link link; @@ -1858,17 +1856,6 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::Pose *_pose, const components::ParentEntity *_parent)->bool { - // std::cerr << "got new link " << _entity << " " << _name->Data() << std::endl; - // sdf::Link link; - // link.SetName(_name->Data()); - // link.SetRawPose(_pose->Data()); - // this->newLinks.push_back( - // std::make_tuple(_entity, link, _parent->Data())); - // // used for collsions - // this->modelToLinkEntities[_parent->Data()].push_back(_entity); - // // used for joints - // this->matchLinksWithEntities[_parent->Data()][_name->Data()] = - // _entity; this->CreateLink(_ecm, _entity, _name, _pose, _parent); return true; }); @@ -3524,7 +3511,6 @@ void RenderUtilPrivate::CreateLink( const components::Pose *_pose, const components::ParentEntity *_parent) { - std::cerr << "create link " << _entity << " " << _name << std::endl; sdf::Link link; link.SetName(_name->Data()); link.SetRawPose(_pose->Data()); @@ -3549,7 +3535,6 @@ void RenderUtilPrivate::CreateVisual( const components::VisibilityFlags *_visibilityFlags, const components::ParentEntity *_parent) { - std::cerr << "create visual " << _entity << " " << _name << std::endl; sdf::Visual visual; visual.SetName(_name->Data()); visual.SetRawPose(_pose->Data()); From fbe09f1775939c46abfd60709461d429dc89d454 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 4 Nov 2021 13:39:12 -0700 Subject: [PATCH 14/25] remove commented out code, add CreateLight function Signed-off-by: Ian Chen --- .../ComponentInspector.qml | 8 ++ .../plugins/scene_manager/GzSceneManager.cc | 14 --- src/rendering/RenderUtil.cc | 108 ++++++------------ 3 files changed, 43 insertions(+), 87 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 3259c348d0..4ebd16302f 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -249,6 +249,14 @@ Rectangle { text: "Sphere" type: "Link" } + ListElement { + text: "Capsule" + type: "Link" + } + ListElement { + text: "Ellipsoid" + type: "Link" + } ListElement { text: "Directional" type: "Light" diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index eb9274797f..fdab1261ea 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -92,28 +92,14 @@ void GzSceneManager::Update(const UpdateInfo &_info, this->dataPtr->renderUtil.UpdateECM(_info, _ecm); -// std::set tmpNewEntities; std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); { this->dataPtr->renderUtil.CreateVisualsForEntities(_ecm, this->dataPtr->newEntities); - // for (const auto &ent : this->dataPtr->newEntities) - // { - // if (!_ecm.IsNewEntity(ent)) - // _ecm.MarkEntityAsNew(ent, true); - // else - // tmpNewEntities.insert(ent); - // } this->dataPtr->newEntities.clear(); } this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); - -// for (const auto &ent : tmpNewEntities) -// { -// _ecm.MarkEntityAsNew(ent, false); -// } - } ///////////////////////////////////////////////// diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index d607b7cfa1..ced666628f 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -179,6 +179,19 @@ class ignition::gazebo::RenderUtilPrivate const components::VisibilityFlags *_visibilityFlags, const components::ParentEntity *_parent); + /// \brief Helper function to create a visual for a light entity + /// \param[in] _ecm The entity-component manager + /// \param[in] _entity Entity to create the visual for + /// \param[in] _light Light component + /// \param[in] _name Name component + /// \param[in] _parent ParentEntity component + public: void CreateLight( + const EntityComponentManager &_ecm, + const Entity &_entity, + const components::Light *_light, + const components::Name *_name, + const components::ParentEntity *_parent); + /// \brief Total time elapsed in simulation. This will not increase while /// paused. public: std::chrono::steady_clock::duration simTime{0}; @@ -1534,18 +1547,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::ParentEntity *_parent)->bool { this->CreateLink(_ecm, _entity, _name, _pose, _parent); - - // sdf::Link link; - // link.SetName(_name->Data()); - // link.SetRawPose(_pose->Data()); - // this->newLinks.push_back( - // std::make_tuple(_entity, link, _parent->Data())); - // // used for collsions - // this->modelToLinkEntities[_parent->Data()].push_back(_entity); - // // used for joints - // this->matchLinksWithEntities[_parent->Data()][_name->Data()] = - // _entity; - return true; }); @@ -1568,63 +1569,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( { this->CreateVisual(_ecm, _entity, _name, _pose, _geom, _castShadows, _transparency, _visibilityFlags, _parent); - - // sdf::Visual visual; - // visual.SetName(_name->Data()); - // visual.SetRawPose(_pose->Data()); - // visual.SetGeom(_geom->Data()); - // visual.SetCastShadows(_castShadows->Data()); - // visual.SetTransparency(_transparency->Data()); - // visual.SetVisibilityFlags(_visibilityFlags->Data()); - - // // Optional components - // auto material = _ecm.Component(_entity); - // if (material != nullptr) - // { - // visual.SetMaterial(material->Data()); - // } - - // auto laserRetro = _ecm.Component(_entity); - // if (laserRetro != nullptr) - // { - // visual.SetLaserRetro(laserRetro->Data()); - // } - - // // set label - // auto label = _ecm.Component(_entity); - // if (label != nullptr) - // { - // this->entityLabel[_entity] = label->Data(); - // } - - // if (auto temp = _ecm.Component(_entity)) - // { - // // get the uniform temperature for the entity - // this->entityTemp[_entity] = std::make_tuple - // (temp->Data().Kelvin(), 0.0, ""); - // } - // else - // { - // // entity doesn't have a uniform temperature. Check if it has - // // a heat signature with an associated temperature range - // auto heatSignature = - // _ecm.Component(_entity); - // auto tempRange = - // _ecm.Component(_entity); - // if (heatSignature && tempRange) - // { - // this->entityTemp[_entity] = - // std::make_tuple( - // tempRange->Data().min.Kelvin(), - // tempRange->Data().max.Kelvin(), - // std::string(heatSignature->Data())); - // } - // } - - // this->newVisuals.push_back( - // std::make_tuple(_entity, visual, _parent->Data())); - - // this->linkToVisualEntities[_parent->Data()].push_back(_entity); return true; }); @@ -1655,8 +1599,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::Name *_name, const components::ParentEntity *_parent) -> bool { - this->newLights.push_back(std::make_tuple(_entity, _light->Data(), - _name->Data(), _parent->Data())); + this->CreateLight(_ecm, _entity, _light, _name, _parent); return true; }); @@ -1967,8 +1910,7 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::Name *_name, const components::ParentEntity *_parent) -> bool { - this->newLights.push_back(std::make_tuple(_entity, _light->Data(), - _name->Data(), _parent->Data())); + this->CreateLight(_ecm, _entity, _light, _name, _parent); return true; }); @@ -3500,6 +3442,15 @@ void RenderUtil::CreateVisualsForEntities( _ecm.Component(ent)); continue; } + auto lightComp = _ecm.Component(ent); + if (lightComp) + { + this->dataPtr->CreateLight(_ecm, ent, + _ecm.Component(ent), + _ecm.Component(ent), + _ecm.Component(ent)); + continue; + } } } @@ -3593,3 +3544,14 @@ void RenderUtilPrivate::CreateVisual( this->linkToVisualEntities[_parent->Data()].push_back(_entity); } +///////////////////////////////////////////////// +void RenderUtilPrivate::CreateLight( + const EntityComponentManager &/*_ecm*/, + const Entity &_entity, + const components::Light *_light, + const components::Name *_name, + const components::ParentEntity *_parent) +{ + this->newLights.push_back(std::make_tuple(_entity, _light->Data(), + _name->Data(), _parent->Data())); +} From b7957c8e1d63e47808e186a588fada1e6fcaea09 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 4 Nov 2021 16:03:52 -0700 Subject: [PATCH 15/25] add model editor src files Signed-off-by: Ian Chen --- src/gui/plugins/model_editor/CMakeLists.txt | 3 --- src/gui/plugins/model_editor/ModelEditor.hh | 6 ++++-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/gui/plugins/model_editor/CMakeLists.txt b/src/gui/plugins/model_editor/CMakeLists.txt index 39f351015e..42650ac9b5 100644 --- a/src/gui/plugins/model_editor/CMakeLists.txt +++ b/src/gui/plugins/model_editor/CMakeLists.txt @@ -1,7 +1,4 @@ gz_add_gui_plugin(ModelEditor SOURCES ModelEditor.cc QT_HEADERS ModelEditor.hh - PUBLIC_LINK_LIBS - ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} - ${PROJECT_LIBRARY_TARGET_NAME}-rendering ) diff --git a/src/gui/plugins/model_editor/ModelEditor.hh b/src/gui/plugins/model_editor/ModelEditor.hh index 94fcde7245..f089b4498c 100644 --- a/src/gui/plugins/model_editor/ModelEditor.hh +++ b/src/gui/plugins/model_editor/ModelEditor.hh @@ -37,13 +37,15 @@ namespace gazebo /// \brief /// - /// ## Configuration - /// None + /// Model Editor gui plugin class ModelEditor : public gazebo::GuiSystem { Q_OBJECT + /// \brief Constructor public: ModelEditor(); + + /// \brief Destructor public: ~ModelEditor() override; // Documentation inherited From 493d155ba86a9730267dce7894603c88e3b9ac21 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 4 Nov 2021 16:12:26 -0700 Subject: [PATCH 16/25] remove more commented out code Signed-off-by: Ian Chen --- .../ignition/gazebo/EntityComponentManager.hh | 7 --- src/rendering/RenderUtil.cc | 57 ------------------- 2 files changed, 64 deletions(-) diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 1de283c9a5..a98c2b854b 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -669,13 +669,6 @@ namespace ignition /// \param[in] _offset Offset value. public: void SetEntityCreateOffset(uint64_t _offset); - /// \internal - /// \brief Mark whether or not an entity is a newly created entity. - /// - /// \param[in] _entity Entity id to mark as new - /// \return True to mark it as new, false to mark it as not new - // public: void MarkEntityAsNew(const Entity _entity, bool _new = true); - /// \brief Clear the list of newly added entities so that a call to /// EachAdded after this will have no entities to iterate. This function /// is protected to facilitate testing. diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index ced666628f..5105930a45 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1822,63 +1822,6 @@ void RenderUtilPrivate::CreateEntitiesRuntime( { this->CreateVisual(_ecm, _entity, _name, _pose, _geom, _castShadows, _transparency, _visibilityFlags, _parent); - - // sdf::Visual visual; - // visual.SetName(_name->Data()); - // visual.SetRawPose(_pose->Data()); - // visual.SetGeom(_geom->Data()); - // visual.SetCastShadows(_castShadows->Data()); - // visual.SetTransparency(_transparency->Data()); - // visual.SetVisibilityFlags(_visibilityFlags->Data()); - - // // Optional components - // auto material = _ecm.Component(_entity); - // if (material != nullptr) - // { - // visual.SetMaterial(material->Data()); - // } - - // auto laserRetro = _ecm.Component(_entity); - // if (laserRetro != nullptr) - // { - // visual.SetLaserRetro(laserRetro->Data()); - // } - - // // set label - // auto label = _ecm.Component(_entity); - // if (label != nullptr) - // { - // this->entityLabel[_entity] = label->Data(); - // } - - // if (auto temp = _ecm.Component(_entity)) - // { - // // get the uniform temperature for the entity - // this->entityTemp[_entity] = std::make_tuple - // (temp->Data().Kelvin(), 0.0, ""); - // } - // else - // { - // // entity doesn't have a uniform temperature. Check if it has - // // a heat signature with an associated temperature range - // auto heatSignature = - // _ecm.Component(_entity); - // auto tempRange = - // _ecm.Component(_entity); - // if (heatSignature && tempRange) - // { - // this->entityTemp[_entity] = - // std::make_tuple( - // tempRange->Data().min.Kelvin(), - // tempRange->Data().max.Kelvin(), - // std::string(heatSignature->Data())); - // } - // } - - // this->newVisuals.push_back( - // std::make_tuple(_entity, visual, _parent->Data())); - - // this->linkToVisualEntities[_parent->Data()].push_back(_entity); return true; }); From 26965e396f5331c97edec6d873435da5ece4534f Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 8 Nov 2021 15:39:09 -0800 Subject: [PATCH 17/25] use entity instead of entity name (#1176) Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- include/ignition/gazebo/gui/GuiEvents.hh | 8 ++--- .../component_inspector/ComponentInspector.cc | 2 +- src/gui/plugins/model_editor/ModelEditor.cc | 29 +++++++++---------- 3 files changed, 18 insertions(+), 21 deletions(-) diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 0d2bf90952..97cc682921 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -165,8 +165,8 @@ namespace events /// \brief Constructor /// \param[in] _tranformModeActive is the transform control mode active public: explicit ModelEditorAddEntity(QString _entity, QString _type, - QString _parent) : QEvent(kType), entity(_entity), type(_type), - parent(_parent) + ignition::gazebo::Entity _parent) : QEvent(kType), entity(_entity), + type(_type), parent(_parent) { } @@ -183,7 +183,7 @@ namespace events } /// \brief Get the parent entity to add the entity to - public: QString ParentEntity() const + public: ignition::gazebo::Entity ParentEntity() const { return this->parent; } @@ -193,7 +193,7 @@ namespace events private: QString entity; private: QString type; - private: QString parent; + private: ignition::gazebo::Entity parent; }; } // namespace events diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index a3a7844a2a..80384bd482 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -1027,7 +1027,7 @@ void ComponentInspector::OnAddEntity(QString _entity, QString _type) // currently just assumes parent is the model // todo(anyone) support adding visuals / collisions / sensors to links ignition::gazebo::gui::events::ModelEditorAddEntity addEntityEvent( - _entity, _type, QString(this->dataPtr->entityName.c_str())); + _entity, _type, this->dataPtr->entity); ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &addEntityEvent); diff --git a/src/gui/plugins/model_editor/ModelEditor.cc b/src/gui/plugins/model_editor/ModelEditor.cc index b8bf80c8e0..aaf438ad72 100644 --- a/src/gui/plugins/model_editor/ModelEditor.cc +++ b/src/gui/plugins/model_editor/ModelEditor.cc @@ -47,8 +47,8 @@ namespace ignition::gazebo /// \brief Type of entity to add public: std::string entityType; - /// \brief Name of parent entity to add the entity to - public: std::string parentName; + /// \brief Parent entity to add the entity to + public: Entity parentEntity; }; class ModelEditorPrivate @@ -56,9 +56,9 @@ namespace ignition::gazebo /// \brief Handle entity addition /// \param[in] _geomOrLightType Geometry or light type, e.g. sphere, directional, etc /// \param[in] _entityType Type of entity: link, visual, collision, etc - /// \param[in] _parentName Name of parent entity + /// \param[in] _parentEntity Name of parent entity public: void HandleAddEntity(const std::string &_geomOrLightType, - const std::string &_entityType, const std::string &_parentName); + const std::string &_entityType, Entity _parentEntity); /// \brief Get a SDF string of a geometry /// \param[in] _geomType Type of geometry @@ -153,12 +153,9 @@ void ModelEditor::Update(const UpdateInfo &, { continue; } - Entity parent = _ecm.EntityByComponents( - components::Model(), components::Name(eta.parentName)); - if (parent == kNullEntity) + if (eta.parentEntity == kNullEntity) { - ignerr << "Unable to find " << eta.parentName << " in the ECM. " - << std::endl; + ignerr << "Parent entity not defined." << std::endl; continue; } @@ -167,20 +164,20 @@ void ModelEditor::Update(const UpdateInfo &, // a crash on exit, see issue #1158 std::string linkName = "link"; Entity linkEnt = _ecm.EntityByComponents( - /*components::Link(),*/ components::ParentEntity(parent), - components::Name(linkName)); + components::ParentEntity(eta.parentEntity), + components::Name(linkName)); int64_t counter = 0; while (linkEnt) { linkName = std::string("link") + "_" + std::to_string(++counter); linkEnt = _ecm.EntityByComponents( - /*components::Link(),*/ components::ParentEntity(parent), + components::ParentEntity(eta.parentEntity), components::Name(linkName)); } linkSdf.SetName(linkName); auto entity = this->dataPtr->entityCreator->CreateEntities(&linkSdf); - this->dataPtr->entityCreator->SetParent(entity, parent); + this->dataPtr->entityCreator->SetParent(entity, eta.parentEntity); // traverse the tree and add all new entities created by the entity creator // to the set @@ -224,7 +221,7 @@ bool ModelEditor::eventFilter(QObject *_obj, QEvent *_event) { this->dataPtr->HandleAddEntity(event->Entity().toStdString(), event->EntityType().toStdString(), - event->ParentEntity().toStdString()); + event->ParentEntity()); } } @@ -387,7 +384,7 @@ std::string ModelEditorPrivate::LinkSDFString( ///////////////////////////////////////////////// void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, - const std::string &_type, const std::string &_parentName) + const std::string &_type, Entity _parentEntity) { std::lock_guard lock(this->mutex); std::string entType = common::lowercase(_type); @@ -396,7 +393,7 @@ void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, EntityToAdd eta; eta.entityType = entType; eta.geomOrLightType = geomLightType; - eta.parentName = _parentName; + eta.parentEntity = _parentEntity; this->entitiesToAdd.push_back(eta); } From 539295cf1250b5a81d3df74099a7f56b0557ddba Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 8 Nov 2021 16:13:39 -0800 Subject: [PATCH 18/25] Add link menu updates (#1177) * use entity instead of entity name Signed-off-by: Nate Koenig * Update link add menu Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig Co-authored-by: Ian Chen --- include/ignition/gazebo/gui/GuiEvents.hh | 11 +- .../component_inspector/ComponentInspector.cc | 31 ++- .../component_inspector/ComponentInspector.hh | 10 +- .../ComponentInspector.qml | 251 ++++++++++-------- src/gui/plugins/entity_tree/EntityTree.hh | 4 +- src/gui/plugins/entity_tree/EntityTree.qml | 32 +++ src/gui/plugins/model_editor/ModelEditor.cc | 82 +++--- 7 files changed, 275 insertions(+), 146 deletions(-) diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 97cc682921..f14f205da4 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -165,8 +165,8 @@ namespace events /// \brief Constructor /// \param[in] _tranformModeActive is the transform control mode active public: explicit ModelEditorAddEntity(QString _entity, QString _type, - ignition::gazebo::Entity _parent) : QEvent(kType), entity(_entity), - type(_type), parent(_parent) + ignition::gazebo::Entity _parent, QString _uri) : QEvent(kType), entity(_entity), + type(_type), parent(_parent), uri(_uri) { } @@ -176,6 +176,12 @@ namespace events return this->entity; } + /// \brief Get the URI, if any, associated with the entity to add + public: QString Uri() const + { + return this->uri; + } + /// \brief Get the entity type public: QString EntityType() const { @@ -194,6 +200,7 @@ namespace events private: QString entity; private: QString type; private: ignition::gazebo::Entity parent; + private: QString uri; }; } // namespace events diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 80384bd482..59f4246e40 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -1022,17 +1023,43 @@ bool ComponentInspector::NestedModel() const } ///////////////////////////////////////////////// -void ComponentInspector::OnAddEntity(QString _entity, QString _type) +void ComponentInspector::OnAddEntity(const QString &_entity, + const QString &_type) { // currently just assumes parent is the model // todo(anyone) support adding visuals / collisions / sensors to links ignition::gazebo::gui::events::ModelEditorAddEntity addEntityEvent( - _entity, _type, this->dataPtr->entity); + _entity, _type, this->dataPtr->entity, QString("")); ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &addEntityEvent); } +///////////////////////////////////////////////// +void ComponentInspector::OnLoadMesh(const QString &_entity, + const QString &_type, const QString &_mesh) +{ + std::string meshStr = _mesh.toStdString(); + if (QUrl(_mesh).isLocalFile()) + { + // mesh to sdf model + common::rtrim(meshStr); + + if (!common::MeshManager::Instance()->IsValidFilename(meshStr)) + { + QString errTxt = QString::fromStdString("Invalid URI: " + meshStr + + "\nOnly mesh file types DAE, OBJ, and STL are supported."); + return; + } + + ignition::gazebo::gui::events::ModelEditorAddEntity addEntityEvent( + _entity, _type, this->dataPtr->entity, QString(meshStr.c_str())); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &addEntityEvent); + } +} + // Register this plugin IGNITION_ADD_PLUGIN(ignition::gazebo::ComponentInspector, ignition::gui::Plugin) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 8a1dea92cc..1790c3b521 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -334,7 +334,15 @@ namespace gazebo /// \brief Callback in Qt thread when an entity is to be added /// \param[in] _entity Entity to add, e.g. box, sphere, cylinder, etc /// \param[in] _type Entity type, e.g. link, visual, collision, etc - public: Q_INVOKABLE void OnAddEntity(QString _entity, QString _type); + public: Q_INVOKABLE void OnAddEntity(const QString &_entity, + const QString &_type); + + /// \brief Callback to insert a new entity + /// \param[in] _entity Entity to add, e.g. box, sphere, cylinder, etc + /// \param[in] _type Entity type, e.g. link, visual, collision, etc + /// \param[in] _mesh Mesh file to load. + public: Q_INVOKABLE void OnLoadMesh(const QString &_entity, + const QString &_type, const QString &_mesh); /// \internal /// \brief Pointer to private data. diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 4ebd16302f..10f9451dfa 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -18,6 +18,7 @@ import QtQuick 2.9 import QtQuick.Controls 1.4 import QtQuick.Controls 2.2 import QtQuick.Controls.Material 2.1 +import QtQuick.Dialogs 1.0 import QtQuick.Layouts 1.3 import QtQuick.Controls.Styles 1.4 import IgnGazebo 1.0 as IgnGazebo @@ -125,6 +126,20 @@ Rectangle { _heading); } + // The component for a menu section header + Component { + id: menuSectionHeading + Rectangle { + height: childrenRect.height + + Text { + text: sectionText + font.pointSize: 10 + padding: 5 + } + } + } + Rectangle { id: header height: lockButton.height @@ -218,6 +233,138 @@ Rectangle { onClicked: { addLinkMenu.open() } + + FileDialog { + id: loadFileDialog + title: "Load mesh" + folder: shortcuts.home + nameFilters: [ "Collada files (*.dae)", "(*.stl)", "(*.obj)" ] + selectMultiple: false + selectExisting: true + onAccepted: { + ComponentInspector.OnLoadMesh("mesh", "link", fileUrl) + } + } + + Menu { + id: addLinkMenu + + Item { + Layout.fillWidth: true + height: childrenRect.height + Loader { + property string sectionText: "Link" + sourceComponent: menuSectionHeading + } + } + + MenuItem { + id: boxLink + text: "Box" + onClicked: { + ComponentInspector.OnAddEntity("box", "link"); + addLinkMenu.close() + } + } + + MenuItem { + id: capsuleLink + text: "Capsule" + onClicked: { + ComponentInspector.OnAddEntity("capsule", "link"); + addLinkMenu.close() + } + } + + MenuItem { + id: cylinderLink + text: "Cylinder" + onClicked: { + ComponentInspector.OnAddEntity("cylinder", "link"); + } + } + + MenuItem { + id: ellipsoidLink + text: "Ellipsoid" + onClicked: { + ComponentInspector.OnAddEntity("ellipsoid", "link"); + } + } + + MenuItem { + id: emptyLink + text: "Empty" + onClicked: { + ComponentInspector.OnAddEntity("empty", "link"); + } + } + + MenuItem { + id: meshLink + text: "Mesh" + onClicked: { + loadFileDialog.open() + } + } + + MenuItem { + id: sphereLink + text: "Sphere" + onClicked: { + ComponentInspector.OnAddEntity("sphere", "link"); + } + } + + MenuSeparator { + padding: 0 + topPadding: 12 + bottomPadding: 12 + contentItem: Rectangle { + implicitWidth: 200 + implicitHeight: 1 + color: "#1E000000" + } + } + + Item { + Layout.fillWidth: true + height: childrenRect.height + Loader { + property string sectionText: "Light" + sourceComponent: menuSectionHeading + } + } + + MenuItem { + id: directionalLink + text: "Directional" + onClicked: { + ComponentInspector.OnAddEntity("directional", "light"); + addLinkMenu.close() + } + } + + MenuItem { + id: pointLink + text: "Point" + onClicked: { + ComponentInspector.OnAddEntity("point", "light"); + addLinkMenu.close() + } + } + + MenuItem { + id: spotLink + text: "Spot" + onClicked: { + ComponentInspector.OnAddEntity("spot", "light"); + addLinkMenu.close() + } + } + + // \todo(anyone) Add joints + } } Label { @@ -231,110 +378,6 @@ Rectangle { } } - ListModel { - id: linkItems - ListElement { - text: "Box" - type: "Link" - } - ListElement { - text: "Cylinder" - type: "Link" - } - ListElement { - text: "Empty" - type: "Link" - } - ListElement { - text: "Sphere" - type: "Link" - } - ListElement { - text: "Capsule" - type: "Link" - } - ListElement { - text: "Ellipsoid" - type: "Link" - } - ListElement { - text: "Directional" - type: "Light" - } - ListElement { - text: "Spot" - type: "Light" - } - ListElement { - text: "Point" - type: "Light" - } - - // \todo Uncomment the following items once they are supported - // ListElement { - // text: "Mesh" - // type: "Link" - // } - // ListElement { - // text: "Ball" - // type: "Joint" - // } - // ListElement { - // text: "Continuous" - // type: "Joint" - // } - // ListElement { - // text: "Fixed" - // type: "Joint" - // } - // ListElement { - // text: "Prismatic" - // type: "Joint" - // } - // ListElement { - // text: "Revolute" - // type: "Joint" - // } - // ListElement { - // text: "Universal" - // type: "Joint" - // } - } - // The delegate for each section header - Component { - id: sectionHeading - Rectangle { - height: childrenRect.height - - Text { - text: section - font.pointSize: 10 - padding: 5 - } - } - } - - Menu { - id: addLinkMenu - ListView { - id: addLinkMenuListView - height: addLinkMenu.height - delegate: ItemDelegate { - width: parent.width - text: model.text - highlighted: ListView.isCurrentItem - onClicked: { - ComponentInspector.OnAddEntity(model.text, "link"); - addLinkMenu.close() - } - } - model: linkItems - section.property: "type" - section.criteria: ViewSection.FullString - section.delegate: sectionHeading - } - } - ListView { anchors.top: header.bottom diff --git a/src/gui/plugins/entity_tree/EntityTree.hh b/src/gui/plugins/entity_tree/EntityTree.hh index b899c12980..83d1ebe736 100644 --- a/src/gui/plugins/entity_tree/EntityTree.hh +++ b/src/gui/plugins/entity_tree/EntityTree.hh @@ -135,8 +135,8 @@ namespace gazebo public: Q_INVOKABLE void OnInsertEntity(const QString &_type); /// \brief Callback to insert a new entity - /// \param[in] _type Type of entity to insert - public: Q_INVOKABLE void OnLoadMesh(const QString &_type); + /// \param[in] _file Mesh file to create a model from. + public: Q_INVOKABLE void OnLoadMesh(const QString &_mesh); // Documentation inherited protected: bool eventFilter(QObject *_obj, QEvent *_event) override; diff --git a/src/gui/plugins/entity_tree/EntityTree.qml b/src/gui/plugins/entity_tree/EntityTree.qml index b0cf0d55a3..0737b6af31 100644 --- a/src/gui/plugins/entity_tree/EntityTree.qml +++ b/src/gui/plugins/entity_tree/EntityTree.qml @@ -101,6 +101,20 @@ Rectangle { } } + // The component for a menu section header + Component { + id: menuSectionHeading + Rectangle { + height: childrenRect.height + + Text { + text: sectionText + font.pointSize: 10 + padding: 5 + } + } + } + Rectangle { id: header visible: true @@ -153,6 +167,15 @@ Rectangle { Menu { id: addEntityMenu + Item { + Layout.fillWidth: true + height: childrenRect.height + Loader { + property string sectionText: "Model" + sourceComponent: menuSectionHeading + } + } + MenuItem { id: box @@ -218,6 +241,15 @@ Rectangle { } } + Item { + Layout.fillWidth: true + height: childrenRect.height + Loader { + property string sectionText: "Light" + sourceComponent: menuSectionHeading + } + } + MenuItem { id: directionalLight diff --git a/src/gui/plugins/model_editor/ModelEditor.cc b/src/gui/plugins/model_editor/ModelEditor.cc index aaf438ad72..3e303ef2d3 100644 --- a/src/gui/plugins/model_editor/ModelEditor.cc +++ b/src/gui/plugins/model_editor/ModelEditor.cc @@ -49,6 +49,9 @@ namespace ignition::gazebo /// \brief Parent entity to add the entity to public: Entity parentEntity; + + /// \brief Entity URI, such as a URI for a mesh. + public: std::string uri; }; class ModelEditorPrivate @@ -57,23 +60,23 @@ namespace ignition::gazebo /// \param[in] _geomOrLightType Geometry or light type, e.g. sphere, directional, etc /// \param[in] _entityType Type of entity: link, visual, collision, etc /// \param[in] _parentEntity Name of parent entity + /// \param[in] _uri URI associated with the entity, needed for mesh + /// types. public: void HandleAddEntity(const std::string &_geomOrLightType, - const std::string &_entityType, Entity _parentEntity); + const std::string &_entityType, Entity _parentEntity, + const std::string &_uri); /// \brief Get a SDF string of a geometry - /// \param[in] _geomType Type of geometry - public: std::string GeomSDFString( - const std::string &_geomType) const; + /// \param[in] _eta Entity to add. + public: std::string GeomSDFString(const EntityToAdd &_eta) const; /// \brief Get a SDF string of a light - /// \param[in] _lightType Type of light - public: std::string LightSDFString( - const std::string &_lightType) const; + /// \param[in] _eta Entity to add. + public: std::string LightSDFString(const EntityToAdd &_eta) const; /// \brief Get a SDF string of a link - /// \param[in] _geomOrLightType Type of light or geometry - public: std::string LinkSDFString( - const std::string &_geomOrLightType) const; + /// \param[in] _eta Entity to add. + public: std::string LinkSDFString(const EntityToAdd &_eta) const; /// \brief Entity Creator API. public: std::unique_ptr entityCreator{nullptr}; @@ -138,7 +141,7 @@ void ModelEditor::Update(const UpdateInfo &, { // create an sdf::Link to it can be added to the ECM throught the // CreateEntities call - std::string linkSDFStr = this->dataPtr->LinkSDFString(eta.geomOrLightType); + std::string linkSDFStr = this->dataPtr->LinkSDFString(eta); if (!linkSDFStr.empty()) { linkSDFStr = std::string("" + @@ -221,7 +224,8 @@ bool ModelEditor::eventFilter(QObject *_obj, QEvent *_event) { this->dataPtr->HandleAddEntity(event->Entity().toStdString(), event->EntityType().toStdString(), - event->ParentEntity()); + event->ParentEntity(), + event->Uri().toStdString()); } } @@ -230,20 +234,19 @@ bool ModelEditor::eventFilter(QObject *_obj, QEvent *_event) } ///////////////////////////////////////////////// -std::string ModelEditorPrivate::LightSDFString( - const std::string &_lightType) const +std::string ModelEditorPrivate::LightSDFString(const EntityToAdd &_eta) const { std::stringstream lightStr; - lightStr << ""; + lightStr << ""; - if (_lightType == "directional") + if (_eta.geomOrLightType == "directional") { lightStr << "false" << "1.0 1.0 1.0 1" << "0.5 0.5 0.5 1"; } - else if (_lightType == "spot") + else if (_eta.geomOrLightType == "spot") { lightStr << "false" @@ -262,7 +265,7 @@ std::string ModelEditorPrivate::LightSDFString( << "0.8" << ""; } - else if (_lightType == "point") + else if (_eta.geomOrLightType == "point") { lightStr << "false" @@ -277,7 +280,8 @@ std::string ModelEditorPrivate::LightSDFString( } else { - ignwarn << "Light type not supported: " << _lightType << std::endl; + ignwarn << "Light type not supported: " + << _eta.geomOrLightType << std::endl; return std::string(); } @@ -286,27 +290,26 @@ std::string ModelEditorPrivate::LightSDFString( } ///////////////////////////////////////////////// -std::string ModelEditorPrivate::GeomSDFString( - const std::string &_geomType) const +std::string ModelEditorPrivate::GeomSDFString(const EntityToAdd &_eta) const { math::Vector3d size = math::Vector3d::One; std::stringstream geomStr; geomStr << ""; - if (_geomType == "box") + if (_eta.geomOrLightType == "box") { geomStr << "" << " " << size << "" << ""; } - else if (_geomType == "sphere") + else if (_eta.geomOrLightType == "sphere") { geomStr << "" << " " << size.X() * 0.5 << "" << ""; } - else if (_geomType == "cylinder") + else if (_eta.geomOrLightType == "cylinder") { geomStr << "" @@ -314,7 +317,7 @@ std::string ModelEditorPrivate::GeomSDFString( << " " << size.Z() << "" << ""; } - else if (_geomType == "capsule") + else if (_eta.geomOrLightType == "capsule") { geomStr << "" @@ -322,16 +325,24 @@ std::string ModelEditorPrivate::GeomSDFString( << " " << size.Z() << "" << ""; } - else if (_geomType == "ellipsoid") + else if (_eta.geomOrLightType == "ellipsoid") { geomStr << "" << " " << size.X() * 0.5 << "" << ""; } + else if (_eta.geomOrLightType == "mesh") + { + geomStr + << "" + << " " << _eta.uri << "" + << ""; + } else { - ignwarn << "Geometry type not supported: " << _geomType << std::endl; + ignwarn << "Geometry type not supported: " + << _eta.geomOrLightType << std::endl; return std::string(); } @@ -341,22 +352,21 @@ std::string ModelEditorPrivate::GeomSDFString( } ///////////////////////////////////////////////// -std::string ModelEditorPrivate::LinkSDFString( - const std::string &_geomOrLightType) const +std::string ModelEditorPrivate::LinkSDFString(const EntityToAdd &_eta) const { std::stringstream linkStr; - if (_geomOrLightType == "empty") + if (_eta.geomOrLightType == "empty") { linkStr << ""; return linkStr.str(); } std::string geomOrLightStr; - if (_geomOrLightType == "spot" || _geomOrLightType == "directional" || - _geomOrLightType == "point") + if (_eta.geomOrLightType == "spot" || _eta.geomOrLightType == "directional" || + _eta.geomOrLightType == "point") { - geomOrLightStr = this->LightSDFString(_geomOrLightType); + geomOrLightStr = this->LightSDFString(_eta); linkStr << "" << geomOrLightStr @@ -364,7 +374,7 @@ std::string ModelEditorPrivate::LinkSDFString( } else { - geomOrLightStr = this->GeomSDFString(_geomOrLightType); + geomOrLightStr = this->GeomSDFString(_eta); linkStr << "" << " " @@ -384,7 +394,8 @@ std::string ModelEditorPrivate::LinkSDFString( ///////////////////////////////////////////////// void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, - const std::string &_type, Entity _parentEntity) + const std::string &_type, Entity _parentEntity, + const std::string &_uri) { std::lock_guard lock(this->mutex); std::string entType = common::lowercase(_type); @@ -394,6 +405,7 @@ void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, eta.entityType = entType; eta.geomOrLightType = geomLightType; eta.parentEntity = _parentEntity; + eta.uri = _uri; this->entitiesToAdd.push_back(eta); } From 1c4f59cfafb0da2242f927516a8feee674f0725a Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 8 Nov 2021 16:30:43 -0800 Subject: [PATCH 19/25] fix adding ellipsoid Signed-off-by: Ian Chen --- src/gui/plugins/model_editor/ModelEditor.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gui/plugins/model_editor/ModelEditor.cc b/src/gui/plugins/model_editor/ModelEditor.cc index 3e303ef2d3..d607aaba85 100644 --- a/src/gui/plugins/model_editor/ModelEditor.cc +++ b/src/gui/plugins/model_editor/ModelEditor.cc @@ -329,7 +329,7 @@ std::string ModelEditorPrivate::GeomSDFString(const EntityToAdd &_eta) const { geomStr << "" - << " " << size.X() * 0.5 << "" + << " " << size * 0.5 << "" << ""; } else if (_eta.geomOrLightType == "mesh") From 0ecc7319d1e74f1f41ee5599df73b6875b109bda Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 8 Nov 2021 18:34:51 -0800 Subject: [PATCH 20/25] merge model_editor into component_inspector Signed-off-by: Ian Chen --- src/gui/plugins/CMakeLists.txt | 1 - .../component_inspector/CMakeLists.txt | 4 +-- .../component_inspector/ComponentInspector.cc | 11 +++++++- .../ModelEditor.cc | 12 ++------ .../ModelEditor.hh | 25 +++++++---------- src/gui/plugins/entity_tree/EntityTree.cc | 13 ++++----- src/gui/plugins/model_editor/CMakeLists.txt | 4 --- src/gui/plugins/model_editor/ModelEditor.qml | 28 ------------------- src/gui/plugins/model_editor/ModelEditor.qrc | 5 ---- 9 files changed, 30 insertions(+), 73 deletions(-) rename src/gui/plugins/{model_editor => component_inspector}/ModelEditor.cc (97%) rename src/gui/plugins/{model_editor => component_inspector}/ModelEditor.hh (68%) delete mode 100644 src/gui/plugins/model_editor/CMakeLists.txt delete mode 100644 src/gui/plugins/model_editor/ModelEditor.qml delete mode 100644 src/gui/plugins/model_editor/ModelEditor.qrc diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index bbcafc434d..d74c63cca3 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -121,7 +121,6 @@ add_subdirectory(modules) add_subdirectory(align_tool) add_subdirectory(banana_for_scale) add_subdirectory(component_inspector) -add_subdirectory(model_editor) add_subdirectory(entity_context_menu) add_subdirectory(entity_tree) add_subdirectory(joint_position_controller) diff --git a/src/gui/plugins/component_inspector/CMakeLists.txt b/src/gui/plugins/component_inspector/CMakeLists.txt index 367278b24d..62200aecb6 100644 --- a/src/gui/plugins/component_inspector/CMakeLists.txt +++ b/src/gui/plugins/component_inspector/CMakeLists.txt @@ -1,4 +1,4 @@ gz_add_gui_plugin(ComponentInspector - SOURCES ComponentInspector.cc - QT_HEADERS ComponentInspector.hh + SOURCES ComponentInspector.cc ModelEditor.cc + QT_HEADERS ComponentInspector.hh ModelEditor.hh ) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 59f4246e40..b75caaf48c 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -67,6 +67,7 @@ #include "ignition/gazebo/gui/GuiEvents.hh" #include "ComponentInspector.hh" +#include "ModelEditor.hh" namespace ignition::gazebo { @@ -101,6 +102,9 @@ namespace ignition::gazebo /// \brief Transport node for making command requests public: transport::Node node; + + /// \brief Transport node for making command requests + public: ModelEditor modelEditor; }; } @@ -405,10 +409,12 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *) // Connect model this->Context()->setContextProperty( "ComponentsModel", &this->dataPtr->componentsModel); + + this->dataPtr->modelEditor.Load(); } ////////////////////////////////////////////////// -void ComponentInspector::Update(const UpdateInfo &, +void ComponentInspector::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { IGN_PROFILE("ComponentInspector::Update"); @@ -782,6 +788,9 @@ void ComponentInspector::Update(const UpdateInfo &, Q_ARG(ignition::gazebo::ComponentTypeId, typeId)); } } + + + this->dataPtr->modelEditor.Update(_info, _ecm); } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/model_editor/ModelEditor.cc b/src/gui/plugins/component_inspector/ModelEditor.cc similarity index 97% rename from src/gui/plugins/model_editor/ModelEditor.cc rename to src/gui/plugins/component_inspector/ModelEditor.cc index d607aaba85..789ac42454 100644 --- a/src/gui/plugins/model_editor/ModelEditor.cc +++ b/src/gui/plugins/component_inspector/ModelEditor.cc @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -102,7 +101,7 @@ using namespace gazebo; ///////////////////////////////////////////////// ModelEditor::ModelEditor() - : GuiSystem(), dataPtr(std::make_unique()) + : dataPtr(std::make_unique()) { } @@ -110,11 +109,8 @@ ModelEditor::ModelEditor() ModelEditor::~ModelEditor() = default; ///////////////////////////////////////////////// -void ModelEditor::LoadConfig(const tinyxml2::XMLElement *) +void ModelEditor::Load() { - if (this->title.empty()) - this->title = "Model editor"; - ignition::gui::App()->findChild< ignition::gui::MainWindow *>()->installEventFilter(this); } @@ -408,7 +404,3 @@ void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, eta.uri = _uri; this->entitiesToAdd.push_back(eta); } - -// Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::ModelEditor, - ignition::gui::Plugin) diff --git a/src/gui/plugins/model_editor/ModelEditor.hh b/src/gui/plugins/component_inspector/ModelEditor.hh similarity index 68% rename from src/gui/plugins/model_editor/ModelEditor.hh rename to src/gui/plugins/component_inspector/ModelEditor.hh index f089b4498c..bfc09df897 100644 --- a/src/gui/plugins/model_editor/ModelEditor.hh +++ b/src/gui/plugins/component_inspector/ModelEditor.hh @@ -18,15 +18,10 @@ #ifndef IGNITION_GAZEBO_GUI_MODELEDITOR_HH_ #define IGNITION_GAZEBO_GUI_MODELEDITOR_HH_ -#include #include -#include -#include -#include +#include -#include -#include #include namespace ignition @@ -35,10 +30,8 @@ namespace gazebo { class ModelEditorPrivate; - /// \brief - /// - /// Model Editor gui plugin - class ModelEditor : public gazebo::GuiSystem + /// \brief Model Editor + class ModelEditor : public QObject { Q_OBJECT @@ -46,13 +39,15 @@ namespace gazebo public: ModelEditor(); /// \brief Destructor - public: ~ModelEditor() override; + public: ~ModelEditor(); - // Documentation inherited - public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + /// \brief Load the model editor + public: void Load(); - // Documentation inherited - public: void Update(const UpdateInfo &, EntityComponentManager &) override; + /// \brief Update the model editor with data from ECM + /// \param[in] _info Simulator update info + /// \param[in] _ecm Reference to Entity Component Manager + public: void Update(const UpdateInfo &_info, EntityComponentManager &_ecm); // Documentation inherited protected: bool eventFilter(QObject *_obj, QEvent *_event) override; diff --git a/src/gui/plugins/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index de909caa38..20aa36916a 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -129,6 +129,12 @@ void TreeModel::AddEntity(Entity _entity, const QString &_entityName, IGN_PROFILE("TreeModel::AddEntity"); QStandardItem *parentItem{nullptr}; + // check if entity has already been added or not. + // This could happen because we get new and removed entity updates from both + // the ECM and GUI events. + if (this->entityItems.find(_entity) != this->entityItems.end()) + return; + // Root if (_parentEntity == kNullEntity) { @@ -151,13 +157,6 @@ void TreeModel::AddEntity(Entity _entity, const QString &_entityName, return; } - if (this->entityItems.find(_entity) != this->entityItems.end()) - { - ignwarn << "Internal error: Trying to create item for entity [" << _entity - << "], but entity already has an item." << std::endl; - return; - } - // New entity item auto entityItem = new QStandardItem(_entityName); entityItem->setData(_entityName, this->roleNames().key("entityName")); diff --git a/src/gui/plugins/model_editor/CMakeLists.txt b/src/gui/plugins/model_editor/CMakeLists.txt deleted file mode 100644 index 42650ac9b5..0000000000 --- a/src/gui/plugins/model_editor/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -gz_add_gui_plugin(ModelEditor - SOURCES ModelEditor.cc - QT_HEADERS ModelEditor.hh -) diff --git a/src/gui/plugins/model_editor/ModelEditor.qml b/src/gui/plugins/model_editor/ModelEditor.qml deleted file mode 100644 index c5a85ffd29..0000000000 --- a/src/gui/plugins/model_editor/ModelEditor.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.9 -import QtQuick.Controls 1.4 -import QtQuick.Controls 2.2 -import QtQuick.Controls.Material 2.1 -import QtQuick.Layouts 1.3 -import QtQuick.Controls.Styles 1.4 -import IgnGazebo 1.0 as IgnGazebo - - -Rectangle { - id: modelEditor -} diff --git a/src/gui/plugins/model_editor/ModelEditor.qrc b/src/gui/plugins/model_editor/ModelEditor.qrc deleted file mode 100644 index bc76232cfa..0000000000 --- a/src/gui/plugins/model_editor/ModelEditor.qrc +++ /dev/null @@ -1,5 +0,0 @@ - - - ModelEditor.qml - - From 5aba7b88bdd2cf9d1f0230cb5b44e2018ee66b96 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 8 Nov 2021 21:15:10 -0800 Subject: [PATCH 21/25] fixing warnings Signed-off-by: Ian Chen --- include/ignition/gazebo/gui/GuiEvents.hh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index f14f205da4..ec912c30d0 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -18,6 +18,8 @@ #define IGNITION_GAZEBO_GUI_GUIEVENTS_HH_ #include +#include + #include #include #include From 7dc2eaec367d6072e6261729efe3af7b7a84c3fb Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 9 Nov 2021 11:02:01 -0800 Subject: [PATCH 22/25] Adjust tool tips Signed-off-by: Nate Koenig --- src/gui/plugins/component_inspector/ComponentInspector.qml | 2 +- src/gui/plugins/entity_tree/EntityTree.qml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 10f9451dfa..d18d34137b 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -227,7 +227,7 @@ Rectangle { sourceSize.width: 18; sourceSize.height: 18; } - ToolTip.text: "Add entity" + ToolTip.text: "Add an entity to a model" ToolTip.visible: hovered ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval onClicked: { diff --git a/src/gui/plugins/entity_tree/EntityTree.qml b/src/gui/plugins/entity_tree/EntityTree.qml index 0737b6af31..ae1d8fec19 100644 --- a/src/gui/plugins/entity_tree/EntityTree.qml +++ b/src/gui/plugins/entity_tree/EntityTree.qml @@ -140,7 +140,7 @@ Rectangle { ToolButton { anchors.right: parent.right id: addEntity - ToolTip.text: "Add Entity" + ToolTip.text: "Add an entity to the world" ToolTip.visible: hovered contentItem: Image { fillMode: Image.Pad From abb667d846b3cd586627cec993c6af9c2c9657f5 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 9 Nov 2021 13:59:13 -0800 Subject: [PATCH 23/25] fix adding light Signed-off-by: Ian Chen --- .../component_inspector/ComponentInspector.cc | 1 + .../ComponentInspector.qml | 42 +++++++++---------- .../component_inspector/ModelEditor.cc | 1 - 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 2c627282a6..d5e2a58df4 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -1046,6 +1046,7 @@ void ComponentInspector::OnAddEntity(const QString &_entity, // todo(anyone) support adding visuals / collisions / sensors to links ignition::gazebo::gui::events::ModelEditorAddEntity addEntityEvent( _entity, _type, this->dataPtr->entity, QString("")); + ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &addEntityEvent); diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 10f9451dfa..a917615ca6 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -245,19 +245,19 @@ Rectangle { ComponentInspector.OnLoadMesh("mesh", "link", fileUrl) } } - + Menu { id: addLinkMenu - + Item { Layout.fillWidth: true height: childrenRect.height - Loader { + Loader { property string sectionText: "Link" sourceComponent: menuSectionHeading } } - + MenuItem { id: boxLink text: "Box" @@ -266,7 +266,7 @@ Rectangle { addLinkMenu.close() } } - + MenuItem { id: capsuleLink text: "Capsule" @@ -275,7 +275,7 @@ Rectangle { addLinkMenu.close() } } - + MenuItem { id: cylinderLink text: "Cylinder" @@ -283,7 +283,7 @@ Rectangle { ComponentInspector.OnAddEntity("cylinder", "link"); } } - + MenuItem { id: ellipsoidLink text: "Ellipsoid" @@ -291,7 +291,7 @@ Rectangle { ComponentInspector.OnAddEntity("ellipsoid", "link"); } } - + MenuItem { id: emptyLink text: "Empty" @@ -299,7 +299,7 @@ Rectangle { ComponentInspector.OnAddEntity("empty", "link"); } } - + MenuItem { id: meshLink text: "Mesh" @@ -307,7 +307,7 @@ Rectangle { loadFileDialog.open() } } - + MenuItem { id: sphereLink text: "Sphere" @@ -315,7 +315,7 @@ Rectangle { ComponentInspector.OnAddEntity("sphere", "link"); } } - + MenuSeparator { padding: 0 topPadding: 12 @@ -326,45 +326,45 @@ Rectangle { color: "#1E000000" } } - + Item { Layout.fillWidth: true height: childrenRect.height - Loader { + Loader { property string sectionText: "Light" sourceComponent: menuSectionHeading } } - + MenuItem { id: directionalLink text: "Directional" onClicked: { - ComponentInspector.OnAddEntity("directional", "light"); + ComponentInspector.OnAddEntity("directional", "link"); addLinkMenu.close() } } - + MenuItem { id: pointLink text: "Point" onClicked: { - ComponentInspector.OnAddEntity("point", "light"); + ComponentInspector.OnAddEntity("point", "link"); addLinkMenu.close() } } - + MenuItem { id: spotLink text: "Spot" onClicked: { - ComponentInspector.OnAddEntity("spot", "light"); + ComponentInspector.OnAddEntity("spot", "link"); addLinkMenu.close() } } - + // \todo(anyone) Add joints - } + } } Label { diff --git a/src/gui/plugins/component_inspector/ModelEditor.cc b/src/gui/plugins/component_inspector/ModelEditor.cc index 789ac42454..a5e157f632 100644 --- a/src/gui/plugins/component_inspector/ModelEditor.cc +++ b/src/gui/plugins/component_inspector/ModelEditor.cc @@ -350,7 +350,6 @@ std::string ModelEditorPrivate::GeomSDFString(const EntityToAdd &_eta) const ///////////////////////////////////////////////// std::string ModelEditorPrivate::LinkSDFString(const EntityToAdd &_eta) const { - std::stringstream linkStr; if (_eta.geomOrLightType == "empty") { From 44609337e818a2a3234bd95f3b6c6ae7f93bb7b5 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 9 Nov 2021 15:29:42 -0800 Subject: [PATCH 24/25] Fix codecheck Signed-off-by: Nate Koenig --- include/ignition/gazebo/gui/GuiEvents.hh | 4 ++-- include/ignition/gazebo/rendering/RenderUtil.hh | 3 ++- src/gui/plugins/component_inspector/ModelEditor.cc | 12 ++++++++---- src/gui/plugins/scene_manager/GzSceneManager.cc | 1 + 4 files changed, 13 insertions(+), 7 deletions(-) diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index ec912c30d0..15d4c5af77 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -167,8 +167,8 @@ namespace events /// \brief Constructor /// \param[in] _tranformModeActive is the transform control mode active public: explicit ModelEditorAddEntity(QString _entity, QString _type, - ignition::gazebo::Entity _parent, QString _uri) : QEvent(kType), entity(_entity), - type(_type), parent(_parent), uri(_uri) + ignition::gazebo::Entity _parent, QString _uri) : + QEvent(kType), entity(_entity), type(_type), parent(_parent), uri(_uri) { } diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 01d34cded0..49a4a5557e 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -18,6 +18,7 @@ #define IGNITION_GAZEBO_RENDERUTIL_HH_ #include +#include #include #include @@ -82,7 +83,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _ecm Const reference to the entity component manager /// \param[in] _entities Entities to create visuals for. public: void CreateVisualsForEntities(const EntityComponentManager &_ecm, - const std::set &_entities); + const std::set &_entities); /// \brief Set the rendering engine to use /// \param[in] _engineName Name of the rendering engine. diff --git a/src/gui/plugins/component_inspector/ModelEditor.cc b/src/gui/plugins/component_inspector/ModelEditor.cc index a5e157f632..d40cae6e1a 100644 --- a/src/gui/plugins/component_inspector/ModelEditor.cc +++ b/src/gui/plugins/component_inspector/ModelEditor.cc @@ -16,6 +16,9 @@ */ #include +#include +#include +#include #include #include #include @@ -56,7 +59,8 @@ namespace ignition::gazebo class ModelEditorPrivate { /// \brief Handle entity addition - /// \param[in] _geomOrLightType Geometry or light type, e.g. sphere, directional, etc + /// \param[in] _geomOrLightType Geometry or light type, e.g. sphere, + /// directional, etc /// \param[in] _entityType Type of entity: link, visual, collision, etc /// \param[in] _parentEntity Name of parent entity /// \param[in] _uri URI associated with the entity, needed for mesh @@ -130,7 +134,7 @@ void ModelEditor::Update(const UpdateInfo &, std::lock_guard lock(this->dataPtr->mutex); // add link entities to the ECM std::set newEntities; - for (auto & eta: this->dataPtr->entitiesToAdd) + for (const auto &eta : this->dataPtr->entitiesToAdd) { sdf::Link linkSdf; if (eta.entityType == "link") @@ -178,8 +182,8 @@ void ModelEditor::Update(const UpdateInfo &, auto entity = this->dataPtr->entityCreator->CreateEntities(&linkSdf); this->dataPtr->entityCreator->SetParent(entity, eta.parentEntity); - // traverse the tree and add all new entities created by the entity creator - // to the set + // traverse the tree and add all new entities created by the entity + // creator to the set std::list entities; entities.push_back(entity); while (!entities.empty()) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index fdab1261ea..a5c1d68cde 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -14,6 +14,7 @@ * limitations under the License. * */ +#include #include "GzSceneManager.hh" From a5f2c5a68d116705b71eb101a08403bd95f61dfb Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 9 Nov 2021 16:14:37 -0800 Subject: [PATCH 25/25] Fixed documentation Signed-off-by: Nate Koenig --- src/gui/plugins/entity_tree/EntityTree.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gui/plugins/entity_tree/EntityTree.hh b/src/gui/plugins/entity_tree/EntityTree.hh index 83d1ebe736..e4fc238943 100644 --- a/src/gui/plugins/entity_tree/EntityTree.hh +++ b/src/gui/plugins/entity_tree/EntityTree.hh @@ -135,7 +135,7 @@ namespace gazebo public: Q_INVOKABLE void OnInsertEntity(const QString &_type); /// \brief Callback to insert a new entity - /// \param[in] _file Mesh file to create a model from. + /// \param[in] _mesh Mesh file to create a model from. public: Q_INVOKABLE void OnLoadMesh(const QString &_mesh); // Documentation inherited