Skip to content

Commit

Permalink
fix scene broadcaster in playback with reduced playback set state rate
Browse files Browse the repository at this point in the history
Signed-off-by: Ian Chen <[email protected]>
  • Loading branch information
iche033 committed Aug 12, 2022
1 parent 43ce137 commit f4728cc
Show file tree
Hide file tree
Showing 5 changed files with 59 additions and 5 deletions.
5 changes: 5 additions & 0 deletions include/ignition/gazebo/EntityComponentManager.hh
Original file line number Diff line number Diff line change
Expand Up @@ -590,6 +590,11 @@ namespace ignition
/// \return True if there are any components with one-time changes.
public: bool HasOneTimeComponentChanges() const;

/// \brief Get whether there are periodic component changes. These changes
/// may happen frequently and are processed periodically.
/// \return True if there are any components with periodic changes.
public: bool HasPeriodicComponentChanges() const;

/// \brief Get the components types that are marked as periodic changes.
/// \return All the components that at least one entity marked as
/// periodic changes.
Expand Down
6 changes: 6 additions & 0 deletions src/EntityComponentManager.cc
Original file line number Diff line number Diff line change
Expand Up @@ -945,6 +945,12 @@ bool EntityComponentManager::HasOneTimeComponentChanges() const
return !this->dataPtr->oneTimeChangedComponents.empty();
}

/////////////////////////////////////////////////
bool EntityComponentManager::HasPeriodicComponentChanges() const
{
return !this->dataPtr->periodicChangedComponents.empty();
}

/////////////////////////////////////////////////
std::unordered_set<ComponentTypeId>
EntityComponentManager::ComponentTypesWithPeriodicChanges() const
Expand Down
5 changes: 5 additions & 0 deletions src/EntityComponentManager_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -2220,6 +2220,7 @@ TEST_P(EntityComponentManagerFixture,
ASSERT_NE(nullptr, c2);

EXPECT_TRUE(manager.HasOneTimeComponentChanges());
EXPECT_FALSE(manager.HasPeriodicComponentChanges());
EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size());
EXPECT_EQ(ComponentState::OneTimeChange,
manager.ComponentState(e1, c1->TypeId()));
Expand All @@ -2232,6 +2233,7 @@ TEST_P(EntityComponentManagerFixture,
// updated
manager.RunSetAllComponentsUnchanged();
EXPECT_FALSE(manager.HasOneTimeComponentChanges());
EXPECT_FALSE(manager.HasPeriodicComponentChanges());
EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size());
EXPECT_EQ(ComponentState::NoChange,
manager.ComponentState(e1, c1->TypeId()));
Expand All @@ -2246,6 +2248,7 @@ TEST_P(EntityComponentManagerFixture,
EXPECT_EQ(ComponentState::NoChange,
manager.ComponentState(e1, c1->TypeId()));
EXPECT_FALSE(manager.HasOneTimeComponentChanges());
EXPECT_FALSE(manager.HasPeriodicComponentChanges());
EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size());
EXPECT_EQ(0, manager.ChangedState().entities_size());

Expand Down Expand Up @@ -2273,6 +2276,7 @@ TEST_P(EntityComponentManagerFixture,

EXPECT_TRUE(manager.HasOneTimeComponentChanges());
// Expect a single component type to be marked as PeriodicChange
EXPECT_TRUE(manager.HasPeriodicComponentChanges());
ASSERT_EQ(1u, manager.ComponentTypesWithPeriodicChanges().size());
EXPECT_EQ(IntComponent().TypeId(),
*manager.ComponentTypesWithPeriodicChanges().begin());
Expand All @@ -2285,6 +2289,7 @@ TEST_P(EntityComponentManagerFixture,
EXPECT_TRUE(manager.RemoveComponent(e1, c1->TypeId()));

EXPECT_TRUE(manager.HasOneTimeComponentChanges());
EXPECT_FALSE(manager.HasPeriodicComponentChanges());
EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size());
EXPECT_EQ(ComponentState::NoChange,
manager.ComponentState(e1, c1->TypeId()));
Expand Down
2 changes: 1 addition & 1 deletion src/systems/log/LogRecord.cc
Original file line number Diff line number Diff line change
Expand Up @@ -709,7 +709,7 @@ void LogRecord::PostUpdate(const UpdateInfo &_info,
(_info.simTime - this->dataPtr->lastRecordSimTime) >=
this->dataPtr->recordPeriod)
{
this->dataPtr->lastRecordSimTime = _info.simTime;
this->dataPtr->lastRecordSimTime = _info.simTime;
}
else
{
Expand Down
46 changes: 42 additions & 4 deletions src/systems/scene_broadcaster/SceneBroadcaster.cc
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@
#include "ignition/gazebo/components/Light.hh"
#include "ignition/gazebo/components/Link.hh"
#include "ignition/gazebo/components/LogicalCamera.hh"
#include "ignition/gazebo/components/LogPlaybackStatistics.hh"
#include "ignition/gazebo/components/Material.hh"
#include "ignition/gazebo/components/Model.hh"
#include "ignition/gazebo/components/Name.hh"
Expand Down Expand Up @@ -252,6 +253,10 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate
/// \brief Store SDF scene information so that it can be inserted into
/// scene message.
public: sdf::Scene sdfScene;

/// \brief Flag used to indicate if periodic changes need to be published
/// This is currently only used in playback mode.
public: bool pubPeriodicChanges{false};
};

//////////////////////////////////////////////////
Expand Down Expand Up @@ -351,8 +356,11 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
auto now = std::chrono::system_clock::now();
bool itsPubTime = (now - this->dataPtr->lastStatePubTime >
this->dataPtr->statePublishPeriod[_info.paused]);
// check if we need to publish periodic changes in playback mode.
bool pubChanges = this->dataPtr->pubPeriodicChanges &&
_manager.HasPeriodicComponentChanges();
auto shouldPublish = this->dataPtr->statePub.HasConnections() &&
(changeEvent || itsPubTime);
(changeEvent || itsPubTime || pubChanges);

if (this->dataPtr->stateServiceRequest || shouldPublish)
{
Expand All @@ -375,9 +383,39 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
else if (!_info.paused)
{
IGN_PROFILE("SceneBroadcast::PostUpdate UpdateState");
auto periodicComponents = _manager.ComponentTypesWithPeriodicChanges();
_manager.State(*this->dataPtr->stepMsg.mutable_state(),
{}, periodicComponents);

if (_manager.HasPeriodicComponentChanges())
{
auto periodicComponents = _manager.ComponentTypesWithPeriodicChanges();
_manager.State(*this->dataPtr->stepMsg.mutable_state(),
{}, periodicComponents);
this->dataPtr->pubPeriodicChanges = false;
}
else
{
// log files may be recorded at lower rate than sim time step. So in
// playback mode, the scene broadcaster may not see any periodic
// changed states here since it no longer happens every iteration.
// As the result, no state changes are published to be GUI, causing
// visuals in the GUI scene to miss updates. The visuals are only
// updated if by some timing coincidence that log playback updates
// the ECM at the same iteration as when the scene broadcaster is going
// to publish perioidc changes here.
// To work around the issue, we force the scene broadcaster
// to publish states at an offcycle iteration the next time it sees
// periodic changes.
auto playbackComp =
_manager.Component<components::LogPlaybackStatistics>(
this->dataPtr->worldEntity);
if (playbackComp)
{
this->dataPtr->pubPeriodicChanges = true;
}
// this creates an empty state in the msg even there are no periodic
// changed components - done to preseve existing behavior.
// we may be able to remove this in the future and update tests
this->dataPtr->stepMsg.mutable_state();
}
}

// Full state on demand
Expand Down

0 comments on commit f4728cc

Please sign in to comment.