Specify System::PreUpdate, Update execution order (#2487)

Specifying an integer value in a <gz:system_priority> tag
for a system will control the order in which System PreUpdate
and Update callbacks are executed, with lower values executing
first.

The PriorityPrinter example plugin is added to illustrate
the feature.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

examples/plugin/priority_printer_plugin/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
+
+find_package(gz-cmake3 REQUIRED)
+
+project(Priority_printer)
+
+gz_find_package(gz-plugin2 REQUIRED COMPONENTS register)
+set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
+
+gz_find_package(gz-sim8 REQUIRED)
+set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
+
+add_library(PriorityPrinter SHARED PriorityPrinter.cc)
+set_property(TARGET PriorityPrinter PROPERTY CXX_STANDARD 17)
+target_link_libraries(PriorityPrinter
+  PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
+  PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER})

examples/plugin/priority_printer_plugin/PriorityPrinter.cc

@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) 2024 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+// We'll use a string and the gzmsg command below for a brief example.
+// Remove these includes if your plugin doesn't need them.
+#include <memory>
+#include <string>
+#include <gz/common/Console.hh>
+
+// This header is required to register plugins. It's good practice to place it
+// in the cc file, like it's done here.
+#include <gz/plugin/Register.hh>
+
+// Don't forget to include the plugin's header.
+#include "PriorityPrinter.hh"
+
+// This is required to register the plugin. Make sure the interfaces match
+// what's in the header.
+GZ_ADD_PLUGIN(
+    priority_printer::PriorityPrinter,
+    gz::sim::System,
+    priority_printer::PriorityPrinter::ISystemConfigure,
+    priority_printer::PriorityPrinter::ISystemPreUpdate,
+    priority_printer::PriorityPrinter::ISystemUpdate)
+
+using namespace priority_printer;
+
+void PriorityPrinter::Configure(
+        const gz::sim::Entity &_entity,
+        const std::shared_ptr<const sdf::Element> &_sdf,
+        gz::sim::EntityComponentManager &_ecm,
+        gz::sim::EventManager &_eventMgr)
+{
+  // Parse priority value as a string for printing
+  const std::string priorityElementName {gz::sim::System::kPriorityElementName};
+  if (_sdf && _sdf->HasElement(priorityElementName))
+  {
+    this->systemPriority = _sdf->Get<std::string>(priorityElementName);
+  }
+
+  const std::string labelElementName {"label"};
+  if (_sdf && _sdf->HasElement(labelElementName))
+  {
+    this->systemLabel = _sdf->Get<std::string>(labelElementName);
+  }
+}
+
+void PriorityPrinter::PreUpdate(const gz::sim::UpdateInfo &_info,
+    gz::sim::EntityComponentManager &/*_ecm*/)
+{
+  gzmsg << "PreUpdate: "
+        << "Iteration " << _info.iterations
+        << ", system priority " << this->systemPriority
+        << ", system label " << this->systemLabel
+        << '\n';
+}
+
+void PriorityPrinter::Update(const gz::sim::UpdateInfo &_info,
+    gz::sim::EntityComponentManager &/*_ecm*/)
+{
+  gzmsg << "Update: "
+        << "Iteration " << _info.iterations
+        << ", system priority " << this->systemPriority
+        << ", system label " << this->systemLabel
+        << '\n';
+}

examples/plugin/priority_printer_plugin/PriorityPrinter.hh

@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) 2024 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#ifndef EXAMPLE_PLUGIN_PRIORITYPRINTER_HH_
+#define EXAMPLE_PLUGIN_PRIORITYPRINTER_HH_
+
+#include <memory>
+#include <string>
+#include <gz/sim/System.hh>
+
+namespace priority_printer
+{
+  // This plugin prints the number of elapsed simulation iterations,
+  // this system's priority value from the XML configuration,
+  // and a custom label from the XML configuration during the Update callback.
+  class PriorityPrinter:
+    public gz::sim::System,
+    public gz::sim::ISystemConfigure,
+    public gz::sim::ISystemPreUpdate,
+    public gz::sim::ISystemUpdate
+  {
+    public: void Configure(const gz::sim::Entity &_entity,
+                 const std::shared_ptr<const sdf::Element> &_sdf,
+                 gz::sim::EntityComponentManager &_ecm,
+                 gz::sim::EventManager &_eventMgr) override;
+
+    public: void PreUpdate(const gz::sim::UpdateInfo &_info,
+                           gz::sim::EntityComponentManager &_ecm) override;
+
+    public: void Update(const gz::sim::UpdateInfo &_info,
+                        gz::sim::EntityComponentManager &_ecm) override;
+
+    public: std::string systemPriority{"unset"};
+    public: std::string systemLabel{"unset"};
+  };
+}
+#endif

examples/plugin/priority_printer_plugin/README.md

@@ -0,0 +1,128 @@
+# Priority Printer
+
+This example illustrates how to control the order of execution of System
+PreUpdate and Update callbacks. As documented in
+[gz/sim/System.hh](https://github.com/gazebosim/gz-sim/tree/main/include/gz/sim/System.hh),
+the PreUpdate and Update phases are executed sequentially in the same
+thread, and the order of execution of these phases can be
+controlled by specifying a signed integer priority value for the System
+in its XML configuration. The default priority value is zero, and
+smaller values are executed earlier. Systems with the same priority
+value are executed in the order in which they are loaded.
+
+## Build
+
+From the root of the `gz-sim` repository, do the following to build the example:
+
+~~~
+cd gz-sim/examples/plugins/priority_printer
+mkdir build
+cd build
+cmake ..
+make
+~~~
+
+This will generate the `PriorityPrinter` library under `build`.
+
+## Run
+
+Multiple instances of the `PriorityPrinter` plugin are added to the
+[priority\_printer\_plugin.sdf](priority_printer_plugin.sdf) world file
+with various priority values and unique labels corresponding to the order
+in which the plugins are specified ("first" for the first plugin and so on).
+Without priority values, the systems would be executed in the order they are
+specified in XML ("first", then "second", etc.).
+With the priority values specified, the systems with smallest integer priority
+values are executed first. For systems with the same priority value, the
+system that is specified earlier in the XML file will be executed first.
+
+Before starting Gazebo, we must make sure it can find the plugin by doing:
+
+~~~
+cd gz-sim/examples/plugins/priority_printer
+export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`/build
+~~~
+
+Then load the example world and run for 5 iterations:
+
+    gz sim -v 3 priority_printer_plugin.sdf -s -r --iterations 5
+
+You should see green messages on the terminal like those given below.
+Note that the system with priority `-100` was executed first, despite being
+the fifth system in the XML ordering. There are two instances of systems with
+the same priority value: the fourth and sixth systems with priority 0 (with
+"unset" defaulting to 0) and the first and seventh systems with priority 100.
+In each case, the system declared earlier in XML executed first.
+
+```
+[Msg] PreUpdate: Iteration 1, system priority -100, system label fifth
+[Msg] PreUpdate: Iteration 1, system priority -10, system label third
+[Msg] PreUpdate: Iteration 1, system priority unset, system label fourth
+[Msg] PreUpdate: Iteration 1, system priority 0, system label sixth
+[Msg] PreUpdate: Iteration 1, system priority 10, system label second
+[Msg] PreUpdate: Iteration 1, system priority 100, system label first
+[Msg] PreUpdate: Iteration 1, system priority 100, system label seventh
+[Msg] Update: Iteration 1, system priority -100, system label fifth
+[Msg] Update: Iteration 1, system priority -10, system label third
+[Msg] Update: Iteration 1, system priority unset, system label fourth
+[Msg] Update: Iteration 1, system priority 0, system label sixth
+[Msg] Update: Iteration 1, system priority 10, system label second
+[Msg] Update: Iteration 1, system priority 100, system label first
+[Msg] Update: Iteration 1, system priority 100, system label seventh
+[Msg] PreUpdate: Iteration 2, system priority -100, system label fifth
+[Msg] PreUpdate: Iteration 2, system priority -10, system label third
+[Msg] PreUpdate: Iteration 2, system priority unset, system label fourth
+[Msg] PreUpdate: Iteration 2, system priority 0, system label sixth
+[Msg] PreUpdate: Iteration 2, system priority 10, system label second
+[Msg] PreUpdate: Iteration 2, system priority 100, system label first
+[Msg] PreUpdate: Iteration 2, system priority 100, system label seventh
+[Msg] Update: Iteration 2, system priority -100, system label fifth
+[Msg] Update: Iteration 2, system priority -10, system label third
+[Msg] Update: Iteration 2, system priority unset, system label fourth
+[Msg] Update: Iteration 2, system priority 0, system label sixth
+[Msg] Update: Iteration 2, system priority 10, system label second
+[Msg] Update: Iteration 2, system priority 100, system label first
+[Msg] Update: Iteration 2, system priority 100, system label seventh
+[Msg] PreUpdate: Iteration 3, system priority -100, system label fifth
+[Msg] PreUpdate: Iteration 3, system priority -10, system label third
+[Msg] PreUpdate: Iteration 3, system priority unset, system label fourth
+[Msg] PreUpdate: Iteration 3, system priority 0, system label sixth
+[Msg] PreUpdate: Iteration 3, system priority 10, system label second
+[Msg] PreUpdate: Iteration 3, system priority 100, system label first
+[Msg] PreUpdate: Iteration 3, system priority 100, system label seventh
+[Msg] Update: Iteration 3, system priority -100, system label fifth
+[Msg] Update: Iteration 3, system priority -10, system label third
+[Msg] Update: Iteration 3, system priority unset, system label fourth
+[Msg] Update: Iteration 3, system priority 0, system label sixth
+[Msg] Update: Iteration 3, system priority 10, system label second
+[Msg] Update: Iteration 3, system priority 100, system label first
+[Msg] Update: Iteration 3, system priority 100, system label seventh
+[Msg] PreUpdate: Iteration 4, system priority -100, system label fifth
+[Msg] PreUpdate: Iteration 4, system priority -10, system label third
+[Msg] PreUpdate: Iteration 4, system priority unset, system label fourth
+[Msg] PreUpdate: Iteration 4, system priority 0, system label sixth
+[Msg] PreUpdate: Iteration 4, system priority 10, system label second
+[Msg] PreUpdate: Iteration 4, system priority 100, system label first
+[Msg] PreUpdate: Iteration 4, system priority 100, system label seventh
+[Msg] Update: Iteration 4, system priority -100, system label fifth
+[Msg] Update: Iteration 4, system priority -10, system label third
+[Msg] Update: Iteration 4, system priority unset, system label fourth
+[Msg] Update: Iteration 4, system priority 0, system label sixth
+[Msg] Update: Iteration 4, system priority 10, system label second
+[Msg] Update: Iteration 4, system priority 100, system label first
+[Msg] Update: Iteration 4, system priority 100, system label seventh
+[Msg] PreUpdate: Iteration 5, system priority -100, system label fifth
+[Msg] PreUpdate: Iteration 5, system priority -10, system label third
+[Msg] PreUpdate: Iteration 5, system priority unset, system label fourth
+[Msg] PreUpdate: Iteration 5, system priority 0, system label sixth
+[Msg] PreUpdate: Iteration 5, system priority 10, system label second
+[Msg] PreUpdate: Iteration 5, system priority 100, system label first
+[Msg] PreUpdate: Iteration 5, system priority 100, system label seventh
+[Msg] Update: Iteration 5, system priority -100, system label fifth
+[Msg] Update: Iteration 5, system priority -10, system label third
+[Msg] Update: Iteration 5, system priority unset, system label fourth
+[Msg] Update: Iteration 5, system priority 0, system label sixth
+[Msg] Update: Iteration 5, system priority 10, system label second
+[Msg] Update: Iteration 5, system priority 100, system label first
+[Msg] Update: Iteration 5, system priority 100, system label seventh
+```

examples/plugin/priority_printer_plugin/priority_printer_plugin.sdf

@@ -0,0 +1,36 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="default">
+    <!--
+      System plugins can be loaded from <plugin> tags attached to entities like
+      the world, models, visuals, etc.
+    -->
+    <plugin filename="PriorityPrinter" name="priority_printer::PriorityPrinter">
+      <gz:system_priority>100</gz:system_priority>
+      <label>first</label>
+    </plugin>
+    <plugin filename="PriorityPrinter" name="priority_printer::PriorityPrinter">
+      <gz:system_priority>10</gz:system_priority>
+      <label>second</label>
+    </plugin>
+    <plugin filename="PriorityPrinter" name="priority_printer::PriorityPrinter">
+      <gz:system_priority>-10</gz:system_priority>
+      <label>third</label>
+    </plugin>
+    <plugin filename="PriorityPrinter" name="priority_printer::PriorityPrinter">
+      <label>fourth</label>
+    </plugin>
+    <plugin filename="PriorityPrinter" name="priority_printer::PriorityPrinter">
+      <gz:system_priority>-100</gz:system_priority>
+      <label>fifth</label>
+    </plugin>
+    <plugin filename="PriorityPrinter" name="priority_printer::PriorityPrinter">
+      <gz:system_priority>0</gz:system_priority>
+      <label>sixth</label>
+    </plugin>
+    <plugin filename="PriorityPrinter" name="priority_printer::PriorityPrinter">
+      <gz:system_priority>100</gz:system_priority>
+      <label>seventh</label>
+    </plugin>
+  </world>
+</sdf>

include/gz/sim/System.hh

@@ -17,6 +17,7 @@
 #ifndef GZ_SIM_SYSTEM_HH_
 #define GZ_SIM_SYSTEM_HH_
 
+#include <cstdint>
 #include <memory>
 
 #include <gz/sim/config.hh>
@@ -64,6 +65,14 @@ namespace gz
     ///    * Used to read out results at the end of a simulation step to be used
     ///      for sensor or controller updates.
     ///
+    /// The PreUpdate and Update phases are executed sequentially in the same
+    /// thread, while the PostUpdate phase is executed in parallel in multiple
+    /// threads. The order of execution of PreUpdate and Update phases can be
+    /// controlled by specifying a signed integer Priority value for the System
+    /// in its XML configuration. The default Priority value is zero, and
+    /// smaller values are executed earlier. Systems with the same Priority
+    /// value are executed in the order in which they are loaded.
+    ///
     /// It's important to note that UpdateInfo::simTime does not refer to the
     /// current time, but the time reached after the PreUpdate and Update calls
     /// have finished. So, if any of the *Update functions are called with
@@ -74,6 +83,19 @@ namespace gz
     /// simulation is started un-paused.
     class System
     {
+      /// \brief Signed integer type used for specifying priority of the
+      /// execution order of PreUpdate and Update phases.
+      public: using PriorityType = int32_t;
+
+      /// \brief Default priority value for execution order of the PreUpdate
+      /// and Update phases.
+      public: constexpr static PriorityType kDefaultPriority = {0};
+
+      /// \brief Name of the XML element from which the priority value will be
+      /// parsed.
+      public: constexpr static std::string_view kPriorityElementName =
+          {"gz:system_priority"};
+
       /// \brief Constructor
       public: System() = default;
 

src/SimulationRunner.cc

@@ -601,14 +601,24 @@ void SimulationRunner::UpdateSystems()
 
   {
     GZ_PROFILE("PreUpdate");
-    for (auto& system : this->systemMgr->SystemsPreUpdate())
-      system->PreUpdate(this->currentInfo, this->entityCompMgr);
+    for (auto& [priority, systems] : this->systemMgr->SystemsPreUpdate())
+    {
+      for (auto& system : systems)
+      {
+        system->PreUpdate(this->currentInfo, this->entityCompMgr);
+      }
+    }
   }
 
   {
     GZ_PROFILE("Update");
-    for (auto& system : this->systemMgr->SystemsUpdate())
-      system->Update(this->currentInfo, this->entityCompMgr);
+    for (auto& [priority, systems] : this->systemMgr->SystemsUpdate())
+    {
+      for (auto& system : systems)
+      {
+        system->Update(this->currentInfo, this->entityCompMgr);
+      }
+    }
   }
 
   {