Refactor FrameSemantics.cc

[azeey]

🎉 New feature

Summary

The buildFrameAttachedToGraph and buildPoseRelativeToGraph have overloads for the type of parent element, but the code in each overload is very similar to each other. This PR refactors these functions so there's less code duplication.

Note: There is one small change in behavior due to the order in which default edges are added. Before this PR, the default edges were added while adding vertices while in this PR all vertices are added first and then all edges are added. The resulting change is only manifested in the graph output (ign sdf -g) which is only there for development/debugging purposes.

Toward #658

Test it

Checklist

• Signed all commits for DCO
• Added tests
• [ ] Added example and/or tutorial
• Updated documentation (as needed)
• [ ] Updated migration guide (as needed)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge

[codecov-commenter]

Codecov Report

Merging #764 (1c1b4c4) into sdf12 (ef48530) will increase coverage by 0.96%.
The diff coverage is 93.48%.

@@            Coverage Diff             @@
##            sdf12     #764      +/-   ##
==========================================
+ Coverage   89.60%   90.57%   +0.96%     
==========================================
  Files          78       78              
  Lines       12691    12336     -355     
==========================================
- Hits        11372    11173     -199     
+ Misses       1319     1163     -156     

Impacted Files	Coverage Δ
src/FrameSemantics.cc	83.06% <93.48%> (+9.16%)	⬆️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update ef48530...1c1b4c4. Read the comment docs.

[scpeters]

just started looking at this; here's what I've noticed so far

[scpeters]

ok, I think I've reviewed all except addIncludedInterfaceModelEdgesToPoseRelativeToGraph

I'll resume later

[EricCousineau-TRI]

Per VC, consider having an (internal / private) base class. e.g. FrameBase -> {Frame, InterfaceFrame} (is-a); however, may break ABI.
Alternative, use has-a, some facade class.

Perhaps better is to produce InterfaceModel (possibly InterfaceWorld too) as the facade.

[azeey]

Converted to a draft to explore use of Interface elements for all frame building logic. @scpeters , @jennuine please hold off on reviewing until I put this back as ready for review.

[azeey]

The issue with trying to use only Interface elements for building frame graphs is they only provide a subset of functions needed for building the graph. For example, sdf::InterfaceLink currently has sdf::InterfaceLink::PoseInModelFrame which limits the possibility of using other frames as @relative_to for the pose of the link. However, sdf::Link has sdf::Link::RawPose and sdf::Link::PoseRelativeTo. Creating an sdf::InterfaceLink from the information contained in an sdf::Link, in the current state of sdf::InterfaceLink, is impossible because we have to provide sdf::InterfaceLink with its pose relative to its model frame, but if the sdf::Link's pose is relative to another frame, we'd have to resolve the pose, which we can't do until the pose graph is built. We can add sdf::InterfaceLink::RawPose andsdf::InterfaceLink::PoseRelativeTo, and change the constructor to take a relativeTo argument, but that would make sdf::InterfaceLink::PoseInModelFrame invalid as it cannot return the pose relative to the model frame.

So the approach I'm thinking of is to create wrapper structs for all elements and use those wrappers in the build* functions.
For example, a wrapper for sdf::Link and sdf::InterfaceLink would be the following:

struct LinkWrapper
{
  explicit LinkWrapper(const sdf::Link &_link)
      : name(_link.Name()),
        rawPose(_link.RawPose()),
        poseRelativeTo(_link.PoseRelativeTo()),
        elementType("Link"),
        frameType(FrameType::LINK)

  {
  }

  explicit LinkWrapper(const sdf::InterfaceLink &_ifaceLink)
      : name(_ifaceLink.Name()),
        rawPose(_ifaceLink.PoseInModelFrame()),
        poseRelativeTo("__model__"),
        elementType("Interface Link"),
        frameType(FrameType::LINK)
  {
  }

  std::string name;
  ignition::math::Pose3d rawPose;
  std::string poseRelativeTo;
  std::string elementType;
  FrameType frameType;
};

The build* functions can then use the members name, rawPose, etc. I'm opting for using a member variables directly instead of getters to limit verbosity. These wrappers would all be in FrameSemantics.cc so we won't have any issue of API/ABI stability.

[azeey]

This is now ready for review.

[jennuine]

LGTM, maybe @scpeters can take a second look?

[scpeters]

LGTM, maybe @scpeters can take a second look?

I'm looking right now

[scpeters]

just a few minor comments. It looks great, nice work removing all this duplicate code!

[scpeters]

I think this error block has been dropped during this refactor

[azeey]

I added it back in 8572ccb, but I'm not sure if we need it anymore, especially now that we are adding more APIs to programmatically create DOM objects. The Element() member function of those objects will always return a nullptr, but I think we should still be able to build frame graphs for them. Perhaps we can address that in a follow up PR.

[osrf-triage]

This pull request has been mentioned on Gazebo Community. There might be relevant details there:

https://community.gazebosim.org/t/new-ignition-releases-2022-04-13-fortress-edifice/1367/1