From cd8efc17004dfabf21f4db8227e33ac6cb3bc81e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 8 Oct 2021 14:43:35 +0800 Subject: [PATCH 01/49] add support for models of the same name Signed-off-by: Arjo Chakravarty --- .../detachable_joint/DetachableJoint.cc | 27 ++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 0946aa9505..537f551b11 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -129,8 +129,33 @@ void DetachableJoint::PreUpdate( } else { - modelEntity = _ecm.EntityByComponents( + auto candidateEntities = _ecm.EntitiesByComponents( components::Model(), components::Name(this->childModelName)); + + if (candidateEntities.size() == 1) + { + // If there is one entity select that entity itself + modelEntity = candidateEntities[0]; + } + else + { + // If there is more than one entity with the same name, look for the + // entity with a parent component. + for(auto entity : candidateEntities) + { + // TODO: do we support grand children? Seems dicy + // what about if there are multiple children with the same name? + auto models = this->model.Models(_ecm); + + for (auto model: models) + { + if (model == entity) + { + modelEntity = entity; + } + } + } + } } if (kNullEntity != modelEntity) { From b072901c83382633b07ce238c51870b0397c6b19 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 8 Oct 2021 14:59:33 +0800 Subject: [PATCH 02/49] codecheck Signed-off-by: Arjo Chakravarty --- src/systems/detachable_joint/DetachableJoint.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 537f551b11..87b909a567 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -139,21 +139,21 @@ void DetachableJoint::PreUpdate( } else { - // If there is more than one entity with the same name, look for the + // If there is more than one entity with the same name, look for the // entity with a parent component. for(auto entity : candidateEntities) { - // TODO: do we support grand children? Seems dicy + // TODO(arjo): do we support grand children? Seems dicy // what about if there are multiple children with the same name? auto models = this->model.Models(_ecm); - for (auto model: models) + for (auto model : models) { if (model == entity) { modelEntity = entity; } - } + } } } } From 5fb5a74b7ebef4bcb280af1b3d659844939957cf Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 8 Oct 2021 20:38:30 +0800 Subject: [PATCH 03/49] Add unit test Signed-off-by: Arjo Chakravarty --- .../detachable_joint/DetachableJoint.cc | 2 +- test/integration/detachable_joint.cc | 72 ++++++++++++ test/worlds/detachable_joint.sdf | 106 ++++++++++++++++++ 3 files changed, 179 insertions(+), 1 deletion(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 87b909a567..caf1847f35 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -143,7 +143,7 @@ void DetachableJoint::PreUpdate( // entity with a parent component. for(auto entity : candidateEntities) { - // TODO(arjo): do we support grand children? Seems dicy + // TODO(arjo): do we want to support grand children? // what about if there are multiple children with the same name? auto models = this->model.Models(_ecm); diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index bbbe91c66f..067bd8d75a 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -25,6 +25,7 @@ #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" #include "ignition/gazebo/test_config.hh" +#include "ignition/gazebo/Util.hh" #include "ignition/gazebo/components/LinearAcceleration.hh" #include "ignition/gazebo/components/LinearVelocity.hh" @@ -211,3 +212,74 @@ TEST_F(DetachableJointTest, LinksInSameModel) // the expected distance. EXPECT_GT(b2Poses.front().Pos().Z() - b2Poses.back().Pos().Z(), expDist); } + +///////////////////////////////////////////////// +TEST_F(DetachableJointTest, NestedModelsWithSameName) +{ + using namespace std::chrono_literals; + + this->StartServer("/test/worlds/detachable_joint.sdf"); + + + std::vector childM4Poses, childM5Poses; + test::Relay testSystem1; + testSystem1.OnPostUpdate([&childM4Poses, &childM5Poses]( + const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto childModels = _ecm.EntitiesByComponents( + components::Model(), components::Name("child_model")); + + auto entityM5 = _ecm.EntityByComponents( + components::Model(), components::Name("M5")); + + Model modelM5(entityM5); + auto childModelsM5 = modelM5.Models(_ecm); + + Entity childEntityM5{kNullEntity}, childEntityM4{kNullEntity}; + for(auto entity : childModelsM5) + { + if (entity == childModels[0]) + { + childEntityM5 = childModels[0]; + childEntityM4 = childModels[1]; + } + if (entity == childModels[1]) + { + childEntityM5 = childModels[1]; + childEntityM4 = childModels[0]; + } + } + + Model childModelM4(childEntityM4); + Model childModelM5(childEntityM5); + + auto poseM4 = _ecm.Component(childEntityM4); + auto poseM5 = _ecm.Component(childEntityM5); + + childM4Poses.push_back(poseM4->Data()); + childM5Poses.push_back(poseM5->Data()); + } + ); + + this->server->AddSystem(testSystem1.systemPtr); + + const std::size_t nIters{20}; + this->server->Run(true, nIters, false); + + // Both children of model1 and model4 should not move as they are held + // in place + EXPECT_EQ(childM4Poses.front(), childM4Poses.back()); + EXPECT_EQ(childM5Poses.front(), childM5Poses.back()); + + // Release M5's child only + transport::Node node; + auto pub = node.Advertise("/model/M5/detachable_joint/detach"); + pub.Publish(msgs::Empty()); + std::this_thread::sleep_for(250ms); + + // Release M5's + this->server->Run(true, nIters, false); + EXPECT_LT(childM5Poses.back().Z(), childM4Poses.front().Z()); + EXPECT_EQ(childM4Poses.front(), childM4Poses.back()); +} diff --git a/test/worlds/detachable_joint.sdf b/test/worlds/detachable_joint.sdf index 821d5a1a0f..57fdc62aa0 100644 --- a/test/worlds/detachable_joint.sdf +++ b/test/worlds/detachable_joint.sdf @@ -122,5 +122,111 @@ body2 + + + 20 0 1 0 0 0 + + + 1 + + 0.667 + 0 + 0 + 0.667 + 0 + 0.667 + + + + + + 2.0 2.0 2.0 + + + + + + + 20 0 5 0 0 0 + + + 1 + + 0.667 + 0 + 0 + 0.667 + 0 + 0.667 + + + + + + 2.0 2.0 2.0 + + + + + + + body + child_model + body + + + + + -20 0 1 0 0 0 + + + 1 + + 0.667 + 0 + 0 + 0.667 + 0 + 0.667 + + + + + + 2.0 2.0 2.0 + + + + + + + -20 0 5 0 0 0 + + + 1 + + 0.667 + 0 + 0 + 0.667 + 0 + 0.667 + + + + + + 2.0 2.0 2.0 + + + + + + + body + child_model + body + + From 0bf387d355cb18edd219171075c3b23329aabeda Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 12 Oct 2021 09:23:16 +0800 Subject: [PATCH 04/49] Address some feedback Signed-off-by: Arjo Chakravarty --- src/systems/detachable_joint/DetachableJoint.cc | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index caf1847f35..0f09fb8312 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -141,15 +141,14 @@ void DetachableJoint::PreUpdate( { // If there is more than one entity with the same name, look for the // entity with a parent component. - for(auto entity : candidateEntities) + for (auto entity : candidateEntities) { // TODO(arjo): do we want to support grand children? - // what about if there are multiple children with the same name? auto models = this->model.Models(_ecm); - for (auto model : models) + for (auto m : models) { - if (model == entity) + if (m == entity) { modelEntity = entity; } From c8aec3fb7b16c1da2e94989d91d354d4d0fa1516 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 5 Jan 2024 17:02:02 -0600 Subject: [PATCH 05/49] =?UTF-8?q?=F0=9F=8E=88=203.15.1=20(#2279)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michael Carroll --- CMakeLists.txt | 2 +- Changelog.md | 30 ++++++++++++++++++++++++++++++ 2 files changed, 31 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8f1a921d1d..ea0a0621ab 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.15.0) +project(ignition-gazebo3 VERSION 3.15.1) set (GZ_DISTRIBUTION "Citadel") #============================================================================ diff --git a/Changelog.md b/Changelog.md index a994ba13b2..901331a512 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,35 @@ ## Gazebo Sim 3.x +### Gazebo Sim 3.15.1 (2024-01-05) + +1. Update github action workflows + * [Pull request #2237](https://github.com/gazebosim/gz-sim/pull/2237) + * [Pull request #1988](https://github.com/gazebosim/gz-sim/pull/1988) + +1. Fix macOS test failures by registering components in the core library + * [Pull request #2220](https://github.com/gazebosim/gz-sim/pull/2220) + +1. Bump Fuel model version in test + * [Pull request #2190](https://github.com/gazebosim/gz-sim/pull/2190) + +1. Fix a minor issue in the documentation of the server API + * [Pull request #2067](https://github.com/gazebosim/gz-sim/pull/2067) + +1. Use sdf::Element::FindElement instead of GetElement in ApplyLinkWrench + * [Pull request #2052](https://github.com/gazebosim/gz-sim/pull/2052) + +1. Adds a warning if the `Server` method of a `TestFixture` is called before `Finalize` + * [Pull request #2047](https://github.com/gazebosim/gz-sim/pull/2047) + +1. Protobuf: Do not require version 3 do support Protobuf 4.23.2 (23.2) + * [Pull request #2006](https://github.com/gazebosim/gz-sim/pull/2006) + +1. Print an error message when trying to load SDF files that don't contain a `` + * [Pull request #1998](https://github.com/gazebosim/gz-sim/pull/1998) + +1. Enable GzWeb visualization of markers by republishing service requests on a topic + * [Pull request #1994](https://github.com/gazebosim/gz-sim/pull/1994) + ### Gazebo Sim 3.15.0 (2023-05-08) 1. Speed up Resource Spawner load time by fetching model list asynchronously From a3e9f8d56e6987df1f3cbb217859bcd32c11ef65 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 25 Mar 2024 17:09:20 -0500 Subject: [PATCH 06/49] Setup rendering environment before cmake runs (#1965) CMake checks if rendering is available to enable some tests, so the rendering environment needs to be set before CMake runs. --------- Signed-off-by: Addisu Z. Taddese --- .github/ci/after_make.sh | 9 --------- .github/ci/before_cmake.sh | 9 +++++++++ .github/ci/packages.apt | 2 ++ .github/workflows/ci.yml | 3 +++ include/gz/sim/playback_server.config | 4 ++-- 5 files changed, 16 insertions(+), 11 deletions(-) create mode 100644 .github/ci/before_cmake.sh diff --git a/.github/ci/after_make.sh b/.github/ci/after_make.sh index a3b79b0ae9..283c904b3a 100644 --- a/.github/ci/after_make.sh +++ b/.github/ci/after_make.sh @@ -4,12 +4,3 @@ set -x # Install (needed for some tests) make install - -# For ign-tools -export IGN_CONFIG_PATH=/usr/local/share/ignition - -# For rendering / window tests -Xvfb :1 -screen 0 1280x1024x24 & -export DISPLAY=:1.0 -export RENDER_ENGINE_VALUES=ogre2 -export MESA_GL_VERSION_OVERRIDE=3.3 diff --git a/.github/ci/before_cmake.sh b/.github/ci/before_cmake.sh new file mode 100644 index 0000000000..308074bc46 --- /dev/null +++ b/.github/ci/before_cmake.sh @@ -0,0 +1,9 @@ +#!/bin/sh -l + +set -x + +# For rendering / window tests +Xvfb :1 -screen 0 1280x1024x24 & +export DISPLAY=:1.0 +export RENDER_ENGINE_VALUES=ogre2 +export MESA_GL_VERSION_OVERRIDE=3.3 diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index 4059cad103..0a95b15483 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -13,3 +13,5 @@ libignition-transport8-dev libsdformat9-dev qml-module-qtqml-models2 xvfb +x11-utils +mesa-utils diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 898b1706f5..0ca0012cdb 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -20,6 +20,7 @@ jobs: uses: ignition-tooling/action-ignition-ci@bionic with: codecov-enabled: true + cmake-args: "-DCMAKE_INSTALL_PREFIX=/usr" focal-ci: runs-on: ubuntu-latest name: Ubuntu Focal CI @@ -29,3 +30,5 @@ jobs: - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@focal + with: + cmake-args: "-DCMAKE_INSTALL_PREFIX=/usr" diff --git a/include/gz/sim/playback_server.config b/include/gz/sim/playback_server.config index 17ac79c8d9..9a88489385 100644 --- a/include/gz/sim/playback_server.config +++ b/include/gz/sim/playback_server.config @@ -3,12 +3,12 @@ + name="gz::sim::systems::UserCommands"> + name="gz::sim::systems::SceneBroadcaster"> From 07e5d9c20da9e09cfc8525254b0b0718e35038e2 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 24 May 2024 13:13:00 -0500 Subject: [PATCH 07/49] Fix bug where iterator was used after the underlying item was erased from the container (#2412) Signed-off-by: Addisu Z. Taddese --- src/systems/physics/EntityFeatureMap.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systems/physics/EntityFeatureMap.hh b/src/systems/physics/EntityFeatureMap.hh index 0e0686333d..aa2023618b 100644 --- a/src/systems/physics/EntityFeatureMap.hh +++ b/src/systems/physics/EntityFeatureMap.hh @@ -226,8 +226,8 @@ namespace systems::physics_system if (it != this->entityMap.end()) { this->reverseMap.erase(it->second); - this->entityMap.erase(it); this->castCache.erase(_entity); + this->entityMap.erase(it); return true; } return false; @@ -242,8 +242,8 @@ namespace systems::physics_system if (it != this->reverseMap.end()) { this->entityMap.erase(it->second); - this->reverseMap.erase(it); this->castCache.erase(it->second); + this->reverseMap.erase(it); return true; } return false; From 9d35f5c7df6cd04bd9de1e7a67da036d01cdf4bc Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 3 Mar 2023 12:17:17 -0600 Subject: [PATCH 08/49] Address a few Windows CI Issues (#1911) Signed-off-by: Michael Carroll Signed-off-by: Michael Carroll --- include/gz/sim/detail/EntityComponentManager.hh | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/include/gz/sim/detail/EntityComponentManager.hh b/include/gz/sim/detail/EntityComponentManager.hh index a5cf6379f6..785cfb3ab4 100644 --- a/include/gz/sim/detail/EntityComponentManager.hh +++ b/include/gz/sim/detail/EntityComponentManager.hh @@ -52,11 +52,18 @@ namespace traits template struct HasEqualityOperator { +#if !defined(_MSC_VER) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wnonnull" +#endif enum { // False positive codecheck "Using C-style cast" value = !std::is_same::value // NOLINT }; +#if !defined(_MSC_VER) +#pragma GCC diagnostic pop +#endif }; } From a2a9c5384aa84aeb0550ca1b9340eaffec2f278e Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 21 Nov 2023 19:53:13 -0600 Subject: [PATCH 09/49] Fix DLL linkage/visibility issues (#2254) Signed-off-by: Michael Carroll --- src/CMakeLists.txt | 3 +-- src/cmd/runGui_main.cc | 23 +++++++++++++++++++ src/gz.cc | 5 ---- .../CameraVideoRecorder.cc | 1 - 4 files changed, 24 insertions(+), 8 deletions(-) create mode 100644 src/cmd/runGui_main.cc diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 8a81a7583d..969da19349 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -68,7 +68,6 @@ set (sources Util.cc View.cc World.cc - cmd/ModelCommandAPI.cc ${PROTO_PRIVATE_SRC} ${network_sources} ${comms_sources} @@ -160,7 +159,7 @@ target_link_libraries(${ign_lib_target} ) # Executable target that runs the GUI without ruby for debugging purposes. -add_executable(runGui gz.cc) +add_executable(runGui cmd/runGui_main.cc) target_link_libraries(runGui PRIVATE ${ign_lib_target}) # Create the library target diff --git a/src/cmd/runGui_main.cc b/src/cmd/runGui_main.cc new file mode 100644 index 0000000000..0b8aa66d4d --- /dev/null +++ b/src/cmd/runGui_main.cc @@ -0,0 +1,23 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "gz/sim/gui/Gui.hh" + +int main(int argc, char* argv[]) +{ + return gz::sim::gui::runGui(argc, argv, nullptr); +} diff --git a/src/gz.cc b/src/gz.cc index a658d2eb34..206d88e4d4 100644 --- a/src/gz.cc +++ b/src/gz.cc @@ -429,8 +429,3 @@ extern "C" int runGui(const char *_guiConfig, const char *_file, int _waitGui, return gz::sim::gui::runGui( argc, &argv, _guiConfig, _file, _waitGui, _renderEngine); } - -int main(int argc, char* argv[]) -{ - return gazebo::gui::runGui(argc, argv, nullptr); -} diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.cc b/src/systems/camera_video_recorder/CameraVideoRecorder.cc index dcfe5f13e9..2913fe1d97 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.cc +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.cc @@ -333,7 +333,6 @@ void CameraVideoRecorderPrivate::OnPostRender() { this->camera->Copy(this->cameraImage); 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 From 9277c0b5415a96c00a0a63e0e868b221f37f1265 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 17 Jul 2024 13:12:27 -0700 Subject: [PATCH 10/49] backport lidar visualization frame_id fix Signed-off-by: Ian Chen --- .../plugins/visualize_lidar/VisualizeLidar.cc | 130 +++++++----------- 1 file changed, 52 insertions(+), 78 deletions(-) diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc index f76ad7d3cc..34057aa864 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc @@ -82,9 +82,6 @@ inline namespace IGNITION_GAZEBO_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 IGNITION_GAZEBO_VERSION_NAMESPACE public: bool visualDirty{false}; /// \brief lidar sensor entity dirty flag - public: bool lidarEntityDirty{true}; + public: bool lidarEntityDirty{false}; }; } } @@ -266,63 +263,39 @@ void VisualizeLidar::Update(const UpdateInfo &, 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) - { - ignerr << "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) - { - ignerr << "The entity could not be found." - << "Error displaying lidar visual" <dataPtr->lidarEntity = parent; - this->dataPtr->lidarEntityDirty = false; - } + ignerr << "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 +305,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 +352,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)) { ignerr << "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 +378,8 @@ void VisualizeLidar::OnTopic(const QString &_topicName) } this->dataPtr->visualDirty = false; ignmsg << "Subscribed to " << this->dataPtr->topicName << std::endl; + + this->dataPtr->lidarEntityDirty = true; } ////////////////////////////////////////////////// @@ -488,27 +467,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 9891844a10f42a7fc3a2c9bab6085347674334bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crist=C3=B3bal=20Arroyo?= Date: Fri, 26 Jul 2024 15:13:49 -0500 Subject: [PATCH 11/49] Disable failing testFixture_TEST for MacOS (#2499) * Disable failing testFixture_TEST for MacOS Signed-off-by: Crola1702 * Add comment with issue Signed-off-by: Crola1702 * Update issue comment Signed-off-by: Crola1702 --------- Signed-off-by: Crola1702 --- python/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 13ee16b004..68f941a513 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -86,6 +86,11 @@ if (BUILD_TESTING) testFixture_TEST ) + # Check issue #1887 for more information + if (APPLE) + list(REMOVE_ITEM python_tests testFixture_TEST) + endif() + execute_process(COMMAND "${PYTHON_EXECUTABLE}" -m pytest --version OUTPUT_VARIABLE PYTEST_output ERROR_VARIABLE PYTEST_error From fb25971d7160cc5d93115822a5561f73aeb5fbc1 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 18 Jul 2024 01:39:40 +0800 Subject: [PATCH 12/49] 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. Signed-off-by: Arjo Chakravarty --- include/gz/sim/EntityComponentManager.hh | 3 + src/SimulationRunner.cc | 10 +- src/SimulationRunner.hh | 3 + src/SimulationRunner_TEST.cc | 16 ++- src/SystemManager.cc | 121 +++++++++++++++++++++++ src/SystemManager.hh | 8 ++ src/SystemManager_TEST.cc | 65 ++++++++++++ 7 files changed, 219 insertions(+), 7 deletions(-) diff --git a/include/gz/sim/EntityComponentManager.hh b/include/gz/sim/EntityComponentManager.hh index 97b0ba1a74..57c55f9a8c 100644 --- a/include/gz/sim/EntityComponentManager.hh +++ b/include/gz/sim/EntityComponentManager.hh @@ -836,6 +836,9 @@ namespace ignition 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 f2d8dfb4ca..07255523b8 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -20,6 +20,7 @@ #include #include +#include #include "gz/common/Profiler.hh" #include "gz/sim/components/Model.hh" @@ -483,12 +484,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(); auto threadCount = this->systemMgr->SystemsPostUpdate().size() + 1u; @@ -852,6 +856,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 0826fdd580..b552d17a8c 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -532,6 +532,9 @@ namespace ignition /// at the appropriate time. private: std::unique_ptr newWorldControlState; + /// \brief Set if we need to remove systems due to entity removal + private: bool threadsNeedCleanUp; + friend class LevelManager; }; } diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 66f590467c..d79d45f5c6 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -105,7 +105,6 @@ void rootClockCb(const msgs::Clock &_msg) rootClockMsgs.push_back(_msg); } - ///////////////////////////////////////////////// TEST_P(SimulationRunnerTest, CreateEntities) { @@ -1503,8 +1502,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); @@ -1552,8 +1550,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 8332301ecb..a7dcbcbbc0 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" @@ -115,7 +118,9 @@ size_t SystemManager::ActivatePendingSystems() this->systemsUpdate.push_back(system.update); if (system.postupdate) + { this->systemsPostupdate.push_back(system.postupdate); + } } this->pendingSystems.clear(); @@ -318,3 +323,119 @@ 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 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.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->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 75d997d469..ce6fe812a4 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -127,6 +127,14 @@ namespace ignition /// \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 02a2957334..fc5d133574 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 67cf543b828a7c7311e027810fa83f6bda0e6393 Mon Sep 17 00:00:00 2001 From: shameekganguly Date: Tue, 30 Jul 2024 20:29:34 -0700 Subject: [PATCH 13/49] 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 b552d17a8c..aec30c3296 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -533,7 +533,7 @@ namespace ignition private: std::unique_ptr newWorldControlState; /// \brief Set if we need to remove systems due to entity removal - private: bool threadsNeedCleanUp; + private: bool threadsNeedCleanUp{false}; friend class LevelManager; }; From 96dddad061f75f755059fdb509984ff611438a62 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 2 Aug 2024 00:32:27 +0000 Subject: [PATCH 14/49] Fix shader param test Signed-off-by: Ian Chen --- examples/worlds/shader_param.sdf | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/worlds/shader_param.sdf b/examples/worlds/shader_param.sdf index 25e8b12f14..b04cd31661 100644 --- a/examples/worlds/shader_param.sdf +++ b/examples/worlds/shader_param.sdf @@ -182,12 +182,12 @@ ShaderParam visual plugin over time. deformable_sphere 0 0 1.5 0 0 0 - https://fuel.ignitionrobotics.org/1.0/openrobotics/models/deformable_sphere + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/deformable_sphere/5 waves 0 0 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/openrobotics/models/waves + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/waves/4 From 45f6dc3da8be17ec52aad228f214499878d07fb7 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 7 Aug 2024 01:34:52 -0400 Subject: [PATCH 15/49] Update test files Signed-off-by: Arjo Chakravarty --- test/integration/detachable_joint.cc | 2 +- test/worlds/detachable_joint_child.sdf | 232 +++++++++++++++++++++++++ 2 files changed, 233 insertions(+), 1 deletion(-) create mode 100644 test/worlds/detachable_joint_child.sdf diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index 3a6f40448f..7e8d42c72c 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -141,7 +141,7 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) { using namespace std::chrono_literals; - this->StartServer("/test/worlds/detachable_joint.sdf"); + this->StartServer("/test/worlds/detachable_joint_child.sdf"); // A lambda that takes a model name and a mutable reference to a vector of // poses and returns another lambda that can be passed to diff --git a/test/worlds/detachable_joint_child.sdf b/test/worlds/detachable_joint_child.sdf new file mode 100644 index 0000000000..de73bacdc1 --- /dev/null +++ b/test/worlds/detachable_joint_child.sdf @@ -0,0 +1,232 @@ + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + + 0 0 1 0 0 0 + + + 1 + + 0.667 + 0 + 0 + 0.667 + 0 + 0.667 + + + + + + 2.0 2.0 2.0 + + + + + + + body + M2 + body + + + + + 0 0 5 0 0 0 + + + 1 + + 0.667 + 0 + 0 + 0.667 + 0 + 0.667 + + + + + + 2.0 2.0 2.0 + + + + + + + + + 10 0 1 0 0 0 + + + 1 + + 0.667 + 0 + 0 + 0.667 + 0 + 0.667 + + + + + + 2.0 2.0 2.0 + + + + + + 0 0 5 0 0 0 + + 1 + + 0.667 + 0 + 0 + 0.667 + 0 + 0.667 + + + + + + 2.0 2.0 2.0 + + + + + + body1 + __model__ + body2 + + + + + 20 0 1 0 0 0 + + + 1 + + 0.667 + 0 + 0 + 0.667 + 0 + 0.667 + + + + + + 2.0 2.0 2.0 + + + + + + + 20 0 5 0 0 0 + + + 1 + + 0.667 + 0 + 0 + 0.667 + 0 + 0.667 + + + + + + 2.0 2.0 2.0 + + + + + + + body + child_model + body + + + + + -20 0 1 0 0 0 + + + 1 + + 0.667 + 0 + 0 + 0.667 + 0 + 0.667 + + + + + + 2.0 2.0 2.0 + + + + + + + -20 0 5 0 0 0 + + + 1 + + 0.667 + 0 + 0 + 0.667 + 0 + 0.667 + + + + + + 2.0 2.0 2.0 + + + + + + + body + child_model + body + + + + \ No newline at end of file From 13f5c93759ae267849fe1b6b0e24a0d60ce27174 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 7 Aug 2024 15:02:00 +0800 Subject: [PATCH 16/49] Fix tests Signed-off-by: Arjo Chakravarty --- test/integration/detachable_joint.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index 7e8d42c72c..26a8484e54 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -141,7 +141,7 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) { using namespace std::chrono_literals; - this->StartServer("/test/worlds/detachable_joint_child.sdf"); + this->StartServer("/test/worlds/detachable_joint.sdf"); // A lambda that takes a model name and a mutable reference to a vector of // poses and returns another lambda that can be passed to @@ -219,7 +219,7 @@ TEST_F(DetachableJointTest, NestedModelsWithSameName) { using namespace std::chrono_literals; - this->StartServer("/test/worlds/detachable_joint.sdf"); + this->StartServer("/test/worlds/detachable_joint_child.sdf"); std::vector childM4Poses, childM5Poses; From 2ae65a32c6c2104fd237ef840eee5f01e7dad59c Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 7 Aug 2024 15:48:02 +0800 Subject: [PATCH 17/49] Use `scopedName` to support grandchildren as well. Signed-off-by: Arjo Chakravarty --- .../detachable_joint/DetachableJoint.cc | 23 ++++++++++--------- test/integration/detachable_joint.cc | 9 +++++--- 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index efc9cee163..b22af7b5e2 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -234,30 +234,31 @@ void DetachableJoint::PreUpdate( } else { - auto candidateEntities = _ecm.EntitiesByComponents( - components::Model(), components::Name(this->childModelName)); + auto candidateEntities = entitiesFromScopedName( + this->childModelName, _ecm); if (candidateEntities.size() == 1) { // If there is one entity select that entity itself - modelEntity = candidateEntities[0]; + modelEntity = *candidateEntities.begin(); } else { + auto parentEntityScopedPath = scopedName(this->model.Entity(), _ecm); // If there is more than one entity with the same name, look for the // entity with a parent component. for (auto entity : candidateEntities) { - // TODO(arjo): do we want to support grand children? - auto models = this->model.Models(_ecm); - - for (auto m : models) + auto childEntityScope = scopedName(entity, _ecm); + if (childEntityScope.size() < parentEntityScopedPath.size()) + { + continue; + } + if (childEntityScope.find(parentEntityScopedPath) != 0) { - if (m == entity) - { - modelEntity = entity; - } + continue; } + modelEntity = entity; } } } diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index 26a8484e54..1fa4fb3484 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -63,7 +63,8 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(StartConnected)) { using namespace std::chrono_literals; - this->StartServer("/test/worlds/detachable_joint.sdf"); + this->StartServer(common::joinPaths("/test", "worlds", + "detachable_joint.sdf")); // A lambda that takes a model name and a mutable reference to a vector of // poses and returns another lambda that can be passed to @@ -141,7 +142,8 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) { using namespace std::chrono_literals; - this->StartServer("/test/worlds/detachable_joint.sdf"); + this->StartServer(common::joinPaths("/test", "worlds", + "detachable_joint.sdf")); // A lambda that takes a model name and a mutable reference to a vector of // poses and returns another lambda that can be passed to @@ -219,7 +221,8 @@ TEST_F(DetachableJointTest, NestedModelsWithSameName) { using namespace std::chrono_literals; - this->StartServer("/test/worlds/detachable_joint_child.sdf"); + this->StartServer(common::joinPaths("/test", "worlds", + "detachable_joint_child.sdf")); std::vector childM4Poses, childM5Poses; From e8e1b16976b2774f3f9cbade1eb8c0978aa3279a Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 12 Aug 2024 12:49:36 +0800 Subject: [PATCH 18/49] Address Feedback - Add better logging - Filter by models only - Check child link immediately Signed-off-by: Arjo Chakravarty --- .../detachable_joint/DetachableJoint.cc | 41 ++++++++++++++++++- 1 file changed, 39 insertions(+), 2 deletions(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index b22af7b5e2..aaa22e2845 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -234,9 +234,21 @@ void DetachableJoint::PreUpdate( } else { - auto candidateEntities = entitiesFromScopedName( + auto entitiesMatchingName = entitiesFromScopedName( this->childModelName, _ecm); + // TODO(arjoc): There is probably a more efficient way of combining entitiesFromScopedName + // With filtering. + // Filter for entities with only models + std::vector candidateEntities; + for (const auto entity: entitiesMatchingName) + { + if (_ecm.EntityHasComponentType(entity, components::Model::typeId)) + { + candidateEntities.push_back(entity); + } + } + if (candidateEntities.size() == 1) { // If there is one entity select that entity itself @@ -244,6 +256,7 @@ void DetachableJoint::PreUpdate( } else { + std::string selectedModelName; auto parentEntityScopedPath = scopedName(this->model.Entity(), _ecm); // If there is more than one entity with the same name, look for the // entity with a parent component. @@ -258,7 +271,31 @@ void DetachableJoint::PreUpdate( { continue; } - modelEntity = entity; + if (modelEntity == kNullEntity) + { + + this->childLinkEntity = _ecm.EntityByComponents( + components::Link(), components::ParentEntity(entity), + components::Name(this->childLinkName)); + + if (kNullEntity != this->childLinkEntity) + { + // Only select child entity if tbe entity has a link + modelEntity = entity; + selectedModelName = childEntityScope; + igndbg << "Selecting " << childEntityScope << " as model to be detached" << std::endl; + } + else + { + ignwarn << "Found " << childEntityScope << " with no link named " << this->childLinkName <suppressChildWarning = true; + } + } + else + { + ignwarn << "Found multiple models skipping " << childEntityScope + << "Using " << selectedModelName << " instead" << std::endl; + } } } } From a5c9cb6cd5f9bd664f40c5c9b674d06da5b7630b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 12 Aug 2024 12:59:11 +0800 Subject: [PATCH 19/49] Better logging Signed-off-by: Arjo Chakravarty --- src/systems/detachable_joint/DetachableJoint.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index aaa22e2845..3a1bcaa865 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -247,6 +247,11 @@ void DetachableJoint::PreUpdate( { candidateEntities.push_back(entity); } + else + { + ignwarn << "Found entity " << scopedName(entity, _ecm) + << " but it is not a model." << std::endl; + } } if (candidateEntities.size() == 1) From 19bdc858b0f28bd1403ece5e1bb95d763a8ac65e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 12 Aug 2024 14:57:20 +0800 Subject: [PATCH 20/49] Clean up tests. Signed-off-by: Arjo Chakravarty --- test/integration/detachable_joint.cc | 8 +++++--- test/worlds/detachable_joint_child.sdf | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index 1fa4fb3484..c461e5ff79 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -271,7 +271,7 @@ TEST_F(DetachableJointTest, NestedModelsWithSameName) const std::size_t nIters{20}; this->server->Run(true, nIters, false); - // Both children of model1 and model4 should not move as they are held + // Children of model4 and model5 should not move as they are held // in place EXPECT_EQ(childM4Poses.front(), childM4Poses.back()); EXPECT_EQ(childM5Poses.front(), childM5Poses.back()); @@ -282,9 +282,11 @@ TEST_F(DetachableJointTest, NestedModelsWithSameName) pub.Publish(msgs::Empty()); std::this_thread::sleep_for(250ms); - // Release M5's this->server->Run(true, nIters, false); - EXPECT_LT(childM5Poses.back().Z(), childM4Poses.front().Z()); + // M5 and M4 start at the same height + // Only M5 should fall. + EXPECT_LT(childM5Poses.back().Z(), childM4Poses.back().Z()); + EXPECT_LT(childM5Poses.back().Z(), childM5Poses.front().Z()); EXPECT_EQ(childM4Poses.front(), childM4Poses.back()); } ///////////////////////////////////////////////// diff --git a/test/worlds/detachable_joint_child.sdf b/test/worlds/detachable_joint_child.sdf index de73bacdc1..b281c42f1e 100644 --- a/test/worlds/detachable_joint_child.sdf +++ b/test/worlds/detachable_joint_child.sdf @@ -229,4 +229,4 @@ - \ No newline at end of file + From 1a7083dfeb05bd50e6358bbbd3377f6a13e0bb4e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 13 Aug 2024 10:28:10 +0800 Subject: [PATCH 21/49] Refactor out link discovery into a new function Signed-off-by: Arjo Chakravarty --- .../detachable_joint/DetachableJoint.cc | 201 +++++++++--------- .../detachable_joint/DetachableJoint.hh | 4 + 2 files changed, 106 insertions(+), 99 deletions(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 3a1bcaa865..91a3917a5d 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -210,136 +210,139 @@ void DetachableJoint::Configure(const Entity &_entity, .first; this->validConfig = true; + + this->GetChildModelAndLinkEntities(_ecm); } ////////////////////////////////////////////////// -void DetachableJoint::PreUpdate( - const UpdateInfo &/*_info*/, +void DetachableJoint::GetChildModelAndLinkEntities( EntityComponentManager &_ecm) { - IGN_PROFILE("DetachableJoint::PreUpdate"); - // only allow attaching if child entity is detached - if (this->validConfig && !this->isAttached) - { - // return if attach is not requested. - if (!this->attachRequested){ - return; - } - // Look for the child model and link - Entity modelEntity{kNullEntity}; + this->childLinkEntity = kNullEntity; + // Look for the child model and link + Entity modelEntity{kNullEntity}; - if ("__model__" == this->childModelName) + if ("__model__" == this->childModelName) + { + modelEntity = this->model.Entity(); + } + else + { + auto entitiesMatchingName = entitiesFromScopedName( + this->childModelName, _ecm); + + // TODO(arjoc): There is probably a more efficient way of combining entitiesFromScopedName + // With filtering. + // Filter for entities with only models + std::vector candidateEntities; + std::copy_if(entitiesMatchingName.begin(), entitiesMatchingName.end(), + std::back_inserter(candidateEntities), + [&_ecm](Entity e) { return _ecm.EntityHasComponentType(e, components::Model::typeId); }); + + if (candidateEntities.size() == 1) { - modelEntity = this->model.Entity(); + // If there is one entity select that entity itself + modelEntity = *candidateEntities.begin(); } else { - auto entitiesMatchingName = entitiesFromScopedName( - this->childModelName, _ecm); - - // TODO(arjoc): There is probably a more efficient way of combining entitiesFromScopedName - // With filtering. - // Filter for entities with only models - std::vector candidateEntities; - for (const auto entity: entitiesMatchingName) + std::string selectedModelName; + auto parentEntityScopedPath = scopedName(this->model.Entity(), _ecm); + // If there is more than one model with the given child model name, the plugin looks for a model which is + // - a descendant of the plugin's parent model with that name, and + // - has a child link with the given child link name + for (auto entity : candidateEntities) { - if (_ecm.EntityHasComponentType(entity, components::Model::typeId)) + auto childEntityScope = scopedName(entity, _ecm); + if (childEntityScope.size() < parentEntityScopedPath.size()) { - candidateEntities.push_back(entity); + continue; } - else + if (childEntityScope.rfind(parentEntityScopedPath, 0) != 0) { - ignwarn << "Found entity " << scopedName(entity, _ecm) - << " but it is not a model." << std::endl; + continue; } - } - - if (candidateEntities.size() == 1) - { - // If there is one entity select that entity itself - modelEntity = *candidateEntities.begin(); - } - else - { - std::string selectedModelName; - auto parentEntityScopedPath = scopedName(this->model.Entity(), _ecm); - // If there is more than one entity with the same name, look for the - // entity with a parent component. - for (auto entity : candidateEntities) + if (modelEntity == kNullEntity) { - auto childEntityScope = scopedName(entity, _ecm); - if (childEntityScope.size() < parentEntityScopedPath.size()) - { - continue; - } - if (childEntityScope.find(parentEntityScopedPath) != 0) - { - continue; - } - if (modelEntity == kNullEntity) - { - this->childLinkEntity = _ecm.EntityByComponents( - components::Link(), components::ParentEntity(entity), - components::Name(this->childLinkName)); - - if (kNullEntity != this->childLinkEntity) - { - // Only select child entity if tbe entity has a link - modelEntity = entity; - selectedModelName = childEntityScope; - igndbg << "Selecting " << childEntityScope << " as model to be detached" << std::endl; - } - else - { - ignwarn << "Found " << childEntityScope << " with no link named " << this->childLinkName <suppressChildWarning = true; - } + this->childLinkEntity = _ecm.EntityByComponents( + components::Link(), components::ParentEntity(entity), + components::Name(this->childLinkName)); + + if (kNullEntity != this->childLinkEntity) + { + // Only select this child model entity if the entity has a link with the given child link name + modelEntity = entity; + selectedModelName = childEntityScope; + igndbg << "Selecting " << childEntityScope << " as model to be detached" << std::endl; } else { - ignwarn << "Found multiple models skipping " << childEntityScope - << "Using " << selectedModelName << " instead" << std::endl; + ignwarn << "Found " << childEntityScope << " with no link named " << this->childLinkName << std::endl; } } + else + { + ignwarn << "Found multiple models skipping " << childEntityScope + << "Using " << selectedModelName << " instead" << std::endl; + } } } - if (kNullEntity != modelEntity) - { - this->childLinkEntity = _ecm.EntityByComponents( - components::Link(), components::ParentEntity(modelEntity), - components::Name(this->childLinkName)); + } + if (kNullEntity != modelEntity) + { + this->childLinkEntity = _ecm.EntityByComponents( + components::Link(), components::ParentEntity(modelEntity), + components::Name(this->childLinkName)); + } + else if (!this->suppressChildWarning) + { + ignwarn << "Child Model " << this->childModelName + << " could not be found.\n"; + } +} +////////////////////////////////////////////////// +void DetachableJoint::PreUpdate( + const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("DetachableJoint::PreUpdate"); + // only allow attaching if child entity is detached + if (this->validConfig && !this->isAttached) + { + // return if attach is not requested. + if (!this->attachRequested){ + return; + } - if (kNullEntity != this->childLinkEntity) - { - // Attach the models - // We do this by creating a detachable joint entity. - this->detachableJointEntity = _ecm.CreateEntity(); - - _ecm.CreateComponent( - this->detachableJointEntity, - components::DetachableJoint({this->parentLinkEntity, - this->childLinkEntity, "fixed"})); - this->attachRequested = false; - this->isAttached = true; - this->PublishJointState(this->isAttached); - igndbg << "Attaching entity: " << this->detachableJointEntity - << std::endl; - } - else - { - ignwarn << "Child Link " << this->childLinkName - << " could not be found.\n"; - } + if (childLinkEntity == kNullEntity || !_ecm.HasEntity(childLinkEntity)) + this->GetChildModelAndLinkEntities(_ecm); + + if (kNullEntity != this->childLinkEntity) + { + // Attach the models + // We do this by creating a detachable joint entity. + this->detachableJointEntity = _ecm.CreateEntity(); + + _ecm.CreateComponent( + this->detachableJointEntity, + components::DetachableJoint({this->parentLinkEntity, + this->childLinkEntity, "fixed"})); + this->attachRequested = false; + this->isAttached = true; + this->PublishJointState(this->isAttached); + igndbg << "Attaching entity: " << this->detachableJointEntity + << std::endl; } - else if (!this->suppressChildWarning) + else { - ignwarn << "Child Model " << this->childModelName + ignwarn << "Child Link " << this->childLinkName << " could not be found.\n"; } + } - // only allow detaching if child entity is attached + // only allow detaching if child entity is attached if (this->isAttached) { if (this->detachRequested && (kNullEntity != this->detachableJointEntity)) diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index f0f800b4e5..800a031b82 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/src/systems/detachable_joint/DetachableJoint.hh @@ -101,6 +101,10 @@ namespace systems /// \brief Callback for detach request topic private: void OnDetachRequest(const msgs::Empty &_msg); + /// \brief Retrieve the relevant link entity + private: void GetChildModelAndLinkEntities( + gz::sim::EntityComponentManager &_ecm); + /// \brief The model associated with this system. private: Model model; From d2e991c05e29a5ef4954cb28165a03b21dd03ab6 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 13 Aug 2024 10:56:13 +0800 Subject: [PATCH 22/49] Style Signed-off-by: Arjo Chakravarty --- src/systems/detachable_joint/DetachableJoint.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 91a3917a5d..b0bd5d9ecc 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -315,7 +315,7 @@ void DetachableJoint::PreUpdate( return; } - if (childLinkEntity == kNullEntity || !_ecm.HasEntity(childLinkEntity)) + if (this->childLinkEntity == kNullEntity || !_ecm.HasEntity(childLinkEntity)) this->GetChildModelAndLinkEntities(_ecm); if (kNullEntity != this->childLinkEntity) From 6fa7157a7beaa8735ecbde4dcc037934e3ca389c Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 13 Aug 2024 10:56:57 +0800 Subject: [PATCH 23/49] Style Signed-off-by: Arjo Chakravarty --- src/systems/detachable_joint/DetachableJoint.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index b0bd5d9ecc..379cd7b98d 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -315,7 +315,7 @@ void DetachableJoint::PreUpdate( return; } - if (this->childLinkEntity == kNullEntity || !_ecm.HasEntity(childLinkEntity)) + if (this->childLinkEntity == kNullEntity || !_ecm.HasEntity(this->childLinkEntity)) this->GetChildModelAndLinkEntities(_ecm); if (kNullEntity != this->childLinkEntity) From c3f271ca03b16d72e9ade88d79ba6dd18124c507 Mon Sep 17 00:00:00 2001 From: shameekganguly Date: Tue, 13 Aug 2024 12:27:36 -0700 Subject: [PATCH 24/49] Disable detachable_joint integration test case on Windows (#2523) Signed-off-by: Shameek Ganguly --- test/integration/detachable_joint.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index c461e5ff79..e585be38d5 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -217,7 +217,8 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) } ///////////////////////////////////////////////// -TEST_F(DetachableJointTest, NestedModelsWithSameName) +TEST_F(DetachableJointTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(NestedModelsWithSameName)) { using namespace std::chrono_literals; From a7ea4ac1bb4e86726b81dcece6712bf68a0854ff Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 12 Nov 2024 06:39:15 -0800 Subject: [PATCH 25/49] check valid pointer (#2674) (#2675) Signed-off-by: Ian Chen (cherry picked from commit 57599fb184090e11ed0cf58628a0327bb67a9f8d) Co-authored-by: Ian Chen --- src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index a6e2fec324..7763769c84 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -631,7 +631,7 @@ void OpticalTactilePluginPrivate::Enable(const ignition::msgs::Boolean &_req) this->enabled = _req.data(); - if (!_req.data()) + if (!_req.data() && this->visualizePtr) { this->visualizePtr->RemoveNormalForcesAndContactsMarkers(); } From 80225795b9994068acdd804865b2e20d51bba452 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 20 Nov 2024 08:54:22 +0800 Subject: [PATCH 26/49] Improve load times by skipping serialization of entities when unecessary. (backport #2596) (#2684) * Improve load times by skipping serialization of entities when unecessary. (#2596) * A hack to greatly improve load times I was investigating https://github.com/gazebosim/gazebo_test_cases/issues/1576 , in my investigation it came to my notice that `sdf::Element` takes forever to destroy (We should open a ticket somewhere about this). If we are skipping serialization we might as well not create and destroy an SDF Element. This hack greatly speeds up the load time for gazebo. Signed-off-by: Arjo Chakravarty Co-authored-by: Arjo Chakravarty Co-authored-by: Arjo Chakravarty --- include/gz/sim/components/Model.hh | 26 ++++++++++++++++++++------ 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/include/gz/sim/components/Model.hh b/include/gz/sim/components/Model.hh index e9aa8996df..6b23c7499e 100644 --- a/include/gz/sim/components/Model.hh +++ b/include/gz/sim/components/Model.hh @@ -75,10 +75,18 @@ namespace serializers } } - _out << "" - << "" - << (skip ? std::string() : modelElem->ToString("")) - << ""; + if (!skip) + { + _out << "" + << "" + << modelElem->ToString("") + << ""; + + } + else + { + _out << ""; + } return _out; } @@ -89,13 +97,19 @@ namespace serializers public: static std::istream &Deserialize(std::istream &_in, sdf::Model &_model) { - sdf::Root root; std::string sdf(std::istreambuf_iterator(_in), {}); + if (sdf.empty()) + { + return _in; + } + // Its super expensive to create an SDFElement for some reason + sdf::Root root; sdf::Errors errors = root.LoadSdfString(sdf); if (!root.Model()) { - ignwarn << "Unable to deserialize sdf::Model" << std::endl; + ignwarn << "Unable to deserialize sdf::Model: " << sdf + << std::endl; return _in; } From 0d3af5bfcfccdfc04b2aab8288c3dc793a142870 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Wed, 27 Nov 2024 19:01:36 +0100 Subject: [PATCH 27/49] Fix uncontrolled cast of size_t to uint (#2687) Signed-off-by: Jose Luis Rivero Signed-off-by: Jose Luis Rivero --- src/SimulationRunner.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 07255523b8..5cd5db461d 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -495,7 +495,8 @@ void SimulationRunner::ProcessSystemQueue() this->systemMgr->ActivatePendingSystems(); - auto threadCount = this->systemMgr->SystemsPostUpdate().size() + 1u; + unsigned int threadCount = static_cast( + this->systemMgr->SystemsPostUpdate().size() + 1u); igndbg << "Creating PostUpdate worker threads: " << threadCount << std::endl; From 5384cd80c23647e90aef84b9ee96a151cd59e5b9 Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Tue, 17 Dec 2024 17:52:13 +0900 Subject: [PATCH 28/49] Add parameter for adjust current sign in battery plugin (#2696) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Tatsuro Sakaguchi Co-authored-by: Alejandro Hernández Cordero (cherry picked from commit dd3e241fa431f45819a3b74e8dd5fe6b060b002f) --- src/systems/battery_plugin/LinearBatteryPlugin.cc | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index d4744f0f13..08ddf1eeb1 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.cc +++ b/src/systems/battery_plugin/LinearBatteryPlugin.cc @@ -178,6 +178,9 @@ class ignition::gazebo::systems::LinearBatteryPluginPrivate /// \brief Initial power load set trough config public: double initialPowerLoad = 0.0; + + /// \brief Flag to invert the current sign + public: bool invertCurrentSign{false}; }; ///////////////////////////////////////////////// @@ -273,6 +276,10 @@ void LinearBatteryPlugin::Configure(const Entity &_entity, if (_sdf->HasElement("fix_issue_225")) this->dataPtr->fixIssue225 = _sdf->Get("fix_issue_225"); + if (_sdf->HasElement("invert_current_sign")) + this->dataPtr->invertCurrentSign = + _sdf->Get("invert_current_sign"); + if (_sdf->HasElement("battery_name") && _sdf->HasElement("voltage")) { this->dataPtr->batteryName = _sdf->Get("battery_name"); @@ -615,7 +622,10 @@ void LinearBatteryPlugin::PostUpdate(const UpdateInfo &_info, msg.mutable_header()->mutable_stamp()->CopyFrom( convert(_info.simTime)); msg.set_voltage(this->dataPtr->battery->Voltage()); - msg.set_current(this->dataPtr->ismooth); + if (this->dataPtr->invertCurrentSign) + msg.set_current(-this->dataPtr->ismooth); + else + msg.set_current(this->dataPtr->ismooth); msg.set_charge(this->dataPtr->q); msg.set_capacity(this->dataPtr->c); From d9b18d85a17400d32494bca4c3589f519aca7e81 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Mon, 13 Jan 2025 13:24:10 +0100 Subject: [PATCH 29/49] Preparation for 6.17.0 release (#2712) * Bump version to 6.17.0 --------- Signed-off-by: Jose Luis Rivero Co-authored-by: Ian Chen --- CMakeLists.txt | 2 +- Changelog.md | 62 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 63 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d287de59aa..9bd795d3bb 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-gazebo6 VERSION 6.16.0) +project(ignition-gazebo6 VERSION 6.17.0) set (GZ_DISTRIBUTION "Fortress") #============================================================================ diff --git a/Changelog.md b/Changelog.md index 454915324a..f97c229d52 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,67 @@ ## Ignition Gazebo 6.x +### Gazebo Sim 6.17.0 (2025-01-10) + +1. Add parameter for adjust current sign in battery plugin + * [Pull request #2696](https://github.com/gazebosim/gz-sim/pull/2696) + +1. Fix uncontrolled cast of size_t to uint + * [Pull request #2687](https://github.com/gazebosim/gz-sim/pull/2687) + +1. Improve load times by skipping serialization of entities when unecessary + * [Pull request #2596](https://github.com/gazebosim/gz-sim/pull/2596) + +1. Fix crash in OpticalTactilePlugin by checking for valid visualize pointer + * [Pull request #2674](https://github.com/gazebosim/gz-sim/pull/2674) + +1. Disable detachable_joint integration test case on Windows + * [Pull request #2523](https://github.com/gazebosim/gz-sim/pull/2523) + +1. Initialize threadsNeedCleanUp + * [Pull request #2503](https://github.com/gazebosim/gz-sim/pull/2503) + +1. Remove systems if their parent entity is removed + * [Pull request #2232](https://github.com/gazebosim/gz-sim/pull/2232) + +1. Disable failing testFixture_TEST for MacOS + * [Pull request #2499](https://github.com/gazebosim/gz-sim/pull/2499) + +1. backport lidar visualization frame_id fix + * [Pull request #2483](https://github.com/gazebosim/gz-sim/pull/2483) + +1. Fix DLL linkage/visibility issues + * [Pull request #2254](https://github.com/gazebosim/gz-sim/pull/2254) + +1. Address a few Windows CI Issues + * [Pull request #1911](https://github.com/gazebosim/gz-sim/pull/1911) + +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. Use VERSION_GREATER_EQUAL in cmake logic + * [Pull request #2418](https://github.com/gazebosim/gz-sim/pull/2418) + +1. Rephrase cmake comment about CMP0077 + * [Pull request #2419](https://github.com/gazebosim/gz-sim/pull/2419) + +1. Fix bug where iterator was used after the underlying item was erased from the container + * [Pull request #2412](https://github.com/gazebosim/gz-sim/pull/2412) + +1. Fix namespace and class links in documentation references that use namespace `gz` + * [Pull request #2385](https://github.com/gazebosim/gz-sim/pull/2385) + +1. Fix ModelPhotoShootTest test failures + * [Pull request #2294](https://github.com/gazebosim/gz-sim/pull/2294) + +1. Setup rendering environment before cmake runs + * [Pull request #1965](https://github.com/gazebosim/gz-sim/pull/1965) + +1. Detachable joint: support for nested models of the same name + * [Pull request 1097](https://github.com/gazebosim/gz-sim/pull/1097) + ### Gazebo Sim 6.16.0 (2024-01-12) 1. Allow using plugin file names and environment variables compatible with Garden and later From 2f1c49f889c875f8d736321975d0f483c5083d71 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 27 Jan 2025 01:01:47 -0800 Subject: [PATCH 30/49] Fix mesh import filters not displaying correctly on KDE #2731 (#2732) (#2735) Signed-off-by: Josh Newans (cherry picked from commit 06e16bc33dfc92e42801537e2da6f9b8036e9108) Co-authored-by: Josh Newans --- src/gui/plugins/entity_tree/EntityTree.qml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gui/plugins/entity_tree/EntityTree.qml b/src/gui/plugins/entity_tree/EntityTree.qml index c96c5d78a8..4a47bb2b71 100644 --- a/src/gui/plugins/entity_tree/EntityTree.qml +++ b/src/gui/plugins/entity_tree/EntityTree.qml @@ -164,7 +164,7 @@ Rectangle { id: loadFileDialog title: "Load mesh" folder: shortcuts.home - nameFilters: [ "Collada files (*.dae)", "(*.stl)", "(*.obj)" ] + nameFilters: [ "Collada files (*.dae)", "STL (*.stl)", "Wavefront OBJ (*.obj)" ] selectMultiple: false selectExisting: true onAccepted: { From 36c15334e426488f94fa8d15d094e9829f6a084e Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 31 Jan 2025 04:25:14 +0100 Subject: [PATCH 31/49] Reduce/Eliminate `sdf::Model` and `sdf::World` serialization warnings (#2742) - Reduces serialization warnings on `sdf::Model` so it only occurs when the `relative_to` attribute is set. This was the intended behavior originally, but there was a bug in the code. - Eliminate `sdf::World` warnings by specializing the `DefaultSerializer` for `sdf::World`. --------- Signed-off-by: Addisu Z. Taddese (cherry picked from commit 1739bdbeaff9118813cd447fcb442e80ec0f846a) Signed-off-by: Addisu Z. Taddese --- include/gz/sim/components/Model.hh | 2 +- include/gz/sim/components/World.hh | 44 ++++++++++++++++++++++++++++++ test/worlds/gpu_lidar.sdf | 2 +- 3 files changed, 46 insertions(+), 2 deletions(-) diff --git a/include/gz/sim/components/Model.hh b/include/gz/sim/components/Model.hh index 6b23c7499e..2e0a756f17 100644 --- a/include/gz/sim/components/Model.hh +++ b/include/gz/sim/components/Model.hh @@ -54,7 +54,7 @@ namespace serializers if (modelElem->HasElement("pose")) { sdf::ElementPtr poseElem = modelElem->GetElement("pose"); - if (poseElem->HasAttribute("relative_to")) + if (poseElem->GetAttribute("relative_to")->GetSet()) { // Skip serializing models with //pose/@relative_to attribute // since deserialization will fail. This could be a nested model. diff --git a/include/gz/sim/components/World.hh b/include/gz/sim/components/World.hh index 565caf1df1..e040f4e317 100644 --- a/include/gz/sim/components/World.hh +++ b/include/gz/sim/components/World.hh @@ -19,6 +19,9 @@ #include +#include +#include + #include #include #include @@ -29,6 +32,47 @@ namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + +// The following is only needed to keep ABI compatibility +// TODO(azeey) Remove in main +namespace traits +{ +template<> +class IsOutStreamable +{ + public: static constexpr bool value = false; // NOLINT +}; + +template<> +class IsInStreamable +{ + public: static constexpr bool value = false; // NOLINT +}; +} + +namespace serializers +{ + +/// \brief Specialize the DefaultSerializer on sdf::World so we can +/// skip serialization +/// TODO(azeey) Do we ever want to serialize this component? +template <> +class DefaultSerializer +{ + public: + static std::ostream &Serialize(std::ostream &_out, const sdf::World &) + { + return _out; + } + + public: + static std::istream &Deserialize(std::istream &_in, sdf::World &) + { + return _in; + } +}; +} + namespace components { /// \brief A component that identifies an entity as being a world. diff --git a/test/worlds/gpu_lidar.sdf b/test/worlds/gpu_lidar.sdf index e029f7fe1f..862080333b 100644 --- a/test/worlds/gpu_lidar.sdf +++ b/test/worlds/gpu_lidar.sdf @@ -55,7 +55,7 @@ 0.01 - 1 + 1 true From cb6fa87454c0eae212c4db6e53538cb8052f67ac Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 13 Mar 2025 21:49:39 +0100 Subject: [PATCH 32/49] Also handle SIGTERM gracefully (#2747) (#2757) Signed-off-by: Erling Rennemo Jellum (cherry picked from commit 13bf5beacdfab28eef549ebdf1016da01f0b42e3) Signed-off-by: Erling Rennemo Jellum Signed-off-by: erlingrj Signed-off-by: erling Signed-off-by: Addisu Z. Taddese Co-authored-by: erling Co-authored-by: Addisu Z. Taddese Co-authored-by: Ian Chen --- src/cmd/cmdgazebo.rb.in | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/cmd/cmdgazebo.rb.in b/src/cmd/cmdgazebo.rb.in index 07d36a7bb2..5a70dcee14 100755 --- a/src/cmd/cmdgazebo.rb.in +++ b/src/cmd/cmdgazebo.rb.in @@ -504,9 +504,19 @@ See https://github.com/gazebosim/gz-sim/issues/168 for more info." options['wait_gui'], options['render_engine_gui']) end - Signal.trap("INT") { + # Handle SIGINT and SIGTERM signals + def handle_signal(guiPid, serverPid) self.killProcess(guiPid, "Ignition Gazebo GUI", 5.0) self.killProcess(serverPid, "Ignition Gazebo Server", 5.0) + end + + Signal.trap("INT") { + handle_signal(guiPid, serverPid) + return 1 + } + + Signal.trap("TERM") { + handle_signal(guiPid, serverPid) return 1 } From 5e3175771eb2d969fa1b0e1342d677a5c281343c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 25 Jun 2025 23:51:07 -0700 Subject: [PATCH 33/49] Set IGN_IP=127.0.0.1 in cmd tests (#2959) Manual backport of #2884. Signed-off-by: Steve Peters --- src/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 969da19349..d135f10295 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -242,6 +242,7 @@ foreach(CMD_TEST set(_env_vars) list(APPEND _env_vars "IGN_CONFIG_PATH=${CMAKE_BINARY_DIR}/test/conf") list(APPEND _env_vars "IGN_GAZEBO_SYSTEM_PLUGIN_PATH=$") + list(APPEND _env_vars "IGN_IP=127.0.0.1") set_tests_properties(${CMD_TEST} PROPERTIES ENVIRONMENT "${_env_vars}") From 9eeb58daa52c19392dba1251a1f71611751de8d6 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 31 Jul 2025 09:34:20 -0700 Subject: [PATCH 34/49] Add support for loading physics engine plugins from static plugin registry (#2991) Signed-off-by: Ian Chen --- BUILD.bazel | 2 + include/gz/sim/Util.hh | 12 ++ src/SystemLoader.cc | 10 +- src/Util.cc | 11 ++ src/Util_TEST.cc | 9 + src/systems/physics/Physics.cc | 158 +++++++++++------- test/BUILD.bazel | 16 ++ .../load_physics_system_static_registry.cc | 114 +++++++++++++ 8 files changed, 262 insertions(+), 70 deletions(-) create mode 100644 test/integration/load_physics_system_static_registry.cc diff --git a/BUILD.bazel b/BUILD.bazel index fa1b95792b..5fbd1714df 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -674,6 +674,7 @@ gz_sim_system_libraries( "@gz-sensors", "@gz-sensors//:dvl", "@gz-transport", + "@gz-utils//:ImplPtr", ], ) @@ -1556,6 +1557,7 @@ gz_sim_system_libraries( visibility = ["//visibility:public"], deps = [ ":gz-sim", + "@gz-common//:gz-common", "@gz-common//profiler", "@gz-math", "@gz-msgs", diff --git a/include/gz/sim/Util.hh b/include/gz/sim/Util.hh index a88dad9b89..fd3f977294 100644 --- a/include/gz/sim/Util.hh +++ b/include/gz/sim/Util.hh @@ -342,6 +342,15 @@ namespace gz GZ_SIM_VISIBLE std::optional meshAxisAlignedBox( const sdf::Mesh &_sdfMesh); + /// \brief Get the static plugin prefix + /// \return The static plugin prefix + GZ_SIM_VISIBLE const std::string &staticPluginPrefixStr(); + + /// \brief Check if input filename of a library is a static plugin or not. + /// \param _filename_ Library filename to check + /// \return True if input filename has a static plugin string format. + GZ_SIM_VISIBLE bool isStaticPlugin(const std::string &_filename); + /// \brief Environment variable holding resource paths. const std::string kResourcePathEnv{"GZ_SIM_RESOURCE_PATH"}; @@ -355,6 +364,9 @@ namespace gz /// \brief Environment variable holding paths to custom rendering engine /// plugins. const std::string kRenderPluginPathEnv{"GZ_SIM_RENDER_ENGINE_PATH"}; + + /// \brief Static plugin filename prefix string. + const std::string kStaticPluginFilenamePrefix{"static://"}; } } } diff --git a/src/SystemLoader.cc b/src/SystemLoader.cc index aaede7742e..3144bf95da 100644 --- a/src/SystemLoader.cc +++ b/src/SystemLoader.cc @@ -34,16 +34,13 @@ #include #include "gz/sim/InstallationDirectories.hh" +#include "gz/sim/Util.hh" #include using namespace gz::sim; class gz::sim::SystemLoaderPrivate { - ////////////////////////////////////////////////// - public: static constexpr std::string_view kStaticPluginFilenamePrefix = - "static://"; - ////////////////////////////////////////////////// public: explicit SystemLoaderPrivate() = default; @@ -85,7 +82,7 @@ class gz::sim::SystemLoaderPrivate public: bool InstantiateStaticSystemPlugin(const sdf::Plugin &_sdfPlugin, gz::plugin::PluginPtr &_gzPlugin) { - const size_t prefixLen = kStaticPluginFilenamePrefix.size(); + const size_t prefixLen = staticPluginPrefixStr().size(); const std::string filenameWoPrefix = _sdfPlugin.Filename().substr(prefixLen); std::string pluginToInstantiate = @@ -136,8 +133,7 @@ class gz::sim::SystemLoaderPrivate << "]. Using [" << filename << "] instead." << std::endl; } - if (filename.substr(0, kStaticPluginFilenamePrefix.size()) == - kStaticPluginFilenamePrefix) + if (isStaticPlugin(filename)) { return this->InstantiateStaticSystemPlugin(_sdfPlugin, _gzPlugin); } diff --git a/src/Util.cc b/src/Util.cc index 175c2b964b..f3ecc17000 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -1022,6 +1022,17 @@ math::AxisAlignedBox transformAxisAlignedBox( ); } +const std::string &staticPluginPrefixStr() +{ + return kStaticPluginFilenamePrefix; +} + +bool isStaticPlugin(const std::string &_filename) +{ + return _filename.substr(0, staticPluginPrefixStr().size()) == + staticPluginPrefixStr(); +} + } } } diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index f382cebd11..08eb11740e 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -1136,3 +1136,12 @@ TEST_F(UtilTest, TransformAxisAlignedBoxFrame) aabExp = aabExp + transform.Pos(); EXPECT_EQ(aabExp, transformAxisAlignedBox(aab, transform)); } + +///////////////////////////////////////////////// +TEST_F(UtilTest, StaticPlugin) +{ + EXPECT_FALSE(staticPluginPrefixStr().empty()); + EXPECT_FALSE(isStaticPlugin("my_plugin")); + EXPECT_FALSE(isStaticPlugin("")); + EXPECT_TRUE(isStaticPlugin(staticPluginPrefixStr() + "my_plugin")); +} diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index d51aea46ca..0718c99881 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -852,87 +852,119 @@ void Physics::Configure(const Entity &_entity, "include_entity_names", true).first; } - // Find engine shared library - // Look in: - // * Paths from environment variable - // * Engines installed with gz-physics - common::SystemPaths systemPaths; - systemPaths.SetPluginPathEnv(this->dataPtr->pluginPathEnv); - systemPaths.AddPluginPaths(gz::physics::getEngineInstallDir()); - - auto pathToLib = systemPaths.FindSharedLibrary(pluginLib); - - if (pathToLib.empty()) + plugin::Loader pluginLoader; + if (isStaticPlugin(pluginLib)) { - gzerr << "Failed to find plugin [" << pluginLib - << "]. Have you checked the " << this->dataPtr->pluginPathEnv - << " environment variable?" << std::endl; + const size_t prefixLen = staticPluginPrefixStr().size(); + const std::string pluginToInstantiate = + pluginLib.substr(prefixLen); + auto plugin = pluginLoader.Instantiate(pluginToInstantiate); + if (!plugin) + { + gzerr << "Failed to load physics engine plugin: " + << "(Reason: static plugin registry does not contain the " + << "requested plugin)\n" + << "- Requested plugin name: [" << pluginLib << "]\n"; + return; + } - return; - } + this->dataPtr->engine = physics::RequestEngine< + physics::FeaturePolicy3d, + PhysicsPrivate::MinimumFeatureList>::From(plugin); - // Load engine plugin - plugin::Loader pluginLoader; - auto plugins = pluginLoader.LoadLib(pathToLib); - if (plugins.empty()) - { - gzerr << "Unable to load the [" << pathToLib << "] library.\n"; - return; + if (!this->dataPtr->engine) + { + gzerr << "Failed to load physics engine from static plugin registry: " + << "(Reason: Physics engine does not meet the minimum features " + << "requirement)\n" + << "- Requested plugin name: [" << pluginLib << "]\n"; + return; + } + gzdbg << "Loaded [" << pluginLib <<"] from the static plugin registry" + << std::endl; } - - auto classNames = pluginLoader.PluginsImplementing< - physics::ForwardStep::Implementation< - physics::FeaturePolicy3d>>(); - if (classNames.empty()) + else { - gzerr << "No physics plugins implementing required interface found in " - << "library [" << pathToLib << "]." << std::endl; - return; - } + // Find engine shared library + // Look in: + // * Paths from environment variable + // * Engines installed with gz-physics + common::SystemPaths systemPaths; + systemPaths.SetPluginPathEnv(this->dataPtr->pluginPathEnv); + systemPaths.AddPluginPaths(gz::physics::getEngineInstallDir()); - // Get the first plugin that works - for (auto className : classNames) - { - auto plugin = pluginLoader.Instantiate(className); + auto pathToLib = systemPaths.FindSharedLibrary(pluginLib); - if (!plugin) + if (pathToLib.empty()) { - gzwarn << "Failed to instantiate [" << className << "] from [" - << pathToLib << "]" << std::endl; - continue; + gzerr << "Failed to find plugin [" << pluginLib + << "]. Have you checked the " << this->dataPtr->pluginPathEnv + << " environment variable?" << std::endl; + + return; } - this->dataPtr->engine = physics::RequestEngine< - physics::FeaturePolicy3d, - PhysicsPrivate::MinimumFeatureList>::From(plugin); + // Load engine plugin + auto plugins = pluginLoader.LoadLib(pathToLib); + if (plugins.empty()) + { + gzerr << "Unable to load the [" << pathToLib << "] library.\n"; + return; + } - if (nullptr != this->dataPtr->engine) + auto classNames = pluginLoader.PluginsImplementing< + physics::ForwardStep::Implementation< + physics::FeaturePolicy3d>>(); + if (classNames.empty()) { - gzdbg << "Loaded [" << className << "] from library [" - << pathToLib << "]" << std::endl; - break; + gzerr << "No physics plugins implementing required interface found in " + << "library [" << pathToLib << "]." << std::endl; + return; } - auto missingFeatures = physics::RequestEngine< + // Get the first plugin that works + for (auto className : classNames) + { + auto plugin = pluginLoader.Instantiate(className); + + if (!plugin) + { + gzwarn << "Failed to instantiate [" << className << "] from [" + << pathToLib << "]" << std::endl; + continue; + } + + this->dataPtr->engine = physics::RequestEngine< physics::FeaturePolicy3d, - PhysicsPrivate::MinimumFeatureList>::MissingFeatureNames(plugin); + PhysicsPrivate::MinimumFeatureList>::From(plugin); - std::stringstream msg; - msg << "Plugin [" << className << "] misses required features:" - << std::endl; - for (auto feature : missingFeatures) + if (nullptr != this->dataPtr->engine) + { + gzdbg << "Loaded [" << className << "] from library [" + << pathToLib << "]" << std::endl; + break; + } + + auto missingFeatures = physics::RequestEngine< + physics::FeaturePolicy3d, + PhysicsPrivate::MinimumFeatureList>::MissingFeatureNames(plugin); + + std::stringstream msg; + msg << "Plugin [" << className << "] misses required features:" + << std::endl; + for (auto feature : missingFeatures) + { + msg << "- " << feature << std::endl; + } + gzwarn << msg.str(); + } + if (nullptr == this->dataPtr->engine) { - msg << "- " << feature << std::endl; + gzerr << "Failed to load a valid physics engine from [" << pathToLib + << "]." + << std::endl; + return; } - gzwarn << msg.str(); - } - - if (nullptr == this->dataPtr->engine) - { - gzerr << "Failed to load a valid physics engine from [" << pathToLib - << "]." - << std::endl; - return; } this->dataPtr->eventManager = &_eventMgr; diff --git a/test/BUILD.bazel b/test/BUILD.bazel index 45285a44c7..0feb9e5295 100644 --- a/test/BUILD.bazel +++ b/test/BUILD.bazel @@ -163,6 +163,22 @@ cc_test( ], ) +cc_test( + name = "INTEGRATION_load_physics_system_static_registry", + srcs = ["integration/load_physics_system_static_registry.cc"], + env = {"GZ_BAZEL": "1"}, + deps = [ + ":Helpers", + ":MockSystem", + "//:gz-sim-physics-system-static", + "//:gz-sim", + "@googletest//:gtest", + "@googletest//:gtest_main", + "@gz-math//:gz-math", + "@gz-physics//dartsim:libgz-physics-dartsim-plugin-static", + ], +) + filegroup( name = "worlds", srcs = glob(["worlds/**"]), diff --git a/test/integration/load_physics_system_static_registry.cc b/test/integration/load_physics_system_static_registry.cc new file mode 100644 index 0000000000..017e04aeda --- /dev/null +++ b/test/integration/load_physics_system_static_registry.cc @@ -0,0 +1,114 @@ +/* + * Copyright (C) 2025 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include +#include + +#include +#include + +#include "../helpers/EnvTestFixture.hh" +#include "gz/sim/Link.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/Util.hh" +#include "gz/sim/components/Link.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/PhysicsEnginePlugin.hh" +#include "plugins/MockSystem.hh" + +using namespace gz; +using namespace sim; + +/// \brief Test loading physics system and physics plugin from static +/// plugin registry +class LoadPhysicsSystemStaticRegistryTest + : public InternalFixture<::testing::Test> {}; + +TEST_F(LoadPhysicsSystemStaticRegistryTest, LoadDartsim) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfString(R"( + + + + + + static://gz::physics::dartsim::Plugin + + + + + + + + 1 1 1 + + + + + + + )"); + + Server server(serverConfig); + + // Verify that server was initialized correctly + auto iterationCount = server.IterationCount(); + ASSERT_NE(iterationCount, std::nullopt); + ASSERT_EQ(*iterationCount, 0); + + std::optional modelId = server.EntityByName("test_model"); + ASSERT_NE(modelId, std::nullopt); + + EntityComponentManager *ecm{nullptr}; + auto mockSystem = std::make_shared(); + mockSystem->preUpdateCallback = + [&ecm](const UpdateInfo &, EntityComponentManager &_ecm) + { + ecm = &_ecm; + }; + ASSERT_TRUE(server.AddSystem(mockSystem)); + + server.RunOnce(); + EXPECT_EQ(1, mockSystem->preUpdateCallCount); + + // Verify that the physics system loaded the static dartsim plugin. + auto plugin = ecm->ComponentData( + worldEntity(*ecm)); + ASSERT_TRUE(plugin.has_value()); + EXPECT_EQ("static://gz::physics::dartsim::Plugin", plugin.value()); + + // Verify that physics engine is running by checking that + // the link is falling and has negative world linear velocity. + const std::string linkName{"link_1"}; + auto linkEntity = ecm->EntityByComponents( + components::Link(), components::Name(linkName)); + sim::Link link(linkEntity); + link.EnableVelocityChecks(*ecm, true); + server.RunOnce(false); + std::optional linkPose = link.WorldPose(*ecm); + ASSERT_TRUE(linkPose.has_value()); + EXPECT_GT(0, linkPose->Pos().Z()); + std::optional linkLinearVel = link.WorldLinearVelocity(*ecm); + ASSERT_TRUE(linkLinearVel.has_value()); + EXPECT_GT(0, linkLinearVel->Z()); +} From ec0cac5675c0a0ec4c744bc603f36ff4da40d6cb Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 5 Aug 2025 23:25:39 -0500 Subject: [PATCH 35/49] Assign new gz-sim maintainer (#3008) Signed-off-by: Addisu Z. Taddese (cherry picked from commit 6a2df6548d16bc5393e6da8e73e45807b4dd4d98) --- .github/CODEOWNERS | 4 +--- README.md | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index af0b4c43fe..26241d9833 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,10 +1,8 @@ # More info: # https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners -* @mjcarroll +* @arjo129 */rendering/* @iche033 -examples/* @mabelzhang src/systems/physics/* @azeey src/systems/sensors/* @iche033 */gui/* @jennuine -tutorials/* @mabelzhang diff --git a/README.md b/README.md index 879f47f06c..9dd298de04 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Gazebo Sim : A Robotic Simulator -**Maintainer:** michael AT openrobotics DOT org +**Maintainer:** arjoc AT intrinsic DOT ai [![GitHub open issues](https://img.shields.io/github/issues-raw/gazebosim/gz-sim.svg)](https://github.com/gazebosim/gz-sim/issues) [![GitHub open pull requests](https://img.shields.io/github/issues-pr-raw/gazebosim/gz-sim.svg)](https://github.com/gazebosim/gz-sim/pulls) From 6f1f8670c679f622fa6fc03caa7b0e7dec47abd7 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 5 Aug 2025 23:25:39 -0500 Subject: [PATCH 36/49] Assign new gz-sim maintainer (#3008) Signed-off-by: Addisu Z. Taddese (cherry picked from commit 6a2df6548d16bc5393e6da8e73e45807b4dd4d98) --- .github/CODEOWNERS | 4 +--- README.md | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index af0b4c43fe..26241d9833 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,10 +1,8 @@ # More info: # https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners -* @mjcarroll +* @arjo129 */rendering/* @iche033 -examples/* @mabelzhang src/systems/physics/* @azeey src/systems/sensors/* @iche033 */gui/* @jennuine -tutorials/* @mabelzhang diff --git a/README.md b/README.md index 96cd4cd756..3ca11782c5 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Gazebo Sim : A Robotic Simulator -**Maintainer:** michael AT openrobotics DOT org +**Maintainer:** arjoc AT intrinsic DOT ai [![GitHub open issues](https://img.shields.io/github/issues-raw/gazebosim/gz-sim.svg)](https://github.com/gazebosim/gz-sim/issues) [![GitHub open pull requests](https://img.shields.io/github/issues-pr-raw/gazebosim/gz-sim.svg)](https://github.com/gazebosim/gz-sim/pulls) From b2a902271de55ff998cc2af88075a521a74e4281 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 5 Aug 2025 23:25:39 -0500 Subject: [PATCH 37/49] Assign new gz-sim maintainer (#3008) Signed-off-by: Addisu Z. Taddese (cherry picked from commit 6a2df6548d16bc5393e6da8e73e45807b4dd4d98) --- .github/CODEOWNERS | 2 +- README.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 08a27c0825..26241d9833 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,7 +1,7 @@ # More info: # https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners -* @mjcarroll +* @arjo129 */rendering/* @iche033 src/systems/physics/* @azeey src/systems/sensors/* @iche033 diff --git a/README.md b/README.md index bf953ce735..a779a219c1 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Gazebo Sim : A Robotic Simulator -**Maintainer:** michael AT openrobotics DOT org +**Maintainer:** arjoc AT intrinsic DOT ai [![GitHub open issues](https://img.shields.io/github/issues-raw/gazebosim/gz-sim.svg)](https://github.com/gazebosim/gz-sim/issues) [![GitHub open pull requests](https://img.shields.io/github/issues-pr-raw/gazebosim/gz-sim.svg)](https://github.com/gazebosim/gz-sim/pulls) From 1ee77fca45d5faba95e2d8eb536fe7be56a4d85c Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 12 Sep 2024 14:28:51 -0700 Subject: [PATCH 38/49] Fix log playback GUI display (#2611) Loading the logging playback plugin before `SceneBroadcaster` prevents playback from being displayed on the GUI. --------- Signed-off-by: Addisu Z. Taddese Co-authored-by: Arjo Chakravarty (cherry picked from commit ca40c1db3dc0b6d862916c4990a9bfb35a62d0e8) # Conflicts: # src/SimulationRunner.cc --- src/SimulationRunner.cc | 42 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 40 insertions(+), 2 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 5ce443ff19..816d5a0bf9 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -1587,8 +1587,6 @@ void SimulationRunner::CreateEntities(const sdf::World &_world) this->entityCompMgr.ProcessRemoveEntityRequests(); this->entityCompMgr.ClearRemovedComponents(); - this->LoadLoggingPlugins(this->serverConfig); - // Load any additional plugins from the Server Configuration this->LoadServerPlugins(this->serverConfig.Plugins()); @@ -1598,9 +1596,49 @@ void SimulationRunner::CreateEntities(const sdf::World &_world) { gzmsg << "No systems loaded from SDF, loading defaults" << std::endl; bool isPlayback = !this->serverConfig.LogPlaybackPath().empty(); +<<<<<<< HEAD auto plugins = gz::sim::loadPluginInfo(isPlayback); this->LoadServerPlugins(plugins); } +======= + auto defaultPlugins = gz::sim::loadPluginInfo(isPlayback); + if (loadedWorldPlugins.empty()) + { + gzmsg << "No systems loaded from SDF, loading defaults" << std::endl; + } + else + { + std::unordered_set loadedWorldPluginFileNames; + for (const auto &pl : loadedWorldPlugins) + { + loadedWorldPluginFileNames.insert(pl.fname); + } + auto isPluginLoaded = + [&loadedWorldPluginFileNames](const ServerConfig::PluginInfo &_pl) + { + return loadedWorldPluginFileNames.count(_pl.Plugin().Filename()) != 0; + }; + + // Remove plugin if it's already loaded so as to not duplicate world + // plugins. + defaultPlugins.remove_if(isPluginLoaded); + + gzdbg << "Additional plugins to load:\n"; + for (const auto &plugin : defaultPlugins) + { + gzdbg << plugin.Plugin().Name() << " " << plugin.Plugin().Filename() + << "\n"; + } + } + + this->LoadServerPlugins(defaultPlugins); + // Load logging plugins after all server plugins so that necessary + // plugins such as SceneBroadcaster are loaded first. This might be + // a bug or an assumption made in the logging plugins. + this->LoadLoggingPlugins(this->serverConfig); + + }; +>>>>>>> ca40c1db (Fix log playback GUI display (#2611)) // Store the initial state of the ECM; this->initialEntityCompMgr.CopyFrom(this->entityCompMgr); From 8943d8270b1a388d1c41fe9c71fa3f704942ec8f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 5 Aug 2025 07:56:23 +0000 Subject: [PATCH 39/49] Backport changes Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.cc | 47 +++++------------------------------------ 1 file changed, 5 insertions(+), 42 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 816d5a0bf9..899ec1cf06 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -504,7 +504,7 @@ void SimulationRunner::AddSystem(const SystemPluginPtr &_system, void SimulationRunner::AddSystem( const std::shared_ptr &_system, std::optional _entity, - std::optional> _sdf) + std::optional> _sdf) { auto entity = _entity.value_or(worldEntity(this->entityCompMgr)); auto sdf = _sdf.value_or(createEmptyPluginElement()); @@ -1596,50 +1596,13 @@ void SimulationRunner::CreateEntities(const sdf::World &_world) { gzmsg << "No systems loaded from SDF, loading defaults" << std::endl; bool isPlayback = !this->serverConfig.LogPlaybackPath().empty(); -<<<<<<< HEAD auto plugins = gz::sim::loadPluginInfo(isPlayback); this->LoadServerPlugins(plugins); } -======= - auto defaultPlugins = gz::sim::loadPluginInfo(isPlayback); - if (loadedWorldPlugins.empty()) - { - gzmsg << "No systems loaded from SDF, loading defaults" << std::endl; - } - else - { - std::unordered_set loadedWorldPluginFileNames; - for (const auto &pl : loadedWorldPlugins) - { - loadedWorldPluginFileNames.insert(pl.fname); - } - auto isPluginLoaded = - [&loadedWorldPluginFileNames](const ServerConfig::PluginInfo &_pl) - { - return loadedWorldPluginFileNames.count(_pl.Plugin().Filename()) != 0; - }; - - // Remove plugin if it's already loaded so as to not duplicate world - // plugins. - defaultPlugins.remove_if(isPluginLoaded); - - gzdbg << "Additional plugins to load:\n"; - for (const auto &plugin : defaultPlugins) - { - gzdbg << plugin.Plugin().Name() << " " << plugin.Plugin().Filename() - << "\n"; - } - } - - this->LoadServerPlugins(defaultPlugins); - // Load logging plugins after all server plugins so that necessary - // plugins such as SceneBroadcaster are loaded first. This might be - // a bug or an assumption made in the logging plugins. - this->LoadLoggingPlugins(this->serverConfig); - - }; ->>>>>>> ca40c1db (Fix log playback GUI display (#2611)) - + // Load logging plugins after all server plugins so that necessary + // plugins such as SceneBroadcaster are loaded first. This might be + // a bug or an assumption made in the logging plugins. + this->LoadLoggingPlugins(this->serverConfig); // Store the initial state of the ECM; this->initialEntityCompMgr.CopyFrom(this->entityCompMgr); From e5cd8f95a836e30bb3adae4e152d9b473c8515a9 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 5 Aug 2025 08:35:55 +0000 Subject: [PATCH 40/49] Style Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 899ec1cf06..21cf1ca3f7 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -504,7 +504,7 @@ void SimulationRunner::AddSystem(const SystemPluginPtr &_system, void SimulationRunner::AddSystem( const std::shared_ptr &_system, std::optional _entity, - std::optional> _sdf) + std::optional> _sdf) { auto entity = _entity.value_or(worldEntity(this->entityCompMgr)); auto sdf = _sdf.value_or(createEmptyPluginElement()); From 98b60379053d6224dd8f2d738f6b9703a8dadaf7 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 6 Aug 2025 05:45:06 +0000 Subject: [PATCH 41/49] Update test expectations Signed-off-by: Arjo Chakravarty --- src/Server_TEST.cc | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index b07157b094..a7c327f0db 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -442,8 +442,7 @@ TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(ServerConfigLogRecord)) EXPECT_EQ(0u, *server.IterationCount()); EXPECT_EQ(3u, *server.EntityCount()); - // Only the log record system is needed and therefore loaded. - EXPECT_EQ(1u, *server.SystemCount()); + EXPECT_EQ(4u, *server.SystemCount()); EXPECT_TRUE(serverConfig.LogRecordTopics().empty()); serverConfig.AddLogRecordTopic("test_topic1"); @@ -482,9 +481,7 @@ TEST_P(ServerFixture, sim::Server server(serverConfig); EXPECT_EQ(0u, *server.IterationCount()); EXPECT_EQ(3u, *server.EntityCount()); - - // Only the log record system is needed and therefore loaded. - EXPECT_EQ(1u, *server.SystemCount()); + EXPECT_EQ(4u, *server.SystemCount()); } EXPECT_FALSE(common::exists(logFile)); From aab29bc4e24cb1b937e01b9ee95bdf0e4b398833 Mon Sep 17 00:00:00 2001 From: Oscmoar07 <108773152+XINJIANGMO@users.noreply.github.com> Date: Fri, 8 Aug 2025 07:12:34 +0800 Subject: [PATCH 42/49] Fix crash when calling setPose service on static and nolink entity (#2988) Gazebo allows loading static models without any links. However, if a /world//set_pose service call is made targeting such a model, the physics system crashes , because the underlying physics code assumes every model has at least one link. After Fix : Before executing set_pose, the code now checks whether the target model contains at least one link. --------- Signed-off-by: momo <2438833481@qq.com> Co-authored-by: Ian Chen --- src/systems/physics/Physics.cc | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 0718c99881..1c52ceac26 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -2485,6 +2485,14 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) << std::endl; return true; } + sim::Model model(_entity); + if (model.Links(_ecm).empty()) + { + gzerr << "SetPose is not supported for models without any link." + << "Entity [" << _entity << "]." + << "Request ignored" << std::endl; + return true; + } auto modelPtrPhys = this->entityModelMap.Get(_entity); if (nullptr == modelPtrPhys) return true; From cdd1a76274eb3dbf8936537d60230960d276e0fb Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 23 Jul 2025 21:40:06 -0500 Subject: [PATCH 43/49] Update our usage of workerpools (#2995) In `SimulationRunner`, we initialize a worker pool but we never actually use it so it's pure overhead. In `ServerPrivate` we only use the worker pool if there are multiple simulation runners, so we can optimize for the most common use case of one runner. Signed-off-by: Addisu Z. Taddese (cherry picked from commit 5ce62a8826b930078fae940288ac9677aae9ec7b) --- src/ServerPrivate.cc | 10 ++++++++-- src/ServerPrivate.hh | 5 ++++- src/SimulationRunner.hh | 4 ---- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index e6a215271a..085b6da77e 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -200,16 +200,22 @@ bool ServerPrivate::Run(const uint64_t _iterations, } else { + if (!this->workerPool.has_value()) + { + // Initialize the workerpool if we do have multiple simulation runners and + // it hasn't been initialized before + this->workerPool.emplace(2); + } for (std::unique_ptr &runner : this->simRunners) { - this->workerPool.AddWork([&runner, &_iterations] () + this->workerPool->AddWork([&runner, &_iterations] () { runner->Run(_iterations); }); } // Wait for the runner to complete. - result = this->workerPool.WaitForResults(); + result = this->workerPool->WaitForResults(); } // See comments ServerPrivate::Stop() for why we lock this mutex here. diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh index 6fe36f8b1f..8dd9930166 100644 --- a/src/ServerPrivate.hh +++ b/src/ServerPrivate.hh @@ -154,7 +154,10 @@ namespace gz const gz::msgs::ServerControl &_req, msgs::Boolean &_res); /// \brief A pool of worker threads. - public: common::WorkerPool workerPool{2}; + /// \note We use optional here since most of the time, there will be a + /// single simulation runner and a workerpool is not needed. We will + /// initialize the workerpool as necessary later on. + public: std::optional workerPool; /// \brief All the simulation runners. public: std::vector> simRunners; diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index b427d023ad..9fe550a99b 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -41,7 +41,6 @@ #include #include -#include #include #include @@ -427,9 +426,6 @@ namespace gz /// \brief Manager of distributing/receiving network work. private: std::unique_ptr networkMgr{nullptr}; - /// \brief A pool of worker threads. - private: common::WorkerPool workerPool{2}; - /// \brief Wall time of the previous update. private: std::chrono::steady_clock::time_point prevUpdateRealTime; From 504ee345a53bd6a35406eda42858b1610a2b6995 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 23 Jul 2025 21:40:06 -0500 Subject: [PATCH 44/49] Update our usage of workerpools (#2995) In `SimulationRunner`, we initialize a worker pool but we never actually use it so it's pure overhead. In `ServerPrivate` we only use the worker pool if there are multiple simulation runners, so we can optimize for the most common use case of one runner. Signed-off-by: Addisu Z. Taddese (cherry picked from commit 5ce62a8826b930078fae940288ac9677aae9ec7b) --- src/ServerPrivate.cc | 10 ++++++++-- src/ServerPrivate.hh | 5 ++++- src/SimulationRunner.hh | 4 ---- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index ba9a3381bc..4ad594f788 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -180,16 +180,22 @@ bool ServerPrivate::Run(const uint64_t _iterations, } else { + if (!this->workerPool.has_value()) + { + // Initialize the workerpool if we do have multiple simulation runners and + // it hasn't been initialized before + this->workerPool.emplace(2); + } for (std::unique_ptr &runner : this->simRunners) { - this->workerPool.AddWork([&runner, &_iterations] () + this->workerPool->AddWork([&runner, &_iterations] () { runner->Run(_iterations); }); } // Wait for the runner to complete. - result = this->workerPool.WaitForResults(); + result = this->workerPool->WaitForResults(); } this->running = false; diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh index 4b81144feb..75d5e31c9e 100644 --- a/src/ServerPrivate.hh +++ b/src/ServerPrivate.hh @@ -154,7 +154,10 @@ namespace gz const gz::msgs::ServerControl &_req, msgs::Boolean &_res); /// \brief A pool of worker threads. - public: common::WorkerPool workerPool{2}; + /// \note We use optional here since most of the time, there will be a + /// single simulation runner and a workerpool is not needed. We will + /// initialize the workerpool as necessary later on. + public: std::optional workerPool; /// \brief All the simulation runners. public: std::vector> simRunners; diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 8fe03511e7..4b3cd8c612 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -41,7 +41,6 @@ #include #include -#include #include #include @@ -427,9 +426,6 @@ namespace gz /// \brief Manager of distributing/receiving network work. private: std::unique_ptr networkMgr{nullptr}; - /// \brief A pool of worker threads. - private: common::WorkerPool workerPool{2}; - /// \brief Wall time of the previous update. private: std::chrono::steady_clock::time_point prevUpdateRealTime; From 0699ffe96758556fe7f54c6072092412881746da Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Fri, 8 Aug 2025 21:12:28 +0100 Subject: [PATCH 45/49] JointController: supported nested joints (#2979) Enables the JointController plugin to search for scoped joint names as well as unscoped ones. The approach is the same as that used in the JointPositionController plugin. Signed-off-by: Rhys Mainwaring Co-authored-by: Ian Chen --- .../joint_controller/JointController.cc | 35 ++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index 5ccc033b7c..98610a7598 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -21,6 +21,8 @@ #include #include +#include +#include #include #include @@ -30,10 +32,12 @@ #include #include "gz/sim/components/Actuators.hh" +#include "gz/sim/components/Joint.hh" #include "gz/sim/components/JointForceCmd.hh" #include "gz/sim/components/JointVelocity.hh" #include "gz/sim/components/JointVelocityCmd.hh" #include "gz/sim/Model.hh" +#include "gz/sim/Util.hh" using namespace gz; using namespace sim; @@ -285,7 +289,35 @@ void JointController::PreUpdate(const UpdateInfo &_info, bool warned{false}; for (const std::string &name : this->dataPtr->jointNames) { - Entity joint = this->dataPtr->model.JointByName(_ecm, name); + // First try to resolve by scoped name. + Entity joint = kNullEntity; + auto entities = entitiesFromScopedName( + name, _ecm, this->dataPtr->model.Entity()); + + if (!entities.empty()) + { + if (entities.size() > 1) + { + gzwarn << "Multiple joint entities with name [" + << name << "] found. " + << "Using the first one." << std::endl; + } + joint = *entities.begin(); + + // Validate + if (!_ecm.EntityHasComponentType(joint, components::Joint::typeId)) + { + gzerr << "Entity with name[" << name + << "] is not a joint" << std::endl; + joint = kNullEntity; + } + else + { + gzdbg << "Identified joint [" << name + << "] as Entity [" << joint << "]" << std::endl; + } + } + if (joint != kNullEntity) { this->dataPtr->jointEntities.push_back(joint); @@ -297,6 +329,7 @@ void JointController::PreUpdate(const UpdateInfo &_info, } } } + if (this->dataPtr->jointEntities.empty()) return; From 0bfe7cfff3d1c64f2f7c362205b792729c98f0a4 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 8 Aug 2025 20:51:15 +0000 Subject: [PATCH 46/49] Revert "Fix crash when calling setPose service on static and nolink entity (#2988)" This reverts commit aab29bc4e24cb1b937e01b9ee95bdf0e4b398833. Signed-off-by: Ian Chen --- src/systems/physics/Physics.cc | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 1c52ceac26..0718c99881 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -2485,14 +2485,6 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) << std::endl; return true; } - sim::Model model(_entity); - if (model.Links(_ecm).empty()) - { - gzerr << "SetPose is not supported for models without any link." - << "Entity [" << _entity << "]." - << "Request ignored" << std::endl; - return true; - } auto modelPtrPhys = this->entityModelMap.Get(_entity); if (nullptr == modelPtrPhys) return true; From e45be2a1dc1ea18908241d83e5be38d5f91b4881 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 8 Aug 2025 20:48:26 -0700 Subject: [PATCH 47/49] Add param in physics system to enforce fixed constraint (#2984) Signed-off-by: Ian Chen --- src/systems/physics/Physics.cc | 41 ++++++++++++++++++++++++++++++++++ src/systems/physics/Physics.hh | 9 ++++++++ 2 files changed, 50 insertions(+) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 0718c99881..f82b41703a 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -397,6 +397,11 @@ class gz::sim::systems::PhysicsPrivate /// \brief Pointer to the underlying gz-physics Engine entity. public: EnginePtrType engine = nullptr; + /// \brief Set whether to enforce fixed constraints. Applicable only if + /// the underlying physics engine supports SetWeldChildToParent feature, e.g. + /// gz-physics bullet-featherstone-plugin + public: bool enforceFixedConstraint = false; + /// \brief Vector3d equality comparison function. public: std::function vec3Eql { [](const math::Vector3d &_a, const math::Vector3d &_b) @@ -516,6 +521,12 @@ class gz::sim::systems::PhysicsPrivate physics::DetachJointFeature, physics::SetJointTransformFromParentFeature>{}; + /// \brief Feature list for setting fixed joint to weld child to parent entity + public: struct SetFixedJointWeldChildToParentFeatureList + : physics::FeatureList< + DetachableJointFeatureList, + physics::SetFixedJointWeldChildToParentFeature>{}; + ////////////////////////////////////////////////// // Joint transmitted wrench /// \brief Feature list for getting joint transmitted wrenches. @@ -730,6 +741,7 @@ class gz::sim::systems::PhysicsPrivate physics::Joint, JointFeatureList, DetachableJointFeatureList, + SetFixedJointWeldChildToParentFeatureList, MimicConstraintJointFeatureList, JointVelocityCommandFeatureList, JointGetTransmittedWrenchFeatureList, @@ -852,6 +864,11 @@ void Physics::Configure(const Entity &_entity, "include_entity_names", true).first; } + // Check if fixed constraints should be enforced. + this->dataPtr->enforceFixedConstraint = + _sdf->Get("enforce_fixed_constraint", + this->dataPtr->enforceFixedConstraint).first; + plugin::Loader pluginLoader; if (isStaticPlugin(pluginLib)) { @@ -1993,6 +2010,30 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm, this->entityJointMap.AddEntity(_entity, jointPtrPhys); this->topLevelModelMap.insert(std::make_pair(_entity, topLevelModel(_entity, _ecm))); + + if (this->enforceFixedConstraint) + { + auto jointPtrWeld = this->entityJointMap + .EntityCast(_entity); + if (!jointPtrWeld) + { + static bool informed{false}; + if (!informed) + { + gzerr << "Attempting to enforce fixed constraint in a " + << "detachable joint but the physics engine doesn't " + << "support feature " + << "[SetFixedJointWeldChildToParentFeature]. " + << "The fixed constraint in detachable joints will not " + << "be enforced." << std::endl; + informed = true; + } + } + else + { + jointPtrWeld->SetWeldChildToParent(true); + } + } } else { diff --git a/src/systems/physics/Physics.hh b/src/systems/physics/Physics.hh index 698050a9dc..2a4ec79a93 100644 --- a/src/systems/physics/Physics.hh +++ b/src/systems/physics/Physics.hh @@ -71,6 +71,14 @@ namespace systems /// - ``: Optional. When set /// to false, the name of colliding entities is not populated in /// the contacts. Remains true by default. + /// - `` (optional): Applicable only when the + /// gz-physics bullet-featherstone-plugin is used as the physics engine. + /// If true, the system will enforce the fixed constraint between the child + /// and parent entities of the fixed joint as if the child is 'welded' to + /// the parent. Fixed constraints can be violated, e.g. due to large + /// external forces, so this setting tells the phyics engine to manually + /// correct and maintain the relative pose between the parent and child + /// entities. Defaults to false. /// /// ## Example /// @@ -81,6 +89,7 @@ namespace systems /// /// false /// + /// false /// /// ``` From b90dd729b18af45670333b5fcac19f4710190d6e Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Sat, 9 Aug 2025 11:06:25 +0200 Subject: [PATCH 48/49] Add missing algorithm include (#3022) backport #2414 Signed-off-by: Guilhem Saurel --- include/gz/sim/components/Factory.hh | 1 + 1 file changed, 1 insertion(+) diff --git a/include/gz/sim/components/Factory.hh b/include/gz/sim/components/Factory.hh index 456cf3fc20..1704e953d1 100644 --- a/include/gz/sim/components/Factory.hh +++ b/include/gz/sim/components/Factory.hh @@ -17,6 +17,7 @@ #ifndef GZ_GAZEBO_COMPONENTS_FACTORY_HH_ #define GZ_GAZEBO_COMPONENTS_FACTORY_HH_ +#include #include #include #include From b682e0a2aaf1330919b44960f61403cb133e90eb Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 12 Aug 2025 07:20:03 +0000 Subject: [PATCH 49/49] Address style related issues in the Detachable Joint Plugin Signed-off-by: Arjo Chakravarty --- .../detachable_joint/DetachableJoint.cc | 23 +++++++++++++------ 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 65fd3d0490..63d13548bd 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -231,13 +231,17 @@ void DetachableJoint::GetChildModelAndLinkEntities( auto entitiesMatchingName = entitiesFromScopedName( this->childModelName, _ecm); - // TODO(arjoc): There is probably a more efficient way of combining entitiesFromScopedName + // TODO(arjoc): There is probably a more efficient way + // of combining entitiesFromScopedName // With filtering. // Filter for entities with only models std::vector candidateEntities; std::copy_if(entitiesMatchingName.begin(), entitiesMatchingName.end(), std::back_inserter(candidateEntities), - [&_ecm](Entity e) { return _ecm.EntityHasComponentType(e, components::Model::typeId); }); + [&_ecm](Entity e) { + return _ecm.EntityHasComponentType(e, + components::Model::typeId); + }); if (candidateEntities.size() == 1) { @@ -248,7 +252,8 @@ void DetachableJoint::GetChildModelAndLinkEntities( { std::string selectedModelName; auto parentEntityScopedPath = scopedName(this->model.Entity(), _ecm); - // If there is more than one model with the given child model name, the plugin looks for a model which is + // If there is more than one model with the given child model name, + // the plugin looks for a model which is // - a descendant of the plugin's parent model with that name, and // - has a child link with the given child link name for (auto entity : candidateEntities) @@ -271,14 +276,17 @@ void DetachableJoint::GetChildModelAndLinkEntities( if (kNullEntity != this->childLinkEntity) { - // Only select this child model entity if the entity has a link with the given child link name + // Only select this child model entity if the entity + // has a link with the given child link name modelEntity = entity; selectedModelName = childEntityScope; - gzdbg << "Selecting " << childEntityScope << " as model to be detached" << std::endl; + gzdbg << "Selecting " << childEntityScope + << " as model to be detached" << std::endl; } else { - gzwarn << "Found " << childEntityScope << " with no link named " << this->childLinkName << std::endl; + gzwarn << "Found " << childEntityScope + << " with no link named " << this->childLinkName << std::endl; } } else @@ -315,7 +323,8 @@ void DetachableJoint::PreUpdate( return; } - if (this->childLinkEntity == kNullEntity || !_ecm.HasEntity(this->childLinkEntity)) + if (this->childLinkEntity == kNullEntity || + !_ecm.HasEntity(this->childLinkEntity)) this->GetChildModelAndLinkEntities(_ecm); if (kNullEntity != this->childLinkEntity)