diff --git a/examples/plugin/priority_printer_plugin/CMakeLists.txt b/examples/plugin/priority_printer_plugin/CMakeLists.txt new file mode 100644 index 0000000000..8af962296d --- /dev/null +++ b/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}) diff --git a/examples/plugin/priority_printer_plugin/PriorityPrinter.cc b/examples/plugin/priority_printer_plugin/PriorityPrinter.cc new file mode 100644 index 0000000000..2835c38126 --- /dev/null +++ b/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 +#include + +// This header is required to register plugins. It's good practice to place it +// in the cc file, like it's done here. +#include + +// 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 &_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(priorityElementName); + } + + const std::string labelElementName {"label"}; + if (_sdf && _sdf->HasElement(labelElementName)) + { + this->systemLabel = _sdf->Get(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'; +} diff --git a/examples/plugin/priority_printer_plugin/PriorityPrinter.hh b/examples/plugin/priority_printer_plugin/PriorityPrinter.hh new file mode 100644 index 0000000000..027c84a927 --- /dev/null +++ b/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 +#include + +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 &_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 diff --git a/examples/plugin/priority_printer_plugin/README.md b/examples/plugin/priority_printer_plugin/README.md new file mode 100644 index 0000000000..40e544342d --- /dev/null +++ b/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 +``` diff --git a/examples/plugin/priority_printer_plugin/priority_printer_plugin.sdf b/examples/plugin/priority_printer_plugin/priority_printer_plugin.sdf new file mode 100644 index 0000000000..e60d75a497 --- /dev/null +++ b/examples/plugin/priority_printer_plugin/priority_printer_plugin.sdf @@ -0,0 +1,36 @@ + + + + + + 100 + + + + 10 + + + + -10 + + + + + + + -100 + + + + 0 + + + + 100 + + + + diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index a5ba51e96c..ecce1af542 100644 --- a/src/SimulationRunner.cc +++ b/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); + } + } } { diff --git a/src/SystemManager.cc b/src/SystemManager.cc index fd43f5330d..3ced3b522d 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -106,6 +106,13 @@ size_t SystemManager::ActivatePendingSystems() { this->systems.push_back(system); + PriorityType p {defaultPriority}; + if (system.configureSdf && + system.configureSdf->HasElement(priorityElementName)) + { + p = system.configureSdf->Get(priorityElementName); + } + if (system.configure) this->systemsConfigure.push_back(system.configure); @@ -119,7 +126,10 @@ size_t SystemManager::ActivatePendingSystems() 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& SystemManager::SystemsPreUpdate() } ////////////////////////////////////////////////// -const std::vector& SystemManager::SystemsUpdate() +const SystemManager::PrioritizedSystems& +SystemManager::SystemsUpdate() { return this->systemsUpdate; } diff --git a/src/SystemManager.hh b/src/SystemManager.hh index acd82c09dc..9cf9dda6a1 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -19,6 +19,8 @@ #include +#include +#include #include #include #include @@ -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 + class PrioritizedSystems : public std::map> + {}; + /// \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& SystemsUpdate(); + public: const PrioritizedSystems& 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 systemsPreupdate; /// \brief Systems implementing Update - private: std::vector systemsUpdate; + private: PrioritizedSystems systemsUpdate; /// \brief Systems implementing PostUpdate private: std::vector systemsPostupdate;