Set collision detector and solver from SDF

src/LevelManager.cc

@@ -113,29 +113,26 @@ void LevelManager::ReadLevelPerformerInfo()
 
   // Populate physics options that aren't accessible outside the Element()
   // See https://github.com/osrf/sdformat/issues/508
-  if (physics->Element())
+  if (physics->Element() && physics->Element()->HasElement("dart"))
   {
-    if (physics->Element()->HasElement("dart"))
-    {
-      auto dartElem = physics->Element()->GetElement("dart");
+    auto dartElem = physics->Element()->GetElement("dart");
 
-      if (dartElem->HasElement("collision_detector"))
-      {
-        auto collisionDetector =
-            dartElem->Get<std::string>("collision_detector");
+    if (dartElem->HasElement("collision_detector"))
+    {
+      auto collisionDetector =
+          dartElem->Get<std::string>("collision_detector");
 
-        this->runner->entityCompMgr.CreateComponent(worldEntity,
-            components::PhysicsCollisionDetector(collisionDetector));
-      }
-      if (dartElem->HasElement("solver") &&
-          dartElem->GetElement("solver")->HasElement("solver_type"))
-      {
-        auto solver =
-            dartElem->GetElement("solver")->Get<std::string>("solver_type");
+      this->runner->entityCompMgr.CreateComponent(worldEntity,
+          components::PhysicsCollisionDetector(collisionDetector));
+    }
+    if (dartElem->HasElement("solver") &&
+        dartElem->GetElement("solver")->HasElement("solver_type"))
+    {
+      auto solver =
+          dartElem->GetElement("solver")->Get<std::string>("solver_type");
 
-        this->runner->entityCompMgr.CreateComponent(worldEntity,
-            components::PhysicsSolver(solver));
-      }
+      this->runner->entityCompMgr.CreateComponent(worldEntity,
+          components::PhysicsSolver(solver));
     }
   }
 

src/SdfEntityCreator.cc

@@ -282,29 +282,26 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world)
 
   // Populate physics options that aren't accessible outside the Element()
   // See https://github.com/osrf/sdformat/issues/508
-  if (physics->Element())
+  if (physics->Element() && physics->Element()->HasElement("dart"))
   {
-    if (physics->Element()->HasElement("dart"))
+    auto dartElem = physics->Element()->GetElement("dart");
+
+    if (dartElem->HasElement("collision_detector"))
     {
-      auto dartElem = physics->Element()->GetElement("dart");
-
-      if (dartElem->HasElement("collision_detector"))
-      {
-        auto collisionDetector =
-            dartElem->Get<std::string>("collision_detector");
-
-        this->dataPtr->ecm->CreateComponent(worldEntity,
-            components::PhysicsCollisionDetector(collisionDetector));
-      }
-      if (dartElem->HasElement("solver") &&
-          dartElem->GetElement("solver")->HasElement("solver_type"))
-      {
-        auto solver =
-            dartElem->GetElement("solver")->Get<std::string>("solver_type");
-
-        this->dataPtr->ecm->CreateComponent(worldEntity,
-            components::PhysicsSolver(solver));
-      }
+      auto collisionDetector =
+          dartElem->Get<std::string>("collision_detector");
+
+      this->dataPtr->ecm->CreateComponent(worldEntity,
+          components::PhysicsCollisionDetector(collisionDetector));
+    }
+    if (dartElem->HasElement("solver") &&
+        dartElem->GetElement("solver")->HasElement("solver_type"))
+    {
+      auto solver =
+          dartElem->GetElement("solver")->Get<std::string>("solver_type");
+
+      this->dataPtr->ecm->CreateComponent(worldEntity,
+          components::PhysicsSolver(solver));
     }
   }
 

src/systems/physics/Physics.cc

@@ -423,6 +423,7 @@ class ignition::gazebo::systems::PhysicsPrivate
           physics::World,
           MinimumFeatureList,
           CollisionFeatureList,
+          ContactFeatureList,
           NestedModelFeatureList,
           CollisionDetectorFeatureList,
           SolverFeatureList>;
@@ -1994,6 +1995,42 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
 {
   IGN_PROFILE("PhysicsPrivate::UpdateSim");
 
+  // Populate world components with default values
+  _ecm.EachNew<components::World>(
+      [&](const Entity &_entity,
+        const components::World *)->bool
+      {
+        // If not provided by ECM, create component with values from physics if
+        // those features are available
+        auto collisionDetectorComp =
+            _ecm.Component<components::PhysicsCollisionDetector>(_entity);
+        if (!collisionDetectorComp)
+        {
+          auto collisionDetectorFeature =
+              this->entityWorldMap.EntityCast<CollisionDetectorFeatureList>(
+              _entity);
+          if (collisionDetectorFeature)
+          {
+            _ecm.CreateComponent(_entity, components::PhysicsCollisionDetector(
+                collisionDetectorFeature->GetCollisionDetector()));
+          }
+        }
+
+        auto solverComp = _ecm.Component<components::PhysicsSolver>(_entity);
+        if (!solverComp)
+        {
+          auto solverFeature =
+              this->entityWorldMap.EntityCast<SolverFeatureList>(_entity);
+          if (solverFeature)
+          {
+            _ecm.CreateComponent(_entity,
+                components::PhysicsSolver(solverFeature->GetSolver()));
+          }
+        }
+
+        return true;
+      });
+
   IGN_PROFILE_BEGIN("Models");
 
   // make sure we have an up-to-date mapping of canonical links to their models

test/integration/physics_system.cc

@@ -1375,12 +1375,53 @@ TEST_F(PhysicsSystemFixture, NestedModelIndividualCanonicalLinks)
   EXPECT_DOUBLE_EQ(model12WorldPose.Z(), model10It->second.Z());
 }
 
+/////////////////////////////////////////////////
+TEST_F(PhysicsSystemFixture, DefaultPhysicsOptions)
+{
+  ignition::gazebo::ServerConfig serverConfig;
+
+  bool checked{false};
+
+  // Create a system to check components
+  test::Relay testSystem;
+  testSystem.OnPostUpdate(
+    [&checked](const gazebo::UpdateInfo &,
+    const gazebo::EntityComponentManager &_ecm)
+    {
+      _ecm.Each<components::World, components::PhysicsCollisionDetector,
+                components::PhysicsSolver>(
+        [&](const ignition::gazebo::Entity &, const components::World *,
+            const components::PhysicsCollisionDetector *_collisionDetector,
+            const components::PhysicsSolver *_solver)->bool
+        {
+          EXPECT_NE(nullptr, _collisionDetector);
+          if (_collisionDetector)
+          {
+            EXPECT_EQ("ode", _collisionDetector->Data());
+          }
+          EXPECT_NE(nullptr, _solver);
+          if (_solver)
+          {
+            EXPECT_EQ("DantzigBoxedLcpSolver", _solver->Data());
+          }
+          checked = true;
+          return true;
+        });
+    });
+
+  gazebo::Server server(serverConfig);
+  server.AddSystem(testSystem.systemPtr);
+  server.Run(true, 1, false);
+
+  EXPECT_TRUE(checked);
+}
+
 /////////////////////////////////////////////////
 TEST_F(PhysicsSystemFixture, PhysicsOptions)
 {
   ignition::gazebo::ServerConfig serverConfig;
-  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
-    "/test/worlds/physics_options.sdf");
+  serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+    "test", "worlds", "physics_options.sdf"));
 
   bool checked{false};