From 8ee0dfdf13b38e1f501b767ba29276acee2f8fa3 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 2 May 2024 10:49:34 -0700 Subject: [PATCH 01/19] Specify System::PreUpdate, Update execution order Specifying an integer value in a 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 --- .../priority_printer_plugin/CMakeLists.txt | 17 +++ .../PriorityPrinter.cc | 79 +++++++++++++ .../PriorityPrinter.hh | 50 ++++++++ .../plugin/priority_printer_plugin/README.md | 110 ++++++++++++++++++ .../priority_printer_plugin.sdf | 36 ++++++ include/gz/sim/System.hh | 21 ++++ src/SimulationRunner.cc | 18 ++- src/SystemManager.cc | 60 +++++++--- src/SystemManager.hh | 36 ++++-- 9 files changed, 393 insertions(+), 34 deletions(-) create mode 100644 examples/plugin/priority_printer_plugin/CMakeLists.txt create mode 100644 examples/plugin/priority_printer_plugin/PriorityPrinter.cc create mode 100644 examples/plugin/priority_printer_plugin/PriorityPrinter.hh create mode 100644 examples/plugin/priority_printer_plugin/README.md create mode 100644 examples/plugin/priority_printer_plugin/priority_printer_plugin.sdf diff --git a/examples/plugin/priority_printer_plugin/CMakeLists.txt b/examples/plugin/priority_printer_plugin/CMakeLists.txt new file mode 100644 index 0000000000..e5497abb73 --- /dev/null +++ b/examples/plugin/priority_printer_plugin/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.22.1 FATAL_ERROR) + +find_package(gz-cmake4 REQUIRED) + +project(Priority_printer) + +gz_find_package(gz-plugin3 REQUIRED COMPONENTS register) +set(GZ_PLUGIN_VER ${gz-plugin3_VERSION_MAJOR}) + +gz_find_package(gz-sim9 REQUIRED) +set(GZ_SIM_VER ${gz-sim9_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..2893507d1e --- /dev/null +++ b/examples/plugin/priority_printer_plugin/PriorityPrinter.cc @@ -0,0 +1,79 @@ +/* + * 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::ISystemPreUpdate, + 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) +{ + // 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(priorityElementName); + } + + const std::string labelElementName {"label"}; + if (_sdf && _sdf->HasElement(labelElementName)) + { + this->systemLabel = _sdf->Get(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'; +} diff --git a/examples/plugin/priority_printer_plugin/PriorityPrinter.hh b/examples/plugin/priority_printer_plugin/PriorityPrinter.hh new file mode 100644 index 0000000000..e9a3176518 --- /dev/null +++ b/examples/plugin/priority_printer_plugin/PriorityPrinter.hh @@ -0,0 +1,50 @@ +/* + * 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::ISystemPreUpdate, + 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 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 diff --git a/examples/plugin/priority_printer_plugin/README.md b/examples/plugin/priority_printer_plugin/README.md new file mode 100644 index 0000000000..2c884dc266 --- /dev/null +++ b/examples/plugin/priority_printer_plugin/README.md @@ -0,0 +1,110 @@ +# 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] 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 +``` 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/include/gz/sim/System.hh b/include/gz/sim/System.hh index cc0139161e..48893c71d8 100644 --- a/include/gz/sim/System.hh +++ b/include/gz/sim/System.hh @@ -64,6 +64,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 +82,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; diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 87f72d1ce2..69be9a6a37 100644 --- a/src/SimulationRunner.cc +++ b/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); + } + } } { diff --git a/src/SystemManager.cc b/src/SystemManager.cc index d1b66346e3..b2b4ef9f08 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -25,6 +25,7 @@ #include "SystemInternal.hh" #include "gz/sim/components/SystemPluginInfo.hh" #include "gz/sim/Conversions.hh" +#include "gz/sim/System.hh" #include "SystemManager.hh" using namespace gz; @@ -109,6 +110,15 @@ size_t SystemManager::ActivatePendingSystems() { this->systems.push_back(system); + PriorityType p {System::kDefaultPriority}; + const std::string kPriorityElementName + {gz::sim::System::kPriorityElementName}; + if (system.configureSdf && + system.configureSdf->HasElement(kPriorityElementName)) + { + p = system.configureSdf->Get(kPriorityElementName); + } + if (system.configure) this->systemsConfigure.push_back(system.configure); @@ -119,10 +129,16 @@ size_t SystemManager::ActivatePendingSystems() this->systemsReset.push_back(system.reset); if (system.preupdate) - this->systemsPreupdate.push_back(system.preupdate); + { + this->systemsPreupdate.try_emplace(p); + this->systemsPreupdate[p].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) { @@ -300,13 +316,15 @@ const std::vector &SystemManager::SystemsReset() } ////////////////////////////////////////////////// -const std::vector& SystemManager::SystemsPreUpdate() +const SystemManager::PrioritizedSystems& +SystemManager::SystemsPreUpdate() { return this->systemsPreupdate; } ////////////////////////////////////////////////// -const std::vector& SystemManager::SystemsUpdate() +const SystemManager::PrioritizedSystems& +SystemManager::SystemsUpdate() { return this->systemsUpdate; } @@ -495,20 +513,26 @@ void SystemManager::ProcessRemovedEntities( } return false; }); - RemoveFromVectorIf(this->systemsPreupdate, - [&](const auto& system) { - if (preupdateSystemsToBeRemoved.count(system)) { - return true; - } - return false; - }); - RemoveFromVectorIf(this->systemsUpdate, - [&](const auto& system) { - if (updateSystemsToBeRemoved.count(system)) { - return true; - } - return false; - }); + for (auto& [priority, systems] : this->systemsPreupdate) + { + RemoveFromVectorIf(systems, + [&](const auto& system) { + if (preupdateSystemsToBeRemoved.count(system)) { + return true; + } + return false; + }); + } + for (auto& [priority, systems] : this->systemsUpdate) + { + RemoveFromVectorIf(systems, + [&](const auto& system) { + if (updateSystemsToBeRemoved.count(system)) { + return true; + } + return false; + }); + } RemoveFromVectorIf(this->systemsPostupdate, [&](const auto& system) { diff --git a/src/SystemManager.hh b/src/SystemManager.hh index d523c4e740..c9c61c7944 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -19,6 +19,8 @@ #include +#include +#include #include #include #include @@ -29,6 +31,7 @@ #include "gz/sim/config.hh" #include "gz/sim/EntityComponentManager.hh" #include "gz/sim/Export.hh" +#include "gz/sim/System.hh" #include "gz/sim/SystemLoader.hh" #include "gz/sim/Types.hh" @@ -44,6 +47,13 @@ namespace gz /// \brief Used to load / unload sysetms as well as iterate over them. class GZ_SIM_VISIBLE SystemManager { + /// \brief Ordered map of priority values to a vector of System + /// interfaces. + using PriorityType = System::PriorityType; + template + class PrioritizedSystems : public std::map> + {}; + /// \brief Constructor /// \param[in] _systemLoader A pointer to a SystemLoader to load plugins /// from files @@ -116,29 +126,31 @@ namespace gz /// \return Vector of systems' configure interfaces. public: const std::vector& SystemsConfigure(); - /// \brief Get an vector of all active systems implementing + /// \brief Get a vector of all active systems implementing /// "ConfigureParameters" /// \return Vector of systems's configure interfaces. public: const std::vector& SystemsConfigureParameters(); - /// \brief Get an vector of all active systems implementing "Reset" + /// \brief Get a vector of all active systems implementing "Reset" /// \return Vector of systems' reset interfaces. public: const std::vector& SystemsReset(); - /// \brief Get an vector of all active systems implementing "PreUpdate" - /// \return Vector of systems's pre-update interfaces. - public: const std::vector& SystemsPreUpdate(); + /// \brief Get an ordered map of systems by priority that implement + /// "PreUpdate" + /// \return Priortized map of systems's pre-update interfaces. + public: const PrioritizedSystems& SystemsPreUpdate(); - /// \brief Get an vector of all active systems implementing "Update" - /// \return Vector of systems's update interfaces. - public: const std::vector& SystemsUpdate(); + /// \brief Get an ordered map of systems by priority that implement + /// "Update" + /// \return Priortized map of systems's update interfaces. + public: const PrioritizedSystems& SystemsUpdate(); - /// \brief Get an vector of all active systems implementing "PostUpdate" + /// \brief Get a vector of all active systems implementing "PostUpdate" /// \return Vector of systems's post-update interfaces. public: const std::vector& SystemsPostUpdate(); - /// \brief Get an vector of all systems attached to a given entity. + /// \brief Get a vector of all systems attached to a given entity. /// \return Vector of systems. public: std::vector TotalByEntity(Entity _entity); @@ -205,10 +217,10 @@ namespace gz private: std::vector systemsReset; /// \brief Systems implementing PreUpdate - private: std::vector systemsPreupdate; + private: PrioritizedSystems systemsPreupdate; /// \brief Systems implementing Update - private: std::vector systemsUpdate; + private: PrioritizedSystems systemsUpdate; /// \brief Systems implementing PostUpdate private: std::vector systemsPostupdate; From ad2b07d8283148112458eb77e326d75cc9de67c6 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 23 Jul 2024 23:41:31 -0700 Subject: [PATCH 02/19] ForceTorque system: use Update, not PostUpdate Set gz:system_priority to positive value for force_torque system in example and test worlds. Signed-off-by: Steve Peters --- examples/worlds/sensors.sdf | 1 + src/systems/force_torque/ForceTorque.cc | 10 +++++----- src/systems/force_torque/ForceTorque.hh | 6 +++--- test/worlds/force_torque.sdf | 4 +++- 4 files changed, 12 insertions(+), 9 deletions(-) diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index 9717bb11b3..8ca7b63789 100644 --- a/examples/worlds/sensors.sdf +++ b/examples/worlds/sensors.sdf @@ -38,6 +38,7 @@ + 10 newSensors; @@ -157,10 +157,10 @@ void ForceTorque::PreUpdate(const UpdateInfo &/*_info*/, } ////////////////////////////////////////////////// -void ForceTorque::PostUpdate(const UpdateInfo &_info, - const EntityComponentManager &_ecm) +void ForceTorque::Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) { - GZ_PROFILE("ForceTorque::PostUpdate"); + GZ_PROFILE("ForceTorque::Update"); // \TODO(anyone) Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) @@ -445,7 +445,7 @@ void ForceTorquePrivate::RemoveForceTorqueEntities( GZ_ADD_PLUGIN(ForceTorque, System, ForceTorque::ISystemPreUpdate, - ForceTorque::ISystemPostUpdate + ForceTorque::ISystemUpdate ) GZ_ADD_PLUGIN_ALIAS(ForceTorque, "gz::sim::systems::ForceTorque") diff --git a/src/systems/force_torque/ForceTorque.hh b/src/systems/force_torque/ForceTorque.hh index 09b5b7ed70..2b13d01cbb 100644 --- a/src/systems/force_torque/ForceTorque.hh +++ b/src/systems/force_torque/ForceTorque.hh @@ -42,7 +42,7 @@ namespace systems class ForceTorque: public System, public ISystemPreUpdate, - public ISystemPostUpdate + public ISystemUpdate { /// \brief Constructor public: ForceTorque(); @@ -55,8 +55,8 @@ namespace systems EntityComponentManager &_ecm) final; /// Documentation inherited - public: void PostUpdate(const UpdateInfo &_info, - const EntityComponentManager &_ecm) final; + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; /// \brief Private data pointer. private: std::unique_ptr dataPtr; diff --git a/test/worlds/force_torque.sdf b/test/worlds/force_torque.sdf index 66c7dd9e87..9a06c97c80 100644 --- a/test/worlds/force_torque.sdf +++ b/test/worlds/force_torque.sdf @@ -5,7 +5,9 @@ 0 - + + 10 + true From d69603f5ac423705a72484c5a4f82ca696012944 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 23 Jul 2024 23:42:37 -0700 Subject: [PATCH 03/19] Fix comment in physics system Signed-off-by: Steve Peters --- src/systems/physics/Physics.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index ed2b24acf2..d06b49e219 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -432,7 +432,7 @@ class gz::sim::systems::PhysicsPrivate } return true; }}; - /// \brief msgs::Contacts equality comparison function. + /// \brief msgs::Wrench equality comparison function. public: std::function wrenchEql{ [](const msgs::Wrench &_a, const msgs::Wrench &_b) From dfb392ba3654ee9dfaa33a44d4061f57210b47ea Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 23 Jul 2024 23:49:53 -0700 Subject: [PATCH 04/19] Add WrenchMeasured component Signed-off-by: Steve Peters --- include/gz/sim/components/WrenchMeasured.hh | 57 +++++++++++++++++++++ 1 file changed, 57 insertions(+) create mode 100644 include/gz/sim/components/WrenchMeasured.hh diff --git a/include/gz/sim/components/WrenchMeasured.hh b/include/gz/sim/components/WrenchMeasured.hh new file mode 100644 index 0000000000..235ed529b2 --- /dev/null +++ b/include/gz/sim/components/WrenchMeasured.hh @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2021 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 GZ_SIM_COMPONENTS_WRENCHMEASURED_HH_ +#define GZ_SIM_COMPONENTS_WRENCHMEASURED_HH_ + +#include + +#include +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { + +namespace components +{ +/// \brief Wrench measured by a ForceTorqueSensor in SI units (Nm for torque, +/// N for force). +/// The wrench is expressed in the Sensor frame and the force component is +/// applied at the sensor origin. +/// \note The term Wrench is used here to mean a pair of 3D vectors representing +/// torque and force quantities expressed in a given frame and where the force +/// is applied at the origin of the frame. This is different from the Wrench +/// used in screw theory. +/// \note The value of force_offset in msgs::Wrench is ignored for this +/// component. The force is assumed to be applied at the origin of the sensor +/// frame. +using WrenchMeasured = + Component; +GZ_SIM_REGISTER_COMPONENT("gz_sim_components.WrenchMeasured", + WrenchMeasured) +} // namespace components +} +} +} + +#endif From ea1c91280ce95104ffa95c13b119d16124d55b93 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 23 Jul 2024 23:50:08 -0700 Subject: [PATCH 05/19] Write WrenchMeasured to ECM in ForceTorque::Update Signed-off-by: Steve Peters --- src/systems/force_torque/ForceTorque.cc | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/systems/force_torque/ForceTorque.cc b/src/systems/force_torque/ForceTorque.cc index e46a099ee9..75f43fba13 100644 --- a/src/systems/force_torque/ForceTorque.cc +++ b/src/systems/force_torque/ForceTorque.cc @@ -44,6 +44,7 @@ #include "gz/sim/components/Pose.hh" #include "gz/sim/components/Sensor.hh" #include "gz/sim/components/World.hh" +#include "gz/sim/components/WrenchMeasured.hh" #include "gz/sim/EntityComponentManager.hh" #include "gz/sim/Util.hh" @@ -152,6 +153,9 @@ void ForceTorque::PreUpdate(const UpdateInfo &/*_info*/, _ecm.Component(entity)->Data(); gzdbg << "Adding JointTransmittedWrench to: " << jointEntity << std::endl; _ecm.CreateComponent(jointEntity, components::JointTransmittedWrench()); + // Enable WrenchMeasured to save sensor measurements + gzdbg << "Adding WrenchMeasured to: " << entity << std::endl; + _ecm.CreateComponent(entity, components::WrenchMeasured()); } this->dataPtr->newSensors.clear(); } @@ -203,6 +207,9 @@ void ForceTorque::Update(const UpdateInfo &_info, // * Apply noise // * Publish to gz-transport topic sensor->Update(_info.simTime, false); + const auto &measuredWrench = sensor->MeasuredWrench(); + _ecm.SetComponentData( + sensorEntity, measuredWrench); } } From d1b1836a5094069d30465b2e73a4a937e22337b5 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 25 Jul 2024 11:22:34 -0700 Subject: [PATCH 06/19] fix variable shadowing Signed-off-by: Steve Peters --- src/SystemManager.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index b2b4ef9f08..b2cd38ae14 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -513,9 +513,9 @@ void SystemManager::ProcessRemovedEntities( } return false; }); - for (auto& [priority, systems] : this->systemsPreupdate) + for (auto& [priority, systemsVector] : this->systemsPreupdate) { - RemoveFromVectorIf(systems, + RemoveFromVectorIf(systemsVector, [&](const auto& system) { if (preupdateSystemsToBeRemoved.count(system)) { return true; @@ -523,9 +523,9 @@ void SystemManager::ProcessRemovedEntities( return false; }); } - for (auto& [priority, systems] : this->systemsUpdate) + for (auto& [priority, systemsVector] : this->systemsUpdate) { - RemoveFromVectorIf(systems, + RemoveFromVectorIf(systemsVector, [&](const auto& system) { if (updateSystemsToBeRemoved.count(system)) { return true; From 21f80aed20531852c90d79696e6e7ca7ad16cbad Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 26 Jul 2024 00:56:26 -0700 Subject: [PATCH 07/19] Add missing includes Signed-off-by: Steve Peters --- examples/plugin/priority_printer_plugin/PriorityPrinter.cc | 1 + examples/plugin/priority_printer_plugin/PriorityPrinter.hh | 1 + include/gz/sim/System.hh | 1 + src/SystemManager.cc | 1 + 4 files changed, 4 insertions(+) diff --git a/examples/plugin/priority_printer_plugin/PriorityPrinter.cc b/examples/plugin/priority_printer_plugin/PriorityPrinter.cc index 2893507d1e..164596d968 100644 --- a/examples/plugin/priority_printer_plugin/PriorityPrinter.cc +++ b/examples/plugin/priority_printer_plugin/PriorityPrinter.cc @@ -17,6 +17,7 @@ // 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 #include diff --git a/examples/plugin/priority_printer_plugin/PriorityPrinter.hh b/examples/plugin/priority_printer_plugin/PriorityPrinter.hh index e9a3176518..741c2f596c 100644 --- a/examples/plugin/priority_printer_plugin/PriorityPrinter.hh +++ b/examples/plugin/priority_printer_plugin/PriorityPrinter.hh @@ -18,6 +18,7 @@ #ifndef EXAMPLE_PLUGIN_PRIORITYPRINTER_HH_ #define EXAMPLE_PLUGIN_PRIORITYPRINTER_HH_ +#include #include #include diff --git a/include/gz/sim/System.hh b/include/gz/sim/System.hh index 48893c71d8..f531c1d023 100644 --- a/include/gz/sim/System.hh +++ b/include/gz/sim/System.hh @@ -17,6 +17,7 @@ #ifndef GZ_SIM_SYSTEM_HH_ #define GZ_SIM_SYSTEM_HH_ +#include #include #include diff --git a/src/SystemManager.cc b/src/SystemManager.cc index b2cd38ae14..82a35b9faa 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -18,6 +18,7 @@ #include #include #include +#include #include #include From 175d934f3c73f00ba614847371a76dffc4487b29 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 26 Jul 2024 02:01:56 -0700 Subject: [PATCH 08/19] Remove priority keys with empty vectors Signed-off-by: Steve Peters --- src/SystemManager.cc | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 82a35b9faa..135eb40d6c 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -514,25 +514,35 @@ void SystemManager::ProcessRemovedEntities( } return false; }); - for (auto& [priority, systemsVector] : this->systemsPreupdate) + for (auto it = this->systemsPreupdate.begin(); + it != this->systemsPreupdate.end();) { - RemoveFromVectorIf(systemsVector, + RemoveFromVectorIf(it->second, [&](const auto& system) { if (preupdateSystemsToBeRemoved.count(system)) { return true; } return false; }); + if (it->second.empty()) + it = this->systemsPreupdate.erase(it); + else + ++it; } - for (auto& [priority, systemsVector] : this->systemsUpdate) + for (auto it = this->systemsUpdate.begin(); + it != this->systemsUpdate.end();) { - RemoveFromVectorIf(systemsVector, + RemoveFromVectorIf(it->second, [&](const auto& system) { if (updateSystemsToBeRemoved.count(system)) { return true; } return false; }); + if (it->second.empty()) + it = this->systemsUpdate.erase(it); + else + ++it; } RemoveFromVectorIf(this->systemsPostupdate, From 9a68307a5d1bf15a3081481a46bce4cebe78f402 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 26 Jul 2024 02:06:09 -0700 Subject: [PATCH 09/19] fix copyright year Signed-off-by: Steve Peters --- include/gz/sim/components/WrenchMeasured.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/gz/sim/components/WrenchMeasured.hh b/include/gz/sim/components/WrenchMeasured.hh index 235ed529b2..74c350514b 100644 --- a/include/gz/sim/components/WrenchMeasured.hh +++ b/include/gz/sim/components/WrenchMeasured.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. From a0e2ee4dc5af241014c6746047c7587710685532 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 27 Jul 2024 00:15:50 -0700 Subject: [PATCH 10/19] Add expectations to SystemManager_TEST Signed-off-by: Steve Peters --- src/SystemManager_TEST.cc | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc index 1265cc5a5f..38ad82d726 100644 --- a/src/SystemManager_TEST.cc +++ b/src/SystemManager_TEST.cc @@ -143,8 +143,14 @@ TEST(SystemManager, AddSystemNoEcm) EXPECT_EQ(0u, systemMgr.PendingCount()); EXPECT_EQ(2u, systemMgr.TotalCount()); EXPECT_EQ(1u, systemMgr.SystemsConfigure().size()); + // Expect PreUpdate and Update to contain one map entry with Priority 0 and + // a vector of length 1. EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().count(0)); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().at(0).size()); EXPECT_EQ(1u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().count(0)); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().at(0).size()); EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size()); EXPECT_EQ(1u, systemMgr.TotalByEntity(updateEntity).size()); } From 8931eabecbc7d32f0fd2daf905d48a9b68942405 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 27 Jul 2024 22:26:01 -0700 Subject: [PATCH 11/19] ForceTorque system: always write to the ECM Currently the system skips the sensor update step when there are no gz-transport subscribers, but to support writing to the ECM, the system should always update when there is new sensor data. Signed-off-by: Steve Peters --- src/systems/force_torque/ForceTorque.cc | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/systems/force_torque/ForceTorque.cc b/src/systems/force_torque/ForceTorque.cc index 75f43fba13..99774f9fa8 100644 --- a/src/systems/force_torque/ForceTorque.cc +++ b/src/systems/force_torque/ForceTorque.cc @@ -180,15 +180,13 @@ void ForceTorque::Update(const UpdateInfo &_info, if (!_info.paused) { // check to see if update is necessary - // we only update if there is at least one sensor that needs data - // and that sensor has subscribers. + // we only update if there is at least one sensor that needs data. // note: gz-sensors does its own throttling. Here the check is mainly // to avoid doing work in the ForceTorquePrivate::Update function bool needsUpdate = false; for (const auto &[sensorEntity, sensor] : this->dataPtr->entitySensorMap) { - if (sensor->NextDataUpdateTime() <= _info.simTime && - sensor->HasConnections()) + if (sensor->NextDataUpdateTime() <= _info.simTime) { needsUpdate = true; break; From 52a73a35154ff98e964471488bc8ae09470792d9 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 28 Jul 2024 01:02:58 -0700 Subject: [PATCH 12/19] Model.hh: add nested model accessor methods Add ModelByName and ModelCount APIs. These are needed for the updated ForceTorque test. Signed-off-by: Steve Peters --- include/gz/sim/Model.hh | 14 ++++++++++++++ src/Model.cc | 16 ++++++++++++++++ test/integration/model.cc | 24 ++++++++++++++++++++++++ 3 files changed, 54 insertions(+) diff --git a/include/gz/sim/Model.hh b/include/gz/sim/Model.hh index 9de49ea0a1..ddb0e8130d 100644 --- a/include/gz/sim/Model.hh +++ b/include/gz/sim/Model.hh @@ -137,6 +137,14 @@ namespace gz public: sim::Entity LinkByName(const EntityComponentManager &_ecm, const std::string &_name) const; + /// \brief Get the ID of a nested model entity which is an immediate + /// child of this model. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _name Nested model name. + /// \return Nested model entity. + public: sim::Entity ModelByName(const EntityComponentManager &_ecm, + const std::string &_name) const; + /// \brief Get all joints which are immediate children of this model. /// \param[in] _ecm Entity-component manager. /// \return All joints in this model. @@ -167,6 +175,12 @@ namespace gz /// \return Number of links in this model. public: uint64_t LinkCount(const EntityComponentManager &_ecm) const; + /// \brief Get the number of nested models which are immediate children + /// of this model. + /// \param[in] _ecm Entity-component manager. + /// \return Number of nested models in this model. + public: uint64_t ModelCount(const EntityComponentManager &_ecm) const; + /// \brief Set a command to change the model's pose. /// \param[in] _ecm Entity-component manager. /// \param[in] _pose New model pose. diff --git a/src/Model.cc b/src/Model.cc index a23779981f..01f4538fb7 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -148,6 +148,16 @@ Entity Model::LinkByName(const EntityComponentManager &_ecm, components::Link()); } +////////////////////////////////////////////////// +Entity Model::ModelByName(const EntityComponentManager &_ecm, + const std::string &_name) const +{ + return _ecm.EntityByComponents( + components::ParentEntity(this->dataPtr->id), + components::Name(_name), + components::Model()); +} + ////////////////////////////////////////////////// std::vector Model::Joints(const EntityComponentManager &_ecm) const { @@ -184,6 +194,12 @@ uint64_t Model::LinkCount(const EntityComponentManager &_ecm) const return this->Links(_ecm).size(); } +////////////////////////////////////////////////// +uint64_t Model::ModelCount(const EntityComponentManager &_ecm) const +{ + return this->Models(_ecm).size(); +} + ////////////////////////////////////////////////// void Model::SetWorldPoseCmd(EntityComponentManager &_ecm, const math::Pose3d &_pose) diff --git a/test/integration/model.cc b/test/integration/model.cc index f878088631..f4a0f00b2c 100644 --- a/test/integration/model.cc +++ b/test/integration/model.cc @@ -164,6 +164,30 @@ TEST_F(ModelIntegrationTest, SourceFilePath) EXPECT_EQ("/tmp/path", model.SourceFilePath(ecm)); } +////////////////////////////////////////////////// +TEST_F(ModelIntegrationTest, ModelByName) +{ + EntityComponentManager ecm; + + // Model + auto eModel = ecm.CreateEntity(); + Model model(eModel); + EXPECT_EQ(eModel, model.Entity()); + EXPECT_EQ(0u, model.ModelCount(ecm)); + + // Nested Model + auto eNestedModel = ecm.CreateEntity(); + ecm.CreateComponent(eNestedModel, components::Model()); + ecm.CreateComponent(eNestedModel, + components::ParentEntity(eModel)); + ecm.CreateComponent(eNestedModel, + components::Name("nested_model_name")); + + // Check model + EXPECT_EQ(eNestedModel, model.ModelByName(ecm, "nested_model_name")); + EXPECT_EQ(1u, model.ModelCount(ecm)); +} + ////////////////////////////////////////////////// TEST_F(ModelIntegrationTest, LinkByName) { From ed6a5fe9e5f9728f88062c71c2097cf8a2f07c79 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 28 Jul 2024 01:05:36 -0700 Subject: [PATCH 13/19] Minor changes to force_torque_system test * Alphabetize headers * Rename test using gz-transport topics * Add const to some local variables Signed-off-by: Steve Peters --- test/integration/force_torque_system.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/test/integration/force_torque_system.cc b/test/integration/force_torque_system.cc index dd4684e627..8277fa2569 100644 --- a/test/integration/force_torque_system.cc +++ b/test/integration/force_torque_system.cc @@ -24,9 +24,9 @@ #include #include +#include "gz/sim/components/ForceTorque.hh" #include "gz/sim/components/Name.hh" #include "gz/sim/components/Sensor.hh" -#include "gz/sim/components/ForceTorque.hh" #include "gz/sim/Server.hh" #include "gz/sim/SystemLoader.hh" @@ -43,7 +43,7 @@ class ForceTorqueTest : public InternalFixture<::testing::Test> }; ///////////////////////////////////////////////// -TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeight)) +TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeightTopic)) { using namespace std::chrono_literals; // Start server @@ -59,8 +59,8 @@ TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeight)) // Having iters exactly in sync with update rate can lead to a race condition // in the test between simulation and transport - size_t iters = 999u; - size_t updates = 100u; + const size_t iters = 999u; + const size_t updates = 100u; std::vector wrenches; wrenches.reserve(updates); From 2a2ebc17ba8d610bc2ea37e0792e1b8596c77d3f Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 28 Jul 2024 01:08:18 -0700 Subject: [PATCH 14/19] Test WrenchMeasured component in force_torque test Signed-off-by: Steve Peters --- test/integration/force_torque_system.cc | 67 +++++++++++++++++++++++++ 1 file changed, 67 insertions(+) diff --git a/test/integration/force_torque_system.cc b/test/integration/force_torque_system.cc index 8277fa2569..d1fe73b707 100644 --- a/test/integration/force_torque_system.cc +++ b/test/integration/force_torque_system.cc @@ -25,9 +25,14 @@ #include #include "gz/sim/components/ForceTorque.hh" +#include "gz/sim/components/Model.hh" #include "gz/sim/components/Name.hh" #include "gz/sim/components/Sensor.hh" +#include "gz/sim/components/WrenchMeasured.hh" +#include "gz/sim/Joint.hh" +#include "gz/sim/Link.hh" +#include "gz/sim/Model.hh" #include "gz/sim/Server.hh" #include "gz/sim/SystemLoader.hh" #include "test_config.hh" @@ -100,6 +105,68 @@ TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeightTopic)) } } +///////////////////////////////////////////////// +TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeightECM)) +{ + using namespace std::chrono_literals; + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths( + std::string(PROJECT_SOURCE_PATH), "test", "worlds", "force_torque.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + server.SetUpdatePeriod(1ns); + + test::Relay testSystem; + + // Get the MeasuredWrench for //test1/scale/sensor_joint/force_torque_sensor + msgs::Wrench wrench; + testSystem.OnPostUpdate( + [&wrench](const UpdateInfo &, + const EntityComponentManager &_ecm) + { + auto test1ModelEntity = _ecm.EntityByComponents( + components::Model(), components::Name("test1")); + Model test1Model(test1ModelEntity); + + Model scaleModel(test1Model.ModelByName(_ecm, "scale")); + ASSERT_TRUE(scaleModel.Valid(_ecm)); + + Joint sensorJoint(scaleModel.JointByName(_ecm, "sensor_joint")); + ASSERT_TRUE(sensorJoint.Valid(_ecm)); + + auto sensorEntity = + sensorJoint.SensorByName(_ecm, "force_torque_sensor"); + auto measuredWrench = + _ecm.Component(sensorEntity); + if (measuredWrench) + { + wrench = measuredWrench->Data(); + } + }); + + // Add the system + server.AddSystem(testSystem.systemPtr); + server.Run(true, 1, false); + + const size_t iters = 999u; + + // Run server (iters-1) steps, since we already took 1 step above + server.Run(true, iters - 1, false); + ASSERT_EQ(iters, *server.IterationCount()); + + const double kSensorMass = 0.2; + const double kWeightMass = 10; + const double kGravity = 9.8; + const math::Vector3 expectedForce = + math::Vector3d{0, 0, kGravity * (kSensorMass + kWeightMass)}; + EXPECT_EQ(expectedForce, msgs::Convert(wrench.force())); + EXPECT_EQ(math::Vector3d::Zero, msgs::Convert(wrench.torque())); +} + ///////////////////////////////////////////////// TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SensorPoseOffset)) { From c6fedb12d30f41e593061d54bd549f94ff447787 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 2 Aug 2024 11:03:43 -0700 Subject: [PATCH 15/19] Only write WrenchMeasured if component exists This allows users to enable writing the sensor data to the ECM by creating the component. Signed-off-by: Steve Peters --- src/systems/force_torque/ForceTorque.cc | 17 ++++--- test/integration/force_torque_system.cc | 59 ++++++++++++++++++------- 2 files changed, 53 insertions(+), 23 deletions(-) diff --git a/src/systems/force_torque/ForceTorque.cc b/src/systems/force_torque/ForceTorque.cc index 99774f9fa8..d704e99ec5 100644 --- a/src/systems/force_torque/ForceTorque.cc +++ b/src/systems/force_torque/ForceTorque.cc @@ -153,9 +153,6 @@ void ForceTorque::PreUpdate(const UpdateInfo &/*_info*/, _ecm.Component(entity)->Data(); gzdbg << "Adding JointTransmittedWrench to: " << jointEntity << std::endl; _ecm.CreateComponent(jointEntity, components::JointTransmittedWrench()); - // Enable WrenchMeasured to save sensor measurements - gzdbg << "Adding WrenchMeasured to: " << entity << std::endl; - _ecm.CreateComponent(entity, components::WrenchMeasured()); } this->dataPtr->newSensors.clear(); } @@ -186,7 +183,9 @@ void ForceTorque::Update(const UpdateInfo &_info, bool needsUpdate = false; for (const auto &[sensorEntity, sensor] : this->dataPtr->entitySensorMap) { - if (sensor->NextDataUpdateTime() <= _info.simTime) + if (sensor->NextDataUpdateTime() <= _info.simTime && + (sensor->HasConnections() || + _ecm.Component(sensorEntity) != nullptr)) { needsUpdate = true; break; @@ -205,9 +204,13 @@ void ForceTorque::Update(const UpdateInfo &_info, // * Apply noise // * Publish to gz-transport topic sensor->Update(_info.simTime, false); - const auto &measuredWrench = sensor->MeasuredWrench(); - _ecm.SetComponentData( - sensorEntity, measuredWrench); + auto wrenchComponent = + _ecm.Component(sensorEntity); + if (wrenchComponent) + { + const auto &measuredWrench = sensor->MeasuredWrench(); + *wrenchComponent = components::WrenchMeasured(measuredWrench); + } } } diff --git a/test/integration/force_torque_system.cc b/test/integration/force_torque_system.cc index d1fe73b707..1841ef1728 100644 --- a/test/integration/force_torque_system.cc +++ b/test/integration/force_torque_system.cc @@ -120,35 +120,62 @@ TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeightECM)) EXPECT_FALSE(*server.Running(0)); server.SetUpdatePeriod(1ns); - test::Relay testSystem; + // Get entity for //test1/scale/sensor_joint/force_torque_sensor and add + // WrenchMeasured component on first PreUpdate + Entity sensorEntity; + bool firstRun = true; + auto addWrenchComponent = + [&firstRun, &sensorEntity]( + const UpdateInfo &, EntityComponentManager &_ecm) + { + if (firstRun) + { + firstRun = false; + + // Get sensorEntity + auto test1ModelEntity = _ecm.EntityByComponents( + components::Model(), components::Name("test1")); + Model test1Model(test1ModelEntity); + + Model scaleModel(test1Model.ModelByName(_ecm, "scale")); + ASSERT_TRUE(scaleModel.Valid(_ecm)); + + Joint sensorJoint(scaleModel.JointByName(_ecm, "sensor_joint")); + ASSERT_TRUE(sensorJoint.Valid(_ecm)); + + sensorEntity = sensorJoint.SensorByName(_ecm, "force_torque_sensor"); + + // Expect it doesn't yet have WrenchMeasured + EXPECT_EQ(nullptr, + _ecm.Component(sensorEntity)); + + // Add WrenchMeasured + _ecm.CreateComponent(sensorEntity, components::WrenchMeasured()); + + EXPECT_NE(nullptr, + _ecm.Component(sensorEntity)); + } + }; // Get the MeasuredWrench for //test1/scale/sensor_joint/force_torque_sensor msgs::Wrench wrench; - testSystem.OnPostUpdate( - [&wrench](const UpdateInfo &, + auto getMeasuredWrench = + [&wrench, &sensorEntity](const UpdateInfo &, const EntityComponentManager &_ecm) { - auto test1ModelEntity = _ecm.EntityByComponents( - components::Model(), components::Name("test1")); - Model test1Model(test1ModelEntity); - - Model scaleModel(test1Model.ModelByName(_ecm, "scale")); - ASSERT_TRUE(scaleModel.Valid(_ecm)); - - Joint sensorJoint(scaleModel.JointByName(_ecm, "sensor_joint")); - ASSERT_TRUE(sensorJoint.Valid(_ecm)); - - auto sensorEntity = - sensorJoint.SensorByName(_ecm, "force_torque_sensor"); auto measuredWrench = _ecm.Component(sensorEntity); + ASSERT_NE(nullptr, measuredWrench); if (measuredWrench) { wrench = measuredWrench->Data(); } - }); + }; // Add the system + test::Relay testSystem; + testSystem.OnPreUpdate(addWrenchComponent); + testSystem.OnPostUpdate(getMeasuredWrench); server.AddSystem(testSystem.systemPtr); server.Run(true, 1, false); From bd0c1538214e8ec5cc1406ef47ac904e5f90602e Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 2 Aug 2024 11:10:11 -0700 Subject: [PATCH 16/19] Update Migration guide Signed-off-by: Steve Peters --- Migration.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Migration.md b/Migration.md index 2e55298ea9..47e8a26a45 100644 --- a/Migration.md +++ b/Migration.md @@ -12,6 +12,11 @@ release will remove the deprecated code. each time step, whereas previously the component values were set to `0` after each time step. Persistent velocity commands should be reapplied at each time step. + + The ForceTorque system has been changed from updating sensor data during + the parallelized `PostUpdate` phase to use the sequential `Update` phase + and writing directly to the ECM if a sensor entity has a `WrenchMeasured` + component. The ForceTorque system priority is specified to ensure that its + `Update` phase executes after `Physics::Update`. ## Gazebo Sim 7.x to 8.0 * **Deprecated** From ba5408ffd47152917cf8fa26447ede4f822c75b1 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 2 Aug 2024 14:35:43 -0700 Subject: [PATCH 17/19] Set priority for python's joint_test.sdf Signed-off-by: Steve Peters --- python/test/joint_test.sdf | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/python/test/joint_test.sdf b/python/test/joint_test.sdf index 4c9d79345c..4fd300ffd7 100644 --- a/python/test/joint_test.sdf +++ b/python/test/joint_test.sdf @@ -1,8 +1,10 @@ - - + + + 10 + From 1f76c6a487a6c47b88100bb582ad58d0cd74f5e0 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 2 Aug 2024 14:50:19 -0700 Subject: [PATCH 18/19] sensor_TEST.py: move PreUpdate callback to Update The test's PreUpdate callback assumes that it executes after the ForceTorque::PreUpdate, so just move it to Update to gurantee it. Also fix a spelling error in the callback variable names. Signed-off-by: Steve Peters --- python/test/sensor_TEST.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/python/test/sensor_TEST.py b/python/test/sensor_TEST.py index aba4c61ca9..156459e11e 100755 --- a/python/test/sensor_TEST.py +++ b/python/test/sensor_TEST.py @@ -36,8 +36,8 @@ def test_model(self): def on_post_udpate_cb(_info, _ecm): self.post_iterations += 1 - def on_pre_udpate_cb(_info, _ecm): - self.pre_iterations += 1 + def on_update_cb(_info, _ecm): + self.iterations += 1 world_e = world_entity(_ecm) self.assertNotEqual(K_NULL_ENTITY, world_e) w = World(world_e) @@ -53,19 +53,19 @@ def on_pre_udpate_cb(_info, _ecm): # Pose Test self.assertEqual(Pose3d(0, 1, 0, 0, 0, 0), sensor.pose(_ecm)) # Topic Test - if self.pre_iterations <= 1: + if self.iterations <= 1: self.assertEqual(None, sensor.topic(_ecm)) else: self.assertEqual('sensor_topic_test', sensor.topic(_ecm)) # Parent Test self.assertEqual(j.entity(), sensor.parent(_ecm)) - def on_udpate_cb(_info, _ecm): - self.iterations += 1 + def on_pre_update_cb(_info, _ecm): + self.pre_iterations += 1 fixture.on_post_update(on_post_udpate_cb) - fixture.on_update(on_udpate_cb) - fixture.on_pre_update(on_pre_udpate_cb) + fixture.on_update(on_update_cb) + fixture.on_pre_update(on_pre_update_cb) fixture.finalize() server = fixture.server() From 9a7785e2c6f93458418b029f775eda93c5d97498 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 2 Aug 2024 14:51:34 -0700 Subject: [PATCH 19/19] fix spelling in on_post_update_cb Signed-off-by: Steve Peters --- python/test/sensor_TEST.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/test/sensor_TEST.py b/python/test/sensor_TEST.py index 156459e11e..61068c3fa3 100755 --- a/python/test/sensor_TEST.py +++ b/python/test/sensor_TEST.py @@ -33,7 +33,7 @@ def test_model(self): file_path = os.path.dirname(os.path.realpath(__file__)) fixture = TestFixture(os.path.join(file_path, 'joint_test.sdf')) - def on_post_udpate_cb(_info, _ecm): + def on_post_update_cb(_info, _ecm): self.post_iterations += 1 def on_update_cb(_info, _ecm): @@ -63,7 +63,7 @@ def on_update_cb(_info, _ecm): def on_pre_update_cb(_info, _ecm): self.pre_iterations += 1 - fixture.on_post_update(on_post_udpate_cb) + fixture.on_post_update(on_post_update_cb) fixture.on_update(on_update_cb) fixture.on_pre_update(on_pre_update_cb) fixture.finalize()