Skip to content

Commit

Permalink
Merge branch 'ign-gazebo3' into viz_collisions
Browse files Browse the repository at this point in the history
  • Loading branch information
Nate Koenig committed Jan 28, 2021
2 parents e49f875 + 347fd51 commit 9f653db
Show file tree
Hide file tree
Showing 5 changed files with 68 additions and 4 deletions.
6 changes: 6 additions & 0 deletions include/ignition/gazebo/EntityComponentManager.hh
Original file line number Diff line number Diff line change
Expand Up @@ -487,6 +487,12 @@ namespace ignition
/// \return True if there are any components with one-time changes.
public: bool HasOneTimeComponentChanges() 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.
public: std::unordered_set<ComponentTypeId>
ComponentTypesWithPeriodicChanges() const;

/// \brief Set the absolute state of the ECM from a serialized message.
/// Entities / components that are in the new state but not in the old
/// one will be created.
Expand Down
30 changes: 28 additions & 2 deletions src/EntityComponentManager.cc
Original file line number Diff line number Diff line change
Expand Up @@ -419,6 +419,18 @@ bool EntityComponentManager::HasOneTimeComponentChanges() const
return !this->dataPtr->oneTimeChangedComponents.empty();
}

/////////////////////////////////////////////////
std::unordered_set<ComponentTypeId>
EntityComponentManager::ComponentTypesWithPeriodicChanges() const
{
std::unordered_set<ComponentTypeId> periodicComponents;
for (const auto& compPair : this->dataPtr->periodicChangedComponents)
{
periodicComponents.insert(compPair.first);
}
return periodicComponents;
}

/////////////////////////////////////////////////
bool EntityComponentManager::HasEntity(const Entity _entity) const
{
Expand Down Expand Up @@ -1224,8 +1236,22 @@ void EntityComponentManager::SetState(
{
std::istringstream istr(compMsg.component());
comp->Deserialize(istr);
this->SetChanged(entity, compIter.first,
ComponentState::OneTimeChange);
// Note on merging forward:
// `has_one_time_component_changes` field is available in Edifice so
// this workaround can be removed
auto flag = ComponentState::PeriodicChange;
for (int i = 0; i < _stateMsg.header().data_size(); ++i)
{
if (_stateMsg.header().data(i).key() ==
"has_one_time_component_changes")
{
int v = stoi(_stateMsg.header().data(i).value(0));
if (v)
flag = ComponentState::OneTimeChange;
break;
}
}
this->SetChanged(entity, compIter.first, flag);
}
}
}
Expand Down
25 changes: 25 additions & 0 deletions src/EntityComponentManager_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -2082,6 +2082,7 @@ TEST_P(EntityComponentManagerFixture, SetChanged)
auto c2 = manager.CreateComponent<IntComponent>(e2, IntComponent(456));

EXPECT_TRUE(manager.HasOneTimeComponentChanges());
EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size());
EXPECT_EQ(ComponentState::OneTimeChange,
manager.ComponentState(e1, c1.first));
EXPECT_EQ(ComponentState::OneTimeChange,
Expand All @@ -2093,16 +2094,39 @@ TEST_P(EntityComponentManagerFixture, SetChanged)
// updated
manager.RunSetAllComponentsUnchanged();
EXPECT_FALSE(manager.HasOneTimeComponentChanges());
EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size());
EXPECT_EQ(ComponentState::NoChange,
manager.ComponentState(e1, c1.first));
EXPECT_EQ(ComponentState::NoChange,
manager.ComponentState(e2, c2.first));

// Mark as changed
manager.SetChanged(e1, c1.first, ComponentState::PeriodicChange);

// check that only e1 c1 is serialized into a message
msgs::SerializedStateMap stateMsg;
manager.State(stateMsg);
{
ASSERT_EQ(1, stateMsg.entities_size());

auto iter = stateMsg.entities().find(e1);
const auto &e1Msg = iter->second;
EXPECT_EQ(e1, e1Msg.id());
ASSERT_EQ(1, e1Msg.components_size());

auto compIter = e1Msg.components().begin();
const auto &e1c1Msg = compIter->second;
EXPECT_EQ(IntComponent::typeId, e1c1Msg.type());
EXPECT_EQ(123, std::stoi(e1c1Msg.component()));
}

manager.SetChanged(e2, c2.first, ComponentState::OneTimeChange);

EXPECT_TRUE(manager.HasOneTimeComponentChanges());
// Expect a single component type to be marked as PeriodicChange
ASSERT_EQ(1u, manager.ComponentTypesWithPeriodicChanges().size());
EXPECT_EQ(IntComponent().TypeId(),
*manager.ComponentTypesWithPeriodicChanges().begin());
EXPECT_EQ(ComponentState::PeriodicChange,
manager.ComponentState(e1, c1.first));
EXPECT_EQ(ComponentState::OneTimeChange,
Expand All @@ -2112,6 +2136,7 @@ TEST_P(EntityComponentManagerFixture, SetChanged)
EXPECT_TRUE(manager.RemoveComponent(e1, c1.first));

EXPECT_TRUE(manager.HasOneTimeComponentChanges());
EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size());
EXPECT_EQ(ComponentState::NoChange,
manager.ComponentState(e1, c1.first));

Expand Down
6 changes: 6 additions & 0 deletions src/network/NetworkManagerSecondary.cc
Original file line number Diff line number Diff line change
Expand Up @@ -167,6 +167,12 @@ void NetworkManagerSecondary::OnStep(
msgs::SerializedStateMap stateMsg;
if (!entities.empty())
this->dataPtr->ecm->State(stateMsg, entities);
// Note on merging forward:
// `has_one_time_component_changes` field is available in Edifice so this
// workaround can be removed
auto data = stateMsg.mutable_header()->add_data();
data->set_key("has_one_time_component_changes");
data->add_value(this->dataPtr->ecm->HasOneTimeComponentChanges() ? "1" : "0");

this->stepAckPub.Publish(stateMsg);

Expand Down
5 changes: 3 additions & 2 deletions src/systems/scene_broadcaster/SceneBroadcaster.cc
Original file line number Diff line number Diff line change
Expand Up @@ -293,12 +293,13 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
{
_manager.State(*this->dataPtr->stepMsg.mutable_state(), {}, {}, true);
}
// Otherwise publish just selected components
// Otherwise publish just periodic change components
else
{
IGN_PROFILE("SceneBroadcast::PostUpdate UpdateState");
auto periodicComponents = _manager.ComponentTypesWithPeriodicChanges();
_manager.State(*this->dataPtr->stepMsg.mutable_state(),
{}, {components::Pose::typeId});
{}, periodicComponents);
}

// Full state on demand
Expand Down

0 comments on commit 9f653db

Please sign in to comment.