From 738579df58c7dd5a3b2c3a53d9b55e9b3410ae98 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 12 May 2022 08:17:55 -0700 Subject: [PATCH 1/2] Extruded 2D polyline geometries (#1456) Signed-off-by: Louise Poubel --- examples/worlds/polylines.sdf | 233 +++++++++++++++++++++++++++++++++ src/Conversions.cc | 37 ++++++ src/Conversions_TEST.cc | 39 +++++- src/rendering/SceneManager.cc | 22 ++++ src/systems/physics/Physics.cc | 130 ++++++++++++++++-- 5 files changed, 446 insertions(+), 15 deletions(-) create mode 100644 examples/worlds/polylines.sdf diff --git a/examples/worlds/polylines.sdf b/examples/worlds/polylines.sdf new file mode 100644 index 0000000000..5178eb54ca --- /dev/null +++ b/examples/worlds/polylines.sdf @@ -0,0 +1,233 @@ + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + -5 0 5 0 1.57 0 + + + 0 0 0.5 0 0 0 + 1.0 + + 0.1666 + 0 + 0 + 0.1666 + 0 + 0.1666 + + + + + + + -0.5 -0.5 + -0.5 0.5 + 0.5 0.5 + 0 0 + 0.5 -0.5 + 1 + + + + + + + + -0.5 -0.5 + -0.5 0.5 + 0.5 0.5 + 0 0 + 0.5 -0.5 + 1 + + + + 1.0 0 0 1 + 1.0 0 0 1 + 1.0 0 0 1 + + + + + + + -5 -3 5 0 1.3 0 + + + 0 0 2 0 0 0 + + + + + -0.3 0.5 + 0.3 0.3 + 0.3 -0.3 + -0.3 -0.5 + -0.5 0 + 4 + + + + + + + + -0.3 0.5 + 0.3 0.3 + 0.3 -0.3 + -0.3 -0.5 + -0.5 0 + 4 + + + + 0 1.0 0 1 + 0 1.0 0 1 + 0 1.0 0 1 + + + + + + + -5 2 5 0 1.57 0 + + + 0.5 0.5 0.75 0 0 0 + + + + + 0 0 + 0 1 + 1 1 + 1 0 + 1.5 + + + + + + + + 0 0 + 0 1 + 1 1 + 1 0 + 1.5 + + + + 0 1.0 1.0 1 + 0 1.0 1.0 1 + 0 1.0 1.0 1 + + + + + + + -5 2 5 0 1.57 0 + + + 2.2 1.5 0.5 0 0 0 + + + + + 2.27467 1.0967 + 1.81094 2.35418 + 2.74009 2.35418 + 1.0 + + + 2.08173 0.7599 + 2.4693 0.7599 + 3.4323 3.28672 + 3.07689 3.28672 + 2.84672 2.63851 + 1.7077 2.63851 + 1.47753 3.28672 + 1.11704 3.28672 + 1.0 + + + + + + + + 2.27467 1.0967 + 1.81094 2.35418 + 2.74009 2.35418 + 1.0 + + + 2.08173 0.7599 + 2.4693 0.7599 + 3.4323 3.28672 + 3.07689 3.28672 + 2.84672 2.63851 + 1.7077 2.63851 + 1.47753 3.28672 + 1.11704 3.28672 + 1.0 + + + + 1.0 0 1.0 1 + 1.0 0 1.0 1 + 1.0 0 1.0 1 + + + + + + diff --git a/src/Conversions.cc b/src/Conversions.cc index 88b5b2817f..64c9097bdd 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -56,6 +56,7 @@ #include #include #include +#include #include #include @@ -188,6 +189,20 @@ msgs::Geometry ignition::gazebo::convert(const sdf::Geometry &_in) meshMsg->set_submesh(meshSdf->Submesh()); meshMsg->set_center_submesh(meshSdf->CenterSubmesh()); } + else if (_in.Type() == sdf::GeometryType::POLYLINE && + !_in.PolylineShape().empty()) + { + out.set_type(msgs::Geometry::POLYLINE); + for (const auto &polyline : _in.PolylineShape()) + { + auto polylineMsg = out.add_polyline(); + polylineMsg->set_height(polyline.Height()); + for (const auto &point : polyline.Points()) + { + msgs::Set(polylineMsg->add_point(), point); + } + } + } else { ignerr << "Geometry type [" << static_cast(_in.Type()) @@ -252,6 +267,28 @@ sdf::Geometry ignition::gazebo::convert(const msgs::Geometry &_in) out.SetMeshShape(meshShape); } + else if (_in.type() == msgs::Geometry::POLYLINE && _in.polyline_size() > 0) + { + out.SetType(sdf::GeometryType::POLYLINE); + + std::vector polylines; + + for (auto i = 0; i < _in.polyline().size(); ++i) + { + auto polylineMsg = _in.polyline(i); + sdf::Polyline polylineShape; + polylineShape.SetHeight(polylineMsg.height()); + + for (auto j = 0; j < polylineMsg.point().size(); ++j) + { + polylineShape.AddPoint(msgs::Convert(polylineMsg.point(j))); + } + + polylines.push_back(polylineShape); + } + + out.SetPolylineShape(polylines); + } else { ignerr << "Geometry type [" << static_cast(_in.type()) diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 71ee1c639b..d3d906b277 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -424,6 +425,40 @@ TEST(Conversions, GeometryPlane) EXPECT_EQ(math::Vector3d::UnitY, newGeometry.PlaneShape()->Normal()); } +///////////////////////////////////////////////// +TEST(Conversions, GeometryPolyline) +{ + sdf::Geometry geometry; + geometry.SetType(sdf::GeometryType::POLYLINE); + + sdf::Polyline polylineShape; + polylineShape.SetHeight(1.23); + polylineShape.AddPoint({4.5, 6.7}); + polylineShape.AddPoint({8.9, 10.11}); + geometry.SetPolylineShape({polylineShape}); + + auto geometryMsg = convert(geometry); + EXPECT_EQ(msgs::Geometry::POLYLINE, geometryMsg.type()); + ASSERT_EQ(1, geometryMsg.polyline_size()); + EXPECT_DOUBLE_EQ(1.23, geometryMsg.polyline(0).height()); + ASSERT_EQ(2, geometryMsg.polyline(0).point_size()); + EXPECT_DOUBLE_EQ(4.5, geometryMsg.polyline(0).point(0).x()); + EXPECT_DOUBLE_EQ(6.7, geometryMsg.polyline(0).point(0).y()); + EXPECT_DOUBLE_EQ(8.9, geometryMsg.polyline(0).point(1).x()); + EXPECT_DOUBLE_EQ(10.11, geometryMsg.polyline(0).point(1).y()); + + auto newGeometry = convert(geometryMsg); + EXPECT_EQ(sdf::GeometryType::POLYLINE, newGeometry.Type()); + ASSERT_FALSE(newGeometry.PolylineShape().empty()); + ASSERT_EQ(1u, newGeometry.PolylineShape().size()); + EXPECT_DOUBLE_EQ(1.23, newGeometry.PolylineShape()[0].Height()); + ASSERT_EQ(2u, newGeometry.PolylineShape()[0].PointCount()); + EXPECT_DOUBLE_EQ(4.5, newGeometry.PolylineShape()[0].PointByIndex(0)->X()); + EXPECT_DOUBLE_EQ(6.7, newGeometry.PolylineShape()[0].PointByIndex(0)->Y()); + EXPECT_DOUBLE_EQ(8.9, newGeometry.PolylineShape()[0].PointByIndex(1)->X()); + EXPECT_DOUBLE_EQ(10.11, newGeometry.PolylineShape()[0].PointByIndex(1)->Y()); +} + ///////////////////////////////////////////////// TEST(Conversions, Inertial) { @@ -543,7 +578,7 @@ TEST(Conversions, Atmosphere) } ///////////////////////////////////////////////// -TEST(CONVERSIONS, MagnetometerSensor) +TEST(Conversions, MagnetometerSensor) { sdf::Sensor sensor; sensor.SetName("my_sensor"); @@ -583,7 +618,7 @@ TEST(CONVERSIONS, MagnetometerSensor) } ///////////////////////////////////////////////// -TEST(CONVERSIONS, AltimeterSensor) +TEST(Conversions, AltimeterSensor) { sdf::Sensor sensor; sensor.SetName("my_sensor"); diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index a5f360259a..a38ed7ffe8 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -33,6 +34,7 @@ #include #include #include +#include #include #include @@ -450,6 +452,26 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, geom = this->dataPtr->scene->CreateMesh(descriptor); scale = _geom.MeshShape()->Scale(); } + else if (_geom.Type() == sdf::GeometryType::POLYLINE) + { + std::vector> vertices; + for (const auto &polyline : _geom.PolylineShape()) + { + vertices.push_back(polyline.Points()); + } + + std::string name("POLYLINE_" + common::Uuid().String()); + + auto meshManager = ignition::common::MeshManager::Instance(); + meshManager->CreateExtrudedPolyline(name, vertices, + _geom.PolylineShape()[0].Height()); + + rendering::MeshDescriptor descriptor; + descriptor.meshName = name; + descriptor.mesh = meshManager->MeshByName(name); + + geom = this->dataPtr->scene->CreateMesh(descriptor); + } else { ignerr << "Unsupported geometry type" << std::endl; diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 647250b4bf..1ec9b9c43d 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -33,6 +33,7 @@ #include #include #include +#include #include #include #include @@ -73,6 +74,7 @@ #include #include #include +#include #include #include @@ -177,6 +179,31 @@ 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); @@ -664,6 +691,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( @@ -689,7 +728,11 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) return true; }); +} +////////////////////////////////////////////////// +void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) +{ _ecm.EachNew( [&](const Entity &_entity, @@ -817,7 +860,11 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) return true; }); +} +////////////////////////////////////////////////// +void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) +{ _ecm.EachNew( [&](const Entity &_entity, @@ -871,10 +918,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( @@ -953,6 +1001,55 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) math::eigen3::convert(_pose->Data()), math::eigen3::convert(meshSdf->Scale())); } + else if (_geom->Data().Type() == sdf::GeometryType::POLYLINE) + { + auto polylineSdf = _geom->Data().PolylineShape(); + if (polylineSdf.empty()) + { + ignwarn << "Polyline geometry for collision [" << _name->Data() + << "] missing polylines." << std::endl; + return true; + } + + std::vector> vertices; + for (const auto &polyline : _geom->Data().PolylineShape()) + { + vertices.push_back(polyline.Points()); + } + + std::string name("POLYLINE_" + common::Uuid().String()); + auto meshManager = ignition::common::MeshManager::Instance(); + meshManager->CreateExtrudedPolyline(name, vertices, + _geom->Data().PolylineShape()[0].Height()); + + auto polyline = meshManager->MeshByName(name); + if (nullptr == polyline) + { + ignwarn << "Failed to create polyline for collision [" + << _name->Data() << "]." << std::endl; + return true; + } + + auto linkMeshFeature = + this->entityLinkMap.EntityCast(_parent->Data()); + if (!linkMeshFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to process polyline geometries, but the" + << " physics engine doesn't support feature " + << "[AttachMeshShapeFeature]. Polylines will be ignored." + << std::endl; + informed = true; + } + return true; + } + + collisionPtrPhys = linkMeshFeature->AttachMeshShape(_name->Data(), + *polyline, + math::eigen3::convert(_pose->Data())); + } else { auto linkCollisionFeature = @@ -1003,8 +1100,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, @@ -1222,6 +1313,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) { From 7225f383919a1aa71cdafa9558fb3895f9f66f11 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 12 May 2022 09:27:23 -0700 Subject: [PATCH 2/2] View polyline collisions on the GUI (#1481) Signed-off-by: Louise Poubel --- .../VisualizationCapabilities.cc | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc index 731cd22bc4..e995949bab 100644 --- a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -65,6 +66,7 @@ #include #include #include +#include #include #include "ignition/gazebo/components/CastShadows.hh" @@ -1295,6 +1297,26 @@ rendering::GeometryPtr VisualizationCapabilitiesPrivate::CreateGeometry( } scale = _geom.HeightmapShape()->Size(); } + else if (_geom.Type() == sdf::GeometryType::POLYLINE) + { + std::vector> vertices; + for (const auto &polyline : _geom.PolylineShape()) + { + vertices.push_back(polyline.Points()); + } + + std::string name("POLYLINE_" + common::Uuid().String()); + + auto meshManager = ignition::common::MeshManager::Instance(); + meshManager->CreateExtrudedPolyline(name, vertices, + _geom.PolylineShape()[0].Height()); + + rendering::MeshDescriptor descriptor; + descriptor.meshName = name; + descriptor.mesh = meshManager->MeshByName(name); + + geom = this->scene->CreateMesh(descriptor); + } else { ignerr << "Unsupported geometry type" << std::endl;