Added Add & Clear function to World, Model, and Link (#765)

* Added Add & Clear function to World, Model, and Link

Signed-off-by: Nate Koenig <[email protected]>

* spelling

Signed-off-by: Nate Koenig <[email protected]>

Co-authored-by: Nate Koenig <[email protected]>

include/sdf/Link.hh

@@ -252,6 +252,42 @@ namespace sdf
     /// \sa Model::SetEnableWind(bool)
     public: void SetEnableWind(bool _enableWind);
 
+    /// \brief Add a collision to the link.
+    /// \param[in] _collision Collision to add.
+    /// \return True if successful, false if a collision with the name already
+    /// exists.
+    public: bool AddCollision(const Collision &_collision);
+
+    /// \brief Add a visual to the link.
+    /// \param[in] _visual Visual to add.
+    /// \return True if successful, false if a visual with the name already
+    /// exists.
+    public: bool AddVisual(const Visual &_visual);
+
+    /// \brief Add a light to the link.
+    /// \param[in] _light Light to add.
+    /// \return True if successful, false if a light with the name already
+    /// exists.
+    public: bool AddLight(const Light &_light);
+
+    /// \brief Add a sensor to the link.
+    /// \param[in] _sensor Sensor to add.
+    /// \return True if successful, false if a sensor with the name already
+    /// exists.
+    public: bool AddSensor(const Sensor &_sensor);
+
+    /// \brief Remove all collisions
+    public: void ClearCollisions();
+
+    /// \brief Remove all visuals
+    public: void ClearVisuals();
+
+    /// \brief Remove all lights
+    public: void ClearLights();
+
+    /// \brief Remove all sensors
+    public: void ClearSensors();
+
     /// \brief Private data pointer.
     IGN_UTILS_IMPL_PTR(dataPtr)
   };

include/sdf/Model.hh

@@ -346,6 +346,33 @@ namespace sdf
     public: const NestedInclude *InterfaceModelNestedIncludeByIndex(
                 const uint64_t _index) const;
 
+    /// \brief Add a link to the model.
+    /// \param[in] _link Link to add.
+    /// \return True if successful, false if a link with the name already
+    /// exists.
+    public: bool AddLink(const Link &_link);
+
+    /// \brief Add a joint to the model.
+    /// \param[in] _link Joint to add.
+    /// \return True if successful, false if a joint with the name already
+    /// exists.
+    public: bool AddJoint(const Joint &_joint);
+
+    /// \brief Add a model to the model.
+    /// \param[in] _model Model to add.
+    /// \return True if successful, false if a model with the name already
+    /// exists.
+    public: bool AddModel(const Model &_model);
+
+    /// \brief Remove all links.
+    public: void ClearLinks();
+
+    /// \brief Remove all joints.
+    public: void ClearJoints();
+
+    /// \brief Remove all models.
+    public: void ClearModels();
+
     /// \brief Give the scoped PoseRelativeToGraph to be used for resolving
     /// poses. This is private and is intended to be called by Root::Load or
     /// World::SetPoseRelativeToGraph if this is a standalone model and

include/sdf/World.hh

@@ -180,6 +180,33 @@ namespace sdf
     /// \return True if there exists a model with the given name.
     public: bool ModelNameExists(const std::string &_name) const;
 
+    /// \brief Add a model to the world.
+    /// \param[in] _model Model to add.
+    /// \return True if successful, false if a model with the name already
+    /// exists.
+    public: bool AddModel(const Model &_model);
+
+    /// \brief Add an actor to the world.
+    /// \param[in] _actor Actor to add.
+    /// \return True if successful, false if an actor with the name already
+    /// exists.
+    public: bool AddActor(const Actor &_actor);
+
+    /// \brief Add a light to the world.
+    /// \param[in] _light Light to add.
+    /// \return True if successful, false if a lights with the name already
+    /// exists.
+    public: bool AddLight(const Light &_light);
+
+    /// \brief Remove all models.
+    public: void ClearModels();
+
+    /// \brief Remove all models.
+    public: void ClearActors();
+
+    /// \brief Remove all models.
+    public: void ClearLights();
+
     /// \brief Get the number of actors.
     /// \return Number of actors contained in this World object.
     public: uint64_t ActorCount() const;

sdf/1.9/actor.sdf

@@ -74,9 +74,9 @@
         </element>
         <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
           <description>The pose which should be reached at the given time.</description>
-        </element>
+        </element> <!-- End Pose -->
       </element> <!-- End Waypoint -->
-    </element> <!-- End Action -->
+    </element> <!-- End Trajectory -->
   </element> <!-- End Script -->
 
   <include filename="link.sdf" required="+"/>

src/Link.cc

@@ -497,3 +497,63 @@ void Link::SetEnableWind(const bool _enableWind)
 {
   this->dataPtr->enableWind = _enableWind;
 }
+
+//////////////////////////////////////////////////
+bool Link::AddCollision(const Collision &_collision)
+{
+  if (this->CollisionNameExists(_collision.Name()))
+    return false;
+  this->dataPtr->collisions.push_back(_collision);
+  return true;
+}
+
+//////////////////////////////////////////////////
+bool Link::AddVisual(const Visual &_visual)
+{
+  if (this->VisualNameExists(_visual.Name()))
+    return false;
+  this->dataPtr->visuals.push_back(_visual);
+  return true;
+}
+
+//////////////////////////////////////////////////
+bool Link::AddLight(const Light &_light)
+{
+  if (this->LightNameExists(_light.Name()))
+    return false;
+  this->dataPtr->lights.push_back(_light);
+  return true;
+}
+
+//////////////////////////////////////////////////
+bool Link::AddSensor(const Sensor &_sensor)
+{
+  if (this->SensorNameExists(_sensor.Name()))
+    return false;
+  this->dataPtr->sensors.push_back(_sensor);
+  return true;
+}
+
+//////////////////////////////////////////////////
+void Link::ClearCollisions()
+{
+  this->dataPtr->collisions.clear();
+}
+
+//////////////////////////////////////////////////
+void Link::ClearVisuals()
+{
+  this->dataPtr->visuals.clear();
+}
+
+//////////////////////////////////////////////////
+void Link::ClearLights()
+{
+  this->dataPtr->lights.clear();
+}
+
+//////////////////////////////////////////////////
+void Link::ClearSensors()
+{
+  this->dataPtr->sensors.clear();
+}

src/Link_TEST.cc

@@ -211,3 +211,95 @@ TEST(DOMLink, InvalidInertia)
   EXPECT_DOUBLE_EQ(3.4, link.Inertial().MassMatrix().OffDiagonalMoments().Z());
   EXPECT_FALSE(link.Inertial().MassMatrix().IsValid());
 }
+
+/////////////////////////////////////////////////
+TEST(DOMLink, AddCollision)
+{
+  sdf::Link link;
+  EXPECT_EQ(0u, link.CollisionCount());
+
+  sdf::Collision collision;
+  collision.SetName("collision1");
+  EXPECT_TRUE(link.AddCollision(collision));
+  EXPECT_EQ(1u, link.CollisionCount());
+  EXPECT_FALSE(link.AddCollision(collision));
+  EXPECT_EQ(1u, link.CollisionCount());
+
+  link.ClearCollisions();
+  EXPECT_EQ(0u, link.CollisionCount());
+
+  EXPECT_TRUE(link.AddCollision(collision));
+  EXPECT_EQ(1u, link.CollisionCount());
+  const sdf::Collision *collisionFromLink = link.CollisionByIndex(0);
+  ASSERT_NE(nullptr, collisionFromLink);
+  EXPECT_EQ(collisionFromLink->Name(), collision.Name());
+}
+
+/////////////////////////////////////////////////
+TEST(DOMLink, AddVisual)
+{
+  sdf::Link link;
+  EXPECT_EQ(0u, link.VisualCount());
+
+  sdf::Visual visual;
+  visual.SetName("visual1");
+  EXPECT_TRUE(link.AddVisual(visual));
+  EXPECT_EQ(1u, link.VisualCount());
+  EXPECT_FALSE(link.AddVisual(visual));
+  EXPECT_EQ(1u, link.VisualCount());
+
+  link.ClearVisuals();
+  EXPECT_EQ(0u, link.VisualCount());
+
+  EXPECT_TRUE(link.AddVisual(visual));
+  EXPECT_EQ(1u, link.VisualCount());
+  const sdf::Visual *visualFromLink = link.VisualByIndex(0);
+  ASSERT_NE(nullptr, visualFromLink);
+  EXPECT_EQ(visualFromLink->Name(), visual.Name());
+}
+
+/////////////////////////////////////////////////
+TEST(DOMLink, AddLight)
+{
+  sdf::Link link;
+  EXPECT_EQ(0u, link.LightCount());
+
+  sdf::Light light;
+  light.SetName("light1");
+  EXPECT_TRUE(link.AddLight(light));
+  EXPECT_EQ(1u, link.LightCount());
+  EXPECT_FALSE(link.AddLight(light));
+  EXPECT_EQ(1u, link.LightCount());
+
+  link.ClearLights();
+  EXPECT_EQ(0u, link.LightCount());
+
+  EXPECT_TRUE(link.AddLight(light));
+  EXPECT_EQ(1u, link.LightCount());
+  const sdf::Light *lightFromLink = link.LightByIndex(0);
+  ASSERT_NE(nullptr, lightFromLink);
+  EXPECT_EQ(lightFromLink->Name(), light.Name());
+}
+
+/////////////////////////////////////////////////
+TEST(DOMLink, AddSensor)
+{
+  sdf::Link link;
+  EXPECT_EQ(0u, link.SensorCount());
+
+  sdf::Sensor sensor;
+  sensor.SetName("sensor1");
+  EXPECT_TRUE(link.AddSensor(sensor));
+  EXPECT_EQ(1u, link.SensorCount());
+  EXPECT_FALSE(link.AddSensor(sensor));
+  EXPECT_EQ(1u, link.SensorCount());
+
+  link.ClearSensors();
+  EXPECT_EQ(0u, link.SensorCount());
+
+  EXPECT_TRUE(link.AddSensor(sensor));
+  EXPECT_EQ(1u, link.SensorCount());
+  const sdf::Sensor *sensorFromLink = link.SensorByIndex(0);
+  ASSERT_NE(nullptr, sensorFromLink);
+  EXPECT_EQ(sensorFromLink->Name(), sensor.Name());
+}

src/Model.cc

@@ -790,3 +790,48 @@ const NestedInclude *Model::InterfaceModelNestedIncludeByIndex(
     return &this->dataPtr->interfaceModels[_index].first;
   return nullptr;
 }
+
+//////////////////////////////////////////////////
+bool Model::AddLink(const Link &_link)
+{
+  if (this->LinkNameExists(_link.Name()))
+    return false;
+  this->dataPtr->links.push_back(_link);
+  return true;
+}
+
+//////////////////////////////////////////////////
+bool Model::AddJoint(const Joint &_joint)
+{
+  if (this->JointNameExists(_joint.Name()))
+    return false;
+  this->dataPtr->joints.push_back(_joint);
+  return true;
+}
+
+//////////////////////////////////////////////////
+bool Model::AddModel(const Model &_model)
+{
+  if (this->ModelNameExists(_model.Name()))
+    return false;
+  this->dataPtr->models.push_back(_model);
+  return true;
+}
+
+//////////////////////////////////////////////////
+void Model::ClearLinks()
+{
+  this->dataPtr->links.clear();
+}
+
+//////////////////////////////////////////////////
+void Model::ClearJoints()
+{
+  this->dataPtr->joints.clear();
+}
+
+//////////////////////////////////////////////////
+void Model::ClearModels()
+{
+  this->dataPtr->models.clear();
+}