diff --git a/gazebo/physics/dart/DARTHeightmapShape.cc b/gazebo/physics/dart/DARTHeightmapShape.cc index 59fbe55f50..b86c9ba07d 100644 --- a/gazebo/physics/dart/DARTHeightmapShape.cc +++ b/gazebo/physics/dart/DARTHeightmapShape.cc @@ -30,6 +30,7 @@ DARTHeightmapShape::DARTHeightmapShape(DARTCollisionPtr _parent) : HeightmapShape(_parent), dataPtr(new DARTHeightmapShapePrivate()) { + this->flipY = false; } ////////////////////////////////////////////////// diff --git a/test/integration/heightmap.cc b/test/integration/heightmap.cc index ef8f1db5dd..4e574af08f 100644 --- a/test/integration/heightmap.cc +++ b/test/integration/heightmap.cc @@ -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(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 diff --git a/worlds/heightmap.world b/worlds/heightmap.world index 3cddc00edf..b102a80296 100644 --- a/worlds/heightmap.world +++ b/worlds/heightmap.world @@ -1,6 +1,11 @@ + + + bullet + + model://sun