diff --git a/CMakeLists.txt b/CMakeLists.txt index 4607f3bec1..aa2f8362b9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,6 +89,7 @@ ign_find_package (Qt5 # Find ignition-physics ign_find_package(ignition-physics4 COMPONENTS + heightmap mesh sdf REQUIRED diff --git a/examples/worlds/heightmap.sdf b/examples/worlds/heightmap.sdf index 69f354b513..6c2de6cb05 100644 --- a/examples/worlds/heightmap.sdf +++ b/examples/worlds/heightmap.sdf @@ -3,12 +3,15 @@ Demonstrates a heightmap terrain. - At the moment, only the visuals work, but not the physics collisions, see - https://github.com/ignitionrobotics/ign-physics/issues/156. - --> + + + + bullet + + @@ -23,7 +26,111 @@ scene 0.4 0.4 0.4 0.8 0.8 0.8 - -16.6 7.3 8.6 0.0 0.4 -0.45 + -80 40 60 0.0 0.4 -0.45 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + Transform control + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 400 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + docked + + + + + + + false + docked + @@ -42,6 +149,150 @@ -0.5 0.1 -0.9 + + 30 30 10 0 0 0 + + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + + + 0.5 + + + + + + + 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 30 -30 10 0 0 0 + + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + + + 0.5 + + + + + + + 0.5 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + -30 30 10 0 0 0 + + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + + + 0.5 + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + -30 -30 20 0 0 0 + + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + + + 0.5 + + + + + + + 0.5 + + + + 1 1 0 1 + 1 1 0 1 + 1 1 0 1 + + + + + https://fuel.ignitionrobotics.org/1.0/chapulina/models/Heightmap Bowl diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index fc89c760f2..c1381aec2e 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1893,9 +1893,15 @@ void RenderUtilPrivate::HighlightNode(const rendering::NodePtr &_node) white->SetEmissive(1.0, 1.0, 1.0); } - ignition::rendering::WireBoxPtr wireBox = - this->scene->CreateWireBox(); - ignition::math::AxisAlignedBox aabb = vis->LocalBoundingBox(); + auto aabb = vis->LocalBoundingBox(); + if (aabb == math::AxisAlignedBox()) + { + // Infinite bounding box, skip highlighting this node. + // This happens for Heightmaps, for example. + return; + } + + auto wireBox = this->scene->CreateWireBox(); wireBox->SetBox(aabb); // Create visual and add wire box diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index f51d04a6d5..a879c5d615 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -31,6 +31,8 @@ #include #include +#include +#include #include #include #include @@ -40,6 +42,7 @@ #include #include #include +#include #include #include @@ -70,6 +73,7 @@ // SDF #include +#include #include #include #include @@ -177,6 +181,30 @@ class ignition::gazebo::systems::PhysicsPrivate /// \param[in] _ecm Constant reference to ECM. public: void CreatePhysicsEntities(const EntityComponentManager &_ecm); + /// \brief Create world entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateWorldEntities(const EntityComponentManager &_ecm); + + /// \brief Create model entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateModelEntities(const EntityComponentManager &_ecm); + + /// \brief Create link entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateLinkEntities(const EntityComponentManager &_ecm); + + /// \brief Create collision entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateCollisionEntities(const EntityComponentManager &_ecm); + + /// \brief Create joint entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateJointEntities(const EntityComponentManager &_ecm); + + /// \brief Create Battery entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateBatteryEntities(const EntityComponentManager &_ecm); + /// \brief Remove physics entities if they are removed from the ECM /// \param[in] _ecm Constant reference to ECM. public: void RemovePhysicsEntities(const EntityComponentManager &_ecm); @@ -457,6 +485,16 @@ class ignition::gazebo::systems::PhysicsPrivate CollisionFeatureList, physics::mesh::AttachMeshShapeFeature>{}; + ////////////////////////////////////////////////// + // Heightmap + + /// \brief Feature list for heightmaps. + /// Include MinimumFeatureList so created collision can be automatically + /// up-cast. + public: struct HeightmapFeatureList : ignition::physics::FeatureList< + CollisionFeatureList, + physics::heightmap::AttachHeightmapShapeFeature>{}; + ////////////////////////////////////////////////// // Collision detector /// \brief Feature list for setting and getting the collision detector @@ -510,6 +548,7 @@ class ignition::gazebo::systems::PhysicsPrivate MinimumFeatureList, DetachableJointFeatureList, CollisionFeatureList, + HeightmapFeatureList, LinkForceFeatureList, MeshFeatureList>; @@ -710,6 +749,18 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) ////////////////////////////////////////////////// void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) +{ + this->CreateWorldEntities(_ecm); + this->CreateModelEntities(_ecm); + this->CreateLinkEntities(_ecm); + // We don't need to add visuals to the physics engine. + this->CreateCollisionEntities(_ecm); + this->CreateJointEntities(_ecm); + this->CreateBatteryEntities(_ecm); +} + +////////////////////////////////////////////////// +void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm) { // Get all the new worlds _ecm.EachNew( @@ -787,7 +838,11 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) return true; }); +} +////////////////////////////////////////////////// +void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) +{ _ecm.EachNew( [&](const Entity &_entity, @@ -915,7 +970,11 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) return true; }); +} +////////////////////////////////////////////////// +void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) +{ _ecm.EachNew( [&](const Entity &_entity, @@ -969,10 +1028,11 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) return true; }); +} - // We don't need to add visuals to the physics engine. - - // collisions +////////////////////////////////////////////////// +void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) +{ _ecm.EachNew( @@ -1051,6 +1111,56 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) math::eigen3::convert(_pose->Data()), math::eigen3::convert(meshSdf->Scale())); } + else if (_geom->Data().Type() == sdf::GeometryType::HEIGHTMAP) + { + auto linkHeightmapFeature = + this->entityLinkMap.EntityCast( + _parent->Data()); + if (!linkHeightmapFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to process heightmap geometries, but the " + << "physics engine doesn't support feature " + << "[AttachHeightmapShapeFeature]. Heightmaps will be " + << "ignored." << std::endl; + informed = true; + } + return true; + } + + auto heightmapSdf = _geom->Data().HeightmapShape(); + if (nullptr == heightmapSdf) + { + ignwarn << "Heightmap geometry for collision [" << _name->Data() + << "] missing heightmap shape." << std::endl; + return true; + } + + auto fullPath = asFullPath(heightmapSdf->Uri(), + heightmapSdf->FilePath()); + if (fullPath.empty()) + { + ignerr << "Heightmap geometry missing URI" << std::endl; + return true; + } + + common::ImageHeightmap data; + if (data.Load(fullPath) < 0) + { + ignerr << "Failed to load heightmap image data from [" << fullPath + << "]" << std::endl; + return true; + } + + collisionPtrPhys = linkHeightmapFeature->AttachHeightmapShape( + _name->Data(), + data, + math::eigen3::convert(_pose->Data()), + math::eigen3::convert(heightmapSdf->Size()), + heightmapSdf->Sampling()); + } else { auto linkCollisionFeature = @@ -1101,8 +1211,11 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) topLevelModel(_entity, _ecm))); return true; }); +} - // joints +////////////////////////////////////////////////// +void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) +{ _ecm.EachNew( - [&](const Entity & _entity, const components::BatterySoC *)->bool - { - // Parent entity of battery is model entity - this->entityOffMap.insert(std::make_pair( - _ecm.ParentEntity(_entity), false)); - return true; - }); - // Detachable joints _ecm.EachNew( [&](const Entity &_entity, @@ -1285,6 +1389,19 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) }); } +////////////////////////////////////////////////// +void PhysicsPrivate::CreateBatteryEntities(const EntityComponentManager &_ecm) +{ + _ecm.EachNew( + [&](const Entity & _entity, const components::BatterySoC *)->bool + { + // Parent entity of battery is model entity + this->entityOffMap.insert(std::make_pair( + _ecm.ParentEntity(_entity), false)); + return true; + }); +} + ////////////////////////////////////////////////// void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) { diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 5fe9702767..d71023d1a0 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -1588,3 +1588,83 @@ TEST_F(PhysicsSystemFixture, MovingCanonicalLinkOnly) EXPECT_EQ(iters, numParentModelLinkChecks); EXPECT_EQ(iters, numBaseLinkChildChecks); } + +///////////////////////////////////////////////// +TEST_F(PhysicsSystemFixture, Heightmap) +{ + ignition::gazebo::ServerConfig serverConfig; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/heightmap.sdf"; + serverConfig.SetSdfFile(sdfFile); + + sdf::Root root; + root.Load(sdfFile); + + bool checked{false}; + int maxIt{0}; + + test::Relay testSystem; + testSystem.OnPostUpdate( + [&](const gazebo::UpdateInfo &_info, + const gazebo::EntityComponentManager &_ecm) + { + double aboveHeight; + double farHeight; + bool checkedAbove{false}; + bool checkedFar{false}; + bool checkedHeightmap{false}; + + _ecm.Each( + [&](const ignition::gazebo::Entity &, const components::Model *, + const components::Name *_name, const components::Pose *_pose)->bool + { + if (_name->Data() == "above_heightmap") + { + aboveHeight = _pose->Data().Pos().Z(); + checkedAbove = true; + } + else if (_name->Data() == "far_from_heightmap") + { + farHeight = _pose->Data().Pos().Z(); + checkedFar = true; + } + else + { + EXPECT_EQ("Heightmap Bowl", _name->Data()); + EXPECT_EQ(math::Pose3d(), _pose->Data()); + checkedHeightmap = true; + } + + return true; + }); + + EXPECT_TRUE(checkedAbove); + EXPECT_TRUE(checkedFar); + EXPECT_TRUE(checkedHeightmap); + + // Both models drop from 7m + EXPECT_GE(7.01, aboveHeight) << _info.iterations; + EXPECT_GE(7.01, farHeight) << _info.iterations; + + // Model above heightmap hits it and never drops below 5.5m + EXPECT_LE(5.5, aboveHeight) << _info.iterations; + + // Model far from heightmap keeps falling + if (_info.iterations > 600) + { + EXPECT_GT(5.5, farHeight) << _info.iterations; + } + + checked = true; + maxIt = _info.iterations; + return true; + }); + + gazebo::Server server(serverConfig); + server.AddSystem(testSystem.systemPtr); + server.Run(true, 1000, false); + + EXPECT_TRUE(checked); + EXPECT_EQ(1000, maxIt); +} diff --git a/test/media/heightmap_bowl.png b/test/media/heightmap_bowl.png new file mode 100644 index 0000000000..52df5fcd07 Binary files /dev/null and b/test/media/heightmap_bowl.png differ diff --git a/test/worlds/heightmap.sdf b/test/worlds/heightmap.sdf new file mode 100644 index 0000000000..08eada3e70 --- /dev/null +++ b/test/worlds/heightmap.sdf @@ -0,0 +1,68 @@ + + + + + + true + + + + + ../media/heightmap_bowl.png + 129 129 10 + 0 0 0 + + + + + + + + 0 0 7 0 0 0 + + + + 0.4 + 0 + 0 + 0.4 + 0 + 0.4 + + 1.0 + + + + + 1 + + + + + + + + 1000 1000 7 0 0 0 + + + + 0.4 + 0 + 0 + 0.4 + 0 + 0.4 + + 1.0 + + + + + 1 + + + + + + +