From 632d7741e6fd3f163241b762526d8003fb078e42 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 16 Nov 2020 17:42:38 -0800 Subject: [PATCH 1/4] Helper function to set component data (#436) Signed-off-by: Louise Poubel --- .../ignition/gazebo/EntityComponentManager.hh | 14 +++ .../ignition/gazebo/components/Component.hh | 2 +- .../gazebo/detail/EntityComponentManager.hh | 62 +++++++++++++ src/EntityComponentManager_TEST.cc | 93 ++++++++++++++++++- 4 files changed, 168 insertions(+), 3 deletions(-) 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); + } } ////////////////////////////////////////////////// From f02c5e426e626f7d0369924922fb0f3b8529aace Mon Sep 17 00:00:00 2001 From: John Shepherd Date: Tue, 17 Nov 2020 20:57:42 -0800 Subject: [PATCH 2/4] Remove unneeded if statement (#432) Signed-off-by: John Shepherd --- src/EntityComponentManager.cc | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index d5cadc408d..9946ccbaf8 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -851,15 +851,6 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, // TODO(anyone) Set component being removed once we have a way to queue it } - - // 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); - } } ////////////////////////////////////////////////// From 08b6c42172270600b0525d012c01c9381769e893 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 20 Nov 2020 15:20:15 -0600 Subject: [PATCH 3/4] Fixes flaky RecordAndPlayback test in INTEGRATION_log_system (#463) The flakiness comes from two sources: 1. Poses recorded by the LogRecorder are published by the SceneBroadcaster system throttled at 60 Hz. The throttle mechanism uses real-time instead of sim-time which causes a variance in the number of recorded poses from run to run. However, the expected number of recorded poses was calculated with the assumption that the simulation would run with a 1.0 RTF. If the CPU load is high, there could be a mismatch between the expected and the actual number of recorded poses, which causes the test to fail. This can be checked by running the test with `cpulimit -l 20 -f bin/INTEGRATION_log_system -- --gtest_filter="*RecordAndPlayback"` 2. An attempt is made to match up played back poses with the closest timestamp in the recorded file. These poses are again published by the SceneBroadcaster, so they are subject to the same kind of timing issues as the recorded poses. The solution in this patch for the first issue is to determine the expected number of recorded poses by counting them in a separate system that mimics the throttling mechanism of ign-transport. For the second issue, a testing system is added to the playback server bypassing the SceSceneBroadcaster altogether. Signed-off-by: Addisu Z. Taddese --- test/integration/log_system.cc | 176 +++++++++++++++++---------------- 1 file changed, 91 insertions(+), 85 deletions(-) diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index 0a0d158011..2aac7734f2 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -707,6 +707,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 @@ -725,6 +728,23 @@ TEST_F(LogSystemTest, RecordAndPlayback) recordServerConfig.SetSdfString(recordSdfRoot.Element()->ToString("")); Server recordServer(recordServerConfig); + 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 recordServer.Run(true, 1000, false); } @@ -814,98 +834,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(); } From 2f76de8f5da2a6d106e3b37e375ad1887bdd45e8 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 23 Nov 2020 13:03:07 -0800 Subject: [PATCH 4/4] Make PeerTracker test more robust (#452) Signed-off-by: Louise Poubel Co-authored-by: Michael Carroll --- src/network/PeerTracker_TEST.cc | 18 ++++++++++++++++-- test/integration/physics_system.cc | 24 ++++++++++++++---------- 2 files changed, 30 insertions(+), 12 deletions(-) diff --git a/src/network/PeerTracker_TEST.cc b/src/network/PeerTracker_TEST.cc index 41c800b497..0e1d678b24 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/physics_system.cc b/test/integration/physics_system.cc index a0fa82f856..171cf8c8c1 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); } /////////////////////////////////////////////////