Merge ign-gazebo6 ➡️ gz-sim8

Changelog.md

@@ -1609,6 +1609,68 @@
 
 ## Gazebo Sim 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

examples/worlds/shader_param.sdf

@@ -181,12 +181,12 @@ ShaderParam visual plugin over time.
     <include>
       <name>deformable_sphere</name>
       <pose>0 0 1.5 0 0 0</pose>
-      <uri>https://fuel.gazebosim.org/1.0/openrobotics/models/deformable_sphere</uri>
+      <uri>https://fuel.gazebosim.org/1.0/openrobotics/models/deformable_sphere/5</uri>
     </include>
     <include>
       <name>waves</name>
       <pose>0 0 0 0 0 0</pose>
-      <uri>https://fuel.gazebosim.org/1.0/openrobotics/models/waves</uri>
+      <uri>https://fuel.gazebosim.org/1.0/openrobotics/models/waves/4</uri>
     </include>
 
     <model name="camera">

src/systems/detachable_joint/DetachableJoint.cc

@@ -210,8 +210,97 @@ void DetachableJoint::Configure(const Entity &_entity,
           .first;
 
   this->validConfig = true;
+
+  this->GetChildModelAndLinkEntities(_ecm);
 }
 
+//////////////////////////////////////////////////
+void DetachableJoint::GetChildModelAndLinkEntities(
+  EntityComponentManager &_ecm)
+{
+  this->childLinkEntity = kNullEntity;
+  // Look for the child model and link
+  Entity modelEntity{kNullEntity};
+
+  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<Entity> 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)
+    {
+      // 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 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)
+      {
+        auto childEntityScope = scopedName(entity, _ecm);
+        if (childEntityScope.size() < parentEntityScopedPath.size())
+        {
+          continue;
+        }
+        if (childEntityScope.rfind(parentEntityScopedPath, 0) != 0)
+        {
+          continue;
+        }
+        if (modelEntity == kNullEntity)
+        {
+
+          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;
+                gzdbg << "Selecting " << childEntityScope << " as model to be detached" << std::endl;
+          }
+          else
+          {
+            gzwarn << "Found " << childEntityScope << " with no link named " << this->childLinkName << std::endl;
+          }
+        }
+        else
+        {
+          gzwarn << "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));
+  }
+  else if (!this->suppressChildWarning)
+  {
+    gzwarn << "Child Model " << this->childModelName
+            << " could not be found.\n";
+  }
+}
 //////////////////////////////////////////////////
 void DetachableJoint::PreUpdate(
   const UpdateInfo &/*_info*/,
@@ -225,54 +314,35 @@ void DetachableJoint::PreUpdate(
     if (!this->attachRequested){
       return;
     }
-    // Look for the child model and link
-    Entity modelEntity{kNullEntity};
 
-    if ("__model__" == this->childModelName)
+    if (this->childLinkEntity == kNullEntity || !_ecm.HasEntity(this->childLinkEntity))
+      this->GetChildModelAndLinkEntities(_ecm);
+
+    if (kNullEntity != this->childLinkEntity)
     {
-      modelEntity = this->model.Entity();
+      // 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);
+      gzdbg << "Attaching entity: " << this->detachableJointEntity
+              << std::endl;
     }
     else
     {
-      modelEntity = _ecm.EntityByComponents(
-          components::Model(), components::Name(this->childModelName));
-    }
-    if (kNullEntity != modelEntity)
-    {
-      this->childLinkEntity = _ecm.EntityByComponents(
-          components::Link(), components::ParentEntity(modelEntity),
-          components::Name(this->childLinkName));
-
-      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);
-        gzdbg << "Attaching entity: " << this->detachableJointEntity
-               << std::endl;
-      }
-      else
-      {
-        gzwarn << "Child Link " << this->childLinkName
-                << " could not be found.\n";
-      }
-    }
-    else if (!this->suppressChildWarning)
-    {
-      gzwarn << "Child Model " << this->childModelName
+      gzwarn << "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))

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;
 

test/integration/detachable_joint.cc

@@ -67,7 +67,8 @@ TEST_F(DetachableJointTest, GZ_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
@@ -145,7 +146,8 @@ TEST_F(DetachableJointTest, GZ_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
@@ -218,6 +220,80 @@ TEST_F(DetachableJointTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel))
   EXPECT_GT(b2Poses.front().Pos().Z() - b2Poses.back().Pos().Z(), expDist);
 }
 
+/////////////////////////////////////////////////
+TEST_F(DetachableJointTest,
+       GZ_UTILS_TEST_DISABLED_ON_WIN32(NestedModelsWithSameName))
+{
+  using namespace std::chrono_literals;
+
+  this->StartServer(common::joinPaths("/test", "worlds",
+       "detachable_joint_child.sdf"));
+
+
+  std::vector<math::Pose3d> childM4Poses, childM5Poses;
+  test::Relay testSystem1;
+  testSystem1.OnPostUpdate([&childM4Poses, &childM5Poses](
+    const sim::UpdateInfo &,
+    const sim::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<components::Pose>(childEntityM4);
+    auto poseM5 = _ecm.Component<components::Pose>(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);
+
+  // 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());
+
+  // Release M5's child only
+  transport::Node node;
+  auto pub = node.Advertise<msgs::Empty>("/model/M5/detachable_joint/detach");
+  pub.Publish(msgs::Empty());
+  std::this_thread::sleep_for(250ms);
+
+  this->server->Run(true, nIters, false);
+  // 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());
+}
  /////////////////////////////////////////////////
  // Test for re-attaching a detached joint. This uses the vehicle_blue and B1
  // box models. The B1 model is first detached from the vehicle. Although