3 ➡️ 4

include/ignition/gazebo/EntityComponentManager.hh

@@ -226,6 +226,20 @@ namespace ignition
               std::optional<typename ComponentTypeT::Type> 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<typename ComponentTypeT>
+              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.

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);

include/ignition/gazebo/detail/EntityComponentManager.hh

@@ -20,15 +20,61 @@
 #include <cstring>
 #include <map>
 #include <set>
+#include <type_traits>
 #include <utility>
 #include <vector>
 
+#include <ignition/math/Helpers.hh>
+
 #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<typename T>
+  TestEqualityOperator operator == (const T&, const T&);
+
+  /// \brief Type trait that determines if an operator== is defined for `T`.
+  template<typename T>
+  struct HasEqualityOperator
+  {
+    enum
+    {
+      // False positive codecheck "Using C-style cast"
+      value = !std::is_same<decltype(*(T*)(0) == *(T*)(0)), TestEqualityOperator>::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<typename DataType>
+auto CompareData = [](const DataType &_a, const DataType &_b) -> bool
+{
+  if constexpr (std::is_same<DataType, double>::value)
+  {
+    return math::equal(_a, _b);
+  }
+  else if constexpr (traits::HasEqualityOperator<DataType>::value)
+  {
+    return _a == _b;
+  }
+
+  return false;
+};
+
 //////////////////////////////////////////////////
 template<typename ComponentTypeT>
 ComponentKey EntityComponentManager::CreateComponent(const Entity _entity,
@@ -90,6 +136,22 @@ std::optional<typename ComponentTypeT::Type>
   return std::make_optional(comp->Data());
 }
 
+//////////////////////////////////////////////////
+template<typename ComponentTypeT>
+bool EntityComponentManager::SetComponentData(const Entity _entity,
+    const typename ComponentTypeT::Type &_data)
+{
+  auto comp = this->Component<ComponentTypeT>(_entity);
+
+  if (nullptr == comp)
+  {
+    this->CreateComponent(_entity, ComponentTypeT(_data));
+    return true;
+  }
+
+  return comp->SetData(_data, CompareData<typename ComponentTypeT::Type>);
+}
+
 //////////////////////////////////////////////////
 template<typename ComponentTypeT>
 const ComponentTypeT *EntityComponentManager::First() const

src/EntityComponentManager.cc

@@ -945,15 +945,6 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg,
   // Add a component to the message and set it to be removed if the component
   // exists in the removedComponents map.
   this->dataPtr->SetRemovedComponentsMsgs(_entity, _msg);
-
-  // Remove the entity from the message if a component for the entity was
-  // not modified or added. This will allow the state message to shrink.
-  if (entIter == _msg.mutable_entities()->end() &&
-      (entIter = _msg.mutable_entities()->find(_entity)) !=
-      _msg.mutable_entities()->end())
-  {
-    _msg.mutable_entities()->erase(entIter);
-  }
 }
 
 //////////////////////////////////////////////////

src/EntityComponentManager_TEST.cc

@@ -63,6 +63,15 @@ IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Even", Even)
 
 using Odd = components::Component<components::NoData, class OddTag>;
 IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Odd", Odd)
+
+struct Custom
+{
+  int dummy{123};
+};
+
+using CustomComponent = components::Component<Custom, class CustomTag>;
+IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.CustomComponent",
+    CustomComponent)
 }
 }
 }
@@ -451,22 +460,34 @@ 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<IntComponent>(eInt, IntComponent(123));
   manager.CreateComponent<DoubleComponent>(eDouble, DoubleComponent(0.123));
   manager.CreateComponent<IntComponent>(eIntDouble, IntComponent(456));
   manager.CreateComponent<DoubleComponent>(eIntDouble, DoubleComponent(0.456));
+  manager.CreateComponent<components::Pose>(ePose,
+      components::Pose({1, 2, 3, 0, 0, 0}));
+  manager.CreateComponent<CustomComponent>(eCustom,
+      CustomComponent(Custom()));
 
-  // Get component values
+  // Get and set component values
   {
     const auto *value = manager.Component<IntComponent>(eInt);
     ASSERT_NE(nullptr, value);
     EXPECT_EQ(123, value->Data());
 
     auto data = manager.ComponentData<IntComponent>(eInt);
     EXPECT_EQ(123, data);
+
+    EXPECT_TRUE(manager.SetComponentData<IntComponent>(eInt, 456));
+    data = manager.ComponentData<IntComponent>(eInt);
+    EXPECT_EQ(456, data);
+
+    EXPECT_FALSE(manager.SetComponentData<IntComponent>(eInt, 456));
   }
 
   {
@@ -476,6 +497,12 @@ TEST_P(EntityComponentManagerFixture, ComponentValues)
 
     auto data = manager.ComponentData<DoubleComponent>(eDouble);
     EXPECT_EQ(0.123, data);
+
+    EXPECT_TRUE(manager.SetComponentData<DoubleComponent>(eDouble, 0.456));
+    data = manager.ComponentData<DoubleComponent>(eDouble);
+    EXPECT_EQ(0.456, data);
+
+    EXPECT_FALSE(manager.SetComponentData<DoubleComponent>(eDouble, 0.456));
   }
 
   {
@@ -485,6 +512,12 @@ TEST_P(EntityComponentManagerFixture, ComponentValues)
 
     auto data = manager.ComponentData<IntComponent>(eIntDouble);
     EXPECT_EQ(456, data);
+
+    EXPECT_TRUE(manager.SetComponentData<IntComponent>(eIntDouble, 789));
+    data = manager.ComponentData<IntComponent>(eIntDouble);
+    EXPECT_EQ(789, data);
+
+    EXPECT_FALSE(manager.SetComponentData<IntComponent>(eIntDouble, 789));
   }
 
   {
@@ -494,6 +527,45 @@ TEST_P(EntityComponentManagerFixture, ComponentValues)
 
     auto data = manager.ComponentData<DoubleComponent>(eIntDouble);
     EXPECT_EQ(0.456, data);
+
+    EXPECT_TRUE(manager.SetComponentData<DoubleComponent>(eIntDouble, 0.789));
+    data = manager.ComponentData<DoubleComponent>(eIntDouble);
+    EXPECT_EQ(0.789, data);
+
+    EXPECT_FALSE(manager.SetComponentData<DoubleComponent>(eIntDouble, 0.789));
+  }
+
+  {
+    const auto *value = manager.Component<components::Pose>(ePose);
+    ASSERT_NE(nullptr, value);
+    EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), value->Data());
+
+    auto data = manager.ComponentData<components::Pose>(ePose);
+    EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), data);
+
+    EXPECT_TRUE(manager.SetComponentData<components::Pose>(ePose,
+        {4, 5, 6, 0, 0, 0}));
+    data = manager.ComponentData<components::Pose>(ePose);
+    EXPECT_EQ(math::Pose3d(4, 5, 6, 0, 0, 0), data);
+
+    EXPECT_FALSE(manager.SetComponentData<components::Pose>(ePose,
+        {4, 5, 6, 0, 0, 0}));
+  }
+
+  {
+    const auto *value = manager.Component<CustomComponent>(eCustom);
+    ASSERT_NE(nullptr, value);
+    EXPECT_EQ(123, value->Data().dummy);
+
+    auto data = manager.ComponentData<CustomComponent>(eCustom);
+    EXPECT_EQ(123, data->dummy);
+
+    EXPECT_TRUE(manager.SetComponentData<CustomComponent>(eCustom, {456}));
+    data = manager.ComponentData<CustomComponent>(eCustom);
+    EXPECT_EQ(456, data->dummy);
+
+    // No equality operator, always returns true
+    EXPECT_TRUE(manager.SetComponentData<CustomComponent>(eCustom, {456}));
   }
 
   // Failure cases
@@ -528,6 +600,23 @@ TEST_P(EntityComponentManagerFixture, ComponentValues)
     auto data = manager.ComponentData<DoubleComponent>(999);
     EXPECT_EQ(std::nullopt, data);
   }
+
+  // Set new component type
+  {
+    const auto *value = manager.Component<IntComponent>(eDouble);
+    EXPECT_EQ(nullptr, value);
+
+    auto data = manager.ComponentData<IntComponent>(eDouble);
+    EXPECT_EQ(std::nullopt, data);
+
+    EXPECT_TRUE(manager.SetComponentData<IntComponent>(eDouble, 123));
+
+    value = manager.Component<IntComponent>(eDouble);
+    ASSERT_NE(nullptr, value);
+
+    data = manager.ComponentData<IntComponent>(eDouble);
+    EXPECT_EQ(123, data);
+  }
 }
 
 //////////////////////////////////////////////////

src/network/PeerTracker_TEST.cc

@@ -74,7 +74,17 @@ TEST(PeerTracker, PeerTracker)
   EXPECT_EQ(5, peers);
 
   // Allow all the heartbeats to propagate
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  int maxSleep{100};
+  int sleep{0};
+  for (; sleep < maxSleep &&
+      (tracker1->NumPeers() < 5 ||
+      tracker2->NumPeers() < 5 ||
+      tracker3->NumPeers() < 5 ||
+      tracker4->NumPeers() < 5 ||
+      tracker5->NumPeers() < 5); ++sleep)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(30));
+  }
 
   // All counts exclude self.
   EXPECT_EQ(5u, tracker1->NumPeers());
@@ -155,10 +165,14 @@ TEST(PeerTracker, PeerTrackerStale)
   auto tracker2 = std::make_shared<PeerTracker>(info2);
   tracker2->SetHeartbeatPeriod(std::chrono::milliseconds(100));
 
-  for (int sleep = 0; sleep < 50 && tracker2->NumPeers() == 0; ++sleep)
+  int maxSleep{100};
+  int sleep{0};
+  for (; sleep < maxSleep && (tracker2->NumPeers() == 0 || stalePeers == 0);
+      ++sleep)
   {
     std::this_thread::sleep_for(std::chrono::milliseconds(30));
   }
+  EXPECT_LT(sleep, maxSleep);
 
   EXPECT_EQ(1, stalePeers);