Skip to content

Commit

Permalink
apply GUI ECM's diff to server ECM at end of pause interval
Browse files Browse the repository at this point in the history 
Signed-off-by: Ashton Larkin <ashton@openrobotics.org>
  • Loading branch information
@adlarkin
adlarkin committed Oct 26, 2021
1 parent 1dbcbd5 commit 5f5c74e
Show file tree
Hide file tree
Showing 4 changed files with 78 additions and 243 deletions.
103 changes: 48 additions & 55 deletions src/SimulationRunner.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,10 +18,12 @@
#include "SimulationRunner.hh"

#include <algorithm>
#include <mutex>

#include <ignition/common/Profiler.hh>
#include <ignition/msgs.hh>
#include <sdf/Root.hh>

#include "ignition/common/Profiler.hh"
#include "ignition/gazebo/components/Model.hh"
#include "ignition/gazebo/components/Name.hh"
#include "ignition/gazebo/components/Sensor.hh"
Expand Down Expand Up @@ -194,8 +196,7 @@ SimulationRunner::SimulationRunner(const sdf::World *_world,
this->node = std::make_unique<transport::Node>(opts);

// TODO(louise) Combine both messages into one.
this->node->Advertise("control", &SimulationRunner::OnWorldStateControl,
this);
this->node->Advertise("control", &SimulationRunner::OnWorldControl, this);
this->node->Advertise("playback/control",
&SimulationRunner::OnPlaybackControl, this);

Expand Down Expand Up @@ -805,6 +806,43 @@ bool SimulationRunner::Run(const uint64_t _iterations)
void SimulationRunner::Step(const UpdateInfo &_info)
{
IGN_PROFILE("SimulationRunner::Step");

{
std::lock_guard<std::mutex> lock(this->guiServerEcmMutex);
if (this->matchGuiEcm)
{
// Update the server's ECM to match any changes that took place in the
// GUI's ECM during the most recent pause interval. We do this before
// processing WorldControl msgs to ensure that GUI ECM changes aren't
// lost (the GUI ECM changes that occurred while paused may overwrite
// the server ECM changes that occurred while paused)
const std::string guiEcmService = "/changedGuiEcm";
msgs::SerializedState guiChanges;
bool result;
if (this->node->Request(guiEcmService, 500, guiChanges, result))
{
if (!result)
ignerr << "Error processing the [" << guiEcmService << "] service.\n";
else
{
igndbg << "Syncing server's ECM state with gui's ECM state.\n";
this->entityCompMgr.SetState(guiChanges);
}
}
else
{
ignerr << "Unable to receive any changes that occurred in the GUI "
<< "while paused.\n";
}
// TODO(anyone) notify server systems of changes made to the ECM,
// if there were any? Server systems are updated later in this method,
// so depending on how a server system is written/implemented, the system
// may automatically detect changes made to the ECM

this->matchGuiEcm = false;
}
}

this->currentInfo = _info;

// Publish info
Expand Down Expand Up @@ -1101,6 +1139,13 @@ bool SimulationRunner::OnWorldControl(const msgs::WorldControl &_req,
{
std::lock_guard<std::mutex> lock(this->msgBufferMutex);

{
std::lock_guard<std::mutex> ecmLock(this->guiServerEcmMutex);
// if we are going from pause to play, we need to update the server ECM
// with any changes that occurred to the GUI ECM while paused.
this->matchGuiEcm = this->Paused() && !_req.pause();
}

WorldControl control;
control.pause = _req.pause();

Expand Down Expand Up @@ -1136,58 +1181,6 @@ bool SimulationRunner::OnWorldControl(const msgs::WorldControl &_req,
return true;
}

/////////////////////////////////////////////////
bool SimulationRunner::OnWorldStateControl(const msgs::WorldControlState &_req,
msgs::Boolean &_res)
{
std::lock_guard<std::mutex> lock(this->msgBufferMutex);

WorldControl control;
control.pause = _req.worldcontrol().pause();

if (_req.worldcontrol().multi_step() != 0)
control.multiStep = _req.worldcontrol().multi_step();
else if (_req.worldcontrol().step())
control.multiStep = 1;

if (_req.worldcontrol().has_reset())
{
control.rewind = _req.worldcontrol().reset().all() ||
_req.worldcontrol().reset().time_only();

if (_req.worldcontrol().reset().model_only())
{
ignwarn << "Model only reset is not supported." << std::endl;
}
}

if (_req.worldcontrol().seed() != 0)
{
ignwarn << "Changing seed is not supported." << std::endl;
}

if (_req.worldcontrol().has_run_to_sim_time())
{
control.runToSimTime = std::chrono::seconds(
_req.worldcontrol().run_to_sim_time().sec()) +
std::chrono::nanoseconds(_req.worldcontrol().run_to_sim_time().nsec());
}

this->worldControls.push_back(control);

// update the server ECM if the GUI ECM was updated
const auto &allGuiEcmUpdates = _req.state();
for (int i = 0; i < allGuiEcmUpdates.state_size(); ++i)
{
this->entityCompMgr.SetState(allGuiEcmUpdates.state(i));
}
// TODO(anyone) notify server systems of changes made to the ECM, if there
// were any?

_res.set_data(true);
return true;
}

/////////////////////////////////////////////////
bool SimulationRunner::OnPlaybackControl(const msgs::LogPlaybackControl &_req,
msgs::Boolean &_res)
Expand Down
20 changes: 9 additions & 11 deletions src/SimulationRunner.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include <functional>
#include <list>
#include <memory>
#include <mutex>
#include <optional>
#include <string>
#include <unordered_map>
Expand Down Expand Up @@ -368,17 +369,7 @@ namespace ignition
/// and multistep.
/// \param[out] _res Response to client, true if successful.
/// \return True for success
private: bool IGN_DEPRECATED(6) OnWorldControl(
const msgs::WorldControl &_req, msgs::Boolean &_res);

/// \brief World control service callback. This function stores the
/// the request which will then be processed by the ProcessMessages
/// function.
/// \param[in] _req Request from client, currently handling play / pause
/// and multistep.
/// \param[out] _res Response to client, true if successful.
/// \return True for success
private: bool OnWorldStateControl(const msgs::WorldControlState &_req,
private: bool OnWorldControl(const msgs::WorldControl &_req,
msgs::Boolean &_res);

/// \brief World control service callback. This function stores the
Expand Down Expand Up @@ -612,6 +603,13 @@ namespace ignition
/// \brief True if Server::RunOnce triggered a blocking paused step
private: bool blockingPausedStepPending{false};

/// \brief Flag that indicates whether the server ECM needs to be updated
/// to match the GUI ECM
private: bool matchGuiEcm{false};

/// \brief Mutex used for reading/writing the gui ECM flag above
private: std::mutex guiServerEcmMutex;

friend class LevelManager;
};
}
Expand Down
177 changes: 0 additions & 177 deletions src/SimulationRunner_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@

#include <ignition/common/Console.hh>
#include <ignition/common/Util.hh>
#include <ignition/msgs.hh>
#include <ignition/transport/Node.hh>
#include <sdf/Box.hh>
#include <sdf/Capsule.hh>
Expand Down Expand Up @@ -1569,182 +1568,6 @@ TEST_P(SimulationRunnerTest, GenerateWorldSdf)
EXPECT_EQ(5u, world->ModelCount());
}

/////////////////////////////////////////////////
TEST_P(SimulationRunnerTest, UpdateECM)
{
// Load SDF file
sdf::Root root;
root.Load(common::joinPaths(PROJECT_SOURCE_PATH,
"test", "worlds", "shapes.sdf"));

ASSERT_EQ(1u, root.WorldCount());

// Create simulation runner
auto systemLoader = std::make_shared<SystemLoader>();
SimulationRunner runner(root.WorldByIndex(0), systemLoader);

// make sure that the simulation runner is advertising the service
// that updates its ECM
const std::string updateECMService = "/world/default/control";
transport::Node node;
std::vector<std::string> advertisedServices;
node.ServiceList(advertisedServices);
bool foundService = false;
for (const auto &service : advertisedServices)
{
if (service == updateECMService)
{
foundService = true;
break;
}
}
ASSERT_TRUE(foundService);

// create a mechanism for sharing changes made to otherECM, and ensure that
// simulation runner received the change information
std::function<void(msgs::SerializedState_V &)> shareECMChanges =
[&updateECMService, &node](msgs::SerializedState_V &_changes)
{
msgs::Boolean resp;
bool serviceResult = false;
msgs::WorldControlState req;

for (int i = 0; i < _changes.state_size(); ++i)
{
auto ecmState = req.mutable_state()->add_state();
ecmState->CopyFrom(_changes.state(i));
}
node.Request(updateECMService, req, 10000, resp, serviceResult);

EXPECT_TRUE(resp.data());
EXPECT_TRUE(serviceResult);
_changes.Clear();
};

// helper function for adding a state message to a msgs::SerializedState_V
std::function<void(msgs::SerializedState_V &,
const EntityComponentManager &)> saveEcmState =
[](msgs::SerializedState_V &_allStates, const EntityComponentManager &_ecm)
{
auto nextEcmChange = _allStates.add_state();
nextEcmChange->CopyFrom(_ecm.State());
};

// create another ECM that will be modified and then shared with
// the simulation runner's ECM. The simulation runner's ECM should
// have the contents of this ECM once it's shared
EntityComponentManager otherECM;
msgs::SerializedState_V ecmChangesMsg;

// add an entity with a components::IntComponent to otherECM
EXPECT_EQ(0u, otherECM.EntityCount());
auto entity = otherECM.CreateEntity();
otherECM.CreateComponent<components::IntComponent>(entity,
components::IntComponent(1));
EXPECT_EQ(1u, otherECM.EntityCount());
EXPECT_TRUE(otherECM.EntityHasComponentType(entity,
components::IntComponent::typeId));

// make sure simulation runner's ECM has no entities with a
// components::IntComponent of a data of 1
EXPECT_TRUE(runner.EntityCompMgr().EntitiesByComponents(
components::IntComponent(1)).empty());

// share this new entity with the simulation runner's ECM
saveEcmState(ecmChangesMsg, otherECM);
shareECMChanges(ecmChangesMsg);

// make sure the simulation runner's ECM has the changes made to otherECM
int foundEntities = 0;
runner.EntityCompMgr().EachNew<components::IntComponent>(
[&foundEntities](const Entity &, const components::IntComponent *_int)
{
foundEntities++;
EXPECT_EQ(1, _int->Data());
return true;
});
EXPECT_EQ(1, foundEntities);

// perform a simulation step, which should clear the new entity state of
// the simulation runner's ECM
UpdateInfo updateInfo;
EXPECT_TRUE(runner.EntityCompMgr().HasNewEntities());
runner.Step(updateInfo);
EXPECT_FALSE(runner.EntityCompMgr().HasNewEntities());

// perform multiple updates to otherECM before sharing it with the
// simulation runner's ECM:
// 1: modify an existing component
// 2: create a new component
otherECM.SetComponentData<components::IntComponent>(entity, 2);
saveEcmState(ecmChangesMsg, otherECM);
otherECM.CreateComponent(entity, components::DoubleComponent(1.0));
saveEcmState(ecmChangesMsg, otherECM);
// (make sure each change is registered as a separate state in order to test
// updating simulation runner's ECM with multiple state changes)
EXPECT_EQ(2, ecmChangesMsg.state_size());

// share the new updates with simulation runner and make sure that simulation
// runner's ECM reflects these updates
shareECMChanges(ecmChangesMsg);
EXPECT_FALSE(runner.EntityCompMgr().HasNewEntities());
foundEntities = 0;
runner.EntityCompMgr().Each<components::IntComponent,
components::DoubleComponent>(
[&foundEntities](const Entity &, const components::IntComponent *_int,
const components::DoubleComponent *_double)
{
foundEntities++;
EXPECT_EQ(2, _int->Data());
EXPECT_DOUBLE_EQ(1.0, _double->Data());
return true;
});
EXPECT_EQ(1, foundEntities);

// test removing a component to make sure that the simulation runner's ECM
// mirrors the component removal
EXPECT_TRUE(otherECM.RemoveComponent(entity,
components::DoubleComponent::typeId));
saveEcmState(ecmChangesMsg, otherECM);
shareECMChanges(ecmChangesMsg);
EXPECT_EQ(1u, runner.EntityCompMgr().EntitiesByComponents(
components::IntComponent(2)).size());
// (Each is used to verify no entities have a DoubleComponent b/c searching
// via EntitiesByComponents could be flaky due to floating point precision)
foundEntities = 0;
runner.EntityCompMgr().Each<components::DoubleComponent>(
[&](const Entity &, const components::DoubleComponent *)
{
foundEntities++;
return true;
});
EXPECT_EQ(0, foundEntities);

// test removing an entity
otherECM.RequestRemoveEntity(entity);
saveEcmState(ecmChangesMsg, otherECM);
shareECMChanges(ecmChangesMsg);
foundEntities = 0;
runner.EntityCompMgr().EachRemoved<components::IntComponent>(
[&](const Entity &, const components::IntComponent *)
{
foundEntities++;
return true;
});
EXPECT_EQ(1, foundEntities);
// (step the simulation runner to actually remove the entity)
updateInfo.realTime++;
runner.Step(updateInfo);
foundEntities = 0;
runner.EntityCompMgr().Each<components::IntComponent>(
[&](const Entity &, const components::IntComponent *)
{
foundEntities++;
return true;
});
EXPECT_EQ(0, foundEntities);
}

// Run multiple times. We want to make sure that static globals don't cause
// problems.
INSTANTIATE_TEST_SUITE_P(ServerRepeat, SimulationRunnerTest,
Expand Down
Loading

0 comments on commit 5f5c74e

Please sign in to comment.