Merge pull request #258 from ignitionrobotics/nested_model

Support nested model

CMakeLists.txt

@@ -38,7 +38,7 @@ endif()
 # Search for project-specific dependencies
 #============================================================================
 
-ign_find_package(sdformat9 REQUIRED VERSION 9.2.0)
+ign_find_package(sdformat9 REQUIRED VERSION 9.3.0)
 set(SDF_VER ${sdformat9_VERSION_MAJOR})
 
 #--------------------------------------

examples/worlds/nested_model.sdf

@@ -0,0 +1,110 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+  <world name="nested_model_world">
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="libignition-gazebo-physics-system.so"
+      name="ignition::gazebo::systems::Physics">
+      <engine><filename>libignition-physics-tpe-plugin.so</filename></engine>
+    </plugin>
+    <plugin
+      filename="libignition-gazebo-user-commands-system.so"
+      name="ignition::gazebo::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="libignition-gazebo-scene-broadcaster-system.so"
+      name="ignition::gazebo::systems::SceneBroadcaster">
+    </plugin>
+
+    <scene>
+      <ambient>1.0 1.0 1.0</ambient>
+      <background>0.8 0.8 0.8</background>
+    </scene>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.8 0.8 0.8 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name="model_00">
+      <pose>0 0 0.5 0 0 0</pose>
+      <link name="link_00">
+        <pose>0 0 0 0 0 0</pose>
+        <collision name="collision_00">
+          <geometry>
+            <sphere>
+              <radius>0.5</radius>
+            </sphere>
+          </geometry>
+        </collision>
+        <visual name="visual_00">
+          <geometry>
+            <sphere>
+              <radius>0.5</radius>
+            </sphere>
+          </geometry>
+        </visual>
+      </link>
+
+      <model name="model_01">
+        <pose>1 0 0.0 0 0 0</pose>
+        <link name="link_01">
+          <pose>0.25 0 0.0 0 0 0.785398</pose>
+          <collision name="collision_01">
+            <geometry>
+              <box>
+                <size>1 1 1</size>
+              </box>
+            </geometry>
+          </collision>
+          <visual name="visual_01">
+            <geometry>
+              <box>
+                <size>1 1 1</size>
+              </box>
+            </geometry>
+          </visual>
+        </link>
+      </model>
+    </model>
+
+  </world>
+</sdf>

include/ignition/gazebo/SdfEntityCreator.hh

@@ -155,6 +155,18 @@ namespace ignition
       /// \param[in] _parent Entity which should be _child's parent.
       public: void SetParent(Entity _child, Entity _parent);
 
+      /// \brief Overloaded function to recursively create model entities
+      /// and make sure a) only one canonical link is created per model tree,
+      /// and b) we override the nested model's static property to true if
+      /// its parent is static
+      /// \param[in] _model SDF model object.
+      /// \param[in] _createCanonicalLink True to create a canonical link
+      /// component and attach to its child link entity
+      /// \param[in] _staticParent True if parent is static, false otherwise.
+      /// \return Model entity.
+      private: Entity CreateEntities(const sdf::Model *_model,
+          bool _createCanonicalLink, bool _staticParent);
+
       /// \brief Pointer to private data.
       private: std::unique_ptr<SdfEntityCreatorPrivate> dataPtr;
     };

include/ignition/gazebo/Util.hh

@@ -131,6 +131,14 @@ namespace ignition
     void IGNITION_GAZEBO_VISIBLE addResourcePaths(
         const std::vector<std::string> &_paths = {});
 
+    /// \brief Get the top level model of an entity
+    /// \param[in] _entity Input entity
+    /// \param[in] _ecm Constant reference to ECM.
+    /// \return Entity of top level model
+    ignition::gazebo::Entity IGNITION_GAZEBO_VISIBLE topLevelModel(
+        const Entity &_entity,
+        const EntityComponentManager &_ecm);
+
     /// \brief Environment variable holding resource paths.
     const std::string kResourcePathEnv{"IGN_GAZEBO_RESOURCE_PATH"};
 

src/SdfEntityCreator.cc

@@ -80,6 +80,10 @@ class ignition::gazebo::SdfEntityCreatorPrivate
   /// only after we have their scoped name.
   public: std::map<Entity, sdf::ElementPtr> newSensors;
 
+  /// \brief Keep track of new models being added, so we load their plugins
+  /// only after we have their scoped name.
+  public: std::map<Entity, sdf::ElementPtr> newModels;
+
   /// \brief Keep track of new visuals being added, so we load their plugins
   /// only after we have their scoped name.
   public: std::map<Entity, sdf::ElementPtr> newVisuals;
@@ -216,6 +220,41 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model)
 {
   IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::Model)");
 
+  // todo(anyone) Support multiple canonical links in nested models
+  // This version of CreateEntties keeps track whether or not to create a
+  // canonical link in a model tree using the second arg in this recursive
+  // function. We also override child nested models static property if parent
+  // model is static
+  auto ent = this->CreateEntities(_model, true, false);
+
+  // Load all model plugins afterwards, so we get scoped name for nested models.
+  for (const auto &[entity, element] : this->dataPtr->newModels)
+  {
+    this->dataPtr->eventManager->Emit<events::LoadPlugins>(entity, element);
+  }
+  this->dataPtr->newModels.clear();
+
+  // Load sensor plugins after model, so we get scoped name.
+  for (const auto &[entity, element] : this->dataPtr->newSensors)
+  {
+    this->dataPtr->eventManager->Emit<events::LoadPlugins>(entity, element);
+  }
+  this->dataPtr->newSensors.clear();
+
+  // Load visual plugins after model, so we get scoped name.
+  for (const auto &[entity, element] : this->dataPtr->newVisuals)
+  {
+    this->dataPtr->eventManager->Emit<events::LoadPlugins>(entity, element);
+  }
+  this->dataPtr->newVisuals.clear();
+
+  return ent;
+}
+
+//////////////////////////////////////////////////
+Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model,
+    bool _createCanonicalLink, bool _staticParent)
+{
   // Entity
   Entity modelEntity = this->dataPtr->ecm->CreateEntity();
 
@@ -225,8 +264,9 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model)
       components::Pose(ResolveSdfPose(_model->SemanticPose())));
   this->dataPtr->ecm->CreateComponent(modelEntity,
       components::Name(_model->Name()));
+  bool isStatic = _model->Static() || _staticParent;
   this->dataPtr->ecm->CreateComponent(modelEntity,
-      components::Static(_model->Static()));
+      components::Static(isStatic));
   this->dataPtr->ecm->CreateComponent(
       modelEntity, components::WindMode(_model->EnableWind()));
   this->dataPtr->ecm->CreateComponent(
@@ -238,18 +278,22 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model)
   // the parent frame until we get frames working.
 
   // Links
+  bool canonicalLinkCreated = false;
   for (uint64_t linkIndex = 0; linkIndex < _model->LinkCount();
       ++linkIndex)
   {
     auto link = _model->LinkByIndex(linkIndex);
     auto linkEntity = this->CreateEntities(link);
 
     this->SetParent(linkEntity, modelEntity);
-    if ((_model->CanonicalLinkName().empty() && linkIndex == 0) ||
-        (link == _model->CanonicalLink()))
+
+    if (_createCanonicalLink &&
+        ((_model->CanonicalLinkName().empty() && linkIndex == 0) ||
+        (link == _model->CanonicalLink())))
     {
       this->dataPtr->ecm->CreateComponent(linkEntity,
           components::CanonicalLink());
+      canonicalLinkCreated = true;
     }
 
     // Set wind mode if the link didn't override it
@@ -270,27 +314,29 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model)
     this->SetParent(jointEntity, modelEntity);
   }
 
-  // Model plugins
-  this->dataPtr->eventManager->Emit<events::LoadPlugins>(modelEntity,
-      _model->Element());
-
-  // Load sensor plugins after model, so we get scoped name.
-  for (const auto &[entity, element] : this->dataPtr->newSensors)
+  // Nested Models
+  for (uint64_t modelIndex = 0; modelIndex < _model->ModelCount();
+      ++modelIndex)
   {
-    this->dataPtr->eventManager->Emit<events::LoadPlugins>(entity, element);
+    auto nestedModel = _model->ModelByIndex(modelIndex);
+
+    // Create nested model. Make sure to only create canonical link component
+    // in the nested model if a canonical link has not been created in this
+    // model yet. Also override static propery of the nested model if this model
+    //  is static
+    auto nestedModelEntity = this->CreateEntities(nestedModel,
+        (_createCanonicalLink && !canonicalLinkCreated), isStatic);
+
+    this->SetParent(nestedModelEntity, modelEntity);
   }
-  this->dataPtr->newSensors.clear();
 
   // Store the model's SDF DOM to be used when saving the world to file
   this->dataPtr->ecm->CreateComponent(
       modelEntity, components::ModelSdf(*_model));
 
-  // Load visual plugins after model, so we get scoped name.
-  for (const auto &[entity, element] : this->dataPtr->newVisuals)
-  {
-    this->dataPtr->eventManager->Emit<events::LoadPlugins>(entity, element);
-  }
-  this->dataPtr->newVisuals.clear();
+  // Keep track of models so we can load their plugins after loading the entire
+  // model and having its full scoped name.
+  this->dataPtr->newModels[modelEntity] = _model->Element();
 
   return modelEntity;
 }

src/SdfGenerator.cc

@@ -24,6 +24,7 @@
 #include "ignition/gazebo/components/Light.hh"
 #include "ignition/gazebo/components/Model.hh"
 #include "ignition/gazebo/components/Name.hh"
+#include "ignition/gazebo/components/ParentEntity.hh"
 #include "ignition/gazebo/components/Pose.hh"
 #include "ignition/gazebo/components/SourceFilePath.hh"
 #include "ignition/gazebo/components/World.hh"
@@ -293,6 +294,12 @@ namespace sdf_generator
         [&](const Entity &_modelEntity, const components::Model *,
             const components::ModelSdf *_modelSdf)
         {
+          // skip nested models as they are not direct children of world
+          auto parentComp = _ecm.Component<components::ParentEntity>(
+              _modelEntity);
+          if (parentComp && parentComp->Data() != _entity)
+            return true;
+
           auto modelDir =
               common::parentPath(_modelSdf->Data().Element()->FilePath());
           const std::string modelName =

src/Util.cc

@@ -392,6 +392,30 @@ void addResourcePaths(const std::vector<std::string> &_paths)
   // SDF is evaluated at find call
   systemPaths->SetFilePathEnv(systemPaths->FilePathEnv());
 }
+
+//////////////////////////////////////////////////
+ignition::gazebo::Entity topLevelModel(const Entity &_entity,
+    const EntityComponentManager &_ecm)
+{
+  auto entity = _entity;
+
+  // check if parent is a model
+  auto parentComp = _ecm.Component<components::ParentEntity>(entity);
+  while (parentComp)
+  {
+    // check if parent is a model
+    auto parentEntity = parentComp->Data();
+    auto modelComp = _ecm.Component<components::Model>(
+        parentEntity);
+    if (!modelComp)
+      break;
+
+    // set current model entity
+    entity = parentEntity;
+    parentComp = _ecm.Component<components::ParentEntity>(entity);
+  }
+  return entity;
+}
 }
 }
 }

src/Util_TEST.cc

@@ -406,3 +406,60 @@ TEST(UtilTest, AsFullPath)
   }
 #endif
 }
+
+/////////////////////////////////////////////////
+TEST(UtilTest, TopLevelModel)
+{
+  EntityComponentManager ecm;
+
+  // world
+  //  - modelA
+  //    - linkA
+  //    - modelB
+  //      - linkB
+  //  - modelC
+
+  // World
+  auto worldEntity = ecm.CreateEntity();
+  ecm.CreateComponent(worldEntity, components::World());
+  ecm.CreateComponent(worldEntity, components::Name("world_name"));
+
+  // Model A
+  auto modelAEntity = ecm.CreateEntity();
+  ecm.CreateComponent(modelAEntity, components::Model());
+  ecm.CreateComponent(modelAEntity, components::Name("modelA_name"));
+  ecm.CreateComponent(modelAEntity, components::ParentEntity(worldEntity));
+
+  // Link A - Child of Model A
+  auto linkAEntity = ecm.CreateEntity();
+  ecm.CreateComponent(linkAEntity, components::Link());
+  ecm.CreateComponent(linkAEntity, components::Name("linkA_name"));
+  ecm.CreateComponent(linkAEntity, components::ParentEntity(modelAEntity));
+
+  // Model B - nested inside Model A
+  auto modelBEntity = ecm.CreateEntity();
+  ecm.CreateComponent(modelBEntity, components::Model());
+  ecm.CreateComponent(modelBEntity, components::Name("modelB_name"));
+  ecm.CreateComponent(modelBEntity, components::ParentEntity(modelAEntity));
+
+  // Link B - child of Model B
+  auto linkBEntity = ecm.CreateEntity();
+  ecm.CreateComponent(linkBEntity, components::Link());
+  ecm.CreateComponent(linkBEntity, components::Name("linkB_name"));
+  ecm.CreateComponent(linkBEntity, components::ParentEntity(modelBEntity));
+
+  // Model C
+  auto modelCEntity = ecm.CreateEntity();
+  ecm.CreateComponent(modelCEntity, components::Model());
+  ecm.CreateComponent(modelCEntity, components::Name("modelC_name"));
+  ecm.CreateComponent(modelCEntity, components::ParentEntity(worldEntity));
+
+  // model A, link A, model B and link B should have model A as top level entity
+  EXPECT_EQ(modelAEntity, topLevelModel(modelAEntity, ecm));
+  EXPECT_EQ(modelAEntity, topLevelModel(linkAEntity, ecm));
+  EXPECT_EQ(modelAEntity, topLevelModel(modelBEntity, ecm));
+  EXPECT_EQ(modelAEntity, topLevelModel(linkBEntity, ecm));
+
+  // model C should have itself as the top level entity
+  EXPECT_EQ(modelCEntity, topLevelModel(modelCEntity, ecm));
+}