Skip to content

Commit

Permalink
Support implicit nested canonical links (gazebosim#341)
Browse files Browse the repository at this point in the history
Allow models without links if they have nested models instead.
When building FrameAttachedToGraph, if model has no links
choose the first link of the first nested model as canonical
link instead.

A new private function `Model::CanonicalLinkAndRelativeName`
is added that provides a `Link*` pointer to the canonical link and its
nested name relative to the current model, which is needed
in the FrameAttachedToGraph. This private prevents
duplicate code in `FrameSemantics.cc` and `Model::CanonicalLink`.
The method is private to hide :: syntax from libsdformat9,
at least until there is a compelling reason to make the API
public.

A helper function is added to FrameSemantics.cc as a friend
of Model so that buildFrameAttachedToGraph can call the
private API. That function can't be added directly
as a friend since it uses a `FrameAttachedToGraph&`
as an argument, which isn't defined in Model.hh.

Signed-off-by: Steve Peters <[email protected]>
  • Loading branch information
scpeters committed Sep 2, 2020
1 parent 021eb8b commit 9dcb45a
Show file tree
Hide file tree
Showing 10 changed files with 231 additions and 24 deletions.
1 change: 1 addition & 0 deletions Changelog.md
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@

1. Support nested models in DOM and frame semantics.
* [Pull request 316](https://github.com/osrf/sdformat/pull/316)
+ [Pull request 341](https://github.com/osrf/sdformat/pull/341)

1. Find python3 in cmake, fix cmake warning.
* [Pull request 328](https://github.com/osrf/sdformat/pull/328)
Expand Down
19 changes: 15 additions & 4 deletions Migration.md
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,21 @@ but with improved human-readability..
+ std::optional<std::string> GetMaxValueAsString() const;
+ bool ValidateValue() const;

## SDFormat 9.0 to 9.3

### Additions

1. **sdf/Model.hh**
+ uint64\_t ModelCount() const
+ const Model \*ModelByIndex(const uint64\_t) const
+ const Model \*ModelByName(const std::string &) const
+ bool ModelNameExists(const std::string &) const

### Modifications

1. Permit models without links if they contain a nested canonical link.
+ [pull request 341](https://github.com/osrf/sdformat/pull/341)

## SDFormat 8.x to 9.0

### Additions
Expand Down Expand Up @@ -92,10 +107,6 @@ but with improved human-readability..
+ const Frame \*FrameByIndex(const uint64\_t) const
+ const Frame \*FrameByName(const std::string &) const
+ bool FrameNameExists(const std::string &) const
+ uint64\_t ModelCount() const
+ const Model \*ModelByIndex(const uint64\_t) const
+ const Model \*ModelByName(const std::string &) const
+ bool ModelNameExists(const std::string &) const
+ sdf::SemanticPose SemanticPose() const

1. **sdf/SDFImpl.hh**
Expand Down
19 changes: 17 additions & 2 deletions include/sdf/Model.hh
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@

#include <memory>
#include <string>
#include <utility>
#include <ignition/math/Pose3.hh>
#include "sdf/Element.hh"
#include "sdf/SemanticPose.hh"
Expand Down Expand Up @@ -238,12 +239,14 @@ namespace sdf
public: const Link *CanonicalLink() const;

/// \brief Get the name of the model's canonical link. An empty value
/// indicates that the first link in the model is the canonical link.
/// indicates that the first link in the model or the first link found
/// in a depth first search of nested models is the canonical link.
/// \return The name of the canonical link.
public: const std::string &CanonicalLinkName() const;

/// \brief Set the name of the model's canonical link. An empty value
/// indicates that the first link in the model is the canonical link.
/// indicates that the first link in the model or the first link found
/// in a depth first search of nested models is the canonical link.
/// \param[in] _canonicalLink The name of the canonical link.
public: void SetCanonicalLinkName(const std::string &_canonicalLink);

Expand Down Expand Up @@ -287,9 +290,21 @@ namespace sdf
private: sdf::Errors SetPoseRelativeToGraph(
std::weak_ptr<const PoseRelativeToGraph> _graph);

/// \brief Get the model's canonical link and the nested name of the link
/// relative to the current model, delimited by "::".
/// \return An immutable pointer to the canonical link and the nested
/// name of the link relative to the current model.
private: std::pair<const Link *, std::string> CanonicalLinkAndRelativeName()
const;

/// \brief Allow World::Load to call SetPoseRelativeToGraph.
friend class World;

/// \brief Allow helper function in FrameSemantics.cc to call
/// CanonicalLinkAndRelativeName.
friend std::pair<const Link *, std::string>
modelCanonicalLinkAndRelativeName(const Model *);

/// \brief Private data pointer.
private: ModelPrivate *dataPtr = nullptr;
};
Expand Down
56 changes: 42 additions & 14 deletions src/FrameSemantics.cc
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
*
*/
#include <string>
#include <utility>
#include <vector>

#include "sdf/Element.hh"
#include "sdf/Error.hh"
Expand Down Expand Up @@ -164,6 +166,17 @@ FindSinkVertex(
return PairType(vertex, edges);
}

/////////////////////////////////////////////////
std::pair<const Link *, std::string>
modelCanonicalLinkAndRelativeName(const Model *_model)
{
if (nullptr == _model)
{
return std::make_pair(nullptr, "");
}
return _model->CanonicalLinkAndRelativeName();
}

/////////////////////////////////////////////////
Errors buildFrameAttachedToGraph(
FrameAttachedToGraph &_out, const Model *_model)
Expand All @@ -190,32 +203,47 @@ Errors buildFrameAttachedToGraph(
"Invalid model element in sdf::Model."});
return errors;
}
else if (_model->LinkCount() < 1)
else if (_model->LinkCount() < 1 && _model->ModelCount() < 1)
{
errors.push_back({ErrorCode::MODEL_WITHOUT_LINK,
"A model must have at least one link."});
return errors;
}

// identify canonical link
const sdf::Link *canonicalLink = nullptr;
if (_model->CanonicalLinkName().empty())
{
canonicalLink = _model->LinkByIndex(0);
}
else
{
canonicalLink = _model->LinkByName(_model->CanonicalLinkName());
}
// identify canonical link, which may be nested
auto canonicalLinkAndName = modelCanonicalLinkAndRelativeName(_model);
const sdf::Link *canonicalLink = canonicalLinkAndName.first;
const std::string canonicalLinkName = canonicalLinkAndName.second;
if (nullptr == canonicalLink)
{
if (canonicalLinkName.empty())
{
errors.push_back({ErrorCode::MODEL_WITHOUT_LINK,
"A model must have at least one link."});
}
else
{
errors.push_back({ErrorCode::MODEL_CANONICAL_LINK_INVALID,
"canonical_link with name[" + canonicalLinkName +
"] not found in model with name[" + _model->Name() + "]."});
}
// return early
errors.push_back({ErrorCode::MODEL_CANONICAL_LINK_INVALID,
"canonical_link with name[" + _model->CanonicalLinkName() +
"] not found in model with name[" + _model->Name() + "]."});
return errors;
}

// Identify if the canonical link is in a nested model.
if (_model->LinkByName(canonicalLink->Name()) != canonicalLink)
{
// The canonical link is nested, so its vertex should be added
// here with an edge from __model__.
// The nested canonical link name should be a nested name
// relative to _model, delimited by "::".
auto linkId =
_out.graph.AddVertex(canonicalLinkName, sdf::FrameType::LINK).Id();
_out.map[canonicalLinkName] = linkId;
_out.graph.AddEdge({modelFrameId, linkId}, true);
}

// add link vertices
for (uint64_t l = 0; l < _model->LinkCount(); ++l)
{
Expand Down
38 changes: 34 additions & 4 deletions src/Model.cc
Original file line number Diff line number Diff line change
Expand Up @@ -276,10 +276,11 @@ Errors Model::Load(ElementPtr _sdf)
frameNames.insert(linkName);
}

// If the model is not static:
// If the model is not static and has no nested models:
// Require at least one link so the implicit model frame can be attached to
// something.
if (!this->Static() && this->dataPtr->links.empty())
if (!this->Static() && this->dataPtr->links.empty() &&
this->dataPtr->models.empty())
{
errors.push_back({ErrorCode::MODEL_WITHOUT_LINK,
"A model must have at least one link."});
Expand Down Expand Up @@ -710,14 +711,43 @@ const Model *Model::ModelByName(const std::string &_name) const

/////////////////////////////////////////////////
const Link *Model::CanonicalLink() const
{
return this->CanonicalLinkAndRelativeName().first;
}

/////////////////////////////////////////////////
std::pair<const Link*, std::string> Model::CanonicalLinkAndRelativeName() const
{
if (this->CanonicalLinkName().empty())
{
return this->LinkByIndex(0);
if (this->LinkCount() > 0)
{
auto firstLink = this->LinkByIndex(0);
return std::make_pair(firstLink, firstLink->Name());
}
else if (this->ModelCount() > 0)
{
// Recursively choose the canonical link of the first nested model
// (depth first search).
auto firstModel = this->ModelByIndex(0);
auto canonicalLinkAndName = firstModel->CanonicalLinkAndRelativeName();
// Prepend firstModelName if a valid link is found.
if (nullptr != canonicalLinkAndName.first)
{
canonicalLinkAndName.second =
firstModel->Name() + "::" + canonicalLinkAndName.second;
}
return canonicalLinkAndName;
}
else
{
return std::make_pair(nullptr, "");
}
}
else
{
return this->LinkByName(this->CanonicalLinkName());
return std::make_pair(this->LinkByName(this->CanonicalLinkName()),
this->CanonicalLinkName());
}
}

Expand Down
26 changes: 26 additions & 0 deletions src/ign_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -358,6 +358,32 @@ TEST(check, SDF)
EXPECT_EQ("Valid.\n", output) << output;
}

// Check an SDF file with a model that has a nested canonical link.
{
std::string path = pathBase +"/nested_canonical_link.sdf";

// Check nested_canonical_link.sdf
std::string output =
custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
EXPECT_EQ("Valid.\n", output) << output;
}

// Check an SDF file with a model that has a nested canonical link
// that is explicitly specified by //model/@canonical_link using ::
// syntax.
{
std::string path = pathBase +"/nested_invalid_explicit_canonical_link.sdf";

// Check nested_invalid_explicit_canonical_link.sdf
std::string output =
custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
EXPECT_NE(output.find("Error: canonical_link with name[nested::link] not "
"found in model with name[top]."),
std::string::npos) << output;
EXPECT_NE(output.find("Error: A model must have at least one link."),
std::string::npos) << output;
}

// Check an invalid SDF file that uses reserved names.
{
std::string path = pathBase +"/model_invalid_reserved_names.sdf";
Expand Down
62 changes: 62 additions & 0 deletions test/integration/model_dom.cc
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include "sdf/Element.hh"
#include "sdf/Error.hh"
#include "sdf/Filesystem.hh"
#include "sdf/Frame.hh"
#include "sdf/Link.hh"
#include "sdf/Model.hh"
#include "sdf/Root.hh"
Expand Down Expand Up @@ -313,5 +314,66 @@ TEST(DOMRoot, LoadCanonicalLink)

EXPECT_EQ(0u, model->JointCount());
EXPECT_EQ(nullptr, model->JointByIndex(0));

EXPECT_EQ(1u, model->FrameCount());
EXPECT_NE(nullptr, model->FrameByIndex(0));
EXPECT_EQ(nullptr, model->FrameByIndex(1));

std::string body;
EXPECT_TRUE(model->FrameByName("F")->ResolveAttachedToBody(body).empty());
EXPECT_EQ("link2", body);
}

/////////////////////////////////////////////////
TEST(DOMRoot, LoadNestedCanonicalLink)
{
const std::string testFile =
sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf",
"nested_canonical_link.sdf");

// Load the SDF file
sdf::Root root;
EXPECT_TRUE(root.Load(testFile).empty());

// Get the first model
const sdf::Model *model = root.ModelByIndex(0);
ASSERT_NE(nullptr, model);
EXPECT_EQ("top", model->Name());
EXPECT_EQ(0u, model->LinkCount());
EXPECT_EQ(nullptr, model->LinkByIndex(0));

EXPECT_EQ(0u, model->JointCount());
EXPECT_EQ(nullptr, model->JointByIndex(0));

EXPECT_EQ(1u, model->FrameCount());
EXPECT_NE(nullptr, model->FrameByIndex(0));
EXPECT_EQ(nullptr, model->FrameByIndex(1));

EXPECT_EQ(2u, model->ModelCount());
EXPECT_TRUE(model->ModelNameExists("nested"));
EXPECT_TRUE(model->ModelNameExists("shallow"));
EXPECT_EQ(model->ModelByName("nested"), model->ModelByIndex(0));
EXPECT_EQ(model->ModelByName("shallow"), model->ModelByIndex(1));
EXPECT_EQ(nullptr, model->ModelByIndex(2));

// expect implicit canonical link
EXPECT_TRUE(model->CanonicalLinkName().empty());

// frame F is attached to __model__ and resolves to canonical link,
// which is "nested::link2"
std::string body;
EXPECT_TRUE(model->FrameByName("F")->ResolveAttachedToBody(body).empty());
EXPECT_EQ("nested::link2", body);

EXPECT_EQ(model->ModelByName("nested")->LinkByName("link2"),
model->CanonicalLink());
// this reports the local name, not the nested name "nested::link2"
EXPECT_EQ("link2", model->CanonicalLink()->Name());

const sdf::Model *shallowModel = model->ModelByName("shallow");
EXPECT_EQ(1u, shallowModel->FrameCount());
EXPECT_TRUE(
shallowModel->FrameByName("F")->ResolveAttachedToBody(body).empty());
EXPECT_EQ("deep::deeper::deepest::deepest_link", body);
}

1 change: 1 addition & 0 deletions test/sdf/model_canonical_link.sdf
Original file line number Diff line number Diff line change
Expand Up @@ -7,5 +7,6 @@
<link name="link2">
<pose>0 2 0 0 0 0</pose>
</link>
<frame name="F" attached_to="__model__"/>
</model>
</sdf>
19 changes: 19 additions & 0 deletions test/sdf/nested_canonical_link.sdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
<sdf version='1.7'>
<model name="top">
<frame name="F"/>
<model name="nested" canonical_link="link2">
<link name="link1"/>
<link name="link2"/>
</model>
<model name="shallow">
<frame name="F"/>
<model name="deep">
<model name="deeper">
<model name="deepest">
<link name="deepest_link"/>
</model>
</model>
</model>
</model>
</model>
</sdf>
14 changes: 14 additions & 0 deletions test/sdf/nested_invalid_explicit_canonical_link.sdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
<sdf version='1.7'>
<model name="top" canonical_link="nested::link">
<model name="nested">
<link name="link"/>
</model>
<model name="nested_without_links1">
<model name="nested_without_links2">
<model name="nested_without_links3">
<frame name="F"/>
</model>
</model>
</model>
</model>
</sdf>

0 comments on commit 9dcb45a

Please sign in to comment.