From 5acd4f7a4bbf95377a261d02f6c001e17c0aec28 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Sat, 15 Jul 2023 12:24:50 -0500 Subject: [PATCH 01/19] Skeleton Signed-off-by: Addisu Z. Taddese --- examples/worlds/python_system_loader.sdf | 270 ++++++++++++++++++ python/CMakeLists.txt | 11 - src/systems/CMakeLists.txt | 1 + .../python_system_loader/CMakeLists.txt | 6 + .../PythonSystemLoader.cc | 54 ++++ .../PythonSystemLoader.hh | 63 ++++ 6 files changed, 394 insertions(+), 11 deletions(-) create mode 100644 examples/worlds/python_system_loader.sdf create mode 100644 src/systems/python_system_loader/CMakeLists.txt create mode 100644 src/systems/python_system_loader/PythonSystemLoader.cc create mode 100644 src/systems/python_system_loader/PythonSystemLoader.hh diff --git a/examples/worlds/python_system_loader.sdf b/examples/worlds/python_system_loader.sdf new file mode 100644 index 0000000000..8991aaad3c --- /dev/null +++ b/examples/worlds/python_system_loader.sdf @@ -0,0 +1,270 @@ + + + + + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + + + + upper_link + lower_link + + 1.0 0 0 + + + + + + + + + + + diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 45d103278a..cea5077127 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -76,17 +76,6 @@ target_link_libraries(common PRIVATE configure_build_install_location(${BINDINGS_MODULE_NAME}) configure_build_install_location(common) -if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") - # Workaround for Clang and pybind11 on Focal - # https://github.com/pybind/pybind11/issues/1604 - # Resolved by newer versions of pybind11 - if(${pybind11_VERSION} VERSION_LESS "2.4.4") - target_compile_options(common PRIVATE -fsized-deallocation) - target_compile_options(sim PRIVATE -fsized-deallocation) - endif() -endif() - - if (BUILD_TESTING) set(python_tests joint_TEST diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index c9c8f441cd..7eb7786908 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -147,6 +147,7 @@ add_subdirectory(performer_detector) add_subdirectory(perfect_comms) add_subdirectory(physics) add_subdirectory(pose_publisher) +add_subdirectory(python_system_loader) add_subdirectory(rf_comms) add_subdirectory(scene_broadcaster) add_subdirectory(sensors) diff --git a/src/systems/python_system_loader/CMakeLists.txt b/src/systems/python_system_loader/CMakeLists.txt new file mode 100644 index 0000000000..d9f25652ae --- /dev/null +++ b/src/systems/python_system_loader/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(python-system-loader + SOURCES + PythonSystemLoader.cc + # PUBLIC_LINK_LIBS +) + diff --git a/src/systems/python_system_loader/PythonSystemLoader.cc b/src/systems/python_system_loader/PythonSystemLoader.cc new file mode 100644 index 0000000000..1719832c37 --- /dev/null +++ b/src/systems/python_system_loader/PythonSystemLoader.cc @@ -0,0 +1,54 @@ +/* + * Copyright (C) 2023 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. + * + */ + +#include "PythonSystemLoader.hh" +#include + +namespace gz::sim::systems +{ +void PythonSystemLoader::Configure( + const Entity &, const std::shared_ptr &_sdf, + EntityComponentManager &, EventManager &) +{ + auto [pythonFile, hasFile] = _sdf->Get("python_file", ""); + if (!hasFile) + { + gzerr << "PythonSystemLoader missing required argument . " + << "Failed to initialize." << std::endl; + return; + } + // TODO(azeey) Where do we look for the python file? Do we need a separate + // environment variable similar to GZ_SYSTEM_PLUGIN_PATH or can we use the + // same variable? For now, only look for the file as an absolute path or + // relative to CWD. + // Alternatively, we can consider PYTHONPATH and the requirement would be that + // we can just import the module. +} + +void PythonSystemLoader::PreUpdate(const UpdateInfo &/* _info */, + EntityComponentManager &/* _ecm */) +{ +} +void PythonSystemLoader::PostUpdate(const UpdateInfo &/* _info */, + const EntityComponentManager &/* _ecm */) +{ +} + +GZ_ADD_PLUGIN(PythonSystemLoader, System, ISystemConfigure, ISystemPreUpdate, + ISystemPostUpdate) +GZ_ADD_PLUGIN_ALIAS(PythonSystemLoader, "gz::sim::systems::PythonSystemLoader") +} // namespace gz::sim::systems diff --git a/src/systems/python_system_loader/PythonSystemLoader.hh b/src/systems/python_system_loader/PythonSystemLoader.hh new file mode 100644 index 0000000000..1d79cfda01 --- /dev/null +++ b/src/systems/python_system_loader/PythonSystemLoader.hh @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2023 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_SYSTEMS_PYTHONSYSTEMLOADER_HH_ +#define GZ_SIM_SYSTEMS_PYTHONSYSTEMLOADER_HH_ + +#include + +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems +{ +// TODO(azeey) Add ParameterConfigure +class PythonSystemLoader : public System, + public ISystemConfigure, + // public ISystemReset, + public ISystemPreUpdate, + // public ISystemUpdate, + public ISystemPostUpdate +{ + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + // Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; + + private: bool validConfig{false}; +}; +} // namespace systems +} // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace sim +} // namespace gz + +#endif /* end of include guard: GZ_SIM_SYSTEMS_PYTHONSYSTEMLOADER_HH_ */ From 1ecec9fdacc754d3afdd7c814faca267a4b93cc6 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 19 Jul 2023 16:15:28 -0500 Subject: [PATCH 02/19] Put pybind11::scoped_interpreter in SystemPrivate This allows multiple PythonSystemLoaders to run albeit with a single python interpreter. Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 7 +- examples/worlds/python_system_loader.sdf | 21 ++-- src/CMakeLists.txt | 6 ++ src/ServerPrivate.hh | 8 ++ .../python_system_loader/CMakeLists.txt | 16 +-- .../PythonSystemLoader.cc | 102 ++++++++++++++++-- .../PythonSystemLoader.hh | 22 ++-- 7 files changed, 140 insertions(+), 42 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 828e7c8ad1..c89b9ce4e2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -198,16 +198,15 @@ set(Protobuf_IMPORT_DIRS ${gz-msgs10_INCLUDE_DIRS}) if (SKIP_PYBIND11) message(STATUS "SKIP_PYBIND11 set - disabling python bindings") else() - find_package(PythonLibs QUIET) - if (NOT PYTHONLIBS_FOUND) + find_package(Python3 QUIET COMPONENTS Interpreter Development) + if (NOT Python3_FOUND) GZ_BUILD_WARNING("Python is missing: Python interfaces are disabled.") message (STATUS "Searching for Python - not found.") else() message (STATUS "Searching for Python - found version ${PYTHONLIBS_VERSION_STRING}.") set(PYBIND11_PYTHON_VERSION 3) - find_package(Python3 QUIET COMPONENTS Interpreter Development) - find_package(pybind11 2.2 QUIET) + find_package(pybind11 2.9 CONFIG QUIET) if (pybind11_FOUND) message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.") diff --git a/examples/worlds/python_system_loader.sdf b/examples/worlds/python_system_loader.sdf index 8991aaad3c..ddb0c4aeef 100644 --- a/examples/worlds/python_system_loader.sdf +++ b/examples/worlds/python_system_loader.sdf @@ -6,18 +6,6 @@ - - - - - - true @@ -258,10 +246,13 @@ - + + test_system + + + + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 5f07404905..3d74614434 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -180,6 +180,12 @@ target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE gz-plugin${GZ_PLUGIN_VER}::loader ) + +if (pybind11_FOUND) + target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE pybind11::embed) + target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE -DHAVE_PYBIND11) +endif() + if (UNIX AND NOT APPLE) target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE stdc++fs) diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh index 03128c56e8..cc28f368e6 100644 --- a/src/ServerPrivate.hh +++ b/src/ServerPrivate.hh @@ -22,11 +22,15 @@ #include #include + #include #include #include #include #include +#ifdef HAVE_PYBIND11 +#include +#endif #include #include #include @@ -147,6 +151,10 @@ namespace gz private: bool ServerControlService( const gz::msgs::ServerControl &_req, msgs::Boolean &_res); +#ifdef HAVE_PYBIND11 + public: pybind11::scoped_interpreter guard{}; +#endif + /// \brief A pool of worker threads. public: common::WorkerPool workerPool{2}; diff --git a/src/systems/python_system_loader/CMakeLists.txt b/src/systems/python_system_loader/CMakeLists.txt index d9f25652ae..c574596762 100644 --- a/src/systems/python_system_loader/CMakeLists.txt +++ b/src/systems/python_system_loader/CMakeLists.txt @@ -1,6 +1,10 @@ -gz_add_system(python-system-loader - SOURCES - PythonSystemLoader.cc - # PUBLIC_LINK_LIBS -) - +if (pybind11_FOUND) + gz_add_system(python-system-loader + SOURCES + PythonSystemLoader.cc + PRIVATE_LINK_LIBS + pybind11::embed + PRIVATE_COMPILE_DEFS + -DPYTHON_LIBRARIES="${Python3_LIBRARIES}" + ) +endif() diff --git a/src/systems/python_system_loader/PythonSystemLoader.cc b/src/systems/python_system_loader/PythonSystemLoader.cc index 1719832c37..baec260332 100644 --- a/src/systems/python_system_loader/PythonSystemLoader.cc +++ b/src/systems/python_system_loader/PythonSystemLoader.cc @@ -16,32 +16,114 @@ */ #include "PythonSystemLoader.hh" + +#include + +#include #include +#include + +namespace py = pybind11; namespace gz::sim::systems { void PythonSystemLoader::Configure( - const Entity &, const std::shared_ptr &_sdf, + const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &, EventManager &) { - auto [pythonFile, hasFile] = _sdf->Get("python_file", ""); - if (!hasFile) + auto [moduleName, hasModule] = _sdf->Get("module_name", ""); + if (!hasModule) { - gzerr << "PythonSystemLoader missing required argument . " + gzerr << "PythonSystemLoader missing required argument . " << "Failed to initialize." << std::endl; return; } - // TODO(azeey) Where do we look for the python file? Do we need a separate - // environment variable similar to GZ_SYSTEM_PLUGIN_PATH or can we use the - // same variable? For now, only look for the file as an absolute path or + // TODO (azeey) Where do we look for the python file? Do we need a separate + // environment variable similar to GZ_SIM_SYSTEM_PLUGIN_PATH or can we use the + // same variable? For now, only look for the file as an absolute path or // relative to CWD. - // Alternatively, we can consider PYTHONPATH and the requirement would be that + // Alternatively, we can consider PYTHONPATH and the requirement would be that // we can just import the module. + try + { + py::module_ sys = py::module_::import("sys"); + SystemLoader systemLoader; + for (const auto &pluginPath : systemLoader.PluginPaths()) + { + sys.attr("path").attr("append")(pluginPath); + } + + gzdbg << "Searching for python module in:\n"; + auto searchPaths = sys.attr("path"); + // TODO (azeey) Improve formatting. + gzdbg << py::str(searchPaths).cast() << std::endl; + } + catch (const pybind11::error_already_set &_err) + { + gzerr << "Error while loading module 'sys'\n" + << _err.what() << std::endl; + return; + } + + try + { + this->pythonModule = py::module_::import(moduleName.c_str()); + } + catch (const pybind11::error_already_set &_err) + { + gzerr << "Error while loading module " << moduleName << "\n" + << _err.what() << std::endl; + return; + } + + try + { + py::object getSystem = this->pythonModule.attr("get_system"); + if (!getSystem) + { + gzerr << "Could not call 'get_system' on the module" << moduleName << "\n"; + return; + } + this->pythonSystem = getSystem(); + } + catch (const pybind11::error_already_set &_err) + { + gzerr << _err.what() << std::endl; + return; + } + if (py::hasattr(this->pythonSystem, "configure")) + { + this->pythonSystem.attr("configure")(_entity); + } + + if (py::hasattr(this->pythonSystem, "pre_update")) + { + this->preUpdateMethod = this->pythonSystem.attr("pre_update"); + } + this->validConfig = true; } -void PythonSystemLoader::PreUpdate(const UpdateInfo &/* _info */, - EntityComponentManager &/* _ecm */) +void PythonSystemLoader::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { + if (!this->validConfig) + { + return; + } + + if (this->preUpdateMethod) + { + try + { + this->preUpdateMethod( + _info, py::cast(_ecm, py::return_value_policy::reference)); + } + catch (const pybind11::error_already_set &_err) + { + gzerr << _err.what() << std::endl; + return; + } + } } void PythonSystemLoader::PostUpdate(const UpdateInfo &/* _info */, const EntityComponentManager &/* _ecm */) diff --git a/src/systems/python_system_loader/PythonSystemLoader.hh b/src/systems/python_system_loader/PythonSystemLoader.hh index 1d79cfda01..cd8236bd73 100644 --- a/src/systems/python_system_loader/PythonSystemLoader.hh +++ b/src/systems/python_system_loader/PythonSystemLoader.hh @@ -18,10 +18,13 @@ #ifndef GZ_SIM_SYSTEMS_PYTHONSYSTEMLOADER_HH_ #define GZ_SIM_SYSTEMS_PYTHONSYSTEMLOADER_HH_ +#include + +#include #include #include -#include +#include namespace gz { @@ -32,12 +35,13 @@ inline namespace GZ_SIM_VERSION_NAMESPACE { namespace systems { // TODO(azeey) Add ParameterConfigure -class PythonSystemLoader : public System, - public ISystemConfigure, - // public ISystemReset, - public ISystemPreUpdate, - // public ISystemUpdate, - public ISystemPostUpdate +class GZ_SIM_PYTHON_SYSTEM_LOADER_SYSTEM_HIDDEN PythonSystemLoader + : public System, + public ISystemConfigure, + // public ISystemReset, + public ISystemPreUpdate, + // public ISystemUpdate, + public ISystemPostUpdate { // Documentation inherited public: void Configure(const Entity &_entity, @@ -54,6 +58,10 @@ class PythonSystemLoader : public System, const EntityComponentManager &_ecm) override; private: bool validConfig{false}; + private: pybind11::module_ pythonModule; + private: pybind11::object pythonSystem; + private: pybind11::object preUpdateMethod; + private: pybind11::object postUpdateMethod; }; } // namespace systems } // namespace GZ_SIM_VERSION_NAMESPACE From 41917631d77f3d26c0ecff62317522a6cbc38851 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 20 Jul 2023 19:09:33 -0500 Subject: [PATCH 03/19] Implement other system interfaces Signed-off-by: Addisu Z. Taddese --- examples/worlds/python_system_loader.sdf | 208 ++---------------- .../PythonSystemLoader.cc | 64 ++++-- .../PythonSystemLoader.hh | 25 ++- 3 files changed, 90 insertions(+), 207 deletions(-) diff --git a/examples/worlds/python_system_loader.sdf b/examples/worlds/python_system_loader.sdf index ddb0c4aeef..9a247f11e3 100644 --- a/examples/worlds/python_system_loader.sdf +++ b/examples/worlds/python_system_loader.sdf @@ -48,212 +48,44 @@ - - + + 0 0 0.5 0 0 0 + - 100 + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 - - 0 0 0.01 0 0 0 - - - 0.8 - 0.02 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - -0.275 0 1.1 0 0 0 + - 0.2 0.2 2.2 + 1 1 1 - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - 0 0 0.01 0 0 0 - - - 0.8 - 0.02 - - - - -0.275 0 1.1 0 0 0 + + - 0.2 0.2 2.2 + 1 1 1 - - - - - 0 0 2.1 -1.5708 0 0 - 0 - - 0 0 0.5 0 0 0 - - - -0.05 0 0 0 1.5708 0 - - - 0.1 - 0.3 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - 0 0 1.0 0 1.5708 0 - - - 0.1 - 0.2 - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - -0.05 0 0 0 1.5708 0 - - - 0.1 - 0.3 - - - - - 0 0 1.0 0 1.5708 0 - - - 0.1 - 0.2 - - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - - - 0.25 1.0 2.1 -2 0 0 - 0 - - 0 0 0.5 0 0 0 - - - 0 0 0 0 1.5708 0 - - - 0.08 - 0.3 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - 0 0 0 0 1.5708 0 - - - 0.08 - 0.3 - - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - - - - base - upper_link - - 1.0 0 0 - - - - - upper_link - lower_link - - 1.0 0 0 - - - test_system - - - - - diff --git a/src/systems/python_system_loader/PythonSystemLoader.cc b/src/systems/python_system_loader/PythonSystemLoader.cc index baec260332..58474846c6 100644 --- a/src/systems/python_system_loader/PythonSystemLoader.cc +++ b/src/systems/python_system_loader/PythonSystemLoader.cc @@ -40,10 +40,8 @@ void PythonSystemLoader::Configure( } // TODO (azeey) Where do we look for the python file? Do we need a separate // environment variable similar to GZ_SIM_SYSTEM_PLUGIN_PATH or can we use the - // same variable? For now, only look for the file as an absolute path or - // relative to CWD. - // Alternatively, we can consider PYTHONPATH and the requirement would be that - // we can just import the module. + // same variable? For now, just load it as a module, which will be affected by + // PYTHONPATH. try { py::module_ sys = py::module_::import("sys"); @@ -95,28 +93,40 @@ void PythonSystemLoader::Configure( { this->pythonSystem.attr("configure")(_entity); } - + // TODO(azeey) Add support for ConfigureParameters if (py::hasattr(this->pythonSystem, "pre_update")) { this->preUpdateMethod = this->pythonSystem.attr("pre_update"); } + if (py::hasattr(this->pythonSystem, "update")) + { + this->updateMethod = this->pythonSystem.attr("update"); + } + if (py::hasattr(this->pythonSystem, "post_update")) + { + this->postUpdateMethod = this->pythonSystem.attr("post_update"); + } + if (py::hasattr(this->pythonSystem, "reset")) + { + this->resetMethod = this->pythonSystem.attr("reset"); + } this->validConfig = true; } -void PythonSystemLoader::PreUpdate(const UpdateInfo &_info, - EntityComponentManager &_ecm) +////////////////////////////////////////////////// +template +void PythonSystemLoader::CallPythonMethod(py::object _method, Args&&... _args) { if (!this->validConfig) { return; } - if (this->preUpdateMethod) + if (_method) { try { - this->preUpdateMethod( - _info, py::cast(_ecm, py::return_value_policy::reference)); + _method(std::forward(_args)...); } catch (const pybind11::error_already_set &_err) { @@ -125,12 +135,40 @@ void PythonSystemLoader::PreUpdate(const UpdateInfo &_info, } } } -void PythonSystemLoader::PostUpdate(const UpdateInfo &/* _info */, - const EntityComponentManager &/* _ecm */) + +////////////////////////////////////////////////// +void PythonSystemLoader::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + CallPythonMethod(this->preUpdateMethod, _info, + py::cast(_ecm, py::return_value_policy::reference)); +} + +////////////////////////////////////////////////// +void PythonSystemLoader::Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + CallPythonMethod(this->updateMethod, _info, + py::cast(_ecm, py::return_value_policy::reference)); +} + +////////////////////////////////////////////////// +void PythonSystemLoader::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) { + CallPythonMethod(this->postUpdateMethod, _info, + py::cast(_ecm, py::return_value_policy::reference)); } +////////////////////////////////////////////////// +void PythonSystemLoader::Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + CallPythonMethod(this->resetMethod, _info, + py::cast(_ecm, py::return_value_policy::reference)); +} + GZ_ADD_PLUGIN(PythonSystemLoader, System, ISystemConfigure, ISystemPreUpdate, - ISystemPostUpdate) + ISystemUpdate, ISystemPostUpdate, ISystemReset) GZ_ADD_PLUGIN_ALIAS(PythonSystemLoader, "gz::sim::systems::PythonSystemLoader") } // namespace gz::sim::systems diff --git a/src/systems/python_system_loader/PythonSystemLoader.hh b/src/systems/python_system_loader/PythonSystemLoader.hh index cd8236bd73..63706b301d 100644 --- a/src/systems/python_system_loader/PythonSystemLoader.hh +++ b/src/systems/python_system_loader/PythonSystemLoader.hh @@ -38,30 +38,43 @@ namespace systems class GZ_SIM_PYTHON_SYSTEM_LOADER_SYSTEM_HIDDEN PythonSystemLoader : public System, public ISystemConfigure, - // public ISystemReset, public ISystemPreUpdate, - // public ISystemUpdate, - public ISystemPostUpdate + public ISystemUpdate, + public ISystemPostUpdate, + public ISystemReset { // Documentation inherited public: void Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, - EventManager &_eventMgr) override; + EventManager &_eventMgr) final; // Documentation inherited public: void PreUpdate(const UpdateInfo &_info, - EntityComponentManager &_ecm) override; + EntityComponentManager &_ecm) final; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; // Documentation inherited public: void PostUpdate(const UpdateInfo &_info, - const EntityComponentManager &_ecm) override; + const EntityComponentManager &_ecm) final; + + // Documentation inherited + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + private: template + void CallPythonMethod(pybind11::object _method, Args&&...); private: bool validConfig{false}; private: pybind11::module_ pythonModule; private: pybind11::object pythonSystem; private: pybind11::object preUpdateMethod; + private: pybind11::object updateMethod; private: pybind11::object postUpdateMethod; + private: pybind11::object resetMethod; }; } // namespace systems } // namespace GZ_SIM_VERSION_NAMESPACE From cb48ec51e9314847f3f10a21512085263ac6ffac Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 20 Jul 2023 19:16:36 -0500 Subject: [PATCH 04/19] Add example python script Signed-off-by: Addisu Z. Taddese --- .../scripts/python_api/systems/test_system.py | 23 +++++++++++++++++++ examples/worlds/python_system_loader.sdf | 13 +++++++++++ 2 files changed, 36 insertions(+) create mode 100644 examples/scripts/python_api/systems/test_system.py diff --git a/examples/scripts/python_api/systems/test_system.py b/examples/scripts/python_api/systems/test_system.py new file mode 100644 index 0000000000..2c51dc8b9c --- /dev/null +++ b/examples/scripts/python_api/systems/test_system.py @@ -0,0 +1,23 @@ +from gz.sim8 import UpdateInfo, EntityComponentManager, Model +from gz.math7 import Vector3d, Pose3d +import random + +class TestSystem(object): + def __init__(self): + self.id = random.randint(1, 100) + + def configure(self, entity): + self.model = Model(entity) + print("Configured on", entity) + + def pre_update(self, info, ecm): + if info.paused: + return + + if info.iterations % 3000 == 0: + print(f"{self.id} {info.real_time} pre_update") + self.model.set_world_pose_cmd(ecm, Pose3d(0, 0, 5, 0,0,0)) + + +def get_system(): + return TestSystem() diff --git a/examples/worlds/python_system_loader.sdf b/examples/worlds/python_system_loader.sdf index 9a247f11e3..f22d8e94ed 100644 --- a/examples/worlds/python_system_loader.sdf +++ b/examples/worlds/python_system_loader.sdf @@ -2,6 +2,19 @@ From b91b9fbc31517965138623ade6c21be53b67ce87 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 24 Jul 2023 20:54:41 -0500 Subject: [PATCH 05/19] WIP: Configure Signed-off-by: Addisu Z. Taddese --- examples/scripts/python_api/systems/test_system.py | 5 ++++- examples/worlds/python_system_loader.sdf | 1 + src/systems/python_system_loader/CMakeLists.txt | 2 -- src/systems/python_system_loader/PythonSystemLoader.cc | 8 ++++---- 4 files changed, 9 insertions(+), 7 deletions(-) diff --git a/examples/scripts/python_api/systems/test_system.py b/examples/scripts/python_api/systems/test_system.py index 2c51dc8b9c..18c70b4b6f 100644 --- a/examples/scripts/python_api/systems/test_system.py +++ b/examples/scripts/python_api/systems/test_system.py @@ -1,14 +1,17 @@ from gz.sim8 import UpdateInfo, EntityComponentManager, Model from gz.math7 import Vector3d, Pose3d +from sdformat13 import Element import random class TestSystem(object): def __init__(self): self.id = random.randint(1, 100) - def configure(self, entity): + def configure(self, entity, sdf, ecm): self.model = Model(entity) print("Configured on", entity) + print("sdf name:", sdf.get_name()) + print("some int:", sdf.get_int("some_int")) def pre_update(self, info, ecm): if info.paused: diff --git a/examples/worlds/python_system_loader.sdf b/examples/worlds/python_system_loader.sdf index f22d8e94ed..b5b183137c 100644 --- a/examples/worlds/python_system_loader.sdf +++ b/examples/worlds/python_system_loader.sdf @@ -98,6 +98,7 @@ your PYTHONPATH test_system + 100 diff --git a/src/systems/python_system_loader/CMakeLists.txt b/src/systems/python_system_loader/CMakeLists.txt index c574596762..2ae9645d00 100644 --- a/src/systems/python_system_loader/CMakeLists.txt +++ b/src/systems/python_system_loader/CMakeLists.txt @@ -4,7 +4,5 @@ if (pybind11_FOUND) PythonSystemLoader.cc PRIVATE_LINK_LIBS pybind11::embed - PRIVATE_COMPILE_DEFS - -DPYTHON_LIBRARIES="${Python3_LIBRARIES}" ) endif() diff --git a/src/systems/python_system_loader/PythonSystemLoader.cc b/src/systems/python_system_loader/PythonSystemLoader.cc index 58474846c6..878df86689 100644 --- a/src/systems/python_system_loader/PythonSystemLoader.cc +++ b/src/systems/python_system_loader/PythonSystemLoader.cc @@ -17,8 +17,6 @@ #include "PythonSystemLoader.hh" -#include - #include #include #include @@ -29,7 +27,7 @@ namespace gz::sim::systems { void PythonSystemLoader::Configure( const Entity &_entity, const std::shared_ptr &_sdf, - EntityComponentManager &, EventManager &) + EntityComponentManager &_ecm, EventManager &_eventMgr) { auto [moduleName, hasModule] = _sdf->Get("module_name", ""); if (!hasModule) @@ -91,7 +89,9 @@ void PythonSystemLoader::Configure( } if (py::hasattr(this->pythonSystem, "configure")) { - this->pythonSystem.attr("configure")(_entity); + sdf::ElementPtr sdfClone = _sdf->Clone(); + this->pythonSystem.attr("configure")( + _entity, sdfClone, py::cast(_ecm, py::return_value_policy::reference)); } // TODO(azeey) Add support for ConfigureParameters if (py::hasattr(this->pythonSystem, "pre_update")) From 9bf542eb39cb931e1f74c9bf0963216a14123c14 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 17 Aug 2023 17:04:57 -0500 Subject: [PATCH 06/19] Compute worldPose when the WorldPose component is not available Signed-off-by: Addisu Z. Taddese --- src/Link.cc | 39 ++++++++++++++++++++++----------------- 1 file changed, 22 insertions(+), 17 deletions(-) diff --git a/src/Link.cc b/src/Link.cc index bf58837f29..c6dfcf4ecc 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -186,7 +186,8 @@ bool Link::WindMode(const EntityComponentManager &_ecm) const std::optional Link::WorldPose( const EntityComponentManager &_ecm) const { - return _ecm.ComponentData(this->dataPtr->id); + return _ecm.ComponentData(this->dataPtr->id) + .value_or(sim::worldPose(this->dataPtr->id, _ecm)); } ////////////////////////////////////////////////// @@ -194,12 +195,13 @@ std::optional Link::WorldInertialPose( const EntityComponentManager &_ecm) const { auto inertial = _ecm.Component(this->dataPtr->id); - auto worldPose = _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.ComponentData(this->dataPtr->id) + .value_or(sim::worldPose(this->dataPtr->id, _ecm)); - if (!worldPose || !inertial) + if (!inertial) return std::nullopt; - return std::make_optional(worldPose->Data() * inertial->Data().Pose()); + return std::make_optional(worldPose * inertial->Data().Pose()); } ////////////////////////////////////////////////// @@ -216,17 +218,17 @@ std::optional Link::WorldLinearVelocity( { auto worldLinVel = _ecm.Component(this->dataPtr->id); - auto worldPose = - _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.ComponentData(this->dataPtr->id) + .value_or(sim::worldPose(this->dataPtr->id, _ecm)); auto worldAngVel = _ecm.Component(this->dataPtr->id); - if (!worldLinVel || !worldPose || !worldAngVel) + if (!worldLinVel || !worldAngVel) return std::nullopt; return std::make_optional( worldLinVel->Data() + - worldAngVel->Data().Cross(worldPose->Data().Rot().RotateVector(_offset))); + worldAngVel->Data().Cross(worldPose.Rot().RotateVector(_offset))); } ////////////////////////////////////////////////// @@ -326,13 +328,14 @@ std::optional Link::WorldInertiaMatrix( const EntityComponentManager &_ecm) const { auto inertial = _ecm.Component(this->dataPtr->id); - auto worldPose = _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.ComponentData(this->dataPtr->id) + .value_or(sim::worldPose(this->dataPtr->id, _ecm)); - if (!worldPose || !inertial) + if (!inertial) return std::nullopt; const math::Pose3d &comWorldPose = - worldPose->Data() * inertial->Data().Pose(); + worldPose * inertial->Data().Pose(); return std::make_optional( math::Inertiald(inertial->Data().MassMatrix(), comWorldPose).Moi()); } @@ -367,17 +370,18 @@ void Link::AddWorldForce(EntityComponentManager &_ecm, const math::Vector3d &_force) const { auto inertial = _ecm.Component(this->dataPtr->id); - auto worldPose = _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.ComponentData(this->dataPtr->id) + .value_or(sim::worldPose(this->dataPtr->id, _ecm)); // Can't apply force if the inertial's pose is not found - if (!inertial || !worldPose) + if (!inertial) return; // We want the force to be applied at the center of mass, but // ExternalWorldWrenchCmd applies the force at the link origin so we need to // compute the resulting force and torque on the link origin. auto posComWorldCoord = - worldPose->Data().Rot().RotateVector(inertial->Data().Pose().Pos()); + worldPose.Rot().RotateVector(inertial->Data().Pose().Pos()); math::Vector3d torque = posComWorldCoord.Cross(_force); @@ -390,16 +394,17 @@ void Link::AddWorldForce(EntityComponentManager &_ecm, const math::Vector3d &_position) const { auto inertial = _ecm.Component(this->dataPtr->id); - auto worldPose = _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.ComponentData(this->dataPtr->id) + .value_or(sim::worldPose(this->dataPtr->id, _ecm)); // Can't apply force if the inertial's pose is not found - if (!inertial || !worldPose) + if (!inertial) return; // We want the force to be applied at an offset from the center of mass, but // ExternalWorldWrenchCmd applies the force at the link origin so we need to // compute the resulting force and torque on the link origin. - auto posComWorldCoord = worldPose->Data().Rot().RotateVector( + auto posComWorldCoord = worldPose.Rot().RotateVector( _position + inertial->Data().Pose().Pos()); math::Vector3d torque = posComWorldCoord.Cross(_force); From 4d3956ba756881d7502f79570aabc8c9a9186d1f Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 17 Aug 2023 17:06:33 -0500 Subject: [PATCH 07/19] Fix passing EventManager, address todos Signed-off-by: Addisu Z. Taddese --- .../scripts/python_api/systems/test_system.py | 17 ++-- examples/worlds/python_system_loader.sdf | 6 +- include/gz/sim/EventManager.hh | 3 + .../PythonSystemLoader.cc | 78 ++++++++++++------- .../PythonSystemLoader.hh | 16 +++- 5 files changed, 82 insertions(+), 38 deletions(-) diff --git a/examples/scripts/python_api/systems/test_system.py b/examples/scripts/python_api/systems/test_system.py index 18c70b4b6f..cbe487f648 100644 --- a/examples/scripts/python_api/systems/test_system.py +++ b/examples/scripts/python_api/systems/test_system.py @@ -1,17 +1,19 @@ -from gz.sim8 import UpdateInfo, EntityComponentManager, Model -from gz.math7 import Vector3d, Pose3d -from sdformat13 import Element +from gz.math7 import Vector3d +from gz.sim8 import Model, Link import random + class TestSystem(object): def __init__(self): self.id = random.randint(1, 100) - def configure(self, entity, sdf, ecm): + def configure(self, entity, sdf, ecm, event_mgr): self.model = Model(entity) + self.link = Link(self.model.canonical_link(ecm)) print("Configured on", entity) print("sdf name:", sdf.get_name()) - print("some int:", sdf.get_int("some_int")) + self.force = sdf.get_double("force") + print(f"Applying {self.force} N on link {self.link.name(ecm)}") def pre_update(self, info, ecm): if info.paused: @@ -19,7 +21,10 @@ def pre_update(self, info, ecm): if info.iterations % 3000 == 0: print(f"{self.id} {info.real_time} pre_update") - self.model.set_world_pose_cmd(ecm, Pose3d(0, 0, 5, 0,0,0)) + + self.link.add_world_force( + ecm, Vector3d(0, 0, self.force), + Vector3d(random.random(), random.random(), 0)) def get_system(): diff --git a/examples/worlds/python_system_loader.sdf b/examples/worlds/python_system_loader.sdf index b5b183137c..f75eed899f 100644 --- a/examples/worlds/python_system_loader.sdf +++ b/examples/worlds/python_system_loader.sdf @@ -3,10 +3,10 @@ Gazebo python system loader example Before running this, add `examples/scripts/python_api/systems` to your - PYTHONPATH + GZ_SIM_SYSTEM_PLUGIN_PATH ``` - export PYTHONPATH=path_to_gazebo/examples/scripts/python_api/systems:$PYTHONPATH + export GZ_SIM_SYSTEM_PLUGIN_PATH=path_to_gazebo/examples/scripts/python_api/systems ``` If you're building with Colcon, you'll also need to put `install/lib/python` in @@ -98,7 +98,7 @@ your PYTHONPATH test_system - 100 + 3000 diff --git a/include/gz/sim/EventManager.hh b/include/gz/sim/EventManager.hh index ace4848c04..1237b801da 100644 --- a/include/gz/sim/EventManager.hh +++ b/include/gz/sim/EventManager.hh @@ -56,6 +56,9 @@ namespace gz /// \brief Constructor public: EventManager() = default; + public: EventManager(const EventManager &) = delete; + public: EventManager &operator=(const EventManager &) = delete; + /// \brief Destructor public: ~EventManager() = default; diff --git a/src/systems/python_system_loader/PythonSystemLoader.cc b/src/systems/python_system_loader/PythonSystemLoader.cc index 878df86689..13987e28a4 100644 --- a/src/systems/python_system_loader/PythonSystemLoader.cc +++ b/src/systems/python_system_loader/PythonSystemLoader.cc @@ -17,6 +17,8 @@ #include "PythonSystemLoader.hh" +#include + #include #include #include @@ -36,28 +38,40 @@ void PythonSystemLoader::Configure( << "Failed to initialize." << std::endl; return; } - // TODO (azeey) Where do we look for the python file? Do we need a separate - // environment variable similar to GZ_SIM_SYSTEM_PLUGIN_PATH or can we use the - // same variable? For now, just load it as a module, which will be affected by - // PYTHONPATH. + + // Load the `gz.sim` and sdformat modules to register all pybind bindings + // necessary for System interface functions + const auto gzSimModule = + std::string("gz.sim") + std::to_string(GZ_SIM_MAJOR_VERSION); + const auto sdformatModule = + std::string("sdformat") + std::to_string(SDF_MAJOR_VERSION); + py::module_ sysModule; + try + { + py::module::import(gzSimModule.c_str()); + py::module::import(sdformatModule.c_str()); + sysModule = py::module_::import("sys"); + } + catch (const pybind11::error_already_set &_err) + { + gzerr << "Error while loading required modules:\n" + << _err.what() << std::endl; + return; + } + + // Add GZ_SIM_SYSTEM_PLUGIN_PATH and other default plugin + // locations to PYTHONPATH try { - py::module_ sys = py::module_::import("sys"); SystemLoader systemLoader; for (const auto &pluginPath : systemLoader.PluginPaths()) { - sys.attr("path").attr("append")(pluginPath); + sysModule.attr("path").attr("append")(pluginPath); } - - gzdbg << "Searching for python module in:\n"; - auto searchPaths = sys.attr("path"); - // TODO (azeey) Improve formatting. - gzdbg << py::str(searchPaths).cast() << std::endl; } - catch (const pybind11::error_already_set &_err) + catch (const std::runtime_error &_err) { - gzerr << "Error while loading module 'sys'\n" - << _err.what() << std::endl; + gzerr << _err.what() << std::endl; return; } @@ -68,7 +82,13 @@ void PythonSystemLoader::Configure( catch (const pybind11::error_already_set &_err) { gzerr << "Error while loading module " << moduleName << "\n" - << _err.what() << std::endl; + << _err.what() << "\n"; + + gzerr << "Searched in:\n"; + for (const auto &path : sysModule.attr("path")) + { + gzerr << " \"" << path << "\"\n"; + } return; } @@ -89,9 +109,18 @@ void PythonSystemLoader::Configure( } if (py::hasattr(this->pythonSystem, "configure")) { - sdf::ElementPtr sdfClone = _sdf->Clone(); - this->pythonSystem.attr("configure")( - _entity, sdfClone, py::cast(_ecm, py::return_value_policy::reference)); + try + { + // _ecm and _eventMgr are passed in as pointers so pybind11 would use the + // write `return_value_policy`. Otherwise, it would use + // `return_value_policy = copy`. + this->pythonSystem.attr("configure")(_entity, _sdf, &_ecm, &_eventMgr); + } + catch (const pybind11::error_already_set &_err) + { + gzerr << _err.what() << std::endl; + return; + } } // TODO(azeey) Add support for ConfigureParameters if (py::hasattr(this->pythonSystem, "pre_update")) @@ -140,34 +169,29 @@ void PythonSystemLoader::CallPythonMethod(py::object _method, Args&&... _args) void PythonSystemLoader::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) { - CallPythonMethod(this->preUpdateMethod, _info, - py::cast(_ecm, py::return_value_policy::reference)); + CallPythonMethod(this->preUpdateMethod, _info, &_ecm); } ////////////////////////////////////////////////// void PythonSystemLoader::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { - CallPythonMethod(this->updateMethod, _info, - py::cast(_ecm, py::return_value_policy::reference)); + CallPythonMethod(this->updateMethod, _info, &_ecm); } ////////////////////////////////////////////////// void PythonSystemLoader::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) { - CallPythonMethod(this->postUpdateMethod, _info, - py::cast(_ecm, py::return_value_policy::reference)); + CallPythonMethod(this->postUpdateMethod, _info, &_ecm); } ////////////////////////////////////////////////// void PythonSystemLoader::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) { - CallPythonMethod(this->resetMethod, _info, - py::cast(_ecm, py::return_value_policy::reference)); + CallPythonMethod(this->resetMethod, _info, &_ecm); } - GZ_ADD_PLUGIN(PythonSystemLoader, System, ISystemConfigure, ISystemPreUpdate, ISystemUpdate, ISystemPostUpdate, ISystemReset) GZ_ADD_PLUGIN_ALIAS(PythonSystemLoader, "gz::sim::systems::PythonSystemLoader") diff --git a/src/systems/python_system_loader/PythonSystemLoader.hh b/src/systems/python_system_loader/PythonSystemLoader.hh index 63706b301d..4a4ae51e82 100644 --- a/src/systems/python_system_loader/PythonSystemLoader.hh +++ b/src/systems/python_system_loader/PythonSystemLoader.hh @@ -21,10 +21,9 @@ #include #include -#include - #include #include +#include namespace gz { @@ -34,6 +33,17 @@ namespace sim inline namespace GZ_SIM_VERSION_NAMESPACE { namespace systems { +/// \brief Allows systems to be written in Python. +/// +/// The convention for a system written in Python supported by the +/// `PythonSystemLoader` is that it's a Python module providing a `get_system` +/// function which itself returns an instance of a class that implements the +/// various interfaces in gz::sim::System. +/// See examples/scripts/python_api/systems/test_system.py for an example +/// +/// ## Parameters +/// : Name of python module to be loaded. The search path includes +/// `GZ_SIM_SYTEM_PLUGIN_PATH` as well as `PYTHONPATH`. // TODO(azeey) Add ParameterConfigure class GZ_SIM_PYTHON_SYSTEM_LOADER_SYSTEM_HIDDEN PythonSystemLoader : public System, @@ -65,6 +75,8 @@ class GZ_SIM_PYTHON_SYSTEM_LOADER_SYSTEM_HIDDEN PythonSystemLoader public: void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) final; + /// \brief Function that calls each of the python equivalents of Configure, + /// PreUpdate, etc. private: template void CallPythonMethod(pybind11::object _method, Args&&...); From 2b7469e67487b3b05e7559455a17f4886f41fabe Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 24 Aug 2023 03:18:28 -0500 Subject: [PATCH 08/19] Add test, fix various segfaults, documentation Signed-off-by: Addisu Z. Taddese --- .../scripts/python_api/systems/test_system.py | 14 +++ python/CMakeLists.txt | 2 + python/test/gz_test_deps/msgs.py | 3 + python/test/gz_test_deps/transport.py | 1 + python/test/plugins/test_model_system.py | 75 ++++++++++++ src/CMakeLists.txt | 2 +- src/Server.cc | 22 ++++ src/ServerPrivate.hh | 7 -- src/SimulationRunner.cc | 36 ++++++ .../PythonSystemLoader.cc | 27 ++++- .../PythonSystemLoader.hh | 23 +++- test/integration/CMakeLists.txt | 10 ++ test/integration/python_system_loader.cc | 109 ++++++++++++++++++ test/worlds/python_system_loader.sdf | 90 +++++++++++++++ 14 files changed, 404 insertions(+), 17 deletions(-) create mode 100644 python/test/gz_test_deps/msgs.py create mode 100644 python/test/gz_test_deps/transport.py create mode 100755 python/test/plugins/test_model_system.py create mode 100644 test/integration/python_system_loader.cc create mode 100644 test/worlds/python_system_loader.sdf diff --git a/examples/scripts/python_api/systems/test_system.py b/examples/scripts/python_api/systems/test_system.py index cbe487f648..fe4e807919 100644 --- a/examples/scripts/python_api/systems/test_system.py +++ b/examples/scripts/python_api/systems/test_system.py @@ -1,3 +1,17 @@ +# Copyright (C) 2023 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. + from gz.math7 import Vector3d from gz.sim8 import Model, Link import random diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index cea5077127..9a72da2369 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -60,6 +60,8 @@ target_link_libraries(${BINDINGS_MODULE_NAME} PRIVATE gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} ) +set_target_properties(${BINDINGS_MODULE_NAME} PROPERTIES CXX_VISIBILITY_PRESET "hidden") + target_compile_definitions(${BINDINGS_MODULE_NAME} PRIVATE BINDINGS_MODULE_NAME=${BINDINGS_MODULE_NAME}) diff --git a/python/test/gz_test_deps/msgs.py b/python/test/gz_test_deps/msgs.py new file mode 100644 index 0000000000..e91bef7942 --- /dev/null +++ b/python/test/gz_test_deps/msgs.py @@ -0,0 +1,3 @@ +import sys +import gz.msgs10 +sys.modules["gz_test_deps.msgs"] = gz.msgs10 diff --git a/python/test/gz_test_deps/transport.py b/python/test/gz_test_deps/transport.py new file mode 100644 index 0000000000..242eeb064a --- /dev/null +++ b/python/test/gz_test_deps/transport.py @@ -0,0 +1 @@ +from gz.transport13 import * diff --git a/python/test/plugins/test_model_system.py b/python/test/plugins/test_model_system.py new file mode 100755 index 0000000000..0860d6d930 --- /dev/null +++ b/python/test/plugins/test_model_system.py @@ -0,0 +1,75 @@ +# Copyright (C) 2023 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. + +import sys +from os.path import dirname, realpath +import threading + +# Add "../__file__" to sys.path to get gz_test_deps +sys.path.append(dirname(dirname(realpath(__file__)))) + +from gz.common import set_verbosity +from gz_test_deps.sim import Model +from gz_test_deps.transport import Node +from gz_test_deps.msgs.pose_pb2 import Pose +from gz_test_deps.msgs.clock_pb2 import Clock + + +# Test system to be used with test/integration/python_system_loader.cc +class TestModelSystem(object): + def __init__(self): + self.node = Node() + self.has_been_reset = False + self.lock = threading.Lock() + self.sim_time_from_clock = None + + def configure(self, entity, sdf, ecm, event_mgr): + self.model = Model(entity) + if not self.model.valid(ecm): + raise RuntimeError(f"Model {entity} is invalid") + + self.target_pose = sdf.get_pose("target_pose") + self.reset_pose = sdf.get_pose("reset_pose") + self.pub = self.node.advertise(f"{self.model.name(ecm)}/pose", Pose) + self.sub = self.node.subscribe(Clock, "/clock", self.clock_cb) + + def pre_update(self, info, ecm): + if info.paused or self.has_been_reset: + return + + self.model.set_world_pose_cmd(ecm, self.target_pose) + + def post_update(self, info, ecm): + msg = Pose() + msg.position.x = self.target_pose.x() + msg.position.y = self.target_pose.y() + msg.position.z = self.target_pose.z() + with self.lock: + if self.sim_time_from_clock is not None: + stamp = msg.header.stamp + stamp.sec = self.sim_time_from_clock.sec + stamp.nsec = self.sim_time_from_clock.nsec + self.pub.publish(msg) + + def reset(self, info, ecm): + self.model.set_world_pose_cmd(ecm, self.reset_pose) + self.has_been_reset = True + + def clock_cb(self, msg): + with self.lock: + self.sim_time_from_clock = msg.sim + + +def get_system(): + return TestModelSystem() diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3d74614434..d95dfcdb7c 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -183,7 +183,7 @@ target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} if (pybind11_FOUND) target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE pybind11::embed) - target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE -DHAVE_PYBIND11) + target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PUBLIC -DHAVE_PYBIND11) endif() if (UNIX AND NOT APPLE) diff --git a/src/Server.cc b/src/Server.cc index 28a3d01725..70d676916c 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -17,6 +17,10 @@ #include +#ifdef HAVE_PYBIND11 +#include +#endif + #include #include #include @@ -55,6 +59,24 @@ struct DefaultWorld Server::Server(const ServerConfig &_config) : dataPtr(new ServerPrivate) { +#ifdef HAVE_PYBIND11 + if (Py_IsInitialized() == 0) + { + // We can't used pybind11::scoped_interpreter because: + // 1. It gets destructed before plugins are unloaded, which can cause + // segfaults if the plugin tries to run python code, e.g. a message + // that arrives during destruction. + // 2. It will prevent instantiation of other Servers. Running python + // systems will not be supported with multiple servers in the same + // process, but we shouldn't break existing behior for non-python use + // cases. + // This means, we will not be calling pybind11::finalize_interpreter to + // clean up the interpreter. This could cause issues with tests suites that + // have multiple tests that load python systems. + pybind11::initialize_interpreter(); + } +#endif + this->dataPtr->config = _config; // Configure the fuel client diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh index cc28f368e6..7901de184f 100644 --- a/src/ServerPrivate.hh +++ b/src/ServerPrivate.hh @@ -28,9 +28,6 @@ #include #include #include -#ifdef HAVE_PYBIND11 -#include -#endif #include #include #include @@ -151,10 +148,6 @@ namespace gz private: bool ServerControlService( const gz::msgs::ServerControl &_req, msgs::Boolean &_res); -#ifdef HAVE_PYBIND11 - public: pybind11::scoped_interpreter guard{}; -#endif - /// \brief A pool of worker threads. public: common::WorkerPool workerPool{2}; diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index f19915dcab..7699ddeb14 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -18,6 +18,9 @@ #include "SimulationRunner.hh" #include +#ifdef HAVE_PYBIND11 +#include +#endif #include #include @@ -53,6 +56,34 @@ using namespace sim; using StringSet = std::unordered_set; +namespace { +#ifdef HAVE_PYBIND11 +// Helper struct to maybe do a scoped release of the Python GIL. There are a +// number of ways where releasing and acquiring the GIL is not necessary: +// 1. Gazebo is built without Pybind11 +// 2. The python interpreter is not initialized. This could happen in tests that +// create a SimulationRunner without sim::Server where the interpreter is +// intialized. +// 3. sim::Server was instantiated by a Python module. In this case, there's a +// chance that this would be called with the GIL already released. +struct MaybeGilScopedRelease +{ + MaybeGilScopedRelease() + { + if (Py_IsInitialized() != 0 && PyGILState_Check() == 1) + { + this->release.emplace(); + } + } + std::optional release; +}; +#else + struct MaybeGilScopedRelease + { + }; +#endif +} + ////////////////////////////////////////////////// SimulationRunner::SimulationRunner(const sdf::World *_world, @@ -560,6 +591,11 @@ void SimulationRunner::UpdateSystems() // the barriers will be uninitialized, so guard against that condition. if (this->postUpdateStartBarrier && this->postUpdateStopBarrier) { + // Release the GIL from the main thread to run PostUpdate threads which + // might be calling into python. The system that does call into python + // needs to lock the GIL from its thread. + MaybeGilScopedRelease release; + // pybind11::gil_scoped_release release; this->postUpdateStartBarrier->Wait(); this->postUpdateStopBarrier->Wait(); } diff --git a/src/systems/python_system_loader/PythonSystemLoader.cc b/src/systems/python_system_loader/PythonSystemLoader.cc index 13987e28a4..a91d9fdba0 100644 --- a/src/systems/python_system_loader/PythonSystemLoader.cc +++ b/src/systems/python_system_loader/PythonSystemLoader.cc @@ -21,12 +21,28 @@ #include #include -#include +#include +#include +#include + +#include "gz/sim/SystemLoader.hh" namespace py = pybind11; namespace gz::sim::systems { + +PythonSystemLoader::~PythonSystemLoader() +{ + if (this->pythonSystem) + { + if (py::hasattr(this->pythonSystem, "shutdown")) + { + this->pythonSystem.attr("shutdown")(); + } + } +} + void PythonSystemLoader::Configure( const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, EventManager &_eventMgr) @@ -39,7 +55,7 @@ void PythonSystemLoader::Configure( return; } - // Load the `gz.sim` and sdformat modules to register all pybind bindings + // Load the `gz.sim` and sdformat modules to register all pybind bindings // necessary for System interface functions const auto gzSimModule = std::string("gz.sim") + std::to_string(GZ_SIM_MAJOR_VERSION); @@ -59,7 +75,7 @@ void PythonSystemLoader::Configure( return; } - // Add GZ_SIM_SYSTEM_PLUGIN_PATH and other default plugin + // Add GZ_SIM_SYSTEM_PLUGIN_PATH and other default plugin // locations to PYTHONPATH try { @@ -97,7 +113,8 @@ void PythonSystemLoader::Configure( py::object getSystem = this->pythonModule.attr("get_system"); if (!getSystem) { - gzerr << "Could not call 'get_system' on the module" << moduleName << "\n"; + gzerr << "Could not call 'get_system' on the module " << moduleName + << "\n"; return; } this->pythonSystem = getSystem(); @@ -160,6 +177,7 @@ void PythonSystemLoader::CallPythonMethod(py::object _method, Args&&... _args) catch (const pybind11::error_already_set &_err) { gzerr << _err.what() << std::endl; + this->validConfig = false; return; } } @@ -183,6 +201,7 @@ void PythonSystemLoader::Update(const UpdateInfo &_info, void PythonSystemLoader::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) { + py::gil_scoped_acquire gil; CallPythonMethod(this->postUpdateMethod, _info, &_ecm); } ////////////////////////////////////////////////// diff --git a/src/systems/python_system_loader/PythonSystemLoader.hh b/src/systems/python_system_loader/PythonSystemLoader.hh index 4a4ae51e82..eeb3b99b1f 100644 --- a/src/systems/python_system_loader/PythonSystemLoader.hh +++ b/src/systems/python_system_loader/PythonSystemLoader.hh @@ -20,11 +20,13 @@ #include -#include -#include -#include +#include #include +#include "gz/sim/System.hh" +#include "gz/sim/config.hh" +#include "gz/sim/python-system-loader-system/Export.hh" + namespace gz { namespace sim @@ -43,9 +45,9 @@ namespace systems /// /// ## Parameters /// : Name of python module to be loaded. The search path includes -/// `GZ_SIM_SYTEM_PLUGIN_PATH` as well as `PYTHONPATH`. +/// `GZ_SIM_SYSTEM_PLUGIN_PATH` as well as `PYTHONPATH`. // TODO(azeey) Add ParameterConfigure -class GZ_SIM_PYTHON_SYSTEM_LOADER_SYSTEM_HIDDEN PythonSystemLoader +class GZ_SIM_PYTHON_SYSTEM_LOADER_SYSTEM_HIDDEN PythonSystemLoader final : public System, public ISystemConfigure, public ISystemPreUpdate, @@ -53,6 +55,7 @@ class GZ_SIM_PYTHON_SYSTEM_LOADER_SYSTEM_HIDDEN PythonSystemLoader public ISystemPostUpdate, public ISystemReset { + public: ~PythonSystemLoader() final; // Documentation inherited public: void Configure(const Entity &_entity, const std::shared_ptr &_sdf, @@ -80,12 +83,22 @@ class GZ_SIM_PYTHON_SYSTEM_LOADER_SYSTEM_HIDDEN PythonSystemLoader private: template void CallPythonMethod(pybind11::object _method, Args&&...); + /// \brief Whether we have a valid configuration after Configure has run. This + /// includes checking if the Python module is found and that the system is + /// instantiated. This is set to false in the *Update/Reset calls if an + /// exception is thrown by the Python module. private: bool validConfig{false}; + /// \brief Represents the imported Python module private: pybind11::module_ pythonModule; + /// \brief Represents the instantiated Python System private: pybind11::object pythonSystem; + /// \brief Pointer to the PreUpdate method inside the Python System private: pybind11::object preUpdateMethod; + /// \brief Pointer to the Update method inside the Python System private: pybind11::object updateMethod; + /// \brief Pointer to the PostUpdate method inside the Python System private: pybind11::object postUpdateMethod; + /// \brief Pointer to the Reset method inside the Python System private: pybind11::object resetMethod; }; } // namespace systems diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 621d9150ce..1df2901b94 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -135,6 +135,11 @@ else() endforeach(test) endif() +# Add tests that need pybind11 +if (${pybind11_FOUND}) + list(APPEND tests python_system_loader.cc) +endif() + if (MSVC) # Warning #4251 is the "dll-interface" warning that tells you when types used # by a class are not being exported. These generated source files have private @@ -228,3 +233,8 @@ if(VALID_DISPLAY AND VALID_DRI_DISPLAY AND TARGET INTEGRATION_sensors_system) gz-rendering${GZ_RENDERING_VER}::gz-rendering${GZ_RENDERING_VER} ) endif() + +if (TARGET INTEGRATION_python_system_loader) + set_tests_properties(INTEGRATION_python_system_loader PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/python/") +endif() diff --git a/test/integration/python_system_loader.cc b/test/integration/python_system_loader.cc new file mode 100644 index 0000000000..9b20202a71 --- /dev/null +++ b/test/integration/python_system_loader.cc @@ -0,0 +1,109 @@ +/* + * Copyright (C) 2023 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. + * + */ + +#include + +#include + +#include "gz/sim/Server.hh" +#include "gz/sim/Util.hh" +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/Name.hh" +#include "helpers/EnvTestFixture.hh" +#include "plugins/MockSystem.hh" + +using namespace gz; +namespace components = gz::sim::components; + +class PythonSystemLoaderTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +void worldReset() +{ + gz::msgs::WorldControl req; + gz::msgs::Boolean rep; + req.mutable_reset()->set_all(true); + transport::Node node; + + unsigned int timeout = 1000; + bool result; + bool executed = + node.Request("/world/default/control", req, timeout, rep, result); + + ASSERT_TRUE(executed); + ASSERT_TRUE(result); + ASSERT_TRUE(rep.data()); +} + +///////////////////////////////////////////////// +TEST_F(PythonSystemLoaderTest, LoadMultipleSystems) +{ + common::setenv("GZ_SIM_SYSTEM_PLUGIN_PATH", + common::joinPaths(std::string(PROJECT_SOURCE_PATH), "python", + "test", "plugins")); + + sim::ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "python_system_loader.sdf")); + + sim::Server server(serverConfig); + using namespace std::chrono_literals; + server.SetUpdatePeriod(1ns); + + std::optional postUpdateModelPose; + // Create a system that adds a recreate component to models + auto testSystem = std::make_shared(); + testSystem->postUpdateCallback = + [&](const sim::UpdateInfo &, const sim::EntityComponentManager &_ecm) + { + auto testModel = _ecm.EntityByComponents(components::Model(), + components::Name("box")); + if (testModel != sim::kNullEntity) + { + postUpdateModelPose = sim::worldPose(testModel, _ecm); + } + return false; + }; + server.AddSystem(testSystem); + server.Run(true, 1, false); + ASSERT_TRUE(postUpdateModelPose.has_value()); + EXPECT_EQ(math::Pose3d(0, 0, 10, 0, 0, 0), *postUpdateModelPose); + + server.RunOnce(true); + std::optional afterResetModelPose; + // Since the order of reset callbacks may not be reliable, we'll use the + // following PreUpdate call to record the model pose after reset. The test + // system does not set the world pose after reset. + testSystem->preUpdateCallback = ( + [&](const sim::UpdateInfo &, const sim::EntityComponentManager &_ecm) + { + auto testModel = _ecm.EntityByComponents(components::Model(), + components::Name("box")); + if (testModel != sim::kNullEntity) + { + afterResetModelPose = sim::worldPose(testModel, _ecm); + } + return false; + }); + + worldReset(); + server.Run(true, 400, false); + ASSERT_TRUE(afterResetModelPose.has_value()); + EXPECT_NEAR(10.0, afterResetModelPose->X(), 1e-3); +} diff --git a/test/worlds/python_system_loader.sdf b/test/worlds/python_system_loader.sdf new file mode 100644 index 0000000000..504c7d4e6e --- /dev/null +++ b/test/worlds/python_system_loader.sdf @@ -0,0 +1,90 @@ + + + + + + 0 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + test_model_system + 0 0 10 0 0 0 + 10 0 0 0 0 0 + + + + + 0 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + test_model_system + 0 0 10 0 0 0 + 10 0 0 0 0 0 + + + + + From c3417d5011ac28023299ffb339e2a4946d9a1491 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 24 Aug 2023 14:29:16 -0500 Subject: [PATCH 09/19] Add to python interfaces tutorial Signed-off-by: Addisu Z. Taddese --- .../PythonSystemLoader.hh | 22 ++++++- tutorials/python_interfaces.md | 63 +++++++++++++++++++ 2 files changed, 82 insertions(+), 3 deletions(-) diff --git a/src/systems/python_system_loader/PythonSystemLoader.hh b/src/systems/python_system_loader/PythonSystemLoader.hh index eeb3b99b1f..710589e064 100644 --- a/src/systems/python_system_loader/PythonSystemLoader.hh +++ b/src/systems/python_system_loader/PythonSystemLoader.hh @@ -40,12 +40,28 @@ namespace systems /// The convention for a system written in Python supported by the /// `PythonSystemLoader` is that it's a Python module providing a `get_system` /// function which itself returns an instance of a class that implements the -/// various interfaces in gz::sim::System. +/// various interfaces in gz::sim::System. The spelling of the interfaces +/// conforms to Python code style guides, so the following name mapping applies +/// +/// * `configure` : Corresponds to System::ISystemConfigure::Configure +/// * `pre_update` : Corresponds to System::ISystemPreUpdate::PreUpdate +/// * `update` : Corresponds to System::ISystemUpdate::Update +/// * `post_update` : Corresponds to System::ISystemPostUpdate::PostUpdate +/// * `reset` : Corresponds to System::ISystemReset::Reset +/// +/// It is not necessary to implement all the interfaces. PythonSystemLoader will +/// check if the corresponding method is implemented in the Python system and +/// skip it if it's not found. +/// /// See examples/scripts/python_api/systems/test_system.py for an example /// /// ## Parameters -/// : Name of python module to be loaded. The search path includes -/// `GZ_SIM_SYSTEM_PLUGIN_PATH` as well as `PYTHONPATH`. +/// * : Name of python module to be loaded. The search path +/// includes `GZ_SIM_SYSTEM_PLUGIN_PATH` as well as +/// `PYTHONPATH`. +/// +/// The contents of the tag will be available in the configure method +/// of the Python system // TODO(azeey) Add ParameterConfigure class GZ_SIM_PYTHON_SYSTEM_LOADER_SYSTEM_HIDDEN PythonSystemLoader final : public System, diff --git a/tutorials/python_interfaces.md b/tutorials/python_interfaces.md index 8671966b0c..b9f509ba9b 100644 --- a/tutorials/python_interfaces.md +++ b/tutorials/python_interfaces.md @@ -81,3 +81,66 @@ iterations 1000 post_iterations 1000 pre_iterations 1000 ``` + +# Gazebo Systems written in Python + +Gazebo also provides a way to write systems in Python. This is done using the +`gz::sim::systems::PythonSystemLoader` system which loads a given python module +specified by its `` parameter. The search path for the module +includes `GZ_SIM_SYSTEM_PLUGIN_PATH` as well as `PYTHONPATH`. The module is +expected to provide a function called `get_system` that returns an instance of +a class that implements the various interfaces in `gz::sim::System`. + +Example python system: + + + +```python + +from gz.math7 import Vector3d +from gz.sim8 import Model, Link +import random + + +class TestSystem(object): + def __init__(self): + self.id = random.randint(1, 100) + + def configure(self, entity, sdf, ecm, event_mgr): + self.model = Model(entity) + self.link = Link(self.model.canonical_link(ecm)) + print("Configured on", entity) + print("sdf name:", sdf.get_name()) + self.force = sdf.get_double("force") + print(f"Applying {self.force} N on link {self.link.name(ecm)}") + + def pre_update(self, info, ecm): + if info.paused: + return + + if info.iterations % 3000 == 0: + print(f"{self.id} {info.real_time} pre_update") + + self.link.add_world_force( + ecm, Vector3d(0, 0, self.force), + Vector3d(random.random(), random.random(), 0)) + + +def get_system(): + return TestSystem() +``` + +The system can be added to SDFormat model or world with: + +```xml + + + test_system + 100 + + +``` + +asuming the name of the module is `test_system` and the directory containing +the module has been added to `GZ_SIM_SYSTEM_PLUGIN_PATH`, From 3d3cddcf6f143b7d4b4204945a32aac1284021d9 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 24 Aug 2023 14:36:27 -0500 Subject: [PATCH 10/19] Remove commented code, pre-commit Signed-off-by: Addisu Z. Taddese --- examples/worlds/python_system_loader.sdf | 1 - python/test/plugins/test_model_system.py | 0 src/SimulationRunner.cc | 1 - src/systems/python_system_loader/CMakeLists.txt | 2 +- test/worlds/python_system_loader.sdf | 1 - 5 files changed, 1 insertion(+), 4 deletions(-) mode change 100755 => 100644 python/test/plugins/test_model_system.py diff --git a/examples/worlds/python_system_loader.sdf b/examples/worlds/python_system_loader.sdf index f75eed899f..a483b095e2 100644 --- a/examples/worlds/python_system_loader.sdf +++ b/examples/worlds/python_system_loader.sdf @@ -104,4 +104,3 @@ your PYTHONPATH - diff --git a/python/test/plugins/test_model_system.py b/python/test/plugins/test_model_system.py old mode 100755 new mode 100644 diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 7699ddeb14..8fec74dc29 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -595,7 +595,6 @@ void SimulationRunner::UpdateSystems() // might be calling into python. The system that does call into python // needs to lock the GIL from its thread. MaybeGilScopedRelease release; - // pybind11::gil_scoped_release release; this->postUpdateStartBarrier->Wait(); this->postUpdateStopBarrier->Wait(); } diff --git a/src/systems/python_system_loader/CMakeLists.txt b/src/systems/python_system_loader/CMakeLists.txt index 2ae9645d00..212dbc080b 100644 --- a/src/systems/python_system_loader/CMakeLists.txt +++ b/src/systems/python_system_loader/CMakeLists.txt @@ -2,7 +2,7 @@ if (pybind11_FOUND) gz_add_system(python-system-loader SOURCES PythonSystemLoader.cc - PRIVATE_LINK_LIBS + PRIVATE_LINK_LIBS pybind11::embed ) endif() diff --git a/test/worlds/python_system_loader.sdf b/test/worlds/python_system_loader.sdf index 504c7d4e6e..d7c2ba1e6f 100644 --- a/test/worlds/python_system_loader.sdf +++ b/test/worlds/python_system_loader.sdf @@ -87,4 +87,3 @@ - From e5228d35afde458958a9272164a03d5d920c4593 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 24 Aug 2023 18:26:29 -0500 Subject: [PATCH 11/19] Add dependency on python3-transport13, fix tests Signed-off-by: Addisu Z. Taddese --- .github/ci/packages.apt | 1 + python/test/link_TEST.py | 7 +++---- test/integration/link.cc | 1 - 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index d3e916b0e2..5abf638e1c 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -33,6 +33,7 @@ python3-gz-math7 python3-pybind11 python3-pytest python3-sdformat14 +python3-transport13 qml-module-qt-labs-folderlistmodel qml-module-qt-labs-settings qml-module-qtgraphicaleffects diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index 961afefa84..67876a0f12 100755 --- a/python/test/link_TEST.py +++ b/python/test/link_TEST.py @@ -18,7 +18,7 @@ from gz.common import set_verbosity from gz_test_deps.sim import K_NULL_ENTITY, TestFixture, Link, Model, World, world_entity -from gz_test_deps.math import Matrix3d, Vector3d +from gz_test_deps.math import Matrix3d, Vector3d, Pose3d class TestModel(unittest.TestCase): post_iterations = 0 @@ -59,9 +59,8 @@ def on_pre_udpate_cb(_info, _ecm): # Visuals Test self.assertNotEqual(K_NULL_ENTITY, link.visual_by_name(_ecm, 'visual_test')) self.assertEqual(1, link.visual_count(_ecm)) - # World Pose Test - self.assertEqual(None, link.world_pose(_ecm)) - self.assertEqual(None, link.world_inertial_pose(_ecm)) + # World Inertial Pose Test. + self.assertEqual(Pose3d(), link.world_inertial_pose(_ecm)) # World Velocity Test self.assertEqual(None, link.world_linear_velocity(_ecm)) self.assertEqual(None, link.world_angular_velocity(_ecm)) diff --git a/test/integration/link.cc b/test/integration/link.cc index fd04e8bddb..981fd4cde4 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -229,7 +229,6 @@ TEST_F(LinkIntegrationTest, LinkPoses) // Before we add components, pose functions should return nullopt - EXPECT_EQ(std::nullopt, link.WorldPose(ecm)); EXPECT_EQ(std::nullopt, link.WorldInertialPose(ecm)); math::Pose3d linkWorldPose; From 5210ffa3f26caa33e7503dde267abb02b0008f9f Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 24 Aug 2023 21:46:16 -0500 Subject: [PATCH 12/19] Use correct package name Signed-off-by: Addisu Z. Taddese --- .github/ci/packages.apt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index 5abf638e1c..948ffdb509 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -33,7 +33,7 @@ python3-gz-math7 python3-pybind11 python3-pytest python3-sdformat14 -python3-transport13 +python3-gz-transport13 qml-module-qt-labs-folderlistmodel qml-module-qt-labs-settings qml-module-qtgraphicaleffects From 5c1c57c58e0ff4fd2019205a934bdaa5bc035b67 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 25 Aug 2023 09:38:57 -0500 Subject: [PATCH 13/19] Fix namespace issue for windows Signed-off-by: Addisu Z. Taddese --- .../python_system_loader/PythonSystemLoader.cc | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/systems/python_system_loader/PythonSystemLoader.cc b/src/systems/python_system_loader/PythonSystemLoader.cc index a91d9fdba0..8521717803 100644 --- a/src/systems/python_system_loader/PythonSystemLoader.cc +++ b/src/systems/python_system_loader/PythonSystemLoader.cc @@ -29,7 +29,11 @@ namespace py = pybind11; -namespace gz::sim::systems +namespace gz +{ +namespace sim +{ +namespace systems { PythonSystemLoader::~PythonSystemLoader() @@ -160,8 +164,8 @@ void PythonSystemLoader::Configure( } ////////////////////////////////////////////////// -template -void PythonSystemLoader::CallPythonMethod(py::object _method, Args&&... _args) +template +void PythonSystemLoader::CallPythonMethod(py::object _method, Args &&..._args) { if (!this->validConfig) { @@ -214,4 +218,6 @@ void PythonSystemLoader::Reset(const UpdateInfo &_info, GZ_ADD_PLUGIN(PythonSystemLoader, System, ISystemConfigure, ISystemPreUpdate, ISystemUpdate, ISystemPostUpdate, ISystemReset) GZ_ADD_PLUGIN_ALIAS(PythonSystemLoader, "gz::sim::systems::PythonSystemLoader") -} // namespace gz::sim::systems +} // namespace systems +} // namespace sim +} // namespace gz From f30128ff50d9453168ec88e99e3d1b41126a7bc4 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 25 Aug 2023 13:05:22 -0500 Subject: [PATCH 14/19] Address review feedback Signed-off-by: Addisu Z. Taddese --- src/ServerPrivate.hh | 1 - src/systems/python_system_loader/PythonSystemLoader.cc | 5 +++++ src/systems/python_system_loader/PythonSystemLoader.hh | 2 ++ test/integration/python_system_loader.cc | 7 +++++++ 4 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh index 7901de184f..03128c56e8 100644 --- a/src/ServerPrivate.hh +++ b/src/ServerPrivate.hh @@ -22,7 +22,6 @@ #include #include - #include #include #include diff --git a/src/systems/python_system_loader/PythonSystemLoader.cc b/src/systems/python_system_loader/PythonSystemLoader.cc index 8521717803..a4786a8778 100644 --- a/src/systems/python_system_loader/PythonSystemLoader.cc +++ b/src/systems/python_system_loader/PythonSystemLoader.cc @@ -22,9 +22,14 @@ #include #include #include +#include #include #include +#include "gz/sim/Entity.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/EventManager.hh" +#include "gz/sim/System.hh" #include "gz/sim/SystemLoader.hh" namespace py = pybind11; diff --git a/src/systems/python_system_loader/PythonSystemLoader.hh b/src/systems/python_system_loader/PythonSystemLoader.hh index 710589e064..9ba0673fd1 100644 --- a/src/systems/python_system_loader/PythonSystemLoader.hh +++ b/src/systems/python_system_loader/PythonSystemLoader.hh @@ -23,6 +23,8 @@ #include #include +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/EventManager.hh" #include "gz/sim/System.hh" #include "gz/sim/config.hh" #include "gz/sim/python-system-loader-system/Export.hh" diff --git a/test/integration/python_system_loader.cc b/test/integration/python_system_loader.cc index 9b20202a71..afd17f99b1 100644 --- a/test/integration/python_system_loader.cc +++ b/test/integration/python_system_loader.cc @@ -16,10 +16,17 @@ */ #include +#include +#include +#include +#include #include +#include +#include #include "gz/sim/Server.hh" +#include "gz/sim/ServerConfig.hh" #include "gz/sim/Util.hh" #include "gz/sim/components/Model.hh" #include "gz/sim/components/Name.hh" From 1932824f530e758545061c0e3fa835418a2cfaff Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 25 Aug 2023 17:28:37 -0500 Subject: [PATCH 15/19] Remove import Signed-off-by: Addisu Z. Taddese --- python/test/plugins/test_model_system.py | 1 - 1 file changed, 1 deletion(-) diff --git a/python/test/plugins/test_model_system.py b/python/test/plugins/test_model_system.py index 0860d6d930..d390022537 100644 --- a/python/test/plugins/test_model_system.py +++ b/python/test/plugins/test_model_system.py @@ -19,7 +19,6 @@ # Add "../__file__" to sys.path to get gz_test_deps sys.path.append(dirname(dirname(realpath(__file__)))) -from gz.common import set_verbosity from gz_test_deps.sim import Model from gz_test_deps.transport import Node from gz_test_deps.msgs.pose_pb2 import Pose From 24c3d52966e4a1514fcc6d0c30e79daf1050e754 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 28 Aug 2023 10:41:15 -0500 Subject: [PATCH 16/19] Review feedback, windows fix? Signed-off-by: Addisu Z. Taddese --- .../python_system_loader/PythonSystemLoader.cc | 13 ++++--------- tutorials/python_interfaces.md | 2 -- 2 files changed, 4 insertions(+), 11 deletions(-) diff --git a/src/systems/python_system_loader/PythonSystemLoader.cc b/src/systems/python_system_loader/PythonSystemLoader.cc index a4786a8778..d9dcdb0547 100644 --- a/src/systems/python_system_loader/PythonSystemLoader.cc +++ b/src/systems/python_system_loader/PythonSystemLoader.cc @@ -34,12 +34,10 @@ namespace py = pybind11; -namespace gz -{ -namespace sim -{ -namespace systems -{ + +using namespace gz; +using namespace sim; +using namespace systems; PythonSystemLoader::~PythonSystemLoader() { @@ -223,6 +221,3 @@ void PythonSystemLoader::Reset(const UpdateInfo &_info, GZ_ADD_PLUGIN(PythonSystemLoader, System, ISystemConfigure, ISystemPreUpdate, ISystemUpdate, ISystemPostUpdate, ISystemReset) GZ_ADD_PLUGIN_ALIAS(PythonSystemLoader, "gz::sim::systems::PythonSystemLoader") -} // namespace systems -} // namespace sim -} // namespace gz diff --git a/tutorials/python_interfaces.md b/tutorials/python_interfaces.md index 854dd72173..ad0e0d8ddc 100644 --- a/tutorials/python_interfaces.md +++ b/tutorials/python_interfaces.md @@ -96,7 +96,6 @@ Example python system: ```python - from gz.math7 import Vector3d from gz.sim8 import Model, Link import random @@ -133,7 +132,6 @@ def get_system(): The system can be added to SDFormat model or world with: ```xml - test_system From 053019813bb7b658988025a45348b4201be4a29c Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 28 Aug 2023 10:43:21 -0500 Subject: [PATCH 17/19] Remove whitespace Signed-off-by: Addisu Z. Taddese --- python/test/link_TEST.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index b68b18547a..35071869e1 100755 --- a/python/test/link_TEST.py +++ b/python/test/link_TEST.py @@ -59,7 +59,7 @@ def on_pre_udpate_cb(_info, _ecm): # Visuals Test self.assertNotEqual(K_NULL_ENTITY, link.visual_by_name(_ecm, 'visual_test')) self.assertEqual(1, link.visual_count(_ecm)) - # World Inertial Pose Test. + # World Inertial Pose Test. self.assertEqual(Pose3d(), link.world_inertial_pose(_ecm)) # World Velocity Test self.assertEqual(None, link.world_linear_velocity(_ecm)) From 72f85dc22888585c25e56fa6434399831f09d5c8 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 28 Aug 2023 12:45:56 -0500 Subject: [PATCH 18/19] Add python3-gz-msgs10 Signed-off-by: Addisu Z. Taddese --- .github/ci/packages.apt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index 948ffdb509..96a302fe1f 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -30,10 +30,11 @@ libxi-dev libxmu-dev python3-distutils python3-gz-math7 +python3-gz-msgs10 +python3-gz-transport13 python3-pybind11 python3-pytest python3-sdformat14 -python3-gz-transport13 qml-module-qt-labs-folderlistmodel qml-module-qt-labs-settings qml-module-qtgraphicaleffects From b46c6a70bf051da48668c2087e6ba41b78b11369 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 28 Aug 2023 14:05:53 -0500 Subject: [PATCH 19/19] Disable test on windows Signed-off-by: Addisu Z. Taddese --- test/integration/python_system_loader.cc | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/test/integration/python_system_loader.cc b/test/integration/python_system_loader.cc index afd17f99b1..baeed3ff33 100644 --- a/test/integration/python_system_loader.cc +++ b/test/integration/python_system_loader.cc @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -59,7 +60,8 @@ void worldReset() } ///////////////////////////////////////////////// -TEST_F(PythonSystemLoaderTest, LoadMultipleSystems) +TEST_F(PythonSystemLoaderTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(LoadMultipleSystems)) { common::setenv("GZ_SIM_SYSTEM_PLUGIN_PATH", common::joinPaths(std::string(PROJECT_SOURCE_PATH), "python",