Ignition Gazebo

API Reference

5.0.0
EntityFeatureMap.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2021 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16  */
17 
18 #ifndef IGNITION_GAZEBO_SYSTEMS_PHYSICS_ENTITY_FEATURE_MAP_HH_
19 #define IGNITION_GAZEBO_SYSTEMS_PHYSICS_ENTITY_FEATURE_MAP_HH_
20 
21 #include <tuple>
22 #include <type_traits>
23 #include <unordered_map>
24 
25 #include <ignition/physics/Entity.hh>
26 #include <ignition/physics/FindFeatures.hh>
27 #include <ignition/physics/FeatureList.hh>
28 #include <ignition/physics/RequestFeatures.hh>
29 
31 
32 namespace ignition::gazebo
33 {
34 inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
35 namespace systems::physics_system
36 {
37  // \brief Helper class that associates Gazebo entities with Physics entities
38  // with required and optional features. It can be used to cast a physics
39  // entity with the required features to another physics entity with one of
40  // the optional features. This class was created to keep all physics entities
41  // in one place so that when a gazebo entity is removed, all the mapped
42  // physics entitities can be removed at the same time. This ensures that
43  // reference counts are properly zeroed out in the underlying physics engines
44  // and the memory associated with the physics entities can be freed.
45  //
46  // DEV WARNING: There is an implicit conversion between physics EntityPtr and
47  // std::size_t in ign-physics. This seems also implicitly convert between
48  // EntityPtr and gazebo Entity. Therefore, any member function that takes a
49  // gazebo Entity can accidentally take an EntityPtr. To prevent this, for
50  // every function that takes a gazebo Entity, we should always have an
51  // overload that also takes an EntityPtr with required features. We can do
52  // this because there's a 1:1 mapping between the two in maps contained in
53  // this class.
54  //
55  // \tparam PhysicsEntityT Type of entity, such as World, Model, or Link
56  // \tparam PolicyT Policy of the physics engine (2D, 3D)
57  // \tparam RequiredFeatureList Required features of the physics entity
58  // \tparam OptionalFeatureLists One or more optional feature lists of the
59  // physics entity.
60  template <template <typename, typename> class PhysicsEntityT,
61  typename PolicyT, typename RequiredFeatureList,
62  typename... OptionalFeatureLists>
64  {
67  public: template <typename T>
68  using PhysicsEntityPtr = physics::EntityPtr<PhysicsEntityT<PolicyT, T>>;
69 
72 
75  public: template <typename T>
76  using HasFeatureList =
77  std::disjunction<std::is_same<T, OptionalFeatureLists>...>;
78 
82  private: using ValueType =
85 
95  public: template <typename ToFeatureList>
97  EntityCast(gazebo::Entity _entity) const
98  {
99  // Using constexpr to limit compiler error message to the static_assert
100  // cppcheck-suppress syntaxError
101  if constexpr (!HasFeatureList<ToFeatureList>::value) // NOLINT
102  {
104  "Trying to cast to a FeatureList not included in the "
105  "optional FeatureLists of this map.");
106  return nullptr;
107  }
108  else
109  {
110  using ToEntityPtr = PhysicsEntityPtr<ToFeatureList>;
111  // Has already been cast
112  auto castIt = this->castCache.find(_entity);
113  if (castIt != this->castCache.end())
114  {
115  auto castEntity = std::get<ToEntityPtr>(castIt->second);
116  if (nullptr != castEntity)
117  {
118  return castEntity;
119  }
120  }
121 
122  auto reqEntity = this->Get(_entity);
123  if (nullptr == reqEntity)
124  {
125  return nullptr;
126  }
127 
128  // Cast
129  auto castEntity = physics::RequestFeatures<ToFeatureList>::From(
130  this->Get(_entity));
131 
132  if (castEntity)
133  {
134  std::get<ToEntityPtr>(this->castCache[_entity]) = castEntity;
135  }
136 
137  return castEntity;
138  }
139  }
140 
149  public: template <typename ToFeatureList>
151  EntityCast(const RequiredEntityPtr &_physicsEntity) const
152  {
153  auto gzEntity = this->Get(_physicsEntity);
154  if (kNullEntity == gzEntity)
155  {
156  return nullptr;
157  }
158  return this->EntityCast<ToFeatureList>(gzEntity);
159  }
160 
166  public: RequiredEntityPtr Get(const Entity &_entity) const
167  {
168  auto it = this->entityMap.find(_entity);
169  if (it != this->entityMap.end())
170  {
171  return it->second;
172  }
173  return nullptr;
174  }
175 
181  public: Entity Get(const RequiredEntityPtr &_physEntity) const
182  {
183  auto it = this->reverseMap.find(_physEntity);
184  if (it != this->reverseMap.end())
185  {
186  return it->second;
187  }
188  return kNullEntity;
189  }
190 
196  public: RequiredEntityPtr GetPhysicsEntityPtr(std::size_t _id) const
197  {
198  auto it = this->physEntityById.find(_id);
199  if (it != this->physEntityById.end())
200  {
201  return it->second;
202  }
203  return nullptr;
204  }
205 
211  public: bool HasEntity(const Entity &_entity) const
212  {
213  return this->entityMap.find(_entity) != this->entityMap.end();
214  }
215 
221  public: bool HasEntity(const RequiredEntityPtr &_physicsEntity) const
222  {
223  return this->reverseMap.find(_physicsEntity) != this->reverseMap.end();
224  }
225 
229  public: void AddEntity(const Entity &_entity,
230  const RequiredEntityPtr &_physicsEntity)
231  {
232  this->entityMap[_entity] = _physicsEntity;
233  this->reverseMap[_physicsEntity] = _entity;
234  this->physEntityById[_physicsEntity->EntityID()] = _physicsEntity;
235  }
236 
240  public: bool Remove(Entity _entity)
241  {
242  auto it = this->entityMap.find(_entity);
243  if (it != this->entityMap.end())
244  {
245  this->reverseMap.erase(it->second);
246  this->physEntityById.erase(it->second->EntityID());
247  this->castCache.erase(_entity);
248  this->entityMap.erase(it);
249  return true;
250  }
251  return false;
252  }
253 
257  public: bool Remove(const RequiredEntityPtr &_physicsEntity)
258  {
259  auto it = this->reverseMap.find(_physicsEntity);
260  if (it != this->reverseMap.end())
261  {
262  this->entityMap.erase(it->second);
263  this->physEntityById.erase(it->first->EntityID());
264  this->castCache.erase(it->second);
265  this->reverseMap.erase(it);
266  return true;
267  }
268  return false;
269  }
270 
275  {
276  return this->entityMap;
277  }
278 
283  {
284  return this->entityMap.size() + this->reverseMap.size() +
285  this->castCache.size() + this->physEntityById.size();
286  }
287 
290 
293 
297 
300  private: mutable std::unordered_map<Entity, ValueType> castCache;
301  };
302 
305  template <template <typename, typename> class PhysicsEntityT,
306  typename RequiredFeatureList, typename... OptionalFeatureLists>
307  using EntityFeatureMap3d =
308  EntityFeatureMap<PhysicsEntityT, physics::FeaturePolicy3d,
309  RequiredFeatureList, OptionalFeatureLists...>;
310 }
311 }
312 }
313 #endif
PhysicsEntityPtr< RequiredFeatureList > RequiredEntityPtr
A EntityPtr with made from RequiredFeatureList.
Definition: EntityFeatureMap.hh:71
RequiredEntityPtr GetPhysicsEntityPtr(std::size_t _id) const
Get the physics entity with required features that has a particular ID.
Definition: EntityFeatureMap.hh:196
PhysicsEntityPtr< ToFeatureList > EntityCast(gazebo::Entity _entity) const
Helper function to cast from an entity type with minimum features to an entity with a different set o...
Definition: EntityFeatureMap.hh:97
physics::EntityPtr< PhysicsEntityT< PolicyT, T > > PhysicsEntityPtr
Template that&#39;s preset with the class&#39;s Policy type.
Definition: EntityFeatureMap.hh:68
std::size_t TotalMapEntryCount() const
Get the total number of entries in the maps. Only used for testing.
Definition: EntityFeatureMap.hh:282
Gazebo is a leading open source robotics simulator, that provides high fidelity physics, rendering, and sensor simulation.
Definition: Actor.hh:36
bool HasEntity(const Entity &_entity) const
Check whether there is a physics entity associated with the given Gazebo entity.
Definition: EntityFeatureMap.hh:211
PhysicsEntityPtr< ToFeatureList > EntityCast(const RequiredEntityPtr &_physicsEntity) const
Helper function to cast from an entity type with minimum features to an entity with a different set o...
Definition: EntityFeatureMap.hh:151
bool HasEntity(const RequiredEntityPtr &_physicsEntity) const
Check whether there is a gazebo entity associated with the given physics entity.
Definition: EntityFeatureMap.hh:221
bool Remove(const RequiredEntityPtr &_physicsEntity)
Remove physics entity from all associated maps.
Definition: EntityFeatureMap.hh:257
const Entity kNullEntity
Indicates a non-existant or invalid Entity.
Definition: Entity.hh:62
std::disjunction< std::is_same< T, OptionalFeatureLists >... > HasFeatureList
Checks whether type T is in the OptionalFeatureLists.
Definition: EntityFeatureMap.hh:77
Entity Get(const RequiredEntityPtr &_physEntity) const
Get Gazebo entity that corresponds to the physics entity with required features.
Definition: EntityFeatureMap.hh:181
const std::unordered_map< Entity, RequiredEntityPtr > & Map() const
Get the map from Gazebo entity to physics entities with required features.
Definition: EntityFeatureMap.hh:274
bool Remove(Entity _entity)
Remove entity from all associated maps.
Definition: EntityFeatureMap.hh:240
void AddEntity(const Entity &_entity, const RequiredEntityPtr &_physicsEntity)
Add a mapping between gazebo and physics entities.
Definition: EntityFeatureMap.hh:229
RequiredEntityPtr Get(const Entity &_entity) const
Get the physics entity with required features that corresponds to the input Gazebo entity...
Definition: EntityFeatureMap.hh:166
uint64_t Entity
An Entity identifies a single object in simulation such as a model, link, or light. At its core, an Entity is just an identifier.
Definition: Entity.hh:59