From ba9403b6e73a04516ef90e6980f9349c50d4c128 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 16 Nov 2021 16:34:13 -0800 Subject: [PATCH 1/2] Support recreating model entities (#1170) * add recreate component and implement recreate entities functionality, add test Signed-off-by: Ian Chen * update test and make recreate entities with same name work Signed-off-by: Ian Chen * revert add include Signed-off-by: Ian Chen * style Signed-off-by: Ian Chen * Support editing air pressure sensor in the GUI Signed-off-by: Nate Koenig * Add noise to qrc Signed-off-by: Nate Koenig * Add noise to qrc Signed-off-by: Nate Koenig * Fix lint Signed-off-by: Michael Carroll * Update sensor icon Signed-off-by: Nate Koenig * Move AirPressure functions out of ComponentInspector (#1179) Signed-off-by: Louise Poubel * Fix get decimals, and address comments Signed-off-by: Nate Koenig * cleanup and simplification Signed-off-by: Nate Koenig * check recreate comp in ancestor Signed-off-by: Ian Chen * require sdf 12.1.0 Signed-off-by: Nate Koenig * Revert sdf version requirement Signed-off-by: Nate Koenig * Fix codecheck Signed-off-by: Nate Koenig * revert my bad merge Signed-off-by: Nate Koenig * remvoe sensor icon Signed-off-by: Nate Koenig * Together (#1187) * add an add entity button to component inspector. Currently only enabled for models Signed-off-by: Ian Chen * add model editor gui plugin that inserts visuals to the scene in the render thread Signed-off-by: Ian Chen * write to ECM Signed-off-by: Ian Chen * support adding light links Signed-off-by: Ian Chen * notify other GUI plugins of added/removed entities via GUI events Signed-off-by: Ashton Larkin * use const ref for constructor input params Signed-off-by: Ashton Larkin * guarantee 64 bit entity IDs with gazebo::Entity instead of unsigned int Signed-off-by: Ashton Larkin * testing makr as new entity func Signed-off-by: Ian Chen * rm printouts Signed-off-by: Ian Chen * register type Signed-off-by: Ian Chen * refactor render util Signed-off-by: Ian Chen * workaround for avoiding crash on exit Signed-off-by: Ian Chen * refactor, comment out unused menu items Signed-off-by: Ian Chen * remove commented out code, add CreateLight function Signed-off-by: Ian Chen * add model editor src files Signed-off-by: Ian Chen * remove more commented out code Signed-off-by: Ian Chen * use entity instead of entity name (#1176) Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig * 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 * fix adding ellipsoid Signed-off-by: Ian Chen * merge model_editor into component_inspector Signed-off-by: Ian Chen * fixing warnings Signed-off-by: Ian Chen * Adjust tool tips Signed-off-by: Nate Koenig * fix adding light Signed-off-by: Ian Chen * Fix codecheck Signed-off-by: Nate Koenig * Fixed documentation Signed-off-by: Nate Koenig * Working on model creation Signed-off-by: Nate Koenig * Fix build Signed-off-by: Nate Koenig * Added debug statements Signed-off-by: Nate Koenig * use each no cache Signed-off-by: Ian Chen * fix removing component from view Signed-off-by: Ian Chen * Fix physics Signed-off-by: Nate Koenig * Fix codecheck Signed-off-by: Nate Koenig Co-authored-by: Ian Chen Co-authored-by: Ashton Larkin Co-authored-by: Nate Koenig * Address comments Signed-off-by: Nate Koenig * update cameras list on sensor removal Signed-off-by: Ian Chen * update cameras list on sensor removal Signed-off-by: Ian Chen * Require sensors 6.1 Signed-off-by: Nate Koenig * sensors 6.0.1 Signed-off-by: Nate Koenig * Test model recreation with jointed models Signed-off-by: Michael Carroll * Fix multiple joints with same names There was an issue in searching for joint parent_link and child_link frames if there was another model with the same frame names. This will correctly search for frame entity ids that are children of the same model. Signed-off-by: Michael Carroll * Recreate entities joints (#1206) * Test model recreation with jointed models * Fix multiple joints with same names There was an issue in searching for joint parent_link and child_link frames if there was another model with the same frame names. This will correctly search for frame entity ids that are children of the same model. Signed-off-by: Michael Carroll * Fix the ecm test, which had bad parent-child relationships between links and joints Signed-off-by: Nate Koenig * Added test for world joints Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig Co-authored-by: Michael Carroll Co-authored-by: Louise Poubel Co-authored-by: Nate Koenig Co-authored-by: Ashton Larkin --- CMakeLists.txt | 2 +- .../ignition/gazebo/components/Recreate.hh | 48 ++ include/ignition/gazebo/detail/View.hh | 4 + src/EntityComponentManager.cc | 112 ++- src/EntityComponentManager_TEST.cc | 45 +- src/SimulationRunner.cc | 117 +++- src/SimulationRunner.hh | 20 + src/SimulationRunner_TEST.cc | 1 + .../component_inspector/ComponentInspector.cc | 1 + .../component_inspector/ComponentInspector.hh | 1 - .../component_inspector/ModelEditor.cc | 4 + src/systems/physics/Physics.cc | 61 +- src/systems/sensors/Sensors.cc | 12 +- test/integration/CMakeLists.txt | 1 + test/integration/recreate_entities.cc | 642 ++++++++++++++++++ test/worlds/double_pendulum.sdf | 451 ++++++++++++ test/worlds/fixed_world_joint.sdf | 88 +++ 17 files changed, 1560 insertions(+), 50 deletions(-) create mode 100644 include/ignition/gazebo/components/Recreate.hh create mode 100644 test/integration/recreate_entities.cc create mode 100644 test/worlds/double_pendulum.sdf create mode 100644 test/worlds/fixed_world_joint.sdf diff --git a/CMakeLists.txt b/CMakeLists.txt index 406ee1b308..35eefea33b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -110,7 +110,7 @@ set(IGN_PHYSICS_VER ${ignition-physics5_VERSION_MAJOR}) #-------------------------------------- # Find ignition-sensors -ign_find_package(ignition-sensors6 REQUIRED +ign_find_package(ignition-sensors6 REQUIRED VERSION 6.0.1 # component order is important COMPONENTS # non-rendering diff --git a/include/ignition/gazebo/components/Recreate.hh b/include/ignition/gazebo/components/Recreate.hh new file mode 100644 index 0000000000..bb053fe5f9 --- /dev/null +++ b/include/ignition/gazebo/components/Recreate.hh @@ -0,0 +1,48 @@ +/* + * 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_COMPONENTS_RECREATE_HH_ +#define IGNITION_GAZEBO_COMPONENTS_RECREATE_HH_ + +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component that identifies an entity needs to be recreated. + /// Currently, only Models will be processed for recreation by the + /// SimulationRunner in the ProcessRecreateEntitiesRemove and + /// ProcessRecreateEntitiesCreate functions. + /// + /// The GUI ModelEditor contains example code on how to use this + /// component. For example, the ModelEditor allows a user to add a link to an + /// existing model. The existing model is tagged with this component so + /// that it can be recreated by the server. + using Recreate = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Recreate", Recreate) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/detail/View.hh b/include/ignition/gazebo/detail/View.hh index 93ea32c1b1..7b6ffd34b8 100644 --- a/include/ignition/gazebo/detail/View.hh +++ b/include/ignition/gazebo/detail/View.hh @@ -280,6 +280,10 @@ template bool View::NotifyComponentRemoval(const Entity _entity, const ComponentTypeId _typeId) { + // if entity is still marked as to add, remove from the view + if (this->RequiresComponent(_typeId)) + this->toAddEntities.erase(_entity); + // make sure that _typeId is a type required by the view and that _entity is // already a part of the view if (!this->RequiresComponent(_typeId) || diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 1b261294d0..5b0f482811 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -39,6 +39,8 @@ #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/ParentLinkName.hh" +#include "ignition/gazebo/components/Recreate.hh" +#include "ignition/gazebo/components/World.hh" using namespace ignition; using namespace gazebo; @@ -420,7 +422,30 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent, } else if (!_name.empty() && !_allowRename) { - if (kNullEntity != this->EntityByComponents(components::Name(_name))) + // Get the entity's original parent. This is used to make sure we get + // the correct entity. For example, two different models may have a + // child with the name "link". + auto origParentComp = + this->Component(_entity); + + // If there is an entity with the same name and user indicated renaming is + // not allowed then return null entity. + // If the entity or one of its ancestor has a Recreate component then carry + // on since the ECM is supposed to create a new entity with the same name. + Entity ent = this->EntityByComponents(components::Name(_name), + components::ParentEntity(origParentComp->Data())); + + bool hasRecreateComp = false; + Entity recreateEnt = ent; + while (recreateEnt != kNullEntity && !hasRecreateComp) + { + hasRecreateComp = this->Component(recreateEnt) != + nullptr; + auto parentComp = this->Component(recreateEnt); + recreateEnt = parentComp ? parentComp->Data() : kNullEntity; + } + + if (kNullEntity != ent && !hasRecreateComp) { ignerr << "Requested to clone entity [" << _entity << "] with a name of [" << _name << "], but another entity already " @@ -497,19 +522,35 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent, Entity originalParentLink = kNullEntity; Entity originalChildLink = kNullEntity; + auto origParentComp = + this->Component(_entity); + const auto &parentName = this->Component(_entity); - if (parentName) + if (parentName && origParentComp) { - originalParentLink = this->EntityByComponents( - components::Name(parentName->Data())); + // CHangel the case where the parent link name is the world. + if (common::lowercase(parentName->Data()) == "world") + { + originalParentLink = this->Component( + origParentComp->Data())->Data(); + } + else + { + originalParentLink = + this->EntityByComponents( + components::Name(parentName->Data()), + components::ParentEntity(origParentComp->Data())); + } } const auto &childName = this->Component(_entity); - if (childName) + if (childName && origParentComp) { - originalChildLink = this->EntityByComponents( - components::Name(childName->Data())); + originalChildLink = + this->EntityByComponents( + components::Name(childName->Data()), + components::ParentEntity(origParentComp->Data())); } if (!originalParentLink || !originalChildLink) @@ -533,7 +574,14 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent, for (const auto &childEntity : this->EntitiesByComponents(components::ParentEntity(_entity))) { - auto clonedChild = this->CloneImpl(childEntity, clonedEntity, "", true); + std::string name; + if (!_allowRename) + { + auto nameComp = this->Component(childEntity); + name = nameComp->Data(); + } + auto clonedChild = this->CloneImpl(childEntity, clonedEntity, name, + _allowRename); if (kNullEntity == clonedChild) { ignerr << "Cloning child entity [" << childEntity << "] failed.\n"; @@ -1038,6 +1086,14 @@ bool EntityComponentManager::CreateComponentImplementation( this->dataPtr->createdCompTypes.insert(_componentTypeId); + // If the component is a components::ParentEntity, then make sure to + // update the entities graph. + if (_componentTypeId == components::ParentEntity::typeId) + { + auto parentComp = this->Component(_entity); + this->SetParentEntity(_entity, parentComp->Data()); + } + return updateData; } @@ -1937,24 +1993,38 @@ bool EntityComponentManagerPrivate::ClonedJointLinkName(Entity _joint, return false; } - auto iter = this->originalToClonedLink.find(_originalLink); - if (iter == this->originalToClonedLink.end()) + Entity clonedLink = kNullEntity; + + + std::string name; + // Handle the case where the link coule have been the world. + if (_ecm->Component(_originalLink) != nullptr) { - ignerr << "Error: attempted to clone links, but link [" - << _originalLink << "] was never cloned.\n"; - return false; + // Use the special identifier "world". + name = "world"; } - auto clonedLink = iter->second; - - auto name = _ecm->Component(clonedLink); - if (!name) + else { - ignerr << "Link [" << _originalLink << "] was cloned, but its clone has no " - << "name.\n"; - return false; + auto iter = this->originalToClonedLink.find(_originalLink); + if (iter == this->originalToClonedLink.end()) + { + ignerr << "Error: attempted to clone links, but link [" + << _originalLink << "] was never cloned.\n"; + return false; + } + clonedLink = iter->second; + + auto nameComp = _ecm->Component(clonedLink); + if (!nameComp) + { + ignerr << "Link [" << _originalLink + << "] was cloned, but its clone has no name.\n"; + return false; + } + name = nameComp->Data(); } - _ecm->SetComponentData(_joint, name->Data()); + _ecm->SetComponentData(_joint, name); return true; } diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index ed45452d66..ee5a5abf18 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -2795,15 +2795,21 @@ TEST_P(EntityComponentManagerFixture, CloneEntities) EXPECT_EQ(kNullEntity, failedClonedEntity); // create a joint with a parent and child link + const std::string parentModelEntityName = "parentModelEntity"; const std::string parentLinkEntityName = "parentLinkEntity"; const std::string childLinkEntityName = "childLinkEntity"; + Entity parentModelEntity = manager.CreateEntity(); + manager.CreateComponent(parentModelEntity, + components::Name(parentModelEntityName)); Entity parentLinkEntity = manager.CreateEntity(); manager.CreateComponent(parentLinkEntity, components::Name(parentLinkEntityName)); manager.CreateComponent(parentLinkEntity, components::CanonicalLink()); + manager.CreateComponent(parentLinkEntity, + components::ParentEntity(parentModelEntity)); Entity jointEntity = manager.CreateEntity(); manager.CreateComponent(jointEntity, - components::ParentEntity(parentLinkEntity)); + components::ParentEntity(parentModelEntity)); manager.CreateComponent(jointEntity, components::Name("jointEntity")); manager.CreateComponent(jointEntity, components::Joint()); manager.CreateComponent(jointEntity, @@ -2812,26 +2818,33 @@ TEST_P(EntityComponentManagerFixture, CloneEntities) components::ChildLinkName(childLinkEntityName)); Entity childLinkEntity = manager.CreateEntity(); manager.CreateComponent(childLinkEntity, - components::ParentEntity(jointEntity)); + components::ParentEntity(parentModelEntity)); manager.CreateComponent(childLinkEntity, components::Name(childLinkEntityName)); manager.CreateComponent(childLinkEntity, components::Link()); - EXPECT_EQ(13u, manager.EntityCount()); + EXPECT_EQ(14u, manager.EntityCount()); // clone a joint that has a parent and child link. - auto clonedParentLinkEntity = manager.Clone(parentLinkEntity, kNullEntity, + auto clonedParentModelEntity = manager.Clone(parentModelEntity, kNullEntity, "", true); - ASSERT_NE(kNullEntity, clonedParentLinkEntity); - EXPECT_EQ(16u, manager.EntityCount()); - clonedEntities.insert(clonedParentLinkEntity); + ASSERT_NE(kNullEntity, clonedParentModelEntity); + // We just cloned a model with two links and a joint, a total of 4 new + // entities. + EXPECT_EQ(18u, manager.EntityCount()); + clonedEntities.insert(clonedParentModelEntity); auto clonedJoints = manager.EntitiesByComponents( - components::ParentEntity(clonedParentLinkEntity)); + components::ParentEntity(clonedParentModelEntity), components::Joint()); ASSERT_EQ(1u, clonedJoints.size()); clonedEntities.insert(clonedJoints[0]); auto clonedChildLinks = manager.EntitiesByComponents( - components::ParentEntity(clonedJoints[0])); + components::ParentEntity(clonedParentModelEntity), components::Link()); ASSERT_EQ(1u, clonedChildLinks.size()); clonedEntities.insert(clonedChildLinks[0]); + auto clonedChildCanonicalLinks = manager.EntitiesByComponents( + components::ParentEntity(clonedParentModelEntity), + components::CanonicalLink()); + ASSERT_EQ(1u, clonedChildCanonicalLinks.size()); + clonedEntities.insert(clonedChildCanonicalLinks[0]); // The cloned joint should have the cloned parent/child link names attached to // it, not the original parent/child link names @@ -2843,17 +2856,19 @@ TEST_P(EntityComponentManagerFixture, CloneEntities) manager.Component(clonedJoints[0]); ASSERT_NE(nullptr, clonedJointChildLinkName); EXPECT_NE(clonedJointChildLinkName->Data(), childLinkEntityName); - auto clonedParentLinkName = - manager.Component(clonedParentLinkEntity); - ASSERT_NE(nullptr, clonedParentLinkName); - EXPECT_EQ(clonedParentLinkName->Data(), clonedJointParentLinkName->Data()); + auto clonedParentModelName = + manager.Component(clonedParentModelEntity); + ASSERT_NE(nullptr, clonedParentModelName); + auto clonedJointParentModelName = manager.Component( + manager.Component(clonedJoints[0])->Data()); + EXPECT_EQ(clonedParentModelName->Data(), clonedJointParentModelName->Data()); auto clonedChildLinkName = manager.Component(clonedChildLinks[0]); ASSERT_NE(nullptr, clonedChildLinkName); EXPECT_EQ(clonedJointChildLinkName->Data(), clonedChildLinkName->Data()); // make sure that the name given to each cloned entity is unique - EXPECT_EQ(8u, clonedEntities.size()); + EXPECT_EQ(9u, clonedEntities.size()); for (const auto &entity : clonedEntities) { auto nameComp = manager.Component(entity); @@ -2864,7 +2879,7 @@ TEST_P(EntityComponentManagerFixture, CloneEntities) // try to clone an entity that does not exist EXPECT_EQ(kNullEntity, manager.Clone(kNullEntity, topLevelEntity, "", allowRename)); - EXPECT_EQ(16u, manager.EntityCount()); + EXPECT_EQ(18u, manager.EntityCount()); } ///////////////////////////////////////////////// diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 0a0ca96b33..ee7e06515e 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -27,8 +27,10 @@ #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/PhysicsCmd.hh" +#include "ignition/gazebo/components/Recreate.hh" #include "ignition/gazebo/Events.hh" #include "ignition/gazebo/SdfEntityCreator.hh" #include "ignition/gazebo/Util.hh" @@ -820,6 +822,10 @@ void SimulationRunner::Step(const UpdateInfo &_info) IGN_PROFILE("SimulationRunner::Step"); this->currentInfo = _info; + // Process new ECM state information, typically sent from the GUI after + // a change was made to the GUI's ECM. + this->ProcessNewWorldControlState(); + // Publish info this->PublishStats(); @@ -831,6 +837,12 @@ void SimulationRunner::Step(const UpdateInfo &_info) // Handle pending systems this->ProcessSystemQueue(); + // Handle entities that need to be recreated. + // Put in a request to mark them as removed so that in the UpdateSystem call + // the systems can remove them first before new ones are created. This is + // so that we can recreate entities with the same name. + this->ProcessRecreateEntitiesRemove(); + // Update all the systems. this->UpdateSystems(); @@ -861,6 +873,12 @@ void SimulationRunner::Step(const UpdateInfo &_info) // Clear all new entities this->entityCompMgr.ClearNewlyCreatedEntities(); + // Recreate any entities that have the Recreate component + // The entities will have different Entity ids but keep the same name + // Make sure this happens after ClearNewlyCreatedEntities, otherwise the + // cloned entities will loose their "New" state. + this->ProcessRecreateEntitiesCreate(); + // Process entity removals. this->entityCompMgr.ProcessRemoveEntityRequests(); @@ -1136,11 +1154,13 @@ bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req, { std::lock_guard lock(this->msgBufferMutex); - // update the server ECM if the request contains SerializedState information + // Copy the state information if it exists if (_req.has_state()) - this->entityCompMgr.SetState(_req.state()); - // TODO(anyone) notify server systems of changes made to the ECM, if there - // were any? + { + if (this->newWorldControlState == nullptr) + this->newWorldControlState = _req.New(); + this->newWorldControlState->CopyFrom(_req); + } WorldControl control; control.pause = _req.world_control().pause(); @@ -1170,7 +1190,7 @@ bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req, { control.runToSimTime = std::chrono::seconds( _req.world_control().run_to_sim_time().sec()) + - std::chrono::nanoseconds(_req.world_control().run_to_sim_time().nsec()); + std::chrono::nanoseconds(_req.world_control().run_to_sim_time().nsec()); } this->worldControls.push_back(control); @@ -1179,6 +1199,22 @@ bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req, return true; } +///////////////////////////////////////////////// +void SimulationRunner::ProcessNewWorldControlState() +{ + std::lock_guard lock(this->msgBufferMutex); + // update the server ECM if the request contains SerializedState information + if (this->newWorldControlState && this->newWorldControlState->has_state()) + { + this->entityCompMgr.SetState(this->newWorldControlState->state()); + + delete this->newWorldControlState; + this->newWorldControlState = nullptr; + } + // TODO(anyone) notify server systems of changes made to the ECM, if there + // were any? +} + ///////////////////////////////////////////////// bool SimulationRunner::OnPlaybackControl(const msgs::LogPlaybackControl &_req, msgs::Boolean &_res) @@ -1252,6 +1288,77 @@ void SimulationRunner::ProcessWorldControl() this->worldControls.clear(); } +///////////////////////////////////////////////// +void SimulationRunner::ProcessRecreateEntitiesRemove() +{ + IGN_PROFILE("SimulationRunner::ProcessRecreateEntitiesRemove"); + + // store the original entities to recreate and put in request to remove them + this->entityCompMgr.Each( + [&](const Entity &_entity, + const components::Model *, + const components::Recreate *)->bool + { + this->entitiesToRecreate.insert(_entity); + this->entityCompMgr.RequestRemoveEntity(_entity, true); + return true; + }); +} + +///////////////////////////////////////////////// +void SimulationRunner::ProcessRecreateEntitiesCreate() +{ + IGN_PROFILE("SimulationRunner::ProcessRecreateEntitiesCreate"); + + // clone the original entities + for (auto & ent : this->entitiesToRecreate) + { + auto nameComp = this->entityCompMgr.Component(ent); + auto parentComp = + this->entityCompMgr.Component(ent); + if (nameComp && parentComp) + { + // set allowRenaming to false so the entities keep their original name + Entity clonedEntity = this->entityCompMgr.Clone(ent, + parentComp->Data(), nameComp->Data(), false); + + // remove the Recreate component so they do not get recreated again in the + // next iteration + if (!this->entityCompMgr.RemoveComponent(ent)) + { + ignerr << "Failed to remove Recreate component from entity[" + << ent << "]" << std::endl; + } + + if (!this->entityCompMgr.RemoveComponent( + clonedEntity)) + { + ignerr << "Failed to remove Recreate component from entity[" + << clonedEntity << "]" << std::endl; + } + } + else + { + if (!nameComp) + { + ignerr << "Missing name component for entity[" << ent << "]. " + << "The entity will not be cloned during the recreation process." + << std::endl; + } + + if (!parentComp) + { + ignerr << "Missing parent component for entity[" << ent << "]. " + << "The entity will not be cloned during the recreation process." + << std::endl; + } + } + } + + this->entitiesToRecreate.clear(); +} + ///////////////////////////////////////////////// bool SimulationRunner::Paused() const { diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 9f17d59b6b..509e2b957a 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -466,6 +467,18 @@ namespace ignition std::optional _entity = std::nullopt, std::optional> _sdf = std::nullopt); + /// \brief Process entities with the components::Recreate component. + /// Put in a request to make them as removed + private: void ProcessRecreateEntitiesRemove(); + + /// \brief Process entities with the components::Recreate component. + /// Reccreate the entities by cloning from the original ones. + private: void ProcessRecreateEntitiesCreate(); + + /// \brief Process the new world state message, if it is present. + /// See the newWorldControlState variable below. + private: void ProcessNewWorldControlState(); + /// \brief This is used to indicate that a stop event has been received. private: std::atomic stopReceived{false}; @@ -627,6 +640,13 @@ namespace ignition /// WorldControl info (true) or not (false) private: bool stepping{false}; + /// \brief A set of entities that need to be recreated + private: std::set entitiesToRecreate; + + /// \brief Holds new world state information so that it can processed + /// at the appropriate time. + private: msgs::WorldControlState *newWorldControlState{nullptr}; + friend class LevelManager; }; } diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 6af7a70a90..c77cfaafc7 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -56,6 +56,7 @@ #include "ignition/gazebo/Events.hh" #include "ignition/gazebo/Util.hh" #include "ignition/gazebo/config.hh" + #include "../test/helpers/EnvTestFixture.hh" #include "SimulationRunner.hh" diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index d5e2a58df4..0632c355a6 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 diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 1790c3b521..6661bce09e 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -61,7 +61,6 @@ namespace gazebo << _item->text().toStdString() << "]" << std::endl; } } - /// \brief Specialized to set string data. /// \param[in] _item Item whose data will be set. /// \param[in] _data Data to set. diff --git a/src/gui/plugins/component_inspector/ModelEditor.cc b/src/gui/plugins/component_inspector/ModelEditor.cc index d40cae6e1a..a40e8117da 100644 --- a/src/gui/plugins/component_inspector/ModelEditor.cc +++ b/src/gui/plugins/component_inspector/ModelEditor.cc @@ -32,6 +32,7 @@ #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Recreate.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/SdfEntityCreator.hh" @@ -181,6 +182,9 @@ void ModelEditor::Update(const UpdateInfo &, linkSdf.SetName(linkName); auto entity = this->dataPtr->entityCreator->CreateEntities(&linkSdf); this->dataPtr->entityCreator->SetParent(entity, eta.parentEntity); + // Make sure to mark the parent as needing recreation. This will + // tell the server to rebuild the model with the new link. + _ecm.CreateComponent(eta.parentEntity, components::Recreate()); // traverse the tree and add all new entities created by the entity // creator to the set diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 9407cc8d33..f2414936ba 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -122,6 +123,7 @@ #include "ignition/gazebo/components/PhysicsEnginePlugin.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" +#include "ignition/gazebo/components/Recreate.hh" #include "ignition/gazebo/components/SelfCollide.hh" #include "ignition/gazebo/components/SlipComplianceCmd.hh" #include "ignition/gazebo/components/Static.hh" @@ -612,6 +614,13 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief A map between collision entity ids in the ECM to FreeGroup Entities /// in ign-physics. public: EntityFreeGroupMap entityFreeGroupMap; + + /// \brief Set of links that were added to an existing model. This set + /// is used to track links that were added to an existing model, such as + /// through the GUI model editor, so that we can avoid premature creation + /// of links and collision elements. This also lets us suppress some + /// invalid error messages. + public: std::set linkAddedToModel; }; ////////////////////////////////////////////////// @@ -778,6 +787,9 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) ////////////////////////////////////////////////// void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) { + // Clear the set of links that were added to a model. + this->linkAddedToModel.clear(); + this->CreateWorldEntities(_ecm); this->CreateModelEntities(_ecm); this->CreateLinkEntities(_ecm); @@ -879,6 +891,9 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) const components::Pose *_pose, const components::ParentEntity *_parent)->bool { + if (_ecm.EntityHasComponentType(_entity, components::Recreate::typeId)) + return true; + // Check if model already exists if (this->entityModelMap.HasEntity(_entity)) { @@ -1011,6 +1026,18 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) const components::Pose *_pose, const components::ParentEntity *_parent)->bool { + // If the parent model is scheduled for recreation, then do not + // try to create a new link. This situation can occur when a link + // is added to a model from the GUI model editor. + if (_ecm.EntityHasComponentType(_parent->Data(), + components::Recreate::typeId)) + { + // Add this entity to the set of newly added links to existing + // models. + this->linkAddedToModel.insert(_entity); + return true; + } + // Check if link already exists if (this->entityLinkMap.HasEntity(_entity)) { @@ -1072,6 +1099,14 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) const components::CollisionElement *_collElement, const components::ParentEntity *_parent) -> bool { + // Check to see if this collision's parent is a link that was + // not created because the parent model is marked for recreation. + if (this->linkAddedToModel.find(_parent->Data()) != + this->linkAddedToModel.end()) + { + return true; + } + if (this->entityCollisionMap.HasEntity(_entity)) { ignwarn << "Collision entity [" << _entity @@ -2182,14 +2217,23 @@ std::map PhysicsPrivate::ChangedLinks( _ecm.Each( [&](const Entity &_entity, components::Link *) -> bool { - if (this->staticEntities.find(_entity) != this->staticEntities.end()) + if (this->staticEntities.find(_entity) != this->staticEntities.end() || + _ecm.EntityHasComponentType(_entity, components::Recreate::typeId)) + { return true; + } + // This `once` variable is here to aid in debugging, make sure to + // remove it. auto linkPhys = this->entityLinkMap.Get(_entity); if (nullptr == linkPhys) { - ignerr << "Internal error: link [" << _entity - << "] not in entity map" << std::endl; + if (this->linkAddedToModel.find(_entity) == + this->linkAddedToModel.end()) + { + ignerr << "Internal error: link [" << _entity + << "] not in entity map" << std::endl; + } return true; } @@ -2339,8 +2383,12 @@ bool PhysicsPrivate::GetFrameDataRelativeToWorld(const Entity _entity, auto entityPhys = this->entityLinkMap.Get(_entity); if (nullptr == entityPhys) { - ignerr << "Internal error: entity [" << _entity - << "] not in entity map" << std::endl; + // Suppress error message if the link has just been added to the model. + if (this->linkAddedToModel.find(_entity) == this->linkAddedToModel.end()) + { + ignerr << "Internal error: entity [" << _entity + << "] not in entity map" << std::endl; + } return false; } @@ -2437,7 +2485,8 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, if (parentModelPoseIt == this->modelWorldPoses.end()) { ignerr << "Internal error: parent model [" << parentEntity - << "] does not have a world pose available" << std::endl; + << "] does not have a world pose available for child entity[" + << entity << "]" << std::endl; continue; } const math::Pose3d &parentWorldPose = parentModelPoseIt->second; diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 9f5b0f434b..a2739240ca 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -91,7 +91,6 @@ class ignition::gazebo::systems::SensorsPrivate /// \brief Keep track of cameras, in case we need to handle stereo cameras. /// Key: Camera's parent scoped name /// Value: Pointer to camera - // TODO(anyone) Remove element when sensor is deleted public: std::map cameras; /// \brief Maps gazebo entity to its matching sensor ID @@ -361,6 +360,17 @@ void Sensors::RemoveSensor(const Entity &_entity) this->dataPtr->activeSensors.erase(activeSensorIt); } } + + // update cameras list + for (auto &it : this->dataPtr->cameras) + { + if (it.second->Id() == idIter->second) + { + this->dataPtr->cameras.erase(it.first); + break; + } + } + this->dataPtr->sensorIds.erase(idIter->second); this->dataPtr->sensorManager.Remove(idIter->second); this->dataPtr->entityToIdMap.erase(idIter); diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index ca60386c8a..a2fec80165 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -47,6 +47,7 @@ set(tests physics_system.cc play_pause.cc pose_publisher_system.cc + recreate_entities.cc save_world.cc scene_broadcaster_system.cc sdf_frame_semantics.cc diff --git a/test/integration/recreate_entities.cc b/test/integration/recreate_entities.cc new file mode 100644 index 0000000000..74ca10950b --- /dev/null +++ b/test/integration/recreate_entities.cc @@ -0,0 +1,642 @@ +/* + * 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 "ignition/gazebo/components/Factory.hh" +#include "ignition/gazebo/components/Joint.hh" +#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/components/ParentLinkName.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/Recreate.hh" +#include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" // NOLINT(build/include) + +#include "../helpers/Relay.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; +using namespace components; + +using namespace std::chrono_literals; + +class RecreateEntitiesFixture : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(RecreateEntitiesFixture, RecreateEntities) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/shapes.sdf"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + EntityComponentManager *ecm = nullptr; + bool recreateEntities = false; + + // Create a system that adds a recreate component to models + test::Relay testSystem; + testSystem.OnUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + if (!ecm) + { + ecm = &_ecm; + recreateEntities = true; + return; + } + + if (recreateEntities) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Model *) -> bool + { + // add a components::Recreate to indicate that this entity + // needs to be recreated + _ecm.CreateComponent(_entity, components::Recreate()); + return true; + }); + + // recreate entities only once so set it back to false + recreateEntities = false; + return; + } + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + EXPECT_NE(nullptr, ecm); + + // model entity ids + Entity boxModelEntity = kNullEntity; + Entity cylModelEntity = kNullEntity; + Entity sphModelEntity = kNullEntity; + Entity capModelEntity = kNullEntity; + Entity ellipModelEntity = kNullEntity; + + // link entity ids + Entity boxLinkEntity = kNullEntity; + Entity cylLinkEntity = kNullEntity; + Entity sphLinkEntity = kNullEntity; + Entity capLinkEntity = kNullEntity; + Entity ellipLinkEntity = kNullEntity; + + auto validateEntities = [&]() + { + boxModelEntity = kNullEntity; + cylModelEntity = kNullEntity; + sphModelEntity = kNullEntity; + capModelEntity = kNullEntity; + ellipModelEntity = kNullEntity; + + boxLinkEntity = kNullEntity; + cylLinkEntity = kNullEntity; + sphLinkEntity = kNullEntity; + capLinkEntity = kNullEntity; + ellipLinkEntity = kNullEntity; + + // Check entities + // 1 x world + 1 x (default) level + 1 x wind + 5 x model + 5 x link + 5 x + // collision + 5 x visual + 1 x light + EXPECT_EQ(24u, ecm->EntityCount()); + + Entity worldEntity = + ecm->EntityByComponents(components::World()); + EXPECT_NE(kNullEntity, worldEntity); + + // Check models + unsigned int modelCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Model *_model, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _model); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + modelCount++; + + EXPECT_EQ(worldEntity, _parent->Data()); + if (modelCount == 1) + { + EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 1), + _pose->Data()); + EXPECT_EQ("box", _name->Data()); + boxModelEntity = _entity; + } + else if (modelCount == 2) + { + EXPECT_EQ(ignition::math::Pose3d(-1, -2, -3, 0, 0, 1), + _pose->Data()); + EXPECT_EQ("cylinder", _name->Data()); + cylModelEntity = _entity; + } + else if (modelCount == 3) + { + EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 0, 1), + _pose->Data()); + EXPECT_EQ("sphere", _name->Data()); + sphModelEntity = _entity; + } + else if (modelCount == 4) + { + EXPECT_EQ(ignition::math::Pose3d(-4, -5, -6, 0, 0, 1), + _pose->Data()); + EXPECT_EQ("capsule", _name->Data()); + capModelEntity = _entity; + } + else if (modelCount == 5) + { + EXPECT_EQ(ignition::math::Pose3d(4, 5, 6, 0, 0, 1), + _pose->Data()); + EXPECT_EQ("ellipsoid", _name->Data()); + ellipModelEntity = _entity; + } + return true; + }); + + EXPECT_EQ(5u, modelCount); + EXPECT_NE(kNullEntity, boxModelEntity); + EXPECT_NE(kNullEntity, cylModelEntity); + EXPECT_NE(kNullEntity, sphModelEntity); + EXPECT_NE(kNullEntity, capModelEntity); + EXPECT_NE(kNullEntity, ellipModelEntity); + + // Check links + unsigned int linkCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Link *_link, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _link); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + linkCount++; + + if (linkCount == 1) + { + EXPECT_EQ(ignition::math::Pose3d(0.1, 0.1, 0.1, 0, 0, 0), + _pose->Data()); + EXPECT_EQ("box_link", _name->Data()); + EXPECT_EQ(boxModelEntity, _parent->Data()); + boxLinkEntity = _entity; + } + else if (linkCount == 2) + { + EXPECT_EQ(ignition::math::Pose3d(0.2, 0.2, 0.2, 0, 0, 0), + _pose->Data()); + EXPECT_EQ("cylinder_link", _name->Data()); + EXPECT_EQ(cylModelEntity, _parent->Data()); + cylLinkEntity = _entity; + } + else if (linkCount == 3) + { + EXPECT_EQ(ignition::math::Pose3d(0.3, 0.3, 0.3, 0, 0, 0), + _pose->Data()); + EXPECT_EQ("sphere_link", _name->Data()); + EXPECT_EQ(sphModelEntity, _parent->Data()); + sphLinkEntity = _entity; + } + else if (linkCount == 4) + { + EXPECT_EQ(ignition::math::Pose3d(0.5, 0.5, 0.5, 0, 0, 0), + _pose->Data()); + EXPECT_EQ("capsule_link", _name->Data()); + EXPECT_EQ(capModelEntity, _parent->Data()); + capLinkEntity = _entity; + } + else if (linkCount == 5) + { + EXPECT_EQ(ignition::math::Pose3d(0.8, 0.8, 0.8, 0, 0, 0), + _pose->Data()); + EXPECT_EQ("ellipsoid_link", _name->Data()); + EXPECT_EQ(ellipModelEntity, _parent->Data()); + ellipLinkEntity = _entity; + } + return true; + }); + + EXPECT_EQ(5u, linkCount); + EXPECT_NE(kNullEntity, boxLinkEntity); + EXPECT_NE(kNullEntity, cylLinkEntity); + EXPECT_NE(kNullEntity, sphLinkEntity); + EXPECT_NE(kNullEntity, capLinkEntity); + EXPECT_NE(kNullEntity, ellipLinkEntity); + }; + + // validate initial state + validateEntities(); + + // store the original entity ids + Entity originalboxModelEntity = boxModelEntity; + Entity originalcylModelEntity = cylModelEntity; + Entity originalsphModelEntity = sphModelEntity; + Entity originalcapModelEntity = capModelEntity; + Entity originalellipModelEntity = ellipModelEntity; + Entity originalboxLinkEntity = boxLinkEntity; + Entity originalcylLinkEntity = cylLinkEntity; + Entity originalsphLinkEntity = sphLinkEntity; + Entity originalcapLinkEntity = capLinkEntity; + Entity originalellipLinkEntity = ellipLinkEntity; + + // Run once to let the test relay system creates the components::Recreate + server.Run(true, 1, false); + + // Run again so that entities get recreated in the ECM + server.Run(true, 1, false); + + // validate that the entities are recreated + validateEntities(); + + // check that the newly recreated entities should have a different entity id + EXPECT_NE(originalboxModelEntity, boxModelEntity); + EXPECT_NE(originalcylModelEntity, cylModelEntity); + EXPECT_NE(originalsphModelEntity, sphModelEntity); + EXPECT_NE(originalcapModelEntity, capModelEntity); + EXPECT_NE(originalellipModelEntity, ellipModelEntity); + EXPECT_NE(originalboxLinkEntity, boxLinkEntity); + EXPECT_NE(originalcylLinkEntity, cylLinkEntity); + EXPECT_NE(originalsphLinkEntity, sphLinkEntity); + EXPECT_NE(originalcapLinkEntity, capLinkEntity); + EXPECT_NE(originalellipLinkEntity, ellipLinkEntity); + + // Run again to make sure the recreate components are removed and no + // entities to to be recreated + server.Run(true, 1, false); + + auto entities = ecm->EntitiesByComponents(components::Model(), + components::Recreate()); + EXPECT_TRUE(entities.empty()); +} + +///////////////////////////////////////////////// +TEST_F(RecreateEntitiesFixture, RecreateEntities_Joints) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/double_pendulum.sdf"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + EntityComponentManager *ecm = nullptr; + bool recreateEntities = false; + + // Create a system that adds a recreate component to models + test::Relay testSystem; + testSystem.OnUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + if (!ecm) + { + ecm = &_ecm; + recreateEntities = true; + return; + } + + if (recreateEntities) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Model *) -> bool + { + // add a components::Recreate to indicate that this entity + // needs to be recreated + _ecm.CreateComponent(_entity, components::Recreate()); + return true; + }); + + // recreate entities only once so set it back to false + recreateEntities = false; + return; + } + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + EXPECT_NE(nullptr, ecm); + + auto validateEntities = [&]() + { + // Check entities + // 1 x world + 1 x (default) level + 1 x wind + 5 x model + 5 x link + 5 x + // collision + 5 x visual + 1 x light + EXPECT_EQ(48u, ecm->EntityCount()); + + Entity worldEntity = + ecm->EntityByComponents(components::World()); + EXPECT_NE(kNullEntity, worldEntity); + + // Check models + unsigned int modelCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Model *_model, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _model); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + modelCount++; + + EXPECT_EQ(worldEntity, _parent->Data()); + return true; + }); + + EXPECT_EQ(3u, modelCount); + + // Check links + unsigned int linkCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Link *_link, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _link); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + linkCount++; + + return true; + }); + + EXPECT_EQ(7u, linkCount); + + // Check links + unsigned int jointCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Joint *_joint, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _joint); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + jointCount++; + + return true; + }); + + EXPECT_EQ(4u, jointCount); + }; + + // validate initial state + validateEntities(); + + // Run once to let the test relay system creates the components::Recreate + server.Run(true, 1, false); + + // Run again so that entities get recreated in the ECM + server.Run(true, 1, false); + + // validate that the entities are recreated + validateEntities(); + + // Run again to make sure the recreate components are removed and no + // entities to to be recreated + server.Run(true, 1, false); + + auto entities = ecm->EntitiesByComponents(components::Model(), + components::Recreate()); + EXPECT_TRUE(entities.empty()); +} + +///////////////////////////////////////////////// +TEST_F(RecreateEntitiesFixture, RecreateEntities_WorldJoint) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/fixed_world_joint.sdf"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + EntityComponentManager *ecm = nullptr; + bool recreateEntities = false; + + // Create a system that adds a recreate component to models + test::Relay testSystem; + testSystem.OnUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + if (!ecm) + { + ecm = &_ecm; + recreateEntities = true; + return; + } + + if (recreateEntities) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Model *) -> bool + { + // add a components::Recreate to indicate that this entity + // needs to be recreated + _ecm.CreateComponent(_entity, components::Recreate()); + return true; + }); + + // recreate entities only once so set it back to false + recreateEntities = false; + return; + } + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + EXPECT_NE(nullptr, ecm); + + auto validateEntities = [&]() + { + // Check entities + // 1 x world + 1 x (default) level + 1 x wind + 1 x model + 2 x link + 2 x + // collision + 2 x visual + 2 x joint + EXPECT_EQ(12u, ecm->EntityCount()); + + Entity worldEntity = + ecm->EntityByComponents(components::World()); + EXPECT_NE(kNullEntity, worldEntity); + + // Check models + unsigned int modelCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Model *_model, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _model); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + modelCount++; + + EXPECT_EQ(worldEntity, _parent->Data()); + return true; + }); + + EXPECT_EQ(1u, modelCount); + + // Check links + unsigned int linkCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Link *_link, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _link); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + linkCount++; + + return true; + }); + + EXPECT_EQ(2u, linkCount); + + // Check joint + unsigned int jointCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Joint *_joint, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name, + const components::ParentLinkName *_parentLinkName)->bool + { + EXPECT_NE(nullptr, _joint); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + EXPECT_NE(nullptr, _parentLinkName); + + std::cout << "JointName[" << _name->Data() << "]\n"; + if (_name->Data() == "world_fixed") + { + EXPECT_EQ("world", _parentLinkName->Data()); + } + jointCount++; + + return true; + }); + + EXPECT_EQ(2u, jointCount); + }; + + // validate initial state + validateEntities(); + + // Run once to let the test relay system creates the components::Recreate + server.Run(true, 1, false); + + // Run again so that entities get recreated in the ECM + server.Run(true, 1, false); + + // validate that the entities are recreated + validateEntities(); + + // Run again to make sure the recreate components are removed and no + // entities to to be recreated + server.Run(true, 1, false); + + auto entities = ecm->EntitiesByComponents(components::Model(), + components::Recreate()); + EXPECT_TRUE(entities.empty()); +} diff --git a/test/worlds/double_pendulum.sdf b/test/worlds/double_pendulum.sdf new file mode 100644 index 0000000000..10940e785f --- /dev/null +++ b/test/worlds/double_pendulum.sdf @@ -0,0 +1,451 @@ + + + + + 0.001 + 1.0 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + + + + upper_link + lower_link + + 1.0 0 0 + + + + + + 5 0 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + + + + upper_link + lower_link + + 1.0 0 0 + + + + + + diff --git a/test/worlds/fixed_world_joint.sdf b/test/worlds/fixed_world_joint.sdf new file mode 100644 index 0000000000..114675b885 --- /dev/null +++ b/test/worlds/fixed_world_joint.sdf @@ -0,0 +1,88 @@ + + + + + 0.001 + 1.0 + + + + 0 0 0 0 1.57 0 + + 0.0 0.0 0.0 0 0 0 + + + 2.501 + 0 + 0 + 2.501 + 0 + 5 + + 120.0 + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + + 0.0 0.0 0.1 0 0 0 + + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.25 0.1 0.05 + + + + 0.2 0.8 0.2 1 + + + + + + 0.25 0.1 0.05 + + + + + + + world + base_link + + + + 0 0 -0.5 0 0 0 + base_link + rotor + + 0 0 1 + + + + + From 9d4cd2411607f462228f36f4a3b4419d8fd63717 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 17 Nov 2021 11:05:57 -0800 Subject: [PATCH 2/2] Support editing air pressure sensor in the GUI (#1171) * Support editing air pressure sensor in the GUI Signed-off-by: Nate Koenig * Add noise to qrc Signed-off-by: Nate Koenig * Add noise to qrc Signed-off-by: Nate Koenig * Fix lint Signed-off-by: Michael Carroll * Update sensor icon Signed-off-by: Nate Koenig * Move AirPressure functions out of ComponentInspector (#1179) Signed-off-by: Louise Poubel * Fix get decimals, and address comments Signed-off-by: Nate Koenig * cleanup and simplification Signed-off-by: Nate Koenig * Require sdf 12.1.0 Signed-off-by: Nate Koenig * missign width Signed-off-by: Nate Koenig * Added simulation state aware spin box Signed-off-by: Nate Koenig * Remove console output Signed-off-by: Nate Koenig * Added state awareness to add entity button Signed-off-by: Nate Koenig * Fix codecheck Signed-off-by: Nate Koenig * Remove extra variable Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig Co-authored-by: Michael Carroll Co-authored-by: Louise Poubel --- CMakeLists.txt | 2 +- src/cmd/ModelCommandAPI.cc | 28 ++ .../component_inspector/AirPressure.cc | 120 +++++ .../component_inspector/AirPressure.hh | 63 +++ .../component_inspector/AirPressure.qml | 134 ++++++ .../component_inspector/CMakeLists.txt | 12 +- .../component_inspector/ComponentInspector.cc | 62 ++- .../component_inspector/ComponentInspector.hh | 34 ++ .../ComponentInspector.qml | 49 +- .../ComponentInspector.qrc | 3 + .../ExpandingTypeHeader.qml | 5 + src/gui/plugins/component_inspector/Noise.qml | 424 ++++++++++++++++++ .../plugins/component_inspector/Pose3d.qml | 2 +- .../component_inspector/StateAwareSpin.qml | 109 +++++ .../component_inspector/TypeHeader.qml | 20 +- src/gui/plugins/component_inspector/Types.cc | 33 ++ src/gui/plugins/component_inspector/Types.hh | 59 +++ .../plugins/component_inspector/Vector3d.qml | 2 +- src/gui/plugins/modules/TypeIcon.qml | 1 + src/gui/resources/gazebo.qrc | 1 + src/gui/resources/images/sensor.png | Bin 0 -> 7013 bytes .../scene_broadcaster/SceneBroadcaster.cc | 25 +- 22 files changed, 1165 insertions(+), 23 deletions(-) create mode 100644 src/gui/plugins/component_inspector/AirPressure.cc create mode 100644 src/gui/plugins/component_inspector/AirPressure.hh create mode 100644 src/gui/plugins/component_inspector/AirPressure.qml create mode 100644 src/gui/plugins/component_inspector/Noise.qml create mode 100644 src/gui/plugins/component_inspector/StateAwareSpin.qml create mode 100644 src/gui/plugins/component_inspector/Types.cc create mode 100644 src/gui/plugins/component_inspector/Types.hh create mode 100644 src/gui/resources/images/sensor.png diff --git a/CMakeLists.txt b/CMakeLists.txt index 35eefea33b..a261ce51c8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,7 +50,7 @@ set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) # as protobuf could be find transitively by any dependency set(protobuf_MODULE_COMPATIBLE TRUE) -ign_find_package(sdformat12 REQUIRED) +ign_find_package(sdformat12 REQUIRED VERSION 12.1) set(SDF_VER ${sdformat12_VERSION_MAJOR}) #-------------------------------------- diff --git a/src/cmd/ModelCommandAPI.cc b/src/cmd/ModelCommandAPI.cc index f467668e9f..77dc9e967c 100644 --- a/src/cmd/ModelCommandAPI.cc +++ b/src/cmd/ModelCommandAPI.cc @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -34,6 +35,7 @@ #include #include #include +#include #include #include #include @@ -182,6 +184,31 @@ void printNoise(const sdf::Noise &_noise, int _spaces) << _noise.DynamicBiasCorrelationTime() << std::endl; } +////////////////////////////////////////////////// +/// \brief Print info about an air pressure sensor. +/// \param[in] _entity Entity to print information for. Nothing is +/// printed if the entity is not an air pressure sensor. +/// \param[in] _ecm The entity component manager. +/// \param[in] _spaces Number of spaces to indent for every line +void printAirPressure(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::AirPressure *air = sensor.AirPressureSensor(); + + std::cout << std::string(_spaces, ' ') << "- Reference altitude: " + << air->ReferenceAltitude() << "\n"; + + std::cout << std::string(_spaces, ' ') << "- Pressure noise:\n"; + printNoise(air->PressureNoise(), _spaces + 2); +} + ////////////////////////////////////////////////// /// \brief Print info about an altimeter sensor. /// \param[in] _entity Entity to print information for. Nothing is @@ -603,6 +630,7 @@ void printLinks(const uint64_t _modelEntity, // Run through all the sensor print statements. Each function will // exit early if the the sensor is the wrong type. + printAirPressure(sensor, _ecm, spaces + 2); printAltimeter(sensor, _ecm, spaces + 2); printCamera(sensor, _ecm, spaces + 2); printImu(sensor, _ecm, spaces + 2); diff --git a/src/gui/plugins/component_inspector/AirPressure.cc b/src/gui/plugins/component_inspector/AirPressure.cc new file mode 100644 index 0000000000..d9b9656598 --- /dev/null +++ b/src/gui/plugins/component_inspector/AirPressure.cc @@ -0,0 +1,120 @@ +/* + * 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 "AirPressure.hh" +#include "ComponentInspector.hh" +#include "Types.hh" + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +AirPressure::AirPressure(ComponentInspector *_inspector) +{ + _inspector->Context()->setContextProperty("AirPressureImpl", this); + this->inspector = _inspector; + + ComponentCreator creator = + [=](EntityComponentManager &_ecm, Entity _entity, QStandardItem *_item) + { + auto comp = _ecm.Component(_entity); + if (nullptr == _item || nullptr == comp) + return; + const sdf::AirPressure *air = comp->Data().AirPressureSensor(); + + _item->setData(QString("AirPressure"), + ComponentsModel::RoleNames().key("dataType")); + _item->setData(QList({ + QVariant(air->ReferenceAltitude()), + QVariant(air->PressureNoise().Mean()), + QVariant(air->PressureNoise().BiasMean()), + QVariant(air->PressureNoise().StdDev()), + QVariant(air->PressureNoise().BiasStdDev()), + QVariant(air->PressureNoise().DynamicBiasStdDev()), + QVariant(air->PressureNoise().DynamicBiasCorrelationTime()), + }), ComponentsModel::RoleNames().key("data")); + }; + + this->inspector->RegisterComponentCreator( + components::AirPressureSensor::typeId, creator); +} + +///////////////////////////////////////////////// +Q_INVOKABLE void AirPressure::OnAirPressureNoise( + double _mean, double _meanBias, double _stdDev, + double _stdDevBias, double _dynamicBiasStdDev, + double _dynamicBiasCorrelationTime) +{ + ignition::gazebo::UpdateCallback cb = + [=](EntityComponentManager &_ecm) + { + auto comp = _ecm.Component( + this->inspector->Entity()); + if (comp) + { + sdf::AirPressure *airpressure = comp->Data().AirPressureSensor(); + if (airpressure) + { + sdf::Noise noise = airpressure->PressureNoise(); + + setNoise(noise, _mean, _meanBias, _stdDev, _stdDevBias, + _dynamicBiasStdDev, _dynamicBiasCorrelationTime); + + airpressure->SetPressureNoise(noise); + } + else + ignerr << "Unable to get the air pressure data.\n"; + } + else + { + ignerr << "Unable to get the air pressure component.\n"; + } + }; + this->inspector->AddUpdateCallback(cb); +} + +///////////////////////////////////////////////// +Q_INVOKABLE void AirPressure::OnAirPressureReferenceAltitude( + double _referenceAltitude) +{ + ignition::gazebo::UpdateCallback cb = + [=](EntityComponentManager &_ecm) + { + auto comp = _ecm.Component( + this->inspector->Entity()); + if (comp) + { + sdf::AirPressure *airpressure = comp->Data().AirPressureSensor(); + if (airpressure) + { + airpressure->SetReferenceAltitude(_referenceAltitude); + } + else + ignerr << "Unable to get the air pressure data.\n"; + } + else + { + ignerr << "Unable to get the air pressure component.\n"; + } + }; + this->inspector->AddUpdateCallback(cb); +} diff --git a/src/gui/plugins/component_inspector/AirPressure.hh b/src/gui/plugins/component_inspector/AirPressure.hh new file mode 100644 index 0000000000..9999b0068d --- /dev/null +++ b/src/gui/plugins/component_inspector/AirPressure.hh @@ -0,0 +1,63 @@ +/* + * 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_COMPONENTINSPECTOR_AIRPRESSURE_HH_ +#define IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_AIRPRESSURE_HH_ + +#include + +namespace ignition +{ +namespace gazebo +{ + class ComponentInspector; + + /// \brief A class that handles air pressure changes. + class AirPressure : public QObject + { + Q_OBJECT + + /// \brief Constructor + /// \param[in] _inspector The component inspector. + public: explicit AirPressure(ComponentInspector *_inspector); + + /// \brief This function is called when a user changes values in the + /// air pressure sensor. + /// \param[in] _mean Mean value + /// \param[in] _meanBias Bias mean value + /// \param[in] _stdDev Standard deviation value + /// \param[in] _stdDevBias Bias standard deviation value + /// \param[in] _dynamicBiasStdDev Dynamic bias standard deviation value + /// \param[in] _dynamicBiasCorrelationTime Dynamic bias correlation time + /// value + public: Q_INVOKABLE void OnAirPressureNoise( + double _mean, double _meanBias, double _stdDev, + double _stdDevBias, double _dynamicBiasStdDev, + double _dynamicBiasCorrelationTime); + + /// \brief This function is called when a user changes the air pressure + /// reference altitude. + /// \param[in] _referenceAltitude New reference altitude value. + public: Q_INVOKABLE void OnAirPressureReferenceAltitude( + double _referenceAltitude); + + /// \brief Pointer to the component inspector. This is used to add + /// update callbacks that modify the ECM. + private: ComponentInspector *inspector{nullptr}; + }; +} +} +#endif diff --git a/src/gui/plugins/component_inspector/AirPressure.qml b/src/gui/plugins/component_inspector/AirPressure.qml new file mode 100644 index 0000000000..21294ef1aa --- /dev/null +++ b/src/gui/plugins/component_inspector/AirPressure.qml @@ -0,0 +1,134 @@ +/* + * 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 air pressure noise information. +Rectangle { + height: header.height + content.height + width: componentInspector.width + color: index % 2 == 0 ? lightGrey : darkGrey + + Column { + anchors.fill: parent + + // The expanding header. Make sure that the content to expand has an id set + // to the value "content". + ExpandingTypeHeader { + id: header + + // Set the 'expandingHeaderText' value to override the default header + // values, which is based on the model. + expandingHeaderText: "Air pressure" + expandingHeaderToolTip: "Air pressure properties" + } + + // This is the content that will be expanded/contracted using the + // ExpandingHeader above. Note the "id: content" attribute. + Rectangle { + id: content + property bool show: false + width: parent.width + height: show ? layout.height : 0 + clip: true + color: "transparent" + Layout.leftMargin: 4 + + Behavior on height { + NumberAnimation { + duration: 200; + easing.type: Easing.InOutQuad + } + } + + ColumnLayout { + id: layout + width: parent.width + + RowLayout { + id: rowLayout + width: layout.width + + Text { + leftPadding: 10 + id: referenceAltitudeText + text: "Reference altitude (m)" + color: "dimgrey" + font.pointSize: 12 + } + StateAwareSpin { + id: referenceAltitudeSpin + Layout.fillWidth: true + height: 40 + numberValue: model.data[0] + minValue: 0 + maxValue: 100000 + stepValue: 0.1 + // Connect to the onNoiseUpdate signal in Noise.qml + Component.onCompleted: { + referenceAltitudeSpin.onChange.connect( + AirPressureImpl.OnAirPressureReferenceAltitude) + } + } + } + + // Space out the section header. + Rectangle { + id: noiseTextRect + width: parent.width + height: childrenRect.height + Layout.leftMargin: 10 + Layout.topMargin: 10 + color: "transparent" + + Text { + text: "Pressure Noise" + color: "dimgrey" + font.pointSize: 12 + } + } + + // Show the pressure noise values. + Noise { + id: pressureNoise + Layout.fillWidth: true + Layout.leftMargin: 20 + + meanValue: model.data[1] + meanBias: model.data[2] + stdDevValue: model.data[3] + stdDevBias: model.data[4] + dynamicBiasStdDev: model.data[5] + dynamicBiasCorrelationTime: model.data[6] + + // Connect to the onNoiseUpdate signal in Noise.qml + Component.onCompleted: { + pressureNoise.onNoiseUpdate.connect( + AirPressureImpl.OnAirPressureNoise) + } + } + } + } + } +} diff --git a/src/gui/plugins/component_inspector/CMakeLists.txt b/src/gui/plugins/component_inspector/CMakeLists.txt index 62200aecb6..0e17f492fd 100644 --- a/src/gui/plugins/component_inspector/CMakeLists.txt +++ b/src/gui/plugins/component_inspector/CMakeLists.txt @@ -1,4 +1,12 @@ gz_add_gui_plugin(ComponentInspector - SOURCES ComponentInspector.cc ModelEditor.cc - QT_HEADERS ComponentInspector.hh ModelEditor.hh + SOURCES + AirPressure.cc + ComponentInspector.cc + ModelEditor.cc + Types.cc + QT_HEADERS + AirPressure.hh + ComponentInspector.hh + ModelEditor.hh + Types.hh ) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 0632c355a6..f30794d833 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -17,7 +17,9 @@ #include #include +#include #include +#include #include #include @@ -68,6 +70,7 @@ #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/gui/GuiEvents.hh" +#include "AirPressure.hh" #include "ComponentInspector.hh" #include "ModelEditor.hh" @@ -102,11 +105,24 @@ namespace ignition::gazebo /// \brief Whether updates are currently paused. public: bool paused{false}; + /// \brief Whether simulation is currently paused. + public: bool simPaused{true}; + /// \brief Transport node for making command requests public: transport::Node node; /// \brief Transport node for making command requests public: ModelEditor modelEditor; + + /// \brief Air pressure sensor inspector elements + public: std::unique_ptr airPressure; + + /// \brief Set of callbacks to execute during the Update function. + public: std::vector< + std::function> updateCallbacks; + + /// \brief A map of component type to creation functions. + public: std::map componentCreators; }; } @@ -413,6 +429,9 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *) "ComponentsModel", &this->dataPtr->componentsModel); this->dataPtr->modelEditor.Load(); + + // Create air pressure + this->dataPtr->airPressure = std::make_unique(this); } ////////////////////////////////////////////////// @@ -421,8 +440,7 @@ void ComponentInspector::Update(const UpdateInfo &_info, { IGN_PROFILE("ComponentInspector::Update"); - if (this->dataPtr->paused) - return; + this->SetSimPaused(_info.paused); auto componentTypes = _ecm.ComponentTypes(this->dataPtr->entity); @@ -776,6 +794,12 @@ void ComponentInspector::Update(const UpdateInfo &_info, if (comp) setData(item, comp->Data()); } + else if (this->dataPtr->componentCreators.find(typeId) != + this->dataPtr->componentCreators.end()) + { + this->dataPtr->componentCreators[typeId]( + _ecm, this->dataPtr->entity, item); + } } // Remove components no longer present - list items to remove @@ -799,6 +823,24 @@ void ComponentInspector::Update(const UpdateInfo &_info, } this->dataPtr->modelEditor.Update(_info, _ecm); + + // Process all of the update callbacks + for (auto cb : this->dataPtr->updateCallbacks) + cb(_ecm); + this->dataPtr->updateCallbacks.clear(); +} + +///////////////////////////////////////////////// +void ComponentInspector::AddUpdateCallback(UpdateCallback _cb) +{ + this->dataPtr->updateCallbacks.push_back(_cb); +} + +///////////////////////////////////////////////// +void ComponentInspector::RegisterComponentCreator(ComponentTypeId _id, + ComponentCreator _creatorFn) +{ + this->dataPtr->componentCreators[_id] = _creatorFn; } ///////////////////////////////////////////////// @@ -877,6 +919,22 @@ void ComponentInspector::SetLocked(bool _locked) this->LockedChanged(); } +///////////////////////////////////////////////// +bool ComponentInspector::SimPaused() const +{ + return this->dataPtr->simPaused; +} + +///////////////////////////////////////////////// +void ComponentInspector::SetSimPaused(bool _paused) +{ + if (_paused != this->dataPtr->simPaused) + { + this->dataPtr->simPaused = _paused; + this->SimPausedChanged(); + } +} + ///////////////////////////////////////////////// bool ComponentInspector::Paused() const { diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 6661bce09e..7f756de233 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -33,6 +33,7 @@ #include +#include "Types.hh" Q_DECLARE_METATYPE(ignition::gazebo::ComponentTypeId) namespace ignition @@ -200,6 +201,13 @@ namespace gazebo NOTIFY PausedChanged ) + /// \brief Simulation paused + Q_PROPERTY( + bool simPaused + READ SimPaused + NOTIFY SimPausedChanged + ) + /// \brief Nested Model Q_PROPERTY( bool nestedModel @@ -319,6 +327,17 @@ namespace gazebo /// \brief Notify that locked has changed. signals: void LockedChanged(); + /// \brief Get whether simulation is currently paused. + /// \return True for paused. + public: Q_INVOKABLE bool SimPaused() const; + + /// \brief Notify that simulation paused state has changed. + signals: void SimPausedChanged(); + + /// \brief Set whether simulation is currently paused. + /// \param[in] _paused True for paused. + public: void SetSimPaused(bool _paused); + /// \brief Get whether the inspector is currently paused for updates. /// \return True for paused. public: Q_INVOKABLE bool Paused() const; @@ -343,6 +362,21 @@ namespace gazebo public: Q_INVOKABLE void OnLoadMesh(const QString &_entity, const QString &_type, const QString &_mesh); + + /// \brief Add a callback that will be executed during the next Update. + /// \param[in] _cb The callback to run. + public: void AddUpdateCallback(UpdateCallback _cb); + + /// \brief Register a component creator. A component creator is + /// responsible for selecting the correct QML and setting the + /// appropriate data for a ComponentTypeId. + /// \param[in] _id The component type id to associate with the creation + /// function. + /// \param[in] _creatorFn Function to call in order to create the QML + /// component. + public: void RegisterComponentCreator(ComponentTypeId _id, + ComponentCreator _creatorFn); + /// \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 e433714fe1..4d6d8b41b4 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -80,13 +80,37 @@ Rectangle { return _model.dataType + '.qml' } - // Get number of decimal digits based on a widget's width + /// \brief Get whether simulation is paused + function getSimPaused() { + return ComponentInspector.simPaused + } + + // Get number of decimal digits based on a width value + // \param[in] _width Pixel width + // \return Number of decimals that fit with the provided width. function getDecimals(_width) { + // Use full decimals if the width is <= 0, which allows the value + // to appear correctly. + if (_width <= 0 || _width > 110) + return 6 + if (_width <= 80) - return 2; - else if (_width <= 100) - return 4; - return 6; + return 2 + + return 4 + } + + // Get number of decimal digits based on a widget's width, and adjust the + // widget's value to prevent zero padding. + // \param[in, out] _widgetId The widget id that will display the value. This + // widget's width attribute is used to determine the number of decimals. + // \param[in] _value The value that should be used to set the _widgetId's + // value attribute. + function getDecimalsAdjustValue(_widgetId, _value) { + // Make sure to update the value, otherwise zeros are used intead of + // the actual values. + _widgetId.value = _widgetId.activeFocus ? _widgetId.value : _value + return getDecimals(_widgetId.width) } /** @@ -231,7 +255,20 @@ Rectangle { ToolTip.visible: hovered ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval onClicked: { - addLinkMenu.open() + getSimPaused() ? addLinkMenu.open() : pausePopup.open() + } + Popup { + id: pausePopup + modal: true + focus: true + x: parent.width - popupContentText.width + y: parent.height + popupContentText.height + contentItem: Text { + id: popupContentText + padding: 10 + text: "Pause simulation to add an entity" + } + closePolicy: Popup.CloseOnEscape | Popup.CloseOnPressOutsideParent } FileDialog { diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qrc b/src/gui/plugins/component_inspector/ComponentInspector.qrc index fde9cbbba7..7b83df0784 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qrc +++ b/src/gui/plugins/component_inspector/ComponentInspector.qrc @@ -1,16 +1,19 @@ + AirPressure.qml Boolean.qml ComponentInspector.qml ExpandingTypeHeader.qml Light.qml NoData.qml + Noise.qml Physics.qml Pose3d.qml SphericalCoordinates.qml String.qml TypeHeader.qml Vector3d.qml + StateAwareSpin.qml plottable_icon.svg diff --git a/src/gui/plugins/component_inspector/ExpandingTypeHeader.qml b/src/gui/plugins/component_inspector/ExpandingTypeHeader.qml index c233ecf775..7f8126801d 100644 --- a/src/gui/plugins/component_inspector/ExpandingTypeHeader.qml +++ b/src/gui/plugins/component_inspector/ExpandingTypeHeader.qml @@ -34,6 +34,9 @@ Rectangle { // Left indentation property int indentation: 10 + property string expandingHeaderText: "" + property string expandingHeaderToolTip: "" + RowLayout { anchors.fill: parent Item { @@ -50,6 +53,8 @@ Rectangle { } TypeHeader { id: typeHeader + headerText: expandingHeaderText + headerToolTip: expandingHeaderToolTip } Item { Layout.fillWidth: true diff --git a/src/gui/plugins/component_inspector/Noise.qml b/src/gui/plugins/component_inspector/Noise.qml new file mode 100644 index 0000000000..f2a39d74de --- /dev/null +++ b/src/gui/plugins/component_inspector/Noise.qml @@ -0,0 +1,424 @@ +/* + * 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 noise information. +Rectangle { + id: noise + height: noiseContent.height + color: "transparent" + + // Mean value + property double meanValue: 0.0 + + // Mean bias + property double meanBias: 0.0 + + // Standard deviation value + property double stdDevValue: 0.0 + + // Standard deviation bias + property double stdDevBias: 0.0 + + // Dynamic bias standard deviation + property double dynamicBiasStdDev: 0.0 + + // Dynamic bias correlation time + property double dynamicBiasCorrelationTime: 0.0 + + // Signal that a user of this component can connect to in order to receive + // noise updates + signal onNoiseUpdate(double _mean, double _meanBias, double _stdDev, + double _stdDevBias, double _dynamicBiasStdDev, + double _dynamicBiasCorrelationTime) + + function onMean(_value) { + meanValue = _value; + onNoiseUpdate(meanValue, meanBias, stdDevValue, stdDevBias, + dynamicBiasStdDev, dynamicBiasCorrelationTime); + } + + function onMeanBias(_value) { + meanBias = _value; + onNoiseUpdate(meanValue, meanBias, stdDevValue, stdDevBias, + dynamicBiasStdDev, dynamicBiasCorrelationTime); + } + + function onStdDev(_value) { + stdDevValue = _value; + onNoiseUpdate(meanValue, meanBias, stdDevValue, stdDevBias, + dynamicBiasStdDev, dynamicBiasCorrelationTime); + } + + function onStdDevBias(_value) { + stdDevBias = _value; + onNoiseUpdate(meanValue, meanBias, stdDevValue, stdDevBias, + dynamicBiasStdDev, dynamicBiasCorrelationTime); + } + + function onDynamicBiasStdDev(_value) { + dynamicBiasStdDev = _value; + onNoiseUpdate(meanValue, meanBias, stdDevValue, stdDevBias, + dynamicBiasStdDev, dynamicBiasCorrelationTime); + } + + function onDynamicBiasCorrelationTime(_value) { + dynamicBiasCorrelationTime = _value; + onNoiseUpdate(meanValue, meanBias, stdDevValue, stdDevBias, + dynamicBiasStdDev, dynamicBiasCorrelationTime); + } + + // Display the main content + Column { + anchors.fill: parent + + // Content + Rectangle { + id: noiseContent + width: parent.width + height: grid.height + clip: true + color: "transparent" + + Behavior on height { + NumberAnimation { + duration: 200; + easing.type: Easing.InOutQuad + } + } + + GridLayout { + id: grid + width: parent.width + columns: 4 + + // Padding + Rectangle { + Layout.columnSpan: 4 + height: 4 + } + + // Mean text label + Text { + Layout.columnSpan: 4 + text: "Mean" + color: "dimgrey" + font.pointSize: 10 + font.bold: true + } + + // Mean + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: meanText.width + + Text { + id : meanText + text: 'Value' + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + ToolTip { + visible: meanMa.containsMouse + delay: tooltipDelay + text: "Mean value" + y: noise.y - 30 + enter: null + exit: null + } + MouseArea { + id: meanMa + anchors.fill: parent + hoverEnabled: true + acceptedButtons: Qt.RightButton + } + } + } + StateAwareSpin { + id: meanSpin + Layout.fillWidth: true + height: 40 + numberValue: meanValue + minValue: -100000 + maxValue: 100000 + stepValue: 0.1 + // Connect to the onNoiseUpdate signal in Noise.qml + Component.onCompleted: { + meanSpin.onChange.connect(onMean) + } + } + // End of mean + + // Mean Bias + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: meanBiasText.width + + Text { + id : meanBiasText + text: 'Bias' + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + ToolTip { + visible: meanBiasMa.containsMouse + delay: tooltipDelay + text: "Mean bias" + y: noise.y - 30 + enter: null + exit: null + } + MouseArea { + id: meanBiasMa + anchors.fill: parent + hoverEnabled: true + acceptedButtons: Qt.RightButton + } + } + } + StateAwareSpin { + id: meanBiasSpin + Layout.fillWidth: true + height: 40 + numberValue: meanBias + minValue: -100000 + maxValue: 100000 + stepValue: 0.1 + // Connect to the onNoiseUpdate signal in Noise.qml + Component.onCompleted: { + meanBiasSpin.onChange.connect(onMeanBias) + } + } + // End of mean bias + + // Padding + Rectangle { + Layout.columnSpan: 4 + height: 4 + } + + // Std Dev text label + Text { + Layout.columnSpan: 4 + text: "Standard deviation" + color: "dimgrey" + font.pointSize: 10 + font.bold: true + } + + // Std Dev + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: stddevText.width + + Text { + id : stddevText + text: 'Value' + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + ToolTip { + visible: stdDevMa.containsMouse + delay: tooltipDelay + text: "Standard deviation value" + y: noise.y - 30 + enter: null + exit: null + } + MouseArea { + id: stdDevMa + anchors.fill: parent + hoverEnabled: true + acceptedButtons: Qt.RightButton + } + } + } + StateAwareSpin { + id: stddevSpin + Layout.fillWidth: true + height: 40 + numberValue: stdDevValue + minValue: 0 + maxValue: 100000 + stepValue: 0.1 + // Connect to the onNoiseUpdate signal in Noise.qml + Component.onCompleted: { + stddevSpin.onChange.connect(onStdDev) + } + } + // End of stddev + + // Std Dev bias + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: stddevBiasText.width + + Text { + id : stddevBiasText + text: 'Bias' + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + ToolTip { + visible: stddevBiasTextMa.containsMouse + delay: tooltipDelay + text: "Standard deviation bias" + y: noise.y - 30 + enter: null + exit: null + } + MouseArea { + id: stddevBiasTextMa + anchors.fill: parent + hoverEnabled: true + acceptedButtons: Qt.RightButton + } + } + } + StateAwareSpin { + id: stddevBiasSpin + Layout.fillWidth: true + height: 40 + numberValue: stdDevBias + minValue: 0 + maxValue: 100000 + stepValue: 0.1 + // Connect to the onNoiseUpdate signal in Noise.qml + Component.onCompleted: { + stddevBiasSpin.onChange.connect(onStdDevBias) + } + } + // End of stddev bias + + // Padding + Rectangle { + Layout.columnSpan: 4 + height: 4 + } + + // Dynamic bias text label + Text { + Layout.columnSpan: 4 + text: "Dynamic bias" + color: "dimgrey" + font.pointSize: 10 + font.bold: true + } + + // Dynamic bias std Dev + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: dynamicBiasStddevText.width + + Text { + id : dynamicBiasStddevText + text: 'Std. Dev.' + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + ToolTip { + visible: dynamicBiasStddevTextMa.containsMouse + delay: tooltipDelay + text: "Standard deviation" + y: noise.y - 30 + enter: null + exit: null + } + MouseArea { + id: dynamicBiasStddevTextMa + anchors.fill: parent + hoverEnabled: true + acceptedButtons: Qt.RightButton + } + + } + } + StateAwareSpin { + id: dynamicBiasStdDevSpin + Layout.fillWidth: true + height: 40 + numberValue: dynamicBiasStdDev + minValue: 0 + maxValue: 100000 + stepValue: 0.1 + // Connect to the onNoiseUpdate signal in Noise.qml + Component.onCompleted: { + dynamicBiasStdDevSpin.onChange.connect(onDynamicBiasStdDev) + } + } + // End of dynamic bias stddev + + // Dynamic bias correlation time + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: dynamicBiasCorrelationTimeText.width + + Text { + id : dynamicBiasCorrelationTimeText + text: 'Cor. Time (s)' + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + + ToolTip { + visible: dynamicBiasCorrelationTimeTextMa.containsMouse + delay: tooltipDelay + text: "Correlation time in seconds." + y: noise.y - 30 + enter: null + exit: null + } + MouseArea { + id: dynamicBiasCorrelationTimeTextMa + anchors.fill: parent + hoverEnabled: true + acceptedButtons: Qt.RightButton + } + } + } + StateAwareSpin { + id: dynamicBiasCorrelationTimeSpin + Layout.fillWidth: true + height: 40 + numberValue: dynamicBiasCorrelationTime + minValue: 0 + maxValue: 100000 + stepValue: 0.1 + // Connect to the onNoiseUpdate signal in Noise.qml + Component.onCompleted: { + dynamicBiasCorrelationTimeSpin.onChange.connect(onDynamicBiasCorrelationTime) + } + } + // End of dynamic bias correlation time stddev bias + } + } + } +} diff --git a/src/gui/plugins/component_inspector/Pose3d.qml b/src/gui/plugins/component_inspector/Pose3d.qml index e9806463e4..45784f9ac9 100644 --- a/src/gui/plugins/component_inspector/Pose3d.qml +++ b/src/gui/plugins/component_inspector/Pose3d.qml @@ -93,7 +93,7 @@ Rectangle { value: writableSpin.activeFocus ? writableSpin.value : numberValue minimumValue: -spinMax maximumValue: spinMax - decimals: getDecimals(writableSpin.width) + decimals: getDecimalsAdjustValue(writableSpin, numberValue) onEditingFinished: { sendPose() } diff --git a/src/gui/plugins/component_inspector/StateAwareSpin.qml b/src/gui/plugins/component_inspector/StateAwareSpin.qml new file mode 100644 index 0000000000..33db4d3e73 --- /dev/null +++ b/src/gui/plugins/component_inspector/StateAwareSpin.qml @@ -0,0 +1,109 @@ +/* + * 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" + +Rectangle { + id: stateAwareSpin + height: stateAwareSpinContent.height + color: "transparent" + + // step size + property double stepValue: 0.1 + + // min value + property double minValue: 0 + + // max value + property double maxValue: 1 + + // value of the spin number value + property double numberValue: 0.0 + + signal onChange(double _value) + + /** + * Used to create a spin box + */ + Component { + id: writableNumber + IgnSpinBox { + id: writableSpin + value: writableSpin.activeFocus ? writableSpin.value : numberValue + minimumValue: minValue + maximumValue: maxValue + stepSize: stepValue + decimals: { + writableSpin.value = writableSpin.activeFocus ? writableSpin.value : numberValue + componentInspector.getDecimals(writableSpin.width) + } + onEditingFinished: { + numberValue = writableSpin.value + onChange(writableSpin.value) + } + } + } + + /** + * Used to create a read-only number + */ + Component { + id: readOnlyNumber + Rectangle { + border.width: 1 + color: index % 2 == 0 ? lightGrey : darkGrey + implicitHeight: 40 + Text { + padding: 10 + id: numberText + anchors.fill: parent + horizontalAlignment: Text.AlignRight + verticalAlignment: Text.AlignVCenter + text: { + return numberValue.toFixed(componentInspector.getDecimals(numberText.width)) + } + } + } + } + + Column { + anchors.fill: parent + + // Content + Rectangle { + id: stateAwareSpinContent + width: parent.width + height: stateAwareSpinContentLoader.height + clip: true + color: "transparent" + + Loader { + id: stateAwareSpinContentLoader + width: parent.width + Layout.fillWidth: true + sourceComponent: componentInspector.getSimPaused() ? writableNumber : readOnlyNumber + } + } + } +} diff --git a/src/gui/plugins/component_inspector/TypeHeader.qml b/src/gui/plugins/component_inspector/TypeHeader.qml index 5cb0b39e2b..80eb283cc3 100644 --- a/src/gui/plugins/component_inspector/TypeHeader.qml +++ b/src/gui/plugins/component_inspector/TypeHeader.qml @@ -23,11 +23,17 @@ import QtQuick.Controls.Styles 1.4 Rectangle { id: typeHeader - height: headerText.height - width: headerText.width + height: headerTextId.height + width: headerTextId.width color: "transparent" + property string headerText: "" + property string headerToolTip: "" function tooltipText(_model) { + if (headerToolTip !== undefined && headerToolTip !== "" ) { + return headerToolTip + } + if (model === null) return "Unknown component" @@ -42,8 +48,14 @@ Rectangle { } Text { - id: headerText - text: model && model.shortName ? model.shortName : '' + id: headerTextId + text: { + if (headerText === undefined || headerText === "") { + model && model.shortName ? model.shortName : '' + } else { + headerText + } + } color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" font.pointSize: 12 diff --git a/src/gui/plugins/component_inspector/Types.cc b/src/gui/plugins/component_inspector/Types.cc new file mode 100644 index 0000000000..fdef409818 --- /dev/null +++ b/src/gui/plugins/component_inspector/Types.cc @@ -0,0 +1,33 @@ +/* + * 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 "Types.hh" + +void ignition::gazebo::setNoise(sdf::Noise &_noise, + double _mean, double _meanBias, double _stdDev, + double _stdDevBias, double _dynamicBiasStdDev, + double _dynamicBiasCorrelationTime) +{ + _noise.SetMean(_mean); + _noise.SetBiasMean(_meanBias); + + _noise.SetStdDev(_stdDev); + _noise.SetBiasStdDev(_stdDevBias); + + _noise.SetDynamicBiasStdDev(_dynamicBiasStdDev); + _noise.SetDynamicBiasCorrelationTime(_dynamicBiasCorrelationTime); +} diff --git a/src/gui/plugins/component_inspector/Types.hh b/src/gui/plugins/component_inspector/Types.hh new file mode 100644 index 0000000000..73b056af30 --- /dev/null +++ b/src/gui/plugins/component_inspector/Types.hh @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_TYPES_HH_ +#define IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_TYPES_HH_ + +#include + +#include + +#include +#include + +namespace ignition +{ +namespace gazebo +{ + /// \brief UpdateCallback is a function defition that is used by a + /// component to manage ECM changes. + /// \sa void ComponentInspector::AddUpdateCallback(UpdateCallback _cb) + using UpdateCallback = std::function; + + // ComponentCreator is a function definition that a component can use + // to create the appropriate UI elements for an Entity based on + // a ComponentTypeId. + /// \sa void ComponentInspector::RegisterComponentCreator(UpdateCallback _cb) + using ComponentCreator = std::function; + + /// \brief Helper function that will set all noise properties. + /// \param[out] _noise Noise to set + /// \param[in] _mean Mean value + /// \param[in] _meanBias Bias mean value + /// \param[in] _stdDev Standard deviation value + /// \param[in] _stdDevBias Bias standard deviation value + /// \param[in] _dynamicBiasStdDev Dynamic bias standard deviation value + /// \param[in] _dynamicBiasCorrelationTime Dynamic bias correlation time + /// value + void setNoise(sdf::Noise &_noise, + double _mean, double _meanBias, double _stdDev, + double _stdDevBias, double _dynamicBiasStdDev, + double _dynamicBiasCorrelationTime); +} +} +#endif diff --git a/src/gui/plugins/component_inspector/Vector3d.qml b/src/gui/plugins/component_inspector/Vector3d.qml index 1cc4f42ca6..61e2dadc07 100644 --- a/src/gui/plugins/component_inspector/Vector3d.qml +++ b/src/gui/plugins/component_inspector/Vector3d.qml @@ -58,7 +58,7 @@ Rectangle { value: numberValue minimumValue: -spinMax maximumValue: spinMax - decimals: getDecimals(writableSpin.width) + decimals: getDecimalsAdjustValue(writableSpin, numberValue) } } diff --git a/src/gui/plugins/modules/TypeIcon.qml b/src/gui/plugins/modules/TypeIcon.qml index 4e8c1a308e..16820d0461 100644 --- a/src/gui/plugins/modules/TypeIcon.qml +++ b/src/gui/plugins/modules/TypeIcon.qml @@ -44,6 +44,7 @@ Image { entityType == 'joint' || entityType == 'light' || entityType == 'link' || + entityType == 'sensor' || entityType == 'model') { entityImage = entityType diff --git a/src/gui/resources/gazebo.qrc b/src/gui/resources/gazebo.qrc index 44c2823540..0845b98594 100644 --- a/src/gui/resources/gazebo.qrc +++ b/src/gui/resources/gazebo.qrc @@ -10,6 +10,7 @@ images/minus.png images/model.png images/plus.png + images/sensor.png images/unlock.svg images/visual.png diff --git a/src/gui/resources/images/sensor.png b/src/gui/resources/images/sensor.png new file mode 100644 index 0000000000000000000000000000000000000000..eeb2ff65c7484da760b1febda756ca0c46dbaf60 GIT binary patch literal 7013 zcmch5c~Dc?w)Z}V1VSPbFbsl>Dh+~wB7!m`%%~tJ4v0dcazUgK5l|#Zf--b7YCECG zpeSmK45ETaNE8vJWfCOF6b6-1!Wge_G7;O{on*ceaEhjedk)9F20(3ko1@*HsLt;_cBj<0Jdy7E7%%VVo zl{0V8YU*ONfxe_%#nkhqmkLY-BnHr-0LqjM5ypT-A6GDMjA2GrG5~!g%sE1EJ}~OQ`}3e$q^#C3mD4fyO_*{2dl$EL7{DL&K7p zWfXq=GuJ7`($G*1cl05Zgw%1}hAX$lXmX_D#IgYLox;~5le#Lfr>O3b#bwEfJW)*dg>V0gg~!)$MX{INZ5 zXd?SYBrXe!9{TI%_RnqQ@BjIhF+V}J@>NH?_71Rm&bep7I8ceG^^j@2jaVX3T@!a2 z>7Y)#NK%mlPVKB-vA%Zy(Ku9-vo$$gQWN{0D1D%2M2Gg#1`ladC*KAA-oj3kva9}} zsjPW&l@t!#`y-;Q;P9;cjvnDI7w8{?BM?PE4Z3W)lyu90@;M)%=a(WNy9kOGYNB;G zq#q-cJ^wGZ{_+nQX6YVV;eOHKRP$Y@@J!%&=V(!CnmB6MhukyJ1P)h$MfdxMof8TM`B2olv38 z%7+&sPxG1UynyXo?GDqO+(V^8kAotCUvb*|CCw?@MaiH+NJFeJdNh)C|N2mc-rd1S z93aSkziD_H*`~e}$fZZ0mGF^X=h9#MwH9q<*JrZ*QcjF*Hf@VwrQ5H9QajB(Vm12n zkx;6~Q0~c!z}A=<#?bBCU)$#4rJ>q zo|9yuAOMf)nYfwZRZ$gTi4D^|JhjEPm~V8ou(bK#&(23cv=gnrwDzgT z&bJ_@{LkqbX6c&JMZO2$vvh|$U%PZ(W(O85gE<%LZA4tFPy5$PE}s^bd0-2cL9epI zuV!F$LF$du1Z$DXi{ExABXEyo;vuD$NW+`@XNE1)21Z`~f1XD6zLflf>s@;k@&dx! zfa59aCm{RxQ_mj#Gs?&_8sE1S1Ef8sS7tq!KyHtDvnCm87&j;}3yyfHM{QmWjLME( zPz=4wGpR7TxwkUb_C@F~xzdh_H`ehUX^9PVC0|pRejJKLF3>{?6u6U1 zp-&}=tli{C^y>LLT+tievf+ae7C-^7C&*4I?#2%msOL<4LO-&r6O2dwf|tx+-XK#} zOs4&@tKS(?0;7K}t<|hV+UO6tJY>Q)dBN_cJZzUS7PA6AKVjj;v_9;mMqWOf6$mya zH+&gH>U)s>;4x7iZ{+LZ_fX?1iHGxom9U&A@=fFVKQfzpI_W!nfO&%J)h}?sfNMC@ zPcIP`w>{_!;&X_nB-wRH7Jf|l*iXDT8=cTy6UQxJwh!QF1rs05Q|TcZ$w(G^sLVY! zKD5Ld;Wwt^d{}zxa|jPagb1ag1`~|5Oj}6Ttt3a=!cSo$t^4%Vx3sySbMP7*WT#K&e!uyKkkZ?axLV8wx@c`Aj>ghk$5b=^ySOW zKDMB>i2P`nw`75c2EX$n7AE|ACN1E4Y_uqbT+@q-}*2cDBQVU|9t9|I1`T3l$ zTngflK+Yc~t4TK&_s@|nmccF=QoEfS5^NP++2b z&aft8)FI}`$3+tFI*&F}y=9s`zVxWVT<%!Rni2uaz#Ka%cLoPiBAWJAS4Ul&V80q> z$8Z@Z)lOEFOjzShv8rQR-8P`N{*0^d%xDi|&s6b=H$M)0dt~Mj98lVAIk;tdt#(5S zk5&~e(b?hUavLV+JjNuU zN|v_r(0an>J}tudbLE)FMU%RKR2{~WoZDO@B`-N=xfIANi0RX(ai!woya4^8nl>Pr z&)`_~3*>2?jYE>O`{1zy)4VU#1>nqQZXCHf)2z|MxXjow#aD!Hx+!%FC8sgWkv*!v z$j8bhRU!uyQpGxXcKadefr-&8-@|Wg{B2eP3ja>`!wYBk zuSu13c|gX!Sr>D|H=lO~G;`mVIj`6?eJwk`eaj#iNWMF8oDRp%+Gt7+bJBxrY9esp zmD8~sjW5D4X}l0J1pJEfBkkWW3Bx%C6t4NMm% zCE=FVlJcp0JuW>NknYRo-}$TS3}NsnvdJ)1F-O#t3zO6o=*F@Ta|)jY@`WawvED_@ ztvg%*h55OY6xxG`yZ{v+4@yoCfktE2P|1;yh1^-vuE@gQ$S>6T@PpuF?JEu=1Z?xx{Bufnj)77(r1_|oAE*9Y?=UpCnAS@!-W zO2Ct%(rxssWY7butaicTFVR&Y5Y#m`$eLP0p}`Mi@xuDnZvd1i%NOYWU!VpP^sNOb z5ClY2;NM78+sN0?DNI!TE}xuJh5L!AD1j&hsKWVa@zc065HrDAJkzSTUp)8lOB4AB zFCWIX*`xyP5P~T?s_*zInZ|lSl>q;MwFhYb<8mg;TTUP-k8U2_{>%0lL2k5sb7UI9 z4`%MHlppst2hF*5Byw7Nd2x$R2RMU7YCOdH?I!aXgVLmML1> zQ)aUr(7GGrM1qcQDhFwWCUkw6quJPH8ST*>li?1%u0$pV86VFYZv(?zCt^-jjq;&4 zb&xysZA>B~TA*^xZ>Q`-b4YaUwj+e_&Bt@#1xLyu0W$V1eN<9Ya8`qx`1=|djLBxg z-6BUOvAJlWe5splf0K&i1%w@9aIER595Hd}67uOPTMmkKCc<4@+ZJs+%z9i_rh z&nrnLp6%u^#m+24;ZZ6jUsJlu97}}fRgh~vNb(>{#Hzvjmcc{Zp6GG*kd0x7*a|VJ6c9i# zuVRz&TS)zF1F>XaQ(x7jg|WoN8QHqi9IgFRi^89h4t`Ok37i>|`h7K%rMqzzQ-ikd zDM)&g$VHhkh-{F=VwhosM#{*uFB>>HX?p80dzQiF&XOCo7;UW0 z48S)ZtD&!e_gN|~9Q(q1B9+_M&E77^0G^3Sc5cue{*C*L*!EoM?$2{ttn!;~{t?K0 z3`DcLln^*S1d`W(=#j^4w!|;YhF08?{y@SQ6BtRCke5Oy0d>$vm!~2hM*yv7np-Tw zny{TEB|FvOfaS1&B#BFA)L&rOaDyWrgh|LO6F*qc%PU`iTVh+i{*BMP;9U@j9?#sEuEA6t~BTAE8XKM~2fx?F8A-@FbASafw2k*)IF&kkLE@}wYjpU7+CQ7`@1zGsmybMk_5XUTdG&+36o zL54@-{rc4u)1l?mgM-^RpMp3pwSsm9y9~j%#)--~=U&~IeK~*Zt=R1K!5gON+^z%k zw_jINJyL?lh_JCmZB`SmyX(f8aa;O{zN8e!$Du(7n9=uaWiEHX3#_y_lYv#Qm`Wmj zy{q7h{)o4*tk1m2?LEifJo8xY*i)n8F(;@n>8(nC9DBvU=UEc6H66jKu3%Oh54g@|>{!I!I_Y%Yima2AkPvHkRc>9$veA|p!K$<`6qzOuBL@;6{mc;I{Nb@3}wpC{4#befWZ=;})b zT8)>a(cCLW8HPSS`EME5p5C8P%B z9(RqJZ0apnb^&Mf-apim{!o#=enG$Q)GHoc^TY*jt8V$&+$&2fkIEym!0QA_NmJS_ zN?BXCO@I7KmB+@WdaG(ZO^JHSYG$o6BVyxnGM=eBMn&%m)87>3rHnVejsJY_@eR0# zJ;es5jDDj{*k&xJBlzwiK`6Q^=#RlqH65mX7|7kc#1#{Okty~h2{^2?_&G{_ph0Hb#C{@$7h1O}HwR;9z* z0dm(h#0P6&&N^K}fIxRdA8hB1?aD{qjhRVQz^OH@L4og}LI2z8wV==xTjAEZ z*6W#q?%uV%T}m!UB?ID+@>9zt0tMiAFXnk2ka>S$Oo9b}p8iW&?U>s}fp!lY&`QtcyfHMg zCajWsjX`#P3*UMZ1rnq_-MIZ*@gyt zk7*}rAEJe-RxPL?mid5lCHqu|$>?0Qt!(Z9y=J_Y{iSXt76h0aIZP;Xz+If&fK_E% zs-fGI>er_GHbTyU8Q*lg;Z?-g+hHo8-7(;-(V~`r`u4`K0f+BUe-v7$Wggqk*Yv)--6RE)kuUUb+cC^k|12dKNcWmqFZ=?oE^IimcWRhEMe!~=Q1proC|k(Y+9wm(Os z`Ady@wGZc<1v*M&(H@0WD)#HIfaF?}v2l?B%@A7T0ra$n#SHEp-0R#`ZKz1tc4P z`wJ^J9-%iu^d!(H_n4ix{t&14)uK>T`^fVOme66fz)V70ZfMnw0pM?uf2-F2y>|ak zqrdAC)%!ni!hiDhFNMFA<-bDxm!bc6m;aRf|MuLc>CyE(yJ{IIxrRukr~YEF{9(B{ Qx>sPE(@w`Cd)oQ`0+WQe<^TWy literal 0 HcmV?d00001 diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index c299ca50f0..85dd9c68fd 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -19,8 +19,10 @@ #include +#include #include #include +#include #include #include @@ -199,9 +201,14 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate public: std::chrono::time_point lastStatePubTime{std::chrono::system_clock::now()}; - /// \brief Period to publish state, defaults to 60 Hz. - public: std::chrono::duration> - statePublishPeriod{std::chrono::milliseconds(1000/60)}; + /// \brief Period to publish state while paused and running. The key for + /// the map is used to store the publish period when paused and not + /// paused. The not-paused (a.ka. running) period has a key=false and a + /// default update rate of 60Hz. The paused period has a key=true and a + /// default update rate of 30Hz. + public: std::map>> + statePublishPeriod{{false, std::chrono::milliseconds(1000/60)}, + {true, std::chrono::milliseconds(1000/30)}}; /// \brief Flag used to indicate if the state service was called. public: bool stateServiceRequest{false}; @@ -237,10 +244,16 @@ void SceneBroadcaster::Configure( this->dataPtr->dyPoseHertz = readHertz.first; auto stateHerz = _sdf->Get("state_hertz", 60); - this->dataPtr->statePublishPeriod = + this->dataPtr->statePublishPeriod[false] = std::chrono::duration>( std::chrono::milliseconds(1000/stateHerz.first)); + // Set the paused update rate to half of the running update rate. + this->dataPtr->statePublishPeriod[true] = + std::chrono::duration>( + std::chrono::milliseconds(1000 / + static_cast(std::max(stateHerz.first * 0.5, 1.0)))); + // Add to graph { std::lock_guard lock(this->dataPtr->graphMutex); @@ -286,8 +299,8 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, _manager.HasNewEntities() || _manager.HasOneTimeComponentChanges() || jumpBackInTime; auto now = std::chrono::system_clock::now(); - bool itsPubTime = !_info.paused && (now - this->dataPtr->lastStatePubTime > - this->dataPtr->statePublishPeriod); + bool itsPubTime = (now - this->dataPtr->lastStatePubTime > + this->dataPtr->statePublishPeriod[_info.paused]); auto shouldPublish = this->dataPtr->statePub.HasConnections() && (changeEvent || itsPubTime);