From a1e48dd819617cfed1a2d3529c56a86775ab6036 Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Tue, 15 Sep 2020 11:40:37 -0400 Subject: [PATCH 1/6] enhanced log playback performance Signed-off-by: Ashton Larkin --- src/systems/log/LogPlayback.cc | 83 ++++++++----------- .../scene_broadcaster/SceneBroadcaster.cc | 2 +- 2 files changed, 35 insertions(+), 50 deletions(-) diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index f9d62da1f4..b84c2e1489 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -69,10 +70,10 @@ class ignition::gazebo::systems::LogPlaybackPrivate /// \return String of prepended path. public: std::string PrependLogPath(const std::string &_uri); - /// \brief Updates the ECM according to the given message. - /// \param[in] _ecm Mutable ECM. + /// \brief Keeps track of which entity poses have updated + /// according to the given message. /// \param[in] _msg Message containing pose updates. - public: void Parse(EntityComponentManager &_ecm, const msgs::Pose_V &_msg); + public: void Parse(const msgs::Pose_V &_msg); /// \brief Updates the ECM according to the given message. /// \param[in] _ecm Mutable ECM. @@ -113,6 +114,10 @@ class ignition::gazebo::systems::LogPlaybackPrivate /// \brief Flag for backward compatibility with log files recorded in older /// plugin versions that did not record resources. False for older log files. public: bool doReplaceResourceURIs{true}; + + /// \brief Saves which entity poses have changed according to the latest + /// LogPlaybackPrivate::Parse call. + public: std::unordered_map recentEntityPoseUpdates; }; bool LogPlaybackPrivate::started{false}; @@ -133,53 +138,15 @@ LogPlayback::~LogPlayback() } ////////////////////////////////////////////////// -void LogPlaybackPrivate::Parse(EntityComponentManager &_ecm, - const msgs::Pose_V &_msg) +void LogPlaybackPrivate::Parse(const msgs::Pose_V &_msg) { - // Maps entity to pose recorded - // Key: entity. Value: pose - std::map idToPose; - - for (int i = 0; i < _msg.pose_size(); ++i) + // save the new entity pose updates + this->recentEntityPoseUpdates.clear(); + for (auto i=0; i < _msg.pose_size(); ++i) { - msgs::Pose pose = _msg.pose(i); - - // Update entity pose in map - idToPose.insert_or_assign(pose.id(), pose); + const auto &pose = _msg.pose(i); + this->recentEntityPoseUpdates.insert_or_assign(pose.id(), pose); } - - // Loop through entities in world - _ecm.Each( - [&](const Entity &_entity, components::Pose *_poseComp) -> bool - { - // Check if we have an updated pose for this entity - if (idToPose.find(_entity) == idToPose.end()) - return true; - - // Look for pose in log entry loaded - msgs::Pose pose = idToPose.at(_entity); - // Set current pose to recorded pose - // Use copy assignment operator - *_poseComp = components::Pose(msgs::Convert(pose)); - - // The ComponentState::OneTimeChange argument forces the - // SceneBroadcaster system to publish a message. This parameter used to - // be ComponentState::PeriodicChange. The problem with PeriodicChange is - // that state updates could be missed by the SceneBroadscaster, which - // publishes at 60Hz using the wall clock. If a 60Hz tick - // doesn't fall on the same update cycle as this state change then the - // GUI will not receive the state information. The result is jumpy - // playback. - // - // \todo(anyone) I don't think using OneTimeChange is necessarily bad, but - // it would be nice if other systems could know that log playback is - // active/enabled. Then a system could make decisions on how to process - // information. - _ecm.SetChanged(_entity, components::Pose::typeId, - ComponentState::OneTimeChange); - - return true; - }); } ////////////////////////////////////////////////// @@ -516,7 +483,7 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) // Only set the last pose of a sequence of poses. if (msgType != "ignition.msgs.Pose_V" && queuedPose.pose_size() > 0) { - this->dataPtr->Parse(_ecm, queuedPose); + this->dataPtr->Parse(queuedPose); queuedPose.Clear(); } @@ -593,9 +560,27 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) if (queuedPose.pose_size() > 0) { - this->dataPtr->Parse(_ecm, queuedPose); + this->dataPtr->Parse(queuedPose); } + // flag changed entity poses as periodically changed based on + // the latest LogPlaybackPrivate::Parse results + _ecm.Each( + [&](const Entity &_entity, components::Pose *_poseComp) -> bool + { + if (this->dataPtr->recentEntityPoseUpdates.find(_entity) == + this->dataPtr->recentEntityPoseUpdates.end()) + return true; + + msgs::Pose pose = this->dataPtr->recentEntityPoseUpdates.at(_entity); + *_poseComp = components::Pose(msgs::Convert(pose)); + + _ecm.SetChanged(_entity, components::Pose::typeId, + ComponentState::PeriodicChange); + + return true; + }); + // for seek back in time only // remove entities that should not be present in the current time step for (auto entity : entitiesToRemove) diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index cc1aa5194e..72f20491c1 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -298,7 +298,7 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, // changed components if (shouldPublish) { - IGN_PROFILE("SceneBroadcast::PoseUpdate Publish State"); + IGN_PROFILE("SceneBroadcast::PostUpdate Publish State"); this->dataPtr->statePub.Publish(this->dataPtr->stepMsg); this->dataPtr->lastStatePubTime = now; } From 06ba47be6a19ecee0e03cb93a8daf80250d22e9a Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Wed, 16 Sep 2020 23:01:28 -0400 Subject: [PATCH 2/6] Handling multiple Parse(...) calls in a single Update Signed-off-by: Ashton Larkin --- src/systems/log/LogPlayback.cc | 23 ++++++++++++++++++----- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index b84c2e1489..f9d298e3b9 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -73,7 +73,10 @@ class ignition::gazebo::systems::LogPlaybackPrivate /// \brief Keeps track of which entity poses have updated /// according to the given message. /// \param[in] _msg Message containing pose updates. - public: void Parse(const msgs::Pose_V &_msg); + /// \param[in] _clear Whether the most recently cached pose updates + /// should be deleted or not. Useful for when Parse is called multiple + /// times in the same Update cycle. + public: void Parse(const msgs::Pose_V &_msg, bool &_clear); /// \brief Updates the ECM according to the given message. /// \param[in] _ecm Mutable ECM. @@ -138,15 +141,21 @@ LogPlayback::~LogPlayback() } ////////////////////////////////////////////////// -void LogPlaybackPrivate::Parse(const msgs::Pose_V &_msg) +void LogPlaybackPrivate::Parse(const msgs::Pose_V &_msg, bool &_clear) { + if (_clear) + this->recentEntityPoseUpdates.clear(); + // save the new entity pose updates - this->recentEntityPoseUpdates.clear(); for (auto i=0; i < _msg.pose_size(); ++i) { const auto &pose = _msg.pose(i); this->recentEntityPoseUpdates.insert_or_assign(pose.id(), pose); } + + // make sure that any future detected pose updates from the same Update + // cycle don't overwrite already-cached pose updates from the same cycle + _clear = false; } ////////////////////////////////////////////////// @@ -475,6 +484,10 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) msgs::Pose_V queuedPose; + // if new pose updates are received, make sure that only the cached poses + // from a previous Update cycle are cleared + bool clearCachedPoseUpdates = true; + auto iter = this->dataPtr->batch.begin(); while (iter != this->dataPtr->batch.end()) { @@ -483,7 +496,7 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) // Only set the last pose of a sequence of poses. if (msgType != "ignition.msgs.Pose_V" && queuedPose.pose_size() > 0) { - this->dataPtr->Parse(queuedPose); + this->dataPtr->Parse(queuedPose, clearCachedPoseUpdates); queuedPose.Clear(); } @@ -560,7 +573,7 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) if (queuedPose.pose_size() > 0) { - this->dataPtr->Parse(queuedPose); + this->dataPtr->Parse(queuedPose, clearCachedPoseUpdates); } // flag changed entity poses as periodically changed based on From b07ebddf7d552e56030213228c0ad3c9384bab58 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 17 Sep 2020 06:09:43 -0700 Subject: [PATCH 3/6] Change clear behavior Signed-off-by: Nate Koenig --- src/systems/log/LogPlayback.cc | 23 ++++++----------------- 1 file changed, 6 insertions(+), 17 deletions(-) diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index f9d298e3b9..359c58c154 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -73,10 +73,7 @@ class ignition::gazebo::systems::LogPlaybackPrivate /// \brief Keeps track of which entity poses have updated /// according to the given message. /// \param[in] _msg Message containing pose updates. - /// \param[in] _clear Whether the most recently cached pose updates - /// should be deleted or not. Useful for when Parse is called multiple - /// times in the same Update cycle. - public: void Parse(const msgs::Pose_V &_msg, bool &_clear); + public: void Parse(const msgs::Pose_V &_msg); /// \brief Updates the ECM according to the given message. /// \param[in] _ecm Mutable ECM. @@ -141,21 +138,14 @@ LogPlayback::~LogPlayback() } ////////////////////////////////////////////////// -void LogPlaybackPrivate::Parse(const msgs::Pose_V &_msg, bool &_clear) +void LogPlaybackPrivate::Parse(const msgs::Pose_V &_msg) { - if (_clear) - this->recentEntityPoseUpdates.clear(); - // save the new entity pose updates for (auto i=0; i < _msg.pose_size(); ++i) { const auto &pose = _msg.pose(i); this->recentEntityPoseUpdates.insert_or_assign(pose.id(), pose); } - - // make sure that any future detected pose updates from the same Update - // cycle don't overwrite already-cached pose updates from the same cycle - _clear = false; } ////////////////////////////////////////////////// @@ -484,9 +474,8 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) msgs::Pose_V queuedPose; - // if new pose updates are received, make sure that only the cached poses - // from a previous Update cycle are cleared - bool clearCachedPoseUpdates = true; + // Clear cached poses once. + this->dataPtr->recentEntityPoseUpdates.clear(); auto iter = this->dataPtr->batch.begin(); while (iter != this->dataPtr->batch.end()) @@ -496,7 +485,7 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) // Only set the last pose of a sequence of poses. if (msgType != "ignition.msgs.Pose_V" && queuedPose.pose_size() > 0) { - this->dataPtr->Parse(queuedPose, clearCachedPoseUpdates); + this->dataPtr->Parse(queuedPose); queuedPose.Clear(); } @@ -573,7 +562,7 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) if (queuedPose.pose_size() > 0) { - this->dataPtr->Parse(queuedPose, clearCachedPoseUpdates); + this->dataPtr->Parse(queuedPose); } // flag changed entity poses as periodically changed based on From c7c0118b76e2cb6437c8266ef2448ac0eae51a60 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 17 Sep 2020 09:38:52 -0700 Subject: [PATCH 4/6] Revert changes Signed-off-by: Nate Koenig --- src/systems/log/LogPlayback.cc | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index 359c58c154..f9d298e3b9 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -73,7 +73,10 @@ class ignition::gazebo::systems::LogPlaybackPrivate /// \brief Keeps track of which entity poses have updated /// according to the given message. /// \param[in] _msg Message containing pose updates. - public: void Parse(const msgs::Pose_V &_msg); + /// \param[in] _clear Whether the most recently cached pose updates + /// should be deleted or not. Useful for when Parse is called multiple + /// times in the same Update cycle. + public: void Parse(const msgs::Pose_V &_msg, bool &_clear); /// \brief Updates the ECM according to the given message. /// \param[in] _ecm Mutable ECM. @@ -138,14 +141,21 @@ LogPlayback::~LogPlayback() } ////////////////////////////////////////////////// -void LogPlaybackPrivate::Parse(const msgs::Pose_V &_msg) +void LogPlaybackPrivate::Parse(const msgs::Pose_V &_msg, bool &_clear) { + if (_clear) + this->recentEntityPoseUpdates.clear(); + // save the new entity pose updates for (auto i=0; i < _msg.pose_size(); ++i) { const auto &pose = _msg.pose(i); this->recentEntityPoseUpdates.insert_or_assign(pose.id(), pose); } + + // make sure that any future detected pose updates from the same Update + // cycle don't overwrite already-cached pose updates from the same cycle + _clear = false; } ////////////////////////////////////////////////// @@ -474,8 +484,9 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) msgs::Pose_V queuedPose; - // Clear cached poses once. - this->dataPtr->recentEntityPoseUpdates.clear(); + // if new pose updates are received, make sure that only the cached poses + // from a previous Update cycle are cleared + bool clearCachedPoseUpdates = true; auto iter = this->dataPtr->batch.begin(); while (iter != this->dataPtr->batch.end()) @@ -485,7 +496,7 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) // Only set the last pose of a sequence of poses. if (msgType != "ignition.msgs.Pose_V" && queuedPose.pose_size() > 0) { - this->dataPtr->Parse(queuedPose); + this->dataPtr->Parse(queuedPose, clearCachedPoseUpdates); queuedPose.Clear(); } @@ -562,7 +573,7 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) if (queuedPose.pose_size() > 0) { - this->dataPtr->Parse(queuedPose); + this->dataPtr->Parse(queuedPose, clearCachedPoseUpdates); } // flag changed entity poses as periodically changed based on From dee2afe3fd488201dd7a7aeb2bbd94113246d2f4 Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Thu, 17 Sep 2020 14:07:18 -0400 Subject: [PATCH 5/6] explain cached pose clearing behavior Signed-off-by: Ashton Larkin --- src/systems/log/LogPlayback.cc | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index f9d298e3b9..23a30a9d2c 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -484,8 +484,16 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) msgs::Pose_V queuedPose; - // if new pose updates are received, make sure that only the cached poses - // from a previous Update cycle are cleared + // If new pose updates are received, make sure that only the cached poses + // from a previous Update cycle are cleared. + // + // Since Parse can be called multiple times in a single Update, + // it's important to make sure that new poses from a given Update aren't + // overwritten by poses received in a later Parse call from the same Update. + // Since Parse may not be called at all for a given Update (it depends on the + // timestamp being investigated from the log file), we will only clear cached + // poses from a previous Update if there are new poses to be saved in the current + // Update (we know that there are new poses to be saved if Parse is called). bool clearCachedPoseUpdates = true; auto iter = this->dataPtr->batch.begin(); From 6155f344b46905bec42a076b4b6ddf14e910dd04 Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Thu, 17 Sep 2020 14:15:54 -0400 Subject: [PATCH 6/6] fixed whitespacing for codecheck Signed-off-by: Ashton Larkin --- src/systems/log/LogPlayback.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index 23a30a9d2c..d4931f1d80 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -492,8 +492,9 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) // overwritten by poses received in a later Parse call from the same Update. // Since Parse may not be called at all for a given Update (it depends on the // timestamp being investigated from the log file), we will only clear cached - // poses from a previous Update if there are new poses to be saved in the current - // Update (we know that there are new poses to be saved if Parse is called). + // poses from a previous Update if there are new poses to be saved in the + // current Update (we know that there are new poses to be saved if Parse + // is called). bool clearCachedPoseUpdates = true; auto iter = this->dataPtr->batch.begin();