Gazebo Gazebo

API Reference

6.16.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 GZ_GAZEBO_SYSTEMS_PHYSICS_ENTITY_FEATURE_MAP_HH_
19 #define GZ_GAZEBO_SYSTEMS_PHYSICS_ENTITY_FEATURE_MAP_HH_
20 
21 #include <tuple>
22 #include <type_traits>
23 #include <unordered_map>
24 
25 #include <gz/physics/Entity.hh>
26 #include <gz/physics/FindFeatures.hh>
27 #include <gz/physics/FeatureList.hh>
28 #include <gz/physics/RequestFeatures.hh>
29 
30 #include "gz/sim/Entity.hh"
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(gz::sim::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>
150  PhysicsEntityPtr<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 
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 
210  public: Entity GetByPhysicsId(std::size_t _id) const
211  {
212  auto it = this->entityByPhysId.find(_id);
213  if (it != this->entityByPhysId.end())
214  {
215  return it->second;
216  }
217  return kNullEntity;
218  }
219 
225  public: bool HasEntity(const Entity &_entity) const
226  {
227  return this->entityMap.find(_entity) != this->entityMap.end();
228  }
229 
235  public: bool HasEntity(const RequiredEntityPtr &_physicsEntity) const
236  {
237  return this->reverseMap.find(_physicsEntity) != this->reverseMap.end();
238  }
239 
243  public: void AddEntity(const Entity &_entity,
244  const RequiredEntityPtr &_physicsEntity)
245  {
246  this->entityMap[_entity] = _physicsEntity;
247  this->reverseMap[_physicsEntity] = _entity;
248  this->physEntityById[_physicsEntity->EntityID()] = _physicsEntity;
249  this->entityByPhysId[_physicsEntity->EntityID()] = _entity;
250  }
251 
255  public: bool Remove(Entity _entity)
256  {
257  auto it = this->entityMap.find(_entity);
258  if (it != this->entityMap.end())
259  {
260  this->reverseMap.erase(it->second);
261  this->physEntityById.erase(it->second->EntityID());
262  this->entityByPhysId.erase(it->second->EntityID());
263  this->castCache.erase(_entity);
264  this->entityMap.erase(it);
265  return true;
266  }
267  return false;
268  }
269 
273  public: bool Remove(const RequiredEntityPtr &_physicsEntity)
274  {
275  auto it = this->reverseMap.find(_physicsEntity);
276  if (it != this->reverseMap.end())
277  {
278  this->entityMap.erase(it->second);
279  this->physEntityById.erase(it->first->EntityID());
280  this->entityByPhysId.erase(it->first->EntityID());
281  this->castCache.erase(it->second);
282  this->reverseMap.erase(it);
283  return true;
284  }
285  return false;
286  }
287 
292  {
293  return this->entityMap;
294  }
295 
300  {
301  return this->entityMap.size() + this->reverseMap.size() +
302  this->castCache.size() + this->physEntityById.size() +
303  this->entityByPhysId.size();
304  }
305 
308 
311 
315 
317  private: std::unordered_map<std::size_t, Entity> entityByPhysId;
318 
321  private: mutable std::unordered_map<Entity, ValueType> castCache;
322  };
323 
326  template <template <typename, typename> class PhysicsEntityT,
327  typename RequiredFeatureList, typename... OptionalFeatureLists>
328  using EntityFeatureMap3d =
329  EntityFeatureMap<PhysicsEntityT, physics::FeaturePolicy3d,
330  RequiredFeatureList, OptionalFeatureLists...>;
331 }
332 }
333 }
334 #endif
uint64_t Entity
An Entity identifies a single object in simulation such as a model, link, or light....
Definition: gz/sim/Entity.hh:58
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:291
T find(T... args)
T size(T... args)
bool Remove(Entity _entity)
Remove entity from all associated maps.
Definition: EntityFeatureMap.hh:255
RequiredEntityPtr Get(const Entity &_entity) const
Get the physics entity with required features that corresponds to the input Gazebo entity.
Definition: EntityFeatureMap.hh:166
PhysicsEntityPtr< ToFeatureList > EntityCast(gz::sim::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
EntityFeatureMap< PhysicsEntityT, physics::FeaturePolicy3d, RequiredFeatureList, OptionalFeatureLists... > EntityFeatureMap3d
Convenience template that presets EntityFeatureMap with FeaturePolicy3d.
Definition: EntityFeatureMap.hh:330
const Entity kNullEntity
Indicates a non-existant or invalid Entity.
Definition: gz/sim/Entity.hh:61
Gazebo is a leading open source robotics simulator, that provides high fidelity physics,...
Definition: gz/sim/Actor.hh:35
std::disjunction< std::is_same< T, OptionalFeatureLists >... > HasFeatureList
Checks whether type T is in the OptionalFeatureLists.
Definition: EntityFeatureMap.hh:77
T erase(T... args)
std::size_t TotalMapEntryCount() const
Get the total number of entries in the maps. Only used for testing.
Definition: EntityFeatureMap.hh:299
void AddEntity(const Entity &_entity, const RequiredEntityPtr &_physicsEntity)
Add a mapping between gazebo and physics entities.
Definition: EntityFeatureMap.hh:243
Entity GetByPhysicsId(std::size_t _id) const
Get Gazebo entity that corresponds to the physics entity ID.
Definition: EntityFeatureMap.hh:210
bool Remove(const RequiredEntityPtr &_physicsEntity)
Remove physics entity from all associated maps.
Definition: EntityFeatureMap.hh:273
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
physics::EntityPtr< PhysicsEntityT< PolicyT, T > > PhysicsEntityPtr
Template that's preset with the class's Policy type.
Definition: EntityFeatureMap.hh:68
bool HasEntity(const Entity &_entity) const
Check whether there is a physics entity associated with the given Gazebo entity.
Definition: EntityFeatureMap.hh:225
T end(T... args)
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
bool HasEntity(const RequiredEntityPtr &_physicsEntity) const
Check whether there is a gazebo entity associated with the given physics entity.
Definition: EntityFeatureMap.hh:235