From c4b55bda986d13410737c1d101004dd2f21b2b53 Mon Sep 17 00:00:00 2001 From: shameekganguly Date: Fri, 12 Jul 2024 11:16:30 -0700 Subject: [PATCH 01/15] =?UTF-8?q?Check=20if=20any=20entity=20actually=20ha?= =?UTF-8?q?s=20a=20ContactSensorData=20component=20before=E2=80=A6=20(#247?= =?UTF-8?q?4)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Shameek Ganguly --- src/systems/physics/Physics.cc | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 93a41a3f80..28f78e9032 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -3794,6 +3794,18 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) if (!_ecm.HasComponentType(components::ContactSensorData::typeId)) return; + // Also check if any entity currently has a ContactSensorData component. + bool needContactSensorData = false; + _ecm.Each( + [&](const Entity &/*unused*/, components::Collision *, + components::ContactSensorData */*unused*/) -> bool + { + needContactSensorData = true; + return false; + }); + if (!needContactSensorData) + return; + // TODO(addisu) If systems are assumed to only have one world, we should // capture the world Entity in a Configure call Entity worldEntity = _ecm.EntityByComponents(components::World()); From 6340db16aab5f5ca1832226e8df21b2d04af848c Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 15 Jul 2024 03:25:23 -0500 Subject: [PATCH 02/15] Add UserCommands plugin to GPU lidar sensor example (#2479) Signed-off-by: Addisu Z. Taddese --- examples/worlds/gpu_lidar_sensor.sdf | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/examples/worlds/gpu_lidar_sensor.sdf b/examples/worlds/gpu_lidar_sensor.sdf index 1e1a69cfd9..756de3eb34 100644 --- a/examples/worlds/gpu_lidar_sensor.sdf +++ b/examples/worlds/gpu_lidar_sensor.sdf @@ -22,6 +22,10 @@ filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"> + + true From c721e77a53cb54c3d4d640a038a11ce9b7b11a33 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 25 Jun 2024 23:52:28 +0000 Subject: [PATCH 03/15] backport convex decomposition visualization Signed-off-by: Ian Chen --- src/Conversions.cc | 36 +++++++++++++++++++ src/Conversions_TEST.cc | 19 ++++++++++ src/Util.cc | 32 ++++++++++++++--- .../VisualizationCapabilities.cc | 19 ++++++++-- 4 files changed, 99 insertions(+), 7 deletions(-) diff --git a/src/Conversions.cc b/src/Conversions.cc index a553ac60e6..d5c054ada9 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -220,6 +220,24 @@ msgs::Geometry gz::sim::convert(const sdf::Geometry &_in) meshMsg->set_filename(asFullPath(meshSdf->Uri(), meshSdf->FilePath())); meshMsg->set_submesh(meshSdf->Submesh()); meshMsg->set_center_submesh(meshSdf->CenterSubmesh()); + + if (!meshSdf->OptimizationStr().empty()) + { + auto header = out.mutable_header()->add_data(); + header->set_key("optimization"); + header->add_value(meshSdf->OptimizationStr()); + } + if (meshSdf->ConvexDecomposition()) + { + auto header = out.mutable_header()->add_data(); + header->set_key("max_convex_hulls"); + header->add_value(std::to_string( + meshSdf->ConvexDecomposition()->MaxConvexHulls())); + header = out.mutable_header()->add_data(); + header->set_key("voxel_resolution"); + header->add_value(std::to_string( + meshSdf->ConvexDecomposition()->VoxelResolution())); + } } else if (_in.Type() == sdf::GeometryType::HEIGHTMAP && _in.HeightmapShape()) { @@ -363,6 +381,24 @@ sdf::Geometry gz::sim::convert(const msgs::Geometry &_in) meshShape.SetSubmesh(_in.mesh().submesh()); meshShape.SetCenterSubmesh(_in.mesh().center_submesh()); + sdf::ConvexDecomposition convexDecomp; + for (int i = 0; i < _in.header().data_size(); ++i) + { + auto data = _in.header().data(i); + if (data.key() == "optimization" && data.value_size() > 0) + { + meshShape.SetOptimization(data.value(0)); + } + if (data.key() == "max_convex_hulls" && data.value_size() > 0) + { + convexDecomp.SetMaxConvexHulls(std::stoul(data.value(0))); + } + if (data.key() == "voxel_resolution" && data.value_size() > 0) + { + convexDecomp.SetVoxelResolution(std::stoul(data.value(0))); + } + } + meshShape.SetConvexDecomposition(convexDecomp); out.SetMeshShape(meshShape); } else if (_in.type() == msgs::Geometry::HEIGHTMAP && _in.has_heightmap()) diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 53246e44f4..5d0f5345ba 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -463,6 +463,11 @@ TEST(Conversions, GeometryMesh) meshShape.SetUri("file://watermelon"); meshShape.SetSubmesh("grape"); meshShape.SetCenterSubmesh(true); + meshShape.SetOptimization("convex_decomposition"); + sdf::ConvexDecomposition convexDecomp; + convexDecomp.SetMaxConvexHulls(4); + convexDecomp.SetVoxelResolution(10000); + meshShape.SetConvexDecomposition(convexDecomp); geometry.SetMeshShape(meshShape); auto geometryMsg = convert(geometry); @@ -473,6 +478,15 @@ TEST(Conversions, GeometryMesh) EXPECT_EQ("file://watermelon", geometryMsg.mesh().filename()); EXPECT_EQ("grape", geometryMsg.mesh().submesh()); EXPECT_TRUE(geometryMsg.mesh().center_submesh()); + auto header = geometryMsg.header().data(0); + EXPECT_EQ("optimization", header.key()); + EXPECT_EQ("convex_decomposition", header.value(0)); + header = geometryMsg.header().data(1); + EXPECT_EQ("max_convex_hulls", header.key()); + EXPECT_EQ("4", header.value(0)); + header = geometryMsg.header().data(2); + EXPECT_EQ("voxel_resolution", header.key()); + EXPECT_EQ("10000", header.value(0)); auto newGeometry = convert(geometryMsg); EXPECT_EQ(sdf::GeometryType::MESH, newGeometry.Type()); @@ -481,6 +495,11 @@ TEST(Conversions, GeometryMesh) EXPECT_EQ("file://watermelon", newGeometry.MeshShape()->Uri()); EXPECT_EQ("grape", newGeometry.MeshShape()->Submesh()); EXPECT_TRUE(newGeometry.MeshShape()->CenterSubmesh()); + EXPECT_EQ("convex_decomposition", newGeometry.MeshShape()->OptimizationStr()); + auto newConvexDecomp = newGeometry.MeshShape()->ConvexDecomposition(); + ASSERT_NE(nullptr, newConvexDecomp); + EXPECT_EQ(4, newConvexDecomp->MaxConvexHulls()); + EXPECT_EQ(10000, newConvexDecomp->VoxelResolution()); } ///////////////////////////////////////////////// diff --git a/src/Util.cc b/src/Util.cc index c73ac54dbc..83ce8faf14 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -912,12 +912,36 @@ const common::Mesh *optimizeMesh(const sdf::Mesh &_meshSdf, auto *optimizedMesh = meshManager.MeshByName(convexMeshName); if (!optimizedMesh) { - // Merge meshes before convex decomposition - auto mergedMesh = gz::common::MeshManager::MergeSubMeshes(_mesh); - if (mergedMesh && mergedMesh->SubMeshCount() == 1u) + std::unique_ptr meshToDecompose = + std::make_unique(); + // check if a particular submesh is requested + if (!_meshSdf.Submesh().empty()) + { + for (unsigned int submeshIdx = 0; + submeshIdx < _mesh.SubMeshCount(); + ++submeshIdx) + { + auto submesh = _mesh.SubMeshByIndex(submeshIdx).lock(); + if (submesh->Name() == _meshSdf.Submesh()) + { + if (_meshSdf.CenterSubmesh()) + submesh->Center(math::Vector3d::Zero); + meshToDecompose->AddSubMesh(*submesh.get()); + break; + } + } + } + else + { + // Merge meshes before convex decomposition + meshToDecompose = + gz::common::MeshManager::MergeSubMeshes(_mesh); + } + + if (meshToDecompose && meshToDecompose->SubMeshCount() == 1u) { // Decompose and add mesh to MeshManager - auto mergedSubmesh = mergedMesh->SubMeshByIndex(0u).lock(); + auto mergedSubmesh = meshToDecompose->SubMeshByIndex(0u).lock(); std::vector decomposed = gz::common::MeshManager::ConvexDecomposition( *mergedSubmesh.get(), maxConvexHulls, voxelResolution); diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc index 2694553d54..92ceedbfbb 100644 --- a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -39,6 +40,7 @@ #include #include #include +#include #include #include @@ -1247,14 +1249,25 @@ rendering::GeometryPtr VisualizationCapabilitiesPrivate::CreateGeometry( // Assume absolute path to mesh file descriptor.meshName = fullPath; - descriptor.subMeshName = _geom.MeshShape()->Submesh(); - descriptor.centerSubMesh = _geom.MeshShape()->CenterSubmesh(); - gz::common::MeshManager *meshManager = gz::common::MeshManager::Instance(); descriptor.mesh = meshManager->Load(descriptor.meshName); if (descriptor.mesh) { + if (_geom.MeshShape()->Optimization() != sdf::MeshOptimization::NONE) + { + const common::Mesh *optimizedMesh = + optimizeMesh(*_geom.MeshShape(), *descriptor.mesh); + if (optimizedMesh) + { + descriptor.mesh = optimizedMesh; + // if submesh is requested, it should be handled in the optimizeMesh + // function so we do not need need to pass these flags to + // gz-rendering + descriptor.subMeshName = ""; + descriptor.centerSubMesh = false; + } + } geom = this->scene->CreateMesh(descriptor); } else From dd8c4d78253a4fdce3b698e67d19e4474651ba4b Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 17 Jul 2024 13:12:27 -0700 Subject: [PATCH 04/15] Fix lidar visualization when gz_frame_id is specified (#2481) Signed-off-by: Ian Chen --- .../plugins/visualize_lidar/VisualizeLidar.cc | 133 ++++++++---------- 1 file changed, 55 insertions(+), 78 deletions(-) diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc index 25dd945633..2a2bb721ca 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc @@ -82,9 +82,6 @@ inline namespace GZ_SIM_VERSION_NAMESPACE public: rendering::LidarVisualType visualType{ rendering::LidarVisualType::LVT_TRIANGLE_STRIPS}; - /// \brief URI sequence to the lidar link - public: std::string lidarString{""}; - /// \brief LaserScan message from sensor public: msgs::LaserScan msg; @@ -122,7 +119,7 @@ inline namespace GZ_SIM_VERSION_NAMESPACE public: bool visualDirty{false}; /// \brief lidar sensor entity dirty flag - public: bool lidarEntityDirty{true}; + public: bool lidarEntityDirty{false}; }; } } @@ -264,65 +261,44 @@ void VisualizeLidar::Update(const UpdateInfo &, std::lock_guard lock(this->dataPtr->serviceMutex); + if (this->dataPtr->topicName.empty()) + return; + if (this->dataPtr->lidarEntityDirty) { - auto lidarURIVec = common::split(common::trimmed( - this->dataPtr->lidarString), "::"); - if (lidarURIVec.size() > 0) + std::string topic = this->dataPtr->topicName; + auto lidarEnt = + _ecm.EntityByComponents(components::SensorTopic(topic)); + if (lidarEnt == kNullEntity) { - auto baseEntity = _ecm.EntityByComponents( - components::Name(lidarURIVec[0])); - if (!baseEntity) - { - gzerr << "Error entity " << lidarURIVec[0] - << " doesn't exist and cannot be used to set lidar visual pose" - << std::endl; - return; - } - else + if (topic[0] == '/') + topic = topic.substr(1); + lidarEnt = + _ecm.EntityByComponents(components::SensorTopic(topic)); + } + + static bool informed{false}; + if (lidarEnt == kNullEntity) + { + if (!informed) { - auto parent = baseEntity; - bool success = false; - for (size_t i = 0u; i < lidarURIVec.size()-1; i++) - { - auto children = _ecm.EntitiesByComponents( - components::ParentEntity(parent)); - bool foundChild = false; - for (auto child : children) - { - std::string nextstring = lidarURIVec[i+1]; - auto comp = _ecm.Component(child); - if (!comp) - { - continue; - } - std::string childname = std::string( - comp->Data()); - if (nextstring.compare(childname) == 0) - { - parent = child; - foundChild = true; - if (i+1 == lidarURIVec.size()-1) - { - success = true; - } - break; - } - } - if (!foundChild) - { - gzerr << "The entity could not be found." - << "Error displaying lidar visual" <dataPtr->lidarEntity = parent; - this->dataPtr->lidarEntityDirty = false; - } + gzerr << "The lidar entity with topic '['" << this->dataPtr->topicName + << "'] could not be found. " + << "Error displaying lidar visual. " << std::endl; + informed = true; } + return; } + informed = false; + this->dataPtr->lidarEntity = lidarEnt; + this->dataPtr->lidarEntityDirty = false; + } + + if (!_ecm.HasEntity(this->dataPtr->lidarEntity)) + { + this->dataPtr->resetVisual = true; + this->dataPtr->topicName = ""; + return; } // Only update lidarPose if the lidarEntity exists and the lidar is @@ -332,7 +308,7 @@ void VisualizeLidar::Update(const UpdateInfo &, // data arrives, the visual is offset from the obstacle if the sensor is // moving fast. if (!this->dataPtr->lidarEntityDirty && this->dataPtr->initialized && - !this->dataPtr->visualDirty) + !this->dataPtr->visualDirty) { this->dataPtr->lidarPose = worldPose(this->dataPtr->lidarEntity, _ecm); } @@ -379,13 +355,17 @@ void VisualizeLidar::UpdateSize(int _size) ////////////////////////////////////////////////// void VisualizeLidar::OnTopic(const QString &_topicName) { + std::string topic = _topicName.toStdString(); + if (this->dataPtr->topicName == topic) + return; + if (!this->dataPtr->topicName.empty() && !this->dataPtr->node.Unsubscribe(this->dataPtr->topicName)) { gzerr << "Unable to unsubscribe from topic [" << this->dataPtr->topicName <<"]" <dataPtr->topicName = _topicName.toStdString(); + this->dataPtr->topicName = topic; std::lock_guard lock(this->dataPtr->serviceMutex); // Reset visualization @@ -401,6 +381,8 @@ void VisualizeLidar::OnTopic(const QString &_topicName) } this->dataPtr->visualDirty = false; gzmsg << "Subscribed to " << this->dataPtr->topicName << std::endl; + + this->dataPtr->lidarEntityDirty = true; } ////////////////////////////////////////////////// @@ -489,27 +471,22 @@ void VisualizeLidar::OnScan(const msgs::LaserScan &_msg) this->dataPtr->msg.ranges().begin(), this->dataPtr->msg.ranges().end())); - this->dataPtr->visualDirty = true; - - for (auto data_values : this->dataPtr->msg.header().data()) + if (!math::equal(this->dataPtr->maxVisualRange, + this->dataPtr->msg.range_max())) { - if (data_values.key() == "frame_id") - { - if (this->dataPtr->lidarString.compare( - common::trimmed(data_values.value(0))) != 0) - { - this->dataPtr->lidarString = common::trimmed(data_values.value(0)); - this->dataPtr->lidarEntityDirty = true; - this->dataPtr->maxVisualRange = this->dataPtr->msg.range_max(); - this->dataPtr->minVisualRange = this->dataPtr->msg.range_min(); - this->dataPtr->lidar->SetMaxRange(this->dataPtr->maxVisualRange); - this->dataPtr->lidar->SetMinRange(this->dataPtr->minVisualRange); - this->MinRangeChanged(); - this->MaxRangeChanged(); - break; - } - } + this->dataPtr->maxVisualRange = this->dataPtr->msg.range_max(); + this->dataPtr->lidar->SetMaxRange(this->dataPtr->maxVisualRange); + this->MaxRangeChanged(); + } + if (!math::equal(this->dataPtr->minVisualRange, + this->dataPtr->msg.range_min())) + { + this->dataPtr->minVisualRange = this->dataPtr->msg.range_min(); + this->dataPtr->lidar->SetMinRange(this->dataPtr->minVisualRange); + this->MinRangeChanged(); } + + this->dataPtr->visualDirty = true; } } From 5af9056ac4763c8ea0a9a0ec6514bd968c7b2d5b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 9 Jul 2024 17:54:08 +0800 Subject: [PATCH 05/15] Fix warnings generated by NetworkConfigTest (#2469) This test was generating a warning about unused vairables. Unless built in debug mode, `aserts`are often optimized out we should be using the `ASSERT` macros from `gtest` instead. Signed-off-by: Arjo Chakravarty --- src/network/NetworkConfig_TEST.cc | 20 ++++++++------------ 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/src/network/NetworkConfig_TEST.cc b/src/network/NetworkConfig_TEST.cc index af6baffc61..4014518033 100644 --- a/src/network/NetworkConfig_TEST.cc +++ b/src/network/NetworkConfig_TEST.cc @@ -15,6 +15,7 @@ * */ + #include #include @@ -30,38 +31,33 @@ TEST(NetworkManager, ValueConstructor) { // Primary without number of secondaries is invalid auto config = NetworkConfig::FromValues("PRIMARY", 0); - assert(config.role == NetworkRole::None); - assert(config.numSecondariesExpected == 0); + ASSERT_EQ(config.role, NetworkRole::None); + ASSERT_EQ(config.numSecondariesExpected, 0); // Expect console warning as well - (void) config; } { // Primary with number of secondaries is valid auto config = NetworkConfig::FromValues("PRIMARY", 3); - assert(config.role == NetworkRole::SimulationPrimary); - assert(config.numSecondariesExpected == 3); - (void) config; + ASSERT_EQ(config.role, NetworkRole::SimulationPrimary); + ASSERT_EQ(config.numSecondariesExpected, 3); } { // Secondary is always valid auto config = NetworkConfig::FromValues("SECONDARY", 0); - assert(config.role == NetworkRole::SimulationSecondary); - (void) config; + ASSERT_EQ(config.role, NetworkRole::SimulationSecondary); } { // Readonly is always valid auto config = NetworkConfig::FromValues("READONLY"); - assert(config.role == NetworkRole::ReadOnly); - (void) config; + ASSERT_EQ(config.role, NetworkRole::ReadOnly); } { // Anything else is invalid auto config = NetworkConfig::FromValues("READ_WRITE"); - assert(config.role == NetworkRole::None); - (void) config; + ASSERT_EQ(config.role, NetworkRole::None); } } From d0c0322c84bd0c4ebcf6eaf8547bdda7883caced Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 18 Jul 2024 01:39:40 +0800 Subject: [PATCH 06/15] Remove systems if their parent entity is removed (#2232) n particular if a user despawns an entity, the associated plugin gets removed. This should prevent issues like #2165. TBH I'm not sure if this is the right way forward as a system should technically be able to access any entity in a traditional ECS. The PR has now been reworked and greatly simplified. All we do is stop all worker threads if an entity is removed and then recreate remaining threads. --- include/gz/sim/EntityComponentManager.hh | 3 + src/SimulationRunner.cc | 10 +- src/SimulationRunner.hh | 3 + src/SimulationRunner_TEST.cc | 17 ++- src/SystemManager.cc | 133 +++++++++++++++++++++++ src/SystemManager.hh | 8 ++ src/SystemManager_TEST.cc | 65 +++++++++++ 7 files changed, 231 insertions(+), 8 deletions(-) diff --git a/include/gz/sim/EntityComponentManager.hh b/include/gz/sim/EntityComponentManager.hh index b87ada778c..7fff308c76 100644 --- a/include/gz/sim/EntityComponentManager.hh +++ b/include/gz/sim/EntityComponentManager.hh @@ -828,6 +828,9 @@ namespace gz friend class GuiRunner; friend class SimulationRunner; + // Make SystemManager friend so it has access to removals + friend class SystemManager; + // Make network managers friends so they have control over component // states. Like the runners, the managers are internal. friend class NetworkManagerPrimary; diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index a5ba51e96c..87f72d1ce2 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -33,6 +33,7 @@ #include #include +#include #include "gz/common/Profiler.hh" #include "gz/sim/components/Model.hh" @@ -533,12 +534,15 @@ void SimulationRunner::ProcessSystemQueue() { auto pending = this->systemMgr->PendingCount(); - if (0 == pending) + if (0 == pending && !this->threadsNeedCleanUp) return; - // If additional systems are to be added, stop the existing threads. + // If additional systems are to be added or have been removed, + // stop the existing threads. this->StopWorkerThreads(); + this->threadsNeedCleanUp = false; + this->systemMgr->ActivatePendingSystems(); unsigned int threadCount = @@ -922,6 +926,8 @@ void SimulationRunner::Step(const UpdateInfo &_info) this->ProcessRecreateEntitiesCreate(); // Process entity removals. + this->systemMgr->ProcessRemovedEntities(this->entityCompMgr, + this->threadsNeedCleanUp); this->entityCompMgr.ProcessRemoveEntityRequests(); // Process components removals diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 438fc329ba..7230ed9b86 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -543,6 +543,9 @@ namespace gz /// at the appropriate time. private: std::unique_ptr newWorldControlState; + /// \brief Set if we need to remove systems due to entity removal + private: bool threadsNeedCleanUp; + private: bool resetInitiated{false}; friend class LevelManager; }; diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 10ca3a9406..a127383b8a 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -16,7 +16,6 @@ */ #include - #include #include @@ -111,7 +110,6 @@ void rootClockCb(const msgs::Clock &_msg) rootClockMsgs.push_back(_msg); } - ///////////////////////////////////////////////// TEST_P(SimulationRunnerTest, CreateEntities) { @@ -1484,8 +1482,7 @@ TEST_P(SimulationRunnerTest, EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(sphereEntity, componentId)) << componentId; - // Remove entities that have plugin - this is not unloading or destroying - // the plugin though! + // Remove entities that have plugin auto entityCount = runner.EntityCompMgr().EntityCount(); const_cast( runner.EntityCompMgr()).RequestRemoveEntity(boxEntity); @@ -1533,8 +1530,16 @@ TEST_P(SimulationRunnerTest, SimulationRunner runner(rootWithout.WorldByIndex(0), systemLoader, serverConfig); - // 1 model plugin from SDF and 2 world plugins from config - ASSERT_EQ(3u, runner.SystemCount()); + // 1 model plugin from SDF and 1 world plugin from config + // and 1 model plugin from theconfig + EXPECT_EQ(3u, runner.SystemCount()); + runner.SetPaused(false); + runner.Run(1); + + // Remove the model. Only 1 world plugin should remain. + EXPECT_TRUE(runner.RequestRemoveEntity("box")); + runner.Run(2); + EXPECT_EQ(1u, runner.SystemCount()); } ///////////////////////////////////////////////// diff --git a/src/SystemManager.cc b/src/SystemManager.cc index fd43f5330d..d1b66346e3 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -16,10 +16,13 @@ */ #include +#include #include +#include #include +#include "SystemInternal.hh" #include "gz/sim/components/SystemPluginInfo.hh" #include "gz/sim/Conversions.hh" #include "SystemManager.hh" @@ -122,7 +125,9 @@ size_t SystemManager::ActivatePendingSystems() this->systemsUpdate.push_back(system.update); if (system.postupdate) + { this->systemsPostupdate.push_back(system.postupdate); + } } this->pendingSystems.clear(); @@ -409,3 +414,131 @@ void SystemManager::ProcessPendingEntitySystems() } this->systemsToAdd.clear(); } + +template +struct identity +{ + using type = T; +}; + +////////////////////////////////////////////////// +/// TODO(arjo) - When we move to C++20 we can just use erase_if +/// Removes all items that match the given predicate. +/// This function runs in O(n) time and uses memory in-place +template +void RemoveFromVectorIf(std::vector& vec, + typename identity>::type pred) +{ + vec.erase(std::remove_if(vec.begin(), vec.end(), pred), vec.end()); +} + +////////////////////////////////////////////////// +void SystemManager::ProcessRemovedEntities( + const EntityComponentManager &_ecm, + bool &_needsCleanUp) +{ + // Note: This function has O(n) time when an entity is removed + // where n is number of systems. Ideally we would only iterate + // over entities marked for removal but that would involve having + // a key value map. This can be marked as a future improvement. + if (!_ecm.HasEntitiesMarkedForRemoval()) + { + return; + } + + std::unordered_set resetSystemsToBeRemoved; + std::unordered_set preupdateSystemsToBeRemoved; + std::unordered_set updateSystemsToBeRemoved; + std::unordered_set postupdateSystemsToBeRemoved; + std::unordered_set configureSystemsToBeRemoved; + std::unordered_set + configureParametersSystemsToBeRemoved; + for (const auto &system : this->systems) + { + if (_ecm.IsMarkedForRemoval(system.parentEntity)) + { + if (system.reset) + { + resetSystemsToBeRemoved.insert(system.reset); + } + if (system.preupdate) + { + preupdateSystemsToBeRemoved.insert(system.preupdate); + } + if (system.update) + { + updateSystemsToBeRemoved.insert(system.update); + } + if (system.postupdate) + { + postupdateSystemsToBeRemoved.insert(system.postupdate); + // If system with a PostUpdate is marked for removal + // mark all worker threads for removal. + _needsCleanUp = true; + } + if (system.configure) + { + configureSystemsToBeRemoved.insert(system.configure); + } + if (system.configureParameters) + { + configureParametersSystemsToBeRemoved.insert( + system.configureParameters); + } + } + } + + RemoveFromVectorIf(this->systemsReset, + [&](const auto system) { + if (resetSystemsToBeRemoved.count(system)) { + return true; + } + return false; + }); + RemoveFromVectorIf(this->systemsPreupdate, + [&](const auto& system) { + if (preupdateSystemsToBeRemoved.count(system)) { + return true; + } + return false; + }); + RemoveFromVectorIf(this->systemsUpdate, + [&](const auto& system) { + if (updateSystemsToBeRemoved.count(system)) { + return true; + } + return false; + }); + + RemoveFromVectorIf(this->systemsPostupdate, + [&](const auto& system) { + if (postupdateSystemsToBeRemoved.count(system)) { + return true; + } + return false; + }); + RemoveFromVectorIf(this->systemsConfigure, + [&](const auto& system) { + if (configureSystemsToBeRemoved.count(system)) { + return true; + } + return false; + }); + RemoveFromVectorIf(this->systemsConfigureParameters, + [&](const auto& system) { + if (configureParametersSystemsToBeRemoved.count(system)) { + return true; + } + return false; + }); + RemoveFromVectorIf(this->systems, + [&](const SystemInternal& _arg) { + return _ecm.IsMarkedForRemoval(_arg.parentEntity); + }); + + std::lock_guard lock(this->pendingSystemsMutex); + RemoveFromVectorIf(this->pendingSystems, + [&](const SystemInternal& _system) { + return _ecm.IsMarkedForRemoval(_system.parentEntity); + }); +} diff --git a/src/SystemManager.hh b/src/SystemManager.hh index acd82c09dc..d523c4e740 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -145,6 +145,14 @@ namespace gz /// \brief Process system messages and add systems to entities public: void ProcessPendingEntitySystems(); + /// \brief Remove systems that are attached to removed entities + /// \param[in] _entityCompMgr - ECM with entities marked for removal + /// \param[out] _needsCleanUp - Set to true if a system with a + /// PostUpdate was removed, and its thread needs to be terminated + public: void ProcessRemovedEntities( + const EntityComponentManager &_entityCompMgr, + bool &_needsCleanUp); + /// \brief Implementation for AddSystem functions that takes an SDF /// element. This calls the AddSystemImpl that accepts an SDF Plugin. /// \param[in] _system Generic representation of a system. diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc index 5026842a9f..1265cc5a5f 100644 --- a/src/SystemManager_TEST.cc +++ b/src/SystemManager_TEST.cc @@ -214,6 +214,71 @@ TEST(SystemManager, AddSystemEcm) EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size()); } +///////////////////////////////////////////////// +TEST(SystemManager, AddAndRemoveSystemEcm) +{ + auto loader = std::make_shared(); + + auto ecm = EntityComponentManager(); + auto eventManager = EventManager(); + + auto paramRegistry = std::make_unique< + gz::transport::parameters::ParametersRegistry>("SystemManager_TEST"); + SystemManager systemMgr( + loader, &ecm, &eventManager, std::string(), paramRegistry.get()); + + EXPECT_EQ(0u, systemMgr.ActiveCount()); + EXPECT_EQ(0u, systemMgr.PendingCount()); + EXPECT_EQ(0u, systemMgr.TotalCount()); + EXPECT_EQ(0u, systemMgr.SystemsConfigure().size()); + EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); + + auto configSystem = std::make_shared(); + systemMgr.AddSystem(configSystem, kNullEntity, nullptr); + + auto entity = ecm.CreateEntity(); + + auto updateSystemWithChild = std::make_shared(); + systemMgr.AddSystem(updateSystemWithChild, entity, nullptr); + + // Configure called during AddSystem + EXPECT_EQ(1, configSystem->configured); + EXPECT_EQ(1, configSystem->configuredParameters); + + EXPECT_EQ(0u, systemMgr.ActiveCount()); + EXPECT_EQ(2u, systemMgr.PendingCount()); + EXPECT_EQ(2u, systemMgr.TotalCount()); + EXPECT_EQ(0u, systemMgr.SystemsConfigure().size()); + EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); + + systemMgr.ActivatePendingSystems(); + EXPECT_EQ(2u, systemMgr.ActiveCount()); + EXPECT_EQ(0u, systemMgr.PendingCount()); + EXPECT_EQ(2u, systemMgr.TotalCount()); + EXPECT_EQ(1u, systemMgr.SystemsConfigure().size()); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size()); + + // Remove the entity + ecm.RequestRemoveEntity(entity); + bool needsCleanUp; + systemMgr.ProcessRemovedEntities(ecm, needsCleanUp); + + EXPECT_TRUE(needsCleanUp); + EXPECT_EQ(1u, systemMgr.ActiveCount()); + EXPECT_EQ(0u, systemMgr.PendingCount()); + EXPECT_EQ(1u, systemMgr.TotalCount()); + EXPECT_EQ(1u, systemMgr.SystemsConfigure().size()); + EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); +} + ///////////////////////////////////////////////// TEST(SystemManager, AddSystemWithInfo) { From 4788a9ae8f89110fe0c5f8667b24338ea37aac76 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 23 Jul 2024 16:04:36 -0700 Subject: [PATCH 07/15] Fix error resolving gazebo classic material when loading world (#2492) Signed-off-by: Ian Chen --- src/SdfEntityCreator.cc | 4 +++- src/Server.cc | 3 ++- src/ServerPrivate.cc | 9 +++++++++ src/ServerPrivate.hh | 5 +++++ test/integration/material.cc | 27 +++++++++++++++++++++++++++ test/worlds/classic_material.sdf | 32 ++++++++++++++++++++++++++++++++ 6 files changed, 78 insertions(+), 2 deletions(-) create mode 100644 test/worlds/classic_material.sdf diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 082f42db11..6985f292d9 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -85,6 +85,7 @@ #include "gz/sim/components/World.hh" #include "rendering/MaterialParser/MaterialParser.hh" +#include "ServerPrivate.hh" class gz::sim::SdfEntityCreatorPrivate { @@ -808,7 +809,8 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual) "https://gazebosim.org/api/sim/8/migrationsdf.html#:~:text=Materials " << "for details." << std::endl; std::string scriptUri = visualMaterial.ScriptUri(); - if (scriptUri != "file://media/materials/scripts/gazebo.material") { + if (scriptUri != ServerPrivate::kClassicMaterialScriptUri) + { gzwarn << "Custom material scripts are not supported." << std::endl; } diff --git a/src/Server.cc b/src/Server.cc index e2286945e7..86b8b7d1cc 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -159,7 +159,8 @@ Server::Server(const ServerConfig &_config) // 'src/gui_main.cc'. errors = sdfRoot.Load(filePath, sdfParserConfig); if (errors.empty() || _config.BehaviorOnSdfErrors() != - ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY) { + ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY) + { if (sdfRoot.Model() == nullptr) { this->dataPtr->sdfRoot = std::move(sdfRoot); } diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 80df07819e..208b582806 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -37,6 +37,9 @@ using namespace gz; using namespace sim; +const char ServerPrivate::kClassicMaterialScriptUri[] = + "file://media/materials/scripts/gazebo.material"; + /// \brief This struct provides access to the record plugin SDF string struct LoggingPlugin { @@ -546,6 +549,12 @@ bool ServerPrivate::ResourcePathsResolveService( ////////////////////////////////////////////////// std::string ServerPrivate::FetchResource(const std::string &_uri) { + // Handle gazebo classic material URIs. + // Return original URI string as the SdfEntityCreator checks for this URI + if (_uri == kClassicMaterialScriptUri) + return _uri; + + // Fetch resource from fuel auto path = fuel_tools::fetchResourceWithClient(_uri, *this->fuelClient.get()); diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh index 03128c56e8..cbb298db3f 100644 --- a/src/ServerPrivate.hh +++ b/src/ServerPrivate.hh @@ -187,6 +187,11 @@ namespace gz /// Server. It is used in the SDFormat world generator when saving worlds public: std::unordered_map fuelUriMap; + /// \brief Gazebo classic material URI string + /// A URI matching this string indicates that it is a gazebo classic + /// material. + public: static const char kClassicMaterialScriptUri[]; + /// \brief List of names for all worlds loaded in this server. private: std::vector worldNames; diff --git a/test/integration/material.cc b/test/integration/material.cc index 6d8ca1599f..22567a8a87 100644 --- a/test/integration/material.cc +++ b/test/integration/material.cc @@ -296,3 +296,30 @@ TEST_F(MaterialTest, InvalidColor) EXPECT_EQ(math::Color(0.0f, 0.0f, 0.0f, 1.0f), boxVisualComp->Data().Specular()); } + +TEST_F(MaterialTest, WorldWithClassicMaterial) +{ + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "classic_material.sdf")); + + std::cout << "Loading: " << serverConfig.SdfFile() << std::endl; + this->StartServer(serverConfig); + + auto model = this->GetModel("box"); + ASSERT_TRUE(model.Valid(*this->ecm)); + + auto boxVisualEntity = + this->ecm->EntityByComponents(components::Name("box_visual")); + ASSERT_NE(kNullEntity, boxVisualEntity); + + // Blue color + auto boxVisualComp = + this->ecm->Component(boxVisualEntity); + EXPECT_EQ(math::Color(0.0f, 0.0f, 1.0f, 1.0f), + boxVisualComp->Data().Ambient()); + EXPECT_EQ(math::Color(0.0f, 0.0f, 1.0f, 1.0f), + boxVisualComp->Data().Diffuse()); + EXPECT_EQ(math::Color(0.1f, 0.1f, 0.1f, 1.0f), + boxVisualComp->Data().Specular()); +} diff --git a/test/worlds/classic_material.sdf b/test/worlds/classic_material.sdf new file mode 100644 index 0000000000..3cf61e6109 --- /dev/null +++ b/test/worlds/classic_material.sdf @@ -0,0 +1,32 @@ + + + + + + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + + + + + + + + From 4226d04fa3e51c6d4cf5970a2915d5891af37501 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 25 Jul 2024 16:09:40 -0700 Subject: [PATCH 08/15] Prepare for 8.6.0 release (#2496) Signed-off-by: Ian Chen --- CMakeLists.txt | 2 +- Changelog.md | 56 ++++++++++++++++++++++++++++++++++++++++++++++++++ package.xml | 2 +- 3 files changed, 58 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a85cfe43ba..d502043b77 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(gz-sim8 VERSION 8.5.0) +project(gz-sim8 VERSION 8.6.0) set (GZ_DISTRIBUTION "Harmonic") #============================================================================ diff --git a/Changelog.md b/Changelog.md index 7a4ba62198..c5ad1ed305 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,61 @@ ## Gazebo Sim 8.x +### Gazebo Sim 8.6.0 (2024-07-25) + +1. Fix error resolving gazebo classic material when loading world + * [Pull request #2492](https://github.com/gazebosim/gz-sim/pull/2492) + +1. Remove systems if their parent entity is removed + * [Pull request #2232](https://github.com/gazebosim/gz-sim/pull/2232) + +1. Fix warnings generated by NetworkConfigTest + * [Pull request #2469](https://github.com/gazebosim/gz-sim/pull/2469) + +1. Fix lidar visualization when `gz_frame_id` is specified + * [Pull request #2481](https://github.com/gazebosim/gz-sim/pull/2481) + +1. Backport convex decomposition visualization + * [Pull request #2454](https://github.com/gazebosim/gz-sim/pull/2454) + +1. Add UserCommands plugin to GPU lidar sensor example + * [Pull request #2479](https://github.com/gazebosim/gz-sim/pull/2479) + +1. Check if any entity actually has a ContactSensorData component before calling GetContactsFromLastStep + * [Pull request #2474](https://github.com/gazebosim/gz-sim/pull/2474) + +1. Enable tests on macOS + * [Pull request #2468](https://github.com/gazebosim/gz-sim/pull/2468) + +1. Update description of reset_sensors test + * [Pull request #2467](https://github.com/gazebosim/gz-sim/pull/2467) + +1. Magnetometer: correct field calculation + * [Pull request #2460](https://github.com/gazebosim/gz-sim/pull/2460) + +1. Address a couple of todos in Conversion.cc + * [Pull request #2461](https://github.com/gazebosim/gz-sim/pull/2461) + +1. Correct name of sensor in warning message + * [Pull request #2457](https://github.com/gazebosim/gz-sim/pull/2457) + +1. Set max contacts for collision pairs + * [Pull request #2270](https://github.com/gazebosim/gz-sim/pull/2270) + +1. Add GravityEnabled boolean component + * [Pull request #2451](https://github.com/gazebosim/gz-sim/pull/2451) + +1. Add support for no gravity link + * [Pull request #2398](https://github.com/gazebosim/gz-sim/pull/2398) + +1. Handle sdf::Geometry::EMPTY in conversions + * [Pull request #2430](https://github.com/gazebosim/gz-sim/pull/2430) + +1. Use topicFromScopedName in a few systems + * [Pull request #2427](https://github.com/gazebosim/gz-sim/pull/2427) + +1. Fix typo in a comment + * [Pull request #2429](https://github.com/gazebosim/gz-sim/pull/2429) + ### Gazebo Sim 8.5.0 (2024-06-26) 1. Backport: Adding cone primitives diff --git a/package.xml b/package.xml index a655c2508c..8be58c01b4 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ gz-sim8 - 8.5.0 + 8.6.0 Gazebo Sim : A Robotic Simulator Michael Carroll Apache License 2.0 From 907efc31f6ff5de0e5d32922f054601e643588f2 Mon Sep 17 00:00:00 2001 From: shameekganguly Date: Tue, 30 Jul 2024 20:29:34 -0700 Subject: [PATCH 09/15] Initialize threadsNeedCleanUp (#2503) Signed-off-by: Shameek Ganguly --- src/SimulationRunner.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 7230ed9b86..49dbaeee1b 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -544,7 +544,7 @@ namespace gz private: std::unique_ptr newWorldControlState; /// \brief Set if we need to remove systems due to entity removal - private: bool threadsNeedCleanUp; + private: bool threadsNeedCleanUp{false}; private: bool resetInitiated{false}; friend class LevelManager; From 5f03199c95f09ef11fbfc5b64c4049e597f6f8d3 Mon Sep 17 00:00:00 2001 From: Abhiroop Bhavsar <126786356+akky20@users.noreply.github.com> Date: Tue, 6 Aug 2024 05:56:57 +0530 Subject: [PATCH 10/15] Fix #2458 - Checking linkEnity is empty (#2509) * Fix #2458 - checking linkEnity is empty Signed-off-by: Abhiroop Bhavsar * Update src/systems/user_commands/UserCommands.cc Co-authored-by: Arjo Chakravarty Signed-off-by: Abhiroop Bhavsar <126786356+akky20@users.noreply.github.com> --------- Signed-off-by: Abhiroop Bhavsar Signed-off-by: Abhiroop Bhavsar <126786356+akky20@users.noreply.github.com> Co-authored-by: Arjo Chakravarty --- src/systems/user_commands/UserCommands.cc | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index c79e392551..8f9ba3102c 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -583,6 +583,12 @@ UserCommands::~UserCommands() = default; bool UserCommandsInterface::HasContactSensor(const Entity _collision) { auto *linkEntity = ecm->Component(_collision); + + if (linkEntity == nullptr) + { + return false; + } + auto allLinkSensors = ecm->EntitiesByComponents(components::Sensor(), components::ParentEntity(*linkEntity)); From c3479e2f5282f3f0702bcf8f3bb9caa970e5bd20 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 6 Aug 2024 20:15:54 -0700 Subject: [PATCH 11/15] Fix adding system to non-existent entity (#2516) * Check entity exists before attempting to add system Signed-off-by: Ian Chen * warn to err, add todo Signed-off-by: Ian Chen --------- Signed-off-by: Ian Chen --- src/SystemManager.cc | 23 +++++++++++++++++-- .../battery_plugin/LinearBatteryPlugin.cc | 9 ++++++++ 2 files changed, 30 insertions(+), 2 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index d1b66346e3..289c7ce5d2 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -25,6 +25,7 @@ #include "SystemInternal.hh" #include "gz/sim/components/SystemPluginInfo.hh" #include "gz/sim/Conversions.hh" +#include "gz/sim/Util.hh" #include "SystemManager.hh" using namespace gz; @@ -339,6 +340,11 @@ bool SystemManager::EntitySystemAddService(const msgs::EntityPlugin_V &_req, { std::lock_guard lock(this->systemsMsgMutex); this->systemsToAdd.push_back(_req); + + // The response is set to true to indicate that the service request is + // handled but it does not necessarily mean the system is added + // successfully + // \todo(iche033) Return false if system is not added successfully? _res.set_data(true); return true; } @@ -398,8 +404,21 @@ void SystemManager::ProcessPendingEntitySystems() if (req.plugins().empty()) { - gzwarn << "Unable to add plugins to Entity: '" << entity - << "'. No plugins specified." << std::endl; + gzerr << "Unable to add plugins to Entity: '" << entity + << "'. No plugins specified." << std::endl; + continue; + } + + // set to world entity if entity id is not specified in the request. + if (entity == kNullEntity || entity == 0u) + { + entity = worldEntity(*this->entityCompMgr); + } + // otherwise check if entity exists before attempting to load the plugin. + else if (!this->entityCompMgr->HasEntity(entity)) + { + gzerr << "Unable to add plugins to Entity: '" << entity + << "'. Entity does not exist." << std::endl; continue; } diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index 523950b172..0f84e1423a 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.cc +++ b/src/systems/battery_plugin/LinearBatteryPlugin.cc @@ -480,6 +480,9 @@ void LinearBatteryPlugin::PreUpdate( { GZ_PROFILE("LinearBatteryPlugin::PreUpdate"); + if (!this->dataPtr->battery) + return; + // Recalculate the total power load among consumers double total_power_load = this->dataPtr->initialPowerLoad; _ecm.Each( @@ -546,6 +549,9 @@ void LinearBatteryPlugin::Update(const UpdateInfo &_info, { GZ_PROFILE("LinearBatteryPlugin::Update"); + if (!this->dataPtr->battery) + return; + // \TODO(anyone) Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) { @@ -610,6 +616,9 @@ void LinearBatteryPlugin::PostUpdate(const UpdateInfo &_info, if (_info.paused || !this->dataPtr->statePub) return; + if (!this->dataPtr->battery) + return; + // Publish battery state msgs::BatteryState msg; msg.mutable_header()->mutable_stamp()->CopyFrom( From fab620a2d1f748de094203f59091e9b2ba475a09 Mon Sep 17 00:00:00 2001 From: jrutgeer Date: Thu, 8 Aug 2024 00:53:11 +0200 Subject: [PATCH 12/15] gui_system_plugin: clarify description in README (#2253) * Clarfied description that was not clear due to rename Ignition to Gazebo. Signed-off-by: Johan Rutgeerts * Slight rephrasing for extra clarity. Signed-off-by: Johan Rutgeerts --------- Signed-off-by: Johan Rutgeerts Co-authored-by: Addisu Z. Taddese Co-authored-by: Ian Chen --- examples/plugin/gui_system_plugin/README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/plugin/gui_system_plugin/README.md b/examples/plugin/gui_system_plugin/README.md index ac8e21d9da..e85a0bb769 100644 --- a/examples/plugin/gui_system_plugin/README.md +++ b/examples/plugin/gui_system_plugin/README.md @@ -2,10 +2,10 @@ This example shows how to create a GUI system plugin. -Gazebo supports any kind of Gazebo GUI plugin -(`gz::gui::Plugin`). Gazebo GUI plugins are a special type of Gazebo -GUI plugin which also have access to entity and component updates coming from -the server. +Gazebo supports any kind of GUI plugin +(`gz-gui` library: `gz::gui::Plugin`). However, GuiSystem plugins +(`gz-sim` library: `gz::sim::GuiSystem`) are a special type of GUI plugin, +which also have access to entity and component updates coming from the server. See `GuiSystemPluginPlugin.hh` for more information. From 523b01b390ed9572522c99f792df01f583bb579f Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 7 Aug 2024 22:05:07 -0500 Subject: [PATCH 13/15] Fix ResourceSpawner (#2490) * Use new ModelIdentifier::Url method --------- Signed-off-by: Michael Carroll Co-authored-by: Addisu Z. Taddese Co-authored-by: Ian Chen --- src/gui/plugins/resource_spawner/ResourceSpawner.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.cc b/src/gui/plugins/resource_spawner/ResourceSpawner.cc index 7e61cc0c4e..e206415218 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.cc +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.cc @@ -553,7 +553,7 @@ void ResourceSpawner::OnDownloadFuelResource(const QString &_path, // Set the waiting cursor while the resource downloads QGuiApplication::setOverrideCursor(Qt::WaitCursor); if (this->dataPtr->fuelClient->DownloadModel( - common::URI(_path.toStdString()), localPath)) + common::URI(_path.toStdString(), true), localPath)) { // Successful download, set thumbnail std::string thumbnailPath = common::joinPaths(localPath, "thumbnails"); @@ -739,7 +739,7 @@ void ResourceSpawner::RunFetchResourceListThread(const std::string &_owner) resource.isFuel = true; resource.isDownloaded = false; resource.owner = id.Owner(); - resource.sdfPath = id.UniqueName(); + resource.sdfPath = id.Url().Str(); QMetaObject::invokeMethod( this, "UpdateOwnerListModel", Qt::QueuedConnection, From c48fa58f9ef20b85cbd7ed1104a6d59af45afb73 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 12 Aug 2024 10:00:34 -0700 Subject: [PATCH 14/15] Make sure steering joints exist before updating velocity / odometry in AckermannSteering plugin (#2521) Signed-off-by: Ian Chen --- src/systems/ackermann_steering/AckermannSteering.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 59d81052f8..f1398c6c95 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -677,6 +677,11 @@ void AckermannSteering::PostUpdate(const UpdateInfo &_info, // Nothing left to do if paused. if (_info.paused) return; + + if (this->dataPtr->leftSteeringJoints.empty() || + this->dataPtr->rightSteeringJoints.empty()) + return; + if (this->dataPtr->steeringOnly) { this->dataPtr->UpdateAngle(_info, _ecm); From e2be4467894deb1229683a0d5447c006d6323526 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 13 Aug 2024 19:21:26 -0700 Subject: [PATCH 15/15] Remove unused var (#2524) Signed-off-by: Ian Chen --- src/systems/thruster/Thruster.cc | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 87aa50dcb4..62e2e0cf11 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -694,9 +694,6 @@ void Thruster::PreUpdate( gz::sim::Link link(this->dataPtr->linkEntity); - - auto pose = worldPose(this->dataPtr->linkEntity, _ecm); - // TODO(arjo129): add logic for custom coordinate frame // Convert joint axis to the world frame const auto linkWorldPose = worldPose(this->dataPtr->linkEntity, _ecm);