From fc9ee3c72e1077a93cd7aeb77d2e9f216900a82c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 6 Oct 2021 17:50:52 -0700 Subject: [PATCH 01/55] 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/55] 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/55] 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 007ce1c48451fe1f0d906e228710b45d7a7c6b45 Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Thu, 14 Oct 2021 16:36:25 -0600 Subject: [PATCH 04/55] get updated GUI ECM info in world control CB Signed-off-by: Ashton Larkin --- src/SimulationRunner.cc | 55 ++++++++++++++++++++++++++++++++++++++++- src/SimulationRunner.hh | 13 +++++++++- 2 files changed, 66 insertions(+), 2 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 250d0f2fb2..ce9afb07f5 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -194,7 +194,8 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, this->node = std::make_unique(opts); // TODO(louise) Combine both messages into one. - this->node->Advertise("control", &SimulationRunner::OnWorldControl, this); + this->node->Advertise("control", &SimulationRunner::OnWorldStateControl, + this); this->node->Advertise("playback/control", &SimulationRunner::OnPlaybackControl, this); @@ -1135,6 +1136,58 @@ bool SimulationRunner::OnWorldControl(const msgs::WorldControl &_req, return true; } +///////////////////////////////////////////////// +bool SimulationRunner::OnWorldStateControl(const msgs::WorldControlState &_req, + msgs::Boolean &_res) +{ + std::lock_guard lock(this->msgBufferMutex); + + WorldControl control; + control.pause = _req.worldcontrol().pause(); + + if (_req.worldcontrol().multi_step() != 0) + control.multiStep = _req.worldcontrol().multi_step(); + else if (_req.worldcontrol().step()) + control.multiStep = 1; + + if (_req.worldcontrol().has_reset()) + { + control.rewind = _req.worldcontrol().reset().all() || + _req.worldcontrol().reset().time_only(); + + if (_req.worldcontrol().reset().model_only()) + { + ignwarn << "Model only reset is not supported." << std::endl; + } + } + + if (_req.worldcontrol().seed() != 0) + { + ignwarn << "Changing seed is not supported." << std::endl; + } + + if (_req.worldcontrol().has_run_to_sim_time()) + { + control.runToSimTime = std::chrono::seconds( + _req.worldcontrol().run_to_sim_time().sec()) + + std::chrono::nanoseconds(_req.worldcontrol().run_to_sim_time().nsec()); + } + + this->worldControls.push_back(control); + + // update the server ECM if the GUI ECM was updated + const auto &allGuiEcmUpdates = _req.state(); + for (int i = 0; i < allGuiEcmUpdates.state_size(); ++i) + { + this->entityCompMgr.SetState(allGuiEcmUpdates.state(i)); + } + // TODO(anyone) notify server systems of changes made to the ECM, if there + // were any? + + _res.set_data(true); + return true; +} + ///////////////////////////////////////////////// bool SimulationRunner::OnPlaybackControl(const msgs::LogPlaybackControl &_req, msgs::Boolean &_res) diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 36754d3388..63133096a1 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -38,6 +38,7 @@ #include #include #include +#include #include #include "ignition/gazebo/config.hh" @@ -367,7 +368,17 @@ namespace ignition /// and multistep. /// \param[out] _res Response to client, true if successful. /// \return True for success - private: bool OnWorldControl(const msgs::WorldControl &_req, + private: bool IGN_DEPRECATED(6) OnWorldControl( + const msgs::WorldControl &_req, msgs::Boolean &_res); + + /// \brief World control service callback. This function stores the + /// the request which will then be processed by the ProcessMessages + /// function. + /// \param[in] _req Request from client, currently handling play / pause + /// and multistep. + /// \param[out] _res Response to client, true if successful. + /// \return True for success + private: bool OnWorldStateControl(const msgs::WorldControlState &_req, msgs::Boolean &_res); /// \brief World control service callback. This function stores the From cdd9e5096ebaf57cd603a877d926a149e58dbc1c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 14 Oct 2021 18:11:36 -0700 Subject: [PATCH 05/55] 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 051db6414734c2630d86bba22c2eb9b3ce6d41b3 Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Tue, 19 Oct 2021 16:49:38 -0600 Subject: [PATCH 06/55] working on adding tests Signed-off-by: Ashton Larkin --- src/SimulationRunner_TEST.cc | 185 +++++++++++++++++++++++++++++++++++ 1 file changed, 185 insertions(+) diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 6af7a70a90..08b2376ef9 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -1568,6 +1569,190 @@ TEST_P(SimulationRunnerTest, GenerateWorldSdf) EXPECT_EQ(5u, world->ModelCount()); } +///////////////////////////////////////////////// +TEST_P(SimulationRunnerTest, UpdateECM) +{ + // Load SDF file + sdf::Root root; + root.Load(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "shapes.sdf")); + + ASSERT_EQ(1u, root.WorldCount()); + + // Create simulation runner + auto systemLoader = std::make_shared(); + SimulationRunner runner(root.WorldByIndex(0), systemLoader); + + // make sure that the simulation runner is advertising the service + // that updates its ECM + const std::string updateECMService = "/world/default/control"; + transport::Node node; + std::vector advertisedServices; + node.ServiceList(advertisedServices); + bool foundService = false; + for (const auto &service : advertisedServices) + { + if (service == updateECMService) + { + foundService = true; + break; + } + } + ASSERT_TRUE(foundService); + + // create a mechanism for sharing changes made to otherECM, and ensure that + // simulation runner received the change information + std::function shareECMChanges = + [&updateECMService, &node](msgs::SerializedState_V &_changes) + { + msgs::Boolean resp; + bool serviceResult = false; + msgs::WorldControlState req; + + for (int i = 0; i < _changes.state_size(); ++i) + { + auto ecmState = req.mutable_state()->add_state(); + ecmState->CopyFrom(_changes.state(i)); + } + node.Request(updateECMService, req, 10000, resp, serviceResult); + + EXPECT_TRUE(resp.data()); + EXPECT_TRUE(serviceResult); + _changes.Clear(); + }; + + // helper function for adding a state message to a msgs::SerializedState_V + std::function saveEcmState = + [](msgs::SerializedState_V &_allStates, const EntityComponentManager &_ecm) + { + auto nextEcmChange = _allStates.add_state(); + nextEcmChange->CopyFrom(_ecm.State()); + }; + + // create another ECM that will be modified and then shared with + // the simulation runner's ECM. The simulation runner's ECM should + // have the contents of this ECM once it's shared + EntityComponentManager otherECM; + msgs::SerializedState_V ecmChangesMsg; + + // add an entity with a components::IntComponent to otherECM + EXPECT_EQ(0u, otherECM.EntityCount()); + auto entity = otherECM.CreateEntity(); + otherECM.CreateComponent(entity, + components::IntComponent(1)); + EXPECT_EQ(1u, otherECM.EntityCount()); + EXPECT_TRUE(otherECM.EntityHasComponentType(entity, + components::IntComponent::typeId)); + + // make sure simulation runner's ECM has no entities with a + // components::IntComponent of a data of 1 + EXPECT_TRUE(runner.EntityCompMgr().EntitiesByComponents( + components::IntComponent(1)).empty()); + + // share this new entity with the simulation runner's ECM + saveEcmState(ecmChangesMsg, otherECM); + shareECMChanges(ecmChangesMsg); + + // make sure the simulation runner's ECM has the changes made to otherECM + int foundEntities = 0; + runner.EntityCompMgr().EachNew( + [&foundEntities](const Entity &, const components::IntComponent *_int) + { + foundEntities++; + EXPECT_EQ(1, _int->Data()); + return true; + }); + EXPECT_EQ(1, foundEntities); + + // perform a simulation step, which should clear the new entity state of + // the simulation runner's ECM + UpdateInfo updateInfo; + EXPECT_TRUE(runner.EntityCompMgr().HasNewEntities()); + runner.Step(updateInfo); + EXPECT_FALSE(runner.EntityCompMgr().HasNewEntities()); + + // perform multiple updates to otherECM before sharing it with the + // simulation runner's ECM: + // 1: modify an existing component + // 2: create a new component + otherECM.SetComponentData(entity, 2); + saveEcmState(ecmChangesMsg, otherECM); + otherECM.CreateComponent(entity, components::DoubleComponent(1.0)); + saveEcmState(ecmChangesMsg, otherECM); + // (make sure each change is registered as a separate state in order to test + // updating simulation runner's ECM with multiple state changes) + EXPECT_EQ(2, ecmChangesMsg.state_size()); + + // share the new updates with simulation runner and make sure that simulation + // runner's ECM reflects these updates + // + // TODO(adlarkin) figure out why serialized state processing doesn't seem to + // pick up component updates (the check for modified int component data fails, + // along with the check later on for an entity with an int component of 2). + // Looking at the ECM unit tests, it looks like this was never tested + // (so, perhaps it's existing buggy behavior?). Looking at the ECM code/tests + // makes me think that updates work for msgs::SerializedStateMap (this + // scenario seems to be tested), so what's wrong with msgs::SerializedState? + shareECMChanges(ecmChangesMsg); + EXPECT_FALSE(runner.EntityCompMgr().HasNewEntities()); + foundEntities = 0; + runner.EntityCompMgr().Each( + [&foundEntities](const Entity &, const components::IntComponent *_int, + const components::DoubleComponent *_double) + { + foundEntities++; + EXPECT_EQ(2, _int->Data()); + EXPECT_DOUBLE_EQ(1.0, _double->Data()); + return true; + }); + EXPECT_EQ(1, foundEntities); + + // test removing a component to make sure that the simulation runner's ECM + // mirrors the component removal + EXPECT_TRUE(otherECM.RemoveComponent(entity, + components::DoubleComponent::typeId)); + saveEcmState(ecmChangesMsg, otherECM); + shareECMChanges(ecmChangesMsg); + EXPECT_EQ(1u, runner.EntityCompMgr().EntitiesByComponents( + components::IntComponent(2)).size()); + // (Each is used to verify no entities have a DoubleComponent b/c searching + // via EntitiesByComponents could be flaky due to floating point precision) + foundEntities = 0; + runner.EntityCompMgr().Each( + [&](const Entity &, const components::DoubleComponent *) + { + foundEntities++; + return true; + }); + EXPECT_EQ(0, foundEntities); + + // test removing an entity + otherECM.RequestRemoveEntity(entity); + saveEcmState(ecmChangesMsg, otherECM); + shareECMChanges(ecmChangesMsg); + foundEntities = 0; + runner.EntityCompMgr().EachRemoved( + [&](const Entity &, const components::IntComponent *) + { + foundEntities++; + return true; + }); + EXPECT_EQ(1, foundEntities); + // (step the simulation runner to actually remove the entity) + updateInfo.realTime++; + runner.Step(updateInfo); + foundEntities = 0; + runner.EntityCompMgr().Each( + [&](const Entity &, const components::IntComponent *) + { + foundEntities++; + return true; + }); + EXPECT_EQ(0, foundEntities); +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(ServerRepeat, SimulationRunnerTest, From 4dc56c98da433fad58697e1febc6484fbbb86ee9 Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Thu, 21 Oct 2021 11:11:34 -0600 Subject: [PATCH 07/55] remove TODO note, issue fixed by #1131 Signed-off-by: Ashton Larkin --- src/SimulationRunner_TEST.cc | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 08b2376ef9..a44c6f0e15 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -1686,14 +1686,6 @@ TEST_P(SimulationRunnerTest, UpdateECM) // share the new updates with simulation runner and make sure that simulation // runner's ECM reflects these updates - // - // TODO(adlarkin) figure out why serialized state processing doesn't seem to - // pick up component updates (the check for modified int component data fails, - // along with the check later on for an entity with an int component of 2). - // Looking at the ECM unit tests, it looks like this was never tested - // (so, perhaps it's existing buggy behavior?). Looking at the ECM code/tests - // makes me think that updates work for msgs::SerializedStateMap (this - // scenario seems to be tested), so what's wrong with msgs::SerializedState? shareECMChanges(ecmChangesMsg); EXPECT_FALSE(runner.EntityCompMgr().HasNewEntities()); foundEntities = 0; From 8f7714cc382593b1896dc8954fc0e30273362403 Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Fri, 22 Oct 2021 15:26:50 -0600 Subject: [PATCH 08/55] 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 09/55] 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 10/55] 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 11/55] 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 12/55] 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 13/55] 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 14/55] 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 6cab7b47d84fcee6b3a75cfe8c42a39381214654 Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Tue, 26 Oct 2021 10:44:04 -0600 Subject: [PATCH 15/55] apply GUI ECM's diff to server ECM at end of pause interval Signed-off-by: Ashton Larkin --- src/SimulationRunner.cc | 103 ++++++++++---------- src/SimulationRunner.hh | 21 ++--- src/SimulationRunner_TEST.cc | 177 ----------------------------------- src/gui/GuiRunner.cc | 21 +++++ 4 files changed, 78 insertions(+), 244 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index ce9afb07f5..727a3e4243 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -18,10 +18,12 @@ #include "SimulationRunner.hh" #include +#include +#include +#include #include -#include "ignition/common/Profiler.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Sensor.hh" @@ -194,8 +196,7 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, this->node = std::make_unique(opts); // TODO(louise) Combine both messages into one. - this->node->Advertise("control", &SimulationRunner::OnWorldStateControl, - this); + this->node->Advertise("control", &SimulationRunner::OnWorldControl, this); this->node->Advertise("playback/control", &SimulationRunner::OnPlaybackControl, this); @@ -805,6 +806,43 @@ bool SimulationRunner::Run(const uint64_t _iterations) void SimulationRunner::Step(const UpdateInfo &_info) { IGN_PROFILE("SimulationRunner::Step"); + + { + std::lock_guard lock(this->guiServerEcmMutex); + if (this->matchGuiEcm) + { + // Update the server's ECM to match any changes that took place in the + // GUI's ECM during the most recent pause interval. We do this before + // processing WorldControl msgs to ensure that GUI ECM changes aren't + // lost (if the GUI and server modified the same entities/components + // while paused, the GUI changes will overwrite the server changes) + const std::string guiEcmService = "/changedGuiEcm"; + msgs::SerializedState guiChanges; + bool result; + if (this->node->Request(guiEcmService, 500, guiChanges, result)) + { + if (!result) + ignerr << "Error processing the [" << guiEcmService << "] service.\n"; + else + { + igndbg << "Syncing server's ECM state with gui's ECM state.\n"; + this->entityCompMgr.SetState(guiChanges); + } + } + else + { + ignerr << "Unable to receive any changes that occurred in the GUI " + << "while paused.\n"; + } + // TODO(anyone) notify server systems of changes made to the ECM, + // if there were any? Server systems are updated later in this method, + // so depending on how a server system is written/implemented, the system + // may automatically detect changes made to the ECM + + this->matchGuiEcm = false; + } + } + this->currentInfo = _info; // Publish info @@ -1101,6 +1139,13 @@ bool SimulationRunner::OnWorldControl(const msgs::WorldControl &_req, { std::lock_guard lock(this->msgBufferMutex); + { + std::lock_guard ecmLock(this->guiServerEcmMutex); + // if we are going from pause to play, we need to update the server ECM + // with any changes that occurred to the GUI ECM while paused. + this->matchGuiEcm = this->Paused() && !_req.pause(); + } + WorldControl control; control.pause = _req.pause(); @@ -1136,58 +1181,6 @@ bool SimulationRunner::OnWorldControl(const msgs::WorldControl &_req, return true; } -///////////////////////////////////////////////// -bool SimulationRunner::OnWorldStateControl(const msgs::WorldControlState &_req, - msgs::Boolean &_res) -{ - std::lock_guard lock(this->msgBufferMutex); - - WorldControl control; - control.pause = _req.worldcontrol().pause(); - - if (_req.worldcontrol().multi_step() != 0) - control.multiStep = _req.worldcontrol().multi_step(); - else if (_req.worldcontrol().step()) - control.multiStep = 1; - - if (_req.worldcontrol().has_reset()) - { - control.rewind = _req.worldcontrol().reset().all() || - _req.worldcontrol().reset().time_only(); - - if (_req.worldcontrol().reset().model_only()) - { - ignwarn << "Model only reset is not supported." << std::endl; - } - } - - if (_req.worldcontrol().seed() != 0) - { - ignwarn << "Changing seed is not supported." << std::endl; - } - - if (_req.worldcontrol().has_run_to_sim_time()) - { - control.runToSimTime = std::chrono::seconds( - _req.worldcontrol().run_to_sim_time().sec()) + - std::chrono::nanoseconds(_req.worldcontrol().run_to_sim_time().nsec()); - } - - this->worldControls.push_back(control); - - // update the server ECM if the GUI ECM was updated - const auto &allGuiEcmUpdates = _req.state(); - for (int i = 0; i < allGuiEcmUpdates.state_size(); ++i) - { - this->entityCompMgr.SetState(allGuiEcmUpdates.state(i)); - } - // TODO(anyone) notify server systems of changes made to the ECM, if there - // were any? - - _res.set_data(true); - return true; -} - ///////////////////////////////////////////////// bool SimulationRunner::OnPlaybackControl(const msgs::LogPlaybackControl &_req, msgs::Boolean &_res) diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 63133096a1..e03c566fc6 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -38,7 +39,6 @@ #include #include #include -#include #include #include "ignition/gazebo/config.hh" @@ -368,17 +368,7 @@ namespace ignition /// and multistep. /// \param[out] _res Response to client, true if successful. /// \return True for success - private: bool IGN_DEPRECATED(6) OnWorldControl( - const msgs::WorldControl &_req, msgs::Boolean &_res); - - /// \brief World control service callback. This function stores the - /// the request which will then be processed by the ProcessMessages - /// function. - /// \param[in] _req Request from client, currently handling play / pause - /// and multistep. - /// \param[out] _res Response to client, true if successful. - /// \return True for success - private: bool OnWorldStateControl(const msgs::WorldControlState &_req, + private: bool OnWorldControl(const msgs::WorldControl &_req, msgs::Boolean &_res); /// \brief World control service callback. This function stores the @@ -612,6 +602,13 @@ namespace ignition /// \brief True if Server::RunOnce triggered a blocking paused step private: bool blockingPausedStepPending{false}; + /// \brief Flag that indicates whether the server ECM needs to be updated + /// to match the GUI ECM + private: bool matchGuiEcm{false}; + + /// \brief Mutex used for reading/writing the gui ECM flag above + private: std::mutex guiServerEcmMutex; + friend class LevelManager; }; } diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index a44c6f0e15..6af7a70a90 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -20,7 +20,6 @@ #include #include -#include #include #include #include @@ -1569,182 +1568,6 @@ TEST_P(SimulationRunnerTest, GenerateWorldSdf) EXPECT_EQ(5u, world->ModelCount()); } -///////////////////////////////////////////////// -TEST_P(SimulationRunnerTest, UpdateECM) -{ - // Load SDF file - sdf::Root root; - root.Load(common::joinPaths(PROJECT_SOURCE_PATH, - "test", "worlds", "shapes.sdf")); - - ASSERT_EQ(1u, root.WorldCount()); - - // Create simulation runner - auto systemLoader = std::make_shared(); - SimulationRunner runner(root.WorldByIndex(0), systemLoader); - - // make sure that the simulation runner is advertising the service - // that updates its ECM - const std::string updateECMService = "/world/default/control"; - transport::Node node; - std::vector advertisedServices; - node.ServiceList(advertisedServices); - bool foundService = false; - for (const auto &service : advertisedServices) - { - if (service == updateECMService) - { - foundService = true; - break; - } - } - ASSERT_TRUE(foundService); - - // create a mechanism for sharing changes made to otherECM, and ensure that - // simulation runner received the change information - std::function shareECMChanges = - [&updateECMService, &node](msgs::SerializedState_V &_changes) - { - msgs::Boolean resp; - bool serviceResult = false; - msgs::WorldControlState req; - - for (int i = 0; i < _changes.state_size(); ++i) - { - auto ecmState = req.mutable_state()->add_state(); - ecmState->CopyFrom(_changes.state(i)); - } - node.Request(updateECMService, req, 10000, resp, serviceResult); - - EXPECT_TRUE(resp.data()); - EXPECT_TRUE(serviceResult); - _changes.Clear(); - }; - - // helper function for adding a state message to a msgs::SerializedState_V - std::function saveEcmState = - [](msgs::SerializedState_V &_allStates, const EntityComponentManager &_ecm) - { - auto nextEcmChange = _allStates.add_state(); - nextEcmChange->CopyFrom(_ecm.State()); - }; - - // create another ECM that will be modified and then shared with - // the simulation runner's ECM. The simulation runner's ECM should - // have the contents of this ECM once it's shared - EntityComponentManager otherECM; - msgs::SerializedState_V ecmChangesMsg; - - // add an entity with a components::IntComponent to otherECM - EXPECT_EQ(0u, otherECM.EntityCount()); - auto entity = otherECM.CreateEntity(); - otherECM.CreateComponent(entity, - components::IntComponent(1)); - EXPECT_EQ(1u, otherECM.EntityCount()); - EXPECT_TRUE(otherECM.EntityHasComponentType(entity, - components::IntComponent::typeId)); - - // make sure simulation runner's ECM has no entities with a - // components::IntComponent of a data of 1 - EXPECT_TRUE(runner.EntityCompMgr().EntitiesByComponents( - components::IntComponent(1)).empty()); - - // share this new entity with the simulation runner's ECM - saveEcmState(ecmChangesMsg, otherECM); - shareECMChanges(ecmChangesMsg); - - // make sure the simulation runner's ECM has the changes made to otherECM - int foundEntities = 0; - runner.EntityCompMgr().EachNew( - [&foundEntities](const Entity &, const components::IntComponent *_int) - { - foundEntities++; - EXPECT_EQ(1, _int->Data()); - return true; - }); - EXPECT_EQ(1, foundEntities); - - // perform a simulation step, which should clear the new entity state of - // the simulation runner's ECM - UpdateInfo updateInfo; - EXPECT_TRUE(runner.EntityCompMgr().HasNewEntities()); - runner.Step(updateInfo); - EXPECT_FALSE(runner.EntityCompMgr().HasNewEntities()); - - // perform multiple updates to otherECM before sharing it with the - // simulation runner's ECM: - // 1: modify an existing component - // 2: create a new component - otherECM.SetComponentData(entity, 2); - saveEcmState(ecmChangesMsg, otherECM); - otherECM.CreateComponent(entity, components::DoubleComponent(1.0)); - saveEcmState(ecmChangesMsg, otherECM); - // (make sure each change is registered as a separate state in order to test - // updating simulation runner's ECM with multiple state changes) - EXPECT_EQ(2, ecmChangesMsg.state_size()); - - // share the new updates with simulation runner and make sure that simulation - // runner's ECM reflects these updates - shareECMChanges(ecmChangesMsg); - EXPECT_FALSE(runner.EntityCompMgr().HasNewEntities()); - foundEntities = 0; - runner.EntityCompMgr().Each( - [&foundEntities](const Entity &, const components::IntComponent *_int, - const components::DoubleComponent *_double) - { - foundEntities++; - EXPECT_EQ(2, _int->Data()); - EXPECT_DOUBLE_EQ(1.0, _double->Data()); - return true; - }); - EXPECT_EQ(1, foundEntities); - - // test removing a component to make sure that the simulation runner's ECM - // mirrors the component removal - EXPECT_TRUE(otherECM.RemoveComponent(entity, - components::DoubleComponent::typeId)); - saveEcmState(ecmChangesMsg, otherECM); - shareECMChanges(ecmChangesMsg); - EXPECT_EQ(1u, runner.EntityCompMgr().EntitiesByComponents( - components::IntComponent(2)).size()); - // (Each is used to verify no entities have a DoubleComponent b/c searching - // via EntitiesByComponents could be flaky due to floating point precision) - foundEntities = 0; - runner.EntityCompMgr().Each( - [&](const Entity &, const components::DoubleComponent *) - { - foundEntities++; - return true; - }); - EXPECT_EQ(0, foundEntities); - - // test removing an entity - otherECM.RequestRemoveEntity(entity); - saveEcmState(ecmChangesMsg, otherECM); - shareECMChanges(ecmChangesMsg); - foundEntities = 0; - runner.EntityCompMgr().EachRemoved( - [&](const Entity &, const components::IntComponent *) - { - foundEntities++; - return true; - }); - EXPECT_EQ(1, foundEntities); - // (step the simulation runner to actually remove the entity) - updateInfo.realTime++; - runner.Step(updateInfo); - foundEntities = 0; - runner.EntityCompMgr().Each( - [&](const Entity &, const components::IntComponent *) - { - foundEntities++; - return true; - }); - EXPECT_EQ(0, foundEntities); -} - // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(ServerRepeat, SimulationRunnerTest, diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index c61264f974..eb9e8d34a4 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include // Include all components so they have first-class support @@ -101,6 +102,26 @@ GuiRunner::GuiRunner(const std::string &_worldName) QPointer timer = new QTimer(this); connect(timer, &QTimer::timeout, this, &GuiRunner::UpdatePlugins); timer->start(33); + + // Advertise a service that shares the changed state of the GUI's ECM. + // "Changed" in this context means the changes that took place since the last + // simulation step + const std::string changedGuiEcmService = "/changedGuiEcm"; + std::function cb = + [this](msgs::SerializedState &_resp) + { + // since GUI plugins may update the GUI's ECM, make sure that no GUI + // plugins are updated while getting the ECM's changed state + std::lock_guard lock(this->dataPtr->updateMutex); + + _resp.CopyFrom(this->dataPtr->ecm.State()); + return true; + }; + if (!this->dataPtr->node.Advertise(changedGuiEcmService, cb)) + { + ignerr << "failed to advertise the [" << changedGuiEcmService + << "] service.\n"; + } } ///////////////////////////////////////////////// From d726c70ed82bf27aa32ddf318b364996326a7817 Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Thu, 28 Oct 2021 21:43:35 -0600 Subject: [PATCH 16/55] use gui event to update server Signed-off-by: Ashton Larkin --- src/SimulationRunner.cc | 81 +++++++++++------------------------------ src/SimulationRunner.hh | 13 ++----- src/gui/GuiRunner.cc | 59 ++++++++++++++++++++---------- src/gui/GuiRunner.hh | 3 ++ 4 files changed, 68 insertions(+), 88 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 727a3e4243..efb40b321b 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -18,12 +18,10 @@ #include "SimulationRunner.hh" #include -#include -#include -#include #include +#include "ignition/common/Profiler.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Sensor.hh" @@ -196,7 +194,8 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, this->node = std::make_unique(opts); // TODO(louise) Combine both messages into one. - this->node->Advertise("control", &SimulationRunner::OnWorldControl, this); + this->node->Advertise("control", &SimulationRunner::OnWorldControlState, + this); this->node->Advertise("playback/control", &SimulationRunner::OnPlaybackControl, this); @@ -806,43 +805,6 @@ bool SimulationRunner::Run(const uint64_t _iterations) void SimulationRunner::Step(const UpdateInfo &_info) { IGN_PROFILE("SimulationRunner::Step"); - - { - std::lock_guard lock(this->guiServerEcmMutex); - if (this->matchGuiEcm) - { - // Update the server's ECM to match any changes that took place in the - // GUI's ECM during the most recent pause interval. We do this before - // processing WorldControl msgs to ensure that GUI ECM changes aren't - // lost (if the GUI and server modified the same entities/components - // while paused, the GUI changes will overwrite the server changes) - const std::string guiEcmService = "/changedGuiEcm"; - msgs::SerializedState guiChanges; - bool result; - if (this->node->Request(guiEcmService, 500, guiChanges, result)) - { - if (!result) - ignerr << "Error processing the [" << guiEcmService << "] service.\n"; - else - { - igndbg << "Syncing server's ECM state with gui's ECM state.\n"; - this->entityCompMgr.SetState(guiChanges); - } - } - else - { - ignerr << "Unable to receive any changes that occurred in the GUI " - << "while paused.\n"; - } - // TODO(anyone) notify server systems of changes made to the ECM, - // if there were any? Server systems are updated later in this method, - // so depending on how a server system is written/implemented, the system - // may automatically detect changes made to the ECM - - this->matchGuiEcm = false; - } - } - this->currentInfo = _info; // Publish info @@ -1134,45 +1096,46 @@ void SimulationRunner::SetRunToSimTime( } ///////////////////////////////////////////////// -bool SimulationRunner::OnWorldControl(const msgs::WorldControl &_req, +bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req, msgs::Boolean &_res) { std::lock_guard lock(this->msgBufferMutex); - { - std::lock_guard ecmLock(this->guiServerEcmMutex); - // if we are going from pause to play, we need to update the server ECM - // with any changes that occurred to the GUI ECM while paused. - this->matchGuiEcm = this->Paused() && !_req.pause(); - } + // update the server ECM if the request contains SerializedState information + if (_req.has_state()) + this->entityCompMgr.SetState(_req.state()); + // TODO(anyone) notify server systems of changes made to the ECM, if there + // were any? WorldControl control; - control.pause = _req.pause(); + control.pause = _req.world_control().pause(); - if (_req.multi_step() != 0) - control.multiStep = _req.multi_step(); - else if (_req.step()) + if (_req.world_control().multi_step() != 0) + control.multiStep = _req.world_control().multi_step(); + else if (_req.world_control().step()) control.multiStep = 1; - if (_req.has_reset()) + if (_req.world_control().has_reset()) { - control.rewind = _req.reset().all() || _req.reset().time_only(); + control.rewind = _req.world_control().reset().all() || + _req.world_control().reset().time_only(); - if (_req.reset().model_only()) + if (_req.world_control().reset().model_only()) { ignwarn << "Model only reset is not supported." << std::endl; } } - if (_req.seed() != 0) + if (_req.world_control().seed() != 0) { ignwarn << "Changing seed is not supported." << std::endl; } - if (_req.has_run_to_sim_time()) + if (_req.world_control().has_run_to_sim_time()) { - control.runToSimTime = std::chrono::seconds(_req.run_to_sim_time().sec()) + - std::chrono::nanoseconds(_req.run_to_sim_time().nsec()); + control.runToSimTime = std::chrono::seconds( + _req.world_control().run_to_sim_time().sec()) + + std::chrono::nanoseconds(_req.world_control().run_to_sim_time().nsec()); } this->worldControls.push_back(control); diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index e03c566fc6..984ed4a7e5 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -26,7 +26,6 @@ #include #include #include -#include #include #include #include @@ -39,6 +38,7 @@ #include #include #include +#include #include #include "ignition/gazebo/config.hh" @@ -365,10 +365,10 @@ namespace ignition /// the request which will then be processed by the ProcessMessages /// function. /// \param[in] _req Request from client, currently handling play / pause - /// and multistep. + /// and multistep. This also may contain SerializedState information. /// \param[out] _res Response to client, true if successful. /// \return True for success - private: bool OnWorldControl(const msgs::WorldControl &_req, + private: bool OnWorldControlState(const msgs::WorldControlState &_req, msgs::Boolean &_res); /// \brief World control service callback. This function stores the @@ -602,13 +602,6 @@ namespace ignition /// \brief True if Server::RunOnce triggered a blocking paused step private: bool blockingPausedStepPending{false}; - /// \brief Flag that indicates whether the server ECM needs to be updated - /// to match the GUI ECM - private: bool matchGuiEcm{false}; - - /// \brief Mutex used for reading/writing the gui ECM flag above - private: std::mutex guiServerEcmMutex; - friend class LevelManager; }; } diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index b32b20c0b1..9b575b0816 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -64,6 +65,9 @@ class ignition::gazebo::GuiRunner::Implementation /// \brief True if the initial state has been received and processed. public: bool receivedInitialState{false}; + + /// \brief Name of WorldControl service + public: std::string controlService; }; ///////////////////////////////////////////////// @@ -112,30 +116,47 @@ GuiRunner::GuiRunner(const std::string &_worldName) connect(timer, &QTimer::timeout, this, &GuiRunner::UpdatePlugins); timer->start(33); - // Advertise a service that shares the changed state of the GUI's ECM. - // "Changed" in this context means the changes that took place since the last - // simulation step - const std::string changedGuiEcmService = "/changedGuiEcm"; - std::function cb = - [this](msgs::SerializedState &_resp) - { - // since GUI plugins may update the GUI's ECM, make sure that no GUI - // plugins are updated while getting the ECM's changed state - std::lock_guard lock(this->dataPtr->updateMutex); - - _resp.CopyFrom(this->dataPtr->ecm.State()); - return true; - }; - if (!this->dataPtr->node.Advertise(changedGuiEcmService, cb)) - { - ignerr << "failed to advertise the [" << changedGuiEcmService - << "] service.\n"; - } + this->dataPtr->controlService = "/world/" + _worldName + "/control"; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); } ///////////////////////////////////////////////// GuiRunner::~GuiRunner() = default; +///////////////////////////////////////////////// +bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == ignition::gui::events::WorldControl::kType) + { + auto worldControlEvent = + reinterpret_cast(_event); + if (worldControlEvent) + { + msgs::WorldControlState req; + req.mutable_world_control()->CopyFrom( + worldControlEvent->WorldControlInfo()); + + // If a user presses play, send the GUI's ECM state to the server + // in case the GUI's ECM was modified while paused + if (worldControlEvent->Play()) + req.mutable_state()->CopyFrom(this->dataPtr->ecm.State()); + + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error sharing WorldControl info with the server.\n"; + }; + this->dataPtr->node.Request(this->dataPtr->controlService, req, cb); + } + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + ///////////////////////////////////////////////// void GuiRunner::RequestState() { diff --git a/src/gui/GuiRunner.hh b/src/gui/GuiRunner.hh index b2bceaf0e9..1a06098161 100644 --- a/src/gui/GuiRunner.hh +++ b/src/gui/GuiRunner.hh @@ -46,6 +46,9 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \brief Destructor public: ~GuiRunner() override; + // Documentation inherited + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + /// \brief Callback when a plugin has been added. /// This function has no effect and is left here for ABI compatibility. /// \param[in] _objectName Plugin's object name. From 4ca97b1881e6e1ea0bf4917cf7d74c6c1daa768a Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 29 Oct 2021 16:49:29 -0700 Subject: [PATCH 17/55] Working on sensor addition and editing Signed-off-by: Nate Koenig --- include/ignition/gazebo/gui/GuiEvents.hh | 6 +- .../plugins/component_inspector/Camera.qml | 291 ++++++++++++++++++ .../component_inspector/ComponentInspector.cc | 75 ++++- .../component_inspector/ComponentInspector.hh | 21 ++ .../ComponentInspector.qml | 141 ++++++++- .../ComponentInspector.qrc | 1 + src/gui/plugins/model_editor/CMakeLists.txt | 1 + src/gui/plugins/model_editor/ModelEditor.cc | 69 ++++- 8 files changed, 588 insertions(+), 17 deletions(-) create mode 100644 src/gui/plugins/component_inspector/Camera.qml diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 0d2bf90952..5403b05e3d 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -165,7 +165,7 @@ 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), + uint64_t _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: gazebo::Entity ParentEntity() const { return this->parent; } @@ -193,7 +193,7 @@ namespace events private: QString entity; private: QString type; - private: QString parent; + private: gazebo::Entity parent; }; } // namespace events diff --git a/src/gui/plugins/component_inspector/Camera.qml b/src/gui/plugins/component_inspector/Camera.qml new file mode 100644 index 0000000000..a397d1767c --- /dev/null +++ b/src/gui/plugins/component_inspector/Camera.qml @@ -0,0 +1,291 @@ +/* + * 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.Dialogs 1.0 +import QtQuick.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 +import "qrc:/ComponentInspector" +import "qrc:/qml" + +// Item displaying spherical coordinates information. +Rectangle { + height: header.height + content.height + width: componentInspector.width + color: index % 2 == 0 ? lightGrey : darkGrey + + // Left indentation + property int indentation: 10 + + // Horizontal margins + property int margin: 5 + + // Send new data to C++ + function sendCameraUpdate() { + componentInspector.onCameraUpdate( + horizontalFovSpin.value, + imageWidthSpin.value, + imageHeightSpin.value, + nearClipSpin.value, + farClipSpin.value + ); + } + + Column { + anchors.fill: parent + + // Header + Rectangle { + id: header + width: parent.width + height: typeHeader.height + color: "transparent" + + RowLayout { + anchors.fill: parent + Item { + width: margin + } + Image { + id: icon + sourceSize.height: indentation + sourceSize.width: indentation + fillMode: Image.Pad + Layout.alignment : Qt.AlignVCenter + source: content.show ? + "qrc:/Gazebo/images/minus.png" : "qrc:/Gazebo/images/plus.png" + } + TypeHeader { + id: typeHeader + } + Item { + Layout.fillWidth: true + } + } + MouseArea { + anchors.fill: parent + hoverEnabled: true + cursorShape: Qt.PointingHandCursor + onClicked: { + content.show = !content.show + } + onEntered: { + header.color = highlightColor + } + onExited: { + header.color = "transparent" + } + } + } + + // Content + Rectangle { + id: content + property bool show: false + width: parent.width + height: show ? grid.height : 0 + clip: true + color: "transparent" + + Behavior on height { + NumberAnimation { + duration: 200; + easing.type: Easing.InOutQuad + } + } + + GridLayout { + id: grid + width: parent.width + columns: 2 + + // Horizontal field of view + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: horizontalFovText.width + indentation*3 + + Text { + id : horizontalFovText + text: 'Horizontal FoV (°)' + leftPadding: 4 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + ToolTip { + visible: ma.containsMouse + delay: Qt.styleHints.mousePressAndHoldInterval + text: "Horizontal field of view (degrees)" + enter: null + exit: null + } + MouseArea { + id: ma + anchors.fill: content + hoverEnabled: true + } + } + IgnSpinBox { + id: horizontalFovSpin + Layout.fillWidth: true + height: 40 + property double numberValue: model.data[0] + value: horizontalFovSpin.activeFocus ? horizontalFovSpin.value : numberValue + + // This is equivalent to 0.1 radians, per the SDF spec + minimumValue: 5.72958 + // This is equivalent to 6.283186 radians, per the SDF spec + maximumValue: 360.0 + decimals:4 + stepSize: 0.1 + onEditingFinished: { + sendCameraUpdate() + } + } + // End of Horizontal field of view + + + // Image width + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: imageWidthText.width + indentation*3 + + Text { + id : imageWidthText + text: 'Image width (px)' + leftPadding: 4 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + IgnSpinBox { + id: imageWidthSpin + Layout.fillWidth: true + property int numberValue: model.data[1] + value: imageWidthSpin.activeFocus ? imageWidthSpin.value : numberValue + + minimumValue: 1 + maximumValue: 100000 + decimals:0 + stepSize: 1 + onEditingFinished: { + sendCameraUpdate() + } + } + // End of Image width + + // Image height + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: imageHeightText.width + indentation*3 + + Text { + id : imageHeightText + text: 'Image height (px)' + leftPadding: 4 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + IgnSpinBox { + id: imageHeightSpin + Layout.fillWidth: true + property int numberValue: model.data[2] + value: imageHeightSpin.activeFocus ? imageHeightSpin.value : numberValue + + minimumValue: 1 + maximumValue: 100000 + decimals:0 + stepSize: 1 + onEditingFinished: { + sendCameraUpdate() + } + } + // End of Image height + + // Near clip + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: nearClipText.width + indentation*3 + + Text { + id : nearClipText + text: 'Near clip (m)' + leftPadding: 4 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + IgnSpinBox { + id: nearClipSpin + Layout.fillWidth: true + property double numberValue: model.data[3] + value: nearClipSpin.activeFocus ? nearClipSpin.value : numberValue + + minimumValue: 0.0 + maximumValue: 100000 + decimals: 4 + stepSize: 0.1 + onEditingFinished: { + sendCameraUpdate() + } + } + // End of near clip + + // Far clip + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: farClipText.width + indentation*3 + + Text { + id : farClipText + text: 'Far clip (m)' + leftPadding: 4 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + IgnSpinBox { + id: farClipSpin + Layout.fillWidth: true + property double numberValue: model.data[4] + value: farClipSpin.activeFocus ? farClipSpin.value : numberValue + + minimumValue: 0.0 + maximumValue: 100000 + decimals: 4 + stepSize: 0.1 + onEditingFinished: { + sendCameraUpdate() + } + } + // End of far clip + } + } + } +} diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index a3a7844a2a..bb1afc9cba 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -17,6 +17,9 @@ #include #include + +#include + #include #include #include @@ -27,6 +30,7 @@ #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/AngularAcceleration.hh" #include "ignition/gazebo/components/AngularVelocity.hh" +#include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/CastShadows.hh" #include "ignition/gazebo/components/ChildLinkName.hh" #include "ignition/gazebo/components/Collision.hh" @@ -100,6 +104,9 @@ namespace ignition::gazebo /// \brief Transport node for making command requests public: transport::Node node; + + public: std::vector< + std::function> updateCallbacks; }; } @@ -288,6 +295,26 @@ void ignition::gazebo::setData(QStandardItem *_item, }), ComponentsModel::RoleNames().key("data")); } +////////////////////////////////////////////////// +template<> +void ignition::gazebo::setData(QStandardItem *_item, const sdf::Sensor &_data) +{ + if (nullptr == _item) + return; + + const sdf::Camera *cam = _data.CameraSensor(); + + _item->setData(QString("Camera"), + ComponentsModel::RoleNames().key("dataType")); + _item->setData(QList({ + QVariant(cam->HorizontalFov().Degree()), + QVariant(cam->ImageWidth()), + QVariant(cam->ImageHeight()), + QVariant(cam->NearClip()), + QVariant(cam->FarClip()), + }), ComponentsModel::RoleNames().key("data")); +} + ////////////////////////////////////////////////// void ignition::gazebo::setUnit(QStandardItem *_item, const std::string &_unit) { @@ -412,9 +439,6 @@ void ComponentInspector::Update(const UpdateInfo &, { IGN_PROFILE("ComponentInspector::Update"); - if (this->dataPtr->paused) - return; - auto componentTypes = _ecm.ComponentTypes(this->dataPtr->entity); // List all components @@ -767,6 +791,12 @@ void ComponentInspector::Update(const UpdateInfo &, if (comp) setData(item, comp->Data()); } + else if (typeId == components::Camera::typeId) + { + auto comp = _ecm.Component(this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } } // Remove components no longer present @@ -781,6 +811,10 @@ void ComponentInspector::Update(const UpdateInfo &, Q_ARG(ignition::gazebo::ComponentTypeId, typeId)); } } + + for (auto cb : this->dataPtr->updateCallbacks) + cb(_ecm); + this->dataPtr->updateCallbacks.clear(); } ///////////////////////////////////////////////// @@ -821,6 +855,7 @@ int ComponentInspector::Entity() const ///////////////////////////////////////////////// void ComponentInspector::SetEntity(const int &_entity) { + std::cout << "SetEntity\n"; // If nothing is selected, display world properties if (_entity == kNullEntity) { @@ -872,6 +907,37 @@ void ComponentInspector::SetPaused(bool _paused) this->PausedChanged(); } +///////////////////////////////////////////////// +void ComponentInspector::OnCameraUpdate( + double _hfov, int _imageWidth, int _imageHeight, double _nearClip, + double _farClip) +{ + std::function cb = + [this, _hfov](EntityComponentManager &_ecm) + { + auto comp = _ecm.Component(this->dataPtr->entity); + if (comp) + { + sdf::Camera *cam = comp->Data().CameraSensor(); + if (cam) + { + cam->SetHorizontalFov(math::Angle(IGN_DTOR(_hfov))); + cam->SetImageWidth(_imageWidth); + cam->SetImageHeight(_imageHeight); + cam->SetNearClip(_nearClip); + cam->SetFarClip(_farClip); + } + else + ignerr << "Unable to get the camera data.\n"; + } + else + { + ignerr << "Unable to get the camera data.\n"; + } + }; + this->dataPtr->updateCallbacks.push_back(cb); +} + ///////////////////////////////////////////////// void ComponentInspector::OnPose(double _x, double _y, double _z, double _roll, double _pitch, double _yaw) @@ -1024,10 +1090,11 @@ bool ComponentInspector::NestedModel() const ///////////////////////////////////////////////// void ComponentInspector::OnAddEntity(QString _entity, QString _type) { + std::cout << "Entity[" << _entity.toStdString() << "] Type[" << _type.toStdString() << "] Name[" << this->dataPtr->entityName.c_str() << "]\n"; // 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/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 8a1dea92cc..b79faeadcb 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -122,6 +123,12 @@ namespace gazebo template<> void setData(QStandardItem *_item, const std::ostream &_data); + /// \brief Specialized to set camera data. + /// \param[in] _item Item whose data will be set. + /// \param[in] _data Data to set. + template<> + void setData(QStandardItem *_item, const sdf::Sensor &_data); + /// \brief Set the unit of a given item. /// \param[in] _item Item whose unit will be set. /// \param[in] _unit Unit to be displayed, such as 'm' for meters. @@ -230,6 +237,15 @@ namespace gazebo public: Q_INVOKABLE void OnPose(double _x, double _y, double _z, double _roll, double _pitch, double _yaw); + /// \brief Callback in Qt thread when a camera changes. + /// \param[in] _hfov Horizontal field of view in degrees. + /// \param[in] _imageWidth Image width in pixels. + /// \param[in] _imageHeight Image height in pixels. + /// \param[in] _nearClip Near clip value in meters. + /// \param[in] _farClip Far clip value in meters. + public: Q_INVOKABLE void OnCameraUpdate(double _hfov, int _imageWidth, + int _imageHeight, double _nearClip, double _farClip); + /// \brief Callback in Qt thread when specular changes. /// \param[in] _rSpecular specular red /// \param[in] _gSpecular specular green @@ -336,6 +352,11 @@ namespace gazebo /// \param[in] _type Entity type, e.g. link, visual, collision, etc public: Q_INVOKABLE void OnAddEntity(QString _entity, QString _type); + /// \brief Callback in Qt thread when a sensor is to be added + /// \param[in] _entity Entity to add, e.g. box, sphere, cylinder, etc + /// \param[in] _type Sensor type, e.g. contact, gps, etc + public: Q_INVOKABLE void OnAddSensor(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 da262d0030..dc4b980e18 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -88,6 +88,13 @@ Rectangle { return 6; } + /** + * Forward camera changes to C++ + */ + function onCameraUpdate(_hfov, _width, _height, _near, _far) { + ComponentInspector.OnCameraUpdate(_hfov, _width, _height, _near, _far) + } + /** * Forward pose changes to C++ */ @@ -200,7 +207,7 @@ Rectangle { } ToolButton { - id: addButton + id: addLinkButton checkable: false text: "Add entity" visible: entityType == "model" @@ -220,6 +227,26 @@ Rectangle { } } + ToolButton { + id: addSensorButton + checkable: false + text: "Add sensor" + visible: entityType == "link" + 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 sensor" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + onClicked: { + addSensorMenu.open() + } + } Label { id: entityLabel text: 'Entity ' + ComponentInspector.entity @@ -290,6 +317,7 @@ Rectangle { type: "Joint" } } + // The delegate for each section header Component { id: sectionHeading @@ -326,6 +354,117 @@ Rectangle { // MenuItem { text: "Box" onTriggered: {} } } + Menu { + id: addSensorMenu + MenuItem { + id: airPressure + text: "Air pressure" + onTriggered: { + addSensorMenu.close() + ComponentInspector.OnAddEntity(contact.text, "sensor"); + } + } + + MenuItem { + id: altimeter + text: "Altimeter" + onTriggered: { + addSensorMenu.close() + ComponentInspector.OnAddEntity(altimeter.text, "sensor"); + } + } + + MenuItem { + id: cameraSensorMenu + text: "Camera >" + MouseArea { + id: viewSubCameraArea + anchors.fill: parent + hoverEnabled: true + onEntered: secondCameraMenu.open() + } + } + + MenuItem { + id: contact + text: "Contact" + onTriggered: { + addSensorMenu.close() + ComponentInspector.OnAddEntity(contact.text, "sensor"); + } + } + + MenuItem { + id: forceTorque + text: "Force torque" + onTriggered: { + addSensorMenu.close() + ComponentInspector.OnAddEntity(forceTorque.text, "sensor"); + } + } + + MenuItem { + id: gps + text: "GPS" + onTriggered: { + addSensorMenu.close() + ComponentInspector.OnAddEntity(gps.text, "sensor"); + } + } + + MenuItem { + id: imu + text: "IMU" + onTriggered: { + addSensorMenu.close() + ComponentInspector.OnAddEntity(imu.text, "sensor"); + } + } + + MenuItem { + id: lidar + text: "Lidar" + } + + MenuItem { + id: magnetometer + text: "magnetometer" + onTriggered: { + addSensorMenu.close() + ComponentInspector.OnAddEntity(magnetometer.text, "sensor"); + } + } + } + + Menu { + id: secondCameraMenu + x: addSensorMenu.x + addSensorMenu.width + y: addSensorMenu.y + cameraSensorMenu.y + MenuItem { + id: depth + text: "Depth" + } + MenuItem { + id: logical + text: "Logical" + } + MenuItem { + id: monocular + text: "Monocular" + } + MenuItem { + id: multicamera + text: "Multicamera" + } + MenuItem { + id: rgbd + text: "RGBD" + } + MenuItem { + id: thermal + text: "Thermal" + } + } ListView { anchors.top: header.bottom diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qrc b/src/gui/plugins/component_inspector/ComponentInspector.qrc index b27a184945..421bad1bad 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qrc +++ b/src/gui/plugins/component_inspector/ComponentInspector.qrc @@ -1,6 +1,7 @@ Boolean.qml + Camera.qml ComponentInspector.qml Light.qml NoData.qml diff --git a/src/gui/plugins/model_editor/CMakeLists.txt b/src/gui/plugins/model_editor/CMakeLists.txt index 39f351015e..5ab7511e80 100644 --- a/src/gui/plugins/model_editor/CMakeLists.txt +++ b/src/gui/plugins/model_editor/CMakeLists.txt @@ -4,4 +4,5 @@ gz_add_gui_plugin(ModelEditor PUBLIC_LINK_LIBS ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} ${PROJECT_LIBRARY_TARGET_NAME}-rendering + sdformat${SDF_VER}::sdformat${SDF_VER} ) diff --git a/src/gui/plugins/model_editor/ModelEditor.cc b/src/gui/plugins/model_editor/ModelEditor.cc index d5cf4155aa..d7b1d7550f 100644 --- a/src/gui/plugins/model_editor/ModelEditor.cc +++ b/src/gui/plugins/model_editor/ModelEditor.cc @@ -28,7 +28,11 @@ #include #include +#include #include +#include +#include +#include #include @@ -50,7 +54,7 @@ namespace ignition::gazebo public: void Initialize(); public: void HandleAddEntity(const std::string &_geomOrLightType, - const std::string &_entityType, const std::string &/*_parent*/); + const std::string &_entityType, const Entity /*_parent*/); public: std::string GeomSDFString( const std::string &_geomType) const; @@ -62,6 +66,8 @@ namespace ignition::gazebo public: std::string LinkSDFString( const std::string &_geomType) const; + public: sdf::Sensor CreateSensor(const std::string &_type) const; + /// \brief Generate a unique entity id. /// \return The unique entity id // public: Entity UniqueId() const; @@ -76,7 +82,7 @@ namespace ignition::gazebo public: std::string entityType; /// \brief Parent entity to add the entity to - public: std::string parentEntity; + public: Entity parentEntity; /// \brief Entity Creator API. public: std::unique_ptr entityCreator{nullptr}; @@ -91,6 +97,9 @@ namespace ignition::gazebo /// \brief Links to add to the ECM public: std::vector linksToAdd; + /// \brief Sensors to add to the ECM + public: std::vector sensorsToAdd; + /// \brief Event Manager public: EventManager eventMgr; }; @@ -142,32 +151,35 @@ void ModelEditor::Update(const UpdateInfo &, std::set newEntities; for (auto & linkSdf : this->dataPtr->linksToAdd) { - Entity parent = _ecm.EntityByComponents( + /*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 unique link name std::string linkName = "link"; Entity linkEnt = _ecm.EntityByComponents( - components::Link(), components::ParentEntity(parent), + components::Link(), + components::ParentEntity(this->dataPtr->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::Link(), + components::ParentEntity(this->dataPtr->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, + this->dataPtr->parentEntity); // traverse the tree and add all new entities created by the entity creator // to the set @@ -213,7 +225,7 @@ bool ModelEditor::eventFilter(QObject *_obj, QEvent *_event) { this->dataPtr->geomOrLightType = event->Entity().toStdString(); this->dataPtr->entityType = event->EntityType().toStdString(); - this->dataPtr->parentEntity = event->ParentEntity().toStdString(); + this->dataPtr->parentEntity = event->ParentEntity(); std::cerr << "model editor add entity event " << event->EntityType().toStdString() << " " << this->dataPtr->geomOrLightType << std::endl; @@ -418,10 +430,39 @@ std::string ModelEditorPrivate::LinkSDFString( return linkStr.str(); } +///////////////////////////////////////////////// +sdf::Sensor ModelEditorPrivate::CreateSensor(const std::string &_type) const +{ + sdf::Sensor sensor; + + std::string type; + + // Replace spaces with underscores. + common::replaceAll(type, _type, " ", "_"); + + sensor.SetName("default"); + sensor.SetType(_type); + if (type == "air_pressure") + { + sdf::AirPressure airpressure; + sensor.SetAirPressureSensor(airpressure); + } + else if (type == "altimeter") + { + sdf::Altimeter altimeter; + sensor.SetAltimeterSensor(altimeter); + } + else + { + ignerr << "Unable to create sensor type[" << _type << "]\n"; + } + + return sensor; +} ///////////////////////////////////////////////// void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, - const std::string &_type, const std::string &/*_parentEntity*/) + const std::string &_type, const Entity /*_parentEntity*/) { std::string entType = common::lowercase(_type); std::string geomLightType = common::lowercase(_geomOrLightType); @@ -468,6 +509,16 @@ void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, this->linksToAdd.push_back(linkSdf); } } + else if (entType == "sensor") + { + std::cout << "Adding a sensor\n"; + sdf::Sensor sensor = this->CreateSensor(geomLightType); + if (sensor.Type() != sdf::SensorType::NONE) + { + std::lock_guard lock(this->mutex); + this->sensorsToAdd.push_back(sensor); + } + } } // Register this plugin From 3ca5d9dd72626b72209c422464cfb65e92d1eafc Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Mon, 1 Nov 2021 20:34:45 -0600 Subject: [PATCH 18/55] handle step and support original control service Signed-off-by: Ashton Larkin --- src/SimulationRunner.cc | 58 ++++++++++++++++++++++++++++++++++++++--- src/SimulationRunner.hh | 12 +++++++++ src/gui/GuiRunner.cc | 13 ++++++--- 3 files changed, 76 insertions(+), 7 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index efb40b321b..58fb99fb1a 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -194,14 +194,17 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, this->node = std::make_unique(opts); // TODO(louise) Combine both messages into one. - this->node->Advertise("control", &SimulationRunner::OnWorldControlState, + // TODO(anyone) remove the control service in ign-gazebo7 (only keep the + // control/state service in ign-gazebo7) + this->node->Advertise("control", &SimulationRunner::OnWorldControl, this); + this->node->Advertise("control/state", &SimulationRunner::OnWorldControlState, this); this->node->Advertise("playback/control", &SimulationRunner::OnPlaybackControl, this); ignmsg << "Serving world controls on [" << opts.NameSpace() - << "/control] and [" << opts.NameSpace() << "/playback/control]" - << std::endl; + << "/control], [" << opts.NameSpace() << "/control/state] and [" + << opts.NameSpace() << "/playback/control]" << std::endl; // Publish empty GUI messages for worlds that have no GUI in the beginning. // In the future, support modifying GUI from the server at runtime. @@ -1095,6 +1098,55 @@ void SimulationRunner::SetRunToSimTime( } } +///////////////////////////////////////////////// +bool SimulationRunner::OnWorldControl(const msgs::WorldControl &_req, + msgs::Boolean &_res) +{ + static bool firstTime = true; + if (firstTime) + { + ignwarn << "Calling the control service, which is deprecated. " + << "Call the control/state service instead.\n"; + firstTime = false; + } + + std::lock_guard lock(this->msgBufferMutex); + + WorldControl control; + control.pause = _req.pause(); + + if (_req.multi_step() != 0) + control.multiStep = _req.multi_step(); + else if (_req.step()) + control.multiStep = 1; + + if (_req.has_reset()) + { + control.rewind = _req.reset().all() || _req.reset().time_only(); + + if (_req.reset().model_only()) + { + ignwarn << "Model only reset is not supported." << std::endl; + } + } + + if (_req.seed() != 0) + { + ignwarn << "Changing seed is not supported." << std::endl; + } + + if (_req.has_run_to_sim_time()) + { + control.runToSimTime = std::chrono::seconds(_req.run_to_sim_time().sec()) + + std::chrono::nanoseconds(_req.run_to_sim_time().nsec()); + } + + this->worldControls.push_back(control); + + _res.set_data(true); + return true; +} + ///////////////////////////////////////////////// bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req, msgs::Boolean &_res) diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 984ed4a7e5..debdb501d2 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -361,6 +361,18 @@ namespace ignition /// \param[in] _step Step size. public: void SetStepSize(const ignition::math::clock::duration &_step); + /// \brief World control service callback. This function stores the + /// the request which will then be processed by the ProcessMessages + /// function. + /// \param[in] _req Request from client, currently handling play / pause + /// and multistep. + /// \param[out] _res Response to client, true if successful. + /// \return True for success + /// \TODO(anyone) remove this in ign-gazebo7. It's being replaced by + /// OnWorldControlState + private: bool OnWorldControl(const msgs::WorldControl &_req, + msgs::Boolean &_res); + /// \brief World control service callback. This function stores the /// the request which will then be processed by the ProcessMessages /// function. diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 24902f4f79..50a2c3f075 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -121,7 +121,7 @@ GuiRunner::GuiRunner(const std::string &_worldName) connect(timer, &QTimer::timeout, this, &GuiRunner::UpdatePlugins); timer->start(33); - this->dataPtr->controlService = "/world/" + _worldName + "/control"; + this->dataPtr->controlService = "/world/" + _worldName + "/control/state"; ignition::gui::App()->findChild< ignition::gui::MainWindow *>()->installEventFilter(this); @@ -143,9 +143,14 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) req.mutable_world_control()->CopyFrom( worldControlEvent->WorldControlInfo()); - // If a user presses play, send the GUI's ECM state to the server - // in case the GUI's ECM was modified while paused - if (worldControlEvent->Play()) + // share the GUI's ECM with the server if: + // 1. Play was pressed + // 2. Step was pressed while paused + const auto &info = worldControlEvent->WorldControlInfo(); + const bool pressedStep = info.multi_step() > 0u; + const bool pressedPlay = !info.pause() && !pressedStep; + const bool pressedStepWhilePaused = info.pause() && pressedStep; + if (pressedPlay || pressedStepWhilePaused) req.mutable_state()->CopyFrom(this->dataPtr->ecm.State()); std::function cb = From 8a36e6ee722fda677e26b2f8af2c9b1310c8bb06 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 2 Nov 2021 08:22:40 -0700 Subject: [PATCH 19/55] Reduced code duplication Signed-off-by: Nate Koenig --- src/SimulationRunner.cc | 37 +++---------------------------------- 1 file changed, 3 insertions(+), 34 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 58fb99fb1a..39a4eae8c9 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -1110,41 +1110,10 @@ bool SimulationRunner::OnWorldControl(const msgs::WorldControl &_req, firstTime = false; } - std::lock_guard lock(this->msgBufferMutex); - - WorldControl control; - control.pause = _req.pause(); - - if (_req.multi_step() != 0) - control.multiStep = _req.multi_step(); - else if (_req.step()) - control.multiStep = 1; - - if (_req.has_reset()) - { - control.rewind = _req.reset().all() || _req.reset().time_only(); - - if (_req.reset().model_only()) - { - ignwarn << "Model only reset is not supported." << std::endl; - } - } - - if (_req.seed() != 0) - { - ignwarn << "Changing seed is not supported." << std::endl; - } - - if (_req.has_run_to_sim_time()) - { - control.runToSimTime = std::chrono::seconds(_req.run_to_sim_time().sec()) + - std::chrono::nanoseconds(_req.run_to_sim_time().nsec()); - } - - this->worldControls.push_back(control); + msgs::WorldControlState req; + req.mutable_world_control()->CopyFrom(_req); - _res.set_data(true); - return true; + return this->OnWorldControlState(req, _res); } ///////////////////////////////////////////////// From ae91a9dab88d717124de89a1fd6d1ee5a2045e3b Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 2 Nov 2021 08:47:11 -0700 Subject: [PATCH 20/55] Set gazebo's default to use the event based system Signed-off-by: Nate Koenig --- src/gui/gui.config | 1 + src/gui/playback_gui.config | 1 + 2 files changed, 2 insertions(+) diff --git a/src/gui/gui.config b/src/gui/gui.config index 36166061f8..36782b6b9d 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -136,6 +136,7 @@ true true true + true diff --git a/src/gui/playback_gui.config b/src/gui/playback_gui.config index 50cffa6992..4e6d386447 100644 --- a/src/gui/playback_gui.config +++ b/src/gui/playback_gui.config @@ -60,6 +60,7 @@ true true true + true From ceddf0312269e334472a26fe7c4c0040bec968f6 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 2 Nov 2021 09:36:12 -0700 Subject: [PATCH 21/55] Testing things out Signed-off-by: Nate Koenig --- src/cmd/ModelCommandAPI.cc | 500 +++++++++++++++--- src/cmd/ModelCommandAPI.hh | 8 +- src/cmd/cmdmodel.rb.in | 23 +- .../component_inspector/ComponentInspector.cc | 54 +- .../ComponentInspector.qml | 6 + .../plugins/component_inspector/Pose3d.qml | 21 +- 6 files changed, 511 insertions(+), 101 deletions(-) diff --git a/src/cmd/ModelCommandAPI.cc b/src/cmd/ModelCommandAPI.cc index 6c9b80eb06..53b0ec1824 100644 --- a/src/cmd/ModelCommandAPI.cc +++ b/src/cmd/ModelCommandAPI.cc @@ -21,23 +21,36 @@ #include #include +#include +#include +#include +#include +#include +#include + #include #include #include #include #include +#include +#include #include +#include #include #include #include #include #include +#include #include #include #include #include #include +#include +#include #include #include @@ -117,19 +130,328 @@ std::string entityInfo(const std::string &_name, ////////////////////////////////////////////////// /// \brief Get pose info in a standard way /// \param[in] _pose Pose to print -/// \param[in] _prefix Indentation prefix for every line +/// \param[in] _spaces Number of spaces to indent for every line /// \return Pose formatted in a standard way -std::string poseInfo(math::Pose3d _pose, const std::string &_prefix) +std::string poseInfo(math::Pose3d _pose, int _spaces) { return - _prefix + "[" + std::to_string(_pose.X()) + " | " - + std::to_string(_pose.Y()) + " | " + std::string(_spaces, ' ') + "[" + std::to_string(_pose.X()) + " " + + std::to_string(_pose.Y()) + " " + std::to_string(_pose.Z()) + "]\n" + - _prefix + "[" + std::to_string(_pose.Roll()) + " | " - + std::to_string(_pose.Pitch()) + " | " + std::string(_spaces, ' ') + "[" + std::to_string(_pose.Roll()) + " " + + std::to_string(_pose.Pitch()) + " " + std::to_string(_pose.Yaw()) + "]"; } +////////////////////////////////////////////////// +/// \brief Print pose info about an entity. +/// \param[in] _entity Entity to print pose information for. Nothing is +/// printed if the entity lack a pose component. +/// \param[in] _ecm The entity component manager. +/// \param[in] _spaces Number of spaces to indent for every line +void printPose(const uint64_t _entity, const EntityComponentManager &_ecm, + int _spaces) +{ + const auto poseComp = _ecm.Component(_entity); + if (poseComp) + { + std::cout << std::string(_spaces, ' ') + << "- Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl + << poseInfo(poseComp->Data(), _spaces + 2) << std::endl; + } +} + +////////////////////////////////////////////////// +/// \brief Print noise information. +/// \param[in] _noise Noise to print. +/// \param[in] _spaces Number of spaces to indent for every line. +void printNoise(const sdf::Noise &_noise, int _spaces) +{ + std::cout << std::string(_spaces, ' ') << "- Mean: " << _noise.Mean() << "\n" + << std::string(_spaces, ' ') << "- Bias mean: " + << _noise.BiasMean() << "\n" + << std::string(_spaces, ' ') << "- Standard deviation: " + << _noise.StdDev() << "\n" + << std::string(_spaces, ' ') << "- Bias standard deviation: " + << _noise.BiasStdDev() << "\n" + << std::string(_spaces, ' ') << "- Precision: " + << _noise.Precision() << "\n" + << std::string(_spaces, ' ') << "- Dynamic bias standard deviation: " + << _noise.DynamicBiasStdDev() << "\n" + << std::string(_spaces, ' ') << "- Dynamic bias correlation time: " + << _noise.DynamicBiasCorrelationTime() << std::endl; +} + +////////////////////////////////////////////////// +/// \brief Print info about an altimeter sensor. +/// \param[in] _entity Entity to print information for. Nothing is +/// printed if the entity is not an altimeter. +/// \param[in] _ecm The entity component manager. +/// \param[in] _spaces Number of spaces to indent for every line +void printAltimeter(const uint64_t _entity, const EntityComponentManager &_ecm, + int _spaces) +{ + // Get the type and return if the _entity does not have the correct + // component. + auto comp = _ecm.Component(_entity); + if (!comp) + return; + + const sdf::Sensor &sensor = comp->Data(); + const sdf::Altimeter *altimeter = sensor.AltimeterSensor(); + + std::cout << std::string(_spaces, ' ') << "- Vertical position noise:\n"; + printNoise(altimeter->VerticalPositionNoise(), _spaces + 2); + + std::cout << std::string(_spaces, ' ') << "- Vertical velocity noise:\n"; + printNoise(altimeter->VerticalVelocityNoise(), _spaces + 2); +} + +////////////////////////////////////////////////// +/// \brief Print info about an SDF camera. +/// \param[in] _camera The camera to output +void printCamera(const sdf::Camera *_camera, int _spaces) +{ + std::cout << std::string(_spaces, ' ') + << "- Horizontal field of view (rad): " << _camera->HorizontalFov() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Image width (px): " << _camera->ImageWidth() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Image height (px): " << _camera->ImageHeight() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Near clip (m): " << _camera->NearClip() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Far clip (m): " << _camera->FarClip() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Pixel format: " << _camera->PixelFormatStr() + << std::endl; + + if (_camera->HasDepthCamera()) + { + std::cout << std::string(_spaces, ' ') + << "- Depth near clip (m): " << _camera->DepthNearClip() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Depth far clip (m): " << _camera->DepthFarClip() + << std::endl; + } + + if (_camera->HasSegmentationType()) + { + std::cout << std::string(_spaces, ' ') + << "- Segmentation type: " << _camera->SegmentationType() + << std::endl; + } + + if (_camera->HasBoundingBoxType()) + { + std::cout << std::string(_spaces, ' ') + << "- Bounding box type: " << _camera->BoundingBoxType() + << std::endl; + } + + std::cout << std::string(_spaces, ' ') + << "- Save frames: " << _camera->SaveFrames() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Save frames path: " << _camera->SaveFramesPath() + << std::endl; + + std::cout << std::string(_spaces, ' ') + << "- Image noise:\n"; + printNoise(_camera->ImageNoise(), _spaces + 2); + + std::cout << std::string(_spaces, ' ') + << "- Distortion K1: " << _camera->DistortionK1() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Distortion K2: " << _camera->DistortionK2() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Distortion K3: " << _camera->DistortionK3() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Distortion P1: " << _camera->DistortionP1() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Distortion P2: " << _camera->DistortionP2() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Distortion center: " << _camera->DistortionCenter() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens type: " << _camera->LensType() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens scale to horizontal field of view (rad): " + << _camera->LensScaleToHfov() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens C1: " << _camera->LensC1() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens C2: " << _camera->LensC2() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens C3: " << _camera->LensC3() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens focal length (m): " << _camera->LensFocalLength() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens function: " << _camera->LensFunction() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens cutoff angle (rad): " << _camera->LensCutoffAngle() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens texture size: " << _camera->LensEnvironmentTextureSize() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens intrinsics Fx: " << _camera->LensIntrinsicsFx() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens intrinsics Fy: " << _camera->LensIntrinsicsFy() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens intrinsics Cx: " << _camera->LensIntrinsicsCx() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens intrinsics Cy: " << _camera->LensIntrinsicsCy() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens intrinsics skew: " << _camera->LensIntrinsicsSkew() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Visibility mask: " << _camera->VisibilityMask() + << std::endl; +} + +////////////////////////////////////////////////// +/// \brief Print info about a camera sensor. +/// \param[in] _entity Entity to print information for. Nothing is +/// printed if the entity is not a camera. +/// \param[in] _ecm The entity component manager. +/// \param[in] _spaces Number of spaces to indent for every line +void printRgbdCamera(const uint64_t _entity, const EntityComponentManager &_ecm, + int _spaces) +{ + // Get the type and return if the _entity does not have the correct + // component. + auto comp = _ecm.Component(_entity); + if (!comp) + return; + + const sdf::Sensor &sensor = comp->Data(); + const sdf::Camera *camera = sensor.CameraSensor(); + + printCamera(camera, _spaces); +} + +////////////////////////////////////////////////// +/// \brief Print info about a camera sensor. +/// \param[in] _entity Entity to print information for. Nothing is +/// printed if the entity is not a camera. +/// \param[in] _ecm The entity component manager. +/// \param[in] _spaces Number of spaces to indent for every line +void printCamera(const uint64_t _entity, const EntityComponentManager &_ecm, + int _spaces) +{ + // Get the type and return if the _entity does not have the correct + // component. + auto comp = _ecm.Component(_entity); + if (!comp) + return; + + const sdf::Sensor &sensor = comp->Data(); + const sdf::Camera *camera = sensor.CameraSensor(); + + printCamera(camera, _spaces); +} + +////////////////////////////////////////////////// +/// \brief Print info about an imu sensor. +/// \param[in] _entity Entity to print information for. Nothing is +/// printed if the entity is not an IMU. +/// \param[in] _ecm The entity component manager. +/// \param[in] _spaces Number of spaces to indent for every line +void printImu(const uint64_t _entity, const EntityComponentManager &_ecm, + int _spaces) +{ + // Get the type and return if the _entity does not have the correct + // component. + auto comp = _ecm.Component(_entity); + if (!comp) + return; + + const sdf::Sensor &sensor = comp->Data(); + const sdf::Imu *imu = sensor.ImuSensor(); + + std::cout << std::string(_spaces, ' ') + << "- Linear acceleration X-axis noise:\n"; + printNoise(imu->LinearAccelerationXNoise(), _spaces + 2); + std::cout << std::string(_spaces, ' ') + << "- Linear acceleration Y-axis noise:\n"; + printNoise(imu->LinearAccelerationYNoise(), _spaces + 2); + std::cout << std::string(_spaces, ' ') + << "- Linear acceleration Z-axis noise:\n"; + printNoise(imu->LinearAccelerationZNoise(), _spaces + 2); + + std::cout << std::string(_spaces, ' ') + << "- Angular velocity X-axis noise:\n"; + printNoise(imu->AngularVelocityXNoise(), _spaces + 2); + std::cout << std::string(_spaces, ' ') + << "- Angular velocity Y-axis noise:\n"; + printNoise(imu->AngularVelocityYNoise(), _spaces + 2); + std::cout << std::string(_spaces, ' ') + << "- Angular velocity Z-axis noise:\n"; + printNoise(imu->AngularVelocityZNoise(), _spaces + 2); + + std::cout << std::string(_spaces, ' ') + << "- Gravity direction X [XYZ]: " + << imu->GravityDirX() << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Gravity direction X parent frame:" << imu->GravityDirXParentFrame() + << std::endl; + + std::cout << std::string(_spaces, ' ') + << "- Localization:" << imu->Localization() << std::endl; + + std::cout << std::string(_spaces, ' ') + << "- Custom RPY: " << imu->CustomRpy() << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Custom RPY parent frame:" << imu->CustomRpyParentFrame() << std::endl; + + std::cout << std::string(_spaces, ' ') + << "- Orientation enabled:" << imu->OrientationEnabled() << std::endl; +} + +////////////////////////////////////////////////// +void printMagnetometer(const uint64_t _entity, + const EntityComponentManager &_ecm, int _spaces) +{ + // Get the type and return if the _entity does not have the correct + // component. + auto comp = _ecm.Component(_entity); + if (!comp) + return; + + const sdf::Sensor &sensor = comp->Data(); + const sdf::Magnetometer *mag = sensor.MagnetometerSensor(); + + std::cout << std::string(_spaces, ' ') << "- X-axis noise:\n"; + printNoise(mag->XNoise(), _spaces + 2); + std::cout << std::string(_spaces, ' ') << "- Y-axis noise:\n"; + printNoise(mag->YNoise(), _spaces + 2); + std::cout << std::string(_spaces, ' ') << "- Z-axis noise:\n"; + printNoise(mag->ZNoise(), _spaces + 2); +} + ////////////////////////////////////////////////// // \brief Set the state of a ECM instance with a world snapshot. // \param _ecm ECM instance to be populated. @@ -189,9 +511,8 @@ void printModelInfo(const uint64_t _entity, if (poseComp && nameComp) { std::cout << "Model: [" << _entity << "]" << std::endl - << " - Name: " << nameComp->Data() << std::endl - << " - Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl - << poseInfo(poseComp->Data(), " ") << std::endl; + << " - Name: " << nameComp->Data() << std::endl; + printPose(_entity, _ecm, 2); } } @@ -202,7 +523,9 @@ void printModelInfo(const uint64_t _entity, // \param[in] _linkName Link to be printed, if empty, print all links. void printLinks(const uint64_t _modelEntity, const EntityComponentManager &_ecm, - const std::string &_linkName) + const std::string &_linkName, + const std::string &_sensorName, + int _spaces) { const auto links = _ecm.EntitiesByComponents( components::ParentEntity(_modelEntity), components::Link()); @@ -210,46 +533,79 @@ void printLinks(const uint64_t _modelEntity, { const auto nameComp = _ecm.Component(entity); + int spaces = _spaces; + if (_linkName.length() && _linkName != nameComp->Data()) continue; - std::cout << " - Link [" << entity << "]" << std::endl - << " - Name: " << nameComp->Data() << std::endl - << " - Parent: " << entityInfo(_modelEntity, _ecm) - << std::endl; - - const auto inertialComp = _ecm.Component(entity); - - if (inertialComp) + if (_sensorName.empty()) { - const auto inertialMatrix = inertialComp->Data().MassMatrix(); - const auto mass = inertialComp->Data().MassMatrix().Mass(); - - const std::string massInfo = "[" + std::to_string(mass) + "]"; - const std::string inertialInfo = - "\n [" + std::to_string(inertialMatrix.Ixx()) + " | " - + std::to_string(inertialMatrix.Ixy()) + " | " - + std::to_string(inertialMatrix.Ixz()) + "]\n" - " [" + std::to_string(inertialMatrix.Ixy()) + " | " - + std::to_string(inertialMatrix.Iyy()) + " | " - + std::to_string(inertialMatrix.Iyz()) + "]\n" - " [" + std::to_string(inertialMatrix.Ixz()) + " | " - + std::to_string(inertialMatrix.Iyz()) + " | " - + std::to_string(inertialMatrix.Izz()) + "]"; - std::cout << " - Mass (kg): " << massInfo << std::endl - << " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:" - << std::endl - << poseInfo(inertialComp->Data().Pose(), " ") - << std::endl - << " - Inertial Matrix (kg.m^2):" - << inertialInfo << std::endl; + std::cout << std::string(spaces, ' ') + << "- Link [" << entity << "]" << std::endl + << std::string(spaces + 2, ' ') + << "- Name: " << nameComp->Data() << std::endl + << std::string(spaces + 2, ' ') + << "- Parent: " << entityInfo(_modelEntity, _ecm) + << std::endl; + + const auto inertialComp = _ecm.Component(entity); + + if (inertialComp) + { + const auto inertialMatrix = inertialComp->Data().MassMatrix(); + const auto mass = inertialComp->Data().MassMatrix().Mass(); + + std::cout << std::string(spaces + 2, ' ') + << "- Mass (kg): " << std::to_string(mass) << std::endl + << std::string(spaces + 2, ' ') + << "- Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:" + << std::endl + << poseInfo(inertialComp->Data().Pose(), spaces + 4) + << std::endl + << std::string(spaces + 2, ' ') + << "- Inertial Matrix (kg.m^2):\n" + << std::string(spaces + 4, ' ') << "[" + << std::to_string(inertialMatrix.Ixx()) << " " + << std::to_string(inertialMatrix.Ixy()) << " " + << std::to_string(inertialMatrix.Ixz()) << "]\n" + << std::string(spaces + 4, ' ') << "[" + << std::to_string(inertialMatrix.Ixy()) << " " + << std::to_string(inertialMatrix.Iyy()) << " " + << std::to_string(inertialMatrix.Iyz()) << "]\n" + << std::string(spaces + 4, ' ') << "[" + << std::to_string(inertialMatrix.Ixz()) << " " + << std::to_string(inertialMatrix.Iyz()) << " " + << std::to_string(inertialMatrix.Izz()) << "]" + << std::endl; + } + + printPose(entity, _ecm, spaces + 2); + + spaces += 2; } - const auto poseComp = _ecm.Component(entity); - if (poseComp) + const auto sensors = _ecm.EntitiesByComponents( + components::ParentEntity(entity), components::Sensor()); + for (const auto &sensor : sensors) { - std::cout << " - Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl - << poseInfo(poseComp->Data(), " ") << std::endl; + const auto sensorNameComp = _ecm.Component(sensor); + if (!_sensorName.empty() && _sensorName != sensorNameComp->Data()) + continue; + + std::cout << std::string(spaces, ' ') + << "- Sensor [" << sensor << "]\n"; + std::cout << std::string(spaces + 2, ' ') + << "- Name: " << sensorNameComp->Data() << "\n" + << std::string(spaces + 2, ' ') + << "- Parent: " << entityInfo(_modelEntity, _ecm) << std::endl; + printPose(sensor, _ecm, spaces + 2); + + // Run through all the sensor print statements. Each function will + // exit early if the the sensor is the wrong type. + printAltimeter(sensor, _ecm, spaces + 2); + printCamera(sensor, _ecm, spaces + 2); + printImu(sensor, _ecm, spaces + 2); + printRgbdCamera(sensor, _ecm, spaces + 2); } } } @@ -261,7 +617,8 @@ void printLinks(const uint64_t _modelEntity, // \param[in] _jointName Joint to be printed, if nullptr, print all joints. void printJoints(const uint64_t _modelEntity, const EntityComponentManager &_ecm, - const std::string &_jointName) + const std::string &_jointName, + int _spaces) { static const std::map jointTypes = { @@ -286,16 +643,22 @@ void printJoints(const uint64_t _modelEntity, if (_jointName.length() && _jointName != nameComp->Data()) continue; - std::cout << " - Joint [" << entity << "]" << std::endl - << " - Name: " << nameComp->Data() << std::endl - << " - Parent: " << entityInfo(_modelEntity, _ecm) - << std::endl; + int spaces = _spaces; + std::cout << std::string(_spaces, ' ') + << "- Joint [" << entity << "]" << std::endl; + spaces += 2; + + std::cout << std::string(spaces, ' ') + << "- Name: " << nameComp->Data() << std::endl + << std::string(spaces, ' ') + << "- Parent: " << entityInfo(_modelEntity, _ecm) + << std::endl; const auto jointTypeComp = _ecm.Component(entity); if (jointTypeComp) { - std::cout << " - Type: " << jointTypes.at(jointTypeComp->Data()) - << std::endl; + std::cout << std::string(spaces, ' ') + << "- Type: " << jointTypes.at(jointTypeComp->Data()) << std::endl; } const auto childLinkComp = @@ -305,26 +668,26 @@ void printJoints(const uint64_t _modelEntity, if (childLinkComp && parentLinkComp) { - std::cout << " - Parent Link: " + std::cout << std::string(spaces, ' ') << "- Parent Link: " << entityInfo(parentLinkComp->Data(), _ecm) << "\n" - << " - Child Link: " + << std::string(spaces, ' ') << "- Child Link: " << entityInfo(childLinkComp->Data(), _ecm) << "\n"; } const auto poseComp = _ecm.Component(entity); if (poseComp) { - std::cout << " - Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl - << poseInfo(poseComp->Data(), " ") << std::endl; + std::cout << std::string(spaces, ' ') + << "- Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl + << poseInfo(poseComp->Data(), spaces + 2) << std::endl; } const auto axisComp = _ecm.Component(entity); if (axisComp) { - std::cout << " - Axis unit vector [ XYZ ]:\n" - " [" << axisComp->Data().Xyz().X() << " | " - << axisComp->Data().Xyz().Y() << " | " - << axisComp->Data().Xyz().Z() << "]\n"; + std::cout << std::string(spaces, ' ') << "- Axis unit vector [ XYZ ]:\n" + << std::string(spaces + 2, ' ') << "[" << axisComp->Data().Xyz() + << "]\n"; } } } @@ -366,7 +729,7 @@ extern "C" void cmdModelList() ////////////////////////////////////////////////// extern "C" void cmdModelInfo( const char *_modelName, int _pose, const char *_linkName, - const char *_jointName) + const char *_jointName, const char *_sensorName) { std::string linkName{""}; if (_linkName) @@ -374,8 +737,11 @@ extern "C" void cmdModelInfo( std::string jointName{""}; if (_jointName) jointName = _jointName; + std::string sensorName{""}; + if (_sensorName) + sensorName = _sensorName; bool printAll{false}; - if (!_pose && !_linkName && !_jointName) + if (!_pose && !_linkName && !_jointName && !_sensorName) printAll = true; if (!_modelName) @@ -395,15 +761,19 @@ extern "C" void cmdModelInfo( if (entity == kNullEntity) std::cout << "No model named <" << _modelName << "> was found" << std::endl; + int spaces = 0; // Get the pose of the model - if (printAll | _pose) + if (printAll || _pose) + { printModelInfo(entity, ecm); + spaces += 2; + } // Get the links information - if (printAll | (_linkName != nullptr)) - printLinks(entity, ecm, linkName); + if (printAll || _linkName != nullptr || _sensorName != nullptr) + printLinks(entity, ecm, linkName, sensorName, spaces); // Get the joints information - if (printAll | (_jointName != nullptr)) - printJoints(entity, ecm, jointName); + if (printAll || (_jointName != nullptr)) + printJoints(entity, ecm, jointName, spaces); } diff --git a/src/cmd/ModelCommandAPI.hh b/src/cmd/ModelCommandAPI.hh index 5cd2ed761f..2ca7248c60 100644 --- a/src/cmd/ModelCommandAPI.hh +++ b/src/cmd/ModelCommandAPI.hh @@ -21,9 +21,11 @@ extern "C" void cmdModelList(); /// \brief External hook to dump model information. /// \param[in] _modelName Model name. /// \param[in] _pose --pose option. -/// \param[in] _link_name Link name. -/// \param[in] _joint_name Joint name. +/// \param[in] _linkName Link name. +/// \param[in] _jointName Joint name. +/// \param[in] _sensorName Sensor name. extern "C" void cmdModelInfo( const char *_modelName, int _pose, const char *_linkName, - const char *_jointName); + const char *_jointName, + const char *_sensorName); diff --git a/src/cmd/cmdmodel.rb.in b/src/cmd/cmdmodel.rb.in index 8070bfec63..88e65a50f3 100644 --- a/src/cmd/cmdmodel.rb.in +++ b/src/cmd/cmdmodel.rb.in @@ -55,6 +55,14 @@ COMMANDS = { 'model' => " caster link in the diff_drive world, run: \n"\ " ign model -m vehicle_blue -l caster \n"\ " \n"\ + " -s [--sensor] arg Select a sensor to show its properties. \n"\ + " If no arg is passed all sensors are printed\n"\ + " Requires the -m and -l options \n"\ + " \n"\ + " E.g. to get information about the \n"\ + " imu sensor in the sensors world, run: \n"\ + " ign model -m sensors_box -l link -s imu \n"\ + " \n"\ " -j [--joint] arg Select a joint to show its properties. \n"\ " If no arg is passed all joints are printed \n"\ " Requires the -m option \n"\ @@ -78,7 +86,8 @@ class Cmd options = { 'pose' => 0, 'link_name' => 0, - 'joint_name' => 0 + 'joint_name' => 0, + 'sensor_name' => 0 } usage = COMMANDS[args[0]] @@ -104,8 +113,14 @@ class Cmd options['link_name'] = l end end + opts.on('-s', '--sensor [arg]', String, + 'Request sensor information') do |s| + options['sensor_name'] = '' + if s + options['sensor_name'] = s + end + end opts.on('-j', '--joint [arg]', String, 'Request joint information') do |j| - # options['joint'] = 1 options['joint_name'] = '' if j options['joint_name'] = j @@ -166,9 +181,9 @@ class Cmd exit(0) elsif options.key?('model_name') Importer.extern 'void cmdModelInfo(const char *, int, const char *, - const char *)' + const char *, const char *)' Importer.cmdModelInfo(options['model_name'], options['pose'], - options['link_name'], options['joint_name']) + options['link_name'], options['joint_name'], options['sensor_name']) else puts 'Command error: I do not have an implementation for '\ "command [ign #{options['command']}]." diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index bb1afc9cba..4c6dede641 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -100,7 +100,7 @@ namespace ignition::gazebo public: bool locked{false}; /// \brief Whether updates are currently paused. - public: bool paused{false}; + public: bool paused{true}; /// \brief Transport node for making command requests public: transport::Node node; @@ -434,9 +434,10 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *) } ////////////////////////////////////////////////// -void ComponentInspector::Update(const UpdateInfo &, +void ComponentInspector::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { + this->SetPaused(_info.paused); IGN_PROFILE("ComponentInspector::Update"); auto componentTypes = _ecm.ComponentTypes(this->dataPtr->entity); @@ -855,7 +856,6 @@ int ComponentInspector::Entity() const ///////////////////////////////////////////////// void ComponentInspector::SetEntity(const int &_entity) { - std::cout << "SetEntity\n"; // If nothing is selected, display world properties if (_entity == kNullEntity) { @@ -903,8 +903,11 @@ bool ComponentInspector::Paused() const ///////////////////////////////////////////////// void ComponentInspector::SetPaused(bool _paused) { - this->dataPtr->paused = _paused; - this->PausedChanged(); + if (this->dataPtr->paused != _paused) + { + this->dataPtr->paused = _paused; + this->PausedChanged(); + } } ///////////////////////////////////////////////// @@ -913,7 +916,7 @@ void ComponentInspector::OnCameraUpdate( double _farClip) { std::function cb = - [this, _hfov](EntityComponentManager &_ecm) + [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component(this->dataPtr->entity); if (comp) @@ -942,20 +945,37 @@ void ComponentInspector::OnCameraUpdate( void ComponentInspector::OnPose(double _x, double _y, double _z, double _roll, double _pitch, double _yaw) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + std::cout << "OnPose\n\n"; + std::function cb = + [=](EntityComponentManager &_ecm) { - if (!_result) - ignerr << "Error setting pose" << std::endl; + auto comp = _ecm.Component(this->dataPtr->entity); + if (comp) + { + comp->Data().Set(_x, _y, _z, _roll, _pitch, _yaw); + } + else + { + ignerr << "Unable to set the pose of entity[" + << this->dataPtr->entity << "]\n"; + } }; + this->dataPtr->updateCallbacks.push_back(cb); - ignition::msgs::Pose req; - req.set_id(this->dataPtr->entity); - msgs::Set(req.mutable_position(), math::Vector3d(_x, _y, _z)); - msgs::Set(req.mutable_orientation(), math::Quaterniond(_roll, _pitch, _yaw)); - auto poseCmdService = "/world/" + this->dataPtr->worldName - + "/set_pose"; - this->dataPtr->node.Request(poseCmdService, req, cb); +// std::function cb = +// [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) +// { +// if (!_result) +// ignerr << "Error setting pose" << std::endl; +// }; +// +// ignition::msgs::Pose req; +// req.set_id(this->dataPtr->entity); +// msgs::Set(req.mutable_position(), math::Vector3d(_x, _y, _z)); +// msgs::Set(req.mutable_orientation(), math::Quaterniond(_roll, _pitch, _yaw)); +// auto poseCmdService = "/world/" + this->dataPtr->worldName +// + "/set_pose"; +// this->dataPtr->node.Request(poseCmdService, req, cb); } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index dc4b980e18..3932f7165e 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -50,6 +50,12 @@ Rectangle { */ property bool nestedModel : ComponentInspector.nestedModel + /** + * Get if simulation is paused or not + */ + property bool paused : ComponentInspector.paused + + /** * Light grey according to theme */ diff --git a/src/gui/plugins/component_inspector/Pose3d.qml b/src/gui/plugins/component_inspector/Pose3d.qml index 56d847a7e2..7a589bb9b5 100644 --- a/src/gui/plugins/component_inspector/Pose3d.qml +++ b/src/gui/plugins/component_inspector/Pose3d.qml @@ -38,12 +38,6 @@ Rectangle { // Maximum spinbox value property double spinMax: 1000000 - // Read-only / write - property bool readOnly: { - var isModel = entityType == "model" - return !(isModel) || nestedModel - } - property int iconWidth: 20 property int iconHeight: 20 @@ -65,6 +59,8 @@ Rectangle { // Loaded item for yaw property var yawItem: {} + + // Send new pose to C++ function sendPose() { // TODO(anyone) There's a loss of precision when these values get to C++ @@ -252,7 +248,8 @@ Rectangle { id: xLoader anchors.fill: parent property double numberValue: model.data[0] - sourceComponent: readOnly ? readOnlyNumber : writableNumber + sourceComponent: writableNumber + active: paused onLoaded: { xItem = xLoader.item } @@ -289,7 +286,7 @@ Rectangle { id: rollLoader anchors.fill: parent property double numberValue: model.data[3] - sourceComponent: readOnly ? readOnlyNumber : writableNumber + sourceComponent: paused ? writableNumber : readOnlyNumber onLoaded: { rollItem = rollLoader.item } @@ -332,7 +329,7 @@ Rectangle { id: yLoader anchors.fill: parent property double numberValue: model.data[1] - sourceComponent: readOnly ? readOnlyNumber : writableNumber + sourceComponent: paused ? writableNumber : readOnlyNumber onLoaded: { yItem = yLoader.item } @@ -368,7 +365,7 @@ Rectangle { id: pitchLoader anchors.fill: parent property double numberValue: model.data[4] - sourceComponent: readOnly ? readOnlyNumber : writableNumber + sourceComponent: paused ? writableNumber : readOnlyNumber onLoaded: { pitchItem = pitchLoader.item } @@ -404,7 +401,7 @@ Rectangle { id: zLoader anchors.fill: parent property double numberValue: model.data[2] - sourceComponent: readOnly ? readOnlyNumber : writableNumber + sourceComponent: paused ? writableNumber : readOnlyNumber onLoaded: { zItem = zLoader.item } @@ -440,7 +437,7 @@ Rectangle { id: yawLoader anchors.fill: parent property double numberValue: model.data[5] - sourceComponent: readOnly ? readOnlyNumber : writableNumber + sourceComponent: paused ? writableNumber : readOnlyNumber onLoaded: { yawItem = yawLoader.item } From aa3fb4348497f0f81ada39f73cb99eed519b294a Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 2 Nov 2021 11:02:58 -0700 Subject: [PATCH 22/55] updates Signed-off-by: Nate Koenig --- examples/plugin/rendering_plugins/rendering_plugins.sdf | 1 + examples/scripts/log_video_recorder/log_video_recorder.sdf | 1 + examples/worlds/camera_sensor.sdf | 1 + examples/worlds/fuel_textured_mesh.sdf | 1 + examples/worlds/grid.sdf | 1 + examples/worlds/minimal_scene.sdf | 1 + examples/worlds/optical_tactile_sensor_plugin.sdf | 1 + examples/worlds/plot_3d.sdf | 1 + examples/worlds/segmentation_camera.sdf | 1 + examples/worlds/sensors_demo.sdf | 1 + examples/worlds/sky.sdf | 1 + examples/worlds/thermal_camera.sdf | 1 + examples/worlds/tunnel.sdf | 1 + examples/worlds/video_record_dbl_pendulum.sdf | 1 + examples/worlds/visibility.sdf | 1 + examples/worlds/visualize_contacts.sdf | 1 + examples/worlds/visualize_lidar.sdf | 1 + 17 files changed, 17 insertions(+) diff --git a/examples/plugin/rendering_plugins/rendering_plugins.sdf b/examples/plugin/rendering_plugins/rendering_plugins.sdf index eb56737121..68f77eb736 100644 --- a/examples/plugin/rendering_plugins/rendering_plugins.sdf +++ b/examples/plugin/rendering_plugins/rendering_plugins.sdf @@ -92,6 +92,7 @@ true true true + true diff --git a/examples/scripts/log_video_recorder/log_video_recorder.sdf b/examples/scripts/log_video_recorder/log_video_recorder.sdf index e2cdc72a9d..e333935ddf 100644 --- a/examples/scripts/log_video_recorder/log_video_recorder.sdf +++ b/examples/scripts/log_video_recorder/log_video_recorder.sdf @@ -88,6 +88,7 @@ true /world/default/control /world/default/stats + true diff --git a/examples/worlds/camera_sensor.sdf b/examples/worlds/camera_sensor.sdf index 48d052d355..8e8c090728 100644 --- a/examples/worlds/camera_sensor.sdf +++ b/examples/worlds/camera_sensor.sdf @@ -105,6 +105,7 @@ true true true + true diff --git a/examples/worlds/fuel_textured_mesh.sdf b/examples/worlds/fuel_textured_mesh.sdf index 8b9e9147b2..2f42cf7608 100644 --- a/examples/worlds/fuel_textured_mesh.sdf +++ b/examples/worlds/fuel_textured_mesh.sdf @@ -108,6 +108,7 @@ true true true + true diff --git a/examples/worlds/grid.sdf b/examples/worlds/grid.sdf index efab4d2549..affe914178 100644 --- a/examples/worlds/grid.sdf +++ b/examples/worlds/grid.sdf @@ -80,6 +80,7 @@ true true true + true diff --git a/examples/worlds/minimal_scene.sdf b/examples/worlds/minimal_scene.sdf index 97254712ab..470a2bfc3c 100644 --- a/examples/worlds/minimal_scene.sdf +++ b/examples/worlds/minimal_scene.sdf @@ -143,6 +143,7 @@ Features: true true true + true diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index d458079a15..06262adb00 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -110,6 +110,7 @@ true true true + true diff --git a/examples/worlds/plot_3d.sdf b/examples/worlds/plot_3d.sdf index 59b7c09c6a..ed66fbf3a6 100644 --- a/examples/worlds/plot_3d.sdf +++ b/examples/worlds/plot_3d.sdf @@ -84,6 +84,7 @@ true true true + true diff --git a/examples/worlds/segmentation_camera.sdf b/examples/worlds/segmentation_camera.sdf index 4e0840a37e..30d515cb4c 100644 --- a/examples/worlds/segmentation_camera.sdf +++ b/examples/worlds/segmentation_camera.sdf @@ -98,6 +98,7 @@ true true true + true diff --git a/examples/worlds/sensors_demo.sdf b/examples/worlds/sensors_demo.sdf index c679e2ebeb..31374a1a3e 100644 --- a/examples/worlds/sensors_demo.sdf +++ b/examples/worlds/sensors_demo.sdf @@ -100,6 +100,7 @@ true true true + true diff --git a/examples/worlds/sky.sdf b/examples/worlds/sky.sdf index c22442bac2..be14b395c5 100644 --- a/examples/worlds/sky.sdf +++ b/examples/worlds/sky.sdf @@ -83,6 +83,7 @@ Currently only supported using ogre2 rendering engine plugin. true true true + true diff --git a/examples/worlds/thermal_camera.sdf b/examples/worlds/thermal_camera.sdf index 03be5f2d64..cb37b61619 100644 --- a/examples/worlds/thermal_camera.sdf +++ b/examples/worlds/thermal_camera.sdf @@ -101,6 +101,7 @@ true true true + true diff --git a/examples/worlds/tunnel.sdf b/examples/worlds/tunnel.sdf index 496c2a1781..583b313641 100644 --- a/examples/worlds/tunnel.sdf +++ b/examples/worlds/tunnel.sdf @@ -129,6 +129,7 @@ true true true + true diff --git a/examples/worlds/video_record_dbl_pendulum.sdf b/examples/worlds/video_record_dbl_pendulum.sdf index d7cf86abbf..5f0775c046 100644 --- a/examples/worlds/video_record_dbl_pendulum.sdf +++ b/examples/worlds/video_record_dbl_pendulum.sdf @@ -180,6 +180,7 @@ true true true + true diff --git a/examples/worlds/visibility.sdf b/examples/worlds/visibility.sdf index 33480d9de5..31bec366b3 100644 --- a/examples/worlds/visibility.sdf +++ b/examples/worlds/visibility.sdf @@ -112,6 +112,7 @@ true true true + true diff --git a/examples/worlds/visualize_contacts.sdf b/examples/worlds/visualize_contacts.sdf index 357e4b5adb..f39994dced 100644 --- a/examples/worlds/visualize_contacts.sdf +++ b/examples/worlds/visualize_contacts.sdf @@ -102,6 +102,7 @@ Contacts will be visualized as blue spheres and green cylinders. true true true + true diff --git a/examples/worlds/visualize_lidar.sdf b/examples/worlds/visualize_lidar.sdf index 7d01bf2a76..d38fe690ce 100644 --- a/examples/worlds/visualize_lidar.sdf +++ b/examples/worlds/visualize_lidar.sdf @@ -96,6 +96,7 @@ true true true + true From 4c81d382ceaf8e1dfd03609c529ad8558731d74b Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 2 Nov 2021 16:05:40 -0700 Subject: [PATCH 23/55] 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 24/55] 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 25/55] 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 26/55] 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 27/55] 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 5624de82d5af60dcd80d63ea8011daf237edf167 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 8 Nov 2021 10:55:00 -0800 Subject: [PATCH 28/55] Fix build Signed-off-by: Nate Koenig --- src/SimulationRunner.hh | 12 ------------ src/gui/plugins/model_editor/ModelEditor.cc | 15 ++++++--------- 2 files changed, 6 insertions(+), 21 deletions(-) diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 443c4502e6..9f17d59b6b 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -379,8 +379,6 @@ namespace ignition /// and multistep. /// \param[out] _res Response to client, true if successful. /// \return True for success - /// \TODO(anyone) remove this in ign-gazebo7. It's being replaced by - /// OnWorldControlState private: bool OnWorldControl(const msgs::WorldControl &_req, msgs::Boolean &_res); @@ -394,16 +392,6 @@ namespace ignition private: bool OnWorldControlState(const msgs::WorldControlState &_req, msgs::Boolean &_res); - /// \brief World control service callback. This function stores the - /// the request which will then be processed by the ProcessMessages - /// function. - /// \param[in] _req Request from client, currently handling play / pause - /// and multistep. This also may contain SerializedState information. - /// \param[out] _res Response to client, true if successful. - /// \return True for success - private: bool OnWorldControlState(const msgs::WorldControlState &_req, - msgs::Boolean &_res); - /// \brief World control service callback. This function stores the /// the request which will then be processed by the ProcessMessages /// function. diff --git a/src/gui/plugins/model_editor/ModelEditor.cc b/src/gui/plugins/model_editor/ModelEditor.cc index a9f0e3d0a2..fd11c3dcf6 100644 --- a/src/gui/plugins/model_editor/ModelEditor.cc +++ b/src/gui/plugins/model_editor/ModelEditor.cc @@ -162,12 +162,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 specified." << std::endl; continue; } @@ -176,20 +173,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 From a246fdd3e4ee08739901bb3d2271d36998f971d6 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 8 Nov 2021 11:07:18 -0800 Subject: [PATCH 29/55] use entity instead of entity name Signed-off-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 c3c6862c3251b5e8eef9c165f422a3dae7db14c3 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 8 Nov 2021 14:23:47 -0800 Subject: [PATCH 30/55] Update link add menu Signed-off-by: Nate Koenig --- include/ignition/gazebo/gui/GuiEvents.hh | 11 +- .../component_inspector/ComponentInspector.cc | 33 ++- .../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, 277 insertions(+), 146 deletions(-) diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 0d2bf90952..f2715b858c 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) + QString _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: QString 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 a3a7844a2a..7f64e7cbad 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,45 @@ 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, QString(this->dataPtr->entityName.c_str())); + _entity, _type, QString(this->dataPtr->entityName.c_str()), + 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, QString(this->dataPtr->entityName.c_str()), + 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 b8bf80c8e0..4f04cc23ed 100644 --- a/src/gui/plugins/model_editor/ModelEditor.cc +++ b/src/gui/plugins/model_editor/ModelEditor.cc @@ -47,6 +47,9 @@ namespace ignition::gazebo /// \brief Type of entity to add public: std::string entityType; + /// \brief Entity URI, such as a URI for a mesh. + public: std::string uri; + /// \brief Name of parent entity to add the entity to public: std::string parentName; }; @@ -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] _parentName 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, const std::string &_parentName); + const std::string &_entityType, const std::string &_parentName, + 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("" + @@ -224,7 +227,8 @@ bool ModelEditor::eventFilter(QObject *_obj, QEvent *_event) { this->dataPtr->HandleAddEntity(event->Entity().toStdString(), event->EntityType().toStdString(), - event->ParentEntity().toStdString()); + event->ParentEntity().toStdString(), + event->Uri().toStdString()); } } @@ -233,20 +237,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" @@ -265,7 +268,7 @@ std::string ModelEditorPrivate::LightSDFString( << "0.8" << ""; } - else if (_lightType == "point") + else if (_eta.geomOrLightType == "point") { lightStr << "false" @@ -280,7 +283,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(); } @@ -289,27 +293,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 << "" @@ -317,7 +320,7 @@ std::string ModelEditorPrivate::GeomSDFString( << " " << size.Z() << "" << ""; } - else if (_geomType == "capsule") + else if (_eta.geomOrLightType == "capsule") { geomStr << "" @@ -325,16 +328,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(); } @@ -344,22 +355,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 @@ -367,7 +377,7 @@ std::string ModelEditorPrivate::LinkSDFString( } else { - geomOrLightStr = this->GeomSDFString(_geomOrLightType); + geomOrLightStr = this->GeomSDFString(_eta); linkStr << "" << " " @@ -387,7 +397,8 @@ std::string ModelEditorPrivate::LinkSDFString( ///////////////////////////////////////////////// void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, - const std::string &_type, const std::string &_parentName) + const std::string &_type, const std::string &_parentName, + const std::string &_uri) { std::lock_guard lock(this->mutex); std::string entType = common::lowercase(_type); @@ -397,6 +408,7 @@ void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, eta.entityType = entType; eta.geomOrLightType = geomLightType; eta.parentName = _parentName; + eta.uri = _uri; this->entitiesToAdd.push_back(eta); } From 2ba0982334f43be0dd5e2abfa832ea1654f8e914 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 8 Nov 2021 14:50:42 -0800 Subject: [PATCH 31/55] Updates Signed-off-by: Nate Koenig --- .../component_inspector/ComponentInspector.cc | 8 +- .../component_inspector/ComponentInspector.hh | 6 +- .../ComponentInspector.qml | 134 ++++++++++++++++++ 3 files changed, 142 insertions(+), 6 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 07bf8ee395..59b9bf1d37 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -294,7 +294,7 @@ void ignition::gazebo::setData(QStandardItem *_item, } ////////////////////////////////////////////////// -template<> +/*template<> void ignition::gazebo::setData(QStandardItem *_item, const sdf::Sensor &_data) { if (nullptr == _item) @@ -311,7 +311,7 @@ void ignition::gazebo::setData(QStandardItem *_item, const sdf::Sensor &_data) QVariant(cam->NearClip()), QVariant(cam->FarClip()), }), ComponentsModel::RoleNames().key("data")); -} +}*/ ////////////////////////////////////////////////// void ignition::gazebo::setUnit(QStandardItem *_item, const std::string &_unit) @@ -909,7 +909,7 @@ void ComponentInspector::SetPaused(bool _paused) } ///////////////////////////////////////////////// -void ComponentInspector::OnCameraUpdate( +/*void ComponentInspector::OnCameraUpdate( double _hfov, int _imageWidth, int _imageHeight, double _nearClip, double _farClip) { @@ -937,7 +937,7 @@ void ComponentInspector::OnCameraUpdate( } }; this->dataPtr->updateCallbacks.push_back(cb); -} +}*/ ///////////////////////////////////////////////// void ComponentInspector::OnPose(double _x, double _y, double _z, double _roll, diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 0fce4c4a91..143ca0252d 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -126,8 +126,9 @@ namespace gazebo /// \brief Specialized to set camera data. /// \param[in] _item Item whose data will be set. /// \param[in] _data Data to set. - template<> + /*template<> void setData(QStandardItem *_item, const sdf::Sensor &_data); + */ /// \brief Set the unit of a given item. /// \param[in] _item Item whose unit will be set. @@ -243,8 +244,9 @@ namespace gazebo /// \param[in] _imageHeight Image height in pixels. /// \param[in] _nearClip Near clip value in meters. /// \param[in] _farClip Far clip value in meters. - public: Q_INVOKABLE void OnCameraUpdate(double _hfov, int _imageWidth, + /*public: Q_INVOKABLE void OnCameraUpdate(double _hfov, int _imageWidth, int _imageHeight, double _nearClip, double _farClip); + */ /// \brief Callback in Qt thread when specular changes. /// \param[in] _rSpecular specular red diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index a835da3e35..5a45780ca0 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -248,6 +248,140 @@ Rectangle { } } + ToolButton { + id: addSensorButton + checkable: false + text: "Add sensor" + visible: entityType == "link" + 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 sensor" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + onClicked: { + addSensorMenu.open() + } + + Menu { + id: addSensorMenu + MenuItem { + id: airPressure + text: "Air pressure" + onTriggered: { + addSensorMenu.close() + ComponentInspector.OnAddEntity(contact.text, "sensor"); + } + } + + MenuItem { + id: altimeter + text: "Altimeter" + onTriggered: { + addSensorMenu.close() + ComponentInspector.OnAddEntity(altimeter.text, "sensor"); + } + } + + MenuItem { + id: cameraSensorMenu + text: "Camera >" + MouseArea { + id: viewSubCameraArea + anchors.fill: parent + hoverEnabled: true + onEntered: secondCameraMenu.open() + } + } + + MenuItem { + id: contact + text: "Contact" + onTriggered: { + addSensorMenu.close() + ComponentInspector.OnAddEntity(contact.text, "sensor"); + } + } + + MenuItem { + id: forceTorque + text: "Force torque" + onTriggered: { + addSensorMenu.close() + ComponentInspector.OnAddEntity(forceTorque.text, "sensor"); + } + } + + MenuItem { + id: gps + text: "GPS" + onTriggered: { + addSensorMenu.close() + ComponentInspector.OnAddEntity(gps.text, "sensor"); + } + } + + MenuItem { + id: imu + text: "IMU" + onTriggered: { + addSensorMenu.close() + ComponentInspector.OnAddEntity(imu.text, "sensor"); + } + } + + MenuItem { + id: lidar + text: "Lidar" + } + + MenuItem { + id: magnetometer + text: "magnetometer" + onTriggered: { + addSensorMenu.close() + ComponentInspector.OnAddEntity(magnetometer.text, "sensor"); + } + } + } + + Menu { + id: secondCameraMenu + x: addSensorMenu.x + addSensorMenu.width + y: addSensorMenu.y + cameraSensorMenu.y + MenuItem { + id: depth + text: "Depth" + } + MenuItem { + id: logical + text: "Logical" + } + MenuItem { + id: monocular + text: "Monocular" + } + MenuItem { + id: multicamera + text: "Multicamera" + } + MenuItem { + id: rgbd + text: "RGBD" + } + MenuItem { + id: thermal + text: "Thermal" + } + } + } + + Label { id: entityLabel text: 'Entity ' + ComponentInspector.entity From 1a15c965426b6165688d05d2146d78c4f955017e Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 8 Nov 2021 14:55:21 -0800 Subject: [PATCH 32/55] Added back in sensor menu Signed-off-by: Nate Koenig --- src/gui/plugins/component_inspector/ComponentInspector.qml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 5a45780ca0..0d0b22251d 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -274,7 +274,6 @@ Rectangle { id: airPressure text: "Air pressure" onTriggered: { - addSensorMenu.close() ComponentInspector.OnAddEntity(contact.text, "sensor"); } } @@ -283,7 +282,6 @@ Rectangle { id: altimeter text: "Altimeter" onTriggered: { - addSensorMenu.close() ComponentInspector.OnAddEntity(altimeter.text, "sensor"); } } @@ -296,6 +294,7 @@ Rectangle { anchors.fill: parent hoverEnabled: true onEntered: secondCameraMenu.open() + onExited:secondCameraMenu.close() } } From f752b04906268a48c0bb4eca18d333942dbab23c Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 8 Nov 2021 15:03:43 -0800 Subject: [PATCH 33/55] Updates Signed-off-by: Nate Koenig --- .../plugins/component_inspector/Camera.qml | 291 ------------------ .../component_inspector/ComponentInspector.cc | 85 +---- .../component_inspector/ComponentInspector.hh | 13 - .../ComponentInspector.qml | 13 - .../ComponentInspector.qrc | 1 - 5 files changed, 12 insertions(+), 391 deletions(-) delete mode 100644 src/gui/plugins/component_inspector/Camera.qml diff --git a/src/gui/plugins/component_inspector/Camera.qml b/src/gui/plugins/component_inspector/Camera.qml deleted file mode 100644 index a397d1767c..0000000000 --- a/src/gui/plugins/component_inspector/Camera.qml +++ /dev/null @@ -1,291 +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.Dialogs 1.0 -import QtQuick.Layouts 1.3 -import QtQuick.Controls.Styles 1.4 -import "qrc:/ComponentInspector" -import "qrc:/qml" - -// Item displaying spherical coordinates information. -Rectangle { - height: header.height + content.height - width: componentInspector.width - color: index % 2 == 0 ? lightGrey : darkGrey - - // Left indentation - property int indentation: 10 - - // Horizontal margins - property int margin: 5 - - // Send new data to C++ - function sendCameraUpdate() { - componentInspector.onCameraUpdate( - horizontalFovSpin.value, - imageWidthSpin.value, - imageHeightSpin.value, - nearClipSpin.value, - farClipSpin.value - ); - } - - Column { - anchors.fill: parent - - // Header - Rectangle { - id: header - width: parent.width - height: typeHeader.height - color: "transparent" - - RowLayout { - anchors.fill: parent - Item { - width: margin - } - Image { - id: icon - sourceSize.height: indentation - sourceSize.width: indentation - fillMode: Image.Pad - Layout.alignment : Qt.AlignVCenter - source: content.show ? - "qrc:/Gazebo/images/minus.png" : "qrc:/Gazebo/images/plus.png" - } - TypeHeader { - id: typeHeader - } - Item { - Layout.fillWidth: true - } - } - MouseArea { - anchors.fill: parent - hoverEnabled: true - cursorShape: Qt.PointingHandCursor - onClicked: { - content.show = !content.show - } - onEntered: { - header.color = highlightColor - } - onExited: { - header.color = "transparent" - } - } - } - - // Content - Rectangle { - id: content - property bool show: false - width: parent.width - height: show ? grid.height : 0 - clip: true - color: "transparent" - - Behavior on height { - NumberAnimation { - duration: 200; - easing.type: Easing.InOutQuad - } - } - - GridLayout { - id: grid - width: parent.width - columns: 2 - - // Horizontal field of view - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: horizontalFovText.width + indentation*3 - - Text { - id : horizontalFovText - text: 'Horizontal FoV (°)' - leftPadding: 4 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } - ToolTip { - visible: ma.containsMouse - delay: Qt.styleHints.mousePressAndHoldInterval - text: "Horizontal field of view (degrees)" - enter: null - exit: null - } - MouseArea { - id: ma - anchors.fill: content - hoverEnabled: true - } - } - IgnSpinBox { - id: horizontalFovSpin - Layout.fillWidth: true - height: 40 - property double numberValue: model.data[0] - value: horizontalFovSpin.activeFocus ? horizontalFovSpin.value : numberValue - - // This is equivalent to 0.1 radians, per the SDF spec - minimumValue: 5.72958 - // This is equivalent to 6.283186 radians, per the SDF spec - maximumValue: 360.0 - decimals:4 - stepSize: 0.1 - onEditingFinished: { - sendCameraUpdate() - } - } - // End of Horizontal field of view - - - // Image width - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: imageWidthText.width + indentation*3 - - Text { - id : imageWidthText - text: 'Image width (px)' - leftPadding: 4 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } - } - IgnSpinBox { - id: imageWidthSpin - Layout.fillWidth: true - property int numberValue: model.data[1] - value: imageWidthSpin.activeFocus ? imageWidthSpin.value : numberValue - - minimumValue: 1 - maximumValue: 100000 - decimals:0 - stepSize: 1 - onEditingFinished: { - sendCameraUpdate() - } - } - // End of Image width - - // Image height - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: imageHeightText.width + indentation*3 - - Text { - id : imageHeightText - text: 'Image height (px)' - leftPadding: 4 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } - } - IgnSpinBox { - id: imageHeightSpin - Layout.fillWidth: true - property int numberValue: model.data[2] - value: imageHeightSpin.activeFocus ? imageHeightSpin.value : numberValue - - minimumValue: 1 - maximumValue: 100000 - decimals:0 - stepSize: 1 - onEditingFinished: { - sendCameraUpdate() - } - } - // End of Image height - - // Near clip - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: nearClipText.width + indentation*3 - - Text { - id : nearClipText - text: 'Near clip (m)' - leftPadding: 4 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } - } - IgnSpinBox { - id: nearClipSpin - Layout.fillWidth: true - property double numberValue: model.data[3] - value: nearClipSpin.activeFocus ? nearClipSpin.value : numberValue - - minimumValue: 0.0 - maximumValue: 100000 - decimals: 4 - stepSize: 0.1 - onEditingFinished: { - sendCameraUpdate() - } - } - // End of near clip - - // Far clip - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: farClipText.width + indentation*3 - - Text { - id : farClipText - text: 'Far clip (m)' - leftPadding: 4 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } - } - IgnSpinBox { - id: farClipSpin - Layout.fillWidth: true - property double numberValue: model.data[4] - value: farClipSpin.activeFocus ? farClipSpin.value : numberValue - - minimumValue: 0.0 - maximumValue: 100000 - decimals: 4 - stepSize: 0.1 - onEditingFinished: { - sendCameraUpdate() - } - } - // End of far clip - } - } - } -} diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 59b9bf1d37..62153ad3ac 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -102,9 +102,6 @@ namespace ignition::gazebo /// \brief Transport node for making command requests public: transport::Node node; - - public: std::vector< - std::function> updateCallbacks; }; } @@ -790,12 +787,6 @@ void ComponentInspector::Update(const UpdateInfo &_info, if (comp) setData(item, comp->Data()); } - else if (typeId == components::Camera::typeId) - { - auto comp = _ecm.Component(this->dataPtr->entity); - if (comp) - setData(item, comp->Data()); - } } // Remove components no longer present @@ -810,10 +801,6 @@ void ComponentInspector::Update(const UpdateInfo &_info, Q_ARG(ignition::gazebo::ComponentTypeId, typeId)); } } - - for (auto cb : this->dataPtr->updateCallbacks) - cb(_ecm); - this->dataPtr->updateCallbacks.clear(); } ///////////////////////////////////////////////// @@ -908,72 +895,24 @@ void ComponentInspector::SetPaused(bool _paused) } } -///////////////////////////////////////////////// -/*void ComponentInspector::OnCameraUpdate( - double _hfov, int _imageWidth, int _imageHeight, double _nearClip, - double _farClip) -{ - std::function cb = - [=](EntityComponentManager &_ecm) - { - auto comp = _ecm.Component(this->dataPtr->entity); - if (comp) - { - sdf::Camera *cam = comp->Data().CameraSensor(); - if (cam) - { - cam->SetHorizontalFov(math::Angle(IGN_DTOR(_hfov))); - cam->SetImageWidth(_imageWidth); - cam->SetImageHeight(_imageHeight); - cam->SetNearClip(_nearClip); - cam->SetFarClip(_farClip); - } - else - ignerr << "Unable to get the camera data.\n"; - } - else - { - ignerr << "Unable to get the camera data.\n"; - } - }; - this->dataPtr->updateCallbacks.push_back(cb); -}*/ - ///////////////////////////////////////////////// void ComponentInspector::OnPose(double _x, double _y, double _z, double _roll, double _pitch, double _yaw) { - std::cout << "OnPose\n\n"; - std::function cb = - [=](EntityComponentManager &_ecm) + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) { - auto comp = _ecm.Component(this->dataPtr->entity); - if (comp) - { - comp->Data().Set(_x, _y, _z, _roll, _pitch, _yaw); - } - else - { - ignerr << "Unable to set the pose of entity[" - << this->dataPtr->entity << "]\n"; - } + if (!_result) + ignerr << "Error setting pose" << std::endl; }; - this->dataPtr->updateCallbacks.push_back(cb); - -// std::function cb = -// [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) -// { -// if (!_result) -// ignerr << "Error setting pose" << std::endl; -// }; -// -// ignition::msgs::Pose req; -// req.set_id(this->dataPtr->entity); -// msgs::Set(req.mutable_position(), math::Vector3d(_x, _y, _z)); -// msgs::Set(req.mutable_orientation(), math::Quaterniond(_roll, _pitch, _yaw)); -// auto poseCmdService = "/world/" + this->dataPtr->worldName -// + "/set_pose"; -// this->dataPtr->node.Request(poseCmdService, req, cb); + + ignition::msgs::Pose req; + req.set_id(this->dataPtr->entity); + msgs::Set(req.mutable_position(), math::Vector3d(_x, _y, _z)); + msgs::Set(req.mutable_orientation(), math::Quaterniond(_roll, _pitch, _yaw)); + auto poseCmdService = "/world/" + this->dataPtr->worldName + + "/set_pose"; + this->dataPtr->node.Request(poseCmdService, req, cb); } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 143ca0252d..10a1dfde5c 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -23,7 +23,6 @@ #include #include -#include #include #include @@ -123,13 +122,6 @@ namespace gazebo template<> void setData(QStandardItem *_item, const std::ostream &_data); - /// \brief Specialized to set camera data. - /// \param[in] _item Item whose data will be set. - /// \param[in] _data Data to set. - /*template<> - void setData(QStandardItem *_item, const sdf::Sensor &_data); - */ - /// \brief Set the unit of a given item. /// \param[in] _item Item whose unit will be set. /// \param[in] _unit Unit to be displayed, such as 'm' for meters. @@ -362,11 +354,6 @@ namespace gazebo public: Q_INVOKABLE void OnLoadMesh(const QString &_entity, const QString &_type, const QString &_mesh); - /// \brief Callback in Qt thread when a sensor is to be added - /// \param[in] _entity Entity to add, e.g. box, sphere, cylinder, etc - /// \param[in] _type Sensor type, e.g. contact, gps, etc - public: Q_INVOKABLE void OnAddSensor(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 0d0b22251d..a57a3010c2 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -51,12 +51,6 @@ Rectangle { */ property bool nestedModel : ComponentInspector.nestedModel - /** - * Get if simulation is paused or not - */ - property bool paused : ComponentInspector.paused - - /** * Light grey according to theme */ @@ -95,13 +89,6 @@ Rectangle { return 6; } - /** - * Forward camera changes to C++ - */ - function onCameraUpdate(_hfov, _width, _height, _near, _far) { - ComponentInspector.OnCameraUpdate(_hfov, _width, _height, _near, _far) - } - /** * Forward pose changes to C++ */ diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qrc b/src/gui/plugins/component_inspector/ComponentInspector.qrc index f8c9b1628f..fde9cbbba7 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qrc +++ b/src/gui/plugins/component_inspector/ComponentInspector.qrc @@ -1,7 +1,6 @@ Boolean.qml - Camera.qml ComponentInspector.qml ExpandingTypeHeader.qml Light.qml From 3b2be66173a90ecf99b2b546a5ece1b94d360c3f Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 8 Nov 2021 15:04:11 -0800 Subject: [PATCH 34/55] Updates Signed-off-by: Nate Koenig --- .../plugins/component_inspector/Pose3d.qml | 21 +++++++++++-------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/gui/plugins/component_inspector/Pose3d.qml b/src/gui/plugins/component_inspector/Pose3d.qml index 4c9e2711a8..e9806463e4 100644 --- a/src/gui/plugins/component_inspector/Pose3d.qml +++ b/src/gui/plugins/component_inspector/Pose3d.qml @@ -38,6 +38,12 @@ Rectangle { // Maximum spinbox value property double spinMax: 1000000 + // Read-only / write + property bool readOnly: { + var isModel = entityType == "model" + return !(isModel) || nestedModel + } + property int iconWidth: 20 property int iconHeight: 20 @@ -59,8 +65,6 @@ Rectangle { // Loaded item for yaw property var yawItem: {} - - // Send new pose to C++ function sendPose() { // TODO(anyone) There's a loss of precision when these values get to C++ @@ -211,8 +215,7 @@ Rectangle { id: xLoader anchors.fill: parent property double numberValue: model.data[0] - sourceComponent: writableNumber - active: paused + sourceComponent: readOnly ? readOnlyNumber : writableNumber onLoaded: { xItem = xLoader.item } @@ -249,7 +252,7 @@ Rectangle { id: rollLoader anchors.fill: parent property double numberValue: model.data[3] - sourceComponent: paused ? writableNumber : readOnlyNumber + sourceComponent: readOnly ? readOnlyNumber : writableNumber onLoaded: { rollItem = rollLoader.item } @@ -292,7 +295,7 @@ Rectangle { id: yLoader anchors.fill: parent property double numberValue: model.data[1] - sourceComponent: paused ? writableNumber : readOnlyNumber + sourceComponent: readOnly ? readOnlyNumber : writableNumber onLoaded: { yItem = yLoader.item } @@ -328,7 +331,7 @@ Rectangle { id: pitchLoader anchors.fill: parent property double numberValue: model.data[4] - sourceComponent: paused ? writableNumber : readOnlyNumber + sourceComponent: readOnly ? readOnlyNumber : writableNumber onLoaded: { pitchItem = pitchLoader.item } @@ -364,7 +367,7 @@ Rectangle { id: zLoader anchors.fill: parent property double numberValue: model.data[2] - sourceComponent: paused ? writableNumber : readOnlyNumber + sourceComponent: readOnly ? readOnlyNumber : writableNumber onLoaded: { zItem = zLoader.item } @@ -400,7 +403,7 @@ Rectangle { id: yawLoader anchors.fill: parent property double numberValue: model.data[5] - sourceComponent: paused ? writableNumber : readOnlyNumber + sourceComponent: readOnly ? readOnlyNumber : writableNumber onLoaded: { yawItem = yawLoader.item } From a8c3eb73435939bdf5ed70e22c535c189e76aa47 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 8 Nov 2021 15:06:41 -0800 Subject: [PATCH 35/55] Updates Signed-off-by: Nate Koenig --- .../component_inspector/ComponentInspector.cc | 21 --- .../component_inspector/ComponentInspector.hh | 10 -- .../ComponentInspector.qml | 132 ++++++++++++++++++ 3 files changed, 132 insertions(+), 31 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 62153ad3ac..98647a97da 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -28,7 +28,6 @@ #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/AngularAcceleration.hh" #include "ignition/gazebo/components/AngularVelocity.hh" -#include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/CastShadows.hh" #include "ignition/gazebo/components/ChildLinkName.hh" #include "ignition/gazebo/components/Collision.hh" @@ -290,26 +289,6 @@ void ignition::gazebo::setData(QStandardItem *_item, }), ComponentsModel::RoleNames().key("data")); } -////////////////////////////////////////////////// -/*template<> -void ignition::gazebo::setData(QStandardItem *_item, const sdf::Sensor &_data) -{ - if (nullptr == _item) - return; - - const sdf::Camera *cam = _data.CameraSensor(); - - _item->setData(QString("Camera"), - ComponentsModel::RoleNames().key("dataType")); - _item->setData(QList({ - QVariant(cam->HorizontalFov().Degree()), - QVariant(cam->ImageWidth()), - QVariant(cam->ImageHeight()), - QVariant(cam->NearClip()), - QVariant(cam->FarClip()), - }), ComponentsModel::RoleNames().key("data")); -}*/ - ////////////////////////////////////////////////// void ignition::gazebo::setUnit(QStandardItem *_item, const std::string &_unit) { diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 10a1dfde5c..1790c3b521 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -230,16 +230,6 @@ namespace gazebo public: Q_INVOKABLE void OnPose(double _x, double _y, double _z, double _roll, double _pitch, double _yaw); - /// \brief Callback in Qt thread when a camera changes. - /// \param[in] _hfov Horizontal field of view in degrees. - /// \param[in] _imageWidth Image width in pixels. - /// \param[in] _imageHeight Image height in pixels. - /// \param[in] _nearClip Near clip value in meters. - /// \param[in] _farClip Far clip value in meters. - /*public: Q_INVOKABLE void OnCameraUpdate(double _hfov, int _imageWidth, - int _imageHeight, double _nearClip, double _farClip); - */ - /// \brief Callback in Qt thread when specular changes. /// \param[in] _rSpecular specular red /// \param[in] _gSpecular specular green diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index a57a3010c2..a19ea78722 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -233,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 + } } ToolButton { From 26965e396f5331c97edec6d873435da5ece4534f Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 8 Nov 2021 15:39:09 -0800 Subject: [PATCH 36/55] 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 fcc87a2cf867a06e449e2ae82066f6556314fdb7 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 8 Nov 2021 15:57:01 -0800 Subject: [PATCH 37/55] Adding sensors Signed-off-by: Nate Koenig --- .../ComponentInspector.qml | 8 +- src/gui/plugins/model_editor/ModelEditor.cc | 161 ++++++++++++------ 2 files changed, 113 insertions(+), 56 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index a19ea78722..20e2bdba64 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -393,7 +393,7 @@ Rectangle { id: airPressure text: "Air pressure" onTriggered: { - ComponentInspector.OnAddEntity(contact.text, "sensor"); + ComponentInspector.OnAddEntity(airPressure.text, "sensor"); } } @@ -421,7 +421,6 @@ Rectangle { id: contact text: "Contact" onTriggered: { - addSensorMenu.close() ComponentInspector.OnAddEntity(contact.text, "sensor"); } } @@ -430,7 +429,6 @@ Rectangle { id: forceTorque text: "Force torque" onTriggered: { - addSensorMenu.close() ComponentInspector.OnAddEntity(forceTorque.text, "sensor"); } } @@ -439,7 +437,6 @@ Rectangle { id: gps text: "GPS" onTriggered: { - addSensorMenu.close() ComponentInspector.OnAddEntity(gps.text, "sensor"); } } @@ -448,7 +445,6 @@ Rectangle { id: imu text: "IMU" onTriggered: { - addSensorMenu.close() ComponentInspector.OnAddEntity(imu.text, "sensor"); } } @@ -462,7 +458,6 @@ Rectangle { id: magnetometer text: "magnetometer" onTriggered: { - addSensorMenu.close() ComponentInspector.OnAddEntity(magnetometer.text, "sensor"); } } @@ -499,7 +494,6 @@ Rectangle { } } - Label { id: entityLabel text: 'Entity ' + ComponentInspector.entity diff --git a/src/gui/plugins/model_editor/ModelEditor.cc b/src/gui/plugins/model_editor/ModelEditor.cc index b4f4498470..8a47f69a2d 100644 --- a/src/gui/plugins/model_editor/ModelEditor.cc +++ b/src/gui/plugins/model_editor/ModelEditor.cc @@ -60,7 +60,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 Parent entity /// \param[in] _uri URI associated with the entity, needed for mesh @@ -81,7 +82,17 @@ namespace ignition::gazebo /// \param[in] _eta Entity to add. public: std::string LinkSDFString(const EntityToAdd &_eta) const; - public: sdf::Sensor CreateSensor(const std::string &_type) const; + /// \brief Create a link + /// \param[in] _eta Entity to add. + public: std::optional CreateLink( + const EntityToAdd &_eta, + EntityComponentManager &_ecm) const; + + /// \brief Create a sensor + /// \param[in] _eta Entity to add. + public: std::optional CreateSensor( + const EntityToAdd &_eta, + EntityComponentManager &_ecm) const; /// \brief Entity Creator API. public: std::unique_ptr entityCreator{nullptr}; @@ -143,56 +154,34 @@ 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; + Entity entity = kNullEntity; + if (eta.entityType == "link") { - // create an sdf::Link to it can be added to the ECM throught the - // CreateEntities call - std::string linkSDFStr = this->dataPtr->LinkSDFString(eta); - 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 + std::optional link = this->dataPtr->CreateLink(eta, _ecm); + if (link) { - continue; + entity = this->dataPtr->entityCreator->CreateEntities(&(*link)); + this->dataPtr->entityCreator->SetParent(entity, eta.parentEntity); } - if (eta.parentEntity == kNullEntity) - { - ignerr << "Parent entity not defined." << 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::ParentEntity(eta.parentEntity), - components::Name(linkName)); - int64_t counter = 0; - while (linkEnt) + } + else if (eta.entityType == "sensor") + { + std::optional sensor = + this->dataPtr->CreateSensor(eta, _ecm); + if (sensor) { - linkName = std::string("link") + "_" + std::to_string(++counter); - linkEnt = _ecm.EntityByComponents( - components::ParentEntity(eta.parentEntity), - components::Name(linkName)); + entity = this->dataPtr->entityCreator->CreateEntities(&(*sensor)); + this->dataPtr->entityCreator->SetParent(entity, eta.parentEntity); } + } - linkSdf.SetName(linkName); - 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 + // If an entity was created, then traverse the tree and add all new + // entities created by the entity creator to the set + if (entity != kNullEntity) + { std::list entities; entities.push_back(entity); while (!entities.empty()) @@ -402,17 +391,73 @@ std::string ModelEditorPrivate::LinkSDFString(const EntityToAdd &_eta) const } ///////////////////////////////////////////////// -sdf::Sensor ModelEditorPrivate::CreateSensor(const std::string &_type) const +std::optional ModelEditorPrivate::CreateLink( + const EntityToAdd &_eta, EntityComponentManager &_ecm) const +{ + sdf::Link linkSdf; + if (_eta.parentEntity == kNullEntity) + { + ignerr << "Parent entity not defined." << std::endl; + return std::nullopt; + } + + // create an sdf::Link to it can be added to the ECM throught the + // CreateEntities call + std::string linkSDFStr = this->LinkSDFString(_eta); + 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 + { + return std::nullopt; + } + + // 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::ParentEntity(_eta.parentEntity), + components::Name(linkName)); + int64_t counter = 0; + while (linkEnt) + { + linkName = std::string("link") + "_" + std::to_string(++counter); + linkEnt = _ecm.EntityByComponents( + components::ParentEntity(_eta.parentEntity), + components::Name(linkName)); + } + + linkSdf.SetName(linkName); + return linkSdf; +} + +///////////////////////////////////////////////// +std::optional ModelEditorPrivate::CreateSensor( + const EntityToAdd &_eta, EntityComponentManager &_ecm) const { + // Exit early if there is no parent entity + if (_eta.parentEntity == kNullEntity) + { + ignerr << "Parent entity not defined." << std::endl; + return std::nullopt; + } + sdf::Sensor sensor; std::string type; // Replace spaces with underscores. - common::replaceAll(type, _type, " ", "_"); + common::replaceAll(type, _eta.geomOrLightType, " ", "_"); - sensor.SetName("default"); - sensor.SetType(_type); + sensor.SetType(_eta.geomOrLightType); if (type == "air_pressure") { sdf::AirPressure airpressure; @@ -425,8 +470,26 @@ sdf::Sensor ModelEditorPrivate::CreateSensor(const std::string &_type) const } else { - ignerr << "Unable to create sensor type[" << _type << "]\n"; + ignerr << "Unable to create sensor type[" << _eta.geomOrLightType << "]\n"; + return std::nullopt; + } + + // generate unique sensor name + // note passing components::Link() as arg to EntityByComponents causes + // a crash on exit, see issue #1158 + std::string sensorName = type; + Entity sensorEnt = _ecm.EntityByComponents( + components::ParentEntity(_eta.parentEntity), + components::Name(sensorName)); + int64_t counter = 0; + while (sensorEnt) + { + sensorName = type + "_" + std::to_string(++counter); + sensorEnt = _ecm.EntityByComponents( + components::ParentEntity(_eta.parentEntity), + components::Name(sensorName)); } + sensor.SetName(sensorName); return sensor; } From 539295cf1250b5a81d3df74099a7f56b0557ddba Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 8 Nov 2021 16:13:39 -0800 Subject: [PATCH 38/55] 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 39/55] 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 40/55] 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 41/55] 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 42/55] 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 c9b984484e4011af494e6f61e3739a31f56b50c7 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 9 Nov 2021 12:26:36 -0800 Subject: [PATCH 43/55] Updates Signed-off-by: Nate Koenig --- .../ComponentInspector.qml | 2 +- .../component_inspector/ModelEditor.cc | 56 +++++++++++++------ 2 files changed, 41 insertions(+), 17 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index bd8c728e83..ec12446931 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -380,7 +380,7 @@ Rectangle { sourceSize.width: 18; sourceSize.height: 18; } - ToolTip.text: "Add sensor" + ToolTip.text: "Add a sensor to a link" ToolTip.visible: hovered ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval onClicked: { diff --git a/src/gui/plugins/component_inspector/ModelEditor.cc b/src/gui/plugins/component_inspector/ModelEditor.cc index a5280b0a09..9223005df3 100644 --- a/src/gui/plugins/component_inspector/ModelEditor.cc +++ b/src/gui/plugins/component_inspector/ModelEditor.cc @@ -78,8 +78,12 @@ namespace ignition::gazebo /// \brief Get a SDF string of a link /// \param[in] _eta Entity to add. + /// \return SDF string public: std::string LinkSDFString(const EntityToAdd &_eta) const; + /// \brief Create a sensor based on a type. + /// \param[in] _type Sensor type. + /// \return SDF sensor object. public: sdf::Sensor CreateSensor(const std::string &_type) const; /// \brief Entity Creator API. @@ -400,29 +404,49 @@ std::string ModelEditorPrivate::LinkSDFString(const EntityToAdd &_eta) const ///////////////////////////////////////////////// sdf::Sensor ModelEditorPrivate::CreateSensor(const std::string &_type) const { + // Exit early if there is no parent entity + if (_eta.parentEntity == kNullEntity) + { + ignerr << "Parent entity not defined." << std::endl; + return std::nullopt; + } + sdf::Sensor sensor; std::string type; // Replace spaces with underscores. - common::replaceAll(type, _type, " ", "_"); - - sensor.SetName("default"); - sensor.SetType(_type); - if (type == "air_pressure") - { - sdf::AirPressure airpressure; - sensor.SetAirPressureSensor(airpressure); - } - else if (type == "altimeter") + common::replaceAll(type, _eta.geomOrLightType, " ", "_"); + + std::ostringstream stream; + stream << "" + << "" + << "<" << type << ">"; + + auto sdfStr = stream.str(); + sdf::ElementPtr sensorElem(new sdf::Element); + sdf::initFile("sensor.sdf", sensorElem); + sdf::readString(sdfStr, sensorElem); + sensor.Load(sensorElem); + + // generate unique sensor name + // note passing components::Link() as arg to EntityByComponents causes + // a crash on exit, see issue #1158 + std::string sensorName = type; + Entity sensorEnt = _ecm.EntityByComponents( + components::ParentEntity(_eta.parentEntity), + components::Name(sensorName)); + int64_t counter = 0; + while (sensorEnt) { - sdf::Altimeter altimeter; - sensor.SetAltimeterSensor(altimeter); - } - else - { - ignerr << "Unable to create sensor type[" << _type << "]\n"; + sensorName = type + "_" + std::to_string(++counter); + sensorEnt = _ecm.EntityByComponents( + components::ParentEntity(_eta.parentEntity), + components::Name(sensorName)); } + sensor.SetName(sensorName); + + sensor.SetTopic("/" + sensorName); return sensor; } From a16b328b3dc3e5d84ab90cc83949783d5bac5717 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 9 Nov 2021 12:26:51 -0800 Subject: [PATCH 44/55] updates Signed-off-by: Nate Koenig --- .../ComponentInspector.qml | 11 ++++++-- src/gui/plugins/model_editor/ModelEditor.cc | 28 ++++++++----------- 2 files changed, 21 insertions(+), 18 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 20e2bdba64..f384bea477 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -408,12 +408,12 @@ Rectangle { MenuItem { id: cameraSensorMenu text: "Camera >" + MouseArea { id: viewSubCameraArea anchors.fill: parent hoverEnabled: true onEntered: secondCameraMenu.open() - onExited:secondCameraMenu.close() } } @@ -465,8 +465,15 @@ Rectangle { Menu { id: secondCameraMenu - x: addSensorMenu.x + addSensorMenu.width + x: addSensorMenu.x - addSensorMenu.width y: addSensorMenu.y + cameraSensorMenu.y + MouseArea { + id: viewSubCameraAreaExit + anchors.fill: parent + hoverEnabled: true + onExited: secondCameraMenu.close() + } + MenuItem { id: depth text: "Depth" diff --git a/src/gui/plugins/model_editor/ModelEditor.cc b/src/gui/plugins/model_editor/ModelEditor.cc index 8a47f69a2d..925d569a68 100644 --- a/src/gui/plugins/model_editor/ModelEditor.cc +++ b/src/gui/plugins/model_editor/ModelEditor.cc @@ -457,22 +457,16 @@ std::optional ModelEditorPrivate::CreateSensor( // Replace spaces with underscores. common::replaceAll(type, _eta.geomOrLightType, " ", "_"); - sensor.SetType(_eta.geomOrLightType); - if (type == "air_pressure") - { - sdf::AirPressure airpressure; - sensor.SetAirPressureSensor(airpressure); - } - else if (type == "altimeter") - { - sdf::Altimeter altimeter; - sensor.SetAltimeterSensor(altimeter); - } - else - { - ignerr << "Unable to create sensor type[" << _eta.geomOrLightType << "]\n"; - return std::nullopt; - } + std::ostringstream stream; + stream << "" + << "" + << "<" << type << ">"; + + auto sdfStr = stream.str(); + sdf::ElementPtr sensorElem(new sdf::Element); + sdf::initFile("sensor.sdf", sensorElem); + sdf::readString(sdfStr, sensorElem); + sensor.Load(sensorElem); // generate unique sensor name // note passing components::Link() as arg to EntityByComponents causes @@ -491,6 +485,8 @@ std::optional ModelEditorPrivate::CreateSensor( } sensor.SetName(sensorName); + sensor.SetTopic("/" + sensorName); + return sensor; } From cafbe77405054e3c4cd4d81180ec176f4fcfd325 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 9 Nov 2021 12:35:01 -0800 Subject: [PATCH 45/55] updates Signed-off-by: Nate Koenig --- .../component_inspector/ComponentInspector.qml | 18 ++++++++++++++++++ .../plugins/component_inspector/ModelEditor.cc | 3 --- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 0d320dba0f..9a47d2d237 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -477,26 +477,44 @@ Rectangle { MenuItem { id: depth text: "Depth" + onTriggered: { + ComponentInspector.OnAddEntity("depth_camera", "sensor"); + } } MenuItem { id: logical text: "Logical" + onTriggered: { + ComponentInspector.OnAddEntity("logical_camera", "sensor"); + } } MenuItem { id: monocular text: "Monocular" + onTriggered: { + ComponentInspector.OnAddEntity("camera", "sensor"); + } } MenuItem { id: multicamera text: "Multicamera" + onTriggered: { + ComponentInspector.OnAddEntity("multicamera", "sensor"); + } } MenuItem { id: rgbd text: "RGBD" + onTriggered: { + ComponentInspector.OnAddEntity("rgbd_camera", "sensor"); + } } MenuItem { id: thermal text: "Thermal" + onTriggered: { + ComponentInspector.OnAddEntity("thermal_camera", "sensor"); + } } } } diff --git a/src/gui/plugins/component_inspector/ModelEditor.cc b/src/gui/plugins/component_inspector/ModelEditor.cc index c4e9ac4a4d..0d25c8fdb1 100644 --- a/src/gui/plugins/component_inspector/ModelEditor.cc +++ b/src/gui/plugins/component_inspector/ModelEditor.cc @@ -25,8 +25,6 @@ #include #include -#include -#include #include #include "ignition/gazebo/components/Model.hh" @@ -431,7 +429,6 @@ std::optional ModelEditorPrivate::CreateLink( components::ParentEntity(_eta.parentEntity), components::Name(linkName)); } - sensor.SetName(sensorName); linkSdf.SetName(linkName); return linkSdf; From 574cadbc09d573ee43c118a3a0220d194f8c0cdb Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 9 Nov 2021 15:50:45 -0800 Subject: [PATCH 46/55] Adding lidar menu Signed-off-by: Nate Koenig --- .../ComponentInspector.qml | 61 +++++++++++++++---- 1 file changed, 50 insertions(+), 11 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 9a47d2d237..236310e99a 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -413,7 +413,7 @@ Rectangle { id: viewSubCameraArea anchors.fill: parent hoverEnabled: true - onEntered: secondCameraMenu.open() + onEntered: cameraSubmenu.open() } } @@ -450,10 +450,17 @@ Rectangle { } MenuItem { - id: lidar - text: "Lidar" + id: lidarSensorMenu + text: "Lidar >" + + MouseArea { + id: viewSubLidarArea + anchors.fill: parent + hoverEnabled: true + onEntered: cameraSubmenu.open() + } } - + MenuItem { id: magnetometer text: "magnetometer" @@ -464,15 +471,9 @@ Rectangle { } Menu { - id: secondCameraMenu + id: cameraSubmenu x: addSensorMenu.x - addSensorMenu.width y: addSensorMenu.y + cameraSensorMenu.y - MouseArea { - id: viewSubCameraAreaExit - anchors.fill: parent - hoverEnabled: true - onExited: secondCameraMenu.close() - } MenuItem { id: depth @@ -517,6 +518,44 @@ Rectangle { } } } + + Menu { + id: lidarSubmenu + x: addSensorMenu.x - addSensorMenu.width + y: addSensorMenu.y + lidarSensorMenu.y + + MenuItem { + id: cpuLidar + text: "CPU Lidar" + onTriggered: { + ComponentInspector.OnAddEntity("cpu_lidar", "sensor"); + } + } + + MenuItem { + id: cpuRay + text: "CPU Ray" + onTriggered: { + ComponentInspector.OnAddEntity("cpu_ray", "sensor"); + } + } + + MenuItem { + id: gpuLidar + text: "GPU Lidar" + onTriggered: { + ComponentInspector.OnAddEntity("gpu_lidar", "sensor"); + } + } + + MenuItem { + id: gpuRay + text: "GPU Ray" + onTriggered: { + ComponentInspector.OnAddEntity("gpu_ray", "sensor"); + } + } + } } Label { From 852b6b47de7bdf4cd35a363d42fa6cfb0a4834e2 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 18 Nov 2021 10:30:52 -0800 Subject: [PATCH 47/55] alphabetize Signed-off-by: Nate Koenig --- src/gui/resources/gazebo.qrc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gui/resources/gazebo.qrc b/src/gui/resources/gazebo.qrc index 967056c8df..96fa71b8ff 100644 --- a/src/gui/resources/gazebo.qrc +++ b/src/gui/resources/gazebo.qrc @@ -2,8 +2,8 @@ GazeboDrawer.qml images/actor.png - images/chevron-right.svg images/chevron-down.svg + images/chevron-right.svg images/collision.png images/joint.png images/light.png From d565cf17539870790057bca612506e51e116b968 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 22 Nov 2021 09:32:21 -0800 Subject: [PATCH 48/55] Comment out sensors not supported Signed-off-by: Nate Koenig --- .../plugins/component_inspector/ComponentInspector.qml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index cb8c2556ce..27a8a4bc04 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -546,13 +546,13 @@ Rectangle { ComponentInspector.OnAddEntity("camera", "sensor"); } } - MenuItem { + /*MenuItem { id: multicamera text: "Multicamera" onTriggered: { ComponentInspector.OnAddEntity("multicamera", "sensor"); } - } + }*/ MenuItem { id: rgbd text: "RGBD" @@ -574,7 +574,7 @@ Rectangle { x: addSensorMenu.x - addSensorMenu.width y: addSensorMenu.y + lidarSensorMenu.y - MenuItem { + /*MenuItem { id: cpuLidar text: "CPU Lidar" onTriggered: { @@ -588,7 +588,7 @@ Rectangle { onTriggered: { ComponentInspector.OnAddEntity("cpu_ray", "sensor"); } - } + }*/ MenuItem { id: gpuLidar From f24375d363640f7bb9bb9702e59b651acc9b133e Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 22 Nov 2021 09:58:58 -0800 Subject: [PATCH 49/55] Added segmentation camera Signed-off-by: Nate Koenig --- src/gui/plugins/component_inspector/ComponentInspector.qml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 27a8a4bc04..48f72ce45d 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -560,6 +560,13 @@ Rectangle { ComponentInspector.OnAddEntity("rgbd_camera", "sensor"); } } + MenuItem { + id: thermal + text: "Segmentation" + onTriggered: { + ComponentInspector.OnAddEntity("segmentation_camera", "sensor"); + } + } MenuItem { id: thermal text: "Thermal" From 88db13571fb979fe497c97b0087a751fea80a5b7 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 22 Nov 2021 10:02:35 -0800 Subject: [PATCH 50/55] fix id Signed-off-by: Nate Koenig --- src/gui/plugins/component_inspector/ComponentInspector.qml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 48f72ce45d..fa20fb8f39 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -561,7 +561,7 @@ Rectangle { } } MenuItem { - id: thermal + id: segmentation text: "Segmentation" onTriggered: { ComponentInspector.OnAddEntity("segmentation_camera", "sensor"); From 635a3ab0b0a7790f9e7cf073e8a347f3f41e3cc6 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 22 Nov 2021 10:15:51 -0800 Subject: [PATCH 51/55] fix codecheck Signed-off-by: Nate Koenig --- src/gui/plugins/component_inspector/ModelEditor.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gui/plugins/component_inspector/ModelEditor.cc b/src/gui/plugins/component_inspector/ModelEditor.cc index 8c94e029c4..8adce850b5 100644 --- a/src/gui/plugins/component_inspector/ModelEditor.cc +++ b/src/gui/plugins/component_inspector/ModelEditor.cc @@ -152,7 +152,7 @@ void ModelEditor::Update(const UpdateInfo &, std::lock_guard lock(this->dataPtr->mutex); // add link entities to the ECM std::set newEntities; - for (const auto &eta: this->dataPtr->entitiesToAdd) + for (const auto &eta : this->dataPtr->entitiesToAdd) { Entity entity = kNullEntity; From 963442899532bedc6f392f63c6f3fe88589e8b8a Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 22 Nov 2021 11:19:55 -0800 Subject: [PATCH 52/55] Fix submenue Signed-off-by: Nate Koenig --- src/gui/plugins/component_inspector/ComponentInspector.qml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index fa20fb8f39..401699f4b4 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -507,7 +507,7 @@ Rectangle { id: viewSubLidarArea anchors.fill: parent hoverEnabled: true - onEntered: cameraSubmenu.open() + onEntered: lidarSubmenu.open() } } From a663abd8d81c9f1a8dad74903d8caf3725b45f91 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 30 Nov 2021 10:55:15 -0800 Subject: [PATCH 53/55] Remove lidar menu, and address comments Signed-off-by: Nate Koenig --- .../ComponentInspector.qml | 66 ++++--------------- 1 file changed, 12 insertions(+), 54 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 401699f4b4..cc801c8ecf 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -482,15 +482,23 @@ Rectangle { ComponentInspector.OnAddEntity(forceTorque.text, "sensor"); } } - - MenuItem { + + /*MenuItem { id: gps text: "GPS" onTriggered: { ComponentInspector.OnAddEntity(gps.text, "sensor"); } + }*/ + + MenuItem { + id: gpuLidar + text: "GPU Lidar" + onTriggered: { + ComponentInspector.OnAddEntity("gpu_lidar", "sensor"); + } } - + MenuItem { id: imu text: "IMU" @@ -499,21 +507,9 @@ Rectangle { } } - MenuItem { - id: lidarSensorMenu - text: "Lidar >" - - MouseArea { - id: viewSubLidarArea - anchors.fill: parent - hoverEnabled: true - onEntered: lidarSubmenu.open() - } - } - MenuItem { id: magnetometer - text: "magnetometer" + text: "Magnetometer" onTriggered: { ComponentInspector.OnAddEntity(magnetometer.text, "sensor"); } @@ -575,44 +571,6 @@ Rectangle { } } } - - Menu { - id: lidarSubmenu - x: addSensorMenu.x - addSensorMenu.width - y: addSensorMenu.y + lidarSensorMenu.y - - /*MenuItem { - id: cpuLidar - text: "CPU Lidar" - onTriggered: { - ComponentInspector.OnAddEntity("cpu_lidar", "sensor"); - } - } - - MenuItem { - id: cpuRay - text: "CPU Ray" - onTriggered: { - ComponentInspector.OnAddEntity("cpu_ray", "sensor"); - } - }*/ - - MenuItem { - id: gpuLidar - text: "GPU Lidar" - onTriggered: { - ComponentInspector.OnAddEntity("gpu_lidar", "sensor"); - } - } - - MenuItem { - id: gpuRay - text: "GPU Ray" - onTriggered: { - ComponentInspector.OnAddEntity("gpu_ray", "sensor"); - } - } - } } Label { From cac737a9a3908813aefdcfbd0d249b3bdf2d15c7 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 1 Dec 2021 15:50:13 -0800 Subject: [PATCH 54/55] fix codecheck Signed-off-by: Nate Koenig --- src/gui/plugins/component_inspector/ModelEditor.cc | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/gui/plugins/component_inspector/ModelEditor.cc b/src/gui/plugins/component_inspector/ModelEditor.cc index 9dd2e3af3f..8735c99f97 100644 --- a/src/gui/plugins/component_inspector/ModelEditor.cc +++ b/src/gui/plugins/component_inspector/ModelEditor.cc @@ -471,9 +471,7 @@ std::optional ModelEditorPrivate::CreateJoint( { sdf::Joint joint; - if (_eta.geomOrLightType == "revolute") - joint.SetType(sdf::JointType::REVOLUTE); - else if (_eta.geomOrLightType == "ball") + if (_eta.geomOrLightType == "ball") joint.SetType(sdf::JointType::BALL); else if (_eta.geomOrLightType == "continuous") joint.SetType(sdf::JointType::CONTINUOUS); From 08bd0c3bdd62b9819421fb942520b2fed6415aa9 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 1 Dec 2021 16:29:28 -0800 Subject: [PATCH 55/55] Merged Signed-off-by: Nate Koenig --- src/gui/plugins/component_inspector/AirPressure.cc | 4 ++-- src/gui/plugins/component_inspector/Altimeter.cc | 4 ++-- .../component_inspector/ComponentInspector.cc | 2 +- .../component_inspector/ComponentInspector.hh | 2 +- src/gui/plugins/component_inspector/Imu.cc | 12 ++++++------ src/gui/plugins/component_inspector/JointType.cc | 4 ++-- src/gui/plugins/component_inspector/Lidar.cc | 4 ++-- src/gui/plugins/component_inspector/Magnetometer.cc | 6 +++--- src/gui/plugins/component_inspector/ModelEditor.cc | 4 ++-- src/gui/plugins/scene_manager/GzSceneManager.cc | 4 ++-- 10 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/gui/plugins/component_inspector/AirPressure.cc b/src/gui/plugins/component_inspector/AirPressure.cc index d9b9656598..c6487f53ef 100644 --- a/src/gui/plugins/component_inspector/AirPressure.cc +++ b/src/gui/plugins/component_inspector/AirPressure.cc @@ -68,7 +68,7 @@ Q_INVOKABLE void AirPressure::OnAirPressureNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::AirPressure *airpressure = comp->Data().AirPressureSensor(); @@ -100,7 +100,7 @@ Q_INVOKABLE void AirPressure::OnAirPressureReferenceAltitude( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::AirPressure *airpressure = comp->Data().AirPressureSensor(); diff --git a/src/gui/plugins/component_inspector/Altimeter.cc b/src/gui/plugins/component_inspector/Altimeter.cc index fca5263475..0179e81c39 100644 --- a/src/gui/plugins/component_inspector/Altimeter.cc +++ b/src/gui/plugins/component_inspector/Altimeter.cc @@ -72,7 +72,7 @@ Q_INVOKABLE void Altimeter::OnAltimeterPositionNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Altimeter *altimeter = comp->Data().AltimeterSensor(); @@ -106,7 +106,7 @@ Q_INVOKABLE void Altimeter::OnAltimeterVelocityNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Altimeter *altimeter = comp->Data().AltimeterSensor(); diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index cb29338b8d..042c94913e 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -1054,7 +1054,7 @@ Entity ComponentInspector::GetEntity() const } ///////////////////////////////////////////////// -void ComponentInspector::SetEntity(const Entity &_entity) +void ComponentInspector::SetEntity(const gazebo::Entity &_entity) { // If nothing is selected, display world properties if (_entity == kNullEntity) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 1c23dddf1a..0686a89d04 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -346,7 +346,7 @@ namespace gazebo /// \brief Set the entity currently inspected. /// \param[in] _entity Entity ID. - public: Q_INVOKABLE void SetEntity(const Entity &_entity); + public: Q_INVOKABLE void SetEntity(const gazebo::Entity &_entity); /// \brief Notify that entity has changed. signals: void EntityChanged(); diff --git a/src/gui/plugins/component_inspector/Imu.cc b/src/gui/plugins/component_inspector/Imu.cc index 6949f9ba1d..18e4969493 100644 --- a/src/gui/plugins/component_inspector/Imu.cc +++ b/src/gui/plugins/component_inspector/Imu.cc @@ -103,7 +103,7 @@ Q_INVOKABLE void Imu::OnLinearAccelerationXNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Imu *imu = comp->Data().ImuSensor(); @@ -137,7 +137,7 @@ Q_INVOKABLE void Imu::OnLinearAccelerationYNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Imu *imu = comp->Data().ImuSensor(); @@ -171,7 +171,7 @@ Q_INVOKABLE void Imu::OnLinearAccelerationZNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Imu *imu = comp->Data().ImuSensor(); @@ -205,7 +205,7 @@ Q_INVOKABLE void Imu::OnAngularVelocityXNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Imu *imu = comp->Data().ImuSensor(); @@ -239,7 +239,7 @@ Q_INVOKABLE void Imu::OnAngularVelocityYNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Imu *imu = comp->Data().ImuSensor(); @@ -273,7 +273,7 @@ Q_INVOKABLE void Imu::OnAngularVelocityZNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Imu *imu = comp->Data().ImuSensor(); diff --git a/src/gui/plugins/component_inspector/JointType.cc b/src/gui/plugins/component_inspector/JointType.cc index ffcc757f3e..63d9dfe5d6 100644 --- a/src/gui/plugins/component_inspector/JointType.cc +++ b/src/gui/plugins/component_inspector/JointType.cc @@ -81,10 +81,10 @@ Q_INVOKABLE void JointType::OnJointType(QString _jointType) [=](EntityComponentManager &_ecm) { components::JointType *comp = - _ecm.Component(this->inspector->Entity()); + _ecm.Component(this->inspector->GetEntity()); components::ParentEntity *parentComp = - _ecm.Component(this->inspector->Entity()); + _ecm.Component(this->inspector->GetEntity()); if (comp && parentComp) { diff --git a/src/gui/plugins/component_inspector/Lidar.cc b/src/gui/plugins/component_inspector/Lidar.cc index 48ad47b2fc..8fbf6e95c1 100644 --- a/src/gui/plugins/component_inspector/Lidar.cc +++ b/src/gui/plugins/component_inspector/Lidar.cc @@ -82,7 +82,7 @@ Q_INVOKABLE void Lidar::OnLidarNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Lidar *lidar = comp->Data().LidarSensor(); @@ -120,7 +120,7 @@ Q_INVOKABLE void Lidar::OnLidarChange( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Lidar *lidar = comp->Data().LidarSensor(); diff --git a/src/gui/plugins/component_inspector/Magnetometer.cc b/src/gui/plugins/component_inspector/Magnetometer.cc index c6f2ca59be..3c789582bc 100644 --- a/src/gui/plugins/component_inspector/Magnetometer.cc +++ b/src/gui/plugins/component_inspector/Magnetometer.cc @@ -81,7 +81,7 @@ Q_INVOKABLE void Magnetometer::OnMagnetometerXNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Magnetometer *mag = comp->Data().MagnetometerSensor(); @@ -115,7 +115,7 @@ Q_INVOKABLE void Magnetometer::OnMagnetometerYNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Magnetometer *mag = comp->Data().MagnetometerSensor(); @@ -149,7 +149,7 @@ Q_INVOKABLE void Magnetometer::OnMagnetometerZNoise( [=](EntityComponentManager &_ecm) { auto comp = _ecm.Component( - this->inspector->Entity()); + this->inspector->GetEntity()); if (comp) { sdf::Magnetometer *mag = comp->Data().MagnetometerSensor(); diff --git a/src/gui/plugins/component_inspector/ModelEditor.cc b/src/gui/plugins/component_inspector/ModelEditor.cc index f59d30e0b5..e5ebb018a6 100644 --- a/src/gui/plugins/component_inspector/ModelEditor.cc +++ b/src/gui/plugins/component_inspector/ModelEditor.cc @@ -233,10 +233,10 @@ void ModelEditor::Update(const UpdateInfo &, entities.push_back(child); } - // use tmp AddedRemovedEntities event to update other gui plugins + // use GuiNewRemovedEntities event to update other gui plugins // note this event will be removed in Ignition Garden std::set removedEntities; - ignition::gazebo::gui::events::AddedRemovedEntities event( + ignition::gazebo::gui::events::GuiNewRemovedEntities event( newEntities, removedEntities); ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 9d2abc81be..cccb6caaa4 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -134,11 +134,11 @@ bool GzSceneManager::eventFilter(QObject *_obj, QEvent *_event) this->dataPtr->OnRender(); } else if (_event->type() == - ignition::gazebo::gui::events::AddedRemovedEntities::kType) + ignition::gazebo::gui::events::GuiNewRemovedEntities::kType) { std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); auto addedRemovedEvent = - reinterpret_cast(_event); + reinterpret_cast(_event); if (addedRemovedEvent) { for (auto entity : addedRemovedEvent->NewEntities())