Get updated GUI ECM info when a user presses 'play' (#1109)

* get updated GUI ECM info in world control CB Signed-off-by: Ashton Larkin <ashton@openrobotics.org> * working on adding tests Signed-off-by: Ashton Larkin <ashton@openrobotics.org> * remove TODO note, issue fixed by #1131 Signed-off-by: Ashton Larkin <ashton@openrobotics.org> * apply GUI ECM's diff to server ECM at end of pause interval Signed-off-by: Ashton Larkin <ashton@openrobotics.org> * use gui event to update server Signed-off-by: Ashton Larkin <ashton@openrobotics.org> * handle step and support original control service Signed-off-by: Ashton Larkin <ashton@openrobotics.org> * Reduced code duplication Signed-off-by: Nate Koenig <nate@openrobotics.org> * Set gazebo's default to use the event based system Signed-off-by: Nate Koenig <nate@openrobotics.org> * Added more <use_event> flags Signed-off-by: Nate Koenig <nate@openrobotics.org> * add header key to indicate a WorldControl step Signed-off-by: Ashton Larkin <ashton@openrobotics.org> * remove deprecation notes (support both event and service) Signed-off-by: Ashton Larkin <ashton@openrobotics.org> * Require version 6.1 Signed-off-by: Nate Koenig <nate@openrobotics.org> Co-authored-by: Nate Koenig <nate@openrobotics.org> Co-authored-by: Nate Koenig <nkoenig@users.noreply.github.com>
gazebosim · Nov 6, 2021 · 827fc3f · 827fc3f
1 parent 9e4b5e4
commit 827fc3f
Show file tree

Hide file tree

Showing 24 changed files with 152 additions and 14 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -87,7 +87,7 @@ set(IGN_FUEL_TOOLS_VER ${ignition-fuel_tools7_VERSION_MAJOR})
 
 #--------------------------------------
 # Find ignition-gui
-ign_find_package(ignition-gui6 REQUIRED)
+ign_find_package(ignition-gui6 REQUIRED VERSION 6.1)
 set(IGN_GUI_VER ${ignition-gui6_VERSION_MAJOR})
 ign_find_package (Qt5
   COMPONENTS

diff --git a/examples/plugin/rendering_plugins/rendering_plugins.sdf b/examples/plugin/rendering_plugins/rendering_plugins.sdf
@@ -92,6 +92,7 @@
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/examples/scripts/log_video_recorder/log_video_recorder.sdf b/examples/scripts/log_video_recorder/log_video_recorder.sdf
@@ -88,6 +88,7 @@
         <start_paused>true</start_paused>
         <service>/world/default/control</service>
         <stats_topic>/world/default/stats</stats_topic>
+        <use_event>true</use_event>
       </plugin>
       <plugin filename='WorldStats' name='World stats'>
         <ignition-gui>

diff --git a/examples/worlds/camera_sensor.sdf b/examples/worlds/camera_sensor.sdf
@@ -105,6 +105,7 @@
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/examples/worlds/fuel_textured_mesh.sdf b/examples/worlds/fuel_textured_mesh.sdf
@@ -108,6 +108,7 @@
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/examples/worlds/grid.sdf b/examples/worlds/grid.sdf
@@ -80,6 +80,7 @@
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/examples/worlds/minimal_scene.sdf b/examples/worlds/minimal_scene.sdf
@@ -147,6 +147,7 @@ Features:
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf
@@ -110,6 +110,7 @@
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/examples/worlds/plot_3d.sdf b/examples/worlds/plot_3d.sdf
@@ -84,6 +84,7 @@
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/examples/worlds/segmentation_camera.sdf b/examples/worlds/segmentation_camera.sdf
@@ -98,6 +98,7 @@
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/examples/worlds/sensors_demo.sdf b/examples/worlds/sensors_demo.sdf
@@ -100,6 +100,7 @@
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/examples/worlds/sky.sdf b/examples/worlds/sky.sdf
@@ -83,6 +83,7 @@ Currently only supported using ogre2 rendering engine plugin.
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/examples/worlds/thermal_camera.sdf b/examples/worlds/thermal_camera.sdf
@@ -101,6 +101,7 @@
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/examples/worlds/tunnel.sdf b/examples/worlds/tunnel.sdf
@@ -129,6 +129,7 @@
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/examples/worlds/video_record_dbl_pendulum.sdf b/examples/worlds/video_record_dbl_pendulum.sdf
@@ -180,6 +180,7 @@
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/examples/worlds/visibility.sdf b/examples/worlds/visibility.sdf
@@ -112,6 +112,7 @@
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/examples/worlds/visualize_contacts.sdf b/examples/worlds/visualize_contacts.sdf
@@ -102,6 +102,7 @@ Contacts will be visualized as blue spheres and green cylinders.
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/examples/worlds/visualize_lidar.sdf b/examples/worlds/visualize_lidar.sdf
@@ -96,6 +96,7 @@
         <play_pause>true</play_pause>
         <step>true</step>
         <start_paused>true</start_paused>
+        <use_event>true</use_event>
 
       </plugin>
 

diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc
@@ -195,12 +195,14 @@ SimulationRunner::SimulationRunner(const sdf::World *_world,
 
   // TODO(louise) Combine both messages into one.
   this->node->Advertise("control", &SimulationRunner::OnWorldControl, this);
+  this->node->Advertise("control/state", &SimulationRunner::OnWorldControlState,
+      this);
   this->node->Advertise("playback/control",
       &SimulationRunner::OnPlaybackControl, this);
 
   ignmsg << "Serving world controls on [" << opts.NameSpace()
-         << "/control] and [" << opts.NameSpace() << "/playback/control]"
-         << std::endl;
+         << "/control], [" << opts.NameSpace() << "/control/state] and ["
+         << opts.NameSpace() << "/playback/control]" << std::endl;
 
   // Publish empty GUI messages for worlds that have no GUI in the beginning.
   // In the future, support modifying GUI from the server at runtime.
@@ -410,6 +412,12 @@ void SimulationRunner::PublishStats()
 
   msg.set_paused(this->currentInfo.paused);
 
+  if (this->Stepping())
+  {
+    auto headerData = msg.mutable_header()->add_data();
+    headerData->set_key("step");
+  }
+
   // Publish the stats message. The stats message is throttled.
   this->statsPub.Publish(msg);
 
@@ -1079,6 +1087,18 @@ void SimulationRunner::SetPaused(const bool _paused)
   this->currentInfo.paused = _paused;
 }
 
+/////////////////////////////////////////////////
+void SimulationRunner::SetStepping(bool _stepping)
+{
+  this->stepping = _stepping;
+}
+
+/////////////////////////////////////////////////
+bool SimulationRunner::Stepping() const
+{
+  return this->stepping;
+}
+
 /////////////////////////////////////////////////
 void SimulationRunner::SetRunToSimTime(
     const std::chrono::steady_clock::duration &_time)
@@ -1097,36 +1117,54 @@ void SimulationRunner::SetRunToSimTime(
 /////////////////////////////////////////////////
 bool SimulationRunner::OnWorldControl(const msgs::WorldControl &_req,
     msgs::Boolean &_res)
+{
+  msgs::WorldControlState req;
+  req.mutable_world_control()->CopyFrom(_req);
+
+  return this->OnWorldControlState(req, _res);
+}
+
+/////////////////////////////////////////////////
+bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req,
+    msgs::Boolean &_res)
 {
   std::lock_guard<std::mutex> lock(this->msgBufferMutex);
 
+  // update the server ECM if the request contains SerializedState information
+  if (_req.has_state())
+    this->entityCompMgr.SetState(_req.state());
+  // TODO(anyone) notify server systems of changes made to the ECM, if there
+  // were any?
+
   WorldControl control;
-  control.pause = _req.pause();
+  control.pause = _req.world_control().pause();
 
-  if (_req.multi_step() != 0)
-    control.multiStep = _req.multi_step();
-  else if (_req.step())
+  if (_req.world_control().multi_step() != 0)
+    control.multiStep = _req.world_control().multi_step();
+  else if (_req.world_control().step())
     control.multiStep = 1;
 
-  if (_req.has_reset())
+  if (_req.world_control().has_reset())
   {
-    control.rewind = _req.reset().all() || _req.reset().time_only();
+    control.rewind = _req.world_control().reset().all() ||
+      _req.world_control().reset().time_only();
 
-    if (_req.reset().model_only())
+    if (_req.world_control().reset().model_only())
     {
       ignwarn << "Model only reset is not supported." << std::endl;
     }
   }
 
-  if (_req.seed() != 0)
+  if (_req.world_control().seed() != 0)
   {
     ignwarn << "Changing seed is not supported." << std::endl;
   }
 
-  if (_req.has_run_to_sim_time())
+  if (_req.world_control().has_run_to_sim_time())
   {
-    control.runToSimTime = std::chrono::seconds(_req.run_to_sim_time().sec()) +
-                   std::chrono::nanoseconds(_req.run_to_sim_time().nsec());
+    control.runToSimTime = std::chrono::seconds(
+        _req.world_control().run_to_sim_time().sec()) +
+        std::chrono::nanoseconds(_req.world_control().run_to_sim_time().nsec());
   }
 
   this->worldControls.push_back(control);
@@ -1175,6 +1213,10 @@ void SimulationRunner::ProcessMessages()
 void SimulationRunner::ProcessWorldControl()
 {
   IGN_PROFILE("SimulationRunner::ProcessWorldControl");
+
+  // assume no stepping unless WorldControl msgs say otherwise
+  this->SetStepping(false);
+
   for (const auto &control : this->worldControls)
   {
     // Play / pause
@@ -1186,6 +1228,7 @@ void SimulationRunner::ProcessWorldControl()
       this->pendingSimIterations += control.multiStep;
       // Unpause so that stepping can occur.
       this->SetPaused(false);
+      this->SetStepping(true);
     }
 
     // Rewind / reset

diff --git a/src/SimulationRunner.hh b/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"
@@ -291,6 +292,17 @@ namespace ignition
       /// \return True if the simulation runner is paused, false otherwise.
       public: bool Paused() const;
 
+      /// \brief Set if the simulation runner is stepping based on WorldControl
+      /// info
+      /// \param[in] _step True if stepping based on WorldControl info, false
+      /// otherwise
+      public: void SetStepping(bool _step);
+
+      /// \brief Get if the simulation runner is stepping based on WorldControl
+      /// info
+      /// \return True if stepping based on WorldControl info, false otherwise
+      public: bool Stepping() const;
+
       /// \brief Set the run to simulation time.
       /// \param[in] _time A simulation time in the future to run to and then
       /// pause. A negative number or a time less than the current simulation
@@ -370,6 +382,16 @@ namespace ignition
       private: bool OnWorldControl(const msgs::WorldControl &_req,
                                          msgs::Boolean &_res);
 
+      /// \brief World control state 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. This also may contain SerializedState information.
+      /// \param[out] _res Response to client, true if successful.
+      /// \return True for success
+      private: bool OnWorldControlState(const msgs::WorldControlState &_req,
+                                         msgs::Boolean &_res);
+
       /// \brief World control service callback. This function stores the
       /// the request which will then be processed by the ProcessMessages
       /// function.
@@ -601,6 +623,10 @@ namespace ignition
       /// \brief True if Server::RunOnce triggered a blocking paused step
       private: bool blockingPausedStepPending{false};
 
+      /// \brief Whether the simulation runner is currently stepping based on
+      /// WorldControl info (true) or not (false)
+      private: bool stepping{false};
+
       friend class LevelManager;
     };
     }