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_SIM_SYSTEMS_PHYSICS_ENTITY_FEATURE_MAP_HH_
19#define GZ_SIM_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>
29
30#include "gz/sim/Entity.hh"
31
32namespace gz::sim
33{
34inline namespace GZ_SIM_VERSION_NAMESPACE {
35namespace 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 gz-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>
69
72
75 public: template <typename T>
77 std::disjunction<std::is_same<T, OptionalFeatureLists>...>;
78
82 private: using ValueType =
85
95 public: template <typename ToFeatureList>
97 EntityCast(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
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
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>
330 RequiredFeatureList, OptionalFeatureLists...>;
331}
332}
333}
334#endif