Skip to content

Commit

Permalink
Set sdf::Root to contain only one model/actor/light
Browse files Browse the repository at this point in the history
Signed-off-by: Stephen Brawner <[email protected]>
  • Loading branch information
brawner committed Dec 28, 2020
1 parent e0e09f9 commit d42d870
Show file tree
Hide file tree
Showing 19 changed files with 227 additions and 296 deletions.
42 changes: 3 additions & 39 deletions include/sdf/Root.hh
Original file line number Diff line number Diff line change
Expand Up @@ -127,51 +127,15 @@ namespace sdf
/// \brief Get the number of models that are immediate (not nested) children
/// of this Root object.
/// \return Number of models contained in this Root object.
public: uint64_t ModelCount() const;

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

/// \brief Get whether a model name exists.
/// \param[in] _name Name of the model to check.
/// \return True if there exists a model with the given name.
public: bool ModelNameExists(const std::string &_name) const;
public: const sdf::Model *Model() const;

/// \brief Get the number of lights.
/// \return Number of lights contained in this Root object.
public: uint64_t LightCount() const;

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

/// \brief Get whether a light name exists.
/// \param[in] _name Name of the light to check.
/// \return True if there exists a light with the given name.
public: bool LightNameExists(const std::string &_name) const;
public: const sdf::Light *Light() const;

/// \brief Get the number of actors.
/// \return Number of actors contained in this Root object.
public: uint64_t ActorCount() const;

/// \brief Get an actor based on an index.
/// \param[in] _index Index of the actor. The actor should be in the
/// range [0..ActorCount()).
/// \return Pointer to the actor. Nullptr if the index does not exist.
/// \sa uint64_t ActorCount() const
public: const Actor *ActorByIndex(const uint64_t _index) const;

/// \brief Get whether an actor name exists.
/// \param[in] _name Name of the actor to check.
/// \return True if there exists an actor with the given name.
public: bool ActorNameExists(const std::string &_name) const;
public: const sdf::Actor *Actor() const;

/// \brief Get a pointer to the SDF element that was generated during
/// load.
Expand Down
8 changes: 4 additions & 4 deletions src/FrameSemantics_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_Model)
EXPECT_TRUE(errors.empty()) << errors;

// Get the first model
const sdf::Model *model = root.ModelByIndex(0);
const sdf::Model *model = root.Model();

auto ownedGraph = std::make_shared<sdf::FrameAttachedToGraph>();
sdf::ScopedGraph<sdf::FrameAttachedToGraph> graph(ownedGraph);
Expand Down Expand Up @@ -217,7 +217,7 @@ TEST(FrameSemantics, buildPoseRelativeToGraph)
EXPECT_TRUE(root.Load(testFile).empty());

// Get the first model
const sdf::Model *model = root.ModelByIndex(0);
const sdf::Model *model = root.Model();

auto ownedGraph = std::make_shared<sdf::PoseRelativeToGraph>();
sdf::ScopedGraph<sdf::PoseRelativeToGraph> graph(ownedGraph);
Expand Down Expand Up @@ -313,7 +313,7 @@ TEST(NestedFrameSemantics, buildFrameAttachedToGraph_Model)
EXPECT_TRUE(errors.empty()) << errors;

// Get the first model
const sdf::Model *model = root.ModelByIndex(0);
const sdf::Model *model = root.Model();

auto ownedGraph = std::make_shared<sdf::FrameAttachedToGraph>();
sdf::ScopedGraph<sdf::FrameAttachedToGraph> graph(ownedGraph);
Expand Down Expand Up @@ -592,7 +592,7 @@ TEST(NestedFrameSemantics, ModelWithoutLinksWithNestedStaticModel)
auto errors = root.Load(testFile);
EXPECT_TRUE(errors.empty()) << errors;

const sdf::Model *model = root.ModelByIndex(0);
const sdf::Model *model = root.Model();
ASSERT_NE(nullptr, model);

auto ownedModelGraph = std::make_shared<sdf::FrameAttachedToGraph>();
Expand Down
168 changes: 68 additions & 100 deletions src/Root.cc
Original file line number Diff line number Diff line change
Expand Up @@ -42,29 +42,27 @@ class sdf::RootPrivate
public: std::vector<World> worlds;

/// \brief The models specified under the root SDF element
public: std::vector<Model> models;
public: std::unique_ptr<Model> model;

/// \brief The lights specified under the root SDF element
public: std::vector<Light> lights;
public: std::unique_ptr<Light> light;

/// \brief The actors specified under the root SDF element
public: std::vector<Actor> actors;
public: std::unique_ptr<Actor> actor;

/// \brief Frame Attached-To Graphs constructed when loading Worlds.
public: std::vector<sdf::ScopedGraph<FrameAttachedToGraph>>
worldFrameAttachedToGraphs;

/// \brief Frame Attached-To Graphs constructed when loading Models.
public: std::vector<sdf::ScopedGraph<FrameAttachedToGraph>>
modelFrameAttachedToGraphs;
/// \brief Frame Attached-To Graph constructed when loading Models.
public: sdf::ScopedGraph<FrameAttachedToGraph> modelFrameAttachedToGraph;

/// \brief Pose Relative-To Graphs constructed when loading Worlds.
public: std::vector<sdf::ScopedGraph<PoseRelativeToGraph>>
worldPoseRelativeToGraphs;

/// \brief Pose Relative-To Graphs constructed when loading Models.
public: std::vector<sdf::ScopedGraph<PoseRelativeToGraph>>
modelPoseRelativeToGraphs;
/// \brief Pose Relative-To Graph constructed when loading Models.
public: sdf::ScopedGraph<PoseRelativeToGraph> modelPoseRelativeToGraph;

/// \brief The SDF element pointer generated during load.
public: sdf::ElementPtr sdf;
Expand Down Expand Up @@ -253,33 +251,66 @@ Errors Root::Load(SDFPtr _sdf)
}
}

// Load all the models.
Errors modelLoadErrors = loadUniqueRepeated<Model>(
this->dataPtr->sdf, "model", this->dataPtr->models);
errors.insert(errors.end(), modelLoadErrors.begin(), modelLoadErrors.end());

// Build the graphs.
for (sdf::Model &model : this->dataPtr->models)
if (this->dataPtr->sdf->HasElement("model"))
{
auto frameAttachedToGraph = addFrameAttachedToGraph(
this->dataPtr->modelFrameAttachedToGraphs, model, errors);

model.SetFrameAttachedToGraph(frameAttachedToGraph);

auto poseRelativeToGraph = addPoseRelativeToGraph(
this->dataPtr->modelPoseRelativeToGraphs, model, errors);
model.SetPoseRelativeToGraph(poseRelativeToGraph);
sdf::ElementPtr model = this->dataPtr->sdf->GetElement("model");
this->dataPtr->model = std::make_unique<sdf::Model>();
sdf::Errors loadErrors = this->dataPtr->model->Load(model);
errors.insert(errors.end(), loadErrors.begin(), loadErrors.end());

this->dataPtr->modelFrameAttachedToGraph =
sdf::ScopedGraph(std::make_shared<FrameAttachedToGraph>());

sdf::Errors buildErrors =
sdf::buildFrameAttachedToGraph(
this->dataPtr->modelFrameAttachedToGraph,
this->dataPtr->model.get());
errors.insert(errors.end(), buildErrors.begin(), buildErrors.end());

sdf::Errors validateErrors =
sdf::validateFrameAttachedToGraph(
this->dataPtr->modelFrameAttachedToGraph);
errors.insert(errors.end(), validateErrors.begin(), validateErrors.end());

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

this->dataPtr->modelPoseRelativeToGraph =
sdf::ScopedGraph(std::make_shared<PoseRelativeToGraph>());

Errors buildPoseErrors =
buildPoseRelativeToGraph(
this->dataPtr->modelPoseRelativeToGraph,
this->dataPtr->model.get());
errors.insert(
errors.end(), buildPoseErrors.begin(), buildPoseErrors.end());

Errors validatePoseErrors =
validatePoseRelativeToGraph(
this->dataPtr->modelPoseRelativeToGraph);
errors.insert(
errors.end(), validatePoseErrors.begin(), validatePoseErrors.end());

this->dataPtr->model->SetPoseRelativeToGraph(
this->dataPtr->modelPoseRelativeToGraph);
}

// Load all the lights.
Errors lightLoadErrors = loadUniqueRepeated<Light>(this->dataPtr->sdf,
"light", this->dataPtr->lights);
errors.insert(errors.end(), lightLoadErrors.begin(), lightLoadErrors.end());
if (this->dataPtr->sdf->HasElement("light"))
{
sdf::ElementPtr lightPtr = this->dataPtr->sdf->GetElement("light");
this->dataPtr->light = std::make_unique<sdf::Light>();
sdf::Errors buildErrors = this->dataPtr->light->Load(lightPtr);
errors.insert(errors.end(), buildErrors.begin(), buildErrors.end());
}

// Load all the actors.
Errors actorLoadErrors = loadUniqueRepeated<Actor>(this->dataPtr->sdf,
"actor", this->dataPtr->actors);
errors.insert(errors.end(), actorLoadErrors.begin(), actorLoadErrors.end());
if (this->dataPtr->sdf->HasElement("actor"))
{
sdf::ElementPtr actorPtr = this->dataPtr->sdf->GetElement("actor");
this->dataPtr->actor = std::make_unique<sdf::Actor>();
sdf::Errors buildErrors = this->dataPtr->actor->Load(actorPtr);
errors.insert(errors.end(), buildErrors.begin(), buildErrors.end());
}

return errors;
}
Expand Down Expand Up @@ -324,84 +355,21 @@ bool Root::WorldNameExists(const std::string &_name) const
}

/////////////////////////////////////////////////
uint64_t Root::ModelCount() const
{
return this->dataPtr->models.size();
}

/////////////////////////////////////////////////
const Model *Root::ModelByIndex(const uint64_t _index) const
{
if (_index < this->dataPtr->models.size())
return &this->dataPtr->models[_index];
return nullptr;
}

/////////////////////////////////////////////////
bool Root::ModelNameExists(const std::string &_name) const
{
for (auto const &m : this->dataPtr->models)
{
if (m.Name() == _name)
{
return true;
}
}
return false;
}

/////////////////////////////////////////////////
uint64_t Root::LightCount() const
const Model *Root::Model() const
{
return this->dataPtr->lights.size();
return this->dataPtr->model.get();
}

/////////////////////////////////////////////////
const Light *Root::LightByIndex(const uint64_t _index) const
const Light *Root::Light() const
{
if (_index < this->dataPtr->lights.size())
return &this->dataPtr->lights[_index];
return nullptr;
}

/////////////////////////////////////////////////
bool Root::LightNameExists(const std::string &_name) const
{
for (auto const &m : this->dataPtr->lights)
{
if (m.Name() == _name)
{
return true;
}
}
return false;
return this->dataPtr->light.get();
}

/////////////////////////////////////////////////
uint64_t Root::ActorCount() const
const Actor *Root::Actor() const
{
return this->dataPtr->actors.size();
}

/////////////////////////////////////////////////
const Actor *Root::ActorByIndex(const uint64_t _index) const
{
if (_index < this->dataPtr->actors.size())
return &this->dataPtr->actors[_index];
return nullptr;
}

/////////////////////////////////////////////////
bool Root::ActorNameExists(const std::string &_name) const
{
for (auto const &m : this->dataPtr->actors)
{
if (m.Name() == _name)
{
return true;
}
}
return false;
return this->dataPtr->actor.get();
}

/////////////////////////////////////////////////
Expand Down
Loading

0 comments on commit d42d870

Please sign in to comment.