Get updated GUI ECM info when a user presses 'play'

src/SimulationRunner.cc

@@ -194,7 +194,8 @@ 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::OnWorldControl, this);
+  this->node->Advertise("control", &SimulationRunner::OnWorldStateControl,
+      this);
   this->node->Advertise("playback/control",
       &SimulationRunner::OnPlaybackControl, this);
 
@@ -1135,6 +1136,58 @@ 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)

src/SimulationRunner.hh

@@ -38,6 +38,7 @@
 #include <ignition/common/Event.hh>
 #include <ignition/common/WorkerPool.hh>
 #include <ignition/math/Stopwatch.hh>
+#include <ignition/msgs.hh>
 #include <ignition/transport/Node.hh>
 
 #include "ignition/gazebo/config.hh"
@@ -367,7 +368,17 @@ namespace ignition
       /// and multistep.
       /// \param[out] _res Response to client, true if successful.
       /// \return True for success
-      private: bool OnWorldControl(const msgs::WorldControl &_req,
+      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,
                                          msgs::Boolean &_res);
 
       /// \brief World control service callback. This function stores the

src/SimulationRunner_TEST.cc

@@ -20,6 +20,7 @@
 
 #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>
@@ -1568,6 +1569,182 @@ 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,