Add more model APIs (#349)

Signed-off-by: Louise Poubel <[email protected]>

Co-authored-by: John Shepherd <[email protected]>

include/ignition/gazebo/Model.hh

@@ -19,6 +19,9 @@
 
 #include <memory>
 #include <string>
+#include <vector>
+
+#include <ignition/math/Pose3.hh>
 
 #include <ignition/gazebo/config.hh>
 #include <ignition/gazebo/EntityComponentManager.hh>
@@ -95,11 +98,41 @@ namespace ignition
       /// \return Model's name.
       public: std::string Name(const EntityComponentManager &_ecm) const;
 
+      /// \brief Get the ID of the entity which is the immediate parent of this
+      /// model.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Parent entity ID.
+      public: gazebo::Entity Parent(const EntityComponentManager &_ecm) const;
+
+      /// \brief Get whether this model is static.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return True if static.
+      public: bool Static(const EntityComponentManager &_ecm) const;
+
+      /// \brief Get whether this model has self-collide enabled.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return True if self-colliding.
+      public: bool SelfCollide(const EntityComponentManager &_ecm) const;
+
+      /// \brief Get whether this model has wind enabled.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return True if wind mode is on.
+      public: bool WindMode(const EntityComponentManager &_ecm) const;
+
+      /// \brief Get the source file where this model came from. If empty,
+      /// the model wasn't loaded directly from a file, probably from an SDF
+      /// string.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Path to the source SDF file.
+      public: std::string SourceFilePath(const EntityComponentManager &_ecm)
+          const;
+
       /// \brief Get the ID of a joint entity which is an immediate child of
       /// this model.
       /// \param[in] _ecm Entity-component manager.
       /// \param[in] _name Joint name.
       /// \return Joint entity.
+      /// \todo(anyone) Make const
       public: gazebo::Entity JointByName(const EntityComponentManager &_ecm,
           const std::string &_name);
 
@@ -108,9 +141,40 @@ namespace ignition
       /// \param[in] _ecm Entity-component manager.
       /// \param[in] _name Link name.
       /// \return Link entity.
+      /// \todo(anyone) Make const
       public: gazebo::Entity LinkByName(const EntityComponentManager &_ecm,
           const std::string &_name);
 
+      /// \brief Get all joints which are immediate children of this model.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return All joints in this model.
+      public: std::vector<gazebo::Entity> Joints(
+          const EntityComponentManager &_ecm) const;
+
+      /// \brief Get all links which are immediate children of this model.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return All links in this model.
+      public: std::vector<gazebo::Entity> Links(
+          const EntityComponentManager &_ecm) const;
+
+      /// \brief Get the number of joints which are immediate children of this
+      /// model.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Number of joints in this model.
+      public: uint64_t JointCount(const EntityComponentManager &_ecm) const;
+
+      /// \brief Get the number of links which are immediate children of this
+      /// model.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Number of links in this model.
+      public: uint64_t LinkCount(const EntityComponentManager &_ecm) const;
+
+      /// \brief Set a command to change the model's pose.
+      /// \param[in] _ecm Entity-component manager.
+      /// \param[in] _pose New model pose.
+      public: void SetWorldPoseCmd(EntityComponentManager &_ecm,
+          const math::Pose3d &_pose);
+
       /// \brief Pointer to private data.
       private: std::unique_ptr<ModelPrivate> dataPtr;
     };

include/ignition/gazebo/Util.hh

@@ -51,6 +51,56 @@ namespace ignition
       const EntityComponentManager &_ecm, const std::string &_delim = "/",
       bool _includePrefix = true);
 
+    /// \brief Generally, each entity will be of some specific high-level type,
+    /// such as World, Sensor, Collision, etc, and one type only.
+    /// The entity type is usually marked by having some component that
+    /// represents that type, such as `ignition::gazebo::components::Visual`.
+    ///
+    /// This function returns the type ID of the given entity's type, which
+    /// can be checked against different types. For example, if the
+    /// entity is a model, this will be true:
+    ///
+    /// `gazebo::components::Model::typeId == entityTypeId(entity, ecm)`
+    ///
+    /// In case the entity isn't of any known type, this will return
+    /// `ignition::gazebo::kComponentTypeIdInvalid`.
+    ///
+    /// In case the entity has more than one type, only one of them will be
+    /// returned. This is not standard usage.
+    ///
+    /// \param[in] _entity Entity to get the type for.
+    /// \param[in] _ecm Immutable reference to ECM.
+    /// \return ID of entity's type-defining components.
+    ComponentTypeId IGNITION_GAZEBO_VISIBLE entityTypeId(const Entity &_entity,
+        const EntityComponentManager &_ecm);
+
+    /// \brief Generally, each entity will be of some specific high-level type,
+    /// such as "world", "sensor", "collision", etc, and one type only.
+    ///
+    /// This function returns a lowercase string for each type. For example,
+    /// "light", "actor", etc.
+    ///
+    /// In case the entity isn't of any known type, this will return an empty
+    /// string.
+    ///
+    /// In case the entity has more than one type, only one of them will be
+    /// returned. This is not standard usage.
+    ///
+    /// Note that this is different from  component type names.
+    ///
+    /// \param[in] _entity Entity to get the type for.
+    /// \param[in] _ecm Immutable reference to ECM.
+    /// \return ID of entity's type-defining components.
+    std::string IGNITION_GAZEBO_VISIBLE entityTypeStr(const Entity &_entity,
+        const EntityComponentManager &_ecm);
+
+    /// \brief Get the world to which the given entity belongs.
+    /// \param[in] _entity Entity to get the world for.
+    /// \param[in] _ecm Immutable reference to ECM.
+    /// \return World entity ID.
+    Entity IGNITION_GAZEBO_VISIBLE worldEntity(const Entity &_entity,
+        const EntityComponentManager &_ecm);
+
     /// \brief Helper function to remove a parent scope from a given name.
     /// This removes the first name found before the delimiter.
     /// \param[in] _name Input name possibly generated by scopedName.

include/ignition/gazebo/components/AngularVelocityCmd.hh

@@ -32,14 +32,14 @@ namespace gazebo
 inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
 namespace components
 {
-  /// \brief A component type that contains angular velocity cmd of an entity
-  /// represented by ignition::math::Vector3d.
+  /// \brief A component type that contains the commanded angular velocity of
+  /// an entity, in its own frame, represented by ignition::math::Vector3d.
   using AngularVelocityCmd =
     Component<math::Vector3d, class AngularVelocityCmdTag>;
   IGN_GAZEBO_REGISTER_COMPONENT(
     "ign_gazebo_components.AngularVelocityCmd", AngularVelocityCmd)
 
-  /// \brief A component type that contains angular velocity cmd
+  /// \brief A component type that contains the commanded angular velocity
   /// of an entity in the world frame represented by ignition::math::Vector3d.
   using WorldAngularVelocityCmd =
       Component<math::Vector3d, class WorldAngularVelocityCmdTag>;

include/ignition/gazebo/components/LinearVelocityCmd.hh

@@ -32,15 +32,17 @@ namespace gazebo
 inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
 namespace components
 {
-  /// \brief A component type that contains linear velocity of an entity
-  /// represented by ignition::math::Vector3d.
+  // \brief A component type that contains the commanded linear velocity of an
+  /// entity represented by ignition::math::Vector3d, expressed in the entity's
+  /// frame.
   using LinearVelocityCmd = Component<
     math::Vector3d, class LinearVelocityCmdTag>;
   IGN_GAZEBO_REGISTER_COMPONENT(
       "ign_gazebo_components.LinearVelocityCmd", LinearVelocityCmd)
 
-  /// \brief A component type that contains linear velocity of an entity in the
-  /// world frame represented by ignition::math::Vector3d.
+  /// \brief A component type that contains the commanded linear velocity of an
+  /// entity represented by ignition::math::Vector3d, expressed in the world
+  /// frame.
   using WorldLinearVelocityCmd =
       Component<math::Vector3d, class WorldLinearVelocityCmdTag>;
   IGN_GAZEBO_REGISTER_COMPONENT(

src/Model.cc

@@ -20,6 +20,11 @@
 #include "ignition/gazebo/components/Model.hh"
 #include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/ParentEntity.hh"
+#include "ignition/gazebo/components/PoseCmd.hh"
+#include "ignition/gazebo/components/SelfCollide.hh"
+#include "ignition/gazebo/components/SourceFilePath.hh"
+#include "ignition/gazebo/components/Static.hh"
+#include "ignition/gazebo/components/WindMode.hh"
 #include "ignition/gazebo/Model.hh"
 
 class ignition::gazebo::ModelPrivate
@@ -81,6 +86,56 @@ std::string Model::Name(const EntityComponentManager &_ecm) const
   return "";
 }
 
+//////////////////////////////////////////////////
+Entity Model::Parent(const EntityComponentManager &_ecm) const
+{
+  auto comp = _ecm.Component<components::ParentEntity>(this->dataPtr->id);
+  if (comp)
+    return comp->Data();
+
+  return kNullEntity;
+}
+
+//////////////////////////////////////////////////
+bool Model::Static(const EntityComponentManager &_ecm) const
+{
+  auto comp = _ecm.Component<components::Static>(this->dataPtr->id);
+  if (comp)
+    return comp->Data();
+
+  return false;
+}
+
+//////////////////////////////////////////////////
+bool Model::SelfCollide(const EntityComponentManager &_ecm) const
+{
+  auto comp = _ecm.Component<components::SelfCollide>(this->dataPtr->id);
+  if (comp)
+    return comp->Data();
+
+  return false;
+}
+
+//////////////////////////////////////////////////
+bool Model::WindMode(const EntityComponentManager &_ecm) const
+{
+  auto comp = _ecm.Component<components::WindMode>(this->dataPtr->id);
+  if (comp)
+    return comp->Data();
+
+  return false;
+}
+
+//////////////////////////////////////////////////
+std::string Model::SourceFilePath(const EntityComponentManager &_ecm) const
+{
+  auto comp = _ecm.Component<components::SourceFilePath>(this->dataPtr->id);
+  if (comp)
+    return comp->Data();
+
+  return "";
+}
+
 //////////////////////////////////////////////////
 Entity Model::JointByName(const EntityComponentManager &_ecm,
     const std::string &_name)
@@ -101,3 +156,50 @@ Entity Model::LinkByName(const EntityComponentManager &_ecm,
       components::Link());
 }
 
+//////////////////////////////////////////////////
+std::vector<Entity> Model::Joints(const EntityComponentManager &_ecm) const
+{
+  return _ecm.EntitiesByComponents(
+      components::ParentEntity(this->dataPtr->id),
+      components::Joint());
+}
+
+//////////////////////////////////////////////////
+std::vector<Entity> Model::Links(const EntityComponentManager &_ecm) const
+{
+  return _ecm.EntitiesByComponents(
+      components::ParentEntity(this->dataPtr->id),
+      components::Link());
+}
+
+//////////////////////////////////////////////////
+uint64_t Model::JointCount(const EntityComponentManager &_ecm) const
+{
+  return this->Joints(_ecm).size();
+}
+
+//////////////////////////////////////////////////
+uint64_t Model::LinkCount(const EntityComponentManager &_ecm) const
+{
+  return this->Links(_ecm).size();
+}
+
+//////////////////////////////////////////////////
+void Model::SetWorldPoseCmd(EntityComponentManager &_ecm,
+    const math::Pose3d &_pose)
+{
+  auto poseCmdComp = _ecm.Component<components::WorldPoseCmd>(
+      this->dataPtr->id);
+  if (!poseCmdComp)
+  {
+    _ecm.CreateComponent(this->dataPtr->id, components::WorldPoseCmd(_pose));
+  }
+  else
+  {
+    poseCmdComp->SetData(_pose,
+        [](const math::Pose3d &, const math::Pose3d &){return false;});
+    _ecm.SetChanged(this->dataPtr->id,
+        components::WorldPoseCmd::typeId, ComponentState::OneTimeChange);
+  }
+}
+