Gazebo Gazebo

API Reference

3.15.1
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  }
148  public: template <typename ToFeatureList>
149  PhysicsEntityPtr<ToFeatureList>
150  EntityCast(const RequiredEntityPtr &_physicsEntity) const
151  {
152  auto gzEntity = this->Get(_physicsEntity);
153  if (kNullEntity == gzEntity)
154  {
155  return nullptr;
156  }
157  return this->EntityCast<ToFeatureList>(gzEntity);
158  }
159 
165  public: RequiredEntityPtr Get(const Entity &_entity) const
166  {
167  auto it = this->entityMap.find(_entity);
168  if (it != this->entityMap.end())
169  {
170  return it->second;
171  }
172  return nullptr;
173  }
174 
180  public: Entity Get(const RequiredEntityPtr &_physEntity) const
181  {
182  auto it = this->reverseMap.find(_physEntity);
183  if (it != this->reverseMap.end())
184  {
185  return it->second;
186  }
187  return kNullEntity;
188  }
189 
195  public: bool HasEntity(const Entity &_entity) const
196  {
197  return this->entityMap.find(_entity) != this->entityMap.end();
198  }
199 
205  public: bool HasEntity(const RequiredEntityPtr &_physicsEntity) const
206  {
207  return this->reverseMap.find(_physicsEntity) != this->reverseMap.end();
208  }
209 
213  public: void AddEntity(const Entity &_entity,
214  const RequiredEntityPtr &_physicsEntity)
215  {
216  this->entityMap[_entity] = _physicsEntity;
217  this->reverseMap[_physicsEntity] = _entity;
218  }
219 
223  public: bool Remove(Entity _entity)
224  {
225  auto it = this->entityMap.find(_entity);
226  if (it != this->entityMap.end())
227  {
228  this->reverseMap.erase(it->second);
229  this->castCache.erase(_entity);
230  this->entityMap.erase(it);
231  return true;
232  }
233  return false;
234  }
235 
239  public: bool Remove(const RequiredEntityPtr &_physicsEntity)
240  {
241  auto it = this->reverseMap.find(_physicsEntity);
242  if (it != this->reverseMap.end())
243  {
244  this->entityMap.erase(it->second);
245  this->castCache.erase(it->second);
246  this->reverseMap.erase(it);
247  return true;
248  }
249  return false;
250  }
251 
256  {
257  return this->entityMap;
258  }
259 
264  {
265  return this->entityMap.size() + this->reverseMap.size() +
266  this->castCache.size();
267  }
268 
271 
274 
277  private: mutable std::unordered_map<Entity, ValueType> castCache;
278  };
279 
282  template <template <typename, typename> class PhysicsEntityT,
283  typename RequiredFeatureList, typename... OptionalFeatureLists>
284  using EntityFeatureMap3d =
285  EntityFeatureMap<PhysicsEntityT, physics::FeaturePolicy3d,
286  RequiredFeatureList, OptionalFeatureLists...>;
287 }
288 }
289 }
290 #endif
uint64_t Entity
An Entity identifies a single object in simulation such as a model, link, or light....
Definition: gz/sim/Entity.hh:59
Entity Get(const RequiredEntityPtr &_physEntity) const
Get Gazebo entity that corresponds to the physics entity with required features.
Definition: EntityFeatureMap.hh:180
const std::unordered_map< Entity, RequiredEntityPtr > & Map() const
Get the map from Gazebo entity to physics entities with required features.
Definition: EntityFeatureMap.hh:255
T find(T... args)
T size(T... args)
bool Remove(Entity _entity)
Remove entity from all associated maps.
Definition: EntityFeatureMap.hh:223
RequiredEntityPtr Get(const Entity &_entity) const
Get the physics entity with required features that corresponds to the input Gazebo entity.
Definition: EntityFeatureMap.hh:165
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:286
const Entity kNullEntity
Indicates a non-existant or invalid Entity.
Definition: gz/sim/Entity.hh:62
Gazebo is a leading open source robotics simulator, that provides high fidelity physics,...
Definition: gz/sim/components/Actor.hh:32
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 three maps. Only used for testing.
Definition: EntityFeatureMap.hh:263
void AddEntity(const Entity &_entity, const RequiredEntityPtr &_physicsEntity)
Add a mapping between gazebo and physics entities.
Definition: EntityFeatureMap.hh:213
bool Remove(const RequiredEntityPtr &_physicsEntity)
Remove physics entity from all associated maps.
Definition: EntityFeatureMap.hh:239
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:150
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:195
T end(T... args)
PhysicsEntityPtr< RequiredFeatureList > RequiredEntityPtr
A EntityPtr with made from RequiredFeatureList.
Definition: EntityFeatureMap.hh:71
bool HasEntity(const RequiredEntityPtr &_physicsEntity) const
Check whether there is a gazebo entity associated with the given physics entity.
Definition: EntityFeatureMap.hh:205