Skip to content

Commit

Permalink
Added Root mutable accessors, and Root::Clone function (#841)
Browse files Browse the repository at this point in the history 
* Added non-const mutable accessors for root child objects

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Remove comment

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Use casts

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Added Root::Clone function

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Added deprecation note

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Added Root::UpdateGraphs functions

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Clear graphs on ClearWorlds

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Review updates

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Cleanup

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Remove extra check

Signed-off-by: Nate Koenig <nate@openrobotics.org>

Co-authored-by: Nate Koenig <nate@openrobotics.org>
  • Loading branch information
@nkoenig
nkoenig and Nate Koenig committed Feb 16, 2022
1 parent 9272cb6 commit ae3655f
Show file tree
Hide file tree
Showing 5 changed files with 306 additions and 37 deletions.
32 changes: 32 additions & 0 deletions include/sdf/Root.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,13 @@ namespace sdf
/// \sa uint64_t WorldCount() const
public: const World *WorldByIndex(const uint64_t _index) const;

/// \brief Get a mutable world based on an index.
/// \param[in] _index Index of the world. The index should be in the
/// range [0..WorldCount()).
/// \return Pointer to the world. Nullptr if the index does not exist.
/// \sa uint64_t WorldCount() const
public: World *WorldByIndex(const uint64_t _index);

/// \brief Get whether a world name exists.
/// \param[in] _name Name of the world to check.
/// \return True if there exists a world with the given name.
Expand All @@ -151,6 +158,31 @@ namespace sdf
/// not been called.
public: sdf::ElementPtr Element() const;

/// \brief Add a world to the root.
/// \param[in] _word World to add.
/// \return True if successful, false if a world with the name already
/// exists.
/// \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 AddWorld(const World &_world);

/// \brief Remove all worlds.
public: void ClearWorlds();

/// \brief Deep copy this Root object and return the new Root object.
/// \return A clone of this Root object.
/// Deprecate this function in SDF version 13, and use
/// IGN_UTILS_IMPL_PTR instead.
public: sdf::Root Clone() const;

/// \brief Recreate the frame and pose graphs for the worlds and model
/// that are children of this Root object. You can call this function
/// to build new graphs when the DOM was created programmatically, or
/// if you want to regenerate the graphs after editing the DOM.
/// \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
21 changes: 1 addition & 20 deletions src/FrameSemantics.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -960,11 +960,6 @@ Errors buildFrameAttachedToGraph(
{
return Errors{{ErrorCode::ELEMENT_INVALID, "Invalid sdf::Model pointer."}};
}
else if (!_model->Element())
{
return Errors{
{ErrorCode::ELEMENT_INVALID, "Invalid model element in sdf::Model."}};
}
return wrapperBuildFrameAttachedToGraph(_out, ModelWrapper(*_model), _isRoot);
}

Expand Down Expand Up @@ -1012,11 +1007,6 @@ Errors buildFrameAttachedToGraph(
{
return Errors{{ErrorCode::ELEMENT_INVALID, "Invalid sdf::World pointer."}};
}
else if (!_world->Element())
{
return Errors{
{ErrorCode::ELEMENT_INVALID, "Invalid world element in sdf::World."}};
}

return buildFrameAttachedToGraph(_out, WorldWrapper(*_world));
}
Expand Down Expand Up @@ -1133,11 +1123,6 @@ Errors buildPoseRelativeToGraph(
{
return Errors{{ErrorCode::ELEMENT_INVALID, "Invalid sdf::Model pointer."}};
}
else if (!_model->Element())
{
return Errors{
{ErrorCode::ELEMENT_INVALID, "Invalid model element in sdf::Model."}};
}

return wrapperBuildPoseRelativeToGraph(_out, ModelWrapper(*_model), _isRoot);
}
Expand Down Expand Up @@ -1197,11 +1182,7 @@ Errors buildPoseRelativeToGraph(
{
return Errors{{ErrorCode::ELEMENT_INVALID, "Invalid sdf::World pointer."}};
}
else if (!_world->Element())
{
return Errors{
{ErrorCode::ELEMENT_INVALID, "Invalid world element in sdf::World."}};
}

return wrapperBuildPoseRelativeToGraph(_out, WorldWrapper(*_world));
}

Expand Down
121 changes: 104 additions & 17 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 @@ -381,6 +376,13 @@ const World *Root::WorldByIndex(const uint64_t _index) const
return nullptr;
}

/////////////////////////////////////////////////
World *Root::WorldByIndex(const uint64_t _index)
{
return const_cast<World*>(
static_cast<const Root*>(this)->WorldByIndex(_index));
}

/////////////////////////////////////////////////
bool Root::WorldNameExists(const std::string &_name) const
{
Expand Down Expand Up @@ -417,3 +419,88 @@ sdf::ElementPtr Root::Element() const
{
return this->dataPtr->sdf;
}

/////////////////////////////////////////////////
sdf::Errors Root::AddWorld(const World &_world)
{
if (!this->WorldNameExists(_world.Name()))
{
this->dataPtr->worlds.push_back(_world);
return this->UpdateGraphs();
}

sdf::Errors errors;
errors.push_back({ErrorCode::DUPLICATE_NAME,
"World with name[" + _world.Name() + "] already exists."});

return errors;
}

/////////////////////////////////////////////////
void Root::ClearWorlds()
{
this->dataPtr->worlds.clear();
this->dataPtr->worldFrameAttachedToGraphs.clear();
this->dataPtr->worldPoseRelativeToGraphs.clear();
}

/////////////////////////////////////////////////
sdf::Root Root::Clone() const
{
sdf::Root r;
r.dataPtr->version = this->dataPtr->version;
r.dataPtr->worlds = this->dataPtr->worlds;
r.dataPtr->modelLightOrActor = this->dataPtr->modelLightOrActor;
r.UpdateGraphs();
return r;
}

/////////////////////////////////////////////////
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);
}

// 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);
}

return errors;
}

//////////////////////////////////////////////////
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->modelPoseRelativeToGraph = createPoseRelativeToGraph(_model, _errors);
_model.SetPoseRelativeToGraph(this->modelPoseRelativeToGraph);
}
69 changes: 69 additions & 0 deletions src/Root_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,6 +109,31 @@ TEST(DOMRoot, StringModelSdfParse)
EXPECT_EQ(nullptr, root.Light());
EXPECT_EQ(nullptr, root.Actor());
EXPECT_EQ(0u, root.WorldCount());

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

const sdf::Model *model2 = root2.Model();
ASSERT_NE(nullptr, model2);
EXPECT_NE(nullptr, model2->Element());

EXPECT_EQ("shapes", model2->Name());
EXPECT_EQ(1u, model2->LinkCount());

const sdf::Link *link2 = model2->LinkByIndex(0);
ASSERT_NE(nullptr, link2);
EXPECT_NE(nullptr, link2->Element());
EXPECT_EQ("link", link2->Name());
EXPECT_EQ(1u, link2->CollisionCount());

const sdf::Collision *collision2 = link2->CollisionByIndex(0);
ASSERT_NE(nullptr, collision2);
EXPECT_NE(nullptr, collision2->Element());
EXPECT_EQ("box_col", collision2->Name());

EXPECT_EQ(nullptr, root2.Light());
EXPECT_EQ(nullptr, root2.Actor());
EXPECT_EQ(0u, root2.WorldCount());
}

/////////////////////////////////////////////////
Expand Down Expand Up @@ -270,3 +295,47 @@ TEST(DOMRoot, FrameSemanticsOnMove)
testFrame1(root2);
}
}

/////////////////////////////////////////////////
TEST(DOMRoot, AddWorld)
{
sdf::Root root;
EXPECT_EQ(0u, root.WorldCount());

sdf::World world;
world.SetName("world1");
sdf::Errors errors = root.AddWorld(world);
EXPECT_TRUE(errors.empty());
EXPECT_EQ(1u, root.WorldCount());
ASSERT_FALSE(root.AddWorld(world).empty());
EXPECT_EQ(sdf::ErrorCode::DUPLICATE_NAME, root.AddWorld(world)[0].Code());
EXPECT_EQ(1u, root.WorldCount());

root.ClearWorlds();
EXPECT_EQ(0u, root.WorldCount());

EXPECT_TRUE(root.AddWorld(world).empty());
EXPECT_EQ(1u, root.WorldCount());
const sdf::World *worldFromRoot = root.WorldByIndex(0);
ASSERT_NE(nullptr, worldFromRoot);
EXPECT_EQ(worldFromRoot->Name(), world.Name());
}

/////////////////////////////////////////////////
TEST(DOMRoot, MutableByIndex)
{
sdf::Root root;
EXPECT_EQ(0u, root.WorldCount());

sdf::World world;
world.SetName("world1");
EXPECT_TRUE(root.AddWorld(world).empty());
EXPECT_EQ(1u, root.WorldCount());

// Modify the world
sdf::World *w = root.WorldByIndex(0);
ASSERT_NE(nullptr, w);
EXPECT_EQ("world1", w->Name());
w->SetName("world2");
EXPECT_EQ("world2", root.WorldByIndex(0)->Name());
}
Loading

0 comments on commit ae3655f

Please sign in to comment.