diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 2aefa6c54c..75e883f15a 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.cc b/src/EntityComponentManager.cc index fb2d0c9bf6..5336d6cd57 100644 --- a/src/EntityComponentManager.cc +++ b/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); - } } ////////////////////////////////////////////////// diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index a2de6a632d..9b72f1368d 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) } } } @@ -451,15 +460,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); @@ -467,6 +482,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)); } { @@ -476,6 +497,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)); } { @@ -485,6 +512,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)); } { @@ -494,6 +527,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 @@ -528,6 +600,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); + } } ////////////////////////////////////////////////// diff --git a/src/network/PeerTracker_TEST.cc b/src/network/PeerTracker_TEST.cc index 05b3bc02c1..abb46cf6b9 100644 --- a/src/network/PeerTracker_TEST.cc +++ b/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(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); diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index acb994435a..b52b66c820 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -702,6 +702,9 @@ TEST_F(LogSystemTest, RecordAndPlayback) // Create temp directory to store log this->CreateLogsDir(); + // Used to count the expected number of poses recorded. Counting is necessary + // as the number varied depending on the CPU load. + int expectedPoseCount = 0; // Record { // World with moving entities @@ -714,6 +717,23 @@ TEST_F(LogSystemTest, RecordAndPlayback) recordServerConfig.SetUseLogRecord(true); recordServerConfig.SetLogRecordPath(this->logDir); + std::chrono::steady_clock::time_point lastPoseTime; + const int poseHz = 60; + const std::chrono::duration msgPeriod{1.0 / poseHz}; + // This system counts the expected number of poses recorded by reproducing + // the throttle mechanism used by ign-transport. + test::Relay recordedPoseCounter; + recordedPoseCounter.OnPostUpdate( + [&](const UpdateInfo &, const EntityComponentManager &) + { + auto tNow = std::chrono::steady_clock::now(); + if ((tNow - lastPoseTime) > msgPeriod) + { + lastPoseTime = tNow; + ++expectedPoseCount; + } + }); + recordServer.AddSystem(recordedPoseCounter.systemPtr); // Run for a few seconds to record different poses Server recordServer(recordServerConfig); recordServer.Run(true, 1000, false); @@ -804,98 +824,84 @@ TEST_F(LogSystemTest, RecordAndPlayback) // Callback function for entities played back // Compare current pose being played back with the pose with the closest // timestamp in the recorded file. - std::function msgCb = - [&](const msgs::Pose_V &_playedMsg) -> void - { - // Playback continues even after the log file is over - if (batch.end() == recordedIter) - return; - - ASSERT_TRUE(_playedMsg.has_header()); - ASSERT_TRUE(_playedMsg.header().has_stamp()); - EXPECT_EQ(0, _playedMsg.header().stamp().sec()); - - // Get next recorded message - EXPECT_EQ("ignition.msgs.Pose_V", recordedIter->Type()); - recordedMsg.ParseFromString(recordedIter->Data()); - - ASSERT_TRUE(recordedMsg.has_header()); - ASSERT_TRUE(recordedMsg.header().has_stamp()); - EXPECT_EQ(0, recordedMsg.header().stamp().sec()); - - // Log clock timestamp matches message timestamp - EXPECT_EQ(recordedMsg.header().stamp().nsec(), - recordedIter->TimeReceived().count()); - - // Dynamic poses are throttled according to real time during playback, - // so we can't guarantee the exact timestamp as recorded. - EXPECT_NEAR(_playedMsg.header().stamp().nsec(), - recordedMsg.header().stamp().nsec(), 100000000); - - // Loop through all recorded poses, update map - std::map entityRecordedPose; - for (int i = 0; i < recordedMsg.pose_size(); ++i) - { - entityRecordedPose[recordedMsg.pose(i).name()] = recordedMsg.pose(i); - } + test::Relay playbackPoseTester; + playbackPoseTester.OnPostUpdate( + [&](const UpdateInfo &_info, const EntityComponentManager &_ecm) + { + // Playback continues even after the log file is over + if (batch.end() == recordedIter) + return; + + // Get next recorded message + EXPECT_EQ("ignition.msgs.Pose_V", recordedIter->Type()); + recordedMsg.ParseFromString(recordedIter->Data()); + + ASSERT_TRUE(recordedMsg.has_header()); + ASSERT_TRUE(recordedMsg.header().has_stamp()); + EXPECT_EQ(0, recordedMsg.header().stamp().sec()); + + // Log clock timestamp matches message timestamp + EXPECT_EQ(recordedMsg.header().stamp().nsec(), + recordedIter->TimeReceived().count()); + + // A recorded messages is matched when its timestamp is within 100us + // of the current iteration's sim time. + if (std::abs((_info.simTime - recordedIter->TimeReceived()).count()) > + 100000) + { + return; + } - // Has 6 dynamic entities: 4 in dbl pendulum and 2 in nested model - EXPECT_EQ(6, _playedMsg.pose().size()); - EXPECT_EQ(6u, entityRecordedPose.size()); + // Has 6 dynamic entities: 4 in dbl pendulum and 2 in nested model + EXPECT_EQ(6, recordedMsg.pose_size()); - // Loop through all entities and compare played poses to recorded ones - for (int i = 0; i < _playedMsg.pose_size(); ++i) - { - auto posePlayed = msgs::Convert(_playedMsg.pose(i)); - auto poseRecorded = msgs::Convert( - entityRecordedPose[_playedMsg.pose(i).name()]); - - EXPECT_NEAR(posePlayed.Pos().X(), poseRecorded.Pos().X(), 0.1) - << _playedMsg.pose(i).name(); - EXPECT_NEAR(posePlayed.Pos().Y(), poseRecorded.Pos().Y(), 0.1) - << _playedMsg.pose(i).name(); - EXPECT_NEAR(posePlayed.Pos().Z(), poseRecorded.Pos().Z(), 0.1) - << _playedMsg.pose(i).name(); - - EXPECT_NEAR(posePlayed.Rot().Roll(), poseRecorded.Rot().Roll(), 0.1) - << _playedMsg.pose(i).name(); - EXPECT_NEAR(posePlayed.Rot().Pitch(), poseRecorded.Rot().Pitch(), 0.1) - << _playedMsg.pose(i).name(); - EXPECT_NEAR(posePlayed.Rot().Yaw(), poseRecorded.Rot().Yaw(), 0.1) - << _playedMsg.pose(i).name(); - } + // Loop through all recorded poses, and check them against the + // playedback poses. + for (int i = 0; i < recordedMsg.pose_size(); ++i) + { + const math::Pose3d &poseRecorded = msgs::Convert(recordedMsg.pose(i)); + const std::string &name = recordedMsg.pose(i).name(); + + auto entity = _ecm.EntityByComponents( + components::Name(recordedMsg.pose(i).name())); + ASSERT_NE(kNullEntity, entity); + auto poseComp = _ecm.Component(entity); + ASSERT_NE(nullptr, poseComp); + const auto &posePlayed = poseComp->Data(); + + EXPECT_NEAR(posePlayed.Pos().X(), poseRecorded.Pos().X(), 0.1) + << name; + EXPECT_NEAR(posePlayed.Pos().Y(), poseRecorded.Pos().Y(), 0.1) + << name; + EXPECT_NEAR(posePlayed.Pos().Z(), poseRecorded.Pos().Z(), 0.1) + << name; + + EXPECT_NEAR(posePlayed.Rot().Roll(), poseRecorded.Rot().Roll(), 0.1) + << name; + EXPECT_NEAR(posePlayed.Rot().Pitch(), poseRecorded.Rot().Pitch(), 0.1) + << name; + EXPECT_NEAR(posePlayed.Rot().Yaw(), poseRecorded.Rot().Yaw(), 0.1) + << name; + } - ++recordedIter; - nTotal++; - }; + ++recordedIter; + }); - // Subscribe to ignition topic and compare to logged poses - transport::Node node; - // TODO(louise) The world name should match the recorded world - node.Subscribe("/world/default/dynamic_pose/info", msgCb); - - int playbackSteps = 500; - int poseHz = 60; - int expectedPoseCount = playbackSteps * 1e-3 / (1.0/poseHz); - // Run for a few seconds to play back different poses - playServer.Run(true, playbackSteps, false); - - int sleep = 0; - int maxSleep = 30; - for (; nTotal < expectedPoseCount && sleep < maxSleep; ++sleep) - { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - EXPECT_NE(maxSleep, sleep); + playServer.AddSystem(playbackPoseTester.systemPtr); + + // Playback a subset of the log file and check for poses. Note: the poses are + // checked in the playbackPoseTester + playServer.Run(true, 500, false); + + // Count the total number of pose messages in the log file + for (auto it = batch.begin(); it != batch.end(); ++it, ++nTotal) { } - #if !defined (__APPLE__) - /// \todo(anyone) there seems to be a race condition that sometimes cause an - /// additional messages to be published by the scene broadcaster - // 60Hz - EXPECT_TRUE(nTotal == expectedPoseCount || nTotal == expectedPoseCount + 1) + // The expectedPoseCount might be off by ±2 because the ign transport's + // throttle mechanism (which is used by the SceneBroadcaster when publishing + // poses) uses real-time + EXPECT_LE(std::abs(nTotal - expectedPoseCount), 2) << "nTotal [" << nTotal << "] expectedPoseCount [" << expectedPoseCount << "]"; - #endif this->RemoveLogsDir(); } diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 730b918814..f8b3598c60 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -626,7 +626,7 @@ TEST_F(PhysicsSystemFixture, ResetPositionComponent) sdf::Root root; root.Load(sdfFile); const sdf::World *world = root.WorldByIndex(0); - ASSERT_TRUE(nullptr != world); + ASSERT_NE(nullptr, world); serverConfig.SetSdfFile(sdfFile); @@ -650,6 +650,8 @@ TEST_F(PhysicsSystemFixture, ResetPositionComponent) [&](const ignition::gazebo::Entity &_entity, const components::Joint *, components::Name *_name) -> bool { + EXPECT_NE(nullptr, _name); + if (_name->Data() == rotatingJointName) { auto resetComp = @@ -669,8 +671,8 @@ TEST_F(PhysicsSystemFixture, ResetPositionComponent) } else { - EXPECT_EQ(nullptr, resetComp); - EXPECT_NE(nullptr, position); + EXPECT_EQ(nullptr, resetComp); + EXPECT_NE(nullptr, position); } } return true; @@ -689,6 +691,8 @@ TEST_F(PhysicsSystemFixture, ResetPositionComponent) const components::Name *_name, const components::JointPosition *_pos) { + EXPECT_NE(nullptr, _name); + if (_name->Data() == rotatingJointName) { positions.push_back(_pos->Data()[0]); @@ -697,16 +701,16 @@ TEST_F(PhysicsSystemFixture, ResetPositionComponent) }); }); - server.AddSystem(testSystem.systemPtr); - server.Run(true, 2, false); + server.AddSystem(testSystem.systemPtr); + server.Run(true, 2, false); - ASSERT_EQ(positions.size(), 2ul); + ASSERT_EQ(positions.size(), 2ul); - // First position should be exactly the same - EXPECT_DOUBLE_EQ(pos0, positions[0]); + // First position should be exactly the same + EXPECT_DOUBLE_EQ(pos0, positions[0]); - // Second position should be different, but close - EXPECT_NEAR(pos0, positions[1], 0.01); + // Second position should be different, but close + EXPECT_NEAR(pos0, positions[1], 0.01); } /////////////////////////////////////////////////