Add tpelib::Shape::GetSize() and tpelib::Model::GetCanonicalLink() (#45)

* implement missing Shape::GetSize(), add Model::GetCanonicalLink()
* add doxygen doc and GetSize() test case

Signed-off-by: claireyywang <clairewang@openrobotics.org>

tpe/lib/src/Model.cc

@@ -46,6 +46,14 @@ Entity &Model::AddLink()
   return *it->second.get();
 }
 
+//////////////////////////////////////////////////
+Entity &Model::GetCanonicalLink()
+{
+  // return first link as canonical link
+  return *this->GetChildren().begin()->second;
+}
+
+
 //////////////////////////////////////////////////
 void Model::SetLinearVelocity(const math::Vector3d _velocity)
 {

tpe/lib/src/Model.hh

@@ -47,6 +47,10 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity
   /// \return Newly created Link
   public: Entity &AddLink();
 
+  /// \brief Get the canonical link of model
+  /// \return Entity the canonical (first) link
+  public: Entity &GetCanonicalLink();
+
   /// \brief Set the linear velocity of model
   /// \param[in] _velocity linear velocity
   public: void SetLinearVelocity(const math::Vector3d _velocity);

tpe/lib/src/Shape.cc

@@ -84,6 +84,12 @@ void BoxShape::SetSize(const math::Vector3d &_size)
   this->dirty = true;
 }
 
+//////////////////////////////////////////////////
+math::Vector3d BoxShape::GetSize()
+{
+  return this->size;
+}
+
 //////////////////////////////////////////////////
 void BoxShape::UpdateBoundingBox()
 {

tpe/lib/src/Shape_TEST.cc

@@ -37,6 +37,7 @@ TEST(Shape, BoxShape)
 
   math::Vector3d size(1.2, 3.6, 5.8);
   shape.SetSize(size);
+  EXPECT_EQ(size, shape.GetSize());
   math::AxisAlignedBox bbox = shape.GetBoundingBox();
   EXPECT_EQ(math::Vector3d::Zero, bbox.Center());
   EXPECT_EQ(size, bbox.Size());