Skip to content

Commit

Permalink
clean up tests
Browse files Browse the repository at this point in the history 
Signed-off-by: Ashton Larkin <[email protected]>
  • Loading branch information
@adlarkin
adlarkin committed Jun 23, 2021
1 parent 6d91d6f commit 78a8297
Show file tree
Hide file tree
Showing 3 changed files with 140 additions and 187 deletions.
183 changes: 88 additions & 95 deletions test/integration/physics_system.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,87 +73,6 @@ class PhysicsSystemFixture : public ::testing::Test
ignition::common::setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH",
(std::string(PROJECT_BINARY_PATH) + "/lib").c_str());
}

/// \brief Test pose updates for models that only have their canonical link
/// move. In this scenario, we want to make sure that all of the other links
/// in the model have their pose updated since non-canonical link poses are
/// reported w.r.t. the model (since the canonical link's pose changed, this
/// means that the model's pose changed).
/// \param[in] _fileName The name of the file to test.
/// \note This method is to avoid code duplication so that the same models can
/// be tested with different links as the canonical link.
protected: void TestStaticNonCanonicalLinkUpdates(
const std::string &_fileName)
{
ignition::gazebo::ServerConfig serverConfig;

const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + _fileName;

sdf::Root root;
root.Load(sdfFile);
const sdf::World *world = root.WorldByIndex(0);
ASSERT_TRUE(nullptr != world);

serverConfig.SetSdfFile(sdfFile);

gazebo::Server server(serverConfig);

server.SetUpdatePeriod(1us);

// Create a system that records the poses of the links after physics
test::Relay testSystem;

size_t numBaseLinkChecks = 0;
size_t numOuterLinkChecks = 0;

size_t currIter = 0;
testSystem.OnPostUpdate(
[&world, &numBaseLinkChecks, &numOuterLinkChecks, &currIter](
const gazebo::UpdateInfo &,
const gazebo::EntityComponentManager &_ecm)
{
currIter++;
_ecm.Each<components::Link, components::Name>(
[&](const ignition::gazebo::Entity &_entity,
const components::Link *, const components::Name *_name)->bool
{
// ignore the link for the ground plane
if (_name->Data() != "surface")
{
if (_name->Data() == "base_link")
{
// link "base_link" falls due to gravity, starting from rest
const double dt = 0.001;
const double zExpected =
0.5 * world->Gravity().Z() * pow(dt * currIter, 2);
EXPECT_NEAR(zExpected,
ignition::gazebo::worldPose(_entity, _ecm).Z(), 1e-2);
numBaseLinkChecks++;
}
else
{
// link "link0_outer" is resting on the ground and does not
// move, so it should always have a pose of (1 0 0 0 0 0)
EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0),
ignition::gazebo::worldPose(_entity, _ecm));
numOuterLinkChecks++;
}
}

return true;
});

return true;
});

server.AddSystem(testSystem.systemPtr);
const size_t iters = 500;
server.Run(true, iters, false);

EXPECT_EQ(iters, numBaseLinkChecks);
EXPECT_EQ(iters, numOuterLinkChecks);
EXPECT_EQ(iters, currIter);
}
};

/////////////////////////////////////////////////
Expand Down Expand Up @@ -1076,19 +995,93 @@ TEST_F(PhysicsSystemFixture, NestedModel)
// changes, but other links do not
TEST_F(PhysicsSystemFixture, MovingCanonicalLinkOnly)
{
TestStaticNonCanonicalLinkUpdates(
"/test/worlds/static_non_canonical_link.sdf");
}
ignition::gazebo::ServerConfig serverConfig;

/////////////////////////////////////////////////
// This tests whether pose updates are correct for a model whose canonical link
// changes, but other links do not.
//
// This is similar to the MovingCanonicalLinkOnly test, with the difference in
// this test being that the canonical link for the model in this test was
// manually specified by the user instead of defaulting to the first link in the
// model.
TEST_F(PhysicsSystemFixture, MovingCustomCanonicalLinkOnly)
{
TestStaticNonCanonicalLinkUpdates("/test/worlds/custom_canonical_links.sdf");
const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
"/test/worlds/only_canonical_link_moves.sdf";

sdf::Root root;
root.Load(sdfFile);
const sdf::World *world = root.WorldByIndex(0);
ASSERT_TRUE(nullptr != world);

serverConfig.SetSdfFile(sdfFile);

gazebo::Server server(serverConfig);

server.SetUpdatePeriod(1us);

// Create a system that records the poses of the links after physics
test::Relay testSystem;

size_t numBaseLinkChecks = 0;
size_t numOuterLinkChecks = 0;
size_t numBaseLinkCustomChecks = 0;
size_t numOuterLinkCustomChecks = 0;

size_t currIter = 0;
testSystem.OnPostUpdate(
[&world, &numBaseLinkChecks, &numOuterLinkChecks, &numBaseLinkCustomChecks,
&numOuterLinkCustomChecks, &currIter](
const gazebo::UpdateInfo &,
const gazebo::EntityComponentManager &_ecm)
{
currIter++;
_ecm.Each<components::Link, components::Name>(
[&](const ignition::gazebo::Entity &_entity,
const components::Link *, const components::Name *_name)->bool
{
// ignore the link for the ground plane
if (_name->Data() != "surface")
{
const double dt = 0.001;
const double zExpected =
0.5 * world->Gravity().Z() * pow(dt * currIter, 2);

if (_name->Data() == "base_link")
{
// link "base_link" falls due to gravity, starting from rest
EXPECT_NEAR(zExpected,
ignition::gazebo::worldPose(_entity, _ecm).Z(), 1e-2);
numBaseLinkChecks++;
}
else if (_name->Data() == "link0_outer")
{
// link "link0_outer" is resting on the ground and does not
// move, so it should always have a pose of (1 0 0 0 0 0)
EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0),
ignition::gazebo::worldPose(_entity, _ecm));
numOuterLinkChecks++;
}
else if (_name->Data() == "base_link_custom")
{
// same as link "base_link"
EXPECT_NEAR(zExpected,
ignition::gazebo::worldPose(_entity, _ecm).Z(), 1e-2);
numBaseLinkCustomChecks++;
}
else if (_name->Data() == "link0_outer_custom")
{
// same as "link0_outer", but with an offset pose
EXPECT_EQ(ignition::math::Pose3d(1, 2, 0, 0, 0, 0),
ignition::gazebo::worldPose(_entity, _ecm));
numOuterLinkCustomChecks++;
}
}

return true;
});

return true;
});

server.AddSystem(testSystem.systemPtr);
const size_t iters = 500;
server.Run(true, iters, false);

EXPECT_EQ(iters, currIter);
EXPECT_EQ(iters, numBaseLinkChecks);
EXPECT_EQ(iters, numOuterLinkChecks);
EXPECT_EQ(iters, numBaseLinkCustomChecks);
EXPECT_EQ(iters, numOuterLinkCustomChecks);
}
92 changes: 0 additions & 92 deletions test/worlds/custom_canonical_links.sdf
View file

This file was deleted.

52 changes: 52 additions & 0 deletions test/worlds/static_non_canonical_link.sdf → test/worlds/only_canonical_link_moves.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,58 @@
</link>
</model>

<!--
Same as the model above, but mark the second link of this model as the
canonical link. This is to test whether all links of a model are updated
after a canonical link update is registered. By default, unless a
canonical link is specified, then the first link of a model is the
canonical link. We want to make sure that model/link poses update
correctly regardless of the order of link updates that are
received/processed.
-->
<model name="cylinder1_custom" canonical_link="base_link_custom">
<pose>0 2 0 0 0 0</pose>
<link name="link0_outer_custom">
<pose>1 0 0 0 0 0</pose>
<collision name="collision_disk">
<geometry>
<cylinder>
<radius>0.2</radius>
<length>0.2</length>
</cylinder>
</geometry>
</collision>
<visual name="visual1">
<geometry>
<cylinder>
<radius>0.2</radius>
<length>0.2</length>
</cylinder>
</geometry>
<material>
<ambient>0.6 0.6 0.9 1</ambient>
<diffuse>0.6 0.6 0.9 1</diffuse>
<specular>1.0 1.0 1.0 1</specular>
</material>
</visual>
</link>

<link name="base_link_custom">
<visual name="visual1">
<geometry>
<cylinder>
<radius>0.2</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<ambient>0.0 0.6 0.9 0.4</ambient>
<diffuse>0.0 0.6 0.9 0.4</diffuse>
</material>
</visual>
</link>
</model>

<!-- TODO(adlarkin) add a similar test with nested models? -->
</world>
</sdf>

0 comments on commit 78a8297

Please sign in to comment.