[TPE] Return first link as canonical link

[iche033]

The SDF spec calls for the first link to be the canonical link if the attribute //model/@canonical_link is unspecified. However, TPE stores entities in an std::map which does not preserve order in which the entities are added.

pull request #86 attempted to workaround this by marking the first link added as the canonical link but the logic does not work when the model has multiple nested models and no top level links.

see this comment for more info: #86 (comment)

[claireyywang]

I'm thinking of two ways of tackling this issue:

1. Adding a std::vector to keep track of the insertion order of all link ids for each model
2. Add SetCanonicalLink() to Model which tracks the first inserted link

Not sure about the specifics yet, but I'm leaning towards the second one. Thoughts?

[iche033]

I think either option works. I'm leaning towards 2. too as that seems to be easier to implement without having to modify other functions that touch the std::map variable.

Note that currently we only support one canonical link for a model (which can consist of nested models). You'll likely have to revert this change that I did that assumes the first link added is the canonical link: https://github.com/ignitionrobotics/ign-physics/blob/ign-physics2/tpe/lib/src/Model.cc#L66, since it does not work for nested models without top level links.

[claireyywang]

Closing since PRs are merged