EntityFeatureMap.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2021 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 GZ_SIM_SYSTEMS_PHYSICS_ENTITY_FEATURE_MAP_HH_
#define GZ_SIM_SYSTEMS_PHYSICS_ENTITY_FEATURE_MAP_HH_
 
#include <tuple>
#include <type_traits>
#include <unordered_map>
 
#include <gz/physics/Entity.hh>
#include <gz/physics/FindFeatures.hh>
#include <gz/physics/FeatureList.hh>
#include <gz/physics/RequestFeatures.hh>
 
#include "gz/sim/Entity.hh"
 
namespace gz::sim
{
inline namespace GZ_SIM_VERSION_NAMESPACE {
namespace systems::physics_system
{
  // \brief Helper class that associates Gazebo entities with Physics entities
  // with required and optional features. It can be used to cast a physics
  // entity with the required features to another physics entity with one of
  // the optional features. This class was created to keep all physics entities
  // in one place so that when a gazebo entity is removed, all the mapped
  // physics entitities can be removed at the same time. This ensures that
  // reference counts are properly zeroed out in the underlying physics engines
  // and the memory associated with the physics entities can be freed.
  //
  // DEV WARNING: There is an implicit conversion between physics EntityPtr and
  // std::size_t in gz-physics. This seems also implicitly convert between
  // EntityPtr and gazebo Entity. Therefore, any member function that takes a
  // gazebo Entity can accidentally take an EntityPtr. To prevent this, for
  // every function that takes a gazebo Entity, we should always have an
  // overload that also takes an EntityPtr with required features. We can do
  // this because there's a 1:1 mapping between the two in maps contained in
  // this class.
  //
  // \tparam PhysicsEntityT Type of entity, such as World, Model, or Link
  // \tparam PolicyT Policy of the physics engine (2D, 3D)
  // \tparam RequiredFeatureList Required features of the physics entity
  // \tparam OptionalFeatureLists One or more optional feature lists of the
  // physics entity.
  template <template <typename, typename> class PhysicsEntityT,
            typename PolicyT, typename RequiredFeatureList,
            typename... OptionalFeatureLists>
  class EntityFeatureMap
  {
    public: template <typename T>
    using PhysicsEntityPtr = physics::EntityPtr<PhysicsEntityT<PolicyT, T>>;
 
    public: using RequiredEntityPtr = PhysicsEntityPtr<RequiredFeatureList>;
 
    public: template <typename T>
            using HasFeatureList =
                std::disjunction<std::is_same<T, OptionalFeatureLists>...>;
 
    private: using ValueType =
                 std::tuple<RequiredEntityPtr,
                            PhysicsEntityPtr<OptionalFeatureLists>...>;
 
    public: template <typename ToFeatureList>
            PhysicsEntityPtr<ToFeatureList>
            EntityCast(sim::Entity _entity) const
    {
      // Using constexpr to limit compiler error message to the static_assert
      // cppcheck-suppress syntaxError
      if constexpr (!HasFeatureList<ToFeatureList>::value) // NOLINT
      {
        static_assert(HasFeatureList<ToFeatureList>::value,
            "Trying to cast to a FeatureList not included in the "
            "optional FeatureLists of this map.");
        return nullptr;
      }
      else
      {
        using ToEntityPtr = PhysicsEntityPtr<ToFeatureList>;
        // Has already been cast
        auto castIt = this->castCache.find(_entity);
        if (castIt != this->castCache.end())
        {
          auto castEntity = std::get<ToEntityPtr>(castIt->second.entities);
          if (nullptr != castEntity)
          {
            return castEntity;
          }
          // A failed cast means the engine doesn't implement ToFeatureList,
          // which can't change at runtime. Don't retry it on every call: a
          // cast instantiates a plugin and looks up its interfaces by name.
          if (std::get<FailedCast<ToFeatureList>>(castIt->second.failed).value)
          {
            return nullptr;
          }
        }
 
        auto reqEntity = this->Get(_entity);
        if (nullptr == reqEntity)
        {
          return nullptr;
        }
 
        // Cast
        auto castEntity = physics::RequestFeatures<ToFeatureList>::From(
            this->Get(_entity));
 
        auto &cached = this->castCache[_entity];
        if (castEntity)
        {
          std::get<ToEntityPtr>(cached.entities) = castEntity;
        }
        else
        {
          std::get<FailedCast<ToFeatureList>>(cached.failed).value = true;
        }
 
        return castEntity;
      }
    }
 
    public: template <typename ToFeatureList>
            PhysicsEntityPtr<ToFeatureList>
            EntityCast(const RequiredEntityPtr &_physicsEntity) const
    {
      auto gzEntity = this->Get(_physicsEntity);
      if (kNullEntity == gzEntity)
      {
        return nullptr;
      }
      return this->EntityCast<ToFeatureList>(gzEntity);
    }
 
    public: RequiredEntityPtr Get(const Entity &_entity) const
    {
      auto it = this->entityMap.find(_entity);
      if (it != this->entityMap.end())
      {
        return it->second;
      }
      return nullptr;
    }
 
    public: Entity Get(const RequiredEntityPtr &_physEntity) const
    {
      auto it = this->reverseMap.find(_physEntity);
      if (it != this->reverseMap.end())
      {
        return it->second;
      }
      return kNullEntity;
    }
 
    public: RequiredEntityPtr GetPhysicsEntityPtr(std::size_t _id) const
    {
      auto it = this->physEntityById.find(_id);
      if (it != this->physEntityById.end())
      {
        return it->second;
      }
      return nullptr;
    }
 
    public: Entity GetByPhysicsId(std::size_t _id) const
    {
      auto it = this->entityByPhysId.find(_id);
      if (it != this->entityByPhysId.end())
      {
        return it->second;
      }
      return kNullEntity;
    }
 
    public: bool HasEntity(const Entity &_entity) const
    {
      return this->entityMap.find(_entity) != this->entityMap.end();
    }
 
    public: bool HasEntity(const RequiredEntityPtr &_physicsEntity) const
    {
      return this->reverseMap.find(_physicsEntity) != this->reverseMap.end();
    }
 
    public: void AddEntity(const Entity &_entity,
                           const RequiredEntityPtr &_physicsEntity)
    {
      this->entityMap[_entity] = _physicsEntity;
      this->reverseMap[_physicsEntity] = _entity;
      this->physEntityById[_physicsEntity->EntityID()] = _physicsEntity;
      this->entityByPhysId[_physicsEntity->EntityID()] = _entity;
    }
 
    public: bool Remove(Entity _entity)
    {
      auto it = this->entityMap.find(_entity);
      if (it != this->entityMap.end())
      {
        this->reverseMap.erase(it->second);
        this->physEntityById.erase(it->second->EntityID());
        this->entityByPhysId.erase(it->second->EntityID());
        this->castCache.erase(_entity);
        this->entityMap.erase(it);
        return true;
      }
      return false;
    }
 
    public: bool Remove(const RequiredEntityPtr &_physicsEntity)
    {
      auto it = this->reverseMap.find(_physicsEntity);
      if (it != this->reverseMap.end())
      {
        this->entityMap.erase(it->second);
        this->physEntityById.erase(it->first->EntityID());
        this->entityByPhysId.erase(it->first->EntityID());
        this->castCache.erase(it->second);
        this->reverseMap.erase(it);
        return true;
      }
      return false;
    }
 
    public: const std::unordered_map<Entity, RequiredEntityPtr> &Map() const
    {
      return this->entityMap;
    }
 
    public: std::size_t TotalMapEntryCount() const
    {
      return this->entityMap.size() + this->reverseMap.size() +
             this->castCache.size() + this->physEntityById.size() +
             this->entityByPhysId.size();
    }
 
    private: std::unordered_map<Entity, RequiredEntityPtr> entityMap;
 
    private: std::unordered_map<RequiredEntityPtr, Entity> reverseMap;
 
    private: std::unordered_map<std::size_t, RequiredEntityPtr> physEntityById;
 
    private: std::unordered_map<std::size_t, Entity> entityByPhysId;
 
    private: template <typename T>
             struct FailedCast
             {
               bool value{false};
             };
 
    private: struct CastCacheEntry
             {
               ValueType entities;
               std::tuple<FailedCast<OptionalFeatureLists>...> failed;
             };
 
    private: mutable std::unordered_map<Entity, CastCacheEntry> castCache;
  };
 
  template <template <typename, typename> class PhysicsEntityT,
            typename RequiredFeatureList, typename... OptionalFeatureLists>
  using EntityFeatureMap3d =
      EntityFeatureMap<PhysicsEntityT, physics::FeaturePolicy3d,
                       RequiredFeatureList, OptionalFeatureLists...>;
}
}
}
#endif