diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc index eeffa3052..2356c766a 100644 --- a/tpe/plugin/src/SDFFeatures.cc +++ b/tpe/plugin/src/SDFFeatures.cc @@ -96,8 +96,7 @@ Identity SDFFeatures::ConstructSdfLink( ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfCollision( const Identity &_linkID, - const ::sdf::Collision &_sdfCollision, - const common::Mesh *_mesh) + const ::sdf::Collision &_sdfCollision) { // Read sdf params const std::string name = _sdfCollision.Name(); @@ -132,7 +131,28 @@ Identity SDFFeatures::ConstructSdfCollision( shape.SetRadius(sphereSdf->Radius()); collision->SetShape(shape); } - else if (geom->Type() == ::sdf::GeometryType::MESH) + const auto collisionIdentity = this->AddCollision(link->GetId(), *collision); + return collisionIdentity; +} + +///////////////////////////////////////////////// +Identity SDFFeatures::ConstructSdfCollision( + const Identity &_linkID, + const ::sdf::Collision &_sdfCollision, + const common::Mesh *_mesh) +{ + // Read sdf params + const std::string name = _sdfCollision.Name(); + std::cerr << "construct collision " << name << std::endl; + const auto pose = _sdfCollision.RawPose(); + const auto geom = _sdfCollision.Geom(); + + auto link = this->links.at(_linkID)->link; + tpelib::Entity &ent = link->AddCollision(); + tpelib::Collision *collision = static_cast(&ent); + collision->SetName(name); + collision->SetPose(pose); + if (geom->Type() == ::sdf::GeometryType::MESH) { // \todo(anyone) the code here assumes that the mesh is loaded externally // and passed in as argument as there is no logic for searching resources @@ -151,6 +171,7 @@ Identity SDFFeatures::ConstructSdfCollision( const auto collisionIdentity = this->AddCollision(link->GetId(), *collision); return collisionIdentity; +} /*///////////////////////////////////////////////// Identity SDFFeatures::BuildSdfLink( @@ -202,4 +223,3 @@ Identity SDFFeatures::BuildSdfLink( return linkIdentity; } */ -} diff --git a/tpe/plugin/src/SDFFeatures.hh b/tpe/plugin/src/SDFFeatures.hh index c9e6b0a8d..615d9c4e3 100644 --- a/tpe/plugin/src/SDFFeatures.hh +++ b/tpe/plugin/src/SDFFeatures.hh @@ -26,6 +26,7 @@ #include #include +#include #include #include @@ -41,6 +42,7 @@ using SDFFeatureList = FeatureList< sdf::ConstructSdfWorld, sdf::ConstructSdfModel, sdf::ConstructSdfLink, + sdf::ConstructSdfCollision, sdf::ConstructSdfVisual >; @@ -65,10 +67,14 @@ class SDFFeatures : const Identity &_modelID, const ::sdf::Link &_sdfLink) override; + private: Identity ConstructSdfCollision( + const Identity &_linkID, + const ::sdf::Collision &_collision); + private: Identity ConstructSdfCollision( const Identity &_linkID, const ::sdf::Collision &_collision, - const ignition::common::Mesh *_mesh = nullptr); + const ignition::common::Mesh *_mesh); }; }