diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index f9d62da1f4..d4931f1d80 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,13 @@ 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); + /// \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. @@ -113,6 +117,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 +141,21 @@ LogPlayback::~LogPlayback() } ////////////////////////////////////////////////// -void LogPlaybackPrivate::Parse(EntityComponentManager &_ecm, - const msgs::Pose_V &_msg) +void LogPlaybackPrivate::Parse(const msgs::Pose_V &_msg, bool &_clear) { - // Maps entity to pose recorded - // Key: entity. Value: pose - std::map idToPose; + if (_clear) + this->recentEntityPoseUpdates.clear(); - for (int i = 0; i < _msg.pose_size(); ++i) + // save the new entity pose updates + 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; - }); + // 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; } ////////////////////////////////////////////////// @@ -508,6 +484,19 @@ 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. + // + // 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(); while (iter != this->dataPtr->batch.end()) { @@ -516,7 +505,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, clearCachedPoseUpdates); queuedPose.Clear(); } @@ -593,9 +582,27 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) if (queuedPose.pose_size() > 0) { - this->dataPtr->Parse(_ecm, queuedPose); + this->dataPtr->Parse(queuedPose, clearCachedPoseUpdates); } + // 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; }