Prototype: specify order of execution for System::Update callbacks

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,68 @@
+/*
+ * 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 <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::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)
+{
+  // TODO(scpeters) get element name from System.hh once it has been defined
+  // there
+  const std::string priorityElementName {"gz:system_priority"};
+  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::Update(const gz::sim::UpdateInfo &_info,
+    gz::sim::EntityComponentManager &/*_ecm*/)
+{
+  gzmsg << "Iteration " << _info.iterations
+        << ", system priority " << this->systemPriority
+        << ", system label " << this->systemLabel
+        << '\n';
+}

examples/plugin/priority_printer_plugin/PriorityPrinter.hh

@@ -0,0 +1,46 @@
+/*
+ * 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 <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::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 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,75 @@
+# Priority Printer
+
+This example illustrates how to control the order of execution of System
+Update callbacks.
+
+## 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 world
+with various priority values and unique labels in the
+`priority_printer_plugin.sdf` file that's going to be loaded.
+
+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:
+
+    gz sim -v 3 priority_printer_plugin.sdf -s -r --iterations 5
+
+You should see green messages on the terminal like:
+
+```
+[Msg] Iteration 1, system priority -100, system label fifth
+[Msg] Iteration 1, system priority -10, system label third
+[Msg] Iteration 1, system priority unset, system label fourth
+[Msg] Iteration 1, system priority 0, system label sixth
+[Msg] Iteration 1, system priority 10, system label second
+[Msg] Iteration 1, system priority 100, system label first
+[Msg] Iteration 1, system priority 100, system label seventh
+[Msg] Iteration 2, system priority -100, system label fifth
+[Msg] Iteration 2, system priority -10, system label third
+[Msg] Iteration 2, system priority unset, system label fourth
+[Msg] Iteration 2, system priority 0, system label sixth
+[Msg] Iteration 2, system priority 10, system label second
+[Msg] Iteration 2, system priority 100, system label first
+[Msg] Iteration 2, system priority 100, system label seventh
+[Msg] Iteration 3, system priority -100, system label fifth
+[Msg] Iteration 3, system priority -10, system label third
+[Msg] Iteration 3, system priority unset, system label fourth
+[Msg] Iteration 3, system priority 0, system label sixth
+[Msg] Iteration 3, system priority 10, system label second
+[Msg] Iteration 3, system priority 100, system label first
+[Msg] Iteration 3, system priority 100, system label seventh
+[Msg] Iteration 4, system priority -100, system label fifth
+[Msg] Iteration 4, system priority -10, system label third
+[Msg] Iteration 4, system priority unset, system label fourth
+[Msg] Iteration 4, system priority 0, system label sixth
+[Msg] Iteration 4, system priority 10, system label second
+[Msg] Iteration 4, system priority 100, system label first
+[Msg] Iteration 4, system priority 100, system label seventh
+[Msg] Iteration 5, system priority -100, system label fifth
+[Msg] Iteration 5, system priority -10, system label third
+[Msg] Iteration 5, system priority unset, system label fourth
+[Msg] Iteration 5, system priority 0, system label sixth
+[Msg] Iteration 5, system priority 10, system label second
+[Msg] Iteration 5, system priority 100, system label first
+[Msg] 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>

src/SimulationRunner.cc

@@ -603,8 +603,13 @@ void SimulationRunner::UpdateSystems()
 
   {
     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);
+      }
+    }
   }
 
   {

src/SystemManager.cc

@@ -106,6 +106,13 @@
   {
     this->systems.push_back(system);
 
+    PriorityType p {defaultPriority};
+    if (system.configureSdf &&
+        system.configureSdf->HasElement(priorityElementName))
+    {
+      p = system.configureSdf->Get<PriorityType>(priorityElementName);
+    }
+
     if (system.configure)
       this->systemsConfigure.push_back(system.configure);
 
@@ -119,7 +126,10 @@
       this->systemsPreupdate.push_back(system.preupdate);
 
     if (system.update)
-      this->systemsUpdate.push_back(system.update);
+    {
+      this->systemsUpdate.try_emplace(p);
+      this->systemsUpdate[p].push_back(system.update);
+    }
 
     if (system.postupdate)
       this->systemsPostupdate.push_back(system.postupdate);
@@ -301,7 +311,8 @@
 }
 
 //////////////////////////////////////////////////
-const std::vector<ISystemUpdate *>& SystemManager::SystemsUpdate()
+const SystemManager::PrioritizedSystems<ISystemUpdate *>&
+SystemManager::SystemsUpdate()
 {
   return this->systemsUpdate;
 }

src/SystemManager.hh

@@ -19,6 +19,8 @@
 
 #include <gz/msgs/entity_plugin_v.pb.h>
 
+#include <cstdint>
+#include <map>
 #include <memory>
 #include <string>
 #include <vector>
@@ -44,6 +46,15 @@ namespace gz
     /// \brief Used to load / unload sysetms as well as iterate over them.
     class GZ_SIM_VISIBLE SystemManager
     {
+      // TODO(scpeters) define these three variables in public System.hh
+      // and document their effect
+      using PriorityType = int32_t;
+      const PriorityType defaultPriority {0};
+      const std::string priorityElementName {"gz:system_priority"};
+      template<typename S>
+      class PrioritizedSystems : public std::map<PriorityType, std::vector<S>>
+      {};
+
       /// \brief Constructor
       /// \param[in] _systemLoader A pointer to a SystemLoader to load plugins
       ///  from files
@@ -132,7 +143,7 @@ namespace gz
 
       /// \brief Get an vector of all active systems implementing "Update"
       /// \return Vector of systems's update interfaces.
-      public: const std::vector<ISystemUpdate *>& SystemsUpdate();
+      public: const PrioritizedSystems<ISystemUpdate *>& SystemsUpdate();
 
       /// \brief Get an vector of all active systems implementing "PostUpdate"
       /// \return Vector of systems's post-update interfaces.
@@ -200,7 +211,7 @@ namespace gz
       private: std::vector<ISystemPreUpdate *> systemsPreupdate;
 
       /// \brief Systems implementing Update
-      private: std::vector<ISystemUpdate *> systemsUpdate;
+      private: PrioritizedSystems<ISystemUpdate *> systemsUpdate;
 
       /// \brief Systems implementing PostUpdate
       private: std::vector<ISystemPostUpdate *> systemsPostupdate;