More world APIs, helper function ComponentData (gazebosim#378)

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

include/ignition/gazebo/EntityComponentManager.hh

@@ -214,6 +214,18 @@ namespace ignition
       public: template<typename ComponentTypeT>
               ComponentTypeT *Component(const ComponentKey &_key);
 
+      /// \brief Get the data from a component.
+      /// * If the component type doesn't hold any data, this won't compile.
+      /// * If the entity doesn't have that component, it will return nullopt.
+      /// * If the entity has the component, return its data.
+      /// \param[in] _entity The entity.
+      /// \tparam ComponentTypeT Component type
+      /// \return The data of the component of the specified type assigned to
+      /// specified Entity, or nullptr if the component could not be found.
+      public: template<typename ComponentTypeT>
+              std::optional<typename ComponentTypeT::Type> ComponentData(
+              const Entity _entity) const;
+
       /// \brief Get the type IDs of all components attached to an entity.
       /// \param[in] _entity Entity to check.
       /// \return All the component type IDs.

include/ignition/gazebo/World.hh

@@ -0,0 +1,189 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef IGNITION_GAZEBO_WORLD_HH_
+#define IGNITION_GAZEBO_WORLD_HH_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <sdf/Atmosphere.hh>
+#include <ignition/math/Vector3.hh>
+
+#include "ignition/gazebo/config.hh"
+#include "ignition/gazebo/EntityComponentManager.hh"
+#include "ignition/gazebo/Export.hh"
+#include "ignition/gazebo/Types.hh"
+
+namespace ignition
+{
+  namespace gazebo
+  {
+    // Inline bracket to help doxygen filtering.
+    inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+    // Forward declarations.
+    class IGNITION_GAZEBO_HIDDEN WorldPrivate;
+    //
+    /// \class World World.hh ignition/gazebo/World.hh
+    /// \brief This class provides wrappers around entities and components
+    /// which are more convenient and straight-forward to use than dealing
+    /// with the `EntityComponentManager` directly.
+    /// All the functions provided here are meant to be used with a world
+    /// entity.
+    ///
+    /// For example, given a world's entity, to find the value of its
+    /// name component, one could use the entity-component manager (`ecm`)
+    /// directly as follows:
+    ///
+    ///     std::string name = ecm.Component<components::Name>(entity)->Data();
+    ///
+    /// Using this class however, the same information can be obtained with
+    /// a simpler function call:
+    ///
+    ///    World world(entity);
+    ///    std::string name = world.Name(ecm);
+    class IGNITION_GAZEBO_VISIBLE World {
+      /// \brief Constructor
+      /// \param[in] _entity World entity
+      public: explicit World(gazebo::Entity _entity = kNullEntity);
+
+      /// \brief Copy constructor
+      /// \param[in] _world World to copy.
+      public: World(const World &_world);
+
+      /// \brief Move constructor
+      /// \param[in] _world World to move.
+      public: World(World &&_world) noexcept;
+
+      /// \brief Move assignment operator.
+      /// \param[in] _world World component to move.
+      /// \return Reference to this.
+      public: World &operator=(World &&_world) noexcept;
+
+      /// \brief Copy assignment operator.
+      /// \param[in] _world World to copy.
+      /// \return Reference to this.
+      public: World &operator=(const World &_world);
+
+      /// \brief Destructor
+      public: virtual ~World();
+
+      /// \brief Get the entity which this World is related to.
+      /// \return World entity.
+      public: gazebo::Entity Entity() const;
+
+      /// \brief Check whether this world correctly refers to an entity that
+      /// has a components::World.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return True if it's a valid world in the manager.
+      public: bool Valid(const EntityComponentManager &_ecm) const;
+
+      /// \brief Get the world's unscoped name.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return World's name or nullopt if the entity does not have a
+      /// components::Name component
+      public: std::optional<std::string> Name(
+          const EntityComponentManager &_ecm) const;
+
+      /// \brief Get the gravity in m/s^2.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Gravity vector or nullopt if the entity does not
+      /// have a components::Gravity component.
+      public: std::optional<math::Vector3d> Gravity(
+          const EntityComponentManager &_ecm) const;
+
+      /// \brief Get the magnetic field in Tesla.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Magnetic field vector or nullopt if the entity does not
+      /// have a components::MagneticField component.
+      public: std::optional<math::Vector3d> MagneticField(
+          const EntityComponentManager &_ecm) const;
+
+      /// \brief Get atmosphere information.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Magnetic field vector or nullopt if the entity does not
+      /// have a components::Atmosphere component.
+      public: std::optional<sdf::Atmosphere> Atmosphere(
+          const EntityComponentManager &_ecm) const;
+
+      /// \brief Get the ID of a light entity which is an immediate child of
+      /// this world.
+      /// \param[in] _ecm Entity-component manager.
+      /// \param[in] _name Light name.
+      /// \return Light entity.
+      public: gazebo::Entity LightByName(const EntityComponentManager &_ecm,
+          const std::string &_name) const;
+
+      /// \brief Get the ID of a actor entity which is an immediate child of
+      /// this world.
+      /// \param[in] _ecm Entity-component manager.
+      /// \param[in] _name Actor name.
+      /// \return Actor entity.
+      public: gazebo::Entity ActorByName(const EntityComponentManager &_ecm,
+          const std::string &_name) const;
+
+      /// \brief Get the ID of a model entity which is an immediate child of
+      /// this world.
+      /// \param[in] _ecm Entity-component manager.
+      /// \param[in] _name Model name.
+      /// \return Model entity.
+      public: gazebo::Entity ModelByName(const EntityComponentManager &_ecm,
+          const std::string &_name) const;
+
+      /// \brief Get all lights which are immediate children of this world.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return All lights in this world.
+      public: std::vector<gazebo::Entity> Lights(
+          const EntityComponentManager &_ecm) const;
+
+      /// \brief Get all actors which are immediate children of this world.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return All actors in this world.
+      public: std::vector<gazebo::Entity> Actors(
+          const EntityComponentManager &_ecm) const;
+
+      /// \brief Get all models which are immediate children of this world.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return All models in this world.
+      public: std::vector<gazebo::Entity> Models(
+          const EntityComponentManager &_ecm) const;
+
+      /// \brief Get the number of lights which are immediate children of this
+      /// world.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Number of lights in this world.
+      public: uint64_t LightCount(const EntityComponentManager &_ecm) const;
+
+      /// \brief Get the number of actors which are immediate children of this
+      /// world.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Number of actors in this world.
+      public: uint64_t ActorCount(const EntityComponentManager &_ecm) const;
+
+      /// \brief Get the number of models which are immediate children of this
+      /// world.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Number of models in this world.
+      public: uint64_t ModelCount(const EntityComponentManager &_ecm) const;
+
+      /// \brief Pointer to private data.
+      private: std::unique_ptr<WorldPrivate> dataPtr;
+    };
+    }
+  }
+}
+#endif

include/ignition/gazebo/detail/EntityComponentManager.hh

@@ -78,6 +78,18 @@ ComponentTypeT *EntityComponentManager::Component(const ComponentKey &_key)
       this->ComponentImplementation(_key));
 }
 
+//////////////////////////////////////////////////
+template<typename ComponentTypeT>
+std::optional<typename ComponentTypeT::Type>
+    EntityComponentManager::ComponentData(const Entity _entity) const
+{
+  auto comp = this->Component<ComponentTypeT>(_entity);
+  if (!comp)
+    return std::nullopt;
+
+  return std::make_optional(comp->Data());
+}
+
 //////////////////////////////////////////////////
 template<typename ComponentTypeT>
 const ComponentTypeT *EntityComponentManager::First() const

src/CMakeLists.txt

@@ -56,6 +56,7 @@ set (sources
   SystemLoader.cc
   Util.cc
   View.cc
+  World.cc
   ${PROTO_PRIVATE_SRC}
   ${network_sources}
 )
@@ -78,6 +79,7 @@ set (gtest_sources
   System_TEST.cc
   SystemLoader_TEST.cc
   Util_TEST.cc
+  World_TEST.cc
   network/NetworkConfig_TEST.cc
   network/PeerTracker_TEST.cc
   network/NetworkManager_TEST.cc

src/EntityComponentManager_TEST.cc

@@ -460,45 +460,69 @@ TEST_P(EntityComponentManagerFixture, ComponentValues)
     const auto *value = manager.Component<IntComponent>(eInt);
     ASSERT_NE(nullptr, value);
     EXPECT_EQ(123, value->Data());
+
+    auto data = manager.ComponentData<IntComponent>(eInt);
+    EXPECT_EQ(123, data);
   }
 
   {
     const auto *value = manager.Component<DoubleComponent>(eDouble);
     ASSERT_NE(nullptr, value);
     EXPECT_DOUBLE_EQ(0.123, value->Data());
+
+    auto data = manager.ComponentData<DoubleComponent>(eDouble);
+    EXPECT_EQ(0.123, data);
   }
 
   {
     const auto *value = manager.Component<IntComponent>(eIntDouble);
     ASSERT_NE(nullptr, value);
     EXPECT_EQ(456, value->Data());
+
+    auto data = manager.ComponentData<IntComponent>(eIntDouble);
+    EXPECT_EQ(456, data);
   }
 
   {
     const auto *value = manager.Component<DoubleComponent>(eIntDouble);
     ASSERT_NE(nullptr, value);
     EXPECT_DOUBLE_EQ(0.456, value->Data());
+
+    auto data = manager.ComponentData<DoubleComponent>(eIntDouble);
+    EXPECT_EQ(0.456, data);
   }
 
   // Failure cases
   {
     const auto *value = manager.Component<IntComponent>(eDouble);
     ASSERT_EQ(nullptr, value);
+
+    auto data = manager.ComponentData<IntComponent>(eDouble);
+    EXPECT_EQ(std::nullopt, data);
   }
 
   {
     const auto *value = manager.Component<DoubleComponent>(eInt);
     ASSERT_EQ(nullptr, value);
+
+    auto data = manager.ComponentData<DoubleComponent>(eInt);
+    EXPECT_EQ(std::nullopt, data);
   }
 
   {
     const auto *value = manager.Component<IntComponent>(999);
     ASSERT_EQ(nullptr, value);
+
+    auto data = manager.ComponentData<IntComponent>(999);
+    EXPECT_EQ(std::nullopt, data);
   }
 
   {
     const auto *value = manager.Component<DoubleComponent>(999);
     ASSERT_EQ(nullptr, value);
+
+    auto data = manager.ComponentData<DoubleComponent>(999);
+    EXPECT_EQ(std::nullopt, data);
   }
 }
 

src/Link.cc

@@ -94,11 +94,7 @@ bool Link::Valid(const EntityComponentManager &_ecm) const
 //////////////////////////////////////////////////
 std::optional<std::string> Link::Name(const EntityComponentManager &_ecm) const
 {
-  auto comp = _ecm.Component<components::Name>(this->dataPtr->id);
-  if (!comp)
-    return std::nullopt;
-
-  return std::make_optional(comp->Data());
+  return _ecm.ComponentData<components::Name>(this->dataPtr->id);
 }
 
 //////////////////////////////////////////////////
@@ -181,11 +177,7 @@ bool Link::WindMode(const EntityComponentManager &_ecm) const
 std::optional<math::Pose3d> Link::WorldPose(
     const EntityComponentManager &_ecm) const
 {
-  auto worldPose = _ecm.Component<components::WorldPose>(this->dataPtr->id);
-  if (!worldPose)
-    return std::nullopt;
-
-  return std::make_optional(worldPose->Data());
+  return _ecm.ComponentData<components::WorldPose>(this->dataPtr->id);
 }
 
 //////////////////////////////////////////////////
@@ -205,13 +197,7 @@ std::optional<math::Pose3d> Link::WorldInertialPose(
 std::optional<math::Vector3d> Link::WorldLinearVelocity(
     const EntityComponentManager &_ecm) const
 {
-  auto worldLinVel =
-      _ecm.Component<components::WorldLinearVelocity>(this->dataPtr->id);
-
-  if (!worldLinVel)
-    return std::nullopt;
-
-  return std::make_optional(worldLinVel->Data());
+  return _ecm.ComponentData<components::WorldLinearVelocity>(this->dataPtr->id);
 }
 
 //////////////////////////////////////////////////
@@ -238,26 +224,16 @@ std::optional<math::Vector3d> Link::WorldLinearVelocity(
 std::optional<math::Vector3d> Link::WorldAngularVelocity(
     const EntityComponentManager &_ecm) const
 {
-  auto worldAngVel =
-      _ecm.Component<components::WorldAngularVelocity>(this->dataPtr->id);
-
-  if (!worldAngVel)
-    return std::nullopt;
-
-  return std::make_optional(worldAngVel->Data());
+  return _ecm.ComponentData<components::WorldAngularVelocity>(
+      this->dataPtr->id);
 }
 
 //////////////////////////////////////////////////
 std::optional<math::Vector3d> Link::WorldLinearAcceleration(
     const EntityComponentManager &_ecm) const
 {
-  auto worldLinAccel =
-      _ecm.Component<components::WorldLinearAcceleration>(this->dataPtr->id);
-
-  if (!worldLinAccel)
-    return std::nullopt;
-
-  return std::make_optional(worldLinAccel->Data());
+  return _ecm.ComponentData<components::WorldLinearAcceleration>(
+      this->dataPtr->id);
 }
 
 //////////////////////////////////////////////////