Added non-const accessors for Model child objects (#839)

* Added non-const accessors for child objects

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Remove helper functions

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Added frame mutators

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Updated tests

Signed-off-by: Nate Koenig <nate@openrobotics.org>

Co-authored-by: Nate Koenig <nate@openrobotics.org>

include/sdf/Model.hh

@@ -148,13 +148,28 @@ namespace sdf
     /// \sa uint64_t LinkCount() const
     public: const Link *LinkByIndex(const uint64_t _index) const;
 
+    /// \brief Get an immediate (not nested) mutable child link based on an
+    /// index.
+    /// \param[in] _index Index of the link. The index should be in the
+    /// range [0..LinkCount()).
+    /// \return Pointer to the link. Nullptr if the index does not exist.
+    /// \sa uint64_t LinkCount() const
+    public: Link *LinkByIndex(uint64_t _index);
+
     /// \brief Get a link based on a name.
     /// \param[in] _name Name of the link.
     /// To get a link in a nested model, prefix the link name with the
     /// sequence of nested models containing this link, delimited by "::".
     /// \return Pointer to the link. Nullptr if the name does not exist.
     public: const Link *LinkByName(const std::string &_name) const;
 
+    /// \brief Get a mutable link based on a name.
+    /// \param[in] _name Name of the link.
+    /// To get a link in a nested model, prefix the link name with the
+    /// sequence of nested models containing this link, delimited by "::".
+    /// \return Pointer to the link. Nullptr if the name does not exist.
+    public: Link *LinkByName(const std::string &_name);
+
     /// \brief Get whether a link name exists.
     /// \param[in] _name Name of the link to check.
     /// To check for a link in a nested model, prefix the link name with
@@ -176,6 +191,14 @@ namespace sdf
     /// \sa uint64_t JointCount() const
     public: const Joint *JointByIndex(const uint64_t _index) const;
 
+    /// \brief Get an immediate (not nested) mutable child joint based on an
+    /// index.
+    /// \param[in] _index Index of the joint. The index should be in the
+    /// range [0..JointCount()).
+    /// \return Pointer to the joint. Nullptr if the index does not exist.
+    /// \sa uint64_t JointCount() const
+    public: Joint *JointByIndex(uint64_t _index);
+
     /// \brief Get whether a joint name exists.
     /// \param[in] _name Name of the joint to check.
     /// To check for a joint in a nested model, prefix the joint name with
@@ -192,6 +215,15 @@ namespace sdf
     /// \sa bool JointNameExists(const std::string &_name) const
     public: const Joint *JointByName(const std::string &_name) const;
 
+    /// \brief Get a mubtable joint based on a name.
+    /// \param[in] _name Name of the joint.
+    /// To get a joint in a nested model, prefix the joint name with the
+    /// sequence of nested models containing this joint, delimited by "::".
+    /// \return Pointer to the joint. Nullptr if a joint with the given name
+    ///  does not exist.
+    /// \sa bool JointNameExists(const std::string &_name) const
+    public: Joint *JointByName(const std::string &_name);
+
     /// \brief Get the number of explicit frames that are immediate (not nested)
     /// children of this Model object.
     /// \remark FrameByName() can find explicit frames that are not immediate
@@ -208,6 +240,15 @@ namespace sdf
     /// \sa uint64_t FrameCount() const
     public: const Frame *FrameByIndex(const uint64_t _index) const;
 
+    /// \brief Get a mutable immediate (not nested) child explicit frame based
+    /// on an index.
+    /// \param[in] _index Index of the explicit frame. The index should be in
+    /// the range [0..FrameCount()).
+    /// \return Pointer to the explicit frame. Nullptr if the index does not
+    /// exist.
+    /// \sa uint64_t FrameCount() const
+    public: Frame *FrameByIndex(uint64_t _index);
+
     /// \brief Get an explicit frame based on a name.
     /// \param[in] _name Name of the explicit frame.
     /// To get a frame in a nested model, prefix the frame name with the
@@ -216,6 +257,14 @@ namespace sdf
     /// exist.
     public: const Frame *FrameByName(const std::string &_name) const;
 
+    /// \brief Get a mutable explicit frame based on a name.
+    /// \param[in] _name Name of the explicit frame.
+    /// To get a frame in a nested model, prefix the frame name with the
+    /// sequence of nested models containing this frame, delimited by "::".
+    /// \return Pointer to the explicit frame. Nullptr if the name does not
+    /// exist.
+    public: Frame *FrameByName(const std::string &_name);
+
     /// \brief Get whether an explicit frame name exists.
     /// \param[in] _name Name of the explicit frame to check.
     /// To check for a frame in a nested model, prefix the frame name with
@@ -238,6 +287,14 @@ namespace sdf
     /// \sa uint64_t ModelCount() const
     public: const Model *ModelByIndex(const uint64_t _index) const;
 
+    /// \brief Get an immediate (not recursively nested) mutable child model
+    // based on an index.
+    /// \param[in] _index Index of the nested 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: Model *ModelByIndex(uint64_t _index);
+
     /// \brief Get whether a nested model name exists.
     /// \param[in] _name Name of the nested model to check.
     /// To check for a model nested in other models, prefix the model name
@@ -254,6 +311,15 @@ namespace sdf
     /// \sa bool ModelNameExists(const std::string &_name) const
     public: const Model *ModelByName(const std::string &_name) const;
 
+    /// \brief Get a mutable nested model based on a name.
+    /// \param[in] _name Name of the nested model.
+    /// To get a model nested in other models, prefix the model name
+    /// with the sequence of nested model names, delimited by "::".
+    /// \return Pointer to the model. Nullptr if a model with the given name
+    ///  does not exist.
+    /// \sa bool ModelNameExists(const std::string &_name) const
+    public: Model *ModelByName(const std::string &_name);
+
     /// \brief Get the pose of the model. This is the pose of the model
     /// as specified in SDF (<model> <pose> ... </pose></model>), and is
     /// typically used to express the position and rotation of a model in a
@@ -380,6 +446,12 @@ namespace sdf
     /// exists.
     public: bool AddModel(const Model &_model);
 
+    /// \brief Add a frame to the model.
+    /// \param[in] _frame Frame to add.
+    /// \return True if successful, false if a frame with the name already
+    /// exists.
+    public: bool AddFrame(const Frame &_frame);
+
     /// \brief Remove all links.
     public: void ClearLinks();
 
@@ -389,6 +461,9 @@ namespace sdf
     /// \brief Remove all models.
     public: void ClearModels();
 
+    /// \brief Remove all frames.
+    public: void ClearFrames();
+
     /// \brief Get the URI associated with this model
     /// \return The model's URI, or empty string if it has not been set.
     public: std::string Uri() const;

src/Model.cc

@@ -468,6 +468,13 @@ const Link *Model::LinkByIndex(const uint64_t _index) const
   return nullptr;
 }
 
+/////////////////////////////////////////////////
+Link *Model::LinkByIndex(uint64_t _index)
+{
+  return const_cast<Link*>(
+      static_cast<const Model*>(this)->LinkByIndex(_index));
+}
+
 /////////////////////////////////////////////////
 bool Model::LinkNameExists(const std::string &_name) const
 {
@@ -488,6 +495,13 @@ const Joint *Model::JointByIndex(const uint64_t _index) const
   return nullptr;
 }
 
+/////////////////////////////////////////////////
+Joint *Model::JointByIndex(uint64_t _index)
+{
+  return const_cast<Joint*>(
+      static_cast<const Model*>(this)->JointByIndex(_index));
+}
+
 /////////////////////////////////////////////////
 bool Model::JointNameExists(const std::string &_name) const
 {
@@ -521,6 +535,14 @@ const Joint *Model::JointByName(const std::string &_name) const
     }
   }
   return nullptr;
+
+}
+
+/////////////////////////////////////////////////
+Joint *Model::JointByName(const std::string &_name)
+{
+  return const_cast<Joint*>(
+      static_cast<const Model*>(this)->JointByName(_name));
 }
 
 /////////////////////////////////////////////////
@@ -537,6 +559,13 @@ const Frame *Model::FrameByIndex(const uint64_t _index) const
   return nullptr;
 }
 
+/////////////////////////////////////////////////
+Frame *Model::FrameByIndex(uint64_t _index)
+{
+  return const_cast<Frame*>(
+      static_cast<const Model*>(this)->FrameByIndex(_index));
+}
+
 /////////////////////////////////////////////////
 bool Model::FrameNameExists(const std::string &_name) const
 {
@@ -572,6 +601,13 @@ const Frame *Model::FrameByName(const std::string &_name) const
   return nullptr;
 }
 
+/////////////////////////////////////////////////
+Frame *Model::FrameByName(const std::string &_name)
+{
+  return const_cast<Frame*>(
+      static_cast<const Model*>(this)->FrameByName(_name));
+}
+
 /////////////////////////////////////////////////
 uint64_t Model::ModelCount() const
 {
@@ -586,6 +622,13 @@ const Model *Model::ModelByIndex(const uint64_t _index) const
   return nullptr;
 }
 
+/////////////////////////////////////////////////
+Model *Model::ModelByIndex(uint64_t _index)
+{
+  return const_cast<Model*>(
+      static_cast<const Model*>(this)->ModelByIndex(_index));
+}
+
 /////////////////////////////////////////////////
 bool Model::ModelNameExists(const std::string &_name) const
 {
@@ -613,6 +656,14 @@ const Model *Model::ModelByName(const std::string &_name) const
     return nextModel->ModelByName(_name.substr(index + 2));
   }
   return nextModel;
+
+}
+
+/////////////////////////////////////////////////
+Model *Model::ModelByName(const std::string &_name)
+{
+  return const_cast<Model*>(
+      static_cast<const Model*>(this)->ModelByName(_name));
 }
 
 /////////////////////////////////////////////////
@@ -825,6 +876,14 @@ const Link *Model::LinkByName(const std::string &_name) const
     }
   }
   return nullptr;
+
+}
+
+/////////////////////////////////////////////////
+Link *Model::LinkByName(const std::string &_name)
+{
+  return const_cast<Link*>(
+      static_cast<const Model*>(this)->LinkByName(_name));
 }
 
 /////////////////////////////////////////////////
@@ -988,3 +1047,18 @@ void Model::ClearModels()
 {
   this->dataPtr->models.clear();
 }
+
+//////////////////////////////////////////////////
+bool Model::AddFrame(const Frame &_frame)
+{
+  if (this->FrameNameExists(_frame.Name()))
+    return false;
+  this->dataPtr->frames.push_back(_frame);
+  return true;
+}
+
+//////////////////////////////////////////////////
+void Model::ClearFrames()
+{
+  this->dataPtr->frames.clear();
+}