Skip to content

Commit

Permalink
Added Root::UpdateGraphs functions
Browse files Browse the repository at this point in the history 
Signed-off-by: Nate Koenig <[email protected]>
  • Loading branch information
Nate Koenig committed Feb 10, 2022
1 parent 5199cdd commit 9c16a72
Show file tree
Hide file tree
Showing 3 changed files with 133 additions and 78 deletions.
6 changes: 6 additions & 0 deletions include/sdf/Root.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -157,6 +157,12 @@ namespace sdf
/// IGN_UTILS_IMPL_PTR instead.
public: void Clone(const sdf::Root &_root);

/// \brief Recreate the frame and pose graphs for the worlds and model
/// that are children of this Root object.
/// \return Errors, which is a vector of Error objects. Each Error includes
/// an error code and message. An empty vector indicates no error.
public: Errors UpdateGraphs();

/// \brief Private data pointer
IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr)
};
Expand Down
85 changes: 59 additions & 26 deletions src/Root.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,16 @@ using namespace sdf;
/// \brief Private data for sdf::Root
class sdf::Root::Implementation
{
/// \brief Build frame and pose graphs for the provided world.
/// \param[in, out] _world World object to build graphs for.
/// \param[out] _errors The list of errors generated by this function.
public: void UpdateGraphs(sdf::World &_world, sdf::Errors &_errors);

/// \brief Build frame and pose graphs for the provided model.
/// \param[in, out] _model Model object to build graphs for.
/// \param[out] _errors The list of errors generated by this function.
public: void UpdateGraphs(sdf::Model &_model, sdf::Errors &_errors);

/// \brief Version string
public: std::string version = "";

Expand Down Expand Up @@ -235,14 +245,7 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config)

Errors worldErrors = world.Load(elem, _config);

// Build the graphs.
auto frameAttachedToGraph = addFrameAttachedToGraph(
this->dataPtr->worldFrameAttachedToGraphs, world, worldErrors);
world.SetFrameAttachedToGraph(frameAttachedToGraph);

auto poseRelativeToGraph = addPoseRelativeToGraph(
this->dataPtr->worldPoseRelativeToGraphs, world, worldErrors);
world.SetPoseRelativeToGraph(poseRelativeToGraph);
this->dataPtr->UpdateGraphs(world, worldErrors);

// Attempt to load the world
if (worldErrors.empty())
Expand Down Expand Up @@ -283,15 +286,7 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config)
}
this->dataPtr->modelLightOrActor = std::move(models.front());
sdf::Model &model = std::get<sdf::Model>(this->dataPtr->modelLightOrActor);
// Build the graphs.
this->dataPtr->modelFrameAttachedToGraph =
createFrameAttachedToGraph(model, errors);

model.SetFrameAttachedToGraph(this->dataPtr->modelFrameAttachedToGraph);

this->dataPtr->modelPoseRelativeToGraph =
createPoseRelativeToGraph(model, errors);
model.SetPoseRelativeToGraph(this->dataPtr->modelPoseRelativeToGraph);
this->dataPtr->UpdateGraphs(model, errors);
}

// Load all the lights.
Expand Down Expand Up @@ -427,17 +422,55 @@ void Root::Clone(const sdf::Root &_root)

this->dataPtr->modelLightOrActor = _root.dataPtr->modelLightOrActor;

this->dataPtr->worldFrameAttachedToGraphs =
_root.dataPtr->worldFrameAttachedToGraphs ;
this->UpdateGraphs();
}

/////////////////////////////////////////////////
Errors Root::UpdateGraphs()
{
sdf::Errors errors;

this->dataPtr->worldFrameAttachedToGraphs.clear();
this->dataPtr->worldPoseRelativeToGraphs.clear();

// Build graphs for each world.
for (World &world : this->dataPtr->worlds)
{
this->dataPtr->UpdateGraphs(world, errors);
}

this->dataPtr->modelFrameAttachedToGraph =
_root.dataPtr->modelFrameAttachedToGraph ;
// Build graphs for the model, if one is present.
if (std::holds_alternative<sdf::Model>(this->dataPtr->modelLightOrActor))
{
sdf::Model &model = std::get<sdf::Model>(this->dataPtr->modelLightOrActor);
this->dataPtr->UpdateGraphs(model, errors);
}

this->dataPtr->worldPoseRelativeToGraphs =
_root.dataPtr->worldPoseRelativeToGraphs;
return errors;
}

this->dataPtr->modelPoseRelativeToGraph =
_root.dataPtr->modelPoseRelativeToGraph;
//////////////////////////////////////////////////
void Root::Implementation::UpdateGraphs(sdf::World &_world,
sdf::Errors &_errors)
{
// Build the frame graph.
auto frameAttachedToGraph = addFrameAttachedToGraph(
this->worldFrameAttachedToGraphs, _world, _errors);
_world.SetFrameAttachedToGraph(frameAttachedToGraph);

// Build the pose graph.
auto poseRelativeToGraph = addPoseRelativeToGraph(
this->worldPoseRelativeToGraphs, _world, _errors);
_world.SetPoseRelativeToGraph(poseRelativeToGraph);
}

//////////////////////////////////////////////////
void Root::Implementation::UpdateGraphs(sdf::Model &_model,
sdf::Errors &_errors)
{
this->modelFrameAttachedToGraph = createFrameAttachedToGraph(_model, _errors);
_model.SetFrameAttachedToGraph(this->modelFrameAttachedToGraph);

this->dataPtr->sdf = _root.dataPtr->sdf;
this->modelPoseRelativeToGraph = createPoseRelativeToGraph(_model, _errors);
_model.SetPoseRelativeToGraph(this->modelPoseRelativeToGraph);
}
120 changes: 68 additions & 52 deletions test/integration/frame.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -676,60 +676,76 @@ TEST(DOMFrame, LoadWorldFramesAttachedTo)
sdf::Root root;
EXPECT_TRUE(root.Load(testFile).empty());

// Get the first world
const sdf::World *world = root.WorldByIndex(0);
ASSERT_NE(nullptr, world);
EXPECT_EQ("world_frame_attached_to", world->Name());
EXPECT_EQ(1u, world->ModelCount());
EXPECT_NE(nullptr, world->ModelByIndex(0));
EXPECT_EQ(nullptr, world->ModelByIndex(1));

EXPECT_TRUE(world->ModelNameExists("M1"));

const sdf::Model *model = world->ModelByIndex(0);
ASSERT_NE(nullptr, model);
EXPECT_EQ("M1", model->Name());
EXPECT_EQ(1u, model->LinkCount());
EXPECT_NE(nullptr, model->LinkByIndex(0));
EXPECT_EQ(nullptr, model->LinkByIndex(1));
EXPECT_EQ(1u, model->FrameCount());
EXPECT_NE(nullptr, model->FrameByIndex(0));
EXPECT_EQ(nullptr, model->FrameByIndex(1));
ASSERT_TRUE(model->LinkNameExists("L"));
ASSERT_TRUE(model->FrameNameExists("F0"));
EXPECT_EQ("L", model->FrameByName("F0")->AttachedTo());

EXPECT_EQ(4u, world->FrameCount());
EXPECT_NE(nullptr, world->FrameByIndex(0));
EXPECT_NE(nullptr, world->FrameByIndex(1));
EXPECT_NE(nullptr, world->FrameByIndex(2));
EXPECT_NE(nullptr, world->FrameByIndex(3));
EXPECT_EQ(nullptr, world->FrameByIndex(4));
ASSERT_TRUE(world->FrameNameExists("world_frame"));
ASSERT_TRUE(world->FrameNameExists("F0"));
ASSERT_TRUE(world->FrameNameExists("F1"));
ASSERT_TRUE(world->FrameNameExists("F2"));
auto testFunc = std::function<void(sdf::Root &)>(
[](sdf::Root &_root)
{
// Get the first world
const sdf::World *world = _root.WorldByIndex(0);
ASSERT_NE(nullptr, world);
EXPECT_EQ("world_frame_attached_to", world->Name());
EXPECT_EQ(1u, world->ModelCount());
EXPECT_NE(nullptr, world->ModelByIndex(0));
EXPECT_EQ(nullptr, world->ModelByIndex(1));

EXPECT_TRUE(world->FrameByName("world_frame")->AttachedTo().empty());
EXPECT_TRUE(world->FrameByName("F0")->AttachedTo().empty());
EXPECT_EQ("F0", world->FrameByName("F1")->AttachedTo());
EXPECT_EQ("M1", world->FrameByName("F2")->AttachedTo());
EXPECT_TRUE(world->ModelNameExists("M1"));

EXPECT_TRUE(world->FrameByName("world_frame")->PoseRelativeTo().empty());
EXPECT_TRUE(world->FrameByName("F0")->PoseRelativeTo().empty());
EXPECT_TRUE(world->FrameByName("F1")->PoseRelativeTo().empty());
EXPECT_TRUE(world->FrameByName("F2")->PoseRelativeTo().empty());

std::string body;
EXPECT_TRUE(
world->FrameByName("world_frame")->ResolveAttachedToBody(body).empty());
EXPECT_EQ("world", body);
EXPECT_TRUE(world->FrameByName("F0")->ResolveAttachedToBody(body).empty());
EXPECT_EQ("world", body);
EXPECT_TRUE(world->FrameByName("F1")->ResolveAttachedToBody(body).empty());
EXPECT_EQ("world", body);
EXPECT_TRUE(world->FrameByName("F2")->ResolveAttachedToBody(body).empty());
EXPECT_EQ("M1::L", body);
const sdf::Model *model = world->ModelByIndex(0);
ASSERT_NE(nullptr, model);
EXPECT_EQ("M1", model->Name());
EXPECT_EQ(1u, model->LinkCount());
EXPECT_NE(nullptr, model->LinkByIndex(0));
EXPECT_EQ(nullptr, model->LinkByIndex(1));
EXPECT_EQ(1u, model->FrameCount());
EXPECT_NE(nullptr, model->FrameByIndex(0));
EXPECT_EQ(nullptr, model->FrameByIndex(1));
ASSERT_TRUE(model->LinkNameExists("L"));
ASSERT_TRUE(model->FrameNameExists("F0"));
EXPECT_EQ("L", model->FrameByName("F0")->AttachedTo());

EXPECT_EQ(4u, world->FrameCount());
EXPECT_NE(nullptr, world->FrameByIndex(0));
EXPECT_NE(nullptr, world->FrameByIndex(1));
EXPECT_NE(nullptr, world->FrameByIndex(2));
EXPECT_NE(nullptr, world->FrameByIndex(3));
EXPECT_EQ(nullptr, world->FrameByIndex(4));
ASSERT_TRUE(world->FrameNameExists("world_frame"));
ASSERT_TRUE(world->FrameNameExists("F0"));
ASSERT_TRUE(world->FrameNameExists("F1"));
ASSERT_TRUE(world->FrameNameExists("F2"));

EXPECT_TRUE(world->FrameByName("world_frame")->AttachedTo().empty());
EXPECT_TRUE(world->FrameByName("F0")->AttachedTo().empty());
EXPECT_EQ("F0", world->FrameByName("F1")->AttachedTo());
EXPECT_EQ("M1", world->FrameByName("F2")->AttachedTo());

EXPECT_TRUE(world->FrameByName("world_frame")->PoseRelativeTo().empty());
EXPECT_TRUE(world->FrameByName("F0")->PoseRelativeTo().empty());
EXPECT_TRUE(world->FrameByName("F1")->PoseRelativeTo().empty());
EXPECT_TRUE(world->FrameByName("F2")->PoseRelativeTo().empty());

std::string body;
EXPECT_TRUE(
world->FrameByName("world_frame")->ResolveAttachedToBody(body).empty());
EXPECT_EQ("world", body);
EXPECT_TRUE(world->FrameByName("F0")->ResolveAttachedToBody(body).empty());
EXPECT_EQ("world", body);
EXPECT_TRUE(world->FrameByName("F1")->ResolveAttachedToBody(body).empty());
EXPECT_EQ("world", body);
EXPECT_TRUE(world->FrameByName("F2")->ResolveAttachedToBody(body).empty());
EXPECT_EQ("M1::L", body);
});

testFunc(root);

// Test UpdateGraphs
EXPECT_TRUE(root.UpdateGraphs().empty());
testFunc(root);

// Test cloning
sdf::Root root2;
root2.Clone(root);
testFunc(root);
testFunc(root2);
}

/////////////////////////////////////////////////
Expand Down

0 comments on commit 9c16a72

Please sign in to comment.