From f83498732b104ff04e254474a6b5e241e80dd80b Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Mon, 27 Sep 2021 17:20:42 -0300 Subject: [PATCH 01/22] Enable new policy to fix protobuf compilation errors (#1059) Signed-off-by: Jorge Perez --- CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 02f4e5f197..0bc4b5193f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,6 +38,9 @@ endif() # Search for project-specific dependencies #============================================================================ +# Setting this policy enables using the protobuf_MODULE_COMPATIBLE +# set command in CMake versions older than 13.13 +set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) # This option is needed to use the PROTOBUF_GENERATE_CPP # in case protobuf is found with the CMake config files # It needs to be set before any find_package(...) call From 4f3dd67eebc0b24cc621fbcd4804916cda743a79 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 30 Sep 2021 15:35:47 -0700 Subject: [PATCH 02/22] Parse new param for enabling / disabling IMU orientation output (#899) * parse imu orientation param Signed-off-by: Ian Chen * use enable_orientation sdf elem Signed-off-by: Ian Chen * use joinPaths Signed-off-by: Ian Chen * add test world file Signed-off-by: Ian Chen Co-authored-by: Louise Poubel Co-authored-by: Nate Koenig --- src/systems/imu/Imu.cc | 7 +++ test/integration/imu_system.cc | 40 ++++++++++++++ test/worlds/imu_no_orientation.sdf | 83 ++++++++++++++++++++++++++++++ 3 files changed, 130 insertions(+) create mode 100644 test/worlds/imu_no_orientation.sdf diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index 6a645a13ec..1e5f22f443 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -186,6 +186,13 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm) // Set topic _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); + // Set whether orientation is enabled + if (data.ImuSensor()) + { + sensor->SetOrientationEnabled( + data.ImuSensor()->OrientationEnabled()); + } + this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); diff --git a/test/integration/imu_system.cc b/test/integration/imu_system.cc index 68147f4e30..b92c34fd68 100644 --- a/test/integration/imu_system.cc +++ b/test/integration/imu_system.cc @@ -206,3 +206,43 @@ TEST_F(ImuTest, ModelFalling) EXPECT_EQ(imuMsgs.back().entity_name(), scopedName); mutex.unlock(); } + +///////////////////////////////////////////////// +// The test checks to make sure orientation is not published if it is deabled +TEST_F(ImuTest, OrientationDisabled) +{ + imuMsgs.clear(); + + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "imu_no_orientation.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + const std::string sensorName = "imu_sensor"; + + auto topic = + "world/imu_sensor/model/imu_model/link/link/sensor/imu_sensor/imu"; + + // subscribe to imu topic + transport::Node node; + node.Subscribe(topic, &imuCb); + + // step world and verify imu's orientation is not published + // Run server + size_t iters200 = 200u; + server.Run(true, iters200, false); + + // Check we received messages + EXPECT_GT(imuMsgs.size(), 0u); + mutex.lock(); + for (const auto &msg : imuMsgs) + { + EXPECT_FALSE(msg.has_orientation()); + } + mutex.unlock(); +} diff --git a/test/worlds/imu_no_orientation.sdf b/test/worlds/imu_no_orientation.sdf new file mode 100644 index 0000000000..22e17f3663 --- /dev/null +++ b/test/worlds/imu_no_orientation.sdf @@ -0,0 +1,83 @@ + + + + 0 0 -5 + + 0.001 + 1.0 + + + + + + + + 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 + + + + + + + 4 0 3.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + 1 + 30 + true + + false + + + + + + + From d000c9a847afa41b4aea16fc238c784c399dc920 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 4 Oct 2021 09:37:45 +0200 Subject: [PATCH 03/22] Fix light control standalone example (#1077) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- examples/standalone/light_control/light_control.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/standalone/light_control/light_control.cc b/examples/standalone/light_control/light_control.cc index 4f499787c2..8b5cf4118a 100644 --- a/examples/standalone/light_control/light_control.cc +++ b/examples/standalone/light_control/light_control.cc @@ -137,7 +137,7 @@ int main(int argc, char **argv) m = std::sqrt(r*r + b*b + g*g); } r /= m; - b /= m; + g /= m; b /= m; //! [random numbers] From 30c6df003e3b8a62b92c88811da8c534e4be2ba4 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 4 Oct 2021 18:08:34 -0700 Subject: [PATCH 04/22] Fix transform controls (#1081) Signed-off-by: Louise Poubel --- .../transform_control/TransformControl.cc | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index b05f48c6a1..54215f8304 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -385,6 +385,20 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) this->dataPtr->mouseEvent = _e->Mouse(); this->dataPtr->mouseDirty = true; } + else if (_event->type() == ignition::gui::events::MousePressOnScene::kType) + { + auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == ignition::gui::events::DragOnScene::kType) + { + auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } else if (_event->type() == ignition::gui::events::KeyPressOnScene::kType) { ignition::gui::events::KeyPressOnScene *_e = @@ -589,7 +603,8 @@ void TransformControlPrivate::HandleTransform() // start the transform process this->transformControl.SetActiveAxis(axis); this->transformControl.Start(); - if (this->transformControl.Node()){ + if (this->transformControl.Node()) + { try { this->transformControl.Node()->SetUserData( From 1eba508c824f50508ec679a8b80f11e87764065b Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Tue, 5 Oct 2021 10:59:29 -0400 Subject: [PATCH 05/22] Performance: use std::unordered_map where possible in SceneManager (#1083) Signed-off-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> --- src/rendering/SceneManager.cc | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index e791ab8223..a84eaf13ab 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -17,6 +17,7 @@ #include +#include #include #include @@ -58,23 +59,23 @@ class ignition::gazebo::SceneManagerPrivate public: rendering::ScenePtr scene; /// \brief Map of visual entity in Gazebo to visual pointers. - public: std::map visuals; + public: std::unordered_map visuals; /// \brief Map of actor entity in Gazebo to actor pointers. - public: std::map actors; + public: std::unordered_map actors; /// \brief Map of actor entity in Gazebo to actor animations. - public: std::map actorSkeletons; + public: std::unordered_map actorSkeletons; /// \brief Map of actor entity to the associated trajectories. - public: std::map> + public: std::unordered_map> actorTrajectories; /// \brief Map of light entity in Gazebo to light pointers. - public: std::map lights; + public: std::unordered_map lights; /// \brief Map of sensor entity in Gazebo to sensor pointers. - public: std::map sensors; + public: std::unordered_map sensors; }; @@ -637,7 +638,7 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, } unsigned int numAnims = 0; - std::map mapAnimNameId; + std::unordered_map mapAnimNameId; mapAnimNameId[descriptor.meshName] = numAnims++; rendering::VisualPtr actorVisual = this->dataPtr->scene->CreateVisual(name); From 447e5c231ec312bd0ce4e4d349222145e0e135d5 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Tue, 5 Oct 2021 16:16:41 -0400 Subject: [PATCH 06/22] Set a cloned joint's parent/child link names to the cloned parent/child link names (#1075) Signed-off-by: Ashton Larkin --- .../ignition/gazebo/EntityComponentManager.hh | 5 +- src/EntityComponentManager.cc | 142 ++++++++++++++++++ src/EntityComponentManager_TEST.cc | 66 +++++++- 3 files changed, 210 insertions(+), 3 deletions(-) diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 5711cb8b38..a98c2b854b 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -85,8 +85,11 @@ namespace ignition /// canonical link. /// 3. Child entities that are cloned will have their parent set to the /// cloned parent entity. - /// 4. Aside from the changes listed above, all other cloned components + /// 4. Cloned joints with parent/child links will have their parent and + /// child links set to the cloned parent/child links. + /// 5. Aside from the changes listed above, all other cloned components /// remain unchanged. + /// Currently, cloning detachable joints is not supported. /// \param[in] _entity The entity to clone. /// \param[in] _parent The parent of the cloned entity. Set this to /// kNullEntity if the cloned entity should not have a parent. diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 9d4ac0c994..814be6a0ce 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -30,10 +30,14 @@ #include #include "ignition/gazebo/components/CanonicalLink.hh" +#include "ignition/gazebo/components/ChildLinkName.hh" #include "ignition/gazebo/components/Component.hh" #include "ignition/gazebo/components/Factory.hh" +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/ParentLinkName.hh" using namespace ignition; using namespace gazebo; @@ -97,6 +101,22 @@ class ignition::gazebo::EntityComponentManagerPrivate public: bool ComponentMarkedAsRemoved(const Entity _entity, const ComponentTypeId _typeId) const; + /// \brief Set a cloned joint's parent or child link name. + /// \param[in] _joint The cloned joint. + /// \param[in] _originalLink The original joint's parent or child link. + /// \param[in] _ecm Entity component manager. + /// \tparam The component type, which must be either + /// components::ParentLinkName or components::ChildLinkName + /// \return True if _joint's parent or child link name was set. + /// False otherwise + /// \note This method should only be called in EntityComponentManager::Clone. + /// This is a temporary workaround until we find a way to clone entites and + /// components that don't require special treatment for particular component + /// types. + public: template + bool ClonedJointLinkName(Entity _joint, Entity _originalLink, + EntityComponentManager *_ecm); + /// \brief All component types that have ever been created. public: std::unordered_set createdCompTypes; @@ -233,6 +253,24 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \brief See above public: std::unordered_map oldToClonedCanonicalLink; + /// \brief During cloning, we populate two maps: + /// - map of link entities to their cloned link + /// - map of cloned joint entities to the original joint entity's parent and + /// child links + /// After cloning is done, these maps can be used to update the cloned joint + /// entity's parent and child links to the cloned parent and child links. + /// \TODO(anyone) We shouldn't be giving joints special treatment. + /// We should figure out a way to update a joint's parent/child links without + /// having to explicitly search/track for the cloned links. + public: std::unordered_map originalToClonedLink; + + /// \brief See above + /// The key is the cloned joint entity, and the value is a pair where the + /// first element is the original joint's parent link, and the second element + /// is the original joint's child link + public: std::unordered_map> + clonedToOriginalJointLinks; + /// \brief Set of entities that are prevented from removal. public: std::unordered_set pinnedEntities; }; @@ -310,11 +348,15 @@ Entity EntityComponentManager::Clone(Entity _entity, Entity _parent, // Clear maps so they're populated for the entity being cloned this->dataPtr->oldToClonedCanonicalLink.clear(); this->dataPtr->oldModelCanonicalLink.clear(); + this->dataPtr->originalToClonedLink.clear(); + this->dataPtr->clonedToOriginalJointLinks.clear(); auto clonedEntity = this->CloneImpl(_entity, _parent, _name, _allowRename); if (kNullEntity != clonedEntity) { + // make sure that cloned models have their canonical link updated to the + // cloned canonical link for (const auto &[clonedModel, oldCanonicalLink] : this->dataPtr->oldModelCanonicalLink) { @@ -331,6 +373,30 @@ Entity EntityComponentManager::Clone(Entity _entity, Entity _parent, this->SetComponentData(clonedModel, clonedCanonicalLink); } + + // make sure that cloned joints have their parent/child links + // updated to the cloned parent/child links + for (const auto &[clonedJoint, originalJointLinks] : + this->dataPtr->clonedToOriginalJointLinks) + { + auto originalParentLink = originalJointLinks.first; + if (!this->dataPtr->ClonedJointLinkName( + clonedJoint, originalParentLink, this)) + { + ignerr << "Error updating the cloned parent link name for cloned " + << "joint [" << clonedJoint << "]\n"; + continue; + } + + auto originalChildLink = originalJointLinks.second; + if (!this->dataPtr->ClonedJointLinkName( + clonedJoint, originalChildLink, this)) + { + ignerr << "Error updating the cloned child link name for cloned " + << "joint [" << clonedJoint << "]\n"; + continue; + } + } } return clonedEntity; @@ -421,6 +487,48 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent, this->dataPtr->oldToClonedCanonicalLink[_entity] = clonedEntity; } + // keep track of all joints and links that have been cloned so that cloned + // joints can be updated to their cloned parent/child links + if (this->Component(clonedEntity)) + { + // this is a joint, so we need to find the original joint's parent and child + // link entities + Entity originalParentLink = kNullEntity; + Entity originalChildLink = kNullEntity; + + const auto &parentName = + this->Component(_entity); + if (parentName) + { + originalParentLink = this->EntityByComponents( + components::Name(parentName->Data())); + } + + const auto &childName = this->Component(_entity); + if (childName) + { + originalChildLink = this->EntityByComponents( + components::Name(childName->Data())); + } + + if (!originalParentLink || !originalChildLink) + { + ignerr << "The cloned joint entity [" << clonedEntity << "] was unable " + << "to find the original joint entity's parent and/or child link.\n"; + this->RequestRemoveEntity(clonedEntity); + return kNullEntity; + } + + this->dataPtr->clonedToOriginalJointLinks[clonedEntity] = + {originalParentLink, originalChildLink}; + } + else if (this->Component(clonedEntity) || + this->Component(clonedEntity)) + { + // save a mapping between the original link and the cloned link + this->dataPtr->originalToClonedLink[_entity] = clonedEntity; + } + for (const auto &childEntity : this->EntitiesByComponents(components::ParentEntity(_entity))) { @@ -1832,6 +1940,40 @@ bool EntityComponentManagerPrivate::ComponentMarkedAsRemoved( return false; } +///////////////////////////////////////////////// +template +bool EntityComponentManagerPrivate::ClonedJointLinkName(Entity _joint, + Entity _originalLink, EntityComponentManager *_ecm) +{ + if (ComponentTypeT::typeId != components::ParentLinkName::typeId && + ComponentTypeT::typeId != components::ChildLinkName::typeId) + { + ignerr << "Template type is invalid. Must be either " + << "components::ParentLinkName or components::ChildLinkName\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; + } + auto clonedLink = iter->second; + + auto name = _ecm->Component(clonedLink); + if (!name) + { + ignerr << "Link [" << _originalLink << "] was cloned, but its clone has no " + << "name.\n"; + return false; + } + + _ecm->SetComponentData(_joint, name->Data()); + return true; +} + ///////////////////////////////////////////////// void EntityComponentManager::PinEntity(const Entity _entity, bool _recursive) { diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 21d0a2d056..1aaefc0fc6 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -24,9 +24,13 @@ #include #include "ignition/gazebo/components/CanonicalLink.hh" +#include "ignition/gazebo/components/ChildLinkName.hh" #include "ignition/gazebo/components/Factory.hh" +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/Link.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/EntityComponentManager.hh" #include "ignition/gazebo/config.hh" @@ -2790,8 +2794,66 @@ TEST_P(EntityComponentManagerFixture, CloneEntities) EXPECT_EQ(10u, manager.EntityCount()); EXPECT_EQ(kNullEntity, failedClonedEntity); + // create a joint with a parent and child link + const std::string parentLinkEntityName = "parentLinkEntity"; + const std::string childLinkEntityName = "childLinkEntity"; + Entity parentLinkEntity = manager.CreateEntity(); + manager.CreateComponent(parentLinkEntity, + components::Name(parentLinkEntityName)); + manager.CreateComponent(parentLinkEntity, components::CanonicalLink()); + Entity jointEntity = manager.CreateEntity(); + manager.CreateComponent(jointEntity, + components::ParentEntity(parentLinkEntity)); + manager.CreateComponent(jointEntity, components::Name("jointEntity")); + manager.CreateComponent(jointEntity, components::Joint()); + manager.CreateComponent(jointEntity, + components::ParentLinkName(parentLinkEntityName)); + manager.CreateComponent(jointEntity, + components::ChildLinkName(childLinkEntityName)); + Entity childLinkEntity = manager.CreateEntity(); + manager.CreateComponent(childLinkEntity, + components::ParentEntity(jointEntity)); + manager.CreateComponent(childLinkEntity, + components::Name(childLinkEntityName)); + manager.CreateComponent(childLinkEntity, components::Link()); + EXPECT_EQ(13u, manager.EntityCount()); + + // clone a joint that has a parent and child link. + auto clonedParentLinkEntity = manager.Clone(parentLinkEntity, kNullEntity, + "", true); + ASSERT_NE(kNullEntity, clonedParentLinkEntity); + EXPECT_EQ(16u, manager.EntityCount()); + clonedEntities.insert(clonedParentLinkEntity); + auto clonedJoints = manager.EntitiesByComponents( + components::ParentEntity(clonedParentLinkEntity)); + ASSERT_EQ(1u, clonedJoints.size()); + clonedEntities.insert(clonedJoints[0]); + auto clonedChildLinks = manager.EntitiesByComponents( + components::ParentEntity(clonedJoints[0])); + ASSERT_EQ(1u, clonedChildLinks.size()); + clonedEntities.insert(clonedChildLinks[0]); + + // The cloned joint should have the cloned parent/child link names attached to + // it, not the original parent/child link names + auto clonedJointParentLinkName = + manager.Component(clonedJoints[0]); + ASSERT_NE(nullptr, clonedJointParentLinkName); + EXPECT_NE(clonedJointParentLinkName->Data(), parentLinkEntityName); + auto clonedJointChildLinkName = + 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 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(5u, clonedEntities.size()); + EXPECT_EQ(8u, clonedEntities.size()); for (const auto &entity : clonedEntities) { auto nameComp = manager.Component(entity); @@ -2802,7 +2864,7 @@ TEST_P(EntityComponentManagerFixture, CloneEntities) // try to clone an entity that does not exist EXPECT_EQ(kNullEntity, manager.Clone(kNullEntity, topLevelEntity, "", allowRename)); - EXPECT_EQ(10u, manager.EntityCount()); + EXPECT_EQ(16u, manager.EntityCount()); } ///////////////////////////////////////////////// From c6037104260b0a619c73ac700dac0a7f67231ea0 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 6 Oct 2021 11:34:33 -0700 Subject: [PATCH 07/22] Add support for configuring point size in Visualize Lidar GUI plugin (#1021) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * add spinbox to configure point size Signed-off-by: Ian Chen * cap letter Signed-off-by: Ian Chen * remove empty line Signed-off-by: Ian Chen Co-authored-by: Alejandro Hernández Cordero --- .../plugins/visualize_lidar/VisualizeLidar.cc | 7 +++++++ .../plugins/visualize_lidar/VisualizeLidar.hh | 4 ++++ .../plugins/visualize_lidar/VisualizeLidar.qml | 18 +++++++++++++++++- 3 files changed, 28 insertions(+), 1 deletion(-) diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc index aed80f1a06..7211d9e8ef 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc @@ -364,6 +364,13 @@ void VisualizeLidar::UpdateType(int _type) this->dataPtr->lidar->SetType(this->dataPtr->visualType); } +////////////////////////////////////////////////// +void VisualizeLidar::UpdateSize(int _size) +{ + std::lock_guard lock(this->dataPtr->serviceMutex); + this->dataPtr->lidar->SetSize(_size); +} + ////////////////////////////////////////////////// void VisualizeLidar::OnTopic(const QString &_topicName) { diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.hh b/src/gui/plugins/visualize_lidar/VisualizeLidar.hh index e06dee0bd2..7eebe69977 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.hh +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.hh @@ -90,6 +90,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \param[in] _type Index of selected visual type public: Q_INVOKABLE void UpdateType(int _type); + /// \brief Set lidar visualization size + /// \param[in] _size Size of lidar visualization + public: Q_INVOKABLE void UpdateSize(int _size); + /// \brief Get the topic list as a string /// \return Message type public: Q_INVOKABLE QStringList TopicList() const; diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.qml b/src/gui/plugins/visualize_lidar/VisualizeLidar.qml index dbe9d680ee..fa6fdea027 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.qml +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.qml @@ -25,7 +25,7 @@ GridLayout { columns: 6 columnSpacing: 10 Layout.minimumWidth: 350 - Layout.minimumHeight: 300 + Layout.minimumHeight: 400 anchors.fill: parent anchors.leftMargin: 10 anchors.rightMargin: 10 @@ -131,4 +131,20 @@ GridLayout { VisualizeLidar.UpdateType(typeCombo.currentIndex); } } + + Text { + Layout.columnSpan: 2 + id: pointSizeText + color: "dimgrey" + text: "Point Size" + } + + IgnSpinBox { + Layout.columnSpan: 2 + id: pointSize + maximumValue: 1000 + minimumValue: 1 + value: 1 + onEditingFinished: VisualizeLidar.UpdateSize(pointSize.value) + } } From 614340cac2eb024ea9bbbae427dfd14291da1eaa Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 8 Oct 2021 11:12:54 +0800 Subject: [PATCH 08/22] Create GUI config folder before copying config (#1092) Signed-off-by: Arjo Chakravarty Signed-off-by: Louise Poubel Co-authored-by: Louise Poubel --- src/gui/Gui.cc | 30 ++++++++++++++++++++++++++++-- 1 file changed, 28 insertions(+), 2 deletions(-) diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index b0e868503d..7de934c158 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -80,6 +80,10 @@ std::unique_ptr createGui( // Set default config file for Gazebo std::string defaultConfig; + + // Default config folder. + std::string defaultConfigFolder; + if (nullptr == _defaultGuiConfig) { // The playback flag (and not the gui-config flag) was @@ -89,8 +93,11 @@ std::unique_ptr createGui( defaultGuiConfigName = "playback_gui.config"; } ignition::common::env(IGN_HOMEDIR, defaultConfig); - defaultConfig = ignition::common::joinPaths(defaultConfig, ".ignition", - "gazebo", IGNITION_GAZEBO_MAJOR_VERSION_STR, defaultGuiConfigName); + defaultConfigFolder = + ignition::common::joinPaths(defaultConfig, ".ignition", + "gazebo", IGNITION_GAZEBO_MAJOR_VERSION_STR); + defaultConfig = ignition::common::joinPaths(defaultConfigFolder, + defaultGuiConfigName); } else { @@ -262,8 +269,27 @@ std::unique_ptr createGui( // the installed file there first. if (!ignition::common::exists(defaultConfig)) { + if (!ignition::common::exists(defaultConfigFolder)) + { + if (!ignition::common::createDirectories(defaultConfigFolder)) + { + ignerr << "Failed to create the default config folder [" + << defaultConfigFolder << "]\n"; + return nullptr; + } + } + auto installedConfig = ignition::common::joinPaths( IGNITION_GAZEBO_GUI_CONFIG_PATH, defaultGuiConfigName); + if (!ignition::common::exists(installedConfig)) + { + ignerr << "Failed to copy installed config [" << installedConfig + << "] to default config [" << defaultConfig << "]." + << "(file " << installedConfig << " doesn't exist)" + << std::endl; + return nullptr; + } + if (!ignition::common::copyFile(installedConfig, defaultConfig)) { ignerr << "Failed to copy installed config [" << installedConfig From c43196237cf6779f3bd4121b8a9de9936c04ae77 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 11 Oct 2021 15:12:20 -0700 Subject: [PATCH 09/22] Prevent crash and print error (#1099) Signed-off-by: Ian Chen --- src/rendering/SceneManager.cc | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index a84eaf13ab..72741c83fd 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -513,8 +513,22 @@ rendering::MaterialPtr SceneManager::LoadMaterial( } else { - ignerr << "PBR material: currently only metal workflow is supported" - << std::endl; + const sdf::PbrWorkflow *specular= + pbr->Workflow(sdf::PbrWorkflowType::SPECULAR); + if (specular) + { + ignerr << "PBR material: currently only metal workflow is supported. " + << "Ignition Gazebo will try to render the material using " + << "metal workflow but without Roughness / Metalness settings." + << std::endl; + } + workflow = const_cast(specular); + } + + if (!workflow) + { + ignerr << "No valid PBR workflow found. " << std::endl; + return rendering::MaterialPtr(); } // albedo map From bed3a9375feea7c195e11352f312c30c0b860a62 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Mon, 11 Oct 2021 18:44:56 -0400 Subject: [PATCH 10/22] Cache top level and static to speed up physics system (Backport #656) (#993) Signed-off-by: Ashton Larkin Signed-off-by: Louise Poubel --- src/systems/physics/Physics.cc | 409 ++++++++++++++++++--------------- 1 file changed, 228 insertions(+), 181 deletions(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 6e675264b2..a0a4015d7e 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -205,6 +205,13 @@ class ignition::gazebo::systems::PhysicsPrivate public: ignition::math::Pose3d RelativePose(const Entity &_from, const Entity &_to, const EntityComponentManager &_ecm) const; + /// \brief Cache the top-level model for each entity. + /// The key is an entity and the value is its top level model. + public: std::unordered_map topLevelModelMap; + + /// \brief Keep track of what entities are static (models and links). + public: std::unordered_set staticEntities; + /// \brief A map between model entity ids in the ECM to whether its battery /// has drained. public: std::unordered_map entityOffMap; @@ -641,6 +648,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) if (staticComp && staticComp->Data()) { model.SetStatic(staticComp->Data()); + this->staticEntities.insert(_entity); } auto selfCollideComp = _ecm.Component(_entity); if (selfCollideComp && selfCollideComp ->Data()) @@ -674,11 +682,15 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) } auto modelPtrPhys = nestedModelFeature->ConstructNestedModel(model); this->entityModelMap.AddEntity(_entity, modelPtrPhys); + this->topLevelModelMap.insert(std::make_pair(_entity, + topLevelModel(_entity, _ecm))); } else { auto modelPtrPhys = worldPtrPhys->ConstructModel(model); this->entityModelMap.AddEntity(_entity, modelPtrPhys); + this->topLevelModelMap.insert(std::make_pair(_entity, + topLevelModel(_entity, _ecm))); } } // check if parent is a model (nested model) @@ -708,13 +720,17 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) auto parentStaticComp = _ecm.Component(_parent->Data()); if (parentStaticComp && parentStaticComp->Data()) + { model.SetStatic(true); - + this->staticEntities.insert(_entity); + } auto modelPtrPhys = nestedModelFeature->ConstructNestedModel(model); if (modelPtrPhys) { this->entityModelMap.AddEntity(_entity, modelPtrPhys); + this->topLevelModelMap.insert(std::make_pair(_entity, + topLevelModel(_entity, _ecm))); } else { @@ -767,6 +783,12 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) link.SetName(_name->Data()); link.SetRawPose(_pose->Data()); + if (this->staticEntities.find(_parent->Data()) != + this->staticEntities.end()) + { + this->staticEntities.insert(_entity); + } + // get link inertial auto inertial = _ecm.Component(_entity); if (inertial) @@ -776,6 +798,8 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) auto linkPtrPhys = modelPtrPhys->ConstructLink(link); this->entityLinkMap.AddEntity(_entity, linkPtrPhys); + this->topLevelModelMap.insert(std::make_pair(_entity, + topLevelModel(_entity, _ecm))); return true; }); @@ -907,6 +931,8 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) } } + this->topLevelModelMap.insert(std::make_pair(_entity, + topLevelModel(_entity, _ecm))); return true; }); @@ -989,6 +1015,8 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) // Some joints may not be supported, so only add them to the map if // the physics entity is valid this->entityJointMap.AddEntity(_entity, jointPtrPhys); + this->topLevelModelMap.insert(std::make_pair(_entity, + topLevelModel(_entity, _ecm))); } return true; }); @@ -1080,6 +1108,8 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) igndbg << "Creating detachable joint [" << _entity << "]" << std::endl; this->entityJointMap.AddEntity(_entity, jointPtrPhys); + this->topLevelModelMap.insert(std::make_pair(_entity, + topLevelModel(_entity, _ecm))); } else { @@ -1114,20 +1144,26 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) _ecm.ChildrenByComponents(childLink, components::Collision())) { this->entityCollisionMap.Remove(childCollision); + this->topLevelModelMap.erase(childCollision); } this->entityLinkMap.Remove(childLink); + this->topLevelModelMap.erase(childLink); + this->staticEntities.erase(childLink); } for (const auto &childJoint : _ecm.ChildrenByComponents(_entity, components::Joint())) { this->entityJointMap.Remove(childJoint); + this->topLevelModelMap.erase(childJoint); } this->entityFreeGroupMap.Remove(_entity); // Remove the model from the physics engine modelPtrPhys->Remove(); this->entityModelMap.Remove(_entity); + this->topLevelModelMap.erase(_entity); + this->staticEntities.erase(_entity); } return true; }); @@ -1376,7 +1412,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) return true; // world pose cmd currently not supported for nested models - if (_entity != topLevelModel(_entity, _ecm)) + if (_entity != this->topLevelModelMap[_entity]) { ignerr << "Unable to set world pose for nested models." << std::endl; @@ -1408,9 +1444,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) linkPose)); // Process pose commands for static models here, as one-time changes - const components::Static *staticComp = - _ecm.Component(_entity); - if (staticComp && staticComp->Data()) + if (this->staticEntities.find(_entity) != this->staticEntities.end()) { auto worldPoseComp = _ecm.Component(_entity); if (worldPoseComp) @@ -1479,7 +1513,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) return true; // angular vel cmd currently not supported for nested models - if (_entity != topLevelModel(_entity, _ecm)) + if (_entity != this->topLevelModelMap[_entity]) { ignerr << "Unable to set angular velocity for nested models." << std::endl; @@ -1529,7 +1563,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) return true; // linear vel cmd currently not supported for nested models - if (_entity != topLevelModel(_entity, _ecm)) + if (_entity != this->topLevelModelMap[_entity]) { ignerr << "Unable to set linear velocity for nested models." << std::endl; @@ -1775,211 +1809,218 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) { IGN_PROFILE("PhysicsPrivate::UpdateSim"); - // local pose + // Link poses, velocities... + IGN_PROFILE_BEGIN("Links"); _ecm.Each( [&](const Entity &_entity, components::Link * /*_link*/, components::Pose *_pose, const components::ParentEntity *_parent)->bool { // If parent is static, don't process pose changes as periodic - const auto *staticComp = - _ecm.Component(_parent->Data()); - - if (staticComp && staticComp->Data()) + if (this->staticEntities.find(_entity) != this->staticEntities.end()) return true; - if (auto linkPhys = this->entityLinkMap.Get(_entity)) + auto linkPtrPhys = this->entityLinkMap.Get(_entity); + if (nullptr == linkPtrPhys) { - // get top level model of this link - auto topLevelModelEnt = topLevelModel(_parent->Data(), _ecm); + ignerr << "Internal error: link [" << _entity + << "] not in entity map" << std::endl; + return true; + } - auto canonicalLink = - _ecm.Component(_entity); + IGN_PROFILE_BEGIN("Local pose"); - auto frameData = linkPhys->FrameDataRelativeToWorld(); - const auto &worldPose = frameData.pose; + // get top level model of this link + auto topLevelModelEnt = this->topLevelModelMap[_parent->Data()]; - if (canonicalLink) - { - // This is the canonical link, update the top level model. - // The pose of this link w.r.t its top level model never changes - // because it's "fixed" to the model. Instead, we change - // the top level model's pose here. The physics engine gives us the - // pose of this link relative to world so to set the top level - // model's pose, we have to post-multiply it by the inverse of the - // transform of the link w.r.t to its top level model. - math::Pose3d linkPoseFromTopLevelModel; - linkPoseFromTopLevelModel = - this->RelativePose(topLevelModelEnt, _entity, _ecm); - - // update top level model's pose - auto mutableModelPose = - _ecm.Component(topLevelModelEnt); - *(mutableModelPose) = components::Pose( - math::eigen3::convert(worldPose) * - linkPoseFromTopLevelModel.Inverse()); - - _ecm.SetChanged(topLevelModelEnt, components::Pose::typeId, - ComponentState::PeriodicChange); - } - else - { - // Compute the relative pose of this link from the top level model - // first get the world pose of the top level model - auto worldComp = - _ecm.Component(topLevelModelEnt); - // if the worldComp is a nullptr, something is wrong with ECS - if (!worldComp) - { - ignerr << "The parent component of " << topLevelModelEnt - << " could not be found. This should never happen!\n"; - return true; - } - math::Pose3d parentWorldPose = - this->RelativePose(worldComp->Data(), _parent->Data(), _ecm); - - // Unlike canonical links, pose of regular links can move relative. - // to the parent. Same for links inside nested models. - *_pose = components::Pose(math::eigen3::convert(worldPose) + - parentWorldPose.Inverse()); - _ecm.SetChanged(_entity, components::Pose::typeId, - ComponentState::PeriodicChange); - } + auto canonicalLink = + _ecm.Component(_entity); - // Populate world poses, velocities and accelerations of the link. For - // now these components are updated only if another system has created - // the corresponding component on the entity. - auto worldPoseComp = _ecm.Component(_entity); - if (worldPoseComp) - { - auto state = - worldPoseComp->SetData(math::eigen3::convert(frameData.pose), - this->pose3Eql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, components::WorldPose::typeId, state); - } + auto frameData = linkPtrPhys->FrameDataRelativeToWorld(); + const auto &worldPose = frameData.pose; - // Velocity in world coordinates - auto worldLinVelComp = - _ecm.Component(_entity); - if (worldLinVelComp) + if (canonicalLink) + { + // This is the canonical link, update the top level model. + // The pose of this link w.r.t its top level model never changes + // because it's "fixed" to the model. Instead, we change + // the top level model's pose here. The physics engine gives us the + // pose of this link relative to world so to set the top level + // model's pose, we have to post-multiply it by the inverse of the + // transform of the link w.r.t to its top level model. + math::Pose3d linkPoseFromTopLevelModel; + linkPoseFromTopLevelModel = + this->RelativePose(topLevelModelEnt, _entity, _ecm); + + // update top level model's pose + auto mutableModelPose = + _ecm.Component(topLevelModelEnt); + *(mutableModelPose) = components::Pose( + math::eigen3::convert(worldPose) * + linkPoseFromTopLevelModel.Inverse()); + + _ecm.SetChanged(topLevelModelEnt, components::Pose::typeId, + ComponentState::PeriodicChange); + } + else + { + // Compute the relative pose of this link from the top level model + // first get the world pose of the top level model + auto worldComp = + _ecm.Component(topLevelModelEnt); + // if the worldComp is a nullptr, something is wrong with ECS + if (!worldComp) { - auto state = worldLinVelComp->SetData( - math::eigen3::convert(frameData.linearVelocity), - this->vec3Eql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, - components::WorldLinearVelocity::typeId, state); + ignerr << "The parent component of " << topLevelModelEnt + << " could not be found. This should never happen!\n"; + return true; } + math::Pose3d parentWorldPose = + this->RelativePose(worldComp->Data(), _parent->Data(), _ecm); + + // Unlike canonical links, pose of regular links can move relative. + // to the parent. Same for links inside nested models. + *_pose = components::Pose(math::eigen3::convert(worldPose) + + parentWorldPose.Inverse()); + _ecm.SetChanged(_entity, components::Pose::typeId, + ComponentState::PeriodicChange); + } + IGN_PROFILE_END(); - // Angular velocity in world frame coordinates - auto worldAngVelComp = - _ecm.Component(_entity); - if (worldAngVelComp) - { - auto state = worldAngVelComp->SetData( - math::eigen3::convert(frameData.angularVelocity), - this->vec3Eql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, - components::WorldAngularVelocity::typeId, state); - } + // Populate world poses, velocities and accelerations of the link. For + // now these components are updated only if another system has created + // the corresponding component on the entity. + auto worldPoseComp = _ecm.Component(_entity); + if (worldPoseComp) + { + auto state = + worldPoseComp->SetData(math::eigen3::convert(frameData.pose), + this->pose3Eql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(_entity, components::WorldPose::typeId, state); + } - // Acceleration in world frame coordinates - auto worldLinAccelComp = - _ecm.Component(_entity); - if (worldLinAccelComp) - { - auto state = worldLinAccelComp->SetData( - math::eigen3::convert(frameData.linearAcceleration), + // Velocity in world coordinates + auto worldLinVelComp = + _ecm.Component(_entity); + if (worldLinVelComp) + { + auto state = worldLinVelComp->SetData( + math::eigen3::convert(frameData.linearVelocity), this->vec3Eql) ? ComponentState::PeriodicChange : ComponentState::NoChange; - _ecm.SetChanged(_entity, - components::WorldLinearAcceleration::typeId, state); - } + _ecm.SetChanged(_entity, + components::WorldLinearVelocity::typeId, state); + } - // Angular acceleration in world frame coordinates - auto worldAngAccelComp = - _ecm.Component(_entity); + // Angular velocity in world frame coordinates + auto worldAngVelComp = + _ecm.Component(_entity); + if (worldAngVelComp) + { + auto state = worldAngVelComp->SetData( + math::eigen3::convert(frameData.angularVelocity), + this->vec3Eql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(_entity, + components::WorldAngularVelocity::typeId, state); + } - if (worldAngAccelComp) - { - auto state = worldAngAccelComp->SetData( - math::eigen3::convert(frameData.angularAcceleration), - this->vec3Eql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, - components::WorldAngularAcceleration::typeId, state); - } + // Acceleration in world frame coordinates + auto worldLinAccelComp = + _ecm.Component(_entity); + if (worldLinAccelComp) + { + auto state = worldLinAccelComp->SetData( + math::eigen3::convert(frameData.linearAcceleration), + this->vec3Eql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(_entity, + components::WorldLinearAcceleration::typeId, state); + } - const Eigen::Matrix3d R_bs = worldPose.linear().transpose(); // NOLINT + // Angular acceleration in world frame coordinates + auto worldAngAccelComp = + _ecm.Component(_entity); - // Velocity in body-fixed frame coordinates - auto bodyLinVelComp = - _ecm.Component(_entity); - if (bodyLinVelComp) - { - Eigen::Vector3d bodyLinVel = R_bs * frameData.linearVelocity; - auto state = - bodyLinVelComp->SetData(math::eigen3::convert(bodyLinVel), - this->vec3Eql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, components::LinearVelocity::typeId, state); - } + if (worldAngAccelComp) + { + auto state = worldAngAccelComp->SetData( + math::eigen3::convert(frameData.angularAcceleration), + this->vec3Eql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(_entity, + components::WorldAngularAcceleration::typeId, state); + } - // Angular velocity in body-fixed frame coordinates - auto bodyAngVelComp = - _ecm.Component(_entity); - if (bodyAngVelComp) - { - Eigen::Vector3d bodyAngVel = R_bs * frameData.angularVelocity; - auto state = - bodyAngVelComp->SetData(math::eigen3::convert(bodyAngVel), - this->vec3Eql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, components::AngularVelocity::typeId, - state); - } + const Eigen::Matrix3d R_bs = worldPose.linear().transpose(); // NOLINT - // Acceleration in body-fixed frame coordinates - auto bodyLinAccelComp = - _ecm.Component(_entity); - if (bodyLinAccelComp) - { - Eigen::Vector3d bodyLinAccel = R_bs * frameData.linearAcceleration; - auto state = - bodyLinAccelComp->SetData(math::eigen3::convert(bodyLinAccel), - this->vec3Eql)? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, components::LinearAcceleration::typeId, - state); - } + // Velocity in body-fixed frame coordinates + auto bodyLinVelComp = + _ecm.Component(_entity); + if (bodyLinVelComp) + { + Eigen::Vector3d bodyLinVel = R_bs * frameData.linearVelocity; + auto state = + bodyLinVelComp->SetData(math::eigen3::convert(bodyLinVel), + this->vec3Eql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(_entity, components::LinearVelocity::typeId, state); + } - // Angular acceleration in world frame coordinates - auto bodyAngAccelComp = - _ecm.Component(_entity); - if (bodyAngAccelComp) - { - Eigen::Vector3d bodyAngAccel = R_bs * frameData.angularAcceleration; - auto state = - bodyAngAccelComp->SetData(math::eigen3::convert(bodyAngAccel), - this->vec3Eql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, components::AngularAcceleration::typeId, - state); - } + // Angular velocity in body-fixed frame coordinates + auto bodyAngVelComp = + _ecm.Component(_entity); + if (bodyAngVelComp) + { + Eigen::Vector3d bodyAngVel = R_bs * frameData.angularVelocity; + auto state = + bodyAngVelComp->SetData(math::eigen3::convert(bodyAngVel), + this->vec3Eql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(_entity, components::AngularVelocity::typeId, + state); + } + + // Acceleration in body-fixed frame coordinates + auto bodyLinAccelComp = + _ecm.Component(_entity); + if (bodyLinAccelComp) + { + Eigen::Vector3d bodyLinAccel = R_bs * frameData.linearAcceleration; + auto state = + bodyLinAccelComp->SetData(math::eigen3::convert(bodyLinAccel), + this->vec3Eql)? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(_entity, components::LinearAcceleration::typeId, + state); + } + + // Angular acceleration in world frame coordinates + auto bodyAngAccelComp = + _ecm.Component(_entity); + if (bodyAngAccelComp) + { + Eigen::Vector3d bodyAngAccel = R_bs * frameData.angularAcceleration; + auto state = + bodyAngAccelComp->SetData(math::eigen3::convert(bodyAngAccel), + this->vec3Eql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(_entity, components::AngularAcceleration::typeId, + state); } return true; }); + IGN_PROFILE_END(); // pose/velocity/acceleration of non-link entities such as sensors / // collisions. These get updated only if another system has created a @@ -1990,6 +2031,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) // * AngularVelocity // * LinearAcceleration + IGN_PROFILE_BEGIN("Sensors / collisions"); // world pose _ecm.Each( @@ -2082,8 +2124,10 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) return true; }); + IGN_PROFILE_END(); // Clear reset components + IGN_PROFILE_BEGIN("Clear / reset components"); std::vector entitiesPositionReset; _ecm.Each( [&](const Entity &_entity, components::JointPositionReset *) -> bool @@ -2138,6 +2182,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) std::fill(_slip->Data().begin(), _slip->Data().end(), 0.0); return true; }); + IGN_PROFILE_END(); _ecm.Each( [&](const Entity &, components::AngularVelocityCmd *_vel) -> bool @@ -2154,6 +2199,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) }); // Update joint positions + IGN_PROFILE_BEGIN("Joints"); _ecm.Each( [&](const Entity &_entity, components::Joint *, components::JointPosition *_jointPos) -> bool @@ -2188,6 +2234,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) } return true; }); + IGN_PROFILE_END(); // TODO(louise) Skip this if there are no collision features this->UpdateCollisions(_ecm); From e9be884fe063219635a0f21831eeae885ea60f4a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 12 Oct 2021 19:31:11 +0200 Subject: [PATCH 11/22] Fixed IMU system plugin (#1043) Signed-off-by: ahcorde Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- src/systems/imu/Imu.cc | 150 ++++++++++++++++++++++++++--------------- 1 file changed, 95 insertions(+), 55 deletions(-) diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index 55cab7001b..d883f6cc91 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -27,8 +27,6 @@ #include -#include - #include #include @@ -58,8 +56,14 @@ class ignition::gazebo::systems::ImuPrivate /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// \brief Keep track of world ID, which is equivalent to the scene's + /// root visual. + /// Defaults to zero, which is considered invalid by Ignition Gazebo. public: Entity worldEntity = kNullEntity; + /// True if the rendering component is initialized + public: bool initialized = false; + /// \brief Create IMU sensor /// \param[in] _ecm Mutable reference to ECM. public: void CreateImuEntities(EntityComponentManager &_ecm); @@ -68,6 +72,17 @@ class ignition::gazebo::systems::ImuPrivate /// \param[in] _ecm Immutable reference to ECM. public: void Update(const EntityComponentManager &_ecm); + /// \brief Create sensor + /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _entity Entity of the IMU + /// \param[in] _imu IMU component. + /// \param[in] _parent Parent entity component. + public: void addIMU( + EntityComponentManager &_ecm, + const Entity _entity, + const components::Imu *_imu, + const components::ParentEntity *_parent); + /// \brief Remove IMU sensors if their entities have been removed from /// simulation. /// \param[in] _ecm Immutable reference to ECM. @@ -121,6 +136,63 @@ void Imu::PostUpdate(const UpdateInfo &_info, this->dataPtr->RemoveImuEntities(_ecm); } +////////////////////////////////////////////////// +void ImuPrivate::addIMU( + EntityComponentManager &_ecm, + const Entity _entity, + const components::Imu *_imu, + const components::ParentEntity *_parent) +{ + // Get the world acceleration (defined in world frame) + auto gravity = _ecm.Component(worldEntity); + if (nullptr == gravity) + { + ignerr << "World missing gravity." << std::endl; + return; + } + + // create sensor + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdf::Sensor data = _imu->Data(); + data.SetName(sensorScopedName); + // check topic + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/imu"; + data.SetTopic(topic); + } + std::unique_ptr sensor = + this->sensorFactory.CreateSensor< + sensors::ImuSensor>(data); + if (nullptr == sensor) + { + ignerr << "Failed to create sensor [" << sensorScopedName << "]" + << std::endl; + return; + } + + // set sensor parent + std::string parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); + + // set gravity - assume it remains fixed + sensor->SetGravity(gravity->Data()); + + // Get initial pose of sensor and set the reference z pos + // The WorldPose component was just created and so it's empty + // We'll compute the world pose manually here + math::Pose3d p = worldPose(_entity, _ecm); + sensor->SetOrientationReference(p.Rot()); + + // Set topic + _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); + + this->entitySensorMap.insert( + std::make_pair(_entity, std::move(sensor))); +} + ////////////////////////////////////////////////// void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm) { @@ -134,63 +206,31 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm) return; } - // Get the world acceleration (defined in world frame) - auto gravity = _ecm.Component(worldEntity); - if (nullptr == gravity) + if (!this->initialized) { - ignerr << "World missing gravity." << std::endl; - return; - } - - // Create IMUs - _ecm.EachNew( - [&](const Entity &_entity, - const components::Imu *_imu, - const components::ParentEntity *_parent)->bool - { - // create sensor - std::string sensorScopedName = - removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); - sdf::Sensor data = _imu->Data(); - data.SetName(sensorScopedName); - // check topic - if (data.Topic().empty()) + // Create IMUs + _ecm.Each( + [&](const Entity &_entity, + const components::Imu *_imu, + const components::ParentEntity *_parent)->bool { - std::string topic = scopedName(_entity, _ecm) + "/imu"; - data.SetTopic(topic); - } - std::unique_ptr sensor = - this->sensorFactory.CreateSensor< - sensors::ImuSensor>(data); - if (nullptr == sensor) + addIMU(_ecm, _entity, _imu, _parent); + return true; + }); + this->initialized = true; + } + else + { + // Create IMUs + _ecm.EachNew( + [&](const Entity &_entity, + const components::Imu *_imu, + const components::ParentEntity *_parent)->bool { - ignerr << "Failed to create sensor [" << sensorScopedName << "]" - << std::endl; + addIMU(_ecm, _entity, _imu, _parent); return true; - } - - // set sensor parent - std::string parentName = _ecm.Component( - _parent->Data())->Data(); - sensor->SetParent(parentName); - - // set gravity - assume it remains fixed - sensor->SetGravity(gravity->Data()); - - // Get initial pose of sensor and set the reference z pos - // The WorldPose component was just created and so it's empty - // We'll compute the world pose manually here - math::Pose3d p = worldPose(_entity, _ecm); - sensor->SetOrientationReference(p.Rot()); - - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - - this->entitySensorMap.insert( - std::make_pair(_entity, std::move(sensor))); - - return true; - }); + }); + } } ////////////////////////////////////////////////// From 97e07f4f57f0f1eca4020319718c53196f2ffa68 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 14 Oct 2021 13:24:00 -0700 Subject: [PATCH 12/22] JointPositionController: Improve misleading error message (#1098) Signed-off-by: Louise Poubel Co-authored-by: Jose Luis Rivero --- src/rendering/SceneManager.cc | 3 +-- .../JointPositionController.cc | 14 +++++++++----- src/systems/physics/Physics.cc | 3 +-- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 72741c83fd..a5f360259a 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -513,8 +513,7 @@ rendering::MaterialPtr SceneManager::LoadMaterial( } else { - const sdf::PbrWorkflow *specular= - pbr->Workflow(sdf::PbrWorkflowType::SPECULAR); + auto specular = pbr->Workflow(sdf::PbrWorkflowType::SPECULAR); if (specular) { ignerr << "PBR material: currently only metal workflow is supported. " diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 691336d901..067c622161 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -206,21 +207,24 @@ void JointPositionController::PreUpdate( _ecm.CreateComponent( this->dataPtr->jointEntity, components::JointPosition()); } - if (jointPosComp == nullptr) + // We just created the joint position component, give one iteration for the + // physics system to update its size + if (jointPosComp == nullptr || jointPosComp->Data().empty()) return; // Sanity check: Make sure that the joint index is valid. if (this->dataPtr->jointIndex >= jointPosComp->Data().size()) { - static bool invalidJointReported = false; - if (!invalidJointReported) + static std::unordered_set reported; + if (reported.find(this->dataPtr->jointEntity) == reported.end()) { ignerr << "[JointPositionController]: Detected an invalid " << "parameter. The index specified is [" - << this->dataPtr->jointIndex << "] but the joint only has [" + << this->dataPtr->jointIndex << "] but joint [" + << this->dataPtr->jointName << "] only has [" << jointPosComp->Data().size() << "] index[es]. " << "This controller will be ignored" << std::endl; - invalidJointReported = true; + reported.insert(this->dataPtr->jointEntity); } return; } diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index a0a4015d7e..710a001682 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -2207,8 +2207,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) if (auto jointPhys = this->entityJointMap.Get(_entity)) { _jointPos->Data().resize(jointPhys->GetDegreesOfFreedom()); - for (std::size_t i = 0; i < jointPhys->GetDegreesOfFreedom(); - ++i) + for (std::size_t i = 0; i < jointPhys->GetDegreesOfFreedom(); ++i) { _jointPos->Data()[i] = jointPhys->GetPosition(i); } From 4f460fb1526c9ca5fdd2076a4bfbb37ae2e9c6c0 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 14 Oct 2021 13:24:47 -0700 Subject: [PATCH 13/22] Adjust pose decimals based on element width (#1089) Signed-off-by: Nate Koenig Signed-off-by: Louise Poubel Co-authored-by: Nate Koenig Co-authored-by: Louise Poubel --- .../plugins/component_inspector/ComponentInspector.qml | 9 +++++++++ src/gui/plugins/component_inspector/Pose3d.qml | 4 ++-- src/gui/plugins/component_inspector/Vector3d.qml | 4 ++-- 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index e48bc4b393..dd688aafaa 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -78,6 +78,15 @@ Rectangle { return _model.dataType + '.qml' } + // Get number of decimal digits based on a widget's width + function getDecimals(_width) { + if (_width <= 80) + return 2; + else if (_width <= 100) + return 4; + return 6; + } + /** * Forward pose changes to C++ */ diff --git a/src/gui/plugins/component_inspector/Pose3d.qml b/src/gui/plugins/component_inspector/Pose3d.qml index 1a198f5402..962acb770e 100644 --- a/src/gui/plugins/component_inspector/Pose3d.qml +++ b/src/gui/plugins/component_inspector/Pose3d.qml @@ -90,7 +90,7 @@ Rectangle { value: writableSpin.activeFocus ? writableSpin.value : numberValue minimumValue: -spinMax maximumValue: spinMax - decimals: 6 + decimals: getDecimals(writableSpin.width) onEditingFinished: { sendPose() } @@ -108,7 +108,7 @@ Rectangle { horizontalAlignment: Text.AlignRight verticalAlignment: Text.AlignVCenter text: { - var decimals = numberText.width < 100 ? 2 : 6 + var decimals = getDecimals(numberText.width) return numberValue.toFixed(decimals) } } diff --git a/src/gui/plugins/component_inspector/Vector3d.qml b/src/gui/plugins/component_inspector/Vector3d.qml index 900543701c..c6da529c8f 100644 --- a/src/gui/plugins/component_inspector/Vector3d.qml +++ b/src/gui/plugins/component_inspector/Vector3d.qml @@ -54,7 +54,7 @@ Rectangle { value: numberValue minimumValue: -spinMax maximumValue: spinMax - decimals: writableSpin.width < 100 ? 2 : 6 + decimals: getDecimals(writableSpin.width) } } @@ -69,7 +69,7 @@ Rectangle { horizontalAlignment: Text.AlignRight verticalAlignment: Text.AlignVCenter text: { - var decimals = numberText.width < 100 ? 2 : 6 + var decimals = getDecimals(numberText.width) return numberValue.toFixed(decimals) } } From 27ac7249978968881f901f76887f109ff0f7e2fa Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Fri, 15 Oct 2021 12:40:09 -0700 Subject: [PATCH 14/22] Use QTimer to update plugins in the Qt thread (#1095) Reorganize code so that the `ecm` and `updateInfo` variables are only accessed from the Qt thread. Signed-off-by: Jenn Nguyen Signed-off-by: Addisu Z. Taddese Co-authored-by: Addisu Z. Taddese --- include/ignition/gazebo/gui/GuiRunner.hh | 9 +++- src/gui/GuiRunner.cc | 51 ++++++++----------- .../component_inspector/ComponentInspector.cc | 7 +-- .../JointPositionController.cc | 7 +-- 4 files changed, 30 insertions(+), 44 deletions(-) diff --git a/include/ignition/gazebo/gui/GuiRunner.hh b/include/ignition/gazebo/gui/GuiRunner.hh index 85520e0446..cc8386945a 100644 --- a/include/ignition/gazebo/gui/GuiRunner.hh +++ b/include/ignition/gazebo/gui/GuiRunner.hh @@ -60,13 +60,18 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \param[in] _res Response containing new state. private: void OnStateAsyncService(const msgs::SerializedStepMap &_res); - /// \brief Callback when a new state is received from the server. + /// \brief Callback when a new state is received from the server. Actual + /// updating of the ECM is delegated to OnStateQt /// \param[in] _msg New state message. private: void OnState(const msgs::SerializedStepMap &_msg); + /// \brief Called by the Qt thread to update the ECM with new state + /// \param[in] _msg New state message. + private: Q_INVOKABLE void OnStateQt(const msgs::SerializedStepMap &_msg); + /// \brief Update the plugins. /// \todo(anyone) Move to GuiRunner::Implementation when porting to v5 - private: void UpdatePlugins(); + private: Q_INVOKABLE void UpdatePlugins(); /// \brief Entity-component manager. private: gazebo::EntityComponentManager ecm; diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 7ca91beefc..a023c9205d 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -30,19 +30,15 @@ using namespace ignition; using namespace gazebo; -/// \todo(anyone) Move to GuiRunner::Implementation when porting to v5 -/// \brief Flag used to end the gUpdateThread. -static bool gRunning = false; - -/// \brief Mutex to protect the plugin update. -static std::mutex gUpdateMutex; - -/// \brief The plugin update thread.. -static std::thread gUpdateThread; +// Register SerializedStepMap to the Qt meta type system so we can pass objects +// of this type in QMetaObject::invokeMethod +Q_DECLARE_METATYPE(msgs::SerializedStepMap) ///////////////////////////////////////////////// GuiRunner::GuiRunner(const std::string &_worldName) { + qRegisterMetaType(); + this->setProperty("worldName", QString::fromStdString(_worldName)); auto win = gui::App()->findChild(); @@ -63,29 +59,13 @@ GuiRunner::GuiRunner(const std::string &_worldName) this->RequestState(); // Periodically update the plugins - // \todo(anyone) Move the global variables to GuiRunner::Implementation on v5 - gRunning = true; - gUpdateThread = std::thread([&]() - { - while (gRunning) - { - { - std::lock_guard lock(gUpdateMutex); - this->UpdatePlugins(); - } - // This is roughly a 30Hz update rate. - std::this_thread::sleep_for(std::chrono::milliseconds(33)); - } - }); + QPointer timer = new QTimer(this); + connect(timer, &QTimer::timeout, this, &GuiRunner::UpdatePlugins); + timer->start(33); } ///////////////////////////////////////////////// -GuiRunner::~GuiRunner() -{ - gRunning = false; - if (gUpdateThread.joinable()) - gUpdateThread.join(); -} +GuiRunner::~GuiRunner() = default; ///////////////////////////////////////////////// void GuiRunner::RequestState() @@ -131,8 +111,19 @@ void GuiRunner::OnState(const msgs::SerializedStepMap &_msg) { IGN_PROFILE_THREAD_NAME("GuiRunner::OnState"); IGN_PROFILE("GuiRunner::Update"); + // Since this function may be called from a transport thread, we push the + // OnStateQt function to the queue so that its called from the Qt thread. This + // ensures that only one thread has access to the ecm and updateInfo + // variables. + QMetaObject::invokeMethod(this, "OnStateQt", Qt::QueuedConnection, + Q_ARG(msgs::SerializedStepMap, _msg)); +} - std::lock_guard lock(gUpdateMutex); +///////////////////////////////////////////////// +void GuiRunner::OnStateQt(const msgs::SerializedStepMap &_msg) +{ + IGN_PROFILE_THREAD_NAME("Qt thread"); + IGN_PROFILE("GuiRunner::Update"); this->ecm.SetState(_msg.state()); // Update all plugins diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 2d926c4fed..a628836956 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -431,12 +431,7 @@ void ComponentInspector::Update(const UpdateInfo &, // Add component to list else { - // TODO(louise) Blocking here is not the best idea - QMetaObject::invokeMethod(&this->dataPtr->componentsModel, - "AddComponentType", - Qt::BlockingQueuedConnection, - Q_RETURN_ARG(QStandardItem *, item), - Q_ARG(ignition::gazebo::ComponentTypeId, typeId)); + item = this->dataPtr->componentsModel.AddComponentType(typeId); } if (nullptr == item) diff --git a/src/gui/plugins/joint_position_controller/JointPositionController.cc b/src/gui/plugins/joint_position_controller/JointPositionController.cc index 0a7bc08e59..132d7b3488 100644 --- a/src/gui/plugins/joint_position_controller/JointPositionController.cc +++ b/src/gui/plugins/joint_position_controller/JointPositionController.cc @@ -241,12 +241,7 @@ void JointPositionController::Update(const UpdateInfo &, // Add joint to list else { - // TODO(louise) Blocking here is not the best idea - QMetaObject::invokeMethod(&this->dataPtr->jointsModel, - "AddJoint", - Qt::BlockingQueuedConnection, - Q_RETURN_ARG(QStandardItem *, item), - Q_ARG(Entity, jointEntity)); + item = this->dataPtr->jointsModel.AddJoint(jointEntity); newItem = true; } From 9d0dfc379db7067f30c1ff8ff288e33b630ac5b0 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Mon, 18 Oct 2021 19:36:30 +0200 Subject: [PATCH 15/22] Prepare release 3.10.0 (#1102) * Prepare release 3.10.0 Signed-off-by: Jose Luis Rivero --- CMakeLists.txt | 2 +- Changelog.md | 31 ++++++++++++++++++++++++++++++- 2 files changed, 31 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0bc4b5193f..b360708c5f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-gazebo3 VERSION 3.9.0) +project(ignition-gazebo3 VERSION 3.10.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 5ea674b9d8..d097e87838 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,7 +1,36 @@ ## Ignition Gazebo 3.x -### Ignition Gazebo 3.X.X (202X-XX-XX) +### Ignition Gazebo 3.10.0 (2021-10-15) +1. Performance: use std::unordered_map where possible in SceneManager + * [Pull request #1083](https://github.com/ignitionrobotics/ign-gazebo/pull/1083) + +1. Enable new CMake policy to fix protobuf compilation + * [Pull request #1059](https://github.com/ignitionrobotics/ign-gazebo/pull/1059) + +1. Fix setting cast_shadows for visuals without material + * [Pull request #1015](https://github.com/ignitionrobotics/ign-gazebo/pull/1015) + +1. Remove duplicate XML tag in pendulum_links example world + * [Pull request #1002](https://github.com/ignitionrobotics/ign-gazebo/pull/1002) + +1. Enable sensor metrics on example worlds + * [Pull request #982](https://github.com/ignitionrobotics/ign-gazebo/pull/982) + +1. Improved doxygen + * [Pull request #996](https://github.com/ignitionrobotics/ign-gazebo/pull/996) + +1. JointPositionController: Improve misleading error message + * [Pull request #1098](https://github.com/ignitionrobotics/ign-gazebo/pull/1098) + +1. Adjust pose decimals based on element width + * [Pull request #1089](https://github.com/ignitionrobotics/ign-gazebo/pull/1089) + +1. Fixed IMU system plugin + * [Pull request #1043](https://github.com/ignitionrobotics/ign-gazebo/pull/1043) + +1. Use QTimer to update plugins in the Qt thread + * [Pull request #1095](https://github.com/ignitionrobotics/ign-gazebo/pull/1095) ### Ignition Gazebo 3.9.0 (2021-08-16) From 6362baed940c6fd9578e38d5abce7e4ca782d2f2 Mon Sep 17 00:00:00 2001 From: Hill Ma Date: Mon, 18 Oct 2021 20:35:11 -0700 Subject: [PATCH 16/22] Better protect this->dataPtr->initialized with renderMutex. (#1119) Signed-off-by: Hill Ma Co-authored-by: Ian Chen --- src/systems/sensors/Sensors.cc | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index a20b5c3280..2a6e6b0c40 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -417,6 +417,7 @@ void Sensors::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { IGN_PROFILE("Sensors::Update"); + std::unique_lock lock(this->dataPtr->renderMutex); if (this->dataPtr->running && this->dataPtr->initialized) { this->dataPtr->renderUtil.UpdateECM(_info, _ecm); @@ -437,17 +438,19 @@ void Sensors::PostUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } - if (!this->dataPtr->initialized && - (_ecm.HasComponentType(components::Camera::typeId) || - _ecm.HasComponentType(components::DepthCamera::typeId) || - _ecm.HasComponentType(components::GpuLidar::typeId) || - _ecm.HasComponentType(components::RgbdCamera::typeId) || - _ecm.HasComponentType(components::ThermalCamera::typeId))) + { - igndbg << "Initialization needed" << std::endl; std::unique_lock lock(this->dataPtr->renderMutex); - this->dataPtr->doInit = true; - this->dataPtr->renderCv.notify_one(); + if (!this->dataPtr->initialized && + (_ecm.HasComponentType(components::Camera::typeId) || + _ecm.HasComponentType(components::DepthCamera::typeId) || + _ecm.HasComponentType(components::GpuLidar::typeId) || + _ecm.HasComponentType(components::RgbdCamera::typeId) || + _ecm.HasComponentType(components::ThermalCamera::typeId))) { + igndbg << "Initialization needed" << std::endl; + this->dataPtr->doInit = true; + this->dataPtr->renderCv.notify_one(); + } } if (this->dataPtr->running && this->dataPtr->initialized) From 3a1adb3152f873348ad5fd4e014e11268e281842 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 20 Oct 2021 08:39:45 -0700 Subject: [PATCH 17/22] Use the actor tension parameter (#1091) * Use the actor tension parameter Signed-off-by: Nate Koenig * bump to ign-common 4.4 Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- CMakeLists.txt | 2 +- examples/worlds/actor.sdf | 54 +++++++++++++++++++++++++++++++++++ src/rendering/SceneManager.cc | 2 +- 3 files changed, 56 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 69582af7f0..fd853cb328 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,7 +68,7 @@ set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) #-------------------------------------- # Find ignition-common # Always use the profiler component to get the headers, regardless of status. -ign_find_package(ignition-common4 VERSION 4.2 +ign_find_package(ignition-common4 VERSION 4.4 COMPONENTS profiler events diff --git a/examples/worlds/actor.sdf b/examples/worlds/actor.sdf index 2c8c3c5679..fc6af99daa 100644 --- a/examples/worlds/actor.sdf +++ b/examples/worlds/actor.sdf @@ -165,5 +165,59 @@ + + + https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae + 1.0 + + + https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae + true + + + + diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 42ed3d82e9..dae751a09a 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -894,7 +894,7 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, static_cast(point->Time()*1000))); waypoints[pointTp] = point->Pose(); } - trajInfo.SetWaypoints(waypoints); + trajInfo.SetWaypoints(waypoints, trajSdf->Tension()); // Animations are offset by 1 because index 0 is taken by the mesh name auto animation = _actor.AnimationByIndex(trajInfo.AnimIndex()-1); From 0640db99265a0bfcb9f5066fb90e1cdddf67bf36 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 20 Oct 2021 10:24:21 -0700 Subject: [PATCH 18/22] Updates to camera video record from subt (#1117) * Updates to camera video record from subt Signed-off-by: Nate Koenig * Fix build Signed-off-by: Nate Koenig * Added documentation Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- .../camera_video_recorder/CMakeLists.txt | 1 + .../CameraVideoRecorder.cc | 150 ++++++++++++++++-- .../CameraVideoRecorder.hh | 19 ++- 3 files changed, 150 insertions(+), 20 deletions(-) diff --git a/src/systems/camera_video_recorder/CMakeLists.txt b/src/systems/camera_video_recorder/CMakeLists.txt index 0518888b9c..1d07b852d9 100644 --- a/src/systems/camera_video_recorder/CMakeLists.txt +++ b/src/systems/camera_video_recorder/CMakeLists.txt @@ -4,4 +4,5 @@ gz_add_system(camera-video-recorder PUBLIC_LINK_LIBS ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + ignition-gazebo${PROJECT_VERSION_MAJOR}-rendering ) diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.cc b/src/systems/camera_video_recorder/CameraVideoRecorder.cc index 22fadcacee..6918036299 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.cc +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.cc @@ -30,7 +30,9 @@ #include #include +#include "ignition/gazebo/rendering/RenderUtil.hh" #include "ignition/gazebo/rendering/Events.hh" +#include "ignition/gazebo/rendering/MarkerManager.hh" #include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/Model.hh" @@ -108,6 +110,33 @@ class ignition::gazebo::systems::CameraVideoRecorderPrivate /// \brief Topic that the sensor publishes to public: std::string sensorTopic; + + /// \brief Video recording statistics publisher + public: transport::Node::Publisher recorderStatsPub; + + /// \brief Start time of video recording. + public: std::chrono::steady_clock::time_point recordStartTime; + + /// \brief Current simulation time. + public: std::chrono::steady_clock::duration simTime{0}; + + /// \brief Use sim time as timestamp during video recording + /// By default (false), video encoding is done using real time. + public: bool recordVideoUseSimTime = false; + + /// \brief Video recorder bitrate (bps). This is rougly 2Mbps which + /// produces decent video quality while not generating overly large + /// video files. + /// + /// Another point of reference is at: + /// https://support.google.com/youtube/answer/1722171?hl=en#zippy=%2Cbitrate + public: unsigned int recordVideoBitrate = 2070000; + + /// \brief Recording frames per second. + public: unsigned int fps = 25; + + /// \brief Marker manager + public: MarkerManager markerManager; }; ////////////////////////////////////////////////// @@ -200,7 +229,13 @@ void CameraVideoRecorder::Configure( // video recorder service topic name if (_sdf->HasElement("service")) { - this->dataPtr->service = _sdf->Get("service"); + this->dataPtr->service = transport::TopicUtils::AsValidTopic( + _sdf->Get("service")); + if (this->dataPtr->service.empty()) + { + ignerr << "Service [" << _sdf->Get("service") + << "] not valid. Ignoring." << std::endl; + } } this->dataPtr->eventMgr = &_eventMgr; @@ -208,8 +243,33 @@ void CameraVideoRecorder::Configure( sdf::Sensor sensorSdf = cameraEntComp->Data(); std::string topic = sensorSdf.Topic(); if (topic.empty()) - topic = scopedName(_entity, _ecm) + "/image"; + { + auto scoped = scopedName(_entity, _ecm); + topic = transport::TopicUtils::AsValidTopic(scoped + "/image"); + if (topic.empty()) + { + ignerr << "Failed to generate valid topic for entity [" << scoped + << "]" << std::endl; + } + } this->dataPtr->sensorTopic = topic; + + // Get whether sim time should be used for recording. + this->dataPtr->recordVideoUseSimTime = _sdf->Get("use_sim_time", + this->dataPtr->recordVideoUseSimTime).first; + + // Get video recoder bitrate param + this->dataPtr->recordVideoBitrate = _sdf->Get("bitrate", + this->dataPtr->recordVideoBitrate).first; + + this->dataPtr->fps = _sdf->Get("fps", this->dataPtr->fps).first; + + // recorder stats topic + std::string recorderStatsTopic = this->dataPtr->sensorTopic + "/stats"; + this->dataPtr->recorderStatsPub = + this->dataPtr->node.Advertise(recorderStatsTopic); + ignmsg << "Camera Video recorder stats topic advertised on [" + << recorderStatsTopic << "]" << std::endl; } ////////////////////////////////////////////////// @@ -219,6 +279,8 @@ void CameraVideoRecorderPrivate::OnPostRender() if (!this->scene) { this->scene = rendering::sceneFromFirstRenderEngine(); + this->markerManager.SetTopic(this->sensorTopic + "/marker"); + this->markerManager.Init(this->scene); } // return if scene not ready or no sensors available. @@ -251,6 +313,9 @@ void CameraVideoRecorderPrivate::OnPostRender() std::lock_guard lock(this->updateMutex); + this->markerManager.SetSimTime(this->simTime); + this->markerManager.Update(); + // record video if (this->recordVideo) { @@ -267,8 +332,36 @@ void CameraVideoRecorderPrivate::OnPostRender() if (this->videoEncoder.IsEncoding()) { this->camera->Copy(this->cameraImage); - this->videoEncoder.AddFrame( - this->cameraImage.Data(), width, height); + std::chrono::steady_clock::time_point t; + std::chrono::steady_clock::now(); + if (this->recordVideoUseSimTime) + t = std::chrono::steady_clock::time_point(this->simTime); + else + t = std::chrono::steady_clock::now(); + + bool frameAdded = this->videoEncoder.AddFrame( + this->cameraImage.Data(), width, height, t); + + if (frameAdded) + { + // publish recorder stats + if (this->recordStartTime == + std::chrono::steady_clock::time_point( + std::chrono::duration(std::chrono::seconds(0)))) + { + // start time, i.e. time when first frame is added + this->recordStartTime = t; + } + + std::chrono::steady_clock::duration dt; + dt = t - this->recordStartTime; + int64_t sec, nsec; + std::tie(sec, nsec) = ignition::math::durationToSecNsec(dt); + msgs::Time msg; + msg.set_sec(sec); + msg.set_nsec(nsec); + this->recorderStatsPub.Publish(msg); + } } // Video recorder is idle. Start recording. else @@ -282,7 +375,11 @@ void CameraVideoRecorderPrivate::OnPostRender() &CameraVideoRecorderPrivate::OnImage, this); this->videoEncoder.Start(this->recordVideoFormat, - this->tmpVideoFilename, width, height); + this->tmpVideoFilename, width, height, this->fps, + this->recordVideoBitrate); + + this->recordStartTime = std::chrono::steady_clock::time_point( + std::chrono::duration(std::chrono::seconds(0))); ignmsg << "Start video recording on [" << this->service << "]. " << "Encoding to tmp file: [" @@ -298,18 +395,31 @@ void CameraVideoRecorderPrivate::OnPostRender() // stop encoding this->videoEncoder.Stop(); - // move the tmp video file to user specified path + ignmsg << "Stop video recording on [" << this->service << "]." << std::endl; + if (common::exists(this->tmpVideoFilename)) { - common::moveFile(this->tmpVideoFilename, - this->recordVideoSavePath); + std::string parentPath = common::parentPath(this->recordVideoSavePath); - // Remove old temp file, if it exists. - std::remove(this->tmpVideoFilename.c_str()); + // move the tmp video file to user specified path + if (parentPath != this->recordVideoSavePath && + !common::exists(parentPath) && !common::createDirectory(parentPath)) + { + ignerr << "Unable to create directory[" << parentPath + << "]. Video file[" << this->tmpVideoFilename + << "] will not be moved." << std::endl; + } + else + { + common::moveFile(this->tmpVideoFilename, this->recordVideoSavePath); + + // Remove old temp file, if it exists. + std::remove(this->tmpVideoFilename.c_str()); + + ignmsg << "Saving tmp video[" << this->tmpVideoFilename << "] file to [" + << this->recordVideoSavePath << "]" << std::endl; + } } - ignmsg << "Stop video recording on [" << this->service << "]. " - << "Saving file to: [" << this->recordVideoSavePath << "]" - << std::endl; // reset the event connection to prevent unnecessary render callbacks this->postRenderConn.reset(); @@ -317,9 +427,10 @@ void CameraVideoRecorderPrivate::OnPostRender() } ////////////////////////////////////////////////// -void CameraVideoRecorder::PostUpdate(const UpdateInfo &, +void CameraVideoRecorder::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) { + this->dataPtr->simTime = _info.simTime; if (!this->dataPtr->cameraName.empty()) return; @@ -332,8 +443,15 @@ void CameraVideoRecorder::PostUpdate(const UpdateInfo &, if (this->dataPtr->service.empty()) { - this->dataPtr->service = scopedName(this->dataPtr->entity, _ecm) + - "/record_video"; + auto scoped = scopedName(this->dataPtr->entity, _ecm); + this->dataPtr->service = transport::TopicUtils::AsValidTopic(scoped + + "/record_video"); + if (this->dataPtr->service.empty()) + { + ignerr << "Failed to create valid service for [" << scoped << "]" + << std::endl; + } + return; } this->dataPtr->node.Advertise(this->dataPtr->service, diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.hh b/src/systems/camera_video_recorder/CameraVideoRecorder.hh index 90c4dfdb06..f5f3dc02c1 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.hh +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.hh @@ -36,10 +36,21 @@ namespace systems **/ /// \brief Record video from a camera sensor /// The system takes in the following parameter: - /// - `` Name of topic for the video recorder service. If this is - /// not specified, the topic defaults to:
- /// `/world//link//` - /// `sensor//record_video` + /// Name of topic for the video recorder service. If this is + /// not specified, the topic defaults to: + /// /world//link// + /// sensor//record_video + /// + /// True/false value that specifies if the video should + /// be recorded using simulation time or real time. The + /// default is false, which indicates the use of real + /// time. + /// + /// Video recorder frames per second. The default value is 25, and + /// the support type is unsigned int. + /// + /// Video recorder bitrate (bps). The default value is + /// 2070000 bps, and the supported type is unsigned int. class CameraVideoRecorder: public System, public ISystemConfigure, From 1b4c9a0d2dc4b4ae0d43e8878d8a8e6a85c40b2f Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 20 Oct 2021 15:41:35 -0700 Subject: [PATCH 19/22] Fix codecheck Signed-off-by: Nate Koenig --- .../visualization_capabilities/VisualizationCapabilities.cc | 2 +- src/rendering/RenderUtil.cc | 2 +- src/systems/sensors/Sensors.cc | 2 +- test/integration/imu_system.cc | 2 -- 4 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc index b7031d0a53..d4f7f9175c 100644 --- a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc @@ -1962,7 +1962,7 @@ void VisualizationCapabilitiesPrivate::FindJointModels( std::stack modelStack; modelStack.push(entity); - std::vector childLinks, childModels; + std::vector childModels; while (!modelStack.empty()) { Entity model = modelStack.top(); diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index c233360e5e..1ea5640e79 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -776,7 +776,7 @@ void RenderUtilPrivate::FindJointModels(const EntityComponentManager &_ecm) std::stack modelStack; modelStack.push(entity); - std::vector childLinks, childModels; + std::vector childModels; while (!modelStack.empty()) { Entity model = modelStack.top(); diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index bca326e022..9f5b0f434b 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -472,7 +472,7 @@ void Sensors::PostUpdate(const UpdateInfo &_info, _ecm.HasComponentType(components::GpuLidar::typeId) || _ecm.HasComponentType(components::RgbdCamera::typeId) || _ecm.HasComponentType(components::ThermalCamera::typeId) || - _ecm.HasComponentType(components::SegmentationCamera::typeId))) + _ecm.HasComponentType(components::SegmentationCamera::typeId))) { igndbg << "Initialization needed" << std::endl; this->dataPtr->doInit = true; diff --git a/test/integration/imu_system.cc b/test/integration/imu_system.cc index d4c91b1ac8..3cdd522c8e 100644 --- a/test/integration/imu_system.cc +++ b/test/integration/imu_system.cc @@ -223,8 +223,6 @@ TEST_F(ImuTest, OrientationDisabled) EXPECT_FALSE(server.Running()); EXPECT_FALSE(*server.Running(0)); - const std::string sensorName = "imu_sensor"; - auto topic = "world/imu_sensor/model/imu_model/link/link/sensor/imu_sensor/imu"; From a2abed818def48b324e35ca3b70b3b09e2b88b2f Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Fri, 22 Oct 2021 09:19:22 -0600 Subject: [PATCH 20/22] fix updating a component's data via SerializedState msg (#1131) Signed-off-by: Ashton Larkin Co-authored-by: Nate Koenig --- src/EntityComponentManager.cc | 48 +++++---------- src/EntityComponentManager_TEST.cc | 96 ++++++++++++++++++++++++++++++ 2 files changed, 112 insertions(+), 32 deletions(-) diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 814be6a0ce..1b261294d0 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -1652,53 +1653,36 @@ void EntityComponentManager::SetState( continue; } - // Create component - auto newComp = components::Factory::Instance()->New(compMsg.type()); - - if (nullptr == newComp) - { - ignerr << "Failed to deserialize component of type [" << compMsg.type() - << "]" << std::endl; - continue; - } - - std::istringstream istr(compMsg.component()); - newComp->Deserialize(istr); - - // Get type id - auto typeId = newComp->TypeId(); - - // TODO(louise) Move into if, see TODO below - this->RemoveComponent(entity, typeId); - // Remove component if (compMsg.remove()) { + this->RemoveComponent(entity, type); continue; } // Get Component - auto comp = this->ComponentImplementation(entity, typeId); + auto comp = this->ComponentImplementation(entity, type); // Create if new if (nullptr == comp) { - this->CreateComponentImplementation(entity, typeId, newComp.get()); + auto newComp = components::Factory::Instance()->New(type); + if (nullptr == newComp) + { + ignerr << "Failed to create component type [" + << compMsg.type() << "]" << std::endl; + continue; + } + std::istringstream istr(compMsg.component()); + newComp->Deserialize(istr); + this->CreateComponentImplementation(entity, type, newComp.get()); } // Update component value else { - ignerr << "Internal error" << std::endl; - // TODO(louise) We're shortcutting above and always removing the - // component so that we don't get here, gotta figure out why this - // doesn't update the component. The following line prints the correct - // values. - // igndbg << *comp << " " << *newComp.get() << std::endl; - // *comp = *newComp.get(); - - // When above TODO is addressed, uncomment AddModifiedComponent below - // unless calling SetChanged (which already calls AddModifiedComponent) - // this->dataPtr->AddModifiedComponent(entity); + std::istringstream istr(compMsg.component()); + comp->Deserialize(istr); + this->dataPtr->AddModifiedComponent(entity); } } } diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 1aaefc0fc6..ed45452d66 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -3000,6 +3000,102 @@ TEST_P(EntityComponentManagerFixture, PinnedEntity) EXPECT_EQ(0u, manager.EntityCount()); } +////////////////////////////////////////////////// +/// \brief Test using msgs::SerializedStateMap and msgs::SerializedState +/// to update existing component data between multiple ECMs +TEST_P(EntityComponentManagerFixture, StateMsgUpdateComponent) +{ + // create 2 ECMs: one will be modified directly, and the other should be + // updated to match the first via msgs::SerializedStateMap + EntityComponentManager originalECMStateMap; + EntityComponentManager otherECMStateMap; + + // create an entity and component + auto entity = originalECMStateMap.CreateEntity(); + originalECMStateMap.CreateComponent(entity, components::IntComponent(1)); + + int foundEntities = 0; + otherECMStateMap.Each( + [&](const Entity &, const components::IntComponent *) + { + foundEntities++; + return true; + }); + EXPECT_EQ(0, foundEntities); + + // update the other ECM to have the new entity and component + msgs::SerializedStateMap stateMapMsg; + originalECMStateMap.State(stateMapMsg); + otherECMStateMap.SetState(stateMapMsg); + foundEntities = 0; + otherECMStateMap.Each( + [&](const Entity &, const components::IntComponent *_intComp) + { + foundEntities++; + EXPECT_EQ(1, _intComp->Data()); + return true; + }); + EXPECT_EQ(1, foundEntities); + + // modify a component and then share the update with the other ECM + stateMapMsg.Clear(); + originalECMStateMap.SetComponentData(entity, 2); + originalECMStateMap.State(stateMapMsg); + otherECMStateMap.SetState(stateMapMsg); + foundEntities = 0; + otherECMStateMap.Each( + [&](const Entity &, const components::IntComponent *_intComp) + { + foundEntities++; + EXPECT_EQ(2, _intComp->Data()); + return true; + }); + EXPECT_EQ(1, foundEntities); + + // Run the same test as above, but this time, use a msgs::SerializedState + // instead of a msgs::SerializedStateMap + EntityComponentManager originalECMState; + EntityComponentManager otherECMState; + + foundEntities = 0; + otherECMState.Each( + [&](const Entity &, const components::IntComponent *) + { + foundEntities++; + return true; + }); + EXPECT_EQ(0, foundEntities); + + entity = originalECMState.CreateEntity(); + originalECMState.CreateComponent(entity, components::IntComponent(1)); + + auto stateMsg = originalECMState.State(); + otherECMState.SetState(stateMsg); + foundEntities = 0; + otherECMState.Each( + [&](const Entity &, const components::IntComponent *_intComp) + { + foundEntities++; + EXPECT_EQ(1, _intComp->Data()); + return true; + }); + EXPECT_EQ(1, foundEntities); + + stateMsg.Clear(); + originalECMState.SetComponentData(entity, 2); + stateMsg = originalECMState.State(); + otherECMState.SetState(stateMsg); + foundEntities = 0; + otherECMState.Each( + [&](const Entity &, const components::IntComponent *_intComp) + { + foundEntities++; + EXPECT_EQ(2, _intComp->Data()); + return true; + }); + EXPECT_EQ(1, foundEntities); +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(EntityComponentManagerRepeat, From f7f6e51dfc06ff1295ffbb14a4622673310f51ca Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Fri, 22 Oct 2021 14:22:42 -0600 Subject: [PATCH 21/22] Add support for GUI entity creation (#1122) Signed-off-by: Ashton Larkin --- src/gui/GuiRunner.cc | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index c61264f974..62dbf3baf1 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -73,6 +73,15 @@ GuiRunner::GuiRunner(const std::string &_worldName) this->setProperty("worldName", QString::fromStdString(_worldName)); + // Allow for creation of entities on GUI side. + // Note we have to start the entity id at an offset so it does not conflict + // with the ones on the server. The log playback starts at max / 2 + // On the gui side, we will start entity id at an offset of max / 4 + // todo(anyone) address + // https://github.com/ignitionrobotics/ign-gazebo/issues/1134 + // so that an offset is not required + this->dataPtr->ecm.SetEntityCreateOffset(math::MAX_I64 / 4); + auto win = gui::App()->findChild(); auto winWorldNames = win->property("worldNames").toStringList(); winWorldNames.append(QString::fromStdString(_worldName)); @@ -198,8 +207,6 @@ void GuiRunner::OnStateQt(const msgs::SerializedStepMap &_msg) // Update all plugins this->dataPtr->updateInfo = convert(_msg.stats()); this->UpdatePlugins(); - this->dataPtr->ecm.ClearNewlyCreatedEntities(); - this->dataPtr->ecm.ProcessRemoveEntityRequests(); } ///////////////////////////////////////////////// @@ -211,4 +218,6 @@ void GuiRunner::UpdatePlugins() plugin->Update(this->dataPtr->updateInfo, this->dataPtr->ecm); } this->dataPtr->ecm.ClearRemovedComponents(); + this->dataPtr->ecm.ClearNewlyCreatedEntities(); + this->dataPtr->ecm.ProcessRemoveEntityRequests(); } From 15b664b29ad62dc53fa24e2cb1cd3864352d8cf6 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 25 Oct 2021 10:26:46 -0700 Subject: [PATCH 22/22] Fix custom sensor example (#1132) Signed-off-by: Louise Poubel --- examples/plugin/custom_sensor_system/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/plugin/custom_sensor_system/CMakeLists.txt b/examples/plugin/custom_sensor_system/CMakeLists.txt index aa424980cc..78fa46f941 100644 --- a/examples/plugin/custom_sensor_system/CMakeLists.txt +++ b/examples/plugin/custom_sensor_system/CMakeLists.txt @@ -20,7 +20,7 @@ include(FetchContent) FetchContent_Declare( sensors_clone GIT_REPOSITORY https://github.com/ignitionrobotics/ign-sensors - GIT_TAG main + GIT_TAG ign-sensors6 ) FetchContent_Populate(sensors_clone) add_subdirectory(${sensors_clone_SOURCE_DIR}/examples/custom_sensor ${sensors_clone_BINARY_DIR})