diff --git a/examples/worlds/ackermann_steering.sdf b/examples/worlds/ackermann_steering.sdf
index 5de5fa13ac..b4fdc88d80 100644
--- a/examples/worlds/ackermann_steering.sdf
+++ b/examples/worlds/ackermann_steering.sdf
@@ -60,6 +60,10 @@
50
+
+ 1
+ 0.1
+
@@ -156,6 +160,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.1
+
@@ -201,6 +210,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.5
+
@@ -246,6 +260,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.1
+
@@ -291,6 +310,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.5
+
diff --git a/examples/worlds/acoustic_comms_demo.sdf b/examples/worlds/acoustic_comms_demo.sdf
index d5ebe9e0e5..6e1be4c4b9 100644
--- a/examples/worlds/acoustic_comms_demo.sdf
+++ b/examples/worlds/acoustic_comms_demo.sdf
@@ -66,12 +66,12 @@
-5 0 0 0 0 0
- https://fuel.ignitionrobotics.org/1.0/mabelzhang/models/Turquoise turbidity generator
+ https://fuel.gazebosim.org/1.0/mabelzhang/models/Turquoise turbidity generator
0 0 1 0 0 1.57
- https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV
+ https://fuel.gazebosim.org/1.0/accurrent/models/MBARI Tethys LRAUV
15 0 1 0 0 1.57
- https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV
+ https://fuel.gazebosim.org/1.0/accurrent/models/MBARI Tethys LRAUV
triton
@@ -274,7 +274,7 @@
-15 0 1 0 0 1.57
- https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV
+ https://fuel.gazebosim.org/1.0/accurrent/models/MBARI Tethys LRAUV
daphne
diff --git a/examples/worlds/apply_link_wrench.sdf b/examples/worlds/apply_link_wrench.sdf
index 03509d9766..9b9bb229b4 100644
--- a/examples/worlds/apply_link_wrench.sdf
+++ b/examples/worlds/apply_link_wrench.sdf
@@ -104,6 +104,15 @@
1 1 1
+
+
+
+ 0.7
+ 1
+ 0.2
+
+
+
@@ -142,6 +151,15 @@
1.0
+
+
+
+ 0.7
+ 1
+ 0.2
+
+
+
diff --git a/examples/worlds/breadcrumbs.sdf b/examples/worlds/breadcrumbs.sdf
index 3eb587456f..bb9a40a80f 100644
--- a/examples/worlds/breadcrumbs.sdf
+++ b/examples/worlds/breadcrumbs.sdf
@@ -152,6 +152,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -197,6 +202,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -242,6 +252,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -287,6 +302,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
diff --git a/examples/worlds/buoyancy_engine.sdf b/examples/worlds/buoyancy_engine.sdf
index 7f7dfae298..1a68f4d323 100644
--- a/examples/worlds/buoyancy_engine.sdf
+++ b/examples/worlds/buoyancy_engine.sdf
@@ -41,13 +41,13 @@ until it breaches and then start oscillating around the surface. On the other
hand the box on the right will rise forever as no surface is set.
Enter the following in a separate terminal:
```
-gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double
+gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double \
-p "data: 0.003"
```
The boxes will float up. Note that the box on the left will start oscillating
once it breaches the surface.
```
-gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double
+gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double \
-p "data: 0.001"
```
The boxes will go down.
diff --git a/examples/worlds/detachable_joint.sdf b/examples/worlds/detachable_joint.sdf
index 9f2bb2f38d..f036956dca 100644
--- a/examples/worlds/detachable_joint.sdf
+++ b/examples/worlds/detachable_joint.sdf
@@ -154,6 +154,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -199,6 +204,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -244,6 +254,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -289,6 +304,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
diff --git a/examples/worlds/diff_drive.sdf b/examples/worlds/diff_drive.sdf
index f40941667e..a85ae7f112 100644
--- a/examples/worlds/diff_drive.sdf
+++ b/examples/worlds/diff_drive.sdf
@@ -144,6 +144,23 @@
0.3
+
+
+
+ 1
+ 1
+ 0.035
+ 0
+ 0 0 1
+
+
+ 1
+ 1
+ 0.1
+ 0.1
+
+
+
@@ -178,6 +195,23 @@
0.3
+
+
+
+ 1
+ 1
+ 0.035
+ 0
+ 0 0 1
+
+
+ 1
+ 1
+ 0.1
+ 0.1
+
+
+
@@ -332,6 +366,23 @@
0.3
+
+
+
+ 1
+ 1
+ 0.035
+ 0
+ 0 0 1
+
+
+ 1
+ 1
+ 0.1
+ 0.1
+
+
+
@@ -366,6 +417,23 @@
0.3
+
+
+
+ 1
+ 1
+ 0.035
+ 0
+ 0 0 1
+
+
+ 1
+ 1
+ 0.1
+ 0.1
+
+
+
diff --git a/examples/worlds/diff_drive_skid.sdf b/examples/worlds/diff_drive_skid.sdf
index da43686013..fe1fdbe833 100644
--- a/examples/worlds/diff_drive_skid.sdf
+++ b/examples/worlds/diff_drive_skid.sdf
@@ -60,6 +60,11 @@
50
+
+ 1
+ 1
+ 0.1
+
@@ -154,6 +159,11 @@
1.0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -197,6 +207,11 @@
1.0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -240,6 +255,11 @@
1.0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -283,6 +303,11 @@
1.0
0 0 1
+
+ 1
+ 1
+ 0.1
+
diff --git a/examples/worlds/pendulum_links.sdf b/examples/worlds/pendulum_links.sdf
index 7b189e0d83..5eecd1893a 100644
--- a/examples/worlds/pendulum_links.sdf
+++ b/examples/worlds/pendulum_links.sdf
@@ -46,11 +46,12 @@
true
-
-
- 0 0 1
-
-
+
+
+ 0 0 1
+ 100 100
+
+
@@ -71,7 +72,16 @@
- 100
+ -0.188749 0 0.75813399999999997 -0.13567899999999999 0 1.5708
+ 256.42500000000001
+
+ 154.202
+ 0
+ 0
+ 152.286
+ 0
+ 28.8249
+
0 0 0.01 0 0 0
@@ -123,7 +133,16 @@
0 0 2.1 -1.5708 0 0
0
- 0 0 0.5 0 0 0
+ -0.0107143 -4.4408900000000002e-16 0.46428599999999998 0.038729600000000003 -3.4694469519536142e-18 1.5708
+ 87.964600000000004
+
+ 11.999000000000001
+ 0
+ 0
+ 11.8742
+ 0
+ 0.56461300000000003
+
-0.05 0 0 0 1.5708 0
diff --git a/examples/worlds/performer_detector.sdf b/examples/worlds/performer_detector.sdf
index 036248851f..4060cf72d0 100644
--- a/examples/worlds/performer_detector.sdf
+++ b/examples/worlds/performer_detector.sdf
@@ -151,6 +151,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -196,6 +201,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -241,6 +251,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -286,6 +301,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
diff --git a/examples/worlds/skid_steer_mecanum.sdf b/examples/worlds/skid_steer_mecanum.sdf
index 637be5faa8..05dfc150b6 100644
--- a/examples/worlds/skid_steer_mecanum.sdf
+++ b/examples/worlds/skid_steer_mecanum.sdf
@@ -60,6 +60,11 @@
50
+
+ 1
+ 1
+ 0.1
+
@@ -159,6 +164,11 @@
0.0
1 -1 0
+
+ 1
+ 1
+ 0.1
+
@@ -203,6 +213,11 @@
0.0
1 1 0
+
+ 1
+ 1
+ 0.1
+
@@ -247,6 +262,11 @@
0.0
1 1 0
+
+ 1
+ 1
+ 0.1
+
@@ -291,6 +311,11 @@
0.0
1 -1 0
+
+ 1
+ 1
+ 0.1
+
diff --git a/examples/worlds/spherical_coordinates.sdf b/examples/worlds/spherical_coordinates.sdf
index d9a9a1ba45..1519bd64d7 100644
--- a/examples/worlds/spherical_coordinates.sdf
+++ b/examples/worlds/spherical_coordinates.sdf
@@ -230,14 +230,14 @@ gz service -s /world/spherical_coordinates/set_spherical_coordinates \
-
+
diff --git a/examples/worlds/trajectory_follower.sdf b/examples/worlds/trajectory_follower.sdf
index 8af0e3e9b8..54aff9d9a2 100644
--- a/examples/worlds/trajectory_follower.sdf
+++ b/examples/worlds/trajectory_follower.sdf
@@ -93,6 +93,15 @@
1 1 1
+
+
+
+ 0.7
+ 1
+ 0.2
+
+
+
@@ -144,6 +153,15 @@
1 1 1
+
+
+
+ 0.7
+ 1
+ 0.2
+
+
+
@@ -195,6 +213,15 @@
1 1 1
+
+
+
+ 0.7
+ 1
+ 0.2
+
+
+
diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf
index 2c7135ac0e..1457f55d9d 100644
--- a/examples/worlds/triggered_publisher.sdf
+++ b/examples/worlds/triggered_publisher.sdf
@@ -157,6 +157,11 @@ start falling.
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -202,6 +207,11 @@ start falling.
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -247,6 +257,11 @@ start falling.
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -292,6 +307,11 @@ start falling.
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
diff --git a/examples/worlds/velocity_control.sdf b/examples/worlds/velocity_control.sdf
index 58d124fb2a..f66efea55f 100644
--- a/examples/worlds/velocity_control.sdf
+++ b/examples/worlds/velocity_control.sdf
@@ -138,6 +138,15 @@
0.3
+
+
+
+ 1
+ 1
+ 0.1
+
+
+
@@ -172,6 +181,15 @@
0.3
+
+
+
+ 1
+ 1
+ 0.1
+
+
+
@@ -206,6 +224,15 @@
0.2
+
+
+
+ 1
+ 1
+ 0.1
+
+
+
diff --git a/src/gui/plugins/joint_position_controller/JointPositionController.cc b/src/gui/plugins/joint_position_controller/JointPositionController.cc
index 136d7eba06..b51c8f1280 100644
--- a/src/gui/plugins/joint_position_controller/JointPositionController.cc
+++ b/src/gui/plugins/joint_position_controller/JointPositionController.cc
@@ -274,7 +274,7 @@ void JointPositionController::Update(const UpdateInfo &,
// Value
double value = 0.0;
auto posComp = _ecm.Component(jointEntity);
- if (posComp)
+ if (posComp && !posComp->Data().empty())
{
value = posComp->Data()[0];
}
diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index c19c019117..0c5b0c6028 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -1068,8 +1068,22 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm,
// Check if parent world / model exists
sdf::Model model;
+ if (const auto *modelSdfComp =
+ _ecm.Component(_entity))
+ {
+ model = modelSdfComp->Data();
+ }
+
+ // Component values should override whatever values were put into the
+ // ModelSdf component.
model.SetName(_name->Data());
model.SetRawPose(_pose->Data());
+ model.SetPoseRelativeTo("");
+
+ sdf::Root root;
+ root.SetModel(model);
+ root.UpdateGraphs();
+
auto staticComp = _ecm.Component(_entity);
if (staticComp && staticComp->Data())
{
@@ -1106,18 +1120,24 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm,
}
return true;
}
- auto modelPtrPhys = nestedModelFeature->ConstructNestedModel(model);
- this->entityModelMap.AddEntity(_entity, modelPtrPhys);
- this->topLevelModelMap.insert(std::make_pair(_entity,
- topLevelModel(_entity, _ecm)));
+ auto modelPtrPhys =
+ nestedModelFeature->ConstructNestedModel(*root.Model());
+ if (modelPtrPhys)
+ {
+ this->entityModelMap.AddEntity(_entity, modelPtrPhys);
+ this->topLevelModelMap.insert(std::make_pair(_entity,
+ topLevelModel(_entity, _ecm)));
+ }
}
else
{
- auto modelPtrPhys = worldPtrPhys->ConstructModel(model);
-
- this->entityModelMap.AddEntity(_entity, modelPtrPhys);
- this->topLevelModelMap.insert(std::make_pair(_entity,
- topLevelModel(_entity, _ecm)));
+ auto modelPtrPhys = worldPtrPhys->ConstructModel(*root.Model());
+ if (modelPtrPhys)
+ {
+ this->entityModelMap.AddEntity(_entity, modelPtrPhys);
+ this->topLevelModelMap.insert(std::make_pair(_entity,
+ topLevelModel(_entity, _ecm)));
+ }
}
}
// check if parent is a model (nested model)
@@ -1223,12 +1243,35 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm,
<< "] not found on model map." << std::endl;
return true;
}
+ auto basicModelPtrPhys = this->entityModelMap.Get(_parent->Data());
+
+ if (const auto existingLink = basicModelPtrPhys->GetLink(_name->Data()))
+ {
+ // No need to create this link because it was already created when
+ // parsing the model (links in models are required to have unique
+ // names). Instead we will register its existence and move along.
+ this->entityLinkMap.AddEntity(_entity, existingLink);
+ this->topLevelModelMap.insert(
+ std::make_pair(_entity, topLevelModel(_entity, _ecm)));
+ return true;
+ }
+
auto modelPtrPhys =
- this->entityModelMap.Get(_parent->Data());
+ this->entityModelMap
+ .EntityCast(_parent->Data());
+
+ if (!modelPtrPhys)
+ {
+ gzwarn << "Cannot create a new link [" << _name->Data() << "] "
+ << "because the physics engine plugin does not support "
+ << "link construction during runtime" << std::endl;
+ return true;
+ }
sdf::Link link;
link.SetName(_name->Data());
link.SetRawPose(_pose->Data());
+ link.SetPoseRelativeTo("");
if (this->staticEntities.find(_parent->Data()) !=
this->staticEntities.end())
@@ -1312,6 +1355,20 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm,
}
auto linkPtrPhys = this->entityLinkMap.Get(_parent->Data());
+ if (const auto existingShape = linkPtrPhys->GetShape(_name->Data()))
+ {
+ // No need to create this collision shape because it was already
+ // created when parsing the model.
+ auto linkCollisionFeature =
+ this->entityLinkMap.EntityCast(
+ _parent->Data());
+ this->entityCollisionMap.AddEntity(
+ _entity, linkCollisionFeature->GetShape(_name->Data()));
+ this->topLevelModelMap.insert(
+ std::make_pair(_entity, topLevelModel(_entity, _ecm)));
+ return true;
+ }
+
// Make a copy of the collision DOM so we can set its pose which has
// been resolved and is now expressed w.r.t the parent link of the
// collision.
@@ -1571,12 +1628,12 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
const components::ChildLinkName *_childLinkName) -> bool
{
// If the parent model is scheduled for recreation, then do not
- // try to create a new link. This situation can occur when a link
+ // try to create a new joint. This situation can occur when a joint
// is added to a model from the GUI model editor.
if (_ecm.EntityHasComponentType(_parentModel->Data(),
components::Recreate::typeId))
{
- // Add this entity to the set of newly added links to existing
+ // Add this entity to the set of newly added joints to existing
// models.
this->jointAddedToModel.insert(_entity);
return true;
@@ -1597,30 +1654,51 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
// Check if parent model exists
if (!this->entityModelMap.HasEntity(_parentModel->Data()))
{
- gzwarn << "Joint's parent entity [" << _parentModel->Data()
+ gzerr << "Joint's parent model entity [" << _parentModel->Data()
<< "] not found on model map." << std::endl;
return true;
}
- auto modelPtrPhys = this->entityModelMap.Get(_parentModel->Data());
- auto modelJointFeature =
- this->entityModelMap.EntityCast(
- _parentModel->Data());
- if (!modelJointFeature)
+ auto basicModelPtrPhys = this->entityModelMap
+ .EntityCast(_parentModel->Data());
+ if (!basicModelPtrPhys)
{
static bool informed{false};
if (!informed)
{
- gzdbg << "Attempting to process joints, but the physics "
- << "engine doesn't support joint features. "
- << "Joints will be ignored." << std::endl;
+ gzerr << "Attempting to create a new joint [" <<_name->Data()
+ << "] but the chosen physics engine does not support the "
+ << "minimal joint features, so no joints will be created."
+ << std::endl;
informed = true;
}
- // Break Each call since no joints can be processed
+ // Skip all other attempts to create joints
return false;
}
+ if (const auto existingJoint =
+ basicModelPtrPhys->GetJoint(_name->Data()))
+ {
+ // No need to create this joint because it was already created when
+ // parsing the model.
+ this->entityJointMap.AddEntity(_entity, existingJoint);
+ this->topLevelModelMap.insert(
+ std::make_pair(_entity, topLevelModel(_entity, _ecm)));
+ return true;
+ }
+
+ auto modelPtrPhys =
+ this->entityModelMap.EntityCast(
+ _parentModel->Data());
+ if (!modelPtrPhys)
+ {
+ gzerr << "Attempting to create a new joint [" << _name->Data()
+ << "], but the physics engine doesn't support constructing "
+ << "joints at runtime." << std::endl;
+ return true;
+ }
+
sdf::Joint joint;
joint.SetName(_name->Data());
joint.SetType(_jointType->Data());
@@ -1642,7 +1720,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
joint.SetAxis(1, jointAxis2->Data());
// Use the parent link's parent model as the model of this joint
- auto jointPtrPhys = modelJointFeature->ConstructJoint(joint);
+ auto jointPtrPhys = modelPtrPhys->ConstructJoint(joint);
if (jointPtrPhys.Valid())
{
@@ -1683,9 +1761,9 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
this->entityLinkMap.Get(_jointInfo->Data().parentLink);
if (!parentLinkPhys)
{
- gzwarn << "DetachableJoint's parent link entity ["
- << _jointInfo->Data().parentLink << "] not found in link map."
- << std::endl;
+ gzerr << "DetachableJoint's parent link entity ["
+ << _jointInfo->Data().parentLink << "] not found in link map."
+ << std::endl;
return true;
}
@@ -1695,8 +1773,8 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
auto childLinkPhys = this->entityLinkMap.Get(childLinkEntity);
if (!childLinkPhys)
{
- gzwarn << "Failed to find joint's child link [" << childLinkEntity
- << "]." << std::endl;
+ gzerr << "Failed to find joint's child link [" << childLinkEntity
+ << "]." << std::endl;
return true;
}
@@ -1708,7 +1786,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
static bool informed{false};
if (!informed)
{
- gzdbg << "Attempting to create a detachable joint, but the physics"
+ gzerr << "Attempting to create a detachable joint, but the physics"
<< " engine doesn't support feature "
<< "[AttachFixedJointFeature]. Detachable joints will be "
<< "ignored." << std::endl;
@@ -1741,7 +1819,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
}
else
{
- gzwarn << "DetachableJoint could not be created." << std::endl;
+ gzerr << "DetachableJoint could not be created." << std::endl;
}
return true;
});
diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc
index f51daef673..b3adede8f6 100644
--- a/src/systems/user_commands/UserCommands.cc
+++ b/src/systems/user_commands/UserCommands.cc
@@ -1220,6 +1220,7 @@ bool CreateCommand::Execute()
// Spherical coordinates
if (createMsg->has_spherical_coordinates())
{
+ gzerr << "HasSphericalCoordinates" << std::endl;
auto scComp = this->iface->ecm->Component(
this->iface->worldEntity);
if (nullptr == scComp)
diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc
index 542335801f..0511fc4d36 100644
--- a/test/integration/physics_system.cc
+++ b/test/integration/physics_system.cc
@@ -2000,8 +2000,8 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(Heightmap))
{
gz::sim::ServerConfig serverConfig;
- const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
- "/test/worlds/heightmap.sdf";
+ const auto sdfFile = common::joinPaths(PROJECT_BINARY_PATH,
+ "test", "worlds", "heightmap.sdf");
serverConfig.SetSdfFile(sdfFile);
sdf::Root root;
diff --git a/test/integration/thermal_system.cc b/test/integration/thermal_system.cc
index 26e7ffa68b..8244f781fa 100644
--- a/test/integration/thermal_system.cc
+++ b/test/integration/thermal_system.cc
@@ -53,8 +53,8 @@ TEST_F(ThermalTest, GZ_UTILS_TEST_DISABLED_ON_MAC(TemperatureComponent))
{
// Start server
ServerConfig serverConfig;
- serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
- "/test/worlds/thermal.sdf");
+ serverConfig.SetSdfFile(common::joinPaths(PROJECT_BINARY_PATH,
+ "test", "worlds", "thermal.sdf"));
Server server(serverConfig);
EXPECT_FALSE(server.Running());
@@ -177,8 +177,8 @@ TEST_F(ThermalTest, GZ_UTILS_TEST_DISABLED_ON_MAC(ThermalSensorSystem))
{
// Start server
ServerConfig serverConfig;
- serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH,
- "test/worlds/thermal.sdf"));
+ serverConfig.SetSdfFile(common::joinPaths(PROJECT_BINARY_PATH,
+ "test", "worlds", "thermal.sdf"));
Server server(serverConfig);
EXPECT_FALSE(server.Running());
diff --git a/test/worlds/CMakeLists.txt b/test/worlds/CMakeLists.txt
index ea81f11e65..70e61f51f3 100644
--- a/test/worlds/CMakeLists.txt
+++ b/test/worlds/CMakeLists.txt
@@ -1 +1,4 @@
configure_file (buoyancy.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/buoyancy.sdf)
+configure_file (mesh.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/mesh.sdf)
+configure_file (thermal.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/thermal.sdf)
+configure_file (heightmap.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/heightmap.sdf)
diff --git a/test/worlds/ackermann_steering.sdf b/test/worlds/ackermann_steering.sdf
index 1930cf3145..ea75b3b70d 100644
--- a/test/worlds/ackermann_steering.sdf
+++ b/test/worlds/ackermann_steering.sdf
@@ -40,6 +40,11 @@
50
+
+ 1
+ 1
+ 0.1
+
diff --git a/test/worlds/ackermann_steering_custom_frame_id.sdf b/test/worlds/ackermann_steering_custom_frame_id.sdf
index e1e318ba72..8e553c9769 100644
--- a/test/worlds/ackermann_steering_custom_frame_id.sdf
+++ b/test/worlds/ackermann_steering_custom_frame_id.sdf
@@ -48,6 +48,11 @@
50
+
+ 1
+ 1
+ 0.1
+
@@ -144,6 +149,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.1
+
@@ -189,6 +199,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.1
+
@@ -234,6 +249,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.1
+
@@ -279,6 +299,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.1
+
diff --git a/test/worlds/ackermann_steering_custom_topics.sdf b/test/worlds/ackermann_steering_custom_topics.sdf
index 1923027d07..a80f065e58 100644
--- a/test/worlds/ackermann_steering_custom_topics.sdf
+++ b/test/worlds/ackermann_steering_custom_topics.sdf
@@ -48,6 +48,11 @@
50
+
+ 1
+ 1
+ 0.1
+
@@ -144,6 +149,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.1
+
@@ -189,6 +199,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.1
+
@@ -234,6 +249,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.1
+
@@ -279,6 +299,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.1
+
diff --git a/test/worlds/ackermann_steering_limited_joints_pub.sdf b/test/worlds/ackermann_steering_limited_joints_pub.sdf
index f11bb3ad23..59c2eae3c1 100644
--- a/test/worlds/ackermann_steering_limited_joints_pub.sdf
+++ b/test/worlds/ackermann_steering_limited_joints_pub.sdf
@@ -48,6 +48,11 @@
50
+
+ 1
+ 1
+ 0.1
+
@@ -144,6 +149,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.1
+
@@ -189,6 +199,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.1
+
@@ -234,6 +249,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.1
+
@@ -279,6 +299,11 @@
1.0
0 0 1
+
+ 0.5
+ 1
+ 0.1
+
diff --git a/test/worlds/ackermann_steering_slow_odom.sdf b/test/worlds/ackermann_steering_slow_odom.sdf
index 1de374c7f9..5847ddacb4 100644
--- a/test/worlds/ackermann_steering_slow_odom.sdf
+++ b/test/worlds/ackermann_steering_slow_odom.sdf
@@ -40,6 +40,11 @@
50
+
+ 1
+ 1
+ 0.1
+
diff --git a/test/worlds/breadcrumbs.sdf b/test/worlds/breadcrumbs.sdf
index 7790557dba..8fe6818a26 100644
--- a/test/worlds/breadcrumbs.sdf
+++ b/test/worlds/breadcrumbs.sdf
@@ -187,6 +187,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -232,6 +237,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -277,6 +287,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -322,6 +337,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
diff --git a/test/worlds/collada_world_exporter.sdf b/test/worlds/collada_world_exporter.sdf
index fdfd1e834f..fff778f3be 100644
--- a/test/worlds/collada_world_exporter.sdf
+++ b/test/worlds/collada_world_exporter.sdf
@@ -1,4 +1,7 @@
+
diff --git a/test/worlds/heightmap.sdf b/test/worlds/heightmap.sdf.in
similarity index 95%
rename from test/worlds/heightmap.sdf
rename to test/worlds/heightmap.sdf.in
index 6cc2834cfe..db573ea196 100644
--- a/test/worlds/heightmap.sdf
+++ b/test/worlds/heightmap.sdf.in
@@ -11,7 +11,7 @@
- ../media/heightmap_bowl.png
+ file://@CMAKE_SOURCE_DIR@/test/media/heightmap_bowl.png
129 129 10
0 0 0
diff --git a/test/worlds/mesh.sdf b/test/worlds/mesh.sdf.in
similarity index 92%
rename from test/worlds/mesh.sdf
rename to test/worlds/mesh.sdf.in
index cdf0453685..abe7df140a 100644
--- a/test/worlds/mesh.sdf
+++ b/test/worlds/mesh.sdf.in
@@ -87,7 +87,7 @@
0.5 0.5 0.5
- file://test/media/duck_collider.dae
+ file://@CMAKE_SOURCE_DIR@/test/media/duck_collider.dae
@@ -97,7 +97,7 @@
0.5 0.5 0.5
- file://test/media/duck.dae
+ file://@CMAKE_SOURCE_DIR@/test/media/duck.dae
@@ -122,7 +122,7 @@
0 0 -0.4 1.57 0.0 0.0
- file://test/media/duck_collider.dae
+ file://@CMAKE_SOURCE_DIR@/test/media/duck_collider.dae
@@ -131,7 +131,7 @@
0 0 -0.4 1.57 0.0 0.0
- file://test/media/duck.dae
+ file://@CMAKE_SOURCE_DIR@/test/media/duck.dae
diff --git a/test/worlds/performer_detector.sdf b/test/worlds/performer_detector.sdf
index de36510e55..9b1cae5d51 100644
--- a/test/worlds/performer_detector.sdf
+++ b/test/worlds/performer_detector.sdf
@@ -119,6 +119,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -164,6 +169,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -209,6 +219,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
@@ -254,6 +269,11 @@
0
0 0 1
+
+ 1
+ 1
+ 0.1
+
diff --git a/test/worlds/thermal.sdf b/test/worlds/thermal.sdf.in
similarity index 96%
rename from test/worlds/thermal.sdf
rename to test/worlds/thermal.sdf.in
index 5cb3adf13f..2ec333e6e5 100644
--- a/test/worlds/thermal.sdf
+++ b/test/worlds/thermal.sdf.in
@@ -204,7 +204,7 @@
- ../media/duck.png
+ file://@CMAKE_SOURCE_DIR@/test/media/duck.png
400.0
@@ -248,7 +248,7 @@
- ../media/duck.png
+ file://@CMAKE_SOURCE_DIR@/test/media/duck.png
500.0
@@ -295,7 +295,7 @@
- ../media/duck.png
+ file://@CMAKE_SOURCE_DIR@/test/media/duck.png
400.0
@@ -317,7 +317,7 @@
- ../media/RescueRandy_Thermal.png
+ file://@CMAKE_SOURCE_DIR@/test/media/RescueRandy_Thermal.png
310