Skip to content

Commit

Permalink
DARTHeightmapShape: correctly load asymmetric terrains (#2818)
Browse files Browse the repository at this point in the history
The heightmap_valley.png used in the heightmap tests for ODE
and DART is symmetric, which obscures the fact that DART
is not treating asymmetric heightmaps properly.
This adds a test using worlds/heightmap.world and
heightmap_bowl.png to illustrate the bug.

* DARTHeightmapShape: set flipY = false

This variable was uninitialized and was causing problems
with asymmetric heightmaps.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
scpeters authored Aug 19, 2020

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
1 parent fa77112 commit cf4a7f0
Showing 3 changed files with 84 additions and 0 deletions.
1 change: 1 addition & 0 deletions gazebo/physics/dart/DARTHeightmapShape.cc
Original file line number Diff line number Diff line change
@@ -30,6 +30,7 @@ DARTHeightmapShape::DARTHeightmapShape(DARTCollisionPtr _parent)
: HeightmapShape(_parent),
dataPtr(new DARTHeightmapShapePrivate<HeightmapShape::HeightType>())
{
this->flipY = false;
}

//////////////////////////////////////////////////
78 changes: 78 additions & 0 deletions test/integration/heightmap.cc
Original file line number Diff line number Diff line change
@@ -69,6 +69,9 @@ class HeightmapTest : public ServerFixture,
// is the collision detector to use in DART. Can be fcl, dart, bullet or ode.
public: void TerrainCollision(const std::string &_physicsEngine,
const std::string &_dartCollision = "");
// \brief Test dropping boxes on asymmetric terrain
// \param[in] _physics the physics engine to test
public: void TerrainCollisionAsymmetric(const std::string &_physics);

/// \brief Test loading a heightmap that has no visuals
public: void NoVisual();
@@ -884,6 +887,75 @@ void HeightmapTest::TerrainCollision(const std::string &_physicsEngine,
EXPECT_GE(spherePose.Pos().Z(), (minHeight + radius*0.99));
}

/////////////////////////////////////////////////
void HeightmapTest::TerrainCollisionAsymmetric(const std::string &_physics)
{
if (_physics == "bullet")
{
gzerr << "Skipping test for bullet. See issue #2506" << std::endl;
return;
}

if (_physics == "simbody")
{
// SimbodyHeightmapShape unimplemented.
gzerr << "Aborting test for " << _physics << std::endl;
return;
}

Load("worlds/heightmap.world", true, _physics);

physics::WorldPtr world = physics::get_world("default");
ASSERT_NE(world, nullptr);

if (_physics == "dart")
{
#ifdef HAVE_DART
physics::PhysicsEnginePtr engine = world->Physics();
ASSERT_NE(engine, nullptr);
physics::DARTPhysicsPtr dartEngine
= boost::dynamic_pointer_cast<physics::DARTPhysics>(engine);
ASSERT_NE(dartEngine, nullptr);
std::string cd = dartEngine->CollisionDetectorInUse();
ASSERT_FALSE(cd.empty());
if (cd != "bullet")
{
// the test only works if DART uses bullet as a collision detector at the
// moment.
gzerr << "Aborting test for dart, see issue #909 and pull request #2956"
<< std::endl;
return;
}
#else
gzerr << "Have no DART installed, skipping test for DART." << std::endl;
return;
#endif
}

// each box has an initial z position of 10. meters
physics::ModelPtr box1 = GetModel("box1");
physics::ModelPtr box2 = GetModel("box2");
physics::ModelPtr box3 = GetModel("box3");
physics::ModelPtr box4 = GetModel("box4");
ASSERT_NE(box1, nullptr);
ASSERT_NE(box2, nullptr);
ASSERT_NE(box3, nullptr);
ASSERT_NE(box4, nullptr);
EXPECT_GE(box1->WorldPose().Pos().Z(), 9.9);
EXPECT_GE(box2->WorldPose().Pos().Z(), 9.9);
EXPECT_GE(box3->WorldPose().Pos().Z(), 9.9);
EXPECT_GE(box4->WorldPose().Pos().Z(), 9.9);

// step the world and verify that only box2 falls
world->Step(1000);

EXPECT_GE(box1->WorldPose().Pos().Z(), 9.9);
EXPECT_GE(box3->WorldPose().Pos().Z(), 9.9);
EXPECT_GE(box4->WorldPose().Pos().Z(), 9.9);

EXPECT_LT(box2->WorldPose().Pos().Z(), 5.5);
}

/////////////////////////////////////////////////
TEST_F(HeightmapTest, NotSquareImage)
{
@@ -1055,6 +1127,12 @@ TEST_F(HeightmapTest, TerrainCollisionDartBullet)
TerrainCollision("dart", "bullet");
}

/////////////////////////////////////////////////
TEST_P(HeightmapTest, TerrainCollisionAsymmetric)
{
TerrainCollisionAsymmetric(GetParam());
}

/////////////////////////////////////////////////
//
// Disabled: segfaults ocassionally
5 changes: 5 additions & 0 deletions worlds/heightmap.world
Original file line number Diff line number Diff line change
@@ -1,6 +1,11 @@
<?xml version="1.0" ?>
<sdf version="1.5">
<world name="default">
<physics type="ode">
<dart>
<collision_detector>bullet</collision_detector>
</dart>
</physics>
<!-- A global light source -->
<include>
<uri>model://sun</uri>

0 comments on commit cf4a7f0

Please sign in to comment.