diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 9a0a417de9..bc99cf7eb1 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -226,6 +226,20 @@ namespace ignition std::optional ComponentData( const Entity _entity) const; + /// \brief Set the data from a component. + /// * If the component type doesn't hold any data, this won't compile. + /// * If the entity doesn't have that component, the component will be + /// created. + /// * If the entity has the component, its data will be updated. + /// \param[in] _entity The entity. + /// \param[in] _data New component data + /// \tparam ComponentTypeT Component type + /// \return True if data has changed. It will always be true if the data + /// type doesn't have an equality operator. + public: template + bool SetComponentData(const Entity _entity, + const typename ComponentTypeT::Type &_data); + /// \brief Get the type IDs of all components attached to an entity. /// \param[in] _entity Entity to check. /// \return All the component type IDs. diff --git a/include/ignition/gazebo/components/Component.hh b/include/ignition/gazebo/components/Component.hh index cdfb2fba23..727c2b48be 100644 --- a/include/ignition/gazebo/components/Component.hh +++ b/include/ignition/gazebo/components/Component.hh @@ -359,7 +359,7 @@ namespace components /// \param[in] _eql Equality comparison function. This function should /// return true if two instances of DataType are equal. /// \param[in} _ecm Pointer to the entity component manager. - /// \return True if the _eql function returns true. + /// \return True if the _eql function returns false. public: bool SetData(const DataType &_data, const std::function< bool(const DataType &, const DataType &)> &_eql); diff --git a/include/ignition/gazebo/detail/EntityComponentManager.hh b/include/ignition/gazebo/detail/EntityComponentManager.hh index 0ea2777849..ee87528162 100644 --- a/include/ignition/gazebo/detail/EntityComponentManager.hh +++ b/include/ignition/gazebo/detail/EntityComponentManager.hh @@ -20,15 +20,61 @@ #include #include #include +#include #include #include +#include + #include "ignition/gazebo/EntityComponentManager.hh" namespace ignition { namespace gazebo { +////////////////////////////////////////////////// +namespace traits +{ + /// \brief Helper struct to determine if an equality operator is present. + struct TestEqualityOperator + { + }; + template + TestEqualityOperator operator == (const T&, const T&); + + /// \brief Type trait that determines if an operator== is defined for `T`. + template + struct HasEqualityOperator + { + enum + { + // False positive codecheck "Using C-style cast" + value = !std::is_same::value // NOLINT + }; + }; +} + +////////////////////////////////////////////////// +/// \brief Helper function to compare two objects of the same type using its +/// equality operator. +/// If `DataType` doesn't have an equality operator defined, it will return +/// false. +/// For doubles, `ignition::math::equal` will be used. +template +auto CompareData = [](const DataType &_a, const DataType &_b) -> bool +{ + if constexpr (std::is_same::value) + { + return math::equal(_a, _b); + } + else if constexpr (traits::HasEqualityOperator::value) + { + return _a == _b; + } + + return false; +}; + ////////////////////////////////////////////////// template ComponentKey EntityComponentManager::CreateComponent(const Entity _entity, @@ -90,6 +136,22 @@ std::optional return std::make_optional(comp->Data()); } +////////////////////////////////////////////////// +template +bool EntityComponentManager::SetComponentData(const Entity _entity, + const typename ComponentTypeT::Type &_data) +{ + auto comp = this->Component(_entity); + + if (nullptr == comp) + { + this->CreateComponent(_entity, ComponentTypeT(_data)); + return true; + } + + return comp->SetData(_data, CompareData); +} + ////////////////////////////////////////////////// template const ComponentTypeT *EntityComponentManager::First() const diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index a87b6e9666..6a9714d5c4 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -63,6 +63,15 @@ IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Even", Even) using Odd = components::Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Odd", Odd) + +struct Custom +{ + int dummy{123}; +}; + +using CustomComponent = components::Component; +IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.CustomComponent", + CustomComponent) } } } @@ -447,15 +456,21 @@ TEST_P(EntityComponentManagerFixture, ComponentValues) Entity eInt = manager.CreateEntity(); Entity eDouble = manager.CreateEntity(); Entity eIntDouble = manager.CreateEntity(); - EXPECT_EQ(3u, manager.EntityCount()); + Entity ePose = manager.CreateEntity(); + Entity eCustom = manager.CreateEntity(); + 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, + components::Pose({1, 2, 3, 0, 0, 0})); + manager.CreateComponent(eCustom, + CustomComponent(Custom())); - // Get component values + // Get and set component values { const auto *value = manager.Component(eInt); ASSERT_NE(nullptr, value); @@ -463,6 +478,12 @@ TEST_P(EntityComponentManagerFixture, ComponentValues) auto data = manager.ComponentData(eInt); EXPECT_EQ(123, data); + + EXPECT_TRUE(manager.SetComponentData(eInt, 456)); + data = manager.ComponentData(eInt); + EXPECT_EQ(456, data); + + EXPECT_FALSE(manager.SetComponentData(eInt, 456)); } { @@ -472,6 +493,12 @@ TEST_P(EntityComponentManagerFixture, ComponentValues) auto data = manager.ComponentData(eDouble); EXPECT_EQ(0.123, data); + + EXPECT_TRUE(manager.SetComponentData(eDouble, 0.456)); + data = manager.ComponentData(eDouble); + EXPECT_EQ(0.456, data); + + EXPECT_FALSE(manager.SetComponentData(eDouble, 0.456)); } { @@ -481,6 +508,12 @@ TEST_P(EntityComponentManagerFixture, ComponentValues) auto data = manager.ComponentData(eIntDouble); EXPECT_EQ(456, data); + + EXPECT_TRUE(manager.SetComponentData(eIntDouble, 789)); + data = manager.ComponentData(eIntDouble); + EXPECT_EQ(789, data); + + EXPECT_FALSE(manager.SetComponentData(eIntDouble, 789)); } { @@ -490,6 +523,45 @@ TEST_P(EntityComponentManagerFixture, ComponentValues) auto data = manager.ComponentData(eIntDouble); EXPECT_EQ(0.456, data); + + EXPECT_TRUE(manager.SetComponentData(eIntDouble, 0.789)); + data = manager.ComponentData(eIntDouble); + EXPECT_EQ(0.789, data); + + EXPECT_FALSE(manager.SetComponentData(eIntDouble, 0.789)); + } + + { + const auto *value = manager.Component(ePose); + ASSERT_NE(nullptr, value); + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), value->Data()); + + auto data = manager.ComponentData(ePose); + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), data); + + EXPECT_TRUE(manager.SetComponentData(ePose, + {4, 5, 6, 0, 0, 0})); + data = manager.ComponentData(ePose); + EXPECT_EQ(math::Pose3d(4, 5, 6, 0, 0, 0), data); + + EXPECT_FALSE(manager.SetComponentData(ePose, + {4, 5, 6, 0, 0, 0})); + } + + { + const auto *value = manager.Component(eCustom); + ASSERT_NE(nullptr, value); + EXPECT_EQ(123, value->Data().dummy); + + auto data = manager.ComponentData(eCustom); + EXPECT_EQ(123, data->dummy); + + EXPECT_TRUE(manager.SetComponentData(eCustom, {456})); + data = manager.ComponentData(eCustom); + EXPECT_EQ(456, data->dummy); + + // No equality operator, always returns true + EXPECT_TRUE(manager.SetComponentData(eCustom, {456})); } // Failure cases @@ -524,6 +596,23 @@ TEST_P(EntityComponentManagerFixture, ComponentValues) auto data = manager.ComponentData(999); EXPECT_EQ(std::nullopt, data); } + + // Set new component type + { + const auto *value = manager.Component(eDouble); + EXPECT_EQ(nullptr, value); + + auto data = manager.ComponentData(eDouble); + EXPECT_EQ(std::nullopt, data); + + EXPECT_TRUE(manager.SetComponentData(eDouble, 123)); + + value = manager.Component(eDouble); + ASSERT_NE(nullptr, value); + + data = manager.ComponentData(eDouble); + EXPECT_EQ(123, data); + } } //////////////////////////////////////////////////