diff --git a/Migration.md b/Migration.md index 745f1f219e..cef3cdea66 100644 --- a/Migration.md +++ b/Migration.md @@ -17,6 +17,33 @@ release will remove the deprecated code. * `dynamic_pose/info` topic is removed from `LogRecord` and `LogPlayback` since pose information is being logged in the `changed_state` topic. +* The internal management of entities and components in the + `EntityComponentManager` has been updated to improve runtime performance. As a + result, several methods have been deprecated, and a few types have changed. + * **Deprecated**: + + All `EntityComponentManager` methods that use `ComponentKey` as an input + parameter. + + The `EntityComponentManager::First` method. + + The `ComponentId` and `ComponentKey` types are now deprecated. A + combination of `Entity` and `ComponentTypeId` should be used instead. + + The `components::StorageDescriptorBase` and + `components::StorageDescriptor` classes. + + Methods in `components::Factory` that have deprecated input parameter + types and/or deprecated return types. + - The version of `components::Factory::Register` which has a + `StorageDescriptorBase *` input parameter. + - `components::Factory::NewStorage` + + The `ComponentStorageBase` and `ComponentStorage` classes. + * **Modified**: + + `EntityComponentManager::CreateComponent` now returns a pointer to the + created component instead of a `ComponentKey`. + + `ComponentKey` has been modified to be a + `std::pair` (it used to be a + `std::pair`) since the `ComponentId` type + is now deprecated. `ComponentKey` has also been deprecated, so usage of + this type is discouraged (see the **Deprecated** section above for more + information about how to replace usage of `ComponentKey`). + ## Ignition Gazebo 4.x to 5.x * Use `cli` component of `ignition-utils1`. diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index be943a1669..3dce1196dc 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -129,7 +129,7 @@ namespace ignition /// \param[in] _entity The entity to check. /// \param[in] _key The component to check. /// \return True if the component key belongs to the entity. - public: bool EntityHasComponent(const Entity _entity, + public: bool IGN_DEPRECATED(6) EntityHasComponent(const Entity _entity, const ComponentKey &_key) const; /// \brief Check whether an entity has a specific component type. @@ -152,7 +152,7 @@ namespace ignition /// \param[in] _key A key that uniquely identifies a component. /// \return True if the entity and component existed and the component was /// removed. - public: bool RemoveComponent( + public: bool IGN_DEPRECATED(6) RemoveComponent( const Entity _entity, const ComponentKey &_key); /// \brief Remove a component from an entity based on a type id. @@ -180,9 +180,12 @@ namespace ignition /// \param[in] _entity The entity that will be associated with /// the component. /// \param[in] _data Data used to construct the component. - /// \return Key that uniquely identifies the component. + /// \return A pointer to the component that was created. nullptr is + /// returned if the component was not able to be created. If _entity + /// does not exist, nullptr will be returned. public: template - ComponentKey CreateComponent(const Entity _entity, + ComponentTypeT *CreateComponent( + const Entity _entity, const ComponentTypeT &_data); /// \brief Get a component assigned to an entity based on a @@ -206,14 +209,16 @@ namespace ignition /// \return The component associated with the key, or nullptr if the /// component could not be found. public: template - const ComponentTypeT *Component(const ComponentKey &_key) const; + const ComponentTypeT IGN_DEPRECATED(6) * Component( + const ComponentKey &_key) const; /// \brief Get a mutable component based on a key. /// \param[in] _key A key that uniquely identifies a component. /// \return The component associated with the key, or nullptr if the /// component could not be found. public: template - ComponentTypeT *Component(const ComponentKey &_key); + ComponentTypeT IGN_DEPRECATED(6) * Component( + const ComponentKey &_key); /// \brief Get a mutable component assigned to an entity based on a /// component type. If the component doesn't exist, create it and @@ -222,7 +227,7 @@ namespace ignition /// \param[in] _default The value that should be used to construct /// the component in case the component doesn't exist. /// \return The component of the specified type assigned to the specified - /// entity. + /// entity. If _entity does not exist, nullptr is returned. public: template ComponentTypeT *ComponentDefault(Entity _entity, const typename ComponentTypeT::Type &_default = @@ -261,20 +266,20 @@ namespace ignition Entity _entity) const; /// \brief The first component instance of the specified type. - /// \return First component instance of the specified type, or nullptr - /// if the type does not exist. + /// This function is now deprecated, and will always return nullptr. + /// \return nullptr. public: template - const ComponentTypeT *First() const; + const ComponentTypeT IGN_DEPRECATED(6) * First() const; /// \brief The first component instance of the specified type. - /// \return First component instance of the specified type, or nullptr - /// if the type does not exist. + /// This function is now deprecated, and will always return nullptr. + /// \return nullptr. public: template - ComponentTypeT *First(); + ComponentTypeT IGN_DEPRECATED(6) * First(); /// \brief Get an entity which matches the value of all the given /// components. For example, the following will return the entity which - /// has an name component equal to "name" and has a model component: + /// has a name component equal to "name" and has a model component: /// /// auto entity = EntityByComponents(components::Name("name"), /// components::Model()); @@ -523,6 +528,7 @@ namespace ignition /// and components. /// \detail The header of the message will not be populated, it is the /// responsibility of the caller to timestamp it before use. + /// \param[out] _state The serialized state message to populate. /// \param[in] _entities Entities to be serialized. Leave empty to get /// all entities. /// \param[in] _types Type ID of components to be serialized. Leave empty @@ -611,24 +617,14 @@ namespace ignition /// \return True if the Entity has been marked to be removed. private: bool IsMarkedForRemoval(const Entity _entity) const; - /// \brief Delete an existing Entity. - /// \param[in] _entity The entity to remove. - /// \returns True if the Entity existed and was deleted. - private: bool RemoveEntity(const Entity _entity); - - /// \brief The first component instance of the specified type. - /// \return First component instance of the specified type, or nullptr - /// if the type does not exist. - private: components::BaseComponent *First( - const ComponentTypeId _componentTypeId); - /// \brief Implementation of CreateComponent. /// \param[in] _entity The entity that will be associated with /// the component. /// \param[in] _componentTypeId Id of the component type. /// \param[in] _data Data used to construct the component. - /// \return Key that uniquely identifies the component. - private: ComponentKey CreateComponentImplementation( + /// \return True if the component's data needs to be set externally; false + /// otherwise. + private: bool CreateComponentImplementation( const Entity _entity, const ComponentTypeId _componentTypeId, const components::BaseComponent *_data); @@ -651,79 +647,29 @@ namespace ignition const Entity _entity, const ComponentTypeId _type); - /// \brief Get a component based on a key. - /// \param[in] _key A key that uniquely identifies a component. - /// \return The component associated with the key, or nullptr if the - /// component could not be found. - private: const components::BaseComponent *ComponentImplementation( - const ComponentKey &_key) const; - - /// \brief Get a mutable component based on a key. - /// \param[in] _key A key that uniquely identifies a component. - /// \return The component associated with the key, or nullptr if the - /// component could not be found. - private: components::BaseComponent *ComponentImplementation( - const ComponentKey &_key); - - /// \brief End of the AddComponentToView recursion. This function is - /// called when Rest is empty. - /// \param[in, out] _view The FirstComponent will be added to the - /// _view. - /// \param[in] _entity The entity. - private: template::type = 0> - void AddComponentsToView(detail::View &_view, - const Entity _entity) const; - - /// \brief Recursively add components to a view. This function is - /// called when Rest is NOT empty. - /// \param[in, out] _view The FirstComponent will be added to the - /// _view. - /// \param[in] _entity The entity. - private: template::type = 0> - void AddComponentsToView(detail::View &_view, - const Entity _entity) const; - /// \brief Find a View that matches the set of ComponentTypeIds. If /// a match is not found, then a new view is created. /// \tparam ComponentTypeTs All the component types that define a view. - /// \return A reference to the view. + /// \return A pointer to the view. private: template - detail::View &FindView() const; + detail::View *FindView() const; /// \brief Find a view based on the provided component type ids. /// \param[in] _types The component type ids that serve as a key into /// a map of views. - /// \param[out] _iter Iterator to the found element in the view map. - /// Check the return value to see if this iterator is valid. - /// \return True if the view was found, false otherwise. - private: bool FindView(const std::set &_types, - std::map::iterator &_iter) const; // NOLINT + /// \return A pointer to the view. nullptr is returned if the view wasn't + /// found. + private: detail::BaseView *FindView( + const std::vector &_types) const; /// \brief Add a new view to the set of stored views. - /// \param[in] _types The set of component type ids that is the key + /// \param[in] _types The set of component type ids that act as the key /// for the view. /// \param[in] _view The view to add. - /// \return An iterator to the view. - private: std::map::iterator - AddView(const std::set &_types, - detail::View &&_view) const; - - /// \brief Update views that contain the provided entity. - /// \param[in] _entity The entity. - private: void UpdateViews(const Entity _entity); - - /// \brief Get a component ID based on an entity and the component's type. - /// \param[in] _entity The entity. - /// \param[in] _type Component type ID. - private: ComponentId EntityComponentIdFromType( - const Entity _entity, const ComponentTypeId _type) const; + /// \return A pointer to the view. + private: detail::BaseView *AddView( + const detail::ComponentTypeKey &_types, + std::unique_ptr _view) const; /// \brief Add an entity and its components to a serialized state message. /// \param[out] _msg The state message. @@ -761,10 +707,6 @@ namespace ignition // states. Like the runners, the managers are internal. friend class NetworkManagerPrimary; friend class NetworkManagerSecondary; - - // Make View a friend so that it can access components. - // This should be safe since View is internal to Gazebo. - friend class detail::View; }; } } diff --git a/include/ignition/gazebo/Types.hh b/include/ignition/gazebo/Types.hh index 84026a0c9e..67dbe2ea3b 100644 --- a/include/ignition/gazebo/Types.hh +++ b/include/ignition/gazebo/Types.hh @@ -22,6 +22,8 @@ #include #include +#include "ignition/gazebo/Entity.hh" + namespace ignition { namespace gazebo @@ -78,6 +80,8 @@ namespace ignition /// \brief A unique identifier for a component instance. The uniqueness /// of a ComponentId is scoped to the component's type. /// \sa ComponentKey. + /// \deprecated Deprecated on version 6, removed on version 7. Use + /// ComponentTypeId + Entity instead. using ComponentId = int; /// \brief A unique identifier for a component type. A component type @@ -87,7 +91,10 @@ namespace ignition /// \brief A key that uniquely identifies, at the global scope, a component /// instance - using ComponentKey = std::pair; + /// \note On version 6, the 2nd element was changed to the entity ID. + /// \deprecated Deprecated on version 6, removed on version 7. Use + /// ComponentTypeId + Entity instead. + using ComponentKey = std::pair; /// \brief typedef for query callbacks using EntityQueryCallback = std::function Create() const = 0; + + /// \brief Create an instance of a Component, populated with specific data. + /// \param[in] _data The data to populate the component with. + /// \return Pointer to a component. + public: virtual std::unique_ptr Create( + const components::BaseComponent *_data) const = 0; }; /// \brief A class for an object responsible for creating components. @@ -57,17 +63,27 @@ namespace components class ComponentDescriptor : public ComponentDescriptorBase { - /// \brief Create an instance of a ComponentTypeT Component. - /// \return Pointer to a component. + /// \brief Documentation inherited public: std::unique_ptr Create() const override { return std::make_unique(); } + + /// \brief Documentation inherited + public: virtual std::unique_ptr Create( + const components::BaseComponent *_data) const override + { + ComponentTypeT comp(*static_cast(_data)); + return std::make_unique(comp); + } }; /// \brief A base class for an object responsible for creating storages. class StorageDescriptorBase { + /// \brief Constructor + public: IGN_DEPRECATED(6) StorageDescriptorBase() = default; + /// \brief Destructor public: virtual ~StorageDescriptorBase() = default; @@ -82,6 +98,9 @@ namespace components class StorageDescriptor : public StorageDescriptorBase { + /// \brief Constructor + public: IGN_DEPRECATED(6) StorageDescriptor() = default; + /// \brief Create an instance of a storage that holds ComponentTypeT /// components. /// \return Pointer to a component. @@ -100,12 +119,25 @@ namespace components /// \param[in] _type Type of component to register. /// \param[in] _compDesc Object to manage the creation of ComponentTypeT /// objects. - /// \param[in] _storageDesc Object to manage the creation of storages for - /// objects of type ComponentTypeT. + /// \param[in] _storageDesc Ignored. /// \tparam ComponentTypeT Type of component to register. + /// \deprecated See function that doesn't accept a storage public: template - void Register(const std::string &_type, ComponentDescriptorBase *_compDesc, - StorageDescriptorBase *_storageDesc) + void IGN_DEPRECATED(6) Register(const std::string &_type, + ComponentDescriptorBase *_compDesc, + StorageDescriptorBase * /*_storageDesc*/) + { + this->Register(_type, _compDesc); + } + + /// \brief Register a component so that the factory can create instances + /// of the component based on an ID. + /// \param[in] _type Type of component to register. + /// \param[in] _compDesc Object to manage the creation of ComponentTypeT + /// objects. + /// \tparam ComponentTypeT Type of component to register. + public: template + void Register(const std::string &_type, ComponentDescriptorBase *_compDesc) { // Every time a plugin which uses a component type is loaded, it attempts // to register it again, so we skip it. @@ -153,13 +185,12 @@ namespace components // Keep track of all types this->compsById[ComponentTypeT::typeId] = _compDesc; - this->storagesById[ComponentTypeT::typeId] = _storageDesc; namesById[ComponentTypeT::typeId] = ComponentTypeT::typeName; runtimeNamesById[ComponentTypeT::typeId] = runtimeName; } /// \brief Unregister a component so that the factory can't create instances - /// of the component or its storage anymore. + /// of the component anymore. /// \tparam ComponentTypeT Type of component to unregister. public: template void Unregister() @@ -170,7 +201,7 @@ namespace components } /// \brief Unregister a component so that the factory can't create instances - /// of the component or its storage anymore. + /// of the component anymore. /// \detail This function will not reset the `typeId` static variable within /// the component type itself. Prefer using the templated `Unregister` /// function when possible. @@ -192,15 +223,6 @@ namespace components } } - { - auto it = this->storagesById.find(_typeId); - if (it != this->storagesById.end()) - { - delete it->second; - this->storagesById.erase(it); - } - } - { auto it = namesById.find(_typeId); if (it != namesById.end()) @@ -245,19 +267,45 @@ namespace components return comp; } + /// \brief Create a new instance of a component, initialized with particular + /// data. + /// \param[in] _type Component id to create. + /// \param[in] _data The data to populate the component instance with. + /// \return Pointer to a component. Null if the component + /// type could not be handled. + public: std::unique_ptr New( + const ComponentTypeId &_type, const components::BaseComponent *_data) + { + std::unique_ptr comp; + + if (nullptr == _data) + { + ignerr << "Requested to create a new component instance with null " + << "data." << std::endl; + } + else if (_type != _data->TypeId()) + { + ignerr << "The typeID of _type [" << _type << "] does not match the " + << "typeID of _data [" << _data->TypeId() << "]." << std::endl; + } + else + { + auto it = this->compsById.find(_type); + if (it != this->compsById.end() && nullptr != it->second) + comp = it->second->Create(_data); + } + + return comp; + } + /// \brief Create a new instance of a component storage. /// \param[in] _typeId Type of component which the storage will hold. - /// \return Pointer to a storage. Null if the component type could not be - /// handled. - public: std::unique_ptr NewStorage( - const ComponentTypeId &_typeId) + /// \return Always returns nullptr. + /// \deprecated Storages aren't necessary anymore. + public: std::unique_ptr IGN_DEPRECATED(6) NewStorage( + const ComponentTypeId & /*_typeId*/) { - std::unique_ptr storage; - auto it = this->storagesById.find(_typeId); - if (it != this->storagesById.end() && nullptr != it->second) - storage = it->second->Create(); - - return storage; + return nullptr; } /// \brief Get all the registered component types by ID. @@ -305,10 +353,6 @@ namespace components /// we just keep a pointer, which will dangle until the program is shutdown. private: std::map compsById; - /// \brief A list of registered storages where the key is its component's - /// type id. - private: std::map storagesById; - /// \brief A list of IDs and their equivalent names. public: std::map namesById; @@ -335,9 +379,8 @@ namespace components return; \ using namespace ignition;\ using Desc = gazebo::components::ComponentDescriptor<_classname>; \ - using StorageDesc = gazebo::components::StorageDescriptor<_classname>; \ gazebo::components::Factory::Instance()->Register<_classname>(\ - _compType, new Desc(), new StorageDesc());\ + _compType, new Desc());\ } \ }; \ static IgnGazeboComponents##_classname\ diff --git a/include/ignition/gazebo/detail/BaseView.hh b/include/ignition/gazebo/detail/BaseView.hh new file mode 100644 index 0000000000..eea6755316 --- /dev/null +++ b/include/ignition/gazebo/detail/BaseView.hh @@ -0,0 +1,219 @@ +/* + * 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 IGNITION_GAZEBO_DETAIL_BASEVIEW_HH_ +#define IGNITION_GAZEBO_DETAIL_BASEVIEW_HH_ + +#include +#include +#include +#include + +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/Types.hh" +#include "ignition/gazebo/config.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace detail +{ +/// \brief A key into the map of views +using ComponentTypeKey = std::vector; + +/// \brief Hash functor for ComponentTypeKey. +/// The implementation was inspired by: +/// * https://stackoverflow.com/a/20511429 +/// * https://stackoverflow.com/a/4948967 +/// * https://stackoverflow.com/a/35991300 +/// * https://softwareengineering.stackexchange.com/a/402543 +struct ComponentTypeHasher +{ + std::size_t operator()(const std::vector &_vec) const + { + auto hash = _vec.size(); + for (const auto &i : _vec) + hash ^= i + 0x9e3779b9 + (hash << 6) + (hash >> 2); + return hash; + } +}; + +/// \brief A view is a cache to entities, and their components, that +/// match a set of component types. A cache is used because systems will +/// frequently, potentially every iteration, query the +/// EntityComponentManager for sets of entities that match a set of +/// component types. Rather than look up the entities every time, we can +/// use a cache to improve performance. +/// +/// Note that symbols for this class are visible because methods from this class +/// are used in templated Ignition::Gazebo::EntityComponentManager methods. +/// However, users should not use this class (or anything else in namespace +/// ignition::gazebo::detail) directly. +class IGNITION_GAZEBO_VISIBLE BaseView +{ + /// \brief Destructor + public: virtual ~BaseView() = default; + + /// \brief See if an entity is a part of the view + /// \param[in] _entity The entity + /// \return true if _entity is a part of the view, false otherwise + public: bool HasEntity(const Entity _entity) const; + + /// \brief See if an entity is marked as an entity to be added to the view + /// \param[in] _entity The entity + /// \return true if _entity is to be added to the view, false otherwise + public: bool IsEntityMarkedForAddition(const Entity _entity) const; + + /// \brief See if the view has cached the component data for an entity + /// \param[in] _entity The entity + /// \return true if _entity has component data cached in the view, false + /// otherwise + public: virtual bool HasCachedComponentData(const Entity _entity) const = 0; + + /// \brief Save an entity as one to be added to the view, if the entity isn't + /// already a part of the view. This entity's component data should be added + /// to the view the next time the view is being used. + /// \param[in] _entity The entity to be added + /// \param[in] _new Whether to add the entity to the list of new entities. + /// The new here is to indicate whether the entity is new to the entity + /// component manager. An existing entity can be added when creating a new + /// view or when rebuilding the view. + /// \return True if _entity isn't already a part of the view and was marked as + /// an entity to be added. False otherwise + /// \sa HasEntity, IsEntityMarkedForAddition + public: bool MarkEntityToAdd(const Entity _entity, bool _new = false); + + /// \brief See if the view requires a particular component type + /// \param[in] _typeId The component type + /// \return true if the view requires components of type _typeId, false + /// otherwise + public: bool RequiresComponent(const ComponentTypeId _typeId) const; + + /// \brief Update the internal data in the view because a component has been + /// added to an entity. It is assumed that the entity is already associated + /// with the view, and that the added component type is required by the view. + /// \param[in] _entity The entity + /// \param[in] _newEntity Whether to add the entity to the list of new + /// entities. The new here is to indicate whether the entity is new to the + /// entity component manager. + /// \param[in] _typeId The type of component that was added to _entity + /// \return true if the notification related to the component addition of type + /// _typeId occurred for _entity, false otherwise + /// \sa HasCachedComponentData, RequiresComponent + public: virtual bool NotifyComponentAddition(const Entity _entity, + bool _newEntity, const ComponentTypeId _typeId) = 0; + + /// \brief Update the internal data in the view because a component has been + /// removed to an entity. It is assumed that the entity is already associated + /// with the view, and that the removed component type is required by the view + /// \param[in] _entity The entity + /// \param[in] _typeId The type of component that was removed from _entity + /// \return true if the notification related to the component removal of type + /// _typeId occurred for _entity, false otherwise + /// \sa HasCachedComponentData, RequiresComponent + public: virtual bool NotifyComponentRemoval(const Entity _entity, + const ComponentTypeId _typeId) = 0; + + /// \brief Remove an entity from the view. + /// \param[in] _entity The entity to remove. + /// \return True if the entity was removed, false if the entity did not + /// exist in the view. + public: virtual bool RemoveEntity(const Entity _entity) = 0; + + /// \brief Add the entity to the list of entities to be removed + /// \param[in] _entity The entity to add. + /// \return True if the entity was added to the list, false if the entity + /// was not associated with the view. + public: bool MarkEntityToRemove(const Entity _entity); + + /// \brief Update the entities in the view to no longer appear as newly + /// created. This method should be called whenever a new simulation step is + /// about to take place. + public: void ResetNewEntityState(); + + /// \brief Get the set of component types that this view requires. + /// \return The set of component types. + public: const std::set &ComponentTypes() const; + + /// \brief Clear all data from the view and reset it to its original, empty + /// state. + public: virtual void Reset() = 0; + + /// \brief Get all of the entities in the view + /// \return The entities in the view + public: const std::set &Entities() const; + + /// \brief Get all of the entities in the view that are considered "newly + /// created". While an entity may be new to the view, it may not be a newly + /// created entity (perhaps this entity has existed for some time, and just + /// had a component added to it that now makes this entity a part of the + /// view). An entity's "newness" is determined by the entity component + /// manager. + /// \return The newly created entities that are a part of the view + public: const std::set &NewEntities() const; + + /// \brief Get all of the entities to be removed from the view + /// \return The entities to be removed from the view + public: const std::set &ToRemoveEntities() const; + + /// \brief Get all of the entities that should be added to the view. This is + /// useful for adding entities to the view before the view is used to ensure + /// that the view is up-to-date. Once all entities marked "to be added" are + /// added to the view, the ClearToAddEntities method should be called. + /// \return The entities to be added to the view, with the key of the map + /// indicating whether an entity is a newly created entity or not. + /// \sa ClearToAddEntities + public: const std::unordered_map &ToAddEntities() const; + + /// \brief Clear all of the entities that are marked as to be added to the + /// view. This should be called after all of the entities marked as to be + /// added to the view are actually added to the view. + /// \sa ToAddEntities + public: void ClearToAddEntities(); + + // TODO(adlarkin) make this a std::unordered_set for better performance. + // We need to make sure nothing else depends on the ordered preserved by + // std::set first + /// \brief All the entities that belong to this view. + protected: std::set entities; + + // TODO(adlarkin) make this a std::unordered_set for better performance. + // We need to make sure nothing else depends on the ordered preserved by + // std::set first + /// \brief List of newly created entities + protected: std::set newEntities; + + // TODO(adlarkin) make this a std::unordered_set for better performance. + // We need to make sure nothing else depends on the ordered preserved by + // std::set first + /// \brief List of entities about to be removed + protected: std::set toRemoveEntities; + + /// \brief List of entities to be added to the view. The value of the map + /// indicates whether the entity is new to the entity component manager or not + protected: std::unordered_map toAddEntities; + + /// \brief The component types in the view + protected: std::set componentTypes; +}; +} // namespace detail +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition +#endif diff --git a/include/ignition/gazebo/detail/ComponentStorageBase.hh b/include/ignition/gazebo/detail/ComponentStorageBase.hh index ca9e5e8960..cb4e0edcc2 100644 --- a/include/ignition/gazebo/detail/ComponentStorageBase.hh +++ b/include/ignition/gazebo/detail/ComponentStorageBase.hh @@ -17,12 +17,7 @@ #ifndef IGNITION_GAZEBO_DETAIL_COMPONENTSTORAGEBASE_HH_ #define IGNITION_GAZEBO_DETAIL_COMPONENTSTORAGEBASE_HH_ -#include -#include -#include -#include "ignition/gazebo/components/Component.hh" #include "ignition/gazebo/Export.hh" -#include "ignition/gazebo/Types.hh" namespace ignition { @@ -37,47 +32,10 @@ namespace ignition class IGNITION_GAZEBO_HIDDEN ComponentStorageBase { /// \brief Constructor - public: ComponentStorageBase() = default; + public: IGN_DEPRECATED(6) ComponentStorageBase() = default; /// \brief Destructor public: virtual ~ComponentStorageBase() = default; - - /// \brief Create a new component using the provided data. - /// \param[in] _data Data used to construct the component. - /// \return Id of the new component, and whether the components array - /// was expanded. kComponentIdInvalid is returned - /// if the component could not be created. - public: virtual std::pair Create( - const components::BaseComponent *_data) = 0; - - /// \brief Remove a component based on an id. - /// \param[in] _id Id of the component to remove. - /// \return True if the component was removed. - public: virtual bool Remove(const ComponentId _id) = 0; - - /// \brief Remove all components - public: virtual void RemoveAll() = 0; - - /// \brief Get a component based on an id. - /// \param[in] _id Id of the component to get. - /// \return A pointer to the component, or nullptr if the component - /// could not be found. - public: virtual const components::BaseComponent *Component( - const ComponentId _id) const = 0; - - /// \brief Get a mutable component based on an id. - /// \param[in] _id Id of the component to get. - /// \return A pointer to the component, or nullptr if the component - /// could not be found. - public: virtual components::BaseComponent *Component( - const ComponentId _id) = 0; - - /// \brief Get the first component. - /// \return First component or nullptr if there are no components. - public: virtual components::BaseComponent *First() = 0; - - /// \brief Mutex used to prevent data corruption. - protected: mutable std::mutex mutex; }; /// \brief Templated implementation of component storage. @@ -85,137 +43,10 @@ namespace ignition class IGNITION_GAZEBO_HIDDEN ComponentStorage : public ComponentStorageBase { /// \brief Constructor - public: explicit ComponentStorage() + public: explicit IGN_DEPRECATED(6) ComponentStorage() : ComponentStorageBase() { - // Reserve a chunk of memory for the components. The size here will - // effect how often Views are rebuilt when - // EntityComponentManager::CreateComponent() is called. - // - // Views would be rebuilt if the components vector capacity is - // exceeded after an EntityComponentManager::Each call has already - // been executed. - // - // See also this class's Create() function, which expands the value - // of components vector whenever the capacity is reached. - this->components.reserve(100); } - - // Documentation inherited. - public: bool Remove(const ComponentId _id) final - { - std::lock_guard lock(this->mutex); - - // Get an iterator to the component that should be removed. - auto iter = this->idMap.find(_id); - - // Make sure the component exists. - if (iter != this->idMap.end()) - { - // Handle the case where there are more components than the - // component to be removed - if (this->components.size() > 1) - { - // Swap the component to be removed with the component at the - // back of the vector. - std::swap(this->components[iter->second], - this->components.back()); - - // After the swap, we have to fix all the id mappings. - for (auto idIter =this->idMap.begin(); - idIter != this->idMap.end(); ++idIter) - { - if (static_cast(idIter->second) == - this->components.size()-1) - { - idIter->second = iter->second; - } - } - } - - // Remove the component. - this->components.pop_back(); - - // Remove the id mapping. - this->idMap.erase(iter); - return true; - } - return false; - } - - // Documentation inherited. - public: void RemoveAll() final - { - this->idCounter = 0; - this->idMap.clear(); - this->components.clear(); - } - - // Documentation inherited. - public: std::pair Create( - const components::BaseComponent *_data) final - { - ComponentId result; // = kComponentIdInvalid; - bool expanded = false; - if (this->components.size() == this->components.capacity()) - { - this->components.reserve(this->components.capacity() + 100); - expanded = true; - } - - std::lock_guard lock(this->mutex); - // cppcheck-suppress unmatchedSuppression - // cppcheck-suppress postfixOperator - result = this->idCounter++; - this->idMap[result] = this->components.size(); - // Copy the component - this->components.push_back(std::move( - ComponentTypeT(*static_cast(_data)))); - - return {result, expanded}; - } - - // Documentation inherited. - public: const components::BaseComponent *Component( - const ComponentId _id) const final - { - return static_cast( - const_cast *>( - this)->Component(_id)); - } - - public: components::BaseComponent *Component(const ComponentId _id) final - { - std::lock_guard lock(this->mutex); - - auto iter = this->idMap.find(_id); - - if (iter != this->idMap.end()) - { - return static_cast( - &this->components.at(iter->second)); - } - return nullptr; - } - - // Documentation inherited. - public: components::BaseComponent *First() final - { - std::lock_guard lock(this->mutex); - if (!this->components.empty()) - return static_cast(&this->components[0]); - return nullptr; - } - - /// \brief The id counter is used to get unique ids within this - /// storage class. - private: ComponentId idCounter = 0; - - /// \brief Map of ComponentId to Components (see the components vector). - private: std::map idMap; - - /// \brief Sequential storage of components. - public: std::vector components; }; } } diff --git a/include/ignition/gazebo/detail/EntityComponentManager.hh b/include/ignition/gazebo/detail/EntityComponentManager.hh index d08722ee80..05c5c34492 100644 --- a/include/ignition/gazebo/detail/EntityComponentManager.hh +++ b/include/ignition/gazebo/detail/EntityComponentManager.hh @@ -19,8 +19,10 @@ #include #include +#include #include #include +#include #include #include #include @@ -81,11 +83,24 @@ auto CompareData = [](const DataType &_a, const DataType &_b) -> bool ////////////////////////////////////////////////// template -ComponentKey EntityComponentManager::CreateComponent(const Entity _entity, +ComponentTypeT *EntityComponentManager::CreateComponent(const Entity _entity, const ComponentTypeT &_data) { - return this->CreateComponentImplementation(_entity, ComponentTypeT::typeId, - &_data); + auto updateData = this->CreateComponentImplementation(_entity, + ComponentTypeT::typeId, &_data); + auto comp = this->Component(_entity); + if (updateData) + { + if (!comp) + { + ignerr << "Internal error. Failure to create a component of type " + << ComponentTypeT::typeId << " for entity " << _entity + << ". This should never happen!\n"; + return comp; + } + *comp = _data; + } + return comp; } ////////////////////////////////////////////////// @@ -117,7 +132,7 @@ const ComponentTypeT *EntityComponentManager::Component( const ComponentKey &_key) const { return static_cast( - this->ComponentImplementation(_key)); + this->ComponentImplementation(_key.second, _key.first)); } ////////////////////////////////////////////////// @@ -125,7 +140,7 @@ template ComponentTypeT *EntityComponentManager::Component(const ComponentKey &_key) { return static_cast( - this->ComponentImplementation(_key)); + this->ComponentImplementation(_key.second, _key.first)); } ////////////////////////////////////////////////// @@ -174,16 +189,18 @@ bool EntityComponentManager::SetComponentData(const Entity _entity, template const ComponentTypeT *EntityComponentManager::First() const { - return static_cast( - this->First(ComponentTypeT::typeId)); + ignwarn << "EntityComponentManager::First is now deprecated and will always " + << "return nullptr.\n"; + return nullptr; } ////////////////////////////////////////////////// template ComponentTypeT *EntityComponentManager::First() { - return static_cast( - this->First(ComponentTypeT::typeId)); + ignwarn << "EntityComponentManager::First is now deprecated and will always " + << "return nullptr.\n"; + return nullptr; } ////////////////////////////////////////////////// @@ -196,7 +213,7 @@ Entity EntityComponentManager::EntityByComponents( // Iterate over entities Entity result{kNullEntity}; - for (const Entity entity : view.entities) + for (const Entity entity : view->Entities()) { bool different{false}; @@ -234,7 +251,7 @@ std::vector EntityComponentManager::EntitiesByComponents( // Iterate over entities std::vector result; - for (const Entity entity : view.entities) + for (const Entity entity : view->Entities()) { bool different{false}; @@ -274,7 +291,7 @@ std::vector EntityComponentManager::ChildrenByComponents(Entity _parent, // Iterate over entities std::vector result; - for (const Entity entity : view.entities) + for (const Entity entity : view->Entities()) { if (children.find(entity) == children.end()) { @@ -361,13 +378,13 @@ void EntityComponentManager::Each(typename identityFindView(); + auto view = this->FindView(); // Iterate over the entities in the view, and invoke the callback // function. - for (const Entity entity : view.entities) + for (const Entity entity : view->Entities()) { - if (!_f(entity, view.Component(entity, this)...)) + if (!std::apply(_f, view->EntityComponentConstData(entity))) { break; } @@ -381,13 +398,13 @@ void EntityComponentManager::Each(typename identityFindView(); + auto view = this->FindView(); // Iterate over the entities in the view, and invoke the callback // function. - for (const Entity entity : view.entities) + for (const Entity entity : view->Entities()) { - if (!_f(entity, view.Component(entity, this)...)) + if (!std::apply(_f, view->EntityComponentData(entity))) { break; } @@ -409,14 +426,14 @@ void EntityComponentManager::EachNew(typename identityFindView(); + auto view = this->FindView(); // Iterate over the entities in the view and in the newly created // entities list, and invoke the callback // function. - for (const Entity entity : view.newEntities) + for (const Entity entity : view->NewEntities()) { - if (!_f(entity, view.Component(entity, this)...)) + if (!std::apply(_f, view->EntityComponentData(entity))) { break; } @@ -430,14 +447,14 @@ void EntityComponentManager::EachNew(typename identityFindView(); + auto view = this->FindView(); // Iterate over the entities in the view and in the newly created // entities list, and invoke the callback // function. - for (const Entity entity : view.newEntities) + for (const Entity entity : view->NewEntities()) { - if (!_f(entity, view.Component(entity, this)...)) + if (!std::apply(_f, view->EntityComponentConstData(entity))) { break; } @@ -451,14 +468,14 @@ void EntityComponentManager::EachRemoved(typename identityFindView(); + auto view = this->FindView(); // Iterate over the entities in the view and in the newly created // entities list, and invoke the callback // function. - for (const Entity entity : view.toRemoveEntities) + for (const Entity entity : view->ToRemoveEntities()) { - if (!_f(entity, view.Component(entity, this)...)) + if (!std::apply(_f, view->EntityComponentConstData(entity))) { break; } @@ -466,93 +483,53 @@ void EntityComponentManager::EachRemoved(typename identity::type> -void EntityComponentManager::AddComponentsToView(detail::View &_view, - const Entity _entity) const +template +detail::View *EntityComponentManager::FindView() const { - const ComponentTypeId typeId = FirstComponent::typeId; + auto viewKey = std::vector{ComponentTypeTs::typeId...}; - const ComponentId compId = - this->EntityComponentIdFromType(_entity, typeId); - if (compId >= 0) + auto baseViewPtr = this->FindView(viewKey); + if (nullptr != baseViewPtr) { - // Add the component to the view. - _view.AddComponent(_entity, typeId, compId); - } - else - { - ignerr << "Entity[" << _entity << "] has no component of type[" - << typeId << "]. This should never happen.\n"; - } -} + auto view = static_cast*>(baseViewPtr); -////////////////////////////////////////////////// -template::type> -void EntityComponentManager::AddComponentsToView(detail::View &_view, - const Entity _entity) const -{ - const ComponentTypeId typeId = FirstComponent::typeId; - const ComponentId compId = - this->EntityComponentIdFromType(_entity, typeId); - if (compId >= 0) - { - // Add the component to the view. - _view.AddComponent(_entity, typeId, compId); - } - else - { - ignerr << "Entity[" << _entity << "] has no component of type[" - << typeId << "]. This should never happen.\n"; - } - - // Add the remaining components to the view. - this->AddComponentsToView(_view, _entity); -} + // add any new entities to the view before using it + for (const auto &[entity, isNew] : view->ToAddEntities()) + { + view->AddEntityWithConstComps(entity, isNew, + this->Component(entity)...); + view->AddEntityWithComps(entity, isNew, + const_cast(this)->Component( + entity)...); + } + view->ClearToAddEntities(); -////////////////////////////////////////////////// -template -detail::View &EntityComponentManager::FindView() const -{ - auto types = std::set{ComponentTypeTs::typeId...}; + return view; + } - std::map::iterator viewIter; + // create a new view if one wasn't found + detail::View view; - // Find the view. If the view doesn't exist, then create a new view. - if (!this->FindView(types, viewIter)) + for (const auto &vertex : this->Entities().Vertices()) { - detail::View view; - // Add all the entities that match the component types to the - // view. - for (const auto &vertex : this->Entities().Vertices()) - { - Entity entity = vertex.first; - if (this->EntityMatches(entity, types)) - { - view.AddEntity(entity, this->IsNewEntity(entity)); - // If there is a request to delete this entity, update the view as - // well - if (this->IsMarkedForRemoval(entity)) - { - view.AddEntityToRemoved(entity); - } - - // Store pointers to all the components. This recursively adds - // all the ComponentTypeTs that belong to the entity to the view. - this->AddComponentsToView(view, entity); - } - } + Entity entity = vertex.first; + + // only add entities to the view that have all of the components in viewKey + if (!this->EntityMatches(entity, view.ComponentTypes())) + continue; - // Store the view. - return this->AddView(types, std::move(view))->second; + view.AddEntityWithConstComps(entity, this->IsNewEntity(entity), + this->Component(entity)...); + view.AddEntityWithComps(entity, this->IsNewEntity(entity), + const_cast(this)->Component( + entity)...); + if (this->IsMarkedForRemoval(entity)) + view.MarkEntityToRemove(entity); } - return viewIter->second; + baseViewPtr = this->AddView(viewKey, + std::make_unique>(view)); + return static_cast*>(baseViewPtr); } ////////////////////////////////////////////////// diff --git a/include/ignition/gazebo/detail/View.hh b/include/ignition/gazebo/detail/View.hh index b45d730d6d..93ea32c1b1 100644 --- a/include/ignition/gazebo/detail/View.hh +++ b/include/ignition/gazebo/detail/View.hh @@ -17,14 +17,16 @@ #ifndef IGNITION_GAZEBO_DETAIL_VIEW_HH_ #define IGNITION_GAZEBO_DETAIL_VIEW_HH_ -#include -#include -#include +#include +#include +#include #include -#include "ignition/gazebo/components/Component.hh" + +#include + #include "ignition/gazebo/Entity.hh" -#include "ignition/gazebo/Export.hh" -#include "ignition/gazebo/Types.hh" +#include "ignition/gazebo/config.hh" +#include "ignition/gazebo/detail/BaseView.hh" namespace ignition { @@ -34,110 +36,297 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace detail { -/// \brief A key into the map of views -using ComponentTypeKey = std::set; - -/// \brief A view is a cache to entities, and their components, that -/// match a set of component types. A cache is used because systems will -/// frequently, potentially every iteration, query the -/// EntityComponentManager for sets of entities that match a set of -/// component types. Rather than look up the entities every time, we can -/// use a cache to improve performance. The assumption is that entities -/// and the types of components assigned to entities change infrequently -/// compared to the frequency of queries performed by systems. -/// -/// Note that symbols for this class are visible because methods from this class -/// are used in templated Ignition::Gazebo::EntityComponentManager methods. -/// However, users should not use this class (or anything else in namespace -/// ignition::gazebo::detail) directly. -class IGNITION_GAZEBO_VISIBLE View +/// \brief A view that caches a particular set of component type data. +/// \tparam ComponentTypeTs The component type(s) that are stored in this view. +template +class View : public BaseView { - /// Get a pointer to a component for an entity based on a component type. - /// \param[in] _entity The entity. - /// \param[in] _ecm Pointer to the entity component manager. - /// \return Pointer to the component. - public: template - const ComponentTypeT *Component(const Entity _entity, - const EntityComponentManager *_ecm) const - { - ComponentTypeId typeId = ComponentTypeT::typeId; - return static_cast( - this->ComponentImplementation(_entity, typeId, _ecm)); - } + /// \brief Alias for containers that hold and entity and its component data. + /// The component types held in this container match the component types that + /// were specified when creating the view. + private: using ComponentData = std::tuple; + private: using ConstComponentData = + std::tuple; - /// Get a pointer to a component for an entity based on a component type. - /// \param[in] _entity The entity. - /// \param[in] _ecm Pointer to the entity component manager. - /// \return Pointer to the component. - public: template - ComponentTypeT *Component(const Entity _entity, - const EntityComponentManager *_ecm) - { - ComponentTypeId typeId = ComponentTypeT::typeId; - return static_cast( - const_cast( - this->ComponentImplementation(_entity, typeId, _ecm))); - } + /// \brief Constructor + public: View(); + + /// \brief Documentation inherited + public: bool HasCachedComponentData(const Entity _entity) const override; + + /// \brief Documentation inherited + public: bool RemoveEntity(const Entity _entity) override; + + /// \brief Get an entity and its component data. It is assumed that the entity + /// being requested exists in the view. + /// \param[_in] _entity The entity + /// \return The entity and its component data. Const pointers to the component + /// data are returned. + public: ConstComponentData EntityComponentConstData( + const Entity _entity) const; - /// \brief Add an entity to the view. - /// \param[in] _entity The entity to add. + /// \brief Get an entity and its component data. It is assumed that the entity + /// being requested exists in the view. + /// \param[_in] _entity The entity + /// \return The entity and its component data. Mutable pointers to the + /// component data are returned. + public: ComponentData EntityComponentData(const Entity _entity); + + /// \brief Add an entity with its component data to the view. It is assunmed + /// that the entity to be added does not already exist in the view. + /// \param[in] _entity The entity /// \param[in] _new Whether to add the entity to the list of new entities. /// The new here is to indicate whether the entity is new to the entity /// component manager. An existing entity can be added when creating a new /// view or when rebuilding the view. - public: void AddEntity(const Entity _entity, const bool _new = false); - - /// \brief Remove an entity from the view. - /// \param[in] _entity The entity to remove. - /// \param[in] _key Components that should also be removed. - /// \return True if the entity was removed, false if the entity did not - /// exist in the view. - public: bool RemoveEntity(const Entity _entity, - const ComponentTypeKey &_key); - - /// \brief Add the entity to the list of entities to be removed - /// \param[in] _entity The entity to add. - /// \return True if the entity was added to the list, false if the entity - /// did not exist in the view. - public: bool AddEntityToRemoved(const Entity _entity); - - /// \brief Add a component to an entity. - /// \param[in] _entity The entity. - /// \param[in] _compTypeId Component type id. - /// \param[in] _compId Component id. - public: void AddComponent(const Entity _entity, - const ComponentTypeId _compTypeId, - const ComponentId _compId); - - /// \brief Implementation of the Component accessor. - /// \param[in] _entity The entity. - /// \param[in] _typeId Type id of the component. - /// \param[in] _ecm Pointer to the EntityComponentManager. - /// \return Pointer to the component, or nullptr if not found. - private: const components::BaseComponent *ComponentImplementation( - const Entity _entity, - ComponentTypeId _typeId, - const EntityComponentManager *_ecm) const; - - /// \brief Clear the list of new entities - public: void ClearNewEntities(); - - /// \brief All the entities that belong to this view. - public: std::set entities; - - /// \brief List of newly created entities - public: std::set newEntities; - - /// \brief List of entities about to be removed - public: std::set toRemoveEntities; - - /// \brief All of the components for each entity. - public: std::map, - ComponentId> components; + /// \param[in] _compPtrs Const pointers to the entity's components + public: void AddEntityWithConstComps(const Entity &_entity, const bool _new, + const ComponentTypeTs*... _compPtrs); + + /// \brief Add an entity with its component data to the view. It is assunmed + /// that the entity to be added does not already exist in the view. + /// \param[in] _entity The entity + /// \param[in] _new Whether to add the entity to the list of new entities. + /// The new here is to indicate whether the entity is new to the entity + /// component manager. An existing entity can be added when creating a new + /// view or when rebuilding the view. + /// \param[in] _compPtrs Pointers to the entity's components + public: void AddEntityWithComps(const Entity &_entity, const bool _new, + ComponentTypeTs*... _compPtrs); + + /// \brief Documentation inherited + public: bool NotifyComponentAddition(const Entity _entity, bool _newEntity, + const ComponentTypeId _typeId) override; + + /// \brief Documentation inherited + public: bool NotifyComponentRemoval(const Entity _entity, + const ComponentTypeId _typeId) override; + + /// \brief Documentation inherited + public: void Reset() override; + + /// \brief A map of entities to their component data. Since tuples are defined + /// at compile time, we need seperate containers that have tuples for both + /// non-const and const component pointers (calls to ECM::Each can have a + /// method signature that uses either non-const or const pointers) + private: std::unordered_map validData; + private: std::unordered_map validConstData; + + /// \brief A map of invalid entities to their component data. The difference + /// between invalidData and validData is that the entities in invalidData were + /// once in validData, but they had a component removed, so the entity no + /// longer meets the component requirements of the view. If the missing + /// component data is ever added back to an entitiy in invalidData, then this + /// entity will be moved back to validData. The usage of invalidData is an + /// implementation detail that should be ignored by those using the View API; + /// from a user's point of view, entities that belong to invalidData don't + /// appear to be a part of the view at all. + /// + /// The reason for moving entities with missing components to invalidData + /// instead of completely deleting them from the view is because if components + /// are added back later and the entity needs to be re-added to the view, + /// tuple creation can be costly. So, this approach is used instead to + /// maintain runtime performance (the tradeoff of mainting performance is + /// increased complexity and memory usage). + /// + /// \sa missingCompTracker + private: std::unordered_map invalidData; + private: std::unordered_map invalidConstData; + + /// \brief A map that keeps track of which component types for entities in + /// invalidData need to be added back to the entity in order to move the + /// entity back to validData. If the set of types (value in the map) becomes + /// empty, then this means that the entity (key in the map) has all of the + /// component types defined by the view, so the entity can be moved back to + /// validData. + /// + /// \sa invalidData + private: std::unordered_map> + missingCompTracker; }; -/// \endcond + +////////////////////////////////////////////////// +template +View::View() +{ + this->componentTypes = {ComponentTypeTs::typeId...}; } + +////////////////////////////////////////////////// +template +bool View::HasCachedComponentData( + const Entity _entity) const +{ + auto cachedComps = + this->validData.find(_entity) != this->validData.end() || + this->invalidData.find(_entity) != this->invalidData.end(); + auto cachedConstComps = + this->validConstData.find(_entity) != this->validConstData.end() || + this->invalidConstData.find(_entity) != this->invalidConstData.end(); + + if (cachedComps && !cachedConstComps) + { + ignwarn << "Non-const component data is cached for entity " << _entity + << ", but const component data is not cached." << std::endl; + } + else if (cachedConstComps && !cachedComps) + { + ignwarn << "Const component data is cached for entity " << _entity + << ", but non-const component data is not cached." << std::endl; + } + + return cachedComps && cachedConstComps; } + +////////////////////////////////////////////////// +template +bool View::RemoveEntity(const Entity _entity) +{ + this->invalidData.erase(_entity); + this->invalidConstData.erase(_entity); + this->missingCompTracker.erase(_entity); + + if (!this->HasEntity(_entity) && !this->IsEntityMarkedForAddition(_entity)) + return false; + + this->entities.erase(_entity); + this->newEntities.erase(_entity); + this->toRemoveEntities.erase(_entity); + this->toAddEntities.erase(_entity); + this->validData.erase(_entity); + this->validConstData.erase(_entity); + + return true; +} + +////////////////////////////////////////////////// +template +typename View::ConstComponentData + View::EntityComponentConstData(const Entity _entity) const +{ + return this->validConstData.at(_entity); +} + +////////////////////////////////////////////////// +template +typename View::ComponentData + View::EntityComponentData(const Entity _entity) +{ + return this->validData.at(_entity); +} + +////////////////////////////////////////////////// +template +void View::AddEntityWithConstComps(const Entity &_entity, + const bool _new, const ComponentTypeTs*... _compPtrs) +{ + this->validConstData[_entity] = std::make_tuple(_entity, _compPtrs...); + this->entities.insert(_entity); + if (_new) + this->newEntities.insert(_entity); +} + +////////////////////////////////////////////////// +template +void View::AddEntityWithComps(const Entity &_entity, + const bool _new, ComponentTypeTs*... _compPtrs) +{ + this->validData[_entity] = std::make_tuple(_entity, _compPtrs...); + this->entities.insert(_entity); + if (_new) + this->newEntities.insert(_entity); +} + +////////////////////////////////////////////////// +template +bool View::NotifyComponentAddition(const Entity _entity, + bool _newEntity, const ComponentTypeId _typeId) +{ + // make sure that _typeId is a type required by the view and that _entity is + // already a part of the view + if (!this->RequiresComponent(_typeId) || + !this->HasCachedComponentData(_entity)) + return false; + + // remove the newly added component type from the missing component types + // list + auto missingCompsIter = this->missingCompTracker.find(_entity); + if (missingCompsIter == this->missingCompTracker.end()) + { + // the component is already added, so nothing else needs to be done + return true; + } + missingCompsIter->second.erase(_typeId); + + // if the entity now has all components that meet the requirements of the + // view, then add the entity back to the view + if (missingCompsIter->second.empty()) + { + auto nh = this->invalidData.extract(_entity); + this->validData.insert(std::move(nh)); + auto constCompNh = this->invalidConstData.extract(_entity); + this->validConstData.insert(std::move(constCompNh)); + this->entities.insert(_entity); + if (_newEntity) + this->newEntities.insert(_entity); + this->missingCompTracker.erase(_entity); + } + + return true; } + +////////////////////////////////////////////////// +template +bool View::NotifyComponentRemoval(const Entity _entity, + const ComponentTypeId _typeId) +{ + // make sure that _typeId is a type required by the view and that _entity is + // already a part of the view + if (!this->RequiresComponent(_typeId) || + !this->HasCachedComponentData(_entity)) + return false; + + // if the component being removed is the first component that causes _entity + // to be invalid for this view, move _entity from validData to invalidData + // since _entity should no longer be considered a part of the view + auto it = this->validData.find(_entity); + auto constCompIt = this->validConstData.find(_entity); + if (it != this->validData.end() && + constCompIt != this->validConstData.end()) + { + auto nh = this->validData.extract(it); + this->invalidData.insert(std::move(nh)); + auto constCompNh = this->validConstData.extract(constCompIt); + this->invalidConstData.insert(std::move(constCompNh)); + this->entities.erase(_entity); + this->newEntities.erase(_entity); + } + + this->missingCompTracker[_entity].insert(_typeId); + + return true; +} + +////////////////////////////////////////////////// +template +void View::Reset() +{ + // reset all data structures in the BaseView except for componentTypes since + // the view always requires the types in componentTypes + this->entities.clear(); + this->newEntities.clear(); + this->toRemoveEntities.clear(); + this->toAddEntities.clear(); + + // reset all data structures unique to the templated view + this->validData.clear(); + this->validConstData.clear(); + this->invalidData.clear(); + this->invalidConstData.clear(); + this->missingCompTracker.clear(); } +} // namespace detail +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition #endif diff --git a/src/BaseView.cc b/src/BaseView.cc new file mode 100644 index 0000000000..23e78ee607 --- /dev/null +++ b/src/BaseView.cc @@ -0,0 +1,109 @@ +/* + * 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. + * +*/ +#include "ignition/gazebo/detail/BaseView.hh" + +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/Types.hh" + +using namespace ignition; +using namespace gazebo; +using namespace detail; + +////////////////////////////////////////////////// +bool BaseView::HasEntity(const Entity _entity) const +{ + return this->entities.find(_entity) != this->entities.end(); +} + +////////////////////////////////////////////////// +bool BaseView::IsEntityMarkedForAddition(const Entity _entity) const +{ + return this->toAddEntities.find(_entity) != this->toAddEntities.end(); +} + +////////////////////////////////////////////////// +bool BaseView::MarkEntityToAdd(const Entity _entity, bool _new) +{ + if (this->HasCachedComponentData(_entity)) + return false; + + this->toAddEntities[_entity] = _new; + return true; +} + +////////////////////////////////////////////////// +bool BaseView::RequiresComponent(const ComponentTypeId _typeId) const +{ + return this->componentTypes.find(_typeId) != this->componentTypes.end(); +} + +////////////////////////////////////////////////// +bool BaseView::MarkEntityToRemove(const Entity _entity) +{ + if (this->HasCachedComponentData(_entity) || + this->IsEntityMarkedForAddition(_entity)) + { + this->toRemoveEntities.insert(_entity); + return true; + } + return false; +} + +////////////////////////////////////////////////// +void BaseView::ResetNewEntityState() +{ + this->newEntities.clear(); + + // mark all entities in the toAddEntities map as not newly created + for (auto &entityNewPair : this->toAddEntities) + entityNewPair.second = false; +} + +////////////////////////////////////////////////// +const std::set &BaseView::ComponentTypes() const +{ + return this->componentTypes; +} + +const std::set &BaseView::Entities() const +{ + return this->entities; +} + +////////////////////////////////////////////////// +const std::set &BaseView::NewEntities() const +{ + return this->newEntities; +} + +////////////////////////////////////////////////// +const std::set &BaseView::ToRemoveEntities() const +{ + return this->toRemoveEntities; +} + +////////////////////////////////////////////////// +const std::unordered_map &BaseView::ToAddEntities() const +{ + return this->toAddEntities; +} + +////////////////////////////////////////////////// +void BaseView::ClearToAddEntities() +{ + this->toAddEntities.clear(); +} diff --git a/src/BaseView_TEST.cc b/src/BaseView_TEST.cc new file mode 100644 index 0000000000..5e424542ea --- /dev/null +++ b/src/BaseView_TEST.cc @@ -0,0 +1,522 @@ +/* + * 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. + * +*/ + +#include + +#include + +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/Types.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Visual.hh" +#include "ignition/gazebo/detail/BaseView.hh" +#include "ignition/gazebo/detail/View.hh" + +using namespace ignition; +using namespace gazebo; + +class BaseViewTest : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + } +}; + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, ComponentTypes) +{ + auto modelNameView = detail::View(); + + // make sure that a view's required component types are initialized properly + EXPECT_EQ(2u, modelNameView.ComponentTypes().size()); + EXPECT_NE(modelNameView.ComponentTypes().find(components::Model::typeId), + modelNameView.ComponentTypes().end()); + EXPECT_NE(modelNameView.ComponentTypes().find(components::Name::typeId), + modelNameView.ComponentTypes().end()); + EXPECT_TRUE(modelNameView.RequiresComponent(components::Model::typeId)); + EXPECT_TRUE(modelNameView.RequiresComponent(components::Name::typeId)); + EXPECT_FALSE(modelNameView.RequiresComponent(components::Visual::typeId)); +} + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, ToAddEntities) +{ + auto modelNameView = detail::View(); + const Entity e1 = 1; + auto e1IsNew = true; + + // by default, the view shouldn't have any entities marked as ToAdd + EXPECT_EQ(0u, modelNameView.ToAddEntities().size()); + EXPECT_FALSE(modelNameView.IsEntityMarkedForAddition(e1)); + + // Calling ClearToAddEntities should have no effect since no entities are + // marked as ToAdd + modelNameView.ClearToAddEntities(); + EXPECT_EQ(0u, modelNameView.ToAddEntities().size()); + EXPECT_FALSE(modelNameView.IsEntityMarkedForAddition(e1)); + + // Mark an entity as ToAdd (and as a "newly created" entity) + EXPECT_TRUE(modelNameView.MarkEntityToAdd(e1, e1IsNew)); + EXPECT_TRUE(modelNameView.IsEntityMarkedForAddition(e1)); + EXPECT_EQ(1u, modelNameView.ToAddEntities().size()); + auto e1ToAddIter = modelNameView.ToAddEntities().find(e1); + ASSERT_NE(e1ToAddIter, modelNameView.ToAddEntities().end()); + EXPECT_EQ(e1ToAddIter->second, e1IsNew); + // (entities marked as ToAdd aren't considered as entities that are a part + // of the view yet) + EXPECT_FALSE(modelNameView.HasEntity(e1)); + + // Try to mark an entity as ToAdd that is already marked as ToAdd. + // This time around, the entity is not flagged as a "newly created" entity + // when the entity is marked as ToAdd, so we should see this update when + // inspecting the entity in the toAddEntities queue + e1IsNew = false; + EXPECT_TRUE(modelNameView.MarkEntityToAdd(e1, e1IsNew)); + EXPECT_TRUE(modelNameView.IsEntityMarkedForAddition(e1)); + EXPECT_EQ(1u, modelNameView.ToAddEntities().size()); + e1ToAddIter = modelNameView.ToAddEntities().find(e1); + ASSERT_NE(e1ToAddIter, modelNameView.ToAddEntities().end()); + EXPECT_EQ(e1ToAddIter->second, e1IsNew); + // (entities marked as ToAdd aren't considered as entities that are a part + // of the view yet) + EXPECT_FALSE(modelNameView.HasEntity(e1)); + + // Remove an entity's ToAdd status and then re-mark it as ToAdd + modelNameView.ClearToAddEntities(); + EXPECT_EQ(0u, modelNameView.ToAddEntities().size()); + EXPECT_FALSE(modelNameView.IsEntityMarkedForAddition(e1)); + e1IsNew = true; + EXPECT_TRUE(modelNameView.MarkEntityToAdd(e1, e1IsNew)); + EXPECT_TRUE(modelNameView.IsEntityMarkedForAddition(e1)); + EXPECT_EQ(1u, modelNameView.ToAddEntities().size()); + e1ToAddIter = modelNameView.ToAddEntities().find(e1); + ASSERT_NE(e1ToAddIter, modelNameView.ToAddEntities().end()); + EXPECT_EQ(e1ToAddIter->second, e1IsNew); + // (entities marked as ToAdd aren't considered as entities that are a part + // of the view yet) + EXPECT_FALSE(modelNameView.HasEntity(e1)); +} + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, AddEntities) +{ + auto modelNameView = detail::View(); + + // Initially, the view should have no entities + EXPECT_EQ(0u, modelNameView.Entities().size()); + EXPECT_EQ(0u, modelNameView.NewEntities().size()); + + // Create a few entities with components for the view + const Entity e1 = 1; + const auto e1IsNew = false; + auto e1ModelComp = components::Model(); + auto e1NameComp = components::Name("e1"); + + const Entity e2 = 2; + const auto e2IsNew = true; + auto e2ModelComp = components::Model(); + auto e2NameComp = components::Name("e2"); + + // Add the entities and their components to the view. + EXPECT_FALSE(modelNameView.HasEntity(e1)); + EXPECT_FALSE(modelNameView.HasEntity(e2)); + EXPECT_FALSE(modelNameView.HasCachedComponentData(e1)); + EXPECT_FALSE(modelNameView.HasCachedComponentData(e2)); + modelNameView.AddEntityWithComps(e1, e1IsNew, &e1ModelComp, &e1NameComp); + modelNameView.AddEntityWithConstComps(e1, e1IsNew, &e1ModelComp, &e1NameComp); + modelNameView.AddEntityWithComps(e2, e2IsNew, &e2ModelComp, &e2NameComp); + modelNameView.AddEntityWithConstComps(e2, e2IsNew, &e2ModelComp, &e2NameComp); + EXPECT_TRUE(modelNameView.HasEntity(e1)); + EXPECT_TRUE(modelNameView.HasEntity(e2)); + EXPECT_TRUE(modelNameView.HasCachedComponentData(e1)); + EXPECT_TRUE(modelNameView.HasCachedComponentData(e2)); + EXPECT_EQ(2u, modelNameView.Entities().size()); + EXPECT_NE(modelNameView.Entities().find(e1), modelNameView.Entities().end()); + EXPECT_NE(modelNameView.Entities().find(e2), modelNameView.Entities().end()); + EXPECT_EQ(1u, modelNameView.NewEntities().size()); + EXPECT_NE(modelNameView.NewEntities().find(e2), + modelNameView.NewEntities().end()); + + auto e1ConstData = modelNameView.EntityComponentConstData(e1); + EXPECT_EQ(e1, std::get(e1ConstData)); + EXPECT_EQ(&e1ModelComp, std::get(e1ConstData)); + EXPECT_EQ(&e1NameComp, std::get(e1ConstData)); + + auto e1Data = modelNameView.EntityComponentData(e1); + EXPECT_EQ(e1, std::get(e1Data)); + EXPECT_EQ(&e1ModelComp, std::get(e1Data)); + EXPECT_EQ(&e1NameComp, std::get(e1Data)); + + auto e2ConstData = modelNameView.EntityComponentConstData(e2); + EXPECT_EQ(e2, std::get(e2ConstData)); + EXPECT_EQ(&e2ModelComp, std::get(e2ConstData)); + EXPECT_EQ(&e2NameComp, std::get(e2ConstData)); + + auto e2Data = modelNameView.EntityComponentData(e2); + EXPECT_EQ(e2, std::get(e2Data)); + EXPECT_EQ(&e2ModelComp, std::get(e2Data)); + EXPECT_EQ(&e2NameComp, std::get(e2Data)); +} + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, RemoveEntities) +{ + auto view = detail::View(); + + const Entity e1 = 1; + auto e1ModelComp = components::Model(); + const Entity e2 = 2; + auto e2ModelComp = components::Model(); + const auto isNewEntity = false; + + // add entities to the view + view.AddEntityWithComps(e1, isNewEntity, &e1ModelComp); + view.AddEntityWithConstComps(e1, isNewEntity, &e1ModelComp); + view.AddEntityWithComps(e2, isNewEntity, &e2ModelComp); + view.AddEntityWithConstComps(e2, isNewEntity, &e2ModelComp); + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_TRUE(view.HasEntity(e2)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_TRUE(view.HasCachedComponentData(e2)); + + // remove entities from the view + EXPECT_EQ(0u, view.ToRemoveEntities().size()); + EXPECT_TRUE(view.RemoveEntity(e1)); + EXPECT_FALSE(view.HasEntity(e1)); + EXPECT_FALSE(view.HasCachedComponentData(e1)); + EXPECT_EQ(0u, view.ToRemoveEntities().size()); + EXPECT_TRUE(view.RemoveEntity(e2)); + EXPECT_FALSE(view.HasEntity(e2)); + EXPECT_FALSE(view.HasCachedComponentData(e2)); + EXPECT_EQ(0u, view.ToRemoveEntities().size()); + + // try to remove an entity that is not in the view + EXPECT_FALSE(view.RemoveEntity(e1)); + + // add entities to the view's toAdd and toRemove queue + EXPECT_TRUE(view.MarkEntityToAdd(e1)); + EXPECT_TRUE(view.MarkEntityToRemove(e1)); + EXPECT_EQ(1u, view.ToAddEntities().size()); + EXPECT_NE(view.ToAddEntities().end(), view.ToAddEntities().find(e1)); + EXPECT_EQ(1u, view.ToRemoveEntities().size()); + EXPECT_NE(view.ToRemoveEntities().end(), view.ToRemoveEntities().find(e1)); + + // remove entities e1 and e2 from the view and make sure that the toAdd and + // toRemove queues are updated to no longer have the removed entities + EXPECT_TRUE(view.RemoveEntity(e1)); + EXPECT_EQ(0u, view.ToAddEntities().size()); + EXPECT_EQ(0u, view.ToRemoveEntities().size()); +} + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, Reset) +{ + auto view = detail::View(); + + // initially, the view should be completely empty, except for its component + // types (the view's component types are defined at object instantiation time + // and never change) + EXPECT_EQ(0u, view.Entities().size()); + EXPECT_EQ(0u, view.NewEntities().size()); + EXPECT_EQ(0u, view.ToAddEntities().size()); + EXPECT_EQ(0u, view.ToRemoveEntities().size()); + EXPECT_EQ(1u, view.ComponentTypes().size()); + EXPECT_NE(view.ComponentTypes().end(), + view.ComponentTypes().find(components::Model::typeId)); + + // populate the view with entity/component information + const auto isNewEntity = true; + const Entity e1 = 1; + auto e1ModelComp = components::Model(); + view.AddEntityWithComps(e1, isNewEntity, &e1ModelComp); + view.AddEntityWithConstComps(e1, isNewEntity, &e1ModelComp); + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_TRUE(view.MarkEntityToRemove(e1)); + const Entity e2 = 2; + EXPECT_TRUE(view.MarkEntityToAdd(e2)); + EXPECT_TRUE(view.IsEntityMarkedForAddition(e2)); + + // make sure the view doesn't have empty entity-related data structures + EXPECT_EQ(1u, view.Entities().size()); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_EQ(1u, view.ToAddEntities().size()); + EXPECT_EQ(1u, view.ToRemoveEntities().size()); + + // reset the view and make sure that it's back in its original state + view.Reset(); + EXPECT_FALSE(view.HasEntity(e1)); + EXPECT_FALSE(view.HasCachedComponentData(e1)); + EXPECT_FALSE(view.IsEntityMarkedForAddition(e2)); + EXPECT_EQ(0u, view.Entities().size()); + EXPECT_EQ(0u, view.NewEntities().size()); + EXPECT_EQ(0u, view.ToAddEntities().size()); + EXPECT_EQ(0u, view.ToRemoveEntities().size()); + EXPECT_EQ(1u, view.ComponentTypes().size()); + EXPECT_NE(view.ComponentTypes().end(), + view.ComponentTypes().find(components::Model::typeId)); + + // add newly created entities to the view + view.AddEntityWithComps(e1, isNewEntity, &e1ModelComp); + view.AddEntityWithConstComps(e1, isNewEntity, &e1ModelComp); + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_TRUE(view.MarkEntityToRemove(e1)); + EXPECT_EQ(1u, view.Entities().size()); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_TRUE(view.MarkEntityToAdd(e2, isNewEntity)); + EXPECT_TRUE(view.IsEntityMarkedForAddition(e2)); + EXPECT_EQ(1u, view.ToAddEntities().size()); + auto e2ToAddIter = view.ToAddEntities().find(e2); + ASSERT_NE(view.ToAddEntities().end(), e2ToAddIter); + EXPECT_EQ(e2ToAddIter->second, isNewEntity); + + // reset the newly created entity state of the view + view.ResetNewEntityState(); + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_TRUE(view.MarkEntityToRemove(e1)); + EXPECT_TRUE(view.IsEntityMarkedForAddition(e2)); + EXPECT_EQ(1u, view.Entities().size()); + EXPECT_EQ(0u, view.NewEntities().size()); + EXPECT_EQ(1u, view.ToAddEntities().size()); + e2ToAddIter = view.ToAddEntities().find(e2); + ASSERT_NE(view.ToAddEntities().end(), e2ToAddIter); + // entity e2 should still be an entity marked as ToAdd, but it shouldn't + // be a newly created entity anymore + EXPECT_FALSE(e2ToAddIter->second); +} + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, CachedComponentData) +{ + auto view = detail::View(); + + const Entity e1 = 1; + const auto e1IsNew = true; + auto e1ModelComp = components::Model(); + + EXPECT_FALSE(view.HasCachedComponentData(e1)); + + // add both const and non-const component data for e1 to the view + view.AddEntityWithComps(e1, e1IsNew, &e1ModelComp); + view.AddEntityWithConstComps(e1, e1IsNew, &e1ModelComp); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + + // reset the view and add only const component data this time + view.Reset(); + EXPECT_FALSE(view.HasCachedComponentData(e1)); + view.AddEntityWithConstComps(e1, e1IsNew, &e1ModelComp); + EXPECT_FALSE(view.HasCachedComponentData(e1)); + + // reset the view and add only non-const component data this time + view.Reset(); + EXPECT_FALSE(view.HasCachedComponentData(e1)); + view.AddEntityWithComps(e1, e1IsNew, &e1ModelComp); + EXPECT_FALSE(view.HasCachedComponentData(e1)); +} + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, ComponentChangeNotification) +{ + auto view = detail::View(); + + const Entity e1 = 1; + const auto e1IsNew = true; + auto e1ModelComp = components::Model(); + auto e1VisualComp = components::Visual(); + + // add the entity and its component data to the view + view.AddEntityWithComps(e1, e1IsNew, &e1ModelComp, &e1VisualComp); + view.AddEntityWithConstComps(e1, e1IsNew, &e1ModelComp, &e1VisualComp); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_EQ(1u, view.Entities().size()); + EXPECT_NE(view.Entities().end(), view.Entities().find(e1)); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_NE(view.NewEntities().end(), view.NewEntities().find(e1)); + + // mimic a removal of e1's model component by notifying the view that this + // component was removed + EXPECT_TRUE(view.NotifyComponentRemoval(e1, components::Model::typeId)); + EXPECT_FALSE(view.HasEntity(e1)); + EXPECT_EQ(0u, view.Entities().size()); + EXPECT_EQ(0u, view.NewEntities().size()); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + + // mimic a removal of e1's visual component by notifying the view that this + // component was removed + EXPECT_TRUE(view.NotifyComponentRemoval(e1, components::Visual::typeId)); + EXPECT_FALSE(view.HasEntity(e1)); + EXPECT_EQ(0u, view.Entities().size()); + EXPECT_EQ(0u, view.NewEntities().size()); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + + // mimic re-addition of e1's model component by notifying the view that this + // component was added. At this point, the visual component is still missing, + // so e1 shouldn't be a part of the view yet + EXPECT_TRUE(view.NotifyComponentAddition(e1, e1IsNew, + components::Model::typeId)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_FALSE(view.HasEntity(e1)); + EXPECT_EQ(0u, view.Entities().size()); + EXPECT_EQ(0u, view.NewEntities().size()); + + // mimic re-addition of e1's visual component by notifying the view that this + // component was added. Now that both the model and visual components have + // been re-added to e1, e1 should be a part of the view + EXPECT_TRUE(view.NotifyComponentAddition(e1, e1IsNew, + components::Visual::typeId)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_EQ(1u, view.Entities().size()); + EXPECT_NE(view.Entities().end(), view.Entities().find(e1)); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_NE(view.NewEntities().end(), view.NewEntities().find(e1)); + + // try to call NotifyComponent* methods with component types that don't + // belong to the view + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_FALSE(view.RequiresComponent(components::Name::typeId)); + EXPECT_FALSE(view.NotifyComponentRemoval(e1, components::Name::typeId)); + EXPECT_FALSE(view.NotifyComponentAddition(e1, e1IsNew, + components::Name::typeId)); + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + + // try to call NotifyComponent* methods with entities that aren't a part of + // the view + const Entity e2 = 2; + const auto e2IsNew = false; + EXPECT_FALSE(view.HasEntity(e2)); + EXPECT_FALSE(view.HasCachedComponentData(e2)); + EXPECT_TRUE(view.RequiresComponent(components::Model::typeId)); + EXPECT_FALSE(view.NotifyComponentRemoval(e2, components::Model::typeId)); + EXPECT_FALSE(view.NotifyComponentAddition(e2, e2IsNew, + components::Model::typeId)); + + // add another entity to the view that isn't a newly created entity to make + // sure that calling NotifyComponent* methods on this entity doesn't modify + // the view's new entity data + EXPECT_FALSE(view.HasEntity(e2)); + EXPECT_FALSE(view.HasCachedComponentData(e2)); + auto e2ModelComp = components::Model(); + auto e2VisualComp = components::Visual(); + view.AddEntityWithComps(e2, e2IsNew, &e2ModelComp, &e2VisualComp); + view.AddEntityWithConstComps(e2, e2IsNew, &e2ModelComp, &e2VisualComp); + EXPECT_TRUE(view.HasCachedComponentData(e2)); + EXPECT_TRUE(view.HasEntity(e2)); + EXPECT_EQ(2u, view.Entities().size()); + EXPECT_NE(view.Entities().end(), view.Entities().find(e1)); + EXPECT_NE(view.Entities().end(), view.Entities().find(e2)); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_EQ(view.NewEntities().end(), view.NewEntities().find(e2)); + + // call NotifyComponentRemoval on the entity that was just added to the view + EXPECT_TRUE(view.NotifyComponentRemoval(e2, components::Model::typeId)); + EXPECT_FALSE(view.HasEntity(e2)); + EXPECT_EQ(1u, view.Entities().size()); + EXPECT_EQ(view.Entities().end(), view.Entities().find(e2)); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_TRUE(view.HasCachedComponentData(e2)); + + // call NotifyComponentRemoval on a component that was already notified of + // removal. While the notification should still take place, it will have no + // effect since this component was already removed + EXPECT_TRUE(view.NotifyComponentRemoval(e2, components::Model::typeId)); + EXPECT_FALSE(view.HasEntity(e2)); + EXPECT_EQ(1u, view.Entities().size()); + EXPECT_EQ(view.Entities().end(), view.Entities().find(e2)); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_TRUE(view.HasCachedComponentData(e2)); + + // call NotifyComponentAddition on the entity that was just added to the view + EXPECT_TRUE(view.NotifyComponentAddition(e2, e2IsNew, + components::Model::typeId)); + EXPECT_TRUE(view.HasCachedComponentData(e2)); + EXPECT_TRUE(view.HasEntity(e2)); + EXPECT_EQ(2u, view.Entities().size()); + EXPECT_NE(view.Entities().end(), view.Entities().find(e1)); + EXPECT_NE(view.Entities().end(), view.Entities().find(e2)); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_EQ(view.NewEntities().end(), view.NewEntities().find(e2)); + + // call NotifyComponentAddition on a component that was already notified of + // addition. While the notification should still take place, it will have no + // effect since this component was already added + EXPECT_TRUE(view.NotifyComponentAddition(e2, e2IsNew, + components::Model::typeId)); + EXPECT_TRUE(view.HasCachedComponentData(e2)); + EXPECT_TRUE(view.HasEntity(e2)); + EXPECT_EQ(2u, view.Entities().size()); + EXPECT_NE(view.Entities().end(), view.Entities().find(e1)); + EXPECT_NE(view.Entities().end(), view.Entities().find(e2)); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_EQ(view.NewEntities().end(), view.NewEntities().find(e2)); +} + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, ComponentTypeHasher) +{ + // Test the hash function for a std::vector to make + // sure that views with different component types (either a different type + // ordering or a different set of types alltogether) are considered unique + using ComponentVec = std::vector; + + // Create vectors with the same types, but different order + ComponentVec vec1 = { + components::Model::typeId, + components::Name::typeId, + components::Visual::typeId + }; + ComponentVec vec2 = { + components::Name::typeId, + components::Model::typeId, + components::Visual::typeId + }; + ComponentVec vec3 = { + components::Model::typeId, + components::Visual::typeId, + components::Name::typeId + }; + + // Create vectors with different types + ComponentVec vec4 = {components::Model::typeId}; + ComponentVec vec5 = {components::Name::typeId}; + ComponentVec vec6 = {components::Visual::typeId}; + ComponentVec vec7 = { + components::Model::typeId, + components::Name::typeId + }; + + // Test the hash function. Each vector defined above should be unique, which + // means that the std::unordered_set should have 7 elements in it after all + // insertions have been attempted (7 unique vectors were created) + std::unordered_set uniqueVecs; + uniqueVecs.insert(vec1); + uniqueVecs.insert(vec2); + uniqueVecs.insert(vec3); + uniqueVecs.insert(vec4); + uniqueVecs.insert(vec5); + uniqueVecs.insert(vec6); + uniqueVecs.insert(vec7); + EXPECT_EQ(7u, uniqueVecs.size()); +} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 4a750222d6..b8c5fffe9c 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -46,6 +46,7 @@ target_link_libraries(${ign_lib_target} set (sources Barrier.cc + BaseView.cc Conversions.cc EntityComponentManager.cc LevelManager.cc @@ -60,7 +61,6 @@ set (sources SystemLoader.cc TestFixture.cc Util.cc - View.cc World.cc cmd/ModelCommandAPI.cc ${PROTO_PRIVATE_SRC} @@ -70,25 +70,26 @@ set (sources set (gtest_sources ${gtest_sources} Barrier_TEST.cc - Component_TEST.cc + BaseView_TEST.cc ComponentFactory_TEST.cc + Component_TEST.cc Conversions_TEST.cc EntityComponentManager_TEST.cc EventManager_TEST.cc - ign_TEST.cc Link_TEST.cc Model_TEST.cc ModelCommandAPI_TEST.cc SdfEntityCreator_TEST.cc SdfGenerator_TEST.cc - Server_TEST.cc ServerConfig_TEST.cc + Server_TEST.cc SimulationRunner_TEST.cc - System_TEST.cc SystemLoader_TEST.cc + System_TEST.cc TestFixture_TEST.cc Util_TEST.cc World_TEST.cc + ign_TEST.cc network/NetworkConfig_TEST.cc network/PeerTracker_TEST.cc network/NetworkManager_TEST.cc diff --git a/src/ComponentFactory_TEST.cc b/src/ComponentFactory_TEST.cc index af782ea8ca..b9021aa605 100644 --- a/src/ComponentFactory_TEST.cc +++ b/src/ComponentFactory_TEST.cc @@ -19,6 +19,7 @@ #include "ignition/gazebo/test_config.hh" #include "ignition/gazebo/components/Component.hh" #include "ignition/gazebo/components/Factory.hh" +#include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Pose.hh" using namespace ignition; @@ -52,8 +53,7 @@ TEST_F(ComponentFactoryTest, Register) auto registeredCount = factory->TypeIds().size(); factory->Register("ign_gazebo_components.MyCustom", - new components::ComponentDescriptor(), - new components::StorageDescriptor()); + new components::ComponentDescriptor()); // Check now it has type id EXPECT_NE(0u, MyCustom::typeId); @@ -68,8 +68,7 @@ TEST_F(ComponentFactoryTest, Register) // Fail to register same component twice factory->Register("ign_gazebo_components.MyCustom", - new components::ComponentDescriptor(), - new components::StorageDescriptor()); + new components::ComponentDescriptor()); EXPECT_EQ(registeredCount + 1, factory->TypeIds().size()); @@ -77,8 +76,7 @@ TEST_F(ComponentFactoryTest, Register) using Duplicate = components::Component; factory->Register("ign_gazebo_components.MyCustom", - new components::ComponentDescriptor(), - new components::StorageDescriptor()); + new components::ComponentDescriptor()); EXPECT_EQ(registeredCount + 1, factory->TypeIds().size()); @@ -99,12 +97,12 @@ TEST_F(ComponentFactoryTest, New) { auto comp = factory->New(123456789); - ASSERT_TRUE(comp == nullptr); + ASSERT_EQ(nullptr, comp); } { auto comp = factory->New(); - ASSERT_TRUE(comp != nullptr); + ASSERT_NE(nullptr, comp); EXPECT_NE(0u, comp->typeId); EXPECT_EQ(comp->typeId, components::Pose::typeId); @@ -112,19 +110,33 @@ TEST_F(ComponentFactoryTest, New) { auto comp = factory->New(components::Pose::typeId); - ASSERT_TRUE(comp != nullptr); + ASSERT_NE(nullptr, comp); EXPECT_NE(0u, comp->TypeId()); - EXPECT_TRUE(nullptr != static_cast(comp.get())); + EXPECT_NE(nullptr, static_cast(comp.get())); } { - auto storage = factory->NewStorage(components::Pose::typeId); - ASSERT_TRUE(storage != nullptr); + // Test constructing a component with pre-defined data - EXPECT_NE(nullptr, static_cast *>( - storage.get())); + // Test a valid pre-defined component + ignition::math::Pose3d pose(1, 2, 3, 4, 5, 6); + components::Pose poseComp(pose); + auto comp = factory->New(components::Pose::typeId, &poseComp); + ASSERT_NE(nullptr, comp); + EXPECT_NE(0u, comp->TypeId()); + auto derivedComp = static_cast(comp.get()); + ASSERT_NE(nullptr, derivedComp); + EXPECT_EQ(pose, derivedComp->Data()); + + // Test an invalid pre-defined component + comp = factory->New(components::Pose::typeId, nullptr); + ASSERT_EQ(nullptr, comp); + + // Test mistmatching component types + comp = factory->New(components::Name::typeId, &poseComp); + ASSERT_EQ(nullptr, comp); } } diff --git a/src/Component_TEST.cc b/src/Component_TEST.cc index 53b07232d2..963948a850 100644 --- a/src/Component_TEST.cc +++ b/src/Component_TEST.cc @@ -67,8 +67,7 @@ TEST_F(ComponentTest, DataByMove) using CustomComponent = components::Component, class CustomComponentTag>; factory->Register("ign_gazebo_components.MyCustom", - new components::ComponentDescriptor(), - new components::StorageDescriptor()); + new components::ComponentDescriptor()); EntityComponentManager ecm; Entity entity = ecm.CreateEntity(); diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index db1b23d10f..906ad24166 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -15,17 +15,21 @@ * */ +#include "ignition/gazebo/EntityComponentManager.hh" + #include +#include #include #include #include +#include #include #include #include + #include "ignition/gazebo/components/Component.hh" #include "ignition/gazebo/components/Factory.hh" -#include "ignition/gazebo/EntityComponentManager.hh" using namespace ignition; using namespace gazebo; @@ -45,11 +49,6 @@ class ignition::gazebo::EntityComponentManagerPrivate public: void InsertEntityRecursive(Entity _entity, std::unordered_set &_set); - /// \brief Register a new component type. - /// \param[in] _typeId Type if of the new component. - /// \return True if created successfully. - public: bool CreateComponentStorage(const ComponentTypeId _typeId); - /// \brief Allots the work for multiple threads prior to running /// `AddEntityToMessage`. public: void CalculateStateThreadLoad(); @@ -78,20 +77,33 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \param[in] _entity Entity that has component newly modified public: void AddModifiedComponent(const Entity &_entity); - /// \brief Map of component storage classes. The key is a component - /// type id, and the value is a pointer to the component storage. - public: std::unordered_map> components; + /// \brief Check whether a component is marked as a component that is + /// currently removed or not. + /// \param[in] _entity The entity + /// \param[in] _typeId The type ID for the component that belongs to _entity + /// \return True if _entity has a component of type _typeId that is currently + /// removed. False otherwise + public: bool ComponentMarkedAsRemoved(const Entity _entity, + const ComponentTypeId _typeId) const; + + /// \brief All component types that have ever been created. + public: std::unordered_set createdCompTypes; /// \brief A graph holding all entities, arranged according to their /// parenting. public: EntityGraph entities; - /// \brief Components that have been changed through a peridic change. - public: std::set periodicChangedComponents; + /// \brief Components that have been changed through a periodic change. + /// The key is the type of component which has changed, and the value is the + /// entities that had this type of component changed. + public: std::unordered_map> + periodicChangedComponents; /// \brief Components that have been changed through a one-time change. - public: std::set oneTimeChangedComponents; + /// The key is the type of component which has changed, and the value is the + /// entities that had this type of component changed. + public: std::unordered_map> + oneTimeChangedComponents; /// \brief Entities that have just been created public: std::unordered_set newlyCreatedEntities; @@ -108,26 +120,6 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \brief Flag that indicates if all entities should be removed. public: bool removeAllEntities{false}; - /// \brief True if the entityComponents map was changed. Primarily used - /// by the multithreading functionality in `State()` to allocate work to - /// each thread. - public: bool entityComponentsDirty{true}; - - /// \brief The set of components that each entity has. - /// NOTE: Any modification of this data structure must be followed - /// by setting `entityComponentsDirty` to true. - public: std::unordered_map> entityComponents; - - /// \brief A vector of iterators to evenly distributed spots in the - /// `entityComponents` map. Threads in the `State` function use this - /// vector for easy access of their pre-allocated work. This vector - /// is recalculated if `entityComponents` is changed (when - /// `entityComponentsDirty` == true). - public: std::vector>::iterator> - entityComponentIterators; - /// \brief A mutex to protect newly created entities. public: std::mutex entityCreatedMutex; @@ -141,7 +133,8 @@ class ignition::gazebo::EntityComponentManagerPrivate public: mutable std::mutex removedComponentsMutex; /// \brief The set of all views. - public: mutable std::map views; + public: mutable std::unordered_map, detail::ComponentTypeHasher> views; /// \brief Cache of previously queried descendants. The key is the parent /// entity for which descendants were queried, and the value are all its @@ -152,10 +145,56 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \brief Keep track of entities already used to ensure uniqueness. public: uint64_t entityCount{0}; - /// \brief Unordered multimap of removed components. The key is the entity to - /// which belongs the component, and the value is the component being - /// removed. - std::unordered_multimap removedComponents; + /// \brief Unordered map of removed components. The key is the entity to + /// which belongs the component, and the value is a set of the component types + /// being removed. + public: std::unordered_map> + removedComponents; + + /// \brief All components that have been removed. The difference between + /// removedComponents and componentsMarkedAsRemoved is that removedComponents + /// keeps track of components that were removed in the current simulation + /// step, while componentsMarkedAsRemoved keeps track of components that are + /// currently removed based on all simulation steps. + public: std::unordered_map> + componentsMarkedAsRemoved; + + /// \brief A map of an entity to its components + public: std::unordered_map>> + componentStorage; + + /// \brief A map that keeps track of where each type of component is + /// located in the componentStorage vector. Since the componentStorage vector + /// is of type BaseComponent, we need to keep track of which component type + /// corresponds to a given index in the vector so that we can cast the + /// BaseComponent to this type if needed. + /// + /// The key of this map is the Entity, and the value is a map of the + /// component type to the corresponding index in the + /// componentStorage vector (a component of a particular type is + /// only a key for the value map if a component of this type exists in + /// the componentStorage vector) + /// + /// NOTE: Any modification of this data structure must be followed + /// by setting `componentTypeIndexDirty` to true. + public: std::unordered_map> + componentTypeIndex; + + /// \brief A vector of iterators to evenly distributed spots in the + /// `componentTypeIndex` map. Threads in the `State` function use this + /// vector for easy access of their pre-allocated work. This vector + /// is recalculated if `componentTypeIndex` is changed (when + /// `componentTypeIndexDirty` == true). + public: std::vector>::iterator> + componentTypeIndexIterators; + + /// \brief True if the componentTypeIndex map was changed. Primarily used + /// by the multithreading functionality in `State()` to allocate work to + /// each thread. + public: bool componentTypeIndexDirty{true}; }; ////////////////////////////////////////////////// @@ -203,6 +242,24 @@ Entity EntityComponentManagerPrivate::CreateEntityImplementation(Entity _entity) // Reset descendants cache this->descendantCache.clear(); + const auto result = this->componentStorage.insert({_entity, + std::vector>()}); + if (!result.second) + { + ignwarn << "Attempted to add entity [" << _entity + << "] to component storage, but this entity is already in component " + << "storage.\n"; + } + + const auto result2 = this->componentTypeIndex.insert({_entity, + std::unordered_map()}); + if (!result2.second) + { + ignwarn << "Attempted to add entity [" << _entity + << "] to component type index, but this entity is already in component " + << "type index.\n"; + } + return _entity; } @@ -214,7 +271,7 @@ void EntityComponentManager::ClearNewlyCreatedEntities() for (auto &view : this->dataPtr->views) { - view.second.ClearNewEntities(); + view.second->ResetNewEntityState(); } } @@ -260,7 +317,10 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity, for (const auto &removedEntity : tmpToRemoveEntities) { - this->UpdateViews(removedEntity); + for (auto &view : this->dataPtr->views) + { + view.second->MarkEntityToRemove(removedEntity); + } } } @@ -285,15 +345,13 @@ void EntityComponentManager::ProcessRemoveEntityRequests() IGN_PROFILE("RemoveAll"); this->dataPtr->removeAllEntities = false; this->dataPtr->entities = EntityGraph(); - this->dataPtr->entityComponents.clear(); this->dataPtr->toRemoveEntities.clear(); - this->dataPtr->entityComponentsDirty = true; + this->dataPtr->componentsMarkedAsRemoved.clear(); - for (std::pair> &comp: this->dataPtr->components) - { - comp.second->RemoveAll(); - } + // reset the entity component storage + this->dataPtr->componentStorage.clear(); + this->dataPtr->componentTypeIndex.clear(); + this->dataPtr->componentTypeIndexDirty = true; // All views are now invalid. this->dataPtr->views.clear(); @@ -311,24 +369,15 @@ void EntityComponentManager::ProcessRemoveEntityRequests() // Remove from graph this->dataPtr->entities.RemoveVertex(entity); - auto entityIter = this->dataPtr->entityComponents.find(entity); - // Remove the components, if any. - if (entityIter != this->dataPtr->entityComponents.end()) - { - for (const auto &key : entityIter->second) - { - this->dataPtr->components.at(key.first)->Remove(key.second); - } - - // Remove the entry in the entityComponent map - this->dataPtr->entityComponents.erase(entity); - this->dataPtr->entityComponentsDirty = true; - } + this->dataPtr->componentsMarkedAsRemoved.erase(entity); + this->dataPtr->componentStorage.erase(entity); + this->dataPtr->componentTypeIndex.erase(entity); + this->dataPtr->componentTypeIndexDirty = true; // Remove the entity from views. for (auto &view : this->dataPtr->views) { - view.second.RemoveEntity(entity, view.first); + view.second->RemoveEntity(entity); } } // Clear the set of entities to remove. @@ -342,48 +391,61 @@ void EntityComponentManager::ProcessRemoveEntityRequests() ///////////////////////////////////////////////// bool EntityComponentManager::RemoveComponent( const Entity _entity, const ComponentTypeId &_typeId) -{ - auto componentId = this->EntityComponentIdFromType(_entity, _typeId); - ComponentKey key{_typeId, componentId}; - return this->RemoveComponent(_entity, key); -} - -///////////////////////////////////////////////// -bool EntityComponentManager::RemoveComponent( - const Entity _entity, const ComponentKey &_key) { IGN_PROFILE("EntityComponentManager::RemoveComponent"); // Make sure the entity exists and has the component. - if (!this->EntityHasComponent(_entity, _key)) + if (!this->EntityHasComponentType(_entity, _typeId)) return false; - this->dataPtr->components.at(_key.first)->Remove(_key.second); - this->dataPtr->entityComponents[_entity].erase(_key.first); - this->dataPtr->oneTimeChangedComponents.erase(_key); - this->dataPtr->periodicChangedComponents.erase(_key); - this->dataPtr->entityComponentsDirty = true; + auto oneTimeIter = this->dataPtr->oneTimeChangedComponents.find(_typeId); + if (oneTimeIter != this->dataPtr->oneTimeChangedComponents.end()) + { + oneTimeIter->second.erase(_entity); + if (oneTimeIter->second.empty()) + this->dataPtr->oneTimeChangedComponents.erase(oneTimeIter); + } - this->UpdateViews(_entity); + auto periodicIter = this->dataPtr->periodicChangedComponents.find(_typeId); + if (periodicIter != this->dataPtr->periodicChangedComponents.end()) + { + periodicIter->second.erase(_entity); + if (periodicIter->second.empty()) + this->dataPtr->periodicChangedComponents.erase(periodicIter); + } + + auto compPtr = this->ComponentImplementation(_entity, _typeId); + if (compPtr) + { + this->dataPtr->componentsMarkedAsRemoved[_entity].insert(_typeId); + + // update views to reflect the component removal + for (auto &viewPair : this->dataPtr->views) + viewPair.second->NotifyComponentRemoval(_entity, _typeId); + } this->dataPtr->AddModifiedComponent(_entity); // Add component to map of removed components { std::lock_guard lock(this->dataPtr->removedComponentsMutex); - this->dataPtr->removedComponents.insert(std::make_pair(_entity, _key)); + this->dataPtr->removedComponents[_entity].insert(_typeId); } return true; } +///////////////////////////////////////////////// +bool EntityComponentManager::RemoveComponent( + const Entity _entity, const ComponentKey &_key) +{ + return this->RemoveComponent(_entity, _key.first); +} + ///////////////////////////////////////////////// bool EntityComponentManager::EntityHasComponent(const Entity _entity, const ComponentKey &_key) const { - if (!this->HasEntity(_entity)) - return false; - auto &compMap = this->dataPtr->entityComponents[_entity]; - return compMap.find(_key.first) != compMap.end(); + return this->EntityHasComponentType(_entity, _key.first); } ///////////////////////////////////////////////// @@ -393,13 +455,9 @@ bool EntityComponentManager::EntityHasComponentType(const Entity _entity, if (!this->HasEntity(_entity)) return false; - auto iter = this->dataPtr->entityComponents.find(_entity); - - if (iter == this->dataPtr->entityComponents.end()) - return false; + auto comp = this->ComponentImplementation(_entity, _typeId); - auto typeIter = iter->second.find(_typeId); - return (typeIter != iter->second.end()); + return comp != nullptr; } ///////////////////////////////////////////////// @@ -428,26 +486,31 @@ ComponentState EntityComponentManager::ComponentState(const Entity _entity, { auto result = ComponentState::NoChange; - auto ecIter = this->dataPtr->entityComponents.find(_entity); + auto ctIter = this->dataPtr->componentTypeIndex.find(_entity); - if (ecIter == this->dataPtr->entityComponents.end()) + if (ctIter == this->dataPtr->componentTypeIndex.end()) return result; - auto typeKey = ecIter->second.find(_typeId); - if (typeKey == ecIter->second.end()) + auto typeIter = ctIter->second.find(_typeId); + if (typeIter == ctIter->second.end() || + this->dataPtr->ComponentMarkedAsRemoved(_entity, _typeId)) return result; - ComponentKey key{_typeId, typeKey->second}; + auto typeId = typeIter->first; - if (this->dataPtr->oneTimeChangedComponents.find(key) != - this->dataPtr->oneTimeChangedComponents.end()) + auto oneTimeIter = this->dataPtr->oneTimeChangedComponents.find(typeId); + if (oneTimeIter != this->dataPtr->oneTimeChangedComponents.end() && + oneTimeIter->second.find(_entity) != oneTimeIter->second.end()) { result = ComponentState::OneTimeChange; } - else if (this->dataPtr->periodicChangedComponents.find(key) != - this->dataPtr->periodicChangedComponents.end()) + else { - result = ComponentState::PeriodicChange; + auto periodicIter = + this->dataPtr->periodicChangedComponents.find(typeId); + if (periodicIter != this->dataPtr->periodicChangedComponents.end() && + periodicIter->second.find(_entity) != periodicIter->second.end()) + result = ComponentState::PeriodicChange; } return result; @@ -479,9 +542,9 @@ std::unordered_set EntityComponentManager::ComponentTypesWithPeriodicChanges() const { std::unordered_set periodicComponents; - for (const auto& compPair : this->dataPtr->periodicChangedComponents) + for (const auto& typeToEntityPtrs : this->dataPtr->periodicChangedComponents) { - periodicComponents.insert(compPair.first); + periodicComponents.insert(typeToEntityPtrs.first); } return periodicComponents; } @@ -528,7 +591,7 @@ bool EntityComponentManager::SetParentEntity(const Entity _child, } ///////////////////////////////////////////////// -ComponentKey EntityComponentManager::CreateComponentImplementation( +bool EntityComponentManager::CreateComponentImplementation( const Entity _entity, const ComponentTypeId _componentTypeId, const components::BaseComponent *_data) { @@ -538,48 +601,114 @@ ComponentKey EntityComponentManager::CreateComponentImplementation( ignerr << "Trying to create a component of type [" << _componentTypeId << "] attached to entity [" << _entity << "], but this entity does not " << "exist. This create component request will be ignored." << std::endl; - return ComponentKey(); + return false; } - // If type hasn't been instantiated yet, create a storage for it - if (!this->HasComponentType(_componentTypeId)) + // if this is the first time this component type is being created, make sure + // the component type to be created is valid + if (!this->HasComponentType(_componentTypeId) && + !components::Factory::Instance()->HasType(_componentTypeId)) { - if (!this->dataPtr->CreateComponentStorage(_componentTypeId)) - { - ignerr << "Failed to create component of type [" << _componentTypeId - << "] for entity [" << _entity - << "]. Type has not been properly registered." << std::endl; - return ComponentKey(); - } + ignerr << "Failed to create component of type [" << _componentTypeId + << "] for entity [" << _entity + << "]. Type has not been properly registered." << std::endl; + return false; } + // assume the component data needs to be updated externally unless this + // component is a brand new creation/addition + bool updateData = true; + this->dataPtr->AddModifiedComponent(_entity); + this->dataPtr->oneTimeChangedComponents[_componentTypeId].insert(_entity); - // Instantiate the new component. - std::pair componentIdPair = - this->dataPtr->components[_componentTypeId]->Create(_data); + // make sure the entity exists + auto typeMapIter = this->dataPtr->componentTypeIndex.find(_entity); + if (typeMapIter == this->dataPtr->componentTypeIndex.end()) + { + ignerr << "Attempt to create a component of type [" << _componentTypeId + << "] attached to entity [" << _entity + << "] failed: entity not in componentTypeIndex." << std::endl; + return false; + } - ComponentKey componentKey{_componentTypeId, componentIdPair.first}; + auto entityCompIter = this->dataPtr->componentStorage.find(_entity); + if (entityCompIter == this->dataPtr->componentStorage.end()) + { + ignerr << "Attempt to create a component of type [" << _componentTypeId + << "] attached to entity [" << _entity + << "] failed: entity not in storage." << std::endl; + return false; + } + + // Instantiate the new component. + auto newComp = components::Factory::Instance()->New(_componentTypeId, _data); - this->dataPtr->entityComponents[_entity].insert( - {_componentTypeId, componentIdPair.first}); - this->dataPtr->oneTimeChangedComponents.insert(componentKey); - this->dataPtr->entityComponentsDirty = true; + const auto compIdxIter = typeMapIter->second.find(_componentTypeId); + // If entity has never had a component of this type + if (compIdxIter == typeMapIter->second.end()) + { + const auto vectorIdx = entityCompIter->second.size(); + entityCompIter->second.push_back(std::move(newComp)); + this->dataPtr->componentTypeIndex[_entity][_componentTypeId] = vectorIdx; + this->dataPtr->componentTypeIndexDirty = true; - if (componentIdPair.second) - this->RebuildViews(); + updateData = false; + for (auto &viewPair : this->dataPtr->views) + { + auto &view = viewPair.second; + if (this->EntityMatches(_entity, view->ComponentTypes())) + view->MarkEntityToAdd(_entity, this->IsNewEntity(_entity)); + } + } else - this->UpdateViews(_entity); + { + // if the pre-existing component is marked as removed, this means that the + // component was added to the entity previously, but later removed. In this + // case, a re-addition of the component is occuring. If the pre-existing + // component is not marked as removed, this means that the component was + // added to the entity previously and never removed. In this case, we are + // simply modifying the data of the pre-existing component (the modification + // of the data is done externally in a templated ECM method call, because we + // need the derived component class in order to update the derived component + // data) + auto existingCompPtr = entityCompIter->second.at(compIdxIter->second).get(); + if (!existingCompPtr) + { + ignerr << "Internal error: entity [" << _entity << "] has a component of " + << "type [" << _componentTypeId << "] in the storage, but the instance " + << "of this component is nullptr. This should never happen!" + << std::endl; + return false; + } + else if (this->dataPtr->ComponentMarkedAsRemoved(_entity, _componentTypeId)) + { + this->dataPtr->componentsMarkedAsRemoved[_entity].erase(_componentTypeId); - return componentKey; + for (auto &viewPair : this->dataPtr->views) + { + viewPair.second->NotifyComponentAddition(_entity, + this->IsNewEntity(_entity), _componentTypeId); + } + } + } + + this->dataPtr->createdCompTypes.insert(_componentTypeId); + + return updateData; } ///////////////////////////////////////////////// bool EntityComponentManager::EntityMatches(Entity _entity, const std::set &_types) const { - auto iter = this->dataPtr->entityComponents.find(_entity); - if (iter == this->dataPtr->entityComponents.end()) + auto iter = this->dataPtr->componentTypeIndex.find(_entity); + if (iter == this->dataPtr->componentTypeIndex.end()) + return false; + + // quick check: the entity cannot match _types if _types is larger than the + // number of component types the entity has + if (_types.size() > iter->second.size()) return false; // \todo(nkoenig) The performance of this could be improved. @@ -589,127 +718,74 @@ bool EntityComponentManager::EntityMatches(Entity _entity, for (const ComponentTypeId &type : _types) { auto typeIter = iter->second.find(type); - if (typeIter == iter->second.end()) + if (typeIter == iter->second.end() || + this->dataPtr->ComponentMarkedAsRemoved(_entity, type)) return false; } return true; } -///////////////////////////////////////////////// -ComponentId EntityComponentManager::EntityComponentIdFromType( - const Entity _entity, const ComponentTypeId _type) const -{ - auto ecIter = this->dataPtr->entityComponents.find(_entity); - - if (ecIter == this->dataPtr->entityComponents.end()) - return -1; - - auto typeIter = ecIter->second.find(_type); - if (typeIter != ecIter->second.end()) - return typeIter->second; - - return -1; -} - ///////////////////////////////////////////////// const components::BaseComponent *EntityComponentManager::ComponentImplementation( const Entity _entity, const ComponentTypeId _type) const { IGN_PROFILE("EntityComponentManager::ComponentImplementation"); - auto ecIter = this->dataPtr->entityComponents.find(_entity); - if (ecIter == this->dataPtr->entityComponents.end()) + // make sure the entity exists + const auto typeMapIter = this->dataPtr->componentTypeIndex.find(_entity); + if (typeMapIter == this->dataPtr->componentTypeIndex.end()) return nullptr; - auto typeIter = ecIter->second.find(_type); - if (typeIter != ecIter->second.end()) - return this->dataPtr->components.at(_type)->Component( - typeIter->second); - - return nullptr; -} - -///////////////////////////////////////////////// -components::BaseComponent *EntityComponentManager::ComponentImplementation( - const Entity _entity, const ComponentTypeId _type) -{ - auto ecIter = this->dataPtr->entityComponents.find(_entity); - - if (ecIter == this->dataPtr->entityComponents.end()) + // make sure the component type exists for the entity + const auto compIdxIter = typeMapIter->second.find(_type); + if (compIdxIter == typeMapIter->second.end()) return nullptr; - auto typeIter = ecIter->second.find(_type); - if (typeIter != ecIter->second.end()) - return this->dataPtr->components.at(_type)->Component(typeIter->second); - - return nullptr; -} + // get the pointer to the component + const auto compVecIter = this->dataPtr->componentStorage.find(_entity); + if (compVecIter == this->dataPtr->componentStorage.end()) + { + ignerr << "Internal error: Entity [" << _entity + << "] is missing in storage, but is in " + << "componentTypeIndex. This should never happen!" << std::endl; + return nullptr; + } -///////////////////////////////////////////////// -const components::BaseComponent - *EntityComponentManager::ComponentImplementation( - const ComponentKey &_key) const -{ - if (this->dataPtr->components.find(_key.first) != - this->dataPtr->components.end()) + auto compPtr = compVecIter->second.at(compIdxIter->second).get(); + if (nullptr == compPtr) { - return this->dataPtr->components.at(_key.first)->Component(_key.second); + ignerr << "Internal error: entity [" << _entity << "] has a component of " + << "type [" << _type << "] in the storage, but the instance " + << "of this component is nullptr. This should never happen!" + << std::endl; + return nullptr; } + + // Return component if not marked as removed. + if (!this->dataPtr->ComponentMarkedAsRemoved(_entity, _type)) + return compPtr; + return nullptr; } ///////////////////////////////////////////////// components::BaseComponent *EntityComponentManager::ComponentImplementation( - const ComponentKey &_key) + const Entity _entity, const ComponentTypeId _type) { - if (this->dataPtr->components.find(_key.first) != - this->dataPtr->components.end()) - { - return this->dataPtr->components.at(_key.first)->Component(_key.second); - } - return nullptr; + // Call the const version of the function + return const_cast( + static_cast( + *this).ComponentImplementation(_entity, _type)); } ///////////////////////////////////////////////// bool EntityComponentManager::HasComponentType( const ComponentTypeId _typeId) const { - return this->dataPtr->components.find(_typeId) != - this->dataPtr->components.end(); -} - -///////////////////////////////////////////////// -bool EntityComponentManagerPrivate::CreateComponentStorage( - const ComponentTypeId _typeId) -{ - auto storage = components::Factory::Instance()->NewStorage(_typeId); - - if (nullptr == storage) - { - ignerr << "Internal errror: failed to create storage for type [" << _typeId - << "]" << std::endl; - return false; - } - - this->components[_typeId] = std::move(storage); - igndbg << "Using components of type [" << _typeId << "] / [" - << components::Factory::Instance()->Name(_typeId) << "].\n"; - - return true; -} - -///////////////////////////////////////////////// -components::BaseComponent *EntityComponentManager::First( - const ComponentTypeId _componentTypeId) -{ - auto iter = this->dataPtr->components.find(_componentTypeId); - if (iter != this->dataPtr->components.end()) - { - return iter->second->First(); - } - return nullptr; + return this->dataPtr->createdCompTypes.find(_typeId) != + this->dataPtr->createdCompTypes.end(); } ////////////////////////////////////////////////// @@ -719,85 +795,51 @@ const EntityGraph &EntityComponentManager::Entities() const } ////////////////////////////////////////////////// -bool EntityComponentManager::FindView(const std::set &_types, - std::map::iterator &_iter) const +detail::BaseView *EntityComponentManager::FindView( + const std::vector &_types) const { std::lock_guard lockViews(this->dataPtr->viewsMutex); - _iter = this->dataPtr->views.find(_types); - return _iter != this->dataPtr->views.end(); + auto iter = this->dataPtr->views.find(_types); + if (iter != this->dataPtr->views.end()) + return iter->second.get(); + return nullptr; } ////////////////////////////////////////////////// -std::map::iterator - EntityComponentManager::AddView(const std::set &_types, - detail::View &&_view) const +detail::BaseView *EntityComponentManager::AddView( + const detail::ComponentTypeKey &_types, + std::unique_ptr _view) const { // If the view already exists, then the map will return the iterator to // the location that prevented the insertion. std::lock_guard lockViews(this->dataPtr->viewsMutex); - return this->dataPtr->views.insert( + auto iter = this->dataPtr->views.insert( std::make_pair(_types, std::move(_view))).first; -} - -////////////////////////////////////////////////// -void EntityComponentManager::UpdateViews(const Entity _entity) -{ - IGN_PROFILE("EntityComponentManager::UpdateViews"); - for (auto &view : this->dataPtr->views) - { - // Add/update the entity if it matches the view. - if (this->EntityMatches(_entity, view.first)) - { - view.second.AddEntity(_entity, this->IsNewEntity(_entity)); - // If there is a request to delete this entity, update the view as - // well - if (this->IsMarkedForRemoval(_entity)) - { - view.second.AddEntityToRemoved(_entity); - } - for (const ComponentTypeId &compTypeId : view.first) - { - view.second.AddComponent(_entity, compTypeId, - this->EntityComponentIdFromType(_entity, compTypeId)); - } - } - else - { - view.second.RemoveEntity(_entity, view.first); - } - } + return iter->second.get(); } ////////////////////////////////////////////////// void EntityComponentManager::RebuildViews() { IGN_PROFILE("EntityComponentManager::RebuildViews"); - for (auto &view : this->dataPtr->views) + for (auto &viewPair : this->dataPtr->views) { - view.second.entities.clear(); - view.second.components.clear(); + auto &view = viewPair.second; + view->Reset(); + // Add all the entities that match the component types to the // view. for (const auto &vertex : this->dataPtr->entities.Vertices()) { Entity entity = vertex.first; - if (this->EntityMatches(entity, view.first)) + if (this->EntityMatches(entity, view->ComponentTypes())) { - view.second.AddEntity(entity, this->IsNewEntity(entity)); + view->MarkEntityToAdd(entity, this->IsNewEntity(entity)); + // If there is a request to delete this entity, update the view as // well if (this->IsMarkedForRemoval(entity)) - { - view.second.AddEntityToRemoved(entity); - } - // Store pointers to all the components. This recursively adds - // all the ComponentTypeTs that belong to the entity to the view. - for (const ComponentTypeId &compTypeId : view.first) - { - view.second.AddComponent(entity, compTypeId, - this->EntityComponentIdFromType( - entity, compTypeId)); - } + view->MarkEntityToRemove(entity); } } } @@ -809,12 +851,12 @@ void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity, const std::unordered_set &_types) { std::lock_guard lock(this->removedComponentsMutex); - auto entRemovedComps = this->removedComponents.equal_range(_entity); - for (auto it = entRemovedComps.first; it != entRemovedComps.second; ++it) + auto entRemovedCompsIter = this->removedComponents.find(_entity); + if (entRemovedCompsIter == this->removedComponents.end()) + return; + for (const auto &compType : entRemovedCompsIter->second) { - auto removedComponent = it->second; - - if (!_types.empty() && _types.find(removedComponent.first) == _types.end()) + if (!_types.empty() && _types.find(compType) == _types.end()) { continue; } @@ -823,7 +865,7 @@ void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity, // Empty data is needed for the component to be processed afterwards compMsg->set_component(" "); - compMsg->set_type(removedComponent.first); + compMsg->set_type(compType); compMsg->set_remove(true); } } @@ -834,13 +876,16 @@ void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity, const std::unordered_set &_types) { std::lock_guard lock(this->removedComponentsMutex); - uint64_t nEntityKeys = this->removedComponents.count(_entity); + auto entRemovedCompsIter = this->removedComponents.find(_entity); + if (entRemovedCompsIter == this->removedComponents.end()) + return; + uint64_t nEntityKeys = entRemovedCompsIter->second.size(); if (nEntityKeys == 0) return; // The message need not necessarily contain the entity initially. For // instance, when AddEntityToMessage() calls this function, the entity may - // have some removed components but none in entityComponents that changed, + // have some removed components but none that changed, // so the entity may not have been added to the message beforehand. auto entIter = _msg.mutable_entities()->find(_entity); if (entIter == _msg.mutable_entities()->end()) @@ -852,12 +897,9 @@ void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity, .first; } - auto entRemovedComps = this->removedComponents.equal_range(_entity); - for (auto it = entRemovedComps.first; it != entRemovedComps.second; ++it) + for (const auto &compType : entRemovedCompsIter->second) { - auto removedComponent = it->second; - - if (!_types.empty() && _types.find(removedComponent.first) == _types.end()) + if (!_types.empty() && _types.find(compType) == _types.end()) { continue; } @@ -866,11 +908,11 @@ void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity, // Empty data is needed for the component to be processed afterwards compMsg.set_component(" "); - compMsg.set_type(removedComponent.first); + compMsg.set_type(compType); compMsg.set_remove(true); (*(entIter->second.mutable_components()))[ - static_cast(removedComponent.first)] = compMsg; + static_cast(compType)] = compMsg; } } @@ -880,8 +922,8 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg, { auto entityMsg = _msg.add_entities(); entityMsg->set_id(_entity); - auto iter = this->dataPtr->entityComponents.find(_entity); - if (iter == this->dataPtr->entityComponents.end()) + auto iter = this->dataPtr->componentTypeIndex.find(_entity); + if (iter == this->dataPtr->componentTypeIndex.end()) return; if (this->dataPtr->toRemoveEntities.find(_entity) != @@ -895,9 +937,10 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg, auto types = _types; if (types.empty()) { - for (auto &type : this->dataPtr->entityComponents[_entity]) + for (auto &type : this->dataPtr->componentTypeIndex[_entity]) { - types.insert(type.first); + if (!this->dataPtr->ComponentMarkedAsRemoved(_entity, type.first)) + types.insert(type.first); } } @@ -906,12 +949,14 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg, // If the entity does not have the component, continue auto typeIter = iter->second.find(type); if (typeIter == iter->second.end()) - { continue; - } - auto compMsg = entityMsg->add_components(); + // The component instance is nullptr if the component was removed auto compBase = this->ComponentImplementation(_entity, type); + if (nullptr == compBase) + continue; + + auto compMsg = entityMsg->add_components(); compMsg->set_type(compBase->TypeId()); std::ostringstream ostr; @@ -930,8 +975,8 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, Entity _entity, const std::unordered_set &_types, bool _full) const { - auto iter = this->dataPtr->entityComponents.find(_entity); - if (iter == this->dataPtr->entityComponents.end()) + auto iter = this->dataPtr->componentTypeIndex.find(_entity); + if (iter == this->dataPtr->componentTypeIndex.end()) return; // Set the default entity iterator to the end. This will allow us to know @@ -960,9 +1005,10 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, auto types = _types; if (types.empty()) { - for (auto &type : this->dataPtr->entityComponents[_entity]) + for (auto &type : this->dataPtr->componentTypeIndex[_entity]) { - types.insert(type.first); + if (!this->dataPtr->ComponentMarkedAsRemoved(_entity, type.first)) + types.insert(type.first); } } @@ -978,16 +1024,30 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, const components::BaseComponent *compBase = this->ComponentImplementation(_entity, type); - ComponentKey comp = {type, typeIter->second}; - // If not sending full state, skip unchanged components - if (!_full && - this->dataPtr->oneTimeChangedComponents.find(comp) == - this->dataPtr->oneTimeChangedComponents.end() && - this->dataPtr->periodicChangedComponents.find(comp) == - this->dataPtr->periodicChangedComponents.end()) + if (!_full) { - continue; + bool noChange = true; + + // see if the entity has a component of this particular type marked as a + // one time change + auto oneTimeIter = this->dataPtr->oneTimeChangedComponents.find(type); + if (oneTimeIter != this->dataPtr->oneTimeChangedComponents.end() && + oneTimeIter->second.find(_entity) != oneTimeIter->second.end()) + noChange = false; + + if (noChange) + { + // see if the entity has a component of this particular type marked as a + // periodic change + auto periodicIter = this->dataPtr->periodicChangedComponents.find(type); + if (periodicIter != this->dataPtr->periodicChangedComponents.end() && + periodicIter->second.find(_entity) != oneTimeIter->second.end()) + noChange = false; + } + + if (noChange) + continue; } /// Find the entity in the message, if not already found. @@ -1004,7 +1064,7 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, } } - auto compIter = entIter->second.mutable_components()->find(comp.first); + auto compIter = entIter->second.mutable_components()->find(type); // Find the component in the message, and add the component to the // message if it's not present. if (compIter == entIter->second.mutable_components()->end()) @@ -1012,8 +1072,8 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, msgs::SerializedComponent cmp; cmp.set_type(compBase->TypeId()); (*(entIter->second.mutable_components()))[ - static_cast(comp.first)] = cmp; - compIter = entIter->second.mutable_components()->find(comp.first); + static_cast(type)] = cmp; + compIter = entIter->second.mutable_components()->find(type); } // Serialize and store the message @@ -1080,44 +1140,44 @@ void EntityComponentManager::ChangedState( void EntityComponentManagerPrivate::CalculateStateThreadLoad() { // If the entity component vector is dirty, we need to recalculate the - // threads and each threads work load - if (!this->entityComponentsDirty) + // threads and each thread's work load + if (!this->componentTypeIndexDirty) return; - this->entityComponentsDirty = false; - this->entityComponentIterators.clear(); - auto startIt = this->entityComponents.begin(); - int numComponents = this->entityComponents.size(); + this->componentTypeIndexDirty = false; + this->componentTypeIndexIterators.clear(); + auto startIt = this->componentTypeIndex.begin(); + int numEntities = this->componentTypeIndex.size(); // Set the number of threads to spawn to the min of the calculated thread // count or max threads that the hardware supports int maxThreads = std::thread::hardware_concurrency(); - uint64_t numThreads = std::min(numComponents, maxThreads); + uint64_t numThreads = std::min(numEntities, maxThreads); - int componentsPerThread = static_cast(std::ceil( - static_cast(numComponents) / numThreads)); + int entitiesPerThread = static_cast(std::ceil( + static_cast(numEntities) / numThreads)); igndbg << "Updated state thread iterators: " << numThreads - << " threads processing around " << componentsPerThread - << " components each." << std::endl; + << " threads processing around " << entitiesPerThread + << " entities each." << std::endl; // Push back the starting iterator - this->entityComponentIterators.push_back(startIt); + this->componentTypeIndexIterators.push_back(startIt); for (uint64_t i = 0; i < numThreads; ++i) { - // If we have added all of the components to the iterator vector, we are + // If we have added all of the entities to the iterator vector, we are // done so push back the end iterator - numComponents -= componentsPerThread; - if (numComponents <= 0) + numEntities -= entitiesPerThread; + if (numEntities <= 0) { - this->entityComponentIterators.push_back( - this->entityComponents.end()); + this->componentTypeIndexIterators.push_back( + this->componentTypeIndex.end()); break; } - // Get the iterator to the next starting group of components - auto nextIt = std::next(startIt, componentsPerThread); - this->entityComponentIterators.push_back(nextIt); + // Get the iterator to the next starting group of entities + auto nextIt = std::next(startIt, entitiesPerThread); + this->componentTypeIndexIterators.push_back(nextIt); startIt = nextIt; } } @@ -1128,7 +1188,7 @@ ignition::msgs::SerializedState EntityComponentManager::State( const std::unordered_set &_types) const { ignition::msgs::SerializedState stateMsg; - for (const auto &it : this->dataPtr->entityComponents) + for (const auto &it : this->dataPtr->componentTypeIndex) { auto entity = it.first; if (!_entities.empty() && _entities.find(entity) == _entities.end()) @@ -1176,12 +1236,12 @@ void EntityComponentManager::State( }; // Spawn workers - uint64_t numThreads = this->dataPtr->entityComponentIterators.size() - 1; + uint64_t numThreads = this->dataPtr->componentTypeIndexIterators.size() - 1; for (uint64_t i = 0; i < numThreads; i++) { workers.push_back(std::thread(functor, - this->dataPtr->entityComponentIterators[i], - this->dataPtr->entityComponentIterators[i+1])); + this->dataPtr->componentTypeIndexIterators[i], + this->dataPtr->componentTypeIndexIterators[i+1])); } // Wait for each thread to finish processing its components @@ -1426,31 +1486,38 @@ void EntityComponentManager::SetChanged( const Entity _entity, const ComponentTypeId _type, gazebo::ComponentState _c) { - auto ecIter = this->dataPtr->entityComponents.find(_entity); - - if (ecIter == this->dataPtr->entityComponents.end()) + // make sure _entity exists + auto ecIter = this->dataPtr->componentTypeIndex.find(_entity); + if (ecIter == this->dataPtr->componentTypeIndex.end()) return; - auto typeIter = ecIter->second.find(_type); - if (typeIter == ecIter->second.end()) + // make sure the entity has a component of type _type + if (ecIter->second.find(_type) == ecIter->second.end() || + this->dataPtr->ComponentMarkedAsRemoved(_entity, _type)) return; - ComponentKey key{_type, typeIter->second}; - if (_c == ComponentState::PeriodicChange) { - this->dataPtr->periodicChangedComponents.insert(key); - this->dataPtr->oneTimeChangedComponents.erase(key); + this->dataPtr->periodicChangedComponents[_type].insert(_entity); + auto oneTimeIter = this->dataPtr->oneTimeChangedComponents.find(_type); + if (oneTimeIter != this->dataPtr->oneTimeChangedComponents.end()) + oneTimeIter->second.erase(_entity); } else if (_c == ComponentState::OneTimeChange) { - this->dataPtr->periodicChangedComponents.erase(key); - this->dataPtr->oneTimeChangedComponents.insert(key); + auto periodicIter = this->dataPtr->periodicChangedComponents.find(_type); + if (periodicIter != this->dataPtr->periodicChangedComponents.end()) + periodicIter->second.erase(_entity); + this->dataPtr->oneTimeChangedComponents[_type].insert(_entity); } else { - this->dataPtr->periodicChangedComponents.erase(key); - this->dataPtr->oneTimeChangedComponents.erase(key); + auto periodicIter = this->dataPtr->periodicChangedComponents.find(_type); + if (periodicIter != this->dataPtr->periodicChangedComponents.end()) + periodicIter->second.erase(_entity); + auto oneTimeIter = this->dataPtr->oneTimeChangedComponents.find(_type); + if (oneTimeIter != this->dataPtr->oneTimeChangedComponents.end()) + oneTimeIter->second.erase(_entity); } this->dataPtr->AddModifiedComponent(_entity); @@ -1460,15 +1527,15 @@ void EntityComponentManager::SetChanged( std::unordered_set EntityComponentManager::ComponentTypes( const Entity _entity) const { - std::unordered_set result; - - auto it = this->dataPtr->entityComponents.find(_entity); - if (it == this->dataPtr->entityComponents.end()) - return result; + auto it = this->dataPtr->componentTypeIndex.find(_entity); + if (it == this->dataPtr->componentTypeIndex.end()) + return {}; - for (const auto &key : it->second) + std::unordered_set result; + for (const auto &type : it->second) { - result.insert(key.first); + if (!this->dataPtr->ComponentMarkedAsRemoved(_entity, type.first)) + result.insert(type.first); } return result; @@ -1502,3 +1569,14 @@ void EntityComponentManagerPrivate::AddModifiedComponent(const Entity &_entity) this->modifiedComponents.insert(_entity); } + +///////////////////////////////////////////////// +bool EntityComponentManagerPrivate::ComponentMarkedAsRemoved( + const Entity _entity, const ComponentTypeId _typeId) const +{ + auto iter = this->componentsMarkedAsRemoved.find(_entity); + if (iter != this->componentsMarkedAsRemoved.end()) + return iter->second.find(_typeId) != iter->second.end(); + + return false; +} diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 29738cc551..dae5244665 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -21,6 +21,7 @@ #include #include #include +#include #include "ignition/gazebo/components/Factory.hh" #include "ignition/gazebo/components/Pose.hh" @@ -107,149 +108,24 @@ class EntityComponentManagerFixture : public ::testing::TestWithParam public: EntityCompMgrTest manager; }; -///////////////////////////////////////////////// -TEST_P(EntityComponentManagerFixture, AdjacentMemorySingleComponentType) -{ - std::vector poses; - std::vector keys; - - int count = 10; - - Entity entity = manager.CreateEntity(); - EXPECT_EQ(1u, entity); - - // Create the components. - for (int i = 0; i < count; ++i) - { - poses.push_back(components::Pose(math::Pose3d( - math::Rand::IntNormal(10, 5), - math::Rand::IntNormal(100, 50), - math::Rand::IntNormal(-100, 30), 0, 0, 0))); - keys.push_back(manager.CreateComponent(entity, poses.back())); - - // The component ids should increment by one for each component. - EXPECT_EQ(keys.back().second, i); - } - - ASSERT_EQ(count, static_cast(poses.size())); - ASSERT_EQ(count, static_cast(keys.size())); - - // Check the component values. - for (int i = 0; i < count; ++i) - { - EXPECT_EQ(poses[i], *(manager.Component(keys[i]))); - } - { - uintptr_t poseSize = sizeof(components::Pose); - const components::Pose *pose = nullptr, *prevPose = nullptr; - - // Check that each component is adjacent in memory - for (int i = 0; i < count; ++i) - { - pose = manager.Component(keys[i]); - if (prevPose != nullptr) - { - EXPECT_EQ(poseSize, reinterpret_cast(pose) - - reinterpret_cast(prevPose)); - } - prevPose = pose; - } - } - { - // Check that the data member of each Component is adjacent in memory - const math::Pose3d *poseData = nullptr, *prevPoseData = nullptr; - for (int i = 0; i < count; ++i) - { - poseData = &(manager.Component(keys[i])->Data()); - uintptr_t poseDataSize = sizeof(math::Pose3d) + - sizeof(components::BaseComponent); - if (prevPoseData != nullptr) - { - EXPECT_EQ(poseDataSize, reinterpret_cast(poseData) - - reinterpret_cast(prevPoseData)); - } - prevPoseData = poseData; - } - } -} - -///////////////////////////////////////////////// -TEST_P(EntityComponentManagerFixture, AdjacentMemoryTwoComponentTypes) -{ - common::setenv("IGN_DEBUG_COMPONENT_FACTORY", "true"); - - std::vector poses; - std::vector ints; - std::vector poseKeys; - std::vector intKeys; - - int count = 100000; - - Entity entity = manager.CreateEntity(); - EXPECT_EQ(1u, entity); - - // Create the components. - for (int i = 0; i < count; ++i) - { - poses.push_back(components::Pose(math::Pose3d(1, 2, 3, 0, 0, 0))); - ints.push_back(IntComponent(i)); - - poseKeys.push_back(manager.CreateComponent(entity, poses.back())); - intKeys.push_back(manager.CreateComponent(entity, ints.back())); - - // The component ids should increment by one for each component. - EXPECT_EQ(poseKeys.back().second, i); - EXPECT_EQ(intKeys.back().second, i); - } - - ASSERT_EQ(static_cast(count), poses.size()); - ASSERT_EQ(static_cast(count), ints.size()); - ASSERT_EQ(static_cast(count), poseKeys.size()); - ASSERT_EQ(static_cast(count), intKeys.size()); - - uintptr_t poseSize = sizeof(components::Pose); - uintptr_t intSize = sizeof(IntComponent); - const components::Pose *pose = nullptr, *prevPose = nullptr; - const IntComponent *it = nullptr, *prevIt = nullptr; - - // Check that each component is adjacent in memory - for (int i = 0; i < count; ++i) - { - pose = manager.Component(poseKeys[i]); - it = manager.Component(intKeys[i]); - if (prevPose != nullptr) - { - EXPECT_EQ(poseSize, reinterpret_cast(pose) - - reinterpret_cast(prevPose)); - } - - if (prevIt != nullptr) - { - EXPECT_EQ(intSize, reinterpret_cast(it) - - reinterpret_cast(prevIt)); - } - prevPose = pose; - prevIt = it; - } -} - ///////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, InvalidComponentType) { - ComponentKey key{999, 0}; - // Can't remove component from an nonexistent entity EXPECT_FALSE(manager.HasEntity(2)); - EXPECT_FALSE(manager.RemoveComponent(2, key)); + EXPECT_FALSE(manager.RemoveComponent(2, IntComponent::typeId)); // Can't remove a component that doesn't exist. EXPECT_EQ(1u, manager.CreateEntity()); EXPECT_EQ(2u, manager.CreateEntity()); EXPECT_TRUE(manager.HasEntity(2)); - EXPECT_FALSE(manager.RemoveComponent(2, key)); + EXPECT_FALSE(manager.RemoveComponent(2, IntComponent::typeId)); // We should get a nullptr if the component type doesn't exist. - EXPECT_EQ(nullptr, manager.Component(key)); + EXPECT_TRUE(manager.HasEntity(1u)); + EXPECT_TRUE(manager.HasEntity(2u)); + EXPECT_EQ(nullptr, manager.Component(1u)); + EXPECT_EQ(nullptr, manager.Component(2u)); } ///////////////////////////////////////////////// @@ -261,162 +137,71 @@ TEST_P(EntityComponentManagerFixture, RemoveComponent) auto eIntDouble = manager.CreateEntity(); EXPECT_EQ(3u, manager.EntityCount()); - // Add components and keep their unique ComponentKeys + // Add components auto cIntEInt = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, cIntEInt); auto cDoubleEDouble = manager.CreateComponent(eDouble, DoubleComponent(0.123)); + ASSERT_NE(nullptr, cDoubleEDouble); auto cIntEIntDouble = manager.CreateComponent(eIntDouble, IntComponent(456)); + ASSERT_NE(nullptr, cIntEIntDouble); auto cDoubleEIntDouble = manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); + ASSERT_NE(nullptr, cDoubleEIntDouble); // Check entities have the components - EXPECT_TRUE(manager.EntityHasComponent(eInt, cIntEInt)); + EXPECT_TRUE(manager.EntityHasComponentType(eInt, IntComponent::typeId)); EXPECT_EQ(1u, manager.ComponentTypes(eInt).size()); EXPECT_EQ(IntComponent::typeId, *manager.ComponentTypes(eInt).begin()); + EXPECT_EQ(cIntEInt, manager.Component(eInt)); - EXPECT_TRUE(manager.EntityHasComponent(eDouble, cDoubleEDouble)); + EXPECT_TRUE(manager.EntityHasComponentType(eDouble, DoubleComponent::typeId)); EXPECT_EQ(1u, manager.ComponentTypes(eDouble).size()); EXPECT_EQ(DoubleComponent::typeId, *manager.ComponentTypes(eDouble).begin()); + EXPECT_EQ(cDoubleEDouble, manager.Component(eDouble)); - EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); - EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); + EXPECT_TRUE(manager.EntityHasComponentType(eIntDouble, IntComponent::typeId)); + EXPECT_TRUE(manager.EntityHasComponentType(eIntDouble, + DoubleComponent::typeId)); EXPECT_EQ(2u, manager.ComponentTypes(eIntDouble).size()); auto types = manager.ComponentTypes(eIntDouble); EXPECT_NE(types.end(), types.find(IntComponent::typeId)); EXPECT_NE(types.end(), types.find(DoubleComponent::typeId)); - - // Remove component by key - EXPECT_TRUE(manager.RemoveComponent(eInt, cIntEInt)); - EXPECT_FALSE(manager.EntityHasComponent(eInt, cIntEInt)); - EXPECT_TRUE(manager.ComponentTypes(eInt).empty()); + EXPECT_EQ(cIntEIntDouble, manager.Component(eIntDouble)); + EXPECT_EQ(cDoubleEIntDouble, manager.Component(eIntDouble)); // Remove component by type id - auto typeDouble = DoubleComponent::typeId; + EXPECT_TRUE(manager.RemoveComponent(eInt, IntComponent::typeId)); + EXPECT_FALSE(manager.EntityHasComponentType(eInt, IntComponent::typeId)); + EXPECT_TRUE(manager.ComponentTypes(eInt).empty()); + EXPECT_EQ(nullptr, manager.Component(eInt)); - EXPECT_TRUE(manager.RemoveComponent(eDouble, typeDouble)); - EXPECT_FALSE(manager.EntityHasComponent(eDouble, cDoubleEDouble)); + EXPECT_TRUE(manager.RemoveComponent(eDouble, DoubleComponent::typeId)); + EXPECT_FALSE(manager.EntityHasComponentType(eDouble, + DoubleComponent::typeId)); EXPECT_TRUE(manager.ComponentTypes(eDouble).empty()); + EXPECT_EQ(nullptr, manager.Component(eDouble)); // Remove component by type EXPECT_TRUE(manager.RemoveComponent(eIntDouble)); - EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); - EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); - EXPECT_EQ(1u, manager.ComponentTypes(eIntDouble).size()); + EXPECT_FALSE(manager.EntityHasComponentType(eIntDouble, + IntComponent::typeId)); + EXPECT_TRUE(manager.EntityHasComponentType(eIntDouble, + DoubleComponent::typeId)); + types = manager.ComponentTypes(eIntDouble); + EXPECT_EQ(1u, types.size()); + EXPECT_EQ(types.end(), types.find(IntComponent::typeId)); + EXPECT_EQ(nullptr, manager.Component(eIntDouble)); EXPECT_TRUE(manager.RemoveComponent(eIntDouble)); - EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); - EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); + EXPECT_FALSE(manager.EntityHasComponentType(eIntDouble, + IntComponent::typeId)); + EXPECT_FALSE(manager.EntityHasComponentType(eIntDouble, + DoubleComponent::typeId)); EXPECT_EQ(0u, manager.ComponentTypes(eIntDouble).size()); -} - -///////////////////////////////////////////////// -// Removing a component should guarantee that existing components remain -// adjacent to each other. -TEST_P(EntityComponentManagerFixture, RemoveAdjacent) -{ - std::vector poses; - std::vector keys; - - Entity entity = manager.CreateEntity(); - - int count = 3; - - // Create the components. - for (int i = 0; i < count; ++i) - { - poses.push_back(components::Pose(math::Pose3d(1, 2, 3, 0, 0, 0))); - keys.push_back(manager.CreateComponent(entity, poses.back())); - EXPECT_EQ(keys.back().second, i); - } - ASSERT_EQ(poses.size(), keys.size()); - - uintptr_t poseSize = sizeof(components::Pose); - const components::Pose *pose = nullptr, *prevPose = nullptr; - - // Check that each component is adjacent in memory - for (int i = 0; i < count; ++i) - { - pose = manager.Component(keys[i]); - if (prevPose != nullptr) - { - EXPECT_EQ(poseSize, reinterpret_cast(pose) - - reinterpret_cast(prevPose)); - } - prevPose = pose; - } - - // Remove the middle component. - EXPECT_TRUE(manager.EntityHasComponent(entity, keys[1])); - EXPECT_TRUE(manager.RemoveComponent(entity, keys[1])); - EXPECT_FALSE(manager.EntityHasComponent(entity, keys[1])); - - // Can't remove the component twice. - EXPECT_FALSE(manager.RemoveComponent(entity, keys[1])); - - // Check that the two remaining components are still adjacent in memory - const components::Pose *pose1 = - manager.Component(keys[0]); - const components::Pose *pose3 = - manager.Component(keys[2]); - EXPECT_EQ(poseSize, - reinterpret_cast(pose3) - reinterpret_cast(pose1)); -} - -///////////////////////////////////////////////// -// Removing a component should guarantee that existing components remain -// adjacent to each other, and addition of a new component is adjacent to -// the last element. -TEST_P(EntityComponentManagerFixture, RemoveAddAdjacent) -{ - Entity entity = manager.CreateEntity(); - - std::vector keys; - - keys.push_back(manager.CreateComponent(entity, - components::Pose(math::Pose3d(1, 2, 3, 0, 0, 0)))); - keys.push_back(manager.CreateComponent(entity, - components::Pose(math::Pose3d(3, 1, 2, 0, 0, 0)))); - keys.push_back(manager.CreateComponent(entity, - components::Pose(math::Pose3d(0, 10, 20, 0, 0, 0)))); - - uintptr_t poseSize = sizeof(components::Pose); - - // Remove the middle component. - EXPECT_TRUE(manager.RemoveComponent(entity, keys[1])); - - // Add two more new component - keys.push_back(manager.CreateComponent(entity, - components::Pose(math::Pose3d(101, 51, 520, 0, 0, 0)))); - - keys.push_back(manager.CreateComponent(entity, - components::Pose(math::Pose3d(1010, 81, 821, 0, 0, 0)))); - - // Check that the components are all adjacent in memory - const components::Pose *pose1 = - manager.Component(keys[0]); - const components::Pose *pose2 = - manager.Component(keys[2]); - const components::Pose *pose3 = - manager.Component(keys[3]); - const components::Pose *pose4 = - manager.Component(keys[4]); - - EXPECT_EQ(poseSize, - reinterpret_cast(pose2) - reinterpret_cast(pose1)); - - EXPECT_EQ(poseSize, - reinterpret_cast(pose3) - reinterpret_cast(pose2)); - - EXPECT_EQ(poseSize, - reinterpret_cast(pose4) - reinterpret_cast(pose3)); - - // Check the values of the components. - EXPECT_EQ(components::Pose(math::Pose3d(1, 2, 3, 0, 0, 0)), *pose1); - EXPECT_EQ(components::Pose(math::Pose3d(0, 10, 20, 0, 0, 0)), *pose2); - EXPECT_EQ(components::Pose(math::Pose3d(101, 51, 520, 0, 0, 0)), *pose3); - EXPECT_EQ(components::Pose(math::Pose3d(1010, 81, 821, 0, 0, 0)), *pose4); + EXPECT_EQ(nullptr, manager.Component(eIntDouble)); } ///////////////////////////////////////////////// @@ -431,12 +216,13 @@ TEST_P(EntityComponentManagerFixture, EntitiesAndComponents) EXPECT_EQ(3u, manager.EntityCount()); // Add a component to an entity - ComponentKey cKey = manager.CreateComponent(entity, + auto compPtr = manager.CreateComponent(entity, IntComponent(123)); + ASSERT_NE(nullptr, compPtr); EXPECT_TRUE(manager.HasComponentType(IntComponent::typeId)); - EXPECT_TRUE(manager.EntityHasComponent(entity, cKey)); EXPECT_TRUE(manager.EntityHasComponentType(entity, IntComponent::typeId)); + EXPECT_EQ(compPtr, manager.Component(entity)); EXPECT_FALSE(manager.EntityHasComponentType(entity, DoubleComponent::typeId)); EXPECT_FALSE(manager.EntityHasComponentType(entity2, IntComponent::typeId)); @@ -444,7 +230,7 @@ TEST_P(EntityComponentManagerFixture, EntitiesAndComponents) EXPECT_FALSE(manager.HasEntity(kNullEntity)); EXPECT_FALSE(manager.EntityHasComponentType(kNullEntity, IntComponent::typeId)); - EXPECT_EQ(ComponentKey(), manager.CreateComponent(kNullEntity, + EXPECT_EQ(nullptr, manager.CreateComponent(kNullEntity, IntComponent(123))); EXPECT_FALSE(manager.HasEntity(kNullEntity)); EXPECT_FALSE(manager.EntityHasComponentType(kNullEntity, @@ -475,6 +261,21 @@ TEST_P(EntityComponentManagerFixture, EntitiesAndComponents) EXPECT_TRUE(manager.EntityHasComponentType(entity, IntComponent::typeId)); EXPECT_EQ(123, intComp->Data()); + // Try to create/query a component from an entity that does not exist. nullptr + // should be returned since a component cannot be attached to a non-existent + // entity + EXPECT_FALSE(manager.HasEntity(kNullEntity)); + EXPECT_EQ(nullptr, manager.CreateComponent(kNullEntity, + IntComponent(123))); + EXPECT_EQ(nullptr, manager.ComponentDefault(kNullEntity, 123)); + EXPECT_EQ(nullptr, manager.Component(kNullEntity)); + EXPECT_FALSE(manager.ComponentData(kNullEntity).has_value()); + EXPECT_EQ(ComponentState::NoChange, manager.ComponentState(kNullEntity, + IntComponent::typeId)); + // (make sure the entity wasn't implicitly created during the invalid + // component calls) + EXPECT_FALSE(manager.HasEntity(kNullEntity)); + // Remove all entities manager.RequestRemoveEntities(); EXPECT_EQ(3u, manager.EntityCount()); @@ -484,7 +285,6 @@ TEST_P(EntityComponentManagerFixture, EntitiesAndComponents) EXPECT_EQ(0u, manager.EntityCount()); EXPECT_FALSE(manager.HasEntity(entity)); EXPECT_FALSE(manager.HasEntity(entity2)); - EXPECT_FALSE(manager.EntityHasComponent(entity, cKey)); EXPECT_FALSE(manager.EntityHasComponentType(entity, IntComponent::typeId)); // The type itself still exists @@ -503,14 +303,23 @@ TEST_P(EntityComponentManagerFixture, ComponentValues) EXPECT_EQ(5u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(123)); - manager.CreateComponent(eDouble, DoubleComponent(0.123)); - manager.CreateComponent(eIntDouble, IntComponent(456)); - manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); - manager.CreateComponent(ePose, + auto comp1 = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eDouble, + DoubleComponent(0.123)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(eIntDouble, + IntComponent(456)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(eIntDouble, + DoubleComponent(0.456)); + ASSERT_NE(nullptr, comp4); + auto comp5 = manager.CreateComponent(ePose, components::Pose({1, 2, 3, 0, 0, 0})); - manager.CreateComponent(eCustom, + ASSERT_NE(nullptr, comp5); + auto comp6 = manager.CreateComponent(eCustom, CustomComponent(Custom())); + ASSERT_NE(nullptr, comp6); // Get and set component values { @@ -667,10 +476,17 @@ TEST_P(EntityComponentManagerFixture, RebuildViews) EXPECT_EQ(3u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(123)); - manager.CreateComponent(eDouble, DoubleComponent(0.123)); - manager.CreateComponent(eIntDouble, IntComponent(456)); - manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); + auto comp1 = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eDouble, + DoubleComponent(0.123)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(eIntDouble, + IntComponent(456)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(eIntDouble, + DoubleComponent(0.456)); + ASSERT_NE(nullptr, comp4); // The first iteration of this loop builds views. At the end, views are // rebuilt. The second iteration should return the same values as the @@ -729,10 +545,17 @@ TEST_P(EntityComponentManagerFixture, ViewsAddComponents) EXPECT_EQ(3u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(123)); - manager.CreateComponent(eDouble, DoubleComponent(0.123)); - manager.CreateComponent(eIntDouble, IntComponent(456)); - manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); + auto comp1 = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eDouble, + DoubleComponent(0.123)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(eIntDouble, + IntComponent(456)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(eIntDouble, + DoubleComponent(0.456)); + ASSERT_NE(nullptr, comp4); for (int i = 0; i < 2; ++i) { @@ -779,7 +602,9 @@ TEST_P(EntityComponentManagerFixture, ViewsAddComponents) else EXPECT_EQ(3, count); - manager.CreateComponent(eInt, DoubleComponent(12.123)); + auto comp5 = manager.CreateComponent(eInt, + DoubleComponent(12.123)); + ASSERT_NE(nullptr, comp5); } } @@ -793,11 +618,17 @@ TEST_P(EntityComponentManagerFixture, ViewsRemoveComponents) EXPECT_EQ(3u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(123)); - manager.CreateComponent(eDouble, DoubleComponent(0.123)); - manager.CreateComponent(eIntDouble, IntComponent(456)); + auto comp1 = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eDouble, + DoubleComponent(0.123)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(eIntDouble, + IntComponent(456)); + ASSERT_NE(nullptr, comp3); auto compToRemove = manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); + ASSERT_NE(nullptr, compToRemove); for (int i = 0; i < 2; ++i) { @@ -846,7 +677,7 @@ TEST_P(EntityComponentManagerFixture, ViewsRemoveComponents) if (i == 0) { - EXPECT_TRUE(manager.RemoveComponent(eIntDouble, compToRemove)); + EXPECT_TRUE(manager.RemoveComponent(eIntDouble, compToRemove->TypeId())); } } } @@ -861,10 +692,17 @@ TEST_P(EntityComponentManagerFixture, ViewsAddEntity) EXPECT_EQ(3u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(123)); - manager.CreateComponent(eDouble, DoubleComponent(0.123)); - manager.CreateComponent(eIntDouble, IntComponent(456)); - manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); + auto comp1 = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eDouble, + DoubleComponent(0.123)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(eIntDouble, + IntComponent(456)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(eIntDouble, + DoubleComponent(0.456)); + ASSERT_NE(nullptr, comp4); Entity newEntity; @@ -926,7 +764,10 @@ TEST_P(EntityComponentManagerFixture, ViewsAddEntity) EXPECT_EQ(2, count); newEntity = manager.CreateEntity(); - manager.CreateComponent(newEntity, IntComponent(789)); + ASSERT_NE(kNullEntity, newEntity); + auto createdComp = manager.CreateComponent(newEntity, + IntComponent(789)); + ASSERT_NE(nullptr, createdComp); } } @@ -940,10 +781,17 @@ TEST_P(EntityComponentManagerFixture, ViewsRemoveEntities) EXPECT_EQ(3u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(123)); - manager.CreateComponent(eDouble, DoubleComponent(0.123)); - manager.CreateComponent(eIntDouble, IntComponent(456)); - manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); + auto comp1 = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eDouble, + DoubleComponent(0.123)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(eIntDouble, + IntComponent(456)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(eIntDouble, + DoubleComponent(0.456)); + ASSERT_NE(nullptr, comp4); for (int i = 0; i < 2; ++i) { @@ -1069,10 +917,17 @@ TEST_P(EntityComponentManagerFixture, ViewsRemoveEntity) EXPECT_EQ(3u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(123)); - manager.CreateComponent(eDouble, DoubleComponent(0.123)); - manager.CreateComponent(eIntDouble, IntComponent(456)); - manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); + auto comp1 = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eDouble, + DoubleComponent(0.123)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(eIntDouble, + IntComponent(456)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(eIntDouble, + DoubleComponent(0.456)); + ASSERT_NE(nullptr, comp4); int count = 0; manager.Each ([&](const Entity &_entity, @@ -1193,8 +1048,10 @@ TEST_P(EntityComponentManagerFixture, EachNewBasic) EXPECT_EQ(2u, manager.EntityCount()); // Add components to each entity - manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e2, IntComponent(456)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); EXPECT_EQ(2, newCount(manager)); EXPECT_TRUE(manager.HasNewEntities()); @@ -1204,6 +1061,26 @@ TEST_P(EntityComponentManagerFixture, EachNewBasic) manager.RunClearNewlyCreatedEntities(); EXPECT_EQ(0, newCount(manager)); EXPECT_FALSE(manager.HasNewEntities()); + + // Below tests adding a new entity, but not using the view until the following + // simulation step. This is to ensure that the views update the status of + // their new entities correctly at the end of each simulation step, regardless + // of whether the view is used in a given simulation step or not + + // Create a new entity and add a component to it that makes this entity a + // part of the IntComponent View + Entity e3 = manager.CreateEntity(); + EXPECT_EQ(3u, manager.EntityCount()); + EXPECT_TRUE(manager.HasNewEntities()); + auto comp3 = manager.CreateComponent(e3, IntComponent(789)); + EXPECT_NE(nullptr, comp3); + // Mimic the end of a simulation step + manager.RunClearNewlyCreatedEntities(); + // Use the IntComponent View, checking that the view has no entities marked as + // new since we are now in a new simulation step + EXPECT_EQ(0, newCount(manager)); + EXPECT_FALSE(manager.HasNewEntities()); + EXPECT_EQ(3, eachCount(manager)); } ////////////////////////////////////////////////// @@ -1212,11 +1089,14 @@ TEST_P(EntityComponentManagerFixture, EachNewAfterRemoveComponent) // Create entities Entity e1 = manager.CreateEntity(); auto comp1 = manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e1, DoubleComponent(0.0)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e1, + DoubleComponent(0.0)); + ASSERT_NE(nullptr, comp2); EXPECT_EQ(1, newCount(manager)); - manager.RemoveComponent(e1, comp1); + EXPECT_TRUE(manager.RemoveComponent(e1, comp1->TypeId())); EXPECT_EQ(1, newCount(manager)); manager.RunClearNewlyCreatedEntities(); @@ -1228,13 +1108,15 @@ TEST_P(EntityComponentManagerFixture, EachNewRemoveComponentFromRemoveEntity) { // Create entities Entity e1 = manager.CreateEntity(); - manager.CreateComponent(e1, IntComponent(123)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); manager.RunClearNewlyCreatedEntities(); // Nothing new after cleared EXPECT_EQ(0, newCount(manager)); Entity e2 = manager.CreateEntity(); - manager.CreateComponent(e2, IntComponent(456)); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); EXPECT_EQ(1, newCount(manager)); // Check if this true after RebuildViews manager.RebuildViews(); @@ -1247,18 +1129,25 @@ TEST_P(EntityComponentManagerFixture, EachNewAddComponentToExistingEntity) // Create entities Entity e1 = manager.CreateEntity(); Entity e2 = manager.CreateEntity(); - manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e2, IntComponent(456)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); manager.RunClearNewlyCreatedEntities(); // Nothing new after cleared EXPECT_EQ(0, newCount(manager)); // Create a new entity Entity e3 = manager.CreateEntity(); - manager.CreateComponent(e3, IntComponent(789)); + auto comp3 = manager.CreateComponent(e3, IntComponent(789)); + ASSERT_NE(nullptr, comp3); // Add a new component to existing entities - manager.CreateComponent(e1, DoubleComponent(0.0)); - manager.CreateComponent(e2, DoubleComponent(2.0)); + auto comp4 = manager.CreateComponent(e1, + DoubleComponent(0.0)); + ASSERT_NE(nullptr, comp4); + auto comp5 = manager.CreateComponent(e2, + DoubleComponent(2.0)); + ASSERT_NE(nullptr, comp5); // e1 and e2 have a new double component, but they are not considered new // entities @@ -1276,8 +1165,10 @@ TEST_P(EntityComponentManagerFixture, EachRemoveBasic) EXPECT_EQ(2u, manager.EntityCount()); // Add components to each entity - manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e2, IntComponent(456)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); // Remove an entity. manager.RequestRemoveEntity(e1); @@ -1306,8 +1197,10 @@ TEST_P(EntityComponentManagerFixture, EachRemoveAlreadyRemove) EXPECT_EQ(2u, manager.EntityCount()); // Add components to each entity - manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e2, IntComponent(456)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); manager.RequestRemoveEntity(e2); manager.ProcessEntityRemovals(); @@ -1324,7 +1217,8 @@ TEST_P(EntityComponentManagerFixture, EachRemoveAfterRebuild) Entity e1 = manager.CreateEntity(); EXPECT_EQ(1u, manager.EntityCount()); - manager.CreateComponent(e1, IntComponent(123)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); EXPECT_EQ(1, newCount(manager)); manager.RunClearNewlyCreatedEntities(); @@ -1339,13 +1233,16 @@ TEST_P(EntityComponentManagerFixture, EachRemoveAfterRebuild) TEST_P(EntityComponentManagerFixture, EachRemoveAddComponentToRemoveEntity) { Entity e1 = manager.CreateEntity(); - manager.CreateComponent(e1, IntComponent(123)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); manager.RunClearNewlyCreatedEntities(); manager.RequestRemoveEntity(e1); // Add a new component to an removed entity. This should be possible since the // entity is only scheduled to be removed. - manager.CreateComponent(e1, DoubleComponent(0.0)); + auto comp2 = manager.CreateComponent(e1, + DoubleComponent(0.0)); + ASSERT_NE(nullptr, comp2); EXPECT_EQ(1, removedCount(manager)); EXPECT_EQ(1, (removedCount(manager))); } @@ -1356,8 +1253,10 @@ TEST_P(EntityComponentManagerFixture, EachRemoveAllRemove) // Test when all entities are removed Entity e1 = manager.CreateEntity(); Entity e2 = manager.CreateEntity(); - manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e2, IntComponent(456)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); EXPECT_EQ(2u, manager.EntityCount()); manager.RequestRemoveEntities(); @@ -1373,8 +1272,10 @@ TEST_P(EntityComponentManagerFixture, EachNewEachRemove) // Test EachNew and EachRemove together Entity e1 = manager.CreateEntity(); Entity e2 = manager.CreateEntity(); - manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e2, IntComponent(456)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); EXPECT_EQ(2u, manager.EntityCount()); EXPECT_EQ(2, newCount(manager)); @@ -1401,8 +1302,10 @@ TEST_P(EntityComponentManagerFixture, EachGetsNewOldRemove) // Test that an Each call gets new, old, and removed entities Entity e1 = manager.CreateEntity(); Entity e2 = manager.CreateEntity(); - manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e2, IntComponent(456)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); EXPECT_EQ(2u, manager.EntityCount()); EXPECT_EQ(2, eachCount(manager)); @@ -1428,6 +1331,66 @@ TEST_P(EntityComponentManagerFixture, EachGetsNewOldRemove) EXPECT_EQ(0, removedCount(manager)); } +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, EachAddRemoveComponent) +{ + // test calling ecm.Each on entities that have components added/removed + // frequently. This is common with *Cmd components + + Entity e1 = manager.CreateEntity(); + EXPECT_EQ(1u, manager.EntityCount()); + EXPECT_EQ(0, eachCount(manager)); + auto comp = manager.Component(e1); + EXPECT_EQ(nullptr, comp); + + // add a component + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + EXPECT_EQ(1, eachCount(manager)); + comp = manager.Component(e1); + ASSERT_NE(nullptr, comp); + EXPECT_EQ(123, comp->Data()); + EXPECT_EQ(123, comp1->Data()); + EXPECT_EQ(comp, comp1); + + // remove a component + EXPECT_TRUE(manager.RemoveComponent(e1, IntComponent::typeId)); + EXPECT_EQ(nullptr, manager.Component(e1)); + EXPECT_EQ(0, eachCount(manager)); + + // add the same type of component back in + auto comp2 = manager.CreateComponent(e1, IntComponent(456)); + ASSERT_NE(nullptr, comp2); + EXPECT_EQ(1, eachCount(manager)); + comp = manager.Component(e1); + ASSERT_NE(nullptr, comp); + EXPECT_EQ(456, comp->Data()); + EXPECT_EQ(456, comp2->Data()); + EXPECT_EQ(comp, comp2); + + // remove the component again + EXPECT_TRUE(manager.RemoveComponent(e1, IntComponent::typeId)); + EXPECT_EQ(nullptr, manager.Component(e1)); + EXPECT_EQ(0, eachCount(manager)); + + // add and remove the component before calling ecm.Each. This is to make sure + // that the views remove any entities in their toAddEntities queue if required + // components for entities in a view's toAddEntities queue are removed before + // calling Each, since a view's toAddEntities queue is processed (and then + // cleared) in an Each call + auto comp3 = manager.CreateComponent(e1, IntComponent(789)); + ASSERT_NE(nullptr, comp3); + comp = manager.Component(e1); + ASSERT_NE(nullptr, comp); + EXPECT_EQ(789, comp->Data()); + EXPECT_EQ(789, comp3->Data()); + EXPECT_EQ(comp, comp3); + EXPECT_TRUE(manager.RemoveComponent(e1, IntComponent::typeId)); + EXPECT_EQ(nullptr, manager.Component(e1)); + // call ecm.Each after adding/removing the component + EXPECT_EQ(0, eachCount(manager)); +} + ////////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, EntityByComponents) { @@ -1438,16 +1401,28 @@ TEST_P(EntityComponentManagerFixture, EntityByComponents) EXPECT_EQ(3u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(-123)); - manager.CreateComponent(eInt, StringComponent("int")); - - manager.CreateComponent(eUint, UIntComponent(456u)); - manager.CreateComponent(eUint, StringComponent("uint")); - - manager.CreateComponent(eIntUint, IntComponent(789)); - manager.CreateComponent(eIntUint, UIntComponent(789u)); - manager.CreateComponent(eIntUint, + auto comp1 = manager.CreateComponent(eInt, IntComponent(-123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eInt, + StringComponent("int")); + ASSERT_NE(nullptr, comp2); + + auto comp3 = manager.CreateComponent(eUint, + UIntComponent(456u)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(eUint, + StringComponent("uint")); + ASSERT_NE(nullptr, comp4); + + auto comp5 = manager.CreateComponent(eIntUint, + IntComponent(789)); + ASSERT_NE(nullptr, comp5); + auto comp6 = manager.CreateComponent(eIntUint, + UIntComponent(789u)); + ASSERT_NE(nullptr, comp6); + auto comp7 = manager.CreateComponent(eIntUint, StringComponent("int-uint")); + ASSERT_NE(nullptr, comp7); // Get entities by the value of their components EXPECT_EQ(eInt, manager.EntityByComponents(IntComponent(-123))); @@ -1483,8 +1458,11 @@ TEST_P(EntityComponentManagerFixture, EntityByComponents) Entity eInt2 = manager.CreateEntity(); EXPECT_EQ(4u, manager.EntityCount()); - manager.CreateComponent(eInt2, IntComponent(-123)); - manager.CreateComponent(eInt2, StringComponent("int2")); + auto comp8 = manager.CreateComponent(eInt2, IntComponent(-123)); + ASSERT_NE(nullptr, comp8); + auto comp9 = manager.CreateComponent(eInt2, + StringComponent("int2")); + ASSERT_NE(nullptr, comp9); auto entities = manager.EntitiesByComponents(IntComponent(-123)); EXPECT_EQ(2u, entities.size()); @@ -1552,14 +1530,21 @@ TEST_P(EntityComponentManagerFixture, EntityGraph) */ // Add components - manager.CreateComponent(e2, {}); - manager.CreateComponent(e4, {}); - manager.CreateComponent(e6, {}); - - manager.CreateComponent(e1, {}); - manager.CreateComponent(e3, {}); - manager.CreateComponent(e5, {}); - manager.CreateComponent(e7, {}); + auto comp1 = manager.CreateComponent(e2, {}); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e4, {}); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(e6, {}); + ASSERT_NE(nullptr, comp3); + + auto comp4 = manager.CreateComponent(e1, {}); + ASSERT_NE(nullptr, comp4); + auto comp5 = manager.CreateComponent(e3, {}); + ASSERT_NE(nullptr, comp5); + auto comp6 = manager.CreateComponent(e5, {}); + ASSERT_NE(nullptr, comp6); + auto comp7 = manager.CreateComponent(e7, {}); + ASSERT_NE(nullptr, comp7); // Get children by components { @@ -1651,10 +1636,16 @@ TEST_P(EntityComponentManagerFixture, State) EXPECT_EQ(3u, manager.EntityCount()); // Components - manager.CreateComponent(e1, IntComponent(e1c0)); - manager.CreateComponent(e2, DoubleComponent(e2c0)); - manager.CreateComponent(e2, StringComponent(e2c1)); - manager.CreateComponent(e3, IntComponent(e3c0)); + auto comp1 = manager.CreateComponent(e1, IntComponent(e1c0)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, + DoubleComponent(e2c0)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(e2, + StringComponent(e2c1)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(e3, IntComponent(e3c0)); + ASSERT_NE(nullptr, comp4); } // Serialize into a message @@ -1913,7 +1904,8 @@ TEST_P(EntityComponentManagerFixture, ChangedStateComponents) EXPECT_EQ(1u, manager.EntityCount()); // Component - manager.CreateComponent(e1, IntComponent(e1c0)); + auto comp1 = manager.CreateComponent(e1, IntComponent(e1c0)); + ASSERT_NE(nullptr, comp1); // Serialize into a message msgs::SerializedStateMap stateMsg; @@ -1926,8 +1918,9 @@ TEST_P(EntityComponentManagerFixture, ChangedStateComponents) EXPECT_EQ(0, changedStateMsg.entities_size()); // create component - auto compKey = + auto compPtr = manager.CreateComponent(e1, StringComponent(e1c1)); + ASSERT_NE(nullptr, compPtr); changedStateMsg = manager.ChangedState(); EXPECT_EQ(1, changedStateMsg.entities_size()); manager.State(stateMsg); @@ -1943,7 +1936,7 @@ TEST_P(EntityComponentManagerFixture, ChangedStateComponents) msgs::SerializedEntityMap &e1Msg = iter->second; - auto compIter = e1Msg.mutable_components()->find(compKey.first); + auto compIter = e1Msg.mutable_components()->find(compPtr->TypeId()); ASSERT_TRUE(compIter != e1Msg.mutable_components()->end()); msgs::SerializedComponent &e1c1Msg = compIter->second; @@ -1962,7 +1955,7 @@ TEST_P(EntityComponentManagerFixture, ChangedStateComponents) EXPECT_EQ(0, changedStateMsg.entities_size()); // remove component - manager.RemoveComponent(e1, StringComponent::typeId); + EXPECT_TRUE(manager.RemoveComponent(e1, StringComponent::typeId)); changedStateMsg = manager.ChangedState(); EXPECT_EQ(1, changedStateMsg.entities_size()); @@ -2189,14 +2182,16 @@ TEST_P(EntityComponentManagerFixture, SetChanged) // Add components to each entity auto c1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, c1); auto c2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, c2); EXPECT_TRUE(manager.HasOneTimeComponentChanges()); EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(ComponentState::OneTimeChange, - manager.ComponentState(e1, c1.first)); + manager.ComponentState(e1, c1->TypeId())); EXPECT_EQ(ComponentState::OneTimeChange, - manager.ComponentState(e2, c2.first)); + manager.ComponentState(e2, c2->TypeId())); EXPECT_EQ(ComponentState::NoChange, manager.ComponentState(999, 888)); EXPECT_EQ(ComponentState::NoChange, manager.ComponentState(e1, 888)); @@ -2206,12 +2201,12 @@ TEST_P(EntityComponentManagerFixture, SetChanged) EXPECT_FALSE(manager.HasOneTimeComponentChanges()); EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(ComponentState::NoChange, - manager.ComponentState(e1, c1.first)); + manager.ComponentState(e1, c1->TypeId())); EXPECT_EQ(ComponentState::NoChange, - manager.ComponentState(e2, c2.first)); + manager.ComponentState(e2, c2->TypeId())); // Mark as changed - manager.SetChanged(e1, c1.first, ComponentState::PeriodicChange); + manager.SetChanged(e1, c1->TypeId(), ComponentState::PeriodicChange); // check that only e1 c1 is serialized into a message msgs::SerializedStateMap stateMsg; @@ -2230,7 +2225,7 @@ TEST_P(EntityComponentManagerFixture, SetChanged) EXPECT_EQ(123, std::stoi(e1c1Msg.component())); } - manager.SetChanged(e2, c2.first, ComponentState::OneTimeChange); + manager.SetChanged(e2, c2->TypeId(), ComponentState::OneTimeChange); EXPECT_TRUE(manager.HasOneTimeComponentChanges()); // Expect a single component type to be marked as PeriodicChange @@ -2238,23 +2233,23 @@ TEST_P(EntityComponentManagerFixture, SetChanged) EXPECT_EQ(IntComponent().TypeId(), *manager.ComponentTypesWithPeriodicChanges().begin()); EXPECT_EQ(ComponentState::PeriodicChange, - manager.ComponentState(e1, c1.first)); + manager.ComponentState(e1, c1->TypeId())); EXPECT_EQ(ComponentState::OneTimeChange, - manager.ComponentState(e2, c2.first)); + manager.ComponentState(e2, c2->TypeId())); // Remove components - EXPECT_TRUE(manager.RemoveComponent(e1, c1.first)); + EXPECT_TRUE(manager.RemoveComponent(e1, c1->TypeId())); EXPECT_TRUE(manager.HasOneTimeComponentChanges()); EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(ComponentState::NoChange, - manager.ComponentState(e1, c1.first)); + manager.ComponentState(e1, c1->TypeId())); - EXPECT_TRUE(manager.RemoveComponent(e2, c2.first)); + EXPECT_TRUE(manager.RemoveComponent(e2, c2->TypeId())); EXPECT_FALSE(manager.HasOneTimeComponentChanges()); EXPECT_EQ(ComponentState::NoChange, - manager.ComponentState(e2, c2.first)); + manager.ComponentState(e2, c2->TypeId())); } ////////////////////////////////////////////////// @@ -2282,20 +2277,23 @@ TEST_P(EntityComponentManagerFixture, SerializedStateMapMsgAfterRemoveComponent) Entity e1 = manager.CreateEntity(); auto e1c0 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, e1c0); auto e1c1 = manager.CreateComponent(e1, DoubleComponent(0.0)); + ASSERT_NE(nullptr, e1c1); auto e1c2 = manager.CreateComponent(e1, StringComponent("int")); + ASSERT_NE(nullptr, e1c2); // We use this map because the order in which components are iterated // through depends on the (undetermined) order of unordered multimaps std::map expectations; - expectations.insert(std::make_pair(e1c0.first, false)); - expectations.insert(std::make_pair(e1c1.first, true)); - expectations.insert(std::make_pair(e1c2.first, true)); + expectations.insert(std::make_pair(e1c0->TypeId(), false)); + expectations.insert(std::make_pair(e1c1->TypeId(), true)); + expectations.insert(std::make_pair(e1c2->TypeId(), true)); - manager.RemoveComponent(e1, e1c1); - manager.RemoveComponent(e1, e1c2); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c1->TypeId())); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c2->TypeId())); // Serialize into a message msgs::SerializedStateMap stateMsg; @@ -2347,20 +2345,23 @@ TEST_P(EntityComponentManagerFixture, SerializedStateMsgAfterRemoveComponent) Entity e1 = manager.CreateEntity(); auto e1c0 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, e1c0); auto e1c1 = manager.CreateComponent(e1, DoubleComponent(0.0)); + ASSERT_NE(nullptr, e1c1); auto e1c2 = manager.CreateComponent(e1, StringComponent("int")); + ASSERT_NE(nullptr, e1c2); // We use this map because the order in which components are iterated // through depends on the (undetermined) order of unordered multimaps std::map expectations; - expectations.insert(std::make_pair(e1c0.first, false)); - expectations.insert(std::make_pair(e1c1.first, true)); - expectations.insert(std::make_pair(e1c2.first, true)); + expectations.insert(std::make_pair(e1c0->TypeId(), false)); + expectations.insert(std::make_pair(e1c1->TypeId(), true)); + expectations.insert(std::make_pair(e1c2->TypeId(), true)); - manager.RemoveComponent(e1, e1c1); - manager.RemoveComponent(e1, e1c2); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c1->TypeId())); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c2->TypeId())); // Serialize into a message msgs::SerializedState stateMsg; @@ -2408,13 +2409,17 @@ TEST_P(EntityComponentManagerFixture, SerializedStateMapMsgCompsRemovedOnly) Entity e1 = manager.CreateEntity(); auto e1c0 = manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e1, DoubleComponent(0.0)); + ASSERT_NE(nullptr, e1c0); + auto e1c1 = manager.CreateComponent(e1, + DoubleComponent(0.0)); + ASSERT_NE(nullptr, e1c1); auto e1c2 = manager.CreateComponent(e1, StringComponent("int")); + ASSERT_NE(nullptr, e1c2); manager.RunSetAllComponentsUnchanged(); - manager.RemoveComponent(e1, e1c0); - manager.RemoveComponent(e1, e1c2); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c0->TypeId())); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c2->TypeId())); // Serialize into a message msgs::SerializedStateMap stateMsg; manager.State(stateMsg); @@ -2448,19 +2453,22 @@ TEST_P(EntityComponentManagerFixture, SetRemovedComponentsMsgTypesFilter) Entity e1 = manager.CreateEntity(); auto e1c0 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, e1c0); auto e1c1 = manager.CreateComponent(e1, DoubleComponent(0.0)); + ASSERT_NE(nullptr, e1c1); auto e1c2 = manager.CreateComponent(e1, StringComponent("foo")); + ASSERT_NE(nullptr, e1c2); manager.RunSetAllComponentsUnchanged(); - manager.RemoveComponent(e1, e1c0); - manager.RemoveComponent(e1, e1c2); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c0->TypeId())); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c2->TypeId())); // Serialize into a message, providing a list of types to be included msgs::SerializedStateMap stateMsg; std::unordered_set entitySet{e1}; - std::unordered_set types{e1c0.first, e1c1.first}; + std::unordered_set types{e1c0->TypeId(), e1c1->TypeId()}; manager.State(stateMsg, entitySet, types, false); // Check message @@ -2475,7 +2483,7 @@ TEST_P(EntityComponentManagerFixture, SetRemovedComponentsMsgTypesFilter) // Only component in message should be e1c2 const auto &c0 = compIter->second; EXPECT_EQ(c0.remove(), true); - EXPECT_EQ(c0.type(), e1c0.first); + EXPECT_EQ(c0.type(), e1c0->TypeId()); } } @@ -2489,17 +2497,20 @@ TEST_P(EntityComponentManagerFixture, RemovedComponentsSyncBetweenServerAndGUI) Entity e1 = manager.CreateEntity(); auto e1c0 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, e1c0); auto e1c1 = manager.CreateComponent(e1, DoubleComponent(0.0)); + ASSERT_NE(nullptr, e1c1); auto e1c2 = manager.CreateComponent(e1, StringComponent("int")); + ASSERT_NE(nullptr, e1c2); // We use this map because the order in which components are iterated // through depends on the (undetermined) order of unordered multimaps std::map expectationsBeforeRemoving; - expectationsBeforeRemoving.insert(std::make_pair(e1c0.first, false)); - expectationsBeforeRemoving.insert(std::make_pair(e1c1.first, false)); - expectationsBeforeRemoving.insert(std::make_pair(e1c2.first, false)); + expectationsBeforeRemoving.insert(std::make_pair(e1c0->TypeId(), false)); + expectationsBeforeRemoving.insert(std::make_pair(e1c1->TypeId(), false)); + expectationsBeforeRemoving.insert(std::make_pair(e1c2->TypeId(), false)); // Serialize server ECM into a message msgs::SerializedStateMap stateMsg; @@ -2532,13 +2543,13 @@ TEST_P(EntityComponentManagerFixture, RemovedComponentsSyncBetweenServerAndGUI) } std::map expectationsAfterRemoving; - expectationsAfterRemoving.insert(std::make_pair(e1c0.first, false)); - expectationsAfterRemoving.insert(std::make_pair(e1c1.first, true)); - expectationsAfterRemoving.insert(std::make_pair(e1c2.first, true)); + expectationsAfterRemoving.insert(std::make_pair(e1c0->TypeId(), false)); + expectationsAfterRemoving.insert(std::make_pair(e1c1->TypeId(), true)); + expectationsAfterRemoving.insert(std::make_pair(e1c2->TypeId(), true)); // Remove components and synchronize again - manager.RemoveComponent(e1, e1c1); - manager.RemoveComponent(e1, e1c2); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c1->TypeId())); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c2->TypeId())); msgs::SerializedStateMap newStateMsg; manager.State(newStateMsg); @@ -2576,6 +2587,81 @@ TEST_P(EntityComponentManagerFixture, RemovedComponentsSyncBetweenServerAndGUI) } } +///////////////////////////////////////////////// +// Check that some widely used deprecated APIs still work +TEST_P(EntityComponentManagerFixture, Deprecated) +{ + IGN_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + + // Fail to create component for inexistent entity + EXPECT_EQ(nullptr, manager.CreateComponent(789, + IntComponent(123))); + + // Create some entities + auto eInt = manager.CreateEntity(); + auto eDouble = manager.CreateEntity(); + auto eIntDouble = manager.CreateEntity(); + EXPECT_EQ(3u, manager.EntityCount()); + + // Add components and keep their unique ComponentKeys + EXPECT_NE(nullptr, manager.CreateComponent(eInt, + IntComponent(123))); + ComponentKey cIntEInt = {IntComponent::typeId, eInt}; + + EXPECT_NE(nullptr, manager.CreateComponent(eDouble, + DoubleComponent(0.123))); + ComponentKey cDoubleEDouble = {DoubleComponent::typeId, eDouble}; + + EXPECT_NE(nullptr, manager.CreateComponent(eIntDouble, + IntComponent(456))); + ComponentKey cIntEIntDouble = {IntComponent::typeId, eIntDouble}; + + EXPECT_NE(nullptr, manager.CreateComponent(eIntDouble, + DoubleComponent(0.456))); + ComponentKey cDoubleEIntDouble = {DoubleComponent::typeId, eIntDouble}; + + // Check entities have the components + EXPECT_TRUE(manager.EntityHasComponent(eInt, cIntEInt)); + EXPECT_EQ(1u, manager.ComponentTypes(eInt).size()); + EXPECT_EQ(IntComponent::typeId, *manager.ComponentTypes(eInt).begin()); + + EXPECT_TRUE(manager.EntityHasComponent(eDouble, cDoubleEDouble)); + EXPECT_EQ(1u, manager.ComponentTypes(eDouble).size()); + EXPECT_EQ(DoubleComponent::typeId, *manager.ComponentTypes(eDouble).begin()); + + EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); + EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); + EXPECT_EQ(2u, manager.ComponentTypes(eIntDouble).size()); + auto types = manager.ComponentTypes(eIntDouble); + EXPECT_NE(types.end(), types.find(IntComponent::typeId)); + EXPECT_NE(types.end(), types.find(DoubleComponent::typeId)); + + // Remove component by key + EXPECT_TRUE(manager.RemoveComponent(eInt, cIntEInt)); + EXPECT_FALSE(manager.EntityHasComponent(eInt, cIntEInt)); + EXPECT_TRUE(manager.ComponentTypes(eInt).empty()); + + // Remove component by type id + auto typeDouble = DoubleComponent::typeId; + + EXPECT_TRUE(manager.RemoveComponent(eDouble, typeDouble)); + EXPECT_FALSE(manager.EntityHasComponent(eDouble, cDoubleEDouble)); + EXPECT_TRUE(manager.ComponentTypes(eDouble).empty()); + + // Remove component by type + EXPECT_TRUE(manager.RemoveComponent(eIntDouble)); + EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); + EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); + EXPECT_EQ(1u, manager.ComponentTypes(eIntDouble).size()); + + EXPECT_TRUE(manager.RemoveComponent(eIntDouble)); + EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); + EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); + EXPECT_EQ(0u, manager.ComponentTypes(eIntDouble).size()); + + IGN_UTILS_WARN_RESUME__DEPRECATED_DECLARATION +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(EntityComponentManagerRepeat, diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index fb9ca29f90..c0839aacec 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -858,10 +858,13 @@ ignition::gazebo::loadPluginInfo(bool _isPlayback) configFilename = "server.config"; } - std::string defaultConfig; - ignition::common::env(IGN_HOMEDIR, defaultConfig); - defaultConfig = ignition::common::joinPaths(defaultConfig, ".ignition", - "gazebo", configFilename); + std::string defaultConfigDir; + ignition::common::env(IGN_HOMEDIR, defaultConfigDir); + defaultConfigDir = ignition::common::joinPaths(defaultConfigDir, ".ignition", + "gazebo"); + + auto defaultConfig = ignition::common::joinPaths(defaultConfigDir, + configFilename); if (!ignition::common::exists(defaultConfig)) { @@ -869,6 +872,13 @@ ignition::gazebo::loadPluginInfo(bool _isPlayback) IGNITION_GAZEBO_SERVER_CONFIG_PATH, configFilename); + if (!ignition::common::createDirectories(defaultConfigDir)) + { + ignerr << "Failed to create directory [" << defaultConfigDir + << "]." << std::endl; + return ret; + } + if (!ignition::common::exists(installedConfig)) { ignerr << "Failed to copy installed config [" << installedConfig diff --git a/src/View.cc b/src/View.cc deleted file mode 100644 index 09ac442984..0000000000 --- a/src/View.cc +++ /dev/null @@ -1,84 +0,0 @@ -/* - * Copyright (C) 2018 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. - * -*/ -#include "ignition/gazebo/detail/View.hh" -#include "ignition/gazebo/EntityComponentManager.hh" - -using namespace ignition; -using namespace gazebo; -using namespace detail; - -////////////////////////////////////////////////// -void View::AddEntity(const Entity _entity, const bool _new) -{ - this->entities.insert(_entity); - if (_new) - { - this->newEntities.insert(_entity); - } -} - -////////////////////////////////////////////////// -void View::AddComponent(const Entity _entity, - const ComponentTypeId _typeId, - const ComponentId _componentId) -{ - this->components.insert( - std::make_pair(std::make_pair(_entity, _typeId), _componentId)); -} - -////////////////////////////////////////////////// -bool View::RemoveEntity(const Entity _entity, const ComponentTypeKey &_key) -{ - if (this->entities.find(_entity) == this->entities.end()) - return false; - - // Otherwise, remove the entity from the view - this->entities.erase(_entity); - this->newEntities.erase(_entity); - this->toRemoveEntities.erase(_entity); - - // Remove the entity from the components map - for (const ComponentTypeId &compTypeId : _key) - this->components.erase(std::make_pair(_entity, compTypeId)); - - return true; -} - -///////////////////////////////////////////////// -const components::BaseComponent *View::ComponentImplementation( - const Entity _entity, - ComponentTypeId _typeId, - const EntityComponentManager *_ecm) const -{ - return _ecm->ComponentImplementation( - {_typeId, this->components.at({_entity, _typeId})}); -} - -////////////////////////////////////////////////// -void View::ClearNewEntities() -{ - this->newEntities.clear(); -} - -////////////////////////////////////////////////// -bool View::AddEntityToRemoved(const Entity _entity) -{ - if (this->entities.find(_entity) == this->entities.end()) - return false; - this->toRemoveEntities.insert(_entity); - return true; -} diff --git a/test/helpers/EnvTestFixture.hh b/test/helpers/EnvTestFixture.hh index 984156ae78..a59baef4f2 100644 --- a/test/helpers/EnvTestFixture.hh +++ b/test/helpers/EnvTestFixture.hh @@ -23,15 +23,37 @@ #include #include "ignition/gazebo/test_config.hh" +using namespace ignition; + +/// \brief Common test setup for various tests class ServerFixture : public ::testing::TestWithParam { + // Documentation inherited protected: void SetUp() override { // Augment the system plugin path. In SetUp to avoid test order issues. - ignition::common::setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + common::setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", (std::string(PROJECT_BINARY_PATH) + "/lib").c_str()); - ignition::common::Console::SetVerbosity(4); + common::Console::SetVerbosity(4); + + // Change environment variable so that test files aren't written to $HOME + common::env(IGN_HOMEDIR, this->realHome); + EXPECT_TRUE(common::setenv(IGN_HOMEDIR, this->fakeHome.c_str())); } + + // Documentation inherited + protected: void TearDown() override + { + // Restore $HOME + EXPECT_TRUE(common::setenv(IGN_HOMEDIR, this->realHome.c_str())); + } + + /// \brief Directory to act as $HOME for tests + public: std::string fakeHome = common::joinPaths(PROJECT_BINARY_PATH, "test", + "fake_home"); + + /// \brief Store user's real $HOME to set it back at the end of tests. + public: std::string realHome; }; #endif