diff --git a/.github/ci/packages-focal.apt b/.github/ci/packages-focal.apt
index 50cee478b6..fe94da46f7 100644
--- a/.github/ci/packages-focal.apt
+++ b/.github/ci/packages-focal.apt
@@ -3,3 +3,4 @@ libdart-dev
libdart-external-ikfast-dev
libdart-external-odelcpsolver-dev
libdart-utils-urdf-dev
+python3-ignition-math6
diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt
index 4a15aa2ddf..5308887a5e 100644
--- a/.github/ci/packages.apt
+++ b/.github/ci/packages.apt
@@ -22,6 +22,8 @@ libsdformat12-dev
libtinyxml2-dev
libxi-dev
libxmu-dev
+python3-distutils
+python3-pybind11
qml-module-qt-labs-folderlistmodel
qml-module-qt-labs-settings
qml-module-qtqml-models2
diff --git a/CMakeLists.txt b/CMakeLists.txt
index eaf570f512..df48d9915a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
#============================================================================
# Initialize the project
#============================================================================
-project(ignition-gazebo6 VERSION 6.5.0)
+project(ignition-gazebo6 VERSION 6.7.0)
#============================================================================
# Find ignition-cmake
@@ -15,6 +15,7 @@ find_package(ignition-cmake2 2.8.0 REQUIRED)
# Configure the project
#============================================================================
ign_configure_project(VERSION_SUFFIX)
+set (CMAKE_CXX_STANDARD 17)
#============================================================================
# Set project-specific options
@@ -37,6 +38,14 @@ endif()
include(test/find_dri.cmake)
FindDRI()
+option(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION
+ "Install python modules in standard system paths in the system"
+ OFF)
+
+option(USE_DIST_PACKAGES_FOR_PYTHON
+ "Use dist-packages instead of site-package to install python modules"
+ OFF)
+
#============================================================================
# Search for project-specific dependencies
#============================================================================
@@ -167,6 +176,26 @@ ign_find_package(IgnProtobuf
PRETTY Protobuf)
set(Protobuf_IMPORT_DIRS ${ignition-msgs8_INCLUDE_DIRS})
+#--------------------------------------
+# Find python
+include(IgnPython)
+find_package(PythonLibs QUIET)
+if (NOT PYTHONLIBS_FOUND)
+ IGN_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(pybind11 2.2 QUIET)
+
+ if (${pybind11_FOUND})
+ message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.")
+ else()
+ IGN_BUILD_WARNING("pybind11 is missing: Python interfaces are disabled.")
+ message (STATUS "Searching for pybind11 - not found.")
+ endif()
+endif()
# Plugin install dirs
set(IGNITION_GAZEBO_PLUGIN_INSTALL_DIR
${CMAKE_INSTALL_PREFIX}/${IGN_LIB_INSTALL_DIR}/ign-${IGN_DESIGNATION}-${PROJECT_VERSION_MAJOR}/plugins
@@ -187,6 +216,9 @@ add_subdirectory(examples)
#============================================================================
ign_create_packages()
+if (${pybind11_FOUND})
+ add_subdirectory(python)
+endif()
#============================================================================
# Configure documentation
#============================================================================
diff --git a/Changelog.md b/Changelog.md
index 36bfd47980..f69e217cfe 100644
--- a/Changelog.md
+++ b/Changelog.md
@@ -1,8 +1,65 @@
## Ignition Gazebo 6.x
-### Ignition Gazebo 6.X.X (202X-XX-XX)
+### Ignition Gazebo 6.7.0 (2022-02-24)
+
+1. Added Python interfaces to some Ignition Gazebo methods
+ * [Pull request #1219](https://github.com/ignitionrobotics/ign-gazebo/pull/1219)
+
+1. Use pose multiplication instead of addition
+ * [Pull request #1369](https://github.com/ignitionrobotics/ign-gazebo/pull/1369)
+
+1. Disables Failing Buoyancy Tests on Win32
+ * [Pull request #1368](https://github.com/ignitionrobotics/ign-gazebo/pull/1368)
+
+1. Extend ShaderParam system to support loading different shader languages
+ * [Pull request #1335](https://github.com/ignitionrobotics/ign-gazebo/pull/1335)
+
+1. Populate names of colliding entities in contact points message
+ * [Pull request #1351](https://github.com/ignitionrobotics/ign-gazebo/pull/1351)
+
+1. Refactor System functionality into SystemManager
+ * [Pull request #1340](https://github.com/ignitionrobotics/ign-gazebo/pull/1340)
+
+1. GzSceneManager: Prevent crash boom when inserted from menu
+ * [Pull request #1371](https://github.com/ignitionrobotics/ign-gazebo/pull/1371)
+
+### Ignition Gazebo 6.6.0 (2022-02-24)
+
+1. Fix accessing empty JointPosition component in lift drag plugin
+ * [Pull request #1366](https://github.com/ignitionrobotics/ign-gazebo/pull/1366)
+
+1. Add parameter to TrajectoryFollower stop rotation when bearing is reached
+ * [Pull request #1349](https://github.com/ignitionrobotics/ign-gazebo/pull/1349)
+
+1. Support disabling pose publisher from publishing top level model pose
+ * [Pull request #1342](https://github.com/ignitionrobotics/ign-gazebo/pull/1342)
+
+1. Added more sensor properties to scene/info topic
+ * [Pull request #1344](https://github.com/ignitionrobotics/ign-gazebo/pull/1344)
+
+1. Adding ability to pause/resume the trajectory follower behavior.
+ * [Pull request #1347](https://github.com/ignitionrobotics/ign-gazebo/pull/1347)
+
+1. Logs a warning if a mode is not clearly sepecified.
+ * [Pull request #1307](https://github.com/ignitionrobotics/ign-gazebo/pull/1307)
+
+1. JointStatePublisher publish parent, child and axis data
+ * [Pull request #1345](https://github.com/ignitionrobotics/ign-gazebo/pull/1345)
+
+1. Fixed light gui component inspector
+ * [Pull request #1337](https://github.com/ignitionrobotics/ign-gazebo/pull/1337)
+
+1. Fix UNIT_SdfGenerator_TEST
+ * [Pull request #1319](https://github.com/ignitionrobotics/ign-gazebo/pull/1319)
+
+1. Add elevator system
+ * [Pull request #535](https://github.com/ignitionrobotics/ign-gazebo/pull/535)
+
+1. Removed unused variables in shapes plugin
+ * [Pull request #1321](https://github.com/ignitionrobotics/ign-gazebo/pull/1321)
+
+### Ignition Gazebo 6.5.0 (2022-02-15)
-### Ignition Gazebo 6.5.0 (202X-XX-XX)
1. New trajectory follower system
* [Pull request #1332](https://github.com/ignitionrobotics/ign-gazebo/pull/1332)
diff --git a/examples/scripts/python_api/README.md b/examples/scripts/python_api/README.md
new file mode 100644
index 0000000000..a828853737
--- /dev/null
+++ b/examples/scripts/python_api/README.md
@@ -0,0 +1,6 @@
+# Gazebo Python API
+
+This example shows how to use Gazebo's Python API.
+
+For more information, see the
+[Python interfaces](https://ignitionrobotics.org/api/gazebo/6.4/python_interfaces.html) tutorial.
diff --git a/examples/scripts/python_api/gravity.sdf b/examples/scripts/python_api/gravity.sdf
new file mode 100644
index 0000000000..ee1161fa5f
--- /dev/null
+++ b/examples/scripts/python_api/gravity.sdf
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+ 0.4
+ 0.4
+ 0.4
+
+ 1.0
+
+
+
+
+
diff --git a/examples/scripts/python_api/testFixture.py b/examples/scripts/python_api/testFixture.py
new file mode 100644
index 0000000000..caedebdc84
--- /dev/null
+++ b/examples/scripts/python_api/testFixture.py
@@ -0,0 +1,85 @@
+#!/usr/bin/python3
+# 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.
+
+# If you compiled Ignition Gazebo from source you should modify your
+# `PYTHONPATH`:
+#
+# export PYTHONPATH=$PYTHONPATH:/install/lib/python
+#
+# Now you can run the example:
+#
+# python3 examples/scripts/python_api/helperFixture.py
+
+import os
+import time
+
+from ignition.common import set_verbosity
+from ignition.gazebo import TestFixture, World, world_entity
+from ignition.math import Vector3d
+from sdformat import Element
+
+set_verbosity(4)
+
+file_path = os.path.dirname(os.path.realpath(__file__))
+
+helper = TestFixture(os.path.join(file_path, 'gravity.sdf'))
+
+post_iterations = 0
+iterations = 0
+pre_iterations = 0
+first_iteration = True
+
+
+def on_pre_udpate_cb(_info, _ecm):
+ global pre_iterations
+ global first_iteration
+ pre_iterations += 1
+ if first_iteration:
+ first_iteration = False
+ world_e = world_entity(_ecm);
+ print('World entity is ', world_e)
+ w = World(world_e)
+ v = w.gravity(_ecm)
+ print('Gravity ', v)
+ modelEntity = w.model_by_name(_ecm, 'falling')
+ print('Entity for falling model is: ', modelEntity)
+
+
+def on_udpate_cb(_info, _ecm):
+ global iterations
+ iterations += 1
+
+
+def on_post_udpate_cb(_info, _ecm):
+ global post_iterations
+ post_iterations += 1
+ if _info.sim_time.seconds == 1:
+ print('Post update sim time: ', _info.sim_time)
+
+
+helper.on_post_update(on_post_udpate_cb)
+helper.on_update(on_udpate_cb)
+helper.on_pre_update(on_pre_udpate_cb)
+helper.finalize()
+
+server = helper.server()
+server.run(False, 1000, False)
+
+while(server.is_running()):
+ time.sleep(0.1)
+
+print('iterations ', iterations)
+print('post_iterations ', post_iterations)
+print('pre_iterations ', pre_iterations)
diff --git a/examples/worlds/empty_gui.sdf b/examples/worlds/empty_gui.sdf
new file mode 100644
index 0000000000..84b5e172c4
--- /dev/null
+++ b/examples/worlds/empty_gui.sdf
@@ -0,0 +1,210 @@
+
+
+
+
+
+
+
+
+
+
+ 3D View
+ false
+ docked
+
+
+ ogre2
+ scene
+ 0.4 0.4 0.4
+ 0.8 0.8 0.8
+ -6 0 6 0 0.5 0
+
+
+
+
+ 1.0 1.0 1.0
+ 0.8 0.8 0.8
+
+
+
+
+ true
+ 0 0 10 0 0 0
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+ 0.0 0.6 -0.9
+
+
+
+ true
+
+
+
+
+ 0 0 1
+
+
+
+
+
+
+ 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
+
+
+
+
+
+
+ 0 0 0.5 0 0 0
+
+
+
+ 1
+ 0
+ 0
+ 1
+ 0
+ 1
+
+ 1.0
+
+
+
+
+ 1 1 1
+
+
+
+
+
+
+
+ 1 1 1
+
+
+
+ 1 0 0 1
+ 1 0 0 1
+ 1 0 0 1
+
+
+
+
+
+
+ 0 -1.5 0.5 0 0 0
+
+
+
+ 2
+ 0
+ 0
+ 2
+ 0
+ 2
+
+ 2.0
+
+
+
+
+ 0.5
+ 1.0
+
+
+
+
+
+
+
+ 0.5
+ 1.0
+
+
+
+ 0 1 0 1
+ 0 1 0 1
+ 0 1 0 1
+
+
+
+
+
+
+ 0 1.5 0.5 0 0 0
+
+
+
+ 3
+ 0
+ 0
+ 3
+ 0
+ 3
+
+ 3.0
+
+
+
+
+ 0.5
+
+
+
+
+
+
+
+ 0.5
+
+
+
+ 0 0 1 1
+ 0 0 1 1
+ 0 0 1 1
+
+
+
+
+
+
+
diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh
index 9dd3d5ccc7..df967d30ca 100644
--- a/include/ignition/gazebo/Link.hh
+++ b/include/ignition/gazebo/Link.hh
@@ -227,6 +227,14 @@ namespace ignition
public: void SetLinearVelocity(EntityComponentManager &_ecm,
const math::Vector3d &_vel) const;
+
+ /// \brief Set the angular velocity on this link. If this is set, wrenches
+ /// on this link will be ignored for the current time step.
+ /// \param[in] _ecm Entity Component manager.
+ /// \param[in] _vel Angular velocity to set in Link's Frame.
+ public: void SetAngularVelocity(EntityComponentManager &_ecm,
+ const math::Vector3d &_vel) const;
+
/// \brief Get the angular acceleration of the body in the world frame.
/// \param[in] _ecm Entity-component manager.
/// \return Angular acceleration of the body in the world frame or
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
new file mode 100644
index 0000000000..bd08e206db
--- /dev/null
+++ b/python/CMakeLists.txt
@@ -0,0 +1,99 @@
+if(WIN32 AND CMAKE_BUILD_TYPE STREQUAL "Debug")
+ # pybind11 logic for setting up a debug build when both a debug and release
+ # python interpreter are present in the system seems to be pretty much broken.
+ # This works around the issue.
+ set(PYTHON_LIBRARIES "${PYTHON_DEBUG_LIBRARIES}")
+endif()
+
+
+if(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION)
+ if(${CMAKE_VERSION} VERSION_LESS "3.12.0")
+ execute_process(
+ COMMAND "${PYTHON_EXECUTABLE}" -c "if True:
+ from distutils import sysconfig as sc
+ print(sc.get_python_lib(plat_specific=True))"
+ OUTPUT_VARIABLE Python3_SITEARCH
+ OUTPUT_STRIP_TRAILING_WHITESPACE)
+ else()
+ # Get install variable from Python3 module
+ # Python3_SITEARCH is available from 3.12 on, workaround if needed:
+ find_package(Python3 COMPONENTS Interpreter)
+ endif()
+
+ if(USE_DIST_PACKAGES_FOR_PYTHON)
+ string(REPLACE "site-packages" "dist-packages" IGN_PYTHON_INSTALL_PATH ${Python3_SITEARCH})
+ endif()
+else()
+ # If not a system installation, respect local paths
+ set(IGN_PYTHON_INSTALL_PATH ${IGN_LIB_INSTALL_DIR}/python)
+endif()
+
+# Set the build location and install location for a CPython extension
+function(configure_build_install_location _library_name)
+ # Install library for actual use
+ install(TARGETS ${_library_name}
+ DESTINATION "${IGN_PYTHON_INSTALL_PATH}/ignition"
+ )
+endfunction()
+
+pybind11_add_module(gazebo SHARED
+ src/ignition/gazebo/_ignition_gazebo_pybind11.cc
+ src/ignition/gazebo/EntityComponentManager.cc
+ src/ignition/gazebo/EventManager.cc
+ src/ignition/gazebo/TestFixture.cc
+ src/ignition/gazebo/Server.cc
+ src/ignition/gazebo/ServerConfig.cc
+ src/ignition/gazebo/UpdateInfo.cc
+ src/ignition/gazebo/Util.cc
+ src/ignition/gazebo/World.cc
+)
+
+target_link_libraries(gazebo PRIVATE
+ ${PROJECT_LIBRARY_TARGET_NAME}
+ sdformat${SDF_VER}::sdformat${SDF_VER}
+ ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+)
+
+# TODO(ahcorde): Move this module to ign-common
+pybind11_add_module(common SHARED
+ src/ignition/common/_ignition_common_pybind11.cc
+ src/ignition/common/Console.cc
+)
+
+target_link_libraries(common PRIVATE
+ ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+)
+
+# TODO(ahcorde): Move this module to sdformat
+pybind11_add_module(sdformat SHARED
+ src/ignition/sdformat/_sdformat_pybind11.cc
+ src/ignition/sdformat/Element.cc
+)
+
+target_link_libraries(sdformat PRIVATE
+ sdformat${SDF_VER}::sdformat${SDF_VER}
+)
+
+install(TARGETS sdformat
+ DESTINATION "${IGN_PYTHON_INSTALL_PATH}"
+)
+
+configure_build_install_location(gazebo)
+configure_build_install_location(common)
+
+if (BUILD_TESTING)
+ set(python_tests
+ testFixture_TEST
+ )
+
+ foreach (test ${python_tests})
+ add_test(NAME ${test}.py COMMAND
+ "${PYTHON_EXECUTABLE}" "${CMAKE_SOURCE_DIR}/python/test/${test}.py")
+
+ set(_env_vars)
+ list(APPEND _env_vars "PYTHONPATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/python/")
+ list(APPEND _env_vars "LD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}:$ENV{LD_LIBRARY_PATH}")
+ set_tests_properties(${test}.py PROPERTIES
+ ENVIRONMENT "${_env_vars}")
+ endforeach()
+endif()
diff --git a/python/src/ignition/common/Console.cc b/python/src/ignition/common/Console.cc
new file mode 100644
index 0000000000..8d904a4487
--- /dev/null
+++ b/python/src/ignition/common/Console.cc
@@ -0,0 +1,31 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// 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 "Console.hh"
+
+namespace ignition
+{
+ namespace common
+ {
+ namespace python
+ {
+ void SetVerbosity(int _verbosity)
+ {
+ ignition::common::Console::SetVerbosity(_verbosity);
+ }
+ }
+ }
+}
diff --git a/python/src/ignition/common/Console.hh b/python/src/ignition/common/Console.hh
new file mode 100644
index 0000000000..4b84fbf514
--- /dev/null
+++ b/python/src/ignition/common/Console.hh
@@ -0,0 +1,31 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// 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 IGNITION_GAZEBO_PYTHON__CONSOLE_HH_
+#define IGNITION_GAZEBO_PYTHON__CONSOLE_HH_
+
+#include
+
+namespace ignition
+{
+ namespace common
+ {
+ namespace python
+ {
+ void SetVerbosity(int _verbosity);
+ }
+ }
+}
+
+#endif
diff --git a/python/src/ignition/common/_ignition_common_pybind11.cc b/python/src/ignition/common/_ignition_common_pybind11.cc
new file mode 100644
index 0000000000..12a67670fc
--- /dev/null
+++ b/python/src/ignition/common/_ignition_common_pybind11.cc
@@ -0,0 +1,25 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// 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 "Console.hh"
+
+PYBIND11_MODULE(common, m) {
+ m.doc() = "Ignition Common Python Library.";
+
+ m.def(
+ "set_verbosity", &ignition::common::python::SetVerbosity,
+ "Set verbosity level.");
+}
diff --git a/python/src/ignition/gazebo/EntityComponentManager.cc b/python/src/ignition/gazebo/EntityComponentManager.cc
new file mode 100644
index 0000000000..1786ba387c
--- /dev/null
+++ b/python/src/ignition/gazebo/EntityComponentManager.cc
@@ -0,0 +1,35 @@
+/*
+ * 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.
+ */
+#include
+
+#include "EntityComponentManager.hh"
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+/////////////////////////////////////////////////
+void defineGazeboEntityComponentManager(pybind11::object module)
+{
+ pybind11::class_(
+ module, "EntityComponentManager")
+ .def(pybind11::init<>());
+}
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
diff --git a/python/src/ignition/gazebo/EntityComponentManager.hh b/python/src/ignition/gazebo/EntityComponentManager.hh
new file mode 100644
index 0000000000..b982483631
--- /dev/null
+++ b/python/src/ignition/gazebo/EntityComponentManager.hh
@@ -0,0 +1,40 @@
+/*
+ * 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 IGNITION_GAZEBO_PYTHON__ENTITY_COMPONENT_MANAGER_HH_
+#define IGNITION_GAZEBO_PYTHON__ENTITY_COMPONENT_MANAGER_HH_
+
+#include
+
+#include "ignition/gazebo/EntityComponentManager.hh"
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+/// Define a pybind11 wrapper for an ignition::gazebo::EntityComponentManager
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void
+defineGazeboEntityComponentManager(pybind11::object module);
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
+
+#endif // IGNITION_GAZEBO_PYTHON__ENTITY_COMPONENT_MANAGER_HH_
diff --git a/python/src/ignition/gazebo/EventManager.cc b/python/src/ignition/gazebo/EventManager.cc
new file mode 100644
index 0000000000..39ecbfb8b5
--- /dev/null
+++ b/python/src/ignition/gazebo/EventManager.cc
@@ -0,0 +1,37 @@
+/*
+ * 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.
+ */
+
+#include
+
+#include "ignition/gazebo/EventManager.hh"
+
+#include "EventManager.hh"
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+/////////////////////////////////////////////////
+void defineGazeboEventManager(pybind11::object module)
+{
+ pybind11::class_(module, "EventManager")
+ .def(pybind11::init<>());
+}
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
diff --git a/python/src/ignition/gazebo/EventManager.hh b/python/src/ignition/gazebo/EventManager.hh
new file mode 100644
index 0000000000..bcd177edcb
--- /dev/null
+++ b/python/src/ignition/gazebo/EventManager.hh
@@ -0,0 +1,39 @@
+/*
+ * 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 IGNITION_GAZEBO_PYTHON__EVENT_MANAGER_HH_
+#define IGNITION_GAZEBO_PYTHON__EVENT_MANAGER_HH_
+
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+/// Define a pybind11 wrapper for an ignition::gazebo::EventManager
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void
+defineGazeboEventManager(pybind11::object module);
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
+
+#endif // IGNITION_GAZEBO_PYTHON__EVENT_MANAGER_HH_
diff --git a/python/src/ignition/gazebo/Server.cc b/python/src/ignition/gazebo/Server.cc
new file mode 100644
index 0000000000..917560a8ae
--- /dev/null
+++ b/python/src/ignition/gazebo/Server.cc
@@ -0,0 +1,52 @@
+/*
+ * 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.
+ */
+
+
+#include
+
+#include
+#include
+
+#include "Server.hh"
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+void defineGazeboServer(pybind11::object module)
+{
+ pybind11::class_>(module, "Server")
+ .def(pybind11::init())
+ .def(
+ "run", &ignition::gazebo::Server::Run,
+ "Run the server. By default this is a non-blocking call, "
+ " which means the server runs simulation in a separate thread. Pass "
+ " in true to the _blocking argument to run the server in the current "
+ " thread.")
+ .def(
+ "has_entity", &ignition::gazebo::Server::HasEntity,
+ "Return true if the specified world has an entity with the provided name.")
+ .def(
+ "is_running",
+ pybind11::overload_cast<>(&ignition::gazebo::Server::Running, pybind11::const_),
+ "Get whether the server is running.");
+}
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
diff --git a/python/src/ignition/gazebo/Server.hh b/python/src/ignition/gazebo/Server.hh
new file mode 100644
index 0000000000..6e969a035b
--- /dev/null
+++ b/python/src/ignition/gazebo/Server.hh
@@ -0,0 +1,39 @@
+/*
+ * 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 IGNITION_GAZEBO_PYTHON__SERVER_HH_
+#define IGNITION_GAZEBO_PYTHON__SERVER_HH_
+
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+/// Define a pybind11 wrapper for an ignition::gazebo::Server
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void
+defineGazeboServer(pybind11::object module);
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
+
+#endif // IGNITION_GAZEBO_PYTHON__SERVER_HH_
diff --git a/python/src/ignition/gazebo/ServerConfig.cc b/python/src/ignition/gazebo/ServerConfig.cc
new file mode 100644
index 0000000000..d8b650aff1
--- /dev/null
+++ b/python/src/ignition/gazebo/ServerConfig.cc
@@ -0,0 +1,40 @@
+/*
+ * 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.
+ */
+
+
+#include
+
+#include
+
+#include "ServerConfig.hh"
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+void defineGazeboServerConfig(pybind11::object module)
+{
+ pybind11::class_(module, "ServerConfig")
+ .def(pybind11::init<>())
+ .def(
+ "set_sdf_file", &ignition::gazebo::ServerConfig::SetSdfFile,
+ "Set an SDF file to be used with the server.");
+}
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
diff --git a/python/src/ignition/gazebo/ServerConfig.hh b/python/src/ignition/gazebo/ServerConfig.hh
new file mode 100644
index 0000000000..d955bd5f49
--- /dev/null
+++ b/python/src/ignition/gazebo/ServerConfig.hh
@@ -0,0 +1,39 @@
+/*
+ * 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 IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HH_
+#define IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HH_
+
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+/// Define a pybind11 wrapper for an ignition::gazebo::ServerConfig
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void
+defineGazeboServerConfig(pybind11::object module);
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
+
+#endif // IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HH_
diff --git a/python/src/ignition/gazebo/TestFixture.cc b/python/src/ignition/gazebo/TestFixture.cc
new file mode 100644
index 0000000000..7be546716a
--- /dev/null
+++ b/python/src/ignition/gazebo/TestFixture.cc
@@ -0,0 +1,103 @@
+/*
+ * 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.
+ *
+*/
+#include
+#include
+
+#include "TestFixture.hh"
+
+#include "ignition/gazebo/TestFixture.hh"
+
+#include "wrap_functions.hh"
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+void
+defineGazeboTestFixture(pybind11::object module)
+{
+ pybind11::class_> testFixture(module, "TestFixture");
+
+ testFixture
+ .def(pybind11::init())
+ .def(
+ "server", &TestFixture::Server,
+ pybind11::return_value_policy::reference,
+ "Get pointer to underlying server."
+ )
+ .def(
+ "finalize", &TestFixture::Finalize,
+ pybind11::return_value_policy::reference,
+ "Finalize all the functions and add fixture to server."
+ )
+ .def(
+ "on_pre_update", WrapCallbacks(
+ [](TestFixture* self, std::function _cb)
+ {
+ self->OnPreUpdate(_cb);
+ }
+ ),
+ pybind11::return_value_policy::reference,
+ "Wrapper around a system's pre-update callback"
+ )
+ .def(
+ "on_update", WrapCallbacks(
+ [](TestFixture* self, std::function _cb)
+ {
+ self->OnUpdate(_cb);
+ }
+ ),
+ pybind11::return_value_policy::reference,
+ "Wrapper around a system's update callback"
+ )
+ .def(
+ "on_post_update", WrapCallbacks(
+ [](TestFixture* self, std::function _cb)
+ {
+ self->OnPostUpdate(_cb);
+ }
+ ),
+ pybind11::return_value_policy::reference,
+ "Wrapper around a system's post-update callback"
+ );
+ // TODO(ahcorde): This method is not compiling for the following reason:
+ // The EventManager class has an unordered_map which holds a unique_ptr
+ // This make the class uncopyable, anyhow we should not copy the class
+ // we just need the reference. Not really sure about what's going on here
+ // .def(
+ // "on_configure", WrapCallbacks(
+ // [](TestFixture* self, std::function &_sdf,
+ // EntityComponentManager &_ecm,
+ // EventManager &_eventMgr)> _cb)
+ // {
+ // self->OnConfigure(_cb);
+ // }
+ // ),
+ // pybind11::return_value_policy::reference,
+ // "Wrapper around a system's configure callback"
+ // );
+}
+}
+}
+}
diff --git a/python/src/ignition/gazebo/TestFixture.hh b/python/src/ignition/gazebo/TestFixture.hh
new file mode 100644
index 0000000000..b355521943
--- /dev/null
+++ b/python/src/ignition/gazebo/TestFixture.hh
@@ -0,0 +1,38 @@
+/*
+ * 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 IGNITION_GAZEBO_PYTHON__TEST_FIXTURE_HH_
+#define IGNITION_GAZEBO_PYTHON__TEST_FIXTURE_HH_
+
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+/// Define a pybind11 wrapper for an ignition::gazebo::TestFixture
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void
+defineGazeboTestFixture(pybind11::object module);
+}
+}
+}
+
+#endif // IGNITION_GAZEBO_PYTHON__TEST_FIXTURE_HH_
diff --git a/python/src/ignition/gazebo/UpdateInfo.cc b/python/src/ignition/gazebo/UpdateInfo.cc
new file mode 100644
index 0000000000..e31dc7cb92
--- /dev/null
+++ b/python/src/ignition/gazebo/UpdateInfo.cc
@@ -0,0 +1,43 @@
+/*
+ * 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.
+ */
+
+
+#include
+#include
+
+#include
+
+#include "UpdateInfo.hh"
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+void defineGazeboUpdateInfo(pybind11::object module)
+{
+ pybind11::class_(module, "UpdateInfo")
+ .def(pybind11::init<>())
+ .def_readwrite("sim_time", &ignition::gazebo::UpdateInfo::simTime)
+ .def_readwrite("real_time", &ignition::gazebo::UpdateInfo::realTime)
+ .def_readwrite("dt", &ignition::gazebo::UpdateInfo::dt)
+ .def_readwrite("paused", &ignition::gazebo::UpdateInfo::paused)
+ .def_readwrite("iterations", &ignition::gazebo::UpdateInfo::iterations);
+}
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
diff --git a/python/src/ignition/gazebo/UpdateInfo.hh b/python/src/ignition/gazebo/UpdateInfo.hh
new file mode 100644
index 0000000000..f2ec4e910d
--- /dev/null
+++ b/python/src/ignition/gazebo/UpdateInfo.hh
@@ -0,0 +1,39 @@
+/*
+ * 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 IGNITION_GAZEBO_PYTHON__UPDATE_INFO_HH_
+#define IGNITION_GAZEBO_PYTHON__UPDATE_INFO_HH_
+
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+/// Define a pybind11 wrapper for an ignition::gazebo::UpdateInfo
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void
+defineGazeboUpdateInfo(pybind11::object module);
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
+
+#endif // IGNITION_GAZEBO_PYTHON__UPDATE_INFO_HH_
diff --git a/python/src/ignition/gazebo/Util.cc b/python/src/ignition/gazebo/Util.cc
new file mode 100644
index 0000000000..3dd225f8a7
--- /dev/null
+++ b/python/src/ignition/gazebo/Util.cc
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) 2022 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
+
+#include
+#include "Util.hh"
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+void defineGazeboUtil(pybind11::module &_module)
+{
+ _module.def("world_entity",
+ pybind11::overload_cast(
+ &ignition::gazebo::worldEntity),
+ "Get the first world entity that's found.");
+}
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
diff --git a/python/src/ignition/gazebo/Util.hh b/python/src/ignition/gazebo/Util.hh
new file mode 100644
index 0000000000..34db116688
--- /dev/null
+++ b/python/src/ignition/gazebo/Util.hh
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2022 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 IGNITION_GAZEBO_PYTHON__UTIL_HH_
+#define IGNITION_GAZEBO_PYTHON__UTIL_HH_
+
+#include
+
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+/// Define a pybind11 wrapper for a ignition::gazebo::Util
+/// \param[in] _module a pybind11 module to add the definition to
+void defineGazeboUtil(pybind11::module &_module);
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
+
+#endif // IGNITION_GAZEBO_PYTHON__WORLD_HH_
diff --git a/python/src/ignition/gazebo/World.cc b/python/src/ignition/gazebo/World.cc
new file mode 100644
index 0000000000..6822c12f8f
--- /dev/null
+++ b/python/src/ignition/gazebo/World.cc
@@ -0,0 +1,45 @@
+/*
+ * 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.
+ */
+
+
+#include
+#include
+
+#include
+
+#include "World.hh"
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+void defineGazeboWorld(pybind11::object module)
+{
+ pybind11::class_(module, "World")
+ .def(pybind11::init())
+ .def(
+ "model_by_name", &ignition::gazebo::World::ModelByName,
+ "Get the ID of a model entity which is an immediate child of "
+ " this world.")
+ .def(
+ "gravity", &ignition::gazebo::World::Gravity,
+ "Get the gravity in m/s^2.");
+}
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
diff --git a/python/src/ignition/gazebo/World.hh b/python/src/ignition/gazebo/World.hh
new file mode 100644
index 0000000000..e77e42b4b6
--- /dev/null
+++ b/python/src/ignition/gazebo/World.hh
@@ -0,0 +1,40 @@
+/*
+ * 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 IGNITION_GAZEBO_PYTHON__WORLD_HH_
+#define IGNITION_GAZEBO_PYTHON__WORLD_HH_
+
+#include
+
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+/// Define a pybind11 wrapper for an ignition::gazebo::World
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void
+defineGazeboWorld(pybind11::object module);
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
+
+#endif // IGNITION_GAZEBO_PYTHON__WORLD_HH_
diff --git a/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc b/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc
new file mode 100644
index 0000000000..2444a16f02
--- /dev/null
+++ b/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc
@@ -0,0 +1,39 @@
+/*
+ * 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.
+ */
+
+#include
+
+#include "EntityComponentManager.hh"
+#include "EventManager.hh"
+#include "Server.hh"
+#include "ServerConfig.hh"
+#include "TestFixture.hh"
+#include "UpdateInfo.hh"
+#include "Util.hh"
+#include "World.hh"
+
+PYBIND11_MODULE(gazebo, m) {
+ m.doc() = "Ignition Gazebo Python Library.";
+
+ ignition::gazebo::python::defineGazeboEntityComponentManager(m);
+ ignition::gazebo::python::defineGazeboEventManager(m);
+ ignition::gazebo::python::defineGazeboServer(m);
+ ignition::gazebo::python::defineGazeboServerConfig(m);
+ ignition::gazebo::python::defineGazeboTestFixture(m);
+ ignition::gazebo::python::defineGazeboUpdateInfo(m);
+ ignition::gazebo::python::defineGazeboWorld(m);
+ ignition::gazebo::python::defineGazeboUtil(m);
+}
diff --git a/python/src/ignition/gazebo/wrap_functions.hh b/python/src/ignition/gazebo/wrap_functions.hh
new file mode 100644
index 0000000000..2a60b93668
--- /dev/null
+++ b/python/src/ignition/gazebo/wrap_functions.hh
@@ -0,0 +1,323 @@
+// This code from copied from:
+// https://github.com/RobotLocomotion/drake/blob/6ee5e9325821277a62bd5cd5456ccf02ca25dab7/bindings/pydrake/common/wrap_function.h
+// It's under BSD 3-Clause License
+
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include
+#include
+#include
+
+namespace internal {
+
+// Collects both a functor object and its signature for ease of inference.
+template
+struct function_info {
+ // TODO(eric.cousineau): Ensure that this permits copy elision when combined
+ // with `std::forward(func)`, while still behaving well with primitive
+ // types.
+ std::decay_t func;
+};
+
+// Factory method for `function_info<>`, to be used by `infer_function_info`.
+template
+auto make_function_info(Func&& func, Return (*infer)(Args...) = nullptr) {
+ (void)infer;
+ return function_info{std::forward(func)};
+}
+
+// SFINAE for functors.
+// N.B. This *only* distinguished between function / method pointers and
+// lambda objects. It does *not* distinguish among other types.
+template
+using enable_if_lambda_t =
+ std::enable_if_t>::value, T>;
+
+// Infers `function_info<>` from a function pointer.
+template
+auto infer_function_info(Return (*func)(Args...)) {
+ return make_function_info(func);
+}
+
+// Infers `function_info<>` from a mutable method pointer.
+template
+auto infer_function_info(Return (Class::*method)(Args...)) {
+ auto func = [method](Class* self, Args... args) {
+ return (self->*method)(std::forward(args)...);
+ };
+ return make_function_info(func);
+}
+
+// Infers `function_info<>` from a const method pointer.
+template
+auto infer_function_info(Return (Class::*method)(Args...) const) {
+ auto func = [method](const Class* self, Args... args) {
+ return (self->*method)(std::forward(args)...);
+ };
+ return make_function_info(func);
+}
+
+// Helpers for general functor objects.
+struct functor_helpers {
+ // Removes class from mutable method pointer for inferring signature
+ // of functor.
+ template
+ static auto remove_class_from_ptr(Return (Class::*)(Args...)) {
+ using Ptr = Return (*)(Args...);
+ return Ptr{};
+ }
+
+ // Removes class from const method pointer for inferring signature of functor.
+ template
+ static auto remove_class_from_ptr(Return (Class::*)(Args...) const) {
+ using Ptr = Return (*)(Args...);
+ return Ptr{};
+ }
+
+ // Infers function pointer from functor.
+ // @pre `Func` must have only *one* overload of `operator()`.
+ template
+ static auto infer_function_ptr() {
+ return remove_class_from_ptr(&Func::operator());
+ }
+};
+
+// Infers `function_info<>` from a generic functor.
+template >
+auto infer_function_info(Func&& func) {
+ return make_function_info(std::forward(func),
+ functor_helpers::infer_function_ptr>());
+}
+
+// Implementation for wrapping a function by scanning and replacing arguments
+// based on their types.
+template class wrap_arg_policy,
+ bool use_functions = true>
+struct wrap_function_impl {
+ // By default `wrap_arg_functions` is the same as `wrap_arg_policy`. However,
+ // below we specialize it for the case when `T` is of the form
+ // `std::function`.
+ // N.B. This must precede `wrap_type`.
+ template
+ struct wrap_arg_functions : public wrap_arg_policy {};
+
+ template
+ using wrap_arg = std::conditional_t,
+ wrap_arg_policy>;
+
+ // Provides wrapped argument type.
+ // Uses `Extra` to specialize within class scope to intercept `void`.
+ template
+ struct wrap_type {
+ using type = decltype(wrap_arg::wrap(std::declval()));
+ };
+
+ // Intercept `void`, since `declval()` is invalid.
+ template
+ struct wrap_type {
+ using type = void;
+ };
+
+ // Convenience helper type.
+ template
+ using wrap_type_t = typename wrap_type::type;
+
+ // Determines which overload should be used, since we cannot wrap a `void`
+ // type using `wrap_arg::wrap()`.
+ template
+ static constexpr bool enable_wrap_output = !std::is_same::value;
+
+ // Specialization for callbacks of the form `std::function<>`.
+ // @note We could generalize this using SFINAE for functors of any form, but
+ // that complicates the details for a relatively low ROI.
+ template
+ struct wrap_arg_functions&> {
+ // Define types explicit, since `auto` is not easily usable as a return type
+ // (compilers struggle with inference).
+ using Func = std::function;
+ using WrappedFunc =
+ std::function(wrap_type_t...)>;
+
+ static WrappedFunc wrap(const Func& func) {
+ return wrap_function_impl::run(infer_function_info(func));
+ }
+
+ // Unwraps a `WrappedFunc`, also unwrapping the return value.
+ // @note We use `Defer` so that we can use SFINAE without a disptach method.
+ template
+ static Func unwrap( // BR
+ const WrappedFunc& func_wrapped,
+ std::enable_if_t, void*> = {}) {
+ return [func_wrapped](Args... args) -> Return {
+ return wrap_arg_functions::unwrap(func_wrapped(
+ wrap_arg_functions::wrap(std::forward(args))...));
+ };
+ }
+
+ // Specialization / overload of above, but not wrapping the return value.
+ template
+ static Func unwrap(const WrappedFunc& func_wrapped,
+ std::enable_if_t, void*> = {}) {
+ return [func_wrapped](Args... args) {
+ func_wrapped(
+ wrap_arg_functions::wrap(std::forward(args))...);
+ };
+ }
+ };
+
+ // Ensure that we also wrap `std::function<>` returned by value.
+ template
+ struct wrap_arg_functions>
+ : public wrap_arg_functions&> {};
+
+ // Wraps function arguments and the return value.
+ // Generally used when `Return` is non-void.
+ template
+ static auto run(function_info&& info,
+ std::enable_if_t, void*> = {}) {
+ // N.B. Since we do not use the `mutable` keyword with this lambda,
+ // any functors passed in *must* provide `operator()(...) const`.
+ auto func_wrapped =
+ [func_f = std::forward(info.func)](
+ wrap_type_t... args_wrapped) -> wrap_type_t {
+ return wrap_arg::wrap(func_f(wrap_arg::unwrap(
+ std::forward>(args_wrapped))...));
+ };
+ return func_wrapped;
+ }
+
+ // Wraps function arguments, but not the return value.
+ // Generally used when `Return` is void.
+ template
+ static auto run(function_info&& info,
+ std::enable_if_t, void*> = {}) {
+ auto func_wrapped = // BR
+ [func_f = std::forward(info.func)](
+ wrap_type_t... args_wrapped) -> Return {
+ return func_f(wrap_arg::unwrap(
+ std::forward>(args_wrapped))...);
+ };
+ return func_wrapped;
+ }
+};
+
+} // namespace internal
+
+/// Wraps the types used in a function signature to produce a new function with
+/// wrapped arguments and return value (if non-void). The wrapping is based on
+/// `wrap_arg_policy`.
+/// Any types that are of the form `std::function` will be recursively
+/// wrapped, such that callbacks will be of a wrapped form (arguments and
+/// return types wrapped). The original form of the callbacks will still be
+/// called in the wrapped callback.
+/// @tparam wrap_arg_policy
+/// User-supplied argument wrapper, that must supply the static functions
+/// `wrap(Arg arg) -> Wrapped` and `unwrap(Wrapped wrapped) -> Arg`.
+/// `Arg arg` is the original argument, and `Wrapped wrapped` is the wrapped
+/// / transformed argument type.
+/// N.B. This template template parameter uses a parameter pack to allow
+/// for SFINAE. If passing a `using` template alias, ensure that the alias
+/// template template parameter uses a parameter pack of the *exact* same
+/// form.
+/// @tparam use_functions
+/// If true (default), will recursively wrap callbacks. If your policy
+/// provides handling for functions, then you should set this to false.
+/// @param func
+/// Functor to be wrapped. Returns a function with wrapped arguments and
+/// return type. If functor is a method pointer, it will return a function of
+/// the form `Return ([const] Class* self, ...)`.
+/// @return Wrapped function lambda.
+/// N.B. Construct a `std::function<>` from this if you encounter inference
+/// issues downstream of this method.
+template class wrap_arg_policy,
+ bool use_functions = true, typename Func = void>
+auto WrapFunction(Func&& func) {
+ // TODO(eric.cousineau): Create an overload with `type_pack` to
+ // handle overloads, to disambiguate when necessary.
+ return internal::wrap_function_impl::run(
+ internal::infer_function_info(std::forward(func)));
+}
+
+/// Default case for argument wrapping, with pure pass-through. Consider
+/// inheriting from this for base cases.
+/// N.B. `Wrapped` is not necessary, but is used for demonstration purposes.
+template
+struct wrap_arg_default {
+ using Wrapped = T;
+ static Wrapped wrap(T arg) { return std::forward(arg); }
+ static T unwrap(Wrapped arg_wrapped) {
+ return std::forward(arg_wrapped);
+ }
+ // N.B. `T` rather than `T&&` is used as arguments here as it behaves well
+ // with primitive types, such as `int`.
+};
+
+/// Policy for explicitly wrapping functions for a given policy.
+template class wrap_arg_policy, typename Signature>
+using wrap_arg_function = typename internal::wrap_function_impl<
+ wrap_arg_policy>::template wrap_arg>;
+
+// The wraps methods were copied from:
+// https://github.com/RobotLocomotion/drake/blob/6ee5e9325821277a62bd5cd5456ccf02ca25dab7/bindings/pydrake/common/wrap_pybind.h
+// It's under BSD 3-Clause License
+
+// Determines if a type will go through pybind11's generic caster. This
+// implies that the type has been declared using `pybind11::class_`, and can have
+// a reference passed through. Otherwise, the type uses type-conversion:
+// https://pybind11.readthedocs.io/en/stable/advanced/cast/index.html
+template
+constexpr inline bool is_generic_pybind_v =
+ std::is_base_of_v>;
+
+template
+struct wrap_ref_ptr : public wrap_arg_default {};
+
+template
+struct wrap_ref_ptr &&
+ !std::is_same_v> >> {
+ // NOLINTNEXTLINE[runtime/references]: Intentional.
+ static T* wrap(T& arg) { return &arg; }
+ static T& unwrap(T* arg_wrapped) {
+ return *arg_wrapped;
+ }
+};
+
+template
+struct wrap_callback : public wrap_arg_default {};
+
+template
+struct wrap_callback&>
+ : public wrap_arg_function {};
+
+template
+struct wrap_callback>
+ : public wrap_callback&> {};
+
+
+/// Ensures that any `std::function<>` arguments are wrapped such that any `T&`
+/// (which can infer for `T = const U`) is wrapped as `U*` (and conversely
+/// unwrapped when returned).
+/// Use this when you have a callback in C++ that has a lvalue reference (const
+/// or mutable) to a C++ argument or return value.
+/// Otherwise, `pybind11` may try and copy the object, will be bad if either
+/// the type is a non-copyable or if you are trying to mutate the object; in
+/// this case, the copy is mutated, but not the original you care about.
+/// For more information, see: https://github.com/pybind/pybind11/issues/1241
+template
+auto WrapCallbacks(Func&& func) {
+ return WrapFunction(std::forward(func));
+}
diff --git a/python/src/ignition/sdformat/Element.cc b/python/src/ignition/sdformat/Element.cc
new file mode 100644
index 0000000000..4e40b0fdec
--- /dev/null
+++ b/python/src/ignition/sdformat/Element.cc
@@ -0,0 +1,36 @@
+/*
+ * 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.
+ */
+
+#include
+#include
+
+#include "sdf/Element.hh"
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+/////////////////////////////////////////////////
+void defineGazeboElement(pybind11::object module)
+{
+ pybind11::class_>(module, "Element")
+ .def(pybind11::init<>());
+}
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
diff --git a/python/src/ignition/sdformat/Element.hh b/python/src/ignition/sdformat/Element.hh
new file mode 100644
index 0000000000..370dcc53a1
--- /dev/null
+++ b/python/src/ignition/sdformat/Element.hh
@@ -0,0 +1,38 @@
+/*
+ * 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 IGNITION_GAZEBO_PYTHON__ELEMENT_HH_
+#define IGNITION_GAZEBO_PYTHON__ELEMENT_HH_
+
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace python
+{
+/// Define a pybind11 wrapper for an sdformat::Element
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void
+defineGazeboElement(pybind11::object module);
+} // namespace python
+} // namespace gazebo
+} // namespace ignition
+
+#endif // IGNITION_GAZEBO_PYTHON__ELEMENT_HH_
diff --git a/python/src/ignition/sdformat/_sdformat_pybind11.cc b/python/src/ignition/sdformat/_sdformat_pybind11.cc
new file mode 100644
index 0000000000..1483727728
--- /dev/null
+++ b/python/src/ignition/sdformat/_sdformat_pybind11.cc
@@ -0,0 +1,23 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// 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 "Element.hh"
+
+PYBIND11_MODULE(sdformat, m) {
+ m.doc() = "Ignition Common Python Library.";
+
+ ignition::gazebo::python::defineGazeboElement(m);
+}
diff --git a/python/test/gravity.sdf b/python/test/gravity.sdf
new file mode 100644
index 0000000000..ee1161fa5f
--- /dev/null
+++ b/python/test/gravity.sdf
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+ 0.4
+ 0.4
+ 0.4
+
+ 1.0
+
+
+
+
+
diff --git a/python/test/testFixture_TEST.py b/python/test/testFixture_TEST.py
new file mode 100644
index 0000000000..b48c501330
--- /dev/null
+++ b/python/test/testFixture_TEST.py
@@ -0,0 +1,69 @@
+# 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.
+
+import os
+import time
+import unittest
+
+from ignition.common import set_verbosity
+from ignition.gazebo import TestFixture, World, world_entity
+from ignition.math import Vector3d
+from sdformat import Element
+
+post_iterations = 0
+iterations = 0
+pre_iterations = 0
+
+class TestTestFixture(unittest.TestCase):
+
+ def test_test_fixture(self):
+ set_verbosity(4)
+
+ file_path = os.path.dirname(os.path.realpath(__file__))
+ helper = TestFixture(os.path.join(file_path, 'gravity.sdf'))
+
+ def on_post_udpate_cb(_info, _ecm):
+ global post_iterations
+ post_iterations += 1
+
+ def on_pre_udpate_cb(_info, _ecm):
+ global pre_iterations
+ pre_iterations += 1
+ world_e = world_entity(_ecm);
+ self.assertEqual(1, world_e)
+ w = World(world_e)
+ v = w.gravity(_ecm)
+ self.assertEqual(Vector3d(0, 0, -9.8), v)
+
+ def on_udpate_cb(_info, _ecm):
+ global iterations
+ iterations += 1
+
+ helper.on_post_update(on_post_udpate_cb)
+ helper.on_update(on_udpate_cb)
+ helper.on_pre_update(on_pre_udpate_cb)
+ helper.finalize()
+
+ server = helper.server()
+ server.run(False, 1000, False)
+
+ while(server.is_running()):
+ time.sleep(0.1)
+
+ self.assertEqual(1000, pre_iterations)
+ self.assertEqual(1000, iterations)
+ self.assertEqual(1000, post_iterations)
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 358256f1e3..c609bec040 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -60,6 +60,7 @@ set (sources
ServerPrivate.cc
SimulationRunner.cc
SystemLoader.cc
+ SystemManager.cc
TestFixture.cc
Util.cc
World.cc
@@ -86,6 +87,7 @@ set (gtest_sources
Server_TEST.cc
SimulationRunner_TEST.cc
SystemLoader_TEST.cc
+ SystemManager_TEST.cc
System_TEST.cc
TestFixture_TEST.cc
Util_TEST.cc
diff --git a/src/Link.cc b/src/Link.cc
index 0d07f17140..82d860fdb1 100644
--- a/src/Link.cc
+++ b/src/Link.cc
@@ -19,6 +19,7 @@
#include "ignition/gazebo/components/AngularAcceleration.hh"
#include "ignition/gazebo/components/AngularVelocity.hh"
+#include "ignition/gazebo/components/AngularVelocityCmd.hh"
#include "ignition/gazebo/components/CanonicalLink.hh"
#include "ignition/gazebo/components/Collision.hh"
#include "ignition/gazebo/components/ExternalWorldWrenchCmd.hh"
@@ -266,6 +267,25 @@ void Link::SetLinearVelocity(EntityComponentManager &_ecm,
}
}
+//////////////////////////////////////////////////
+void Link::SetAngularVelocity(EntityComponentManager &_ecm,
+ const math::Vector3d &_vel) const
+{
+ auto vel =
+ _ecm.Component(this->dataPtr->id);
+
+ if (vel == nullptr)
+ {
+ _ecm.CreateComponent(
+ this->dataPtr->id,
+ components::AngularVelocityCmd(_vel));
+ }
+ else
+ {
+ vel->Data() = _vel;
+ }
+}
+
//////////////////////////////////////////////////
std::optional Link::WorldAngularAcceleration(
const EntityComponentManager &_ecm) const
diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc
index 9264a41713..2840b3f0ea 100644
--- a/src/SimulationRunner.cc
+++ b/src/SimulationRunner.cc
@@ -61,9 +61,6 @@ SimulationRunner::SimulationRunner(const sdf::World *_world,
// Keep world name
this->worldName = _world->Name();
- // Keep system loader so plugins can be loaded at runtime
- this->systemLoader = _systemLoader;
-
// Get the physics profile
// TODO(luca): remove duplicated logic in SdfEntityCreator and LevelManager
auto physics = _world->PhysicsByIndex(0);
@@ -115,6 +112,10 @@ SimulationRunner::SimulationRunner(const sdf::World *_world,
static_cast(this->stepSize.count() / this->desiredRtf));
}
+ // Create the system manager
+ this->systemMgr = std::make_unique(_systemLoader,
+ &this->entityCompMgr, &this->eventMgr);
+
this->pauseConn = this->eventMgr.Connect(
std::bind(&SimulationRunner::SetPaused, this, std::placeholders::_1));
@@ -173,7 +174,7 @@ SimulationRunner::SimulationRunner(const sdf::World *_world,
// If we have reached this point and no systems have been loaded, then load
// a default set of systems.
- if (this->systems.empty() && this->pendingSystems.empty())
+ if (this->systemMgr->TotalCount() == 0)
{
ignmsg << "No systems loaded from SDF, loading defaults" << std::endl;
bool isPlayback = !this->serverConfig.LogPlaybackPath().empty();
@@ -455,7 +456,9 @@ void SimulationRunner::AddSystem(const SystemPluginPtr &_system,
std::optional _entity,
std::optional> _sdf)
{
- this->AddSystemImpl(SystemInternal(_system), _entity, _sdf);
+ auto entity = _entity.value_or(worldEntity(this->entityCompMgr));
+ auto sdf = _sdf.value_or(this->sdfWorld->Element());
+ this->systemMgr->AddSystem(_system, entity, sdf);
}
//////////////////////////////////////////////////
@@ -464,104 +467,57 @@ void SimulationRunner::AddSystem(
std::optional _entity,
std::optional> _sdf)
{
- this->AddSystemImpl(SystemInternal(_system), _entity, _sdf);
-}
-
-//////////////////////////////////////////////////
-void SimulationRunner::AddSystemImpl(
- SystemInternal _system,
- std::optional _entity,
- std::optional> _sdf)
-{
- // Call configure
- if (_system.configure)
- {
- // Default to world entity and SDF
- auto entity = _entity.has_value() ? _entity.value()
- : worldEntity(this->entityCompMgr);
- auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element();
-
- _system.configure->Configure(
- entity, sdf,
- this->entityCompMgr,
- this->eventMgr);
- }
-
- // Update callbacks will be handled later, add to queue
- std::lock_guard lock(this->pendingSystemsMutex);
- this->pendingSystems.push_back(_system);
+ auto entity = _entity.value_or(worldEntity(this->entityCompMgr));
+ auto sdf = _sdf.value_or(this->sdfWorld->Element());
+ this->systemMgr->AddSystem(_system, entity, sdf);
}
/////////////////////////////////////////////////
-void SimulationRunner::AddSystemToRunner(SystemInternal _system)
+void SimulationRunner::ProcessSystemQueue()
{
- this->systems.push_back(_system);
+ auto pending = this->systemMgr->PendingCount();
- if (_system.preupdate)
- this->systemsPreupdate.push_back(_system.preupdate);
+ if (0 == pending)
+ return;
- if (_system.update)
- this->systemsUpdate.push_back(_system.update);
+ // If additional systems are to be added, stop the existing threads.
+ this->StopWorkerThreads();
- if (_system.postupdate)
- this->systemsPostupdate.push_back(_system.postupdate);
-}
+ this->systemMgr->ActivatePendingSystems();
-/////////////////////////////////////////////////
-void SimulationRunner::ProcessSystemQueue()
-{
- std::lock_guard lock(this->pendingSystemsMutex);
- auto pending = this->pendingSystems.size();
-
- if (pending > 0)
- {
- // If additional systems are to be added, stop the existing threads.
- this->StopWorkerThreads();
- }
+ auto threadCount = this->systemMgr->SystemsPostUpdate().size() + 1u;
- for (const auto &system : this->pendingSystems)
- {
- this->AddSystemToRunner(system);
- }
- this->pendingSystems.clear();
+ igndbg << "Creating PostUpdate worker threads: "
+ << threadCount << std::endl;
- // If additional systems were added, recreate the worker threads.
- if (pending > 0)
- {
- igndbg << "Creating PostUpdate worker threads: "
- << this->systemsPostupdate.size() + 1 << std::endl;
+ this->postUpdateStartBarrier = std::make_unique(threadCount);
+ this->postUpdateStopBarrier = std::make_unique(threadCount);
- this->postUpdateStartBarrier =
- std::make_unique(this->systemsPostupdate.size() + 1u);
- this->postUpdateStopBarrier =
- std::make_unique(this->systemsPostupdate.size() + 1u);
+ this->postUpdateThreadsRunning = true;
+ int id = 0;
- this->postUpdateThreadsRunning = true;
- int id = 0;
+ for (auto &system : this->systemMgr->SystemsPostUpdate())
+ {
+ igndbg << "Creating postupdate worker thread (" << id << ")" << std::endl;
- for (auto &system : this->systemsPostupdate)
+ this->postUpdateThreads.push_back(std::thread([&, id]()
{
- igndbg << "Creating postupdate worker thread (" << id << ")" << std::endl;
-
- this->postUpdateThreads.push_back(std::thread([&, id]()
+ std::stringstream ss;
+ ss << "PostUpdateThread: " << id;
+ IGN_PROFILE_THREAD_NAME(ss.str().c_str());
+ while (this->postUpdateThreadsRunning)
{
- std::stringstream ss;
- ss << "PostUpdateThread: " << id;
- IGN_PROFILE_THREAD_NAME(ss.str().c_str());
- while (this->postUpdateThreadsRunning)
+ this->postUpdateStartBarrier->Wait();
+ if (this->postUpdateThreadsRunning)
{
- this->postUpdateStartBarrier->Wait();
- if (this->postUpdateThreadsRunning)
- {
- system->PostUpdate(this->currentInfo, this->entityCompMgr);
- }
- this->postUpdateStopBarrier->Wait();
+ system->PostUpdate(this->currentInfo, this->entityCompMgr);
}
- igndbg << "Exiting postupdate worker thread ("
- << id << ")" << std::endl;
- }));
- id++;
- }
+ this->postUpdateStopBarrier->Wait();
+ }
+ igndbg << "Exiting postupdate worker thread ("
+ << id << ")" << std::endl;
+ }));
+ id++;
}
}
@@ -577,13 +533,13 @@ void SimulationRunner::UpdateSystems()
{
IGN_PROFILE("PreUpdate");
- for (auto& system : this->systemsPreupdate)
+ for (auto& system : this->systemMgr->SystemsPreUpdate())
system->PreUpdate(this->currentInfo, this->entityCompMgr);
}
{
IGN_PROFILE("Update");
- for (auto& system : this->systemsUpdate)
+ for (auto& system : this->systemMgr->SystemsUpdate())
system->Update(this->currentInfo, this->entityCompMgr);
}
@@ -903,19 +859,7 @@ void SimulationRunner::LoadPlugin(const Entity _entity,
const std::string &_name,
const sdf::ElementPtr &_sdf)
{
- std::optional system;
- {
- std::lock_guard lock(this->systemLoaderMutex);
- system = this->systemLoader->LoadPlugin(_fname, _name, _sdf);
- }
-
- // System correctly loaded from library
- if (system)
- {
- this->AddSystem(system.value(), _entity, _sdf);
- igndbg << "Loaded system [" << _name
- << "] for entity [" << _entity << "]" << std::endl;
- }
+ this->systemMgr->LoadPlugin(_entity, _fname, _name, _sdf);
}
//////////////////////////////////////////////////
@@ -1085,8 +1029,7 @@ size_t SimulationRunner::EntityCount() const
/////////////////////////////////////////////////
size_t SimulationRunner::SystemCount() const
{
- std::lock_guard lock(this->pendingSystemsMutex);
- return this->systems.size() + this->pendingSystems.size();
+ return this->systemMgr->TotalCount();
}
/////////////////////////////////////////////////
diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh
index 5da194e209..351dfe6fa0 100644
--- a/src/SimulationRunner.hh
+++ b/src/SimulationRunner.hh
@@ -48,14 +48,14 @@
#include "ignition/gazebo/EventManager.hh"
#include "ignition/gazebo/Export.hh"
#include "ignition/gazebo/ServerConfig.hh"
-#include "ignition/gazebo/System.hh"
#include "ignition/gazebo/SystemLoader.hh"
-#include "ignition/gazebo/SystemPluginPtr.hh"
#include "ignition/gazebo/Types.hh"
#include "network/NetworkManager.hh"
#include "LevelManager.hh"
+#include "SystemManager.hh"
#include "Barrier.hh"
+#include "WorldControl.hh"
using namespace std::chrono_literals;
@@ -68,93 +68,6 @@ namespace ignition
// Forward declarations.
class SimulationRunnerPrivate;
- /// \brief Helper struct to control world time. It's used to hold
- /// input from either msgs::WorldControl or msgs::LogPlaybackControl.
- struct WorldControl
- {
- /// \brief True to pause simulation.
- // cppcheck-suppress unusedStructMember
- bool pause{false}; // NOLINT
-
- /// \biref Run a given number of simulation iterations.
- // cppcheck-suppress unusedStructMember
- uint64_t multiStep{0u}; // NOLINT
-
- /// \brief Reset simulation back to time zero. Rewinding resets sim time,
- /// real time and iterations.
- // cppcheck-suppress unusedStructMember
- bool rewind{false}; // NOLINT
-
- /// \brief A simulation time in the future to run to and then pause.
- /// A negative number indicates that this variable it not being used.
- std::chrono::steady_clock::duration runToSimTime{-1}; // NOLINT
-
- /// \brief Sim time to jump to. A negative value means don't seek.
- /// Seeking changes sim time but doesn't affect real time.
- /// It also resets iterations back to zero.
- std::chrono::steady_clock::duration seek{-1};
- };
-
- /// \brief Class to hold systems internally. It supports systems loaded
- /// from plugins, as well as systems created at runtime.
- class SystemInternal
- {
- /// \brief Constructor
- /// \param[in] _systemPlugin A system loaded from a plugin.
- public: explicit SystemInternal(SystemPluginPtr _systemPlugin)
- : systemPlugin(std::move(_systemPlugin)),
- system(systemPlugin->QueryInterface()),
- configure(systemPlugin->QueryInterface()),
- preupdate(systemPlugin->QueryInterface()),
- update(systemPlugin->QueryInterface()),
- postupdate(systemPlugin->QueryInterface())
- {
- }
-
- /// \brief Constructor
- /// \param[in] _system Pointer to a system.
- public: explicit SystemInternal(const std::shared_ptr &_system)
- : systemShared(_system),
- system(_system.get()),
- configure(dynamic_cast(_system.get())),
- preupdate(dynamic_cast(_system.get())),
- update(dynamic_cast(_system.get())),
- postupdate(dynamic_cast(_system.get()))
- {
- }
-
- /// \brief Plugin object. This manages the lifecycle of the instantiated
- /// class as well as the shared library.
- /// This will be null if the system wasn't loaded from a plugin.
- public: SystemPluginPtr systemPlugin;
-
- /// \brief Pointer to a system.
- /// This will be null if the system wasn't loaded from a pointer.
- public: std::shared_ptr systemShared{nullptr};
-
- /// \brief Access this system via the `System` interface
- public: System *system = nullptr;
-
- /// \brief Access this system via the ISystemConfigure interface
- /// Will be nullptr if the System doesn't implement this interface.
- public: ISystemConfigure *configure = nullptr;
-
- /// \brief Access this system via the ISystemPreUpdate interface
- /// Will be nullptr if the System doesn't implement this interface.
- public: ISystemPreUpdate *preupdate = nullptr;
-
- /// \brief Access this system via the ISystemUpdate interface
- /// Will be nullptr if the System doesn't implement this interface.
- public: ISystemUpdate *update = nullptr;
-
- /// \brief Access this system via the ISystemPostUpdate interface
- /// Will be nullptr if the System doesn't implement this interface.
- public: ISystemPostUpdate *postupdate = nullptr;
-
- /// \brief Vector of queries and callbacks
- public: std::vector updates;
- };
-
class IGNITION_GAZEBO_VISIBLE SimulationRunner
{
/// \brief Constructor
@@ -457,16 +370,6 @@ namespace ignition
/// Physics component of the world, if any.
public: void UpdatePhysicsParams();
- /// \brief Implementation for AddSystem functions. This only adds systems
- /// to a queue, the actual addition is performed by `AddSystemToRunner` at
- /// the appropriate time.
- /// \param[in] _system Generic representation of a system.
- /// \param[in] _entity Entity received from AddSystem.
- /// \param[in] _sdf SDF received from AddSystem.
- private: void AddSystemImpl(SystemInternal _system,
- std::optional _entity = std::nullopt,
- std::optional> _sdf = std::nullopt);
-
/// \brief Process entities with the components::Recreate component.
/// Put in a request to make them as removed
private: void ProcessRecreateEntitiesRemove();
@@ -486,25 +389,16 @@ namespace ignition
/// server is in the run state.
private: std::atomic running{false};
- /// \brief All the systems.
- private: std::vector systems;
-
- /// \brief Pending systems to be added to systems.
- private: std::vector pendingSystems;
-
- /// \brief Mutex to protect pendingSystems
- private: mutable std::mutex pendingSystemsMutex;
-
- /// \brief Systems implementing PreUpdate
- private: std::vector systemsPreupdate;
-
- /// \brief Systems implementing Update
- private: std::vector systemsUpdate;
-
- /// \brief Systems implementing PostUpdate
- private: std::vector systemsPostupdate;
+ /// \brief Manager of all systems.
+ /// Note: must be before EntityComponentManager
+ /// Note: must be before EventMgr
+ /// Because systems have access to the ECM and Events, they need to be
+ /// cleanly stopped and destructed before destroying the event manager
+ /// and entity component manager.
+ private: std::unique_ptr systemMgr;
/// \brief Manager of all events.
+ /// Note: must be before EntityComponentManager
private: EventManager eventMgr;
/// \brief Manager of all components.
@@ -536,12 +430,6 @@ namespace ignition
/// \brief List of real times used to compute averages.
private: std::list realTimes;
- /// \brief System loader, for loading system plugins.
- private: SystemLoaderPtr systemLoader;
-
- /// \brief Mutex to protect systemLoader
- private: std::mutex systemLoaderMutex;
-
/// \brief Node for communication.
private: std::unique_ptr node{nullptr};
diff --git a/src/SystemInternal.hh b/src/SystemInternal.hh
new file mode 100644
index 0000000000..13158688c3
--- /dev/null
+++ b/src/SystemInternal.hh
@@ -0,0 +1,107 @@
+/*
+ * Copyright (C) 2022 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 IGNITION_GAZEBO_SYSTEMINTERNAL_HH_
+#define IGNITION_GAZEBO_SYSTEMINTERNAL_HH_
+
+#include
+#include
+#include
+#include
+
+#include "ignition/gazebo/config.hh"
+#include "ignition/gazebo/System.hh"
+#include "ignition/gazebo/SystemPluginPtr.hh"
+
+namespace ignition
+{
+ namespace gazebo
+ {
+ // Inline bracket to help doxygen filtering.
+ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+
+ /// \brief Class to hold systems internally. It supports systems loaded
+ /// from plugins, as well as systems created at runtime.
+ class SystemInternal
+ {
+ /// \brief Constructor
+ /// \param[in] _systemPlugin A system loaded from a plugin.
+ public: explicit SystemInternal(SystemPluginPtr _systemPlugin)
+ : systemPlugin(std::move(_systemPlugin)),
+ system(systemPlugin->QueryInterface()),
+ configure(systemPlugin->QueryInterface()),
+ preupdate(systemPlugin->QueryInterface()),
+ update(systemPlugin->QueryInterface()),
+ postupdate(systemPlugin->QueryInterface())
+ {
+ }
+
+ /// \brief Constructor
+ /// \param[in] _system Pointer to a system.
+ public: explicit SystemInternal(const std::shared_ptr &_system)
+ : systemShared(_system),
+ system(_system.get()),
+ configure(dynamic_cast(_system.get())),
+ preupdate(dynamic_cast(_system.get())),
+ update(dynamic_cast(_system.get())),
+ postupdate(dynamic_cast(_system.get()))
+ {
+ }
+
+ /// \brief Plugin object. This manages the lifecycle of the instantiated
+ /// class as well as the shared library.
+ /// This will be null if the system wasn't loaded from a plugin.
+ public: SystemPluginPtr systemPlugin;
+
+ /// \brief Pointer to a system.
+ /// This will be null if the system wasn't loaded from a pointer.
+ public: std::shared_ptr systemShared{nullptr};
+
+ /// \brief Access this system via the `System` interface
+ public: System *system = nullptr;
+
+ /// \brief Access this system via the ISystemConfigure interface
+ /// Will be nullptr if the System doesn't implement this interface.
+ public: ISystemConfigure *configure = nullptr;
+
+ /// \brief Access this system via the ISystemPreUpdate interface
+ /// Will be nullptr if the System doesn't implement this interface.
+ public: ISystemPreUpdate *preupdate = nullptr;
+
+ /// \brief Access this system via the ISystemUpdate interface
+ /// Will be nullptr if the System doesn't implement this interface.
+ public: ISystemUpdate *update = nullptr;
+
+ /// \brief Access this system via the ISystemPostUpdate interface
+ /// Will be nullptr if the System doesn't implement this interface.
+ public: ISystemPostUpdate *postupdate = nullptr;
+
+ /// \brief Cached entity that was used to call `Configure` on the system
+ /// Useful for if a system needs to be reconfigured at runtime
+ public: Entity configureEntity = {kNullEntity};
+
+ /// \brief Cached sdf that was used to call `Configure` on the system
+ /// Useful for if a system needs to be reconfigured at runtime
+ public: std::shared_ptr configureSdf = nullptr;
+
+ /// \brief Vector of queries and callbacks
+ public: std::vector updates;
+ };
+ }
+ } // namespace gazebo
+} // namespace ignition
+#endif // IGNITION_GAZEBO_SYSTEMINTERNAL_HH_
+
diff --git a/src/SystemManager.cc b/src/SystemManager.cc
new file mode 100644
index 0000000000..ba716f99a9
--- /dev/null
+++ b/src/SystemManager.cc
@@ -0,0 +1,159 @@
+/*
+ * Copyright (C) 2022 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 "SystemManager.hh"
+
+using namespace ignition;
+using namespace gazebo;
+
+//////////////////////////////////////////////////
+SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader,
+ EntityComponentManager *_entityCompMgr,
+ EventManager *_eventMgr)
+ : systemLoader(_systemLoader),
+ entityCompMgr(_entityCompMgr),
+ eventMgr(_eventMgr)
+{
+}
+
+//////////////////////////////////////////////////
+void SystemManager::LoadPlugin(const Entity _entity,
+ const std::string &_fname,
+ const std::string &_name,
+ const sdf::ElementPtr &_sdf)
+{
+ std::optional system;
+ {
+ std::lock_guard lock(this->systemLoaderMutex);
+ system = this->systemLoader->LoadPlugin(_fname, _name, _sdf);
+ }
+
+ // System correctly loaded from library
+ if (system)
+ {
+ this->AddSystem(system.value(), _entity, _sdf);
+ igndbg << "Loaded system [" << _name
+ << "] for entity [" << _entity << "]" << std::endl;
+ }
+}
+
+//////////////////////////////////////////////////
+size_t SystemManager::TotalCount() const
+{
+ return this->ActiveCount() + this->PendingCount();
+}
+
+//////////////////////////////////////////////////
+size_t SystemManager::ActiveCount() const
+{
+ return this->systems.size();
+}
+
+//////////////////////////////////////////////////
+size_t SystemManager::PendingCount() const
+{
+ std::lock_guard lock(this->pendingSystemsMutex);
+ return this->pendingSystems.size();
+}
+
+//////////////////////////////////////////////////
+size_t SystemManager::ActivatePendingSystems()
+{
+ std::lock_guard lock(this->pendingSystemsMutex);
+
+ auto count = this->pendingSystems.size();
+
+ for (const auto& system : this->pendingSystems)
+ {
+ this->systems.push_back(system);
+
+ if (system.configure)
+ this->systemsConfigure.push_back(system.configure);
+
+ if (system.preupdate)
+ this->systemsPreupdate.push_back(system.preupdate);
+
+ if (system.update)
+ this->systemsUpdate.push_back(system.update);
+
+ if (system.postupdate)
+ this->systemsPostupdate.push_back(system.postupdate);
+ }
+
+ this->pendingSystems.clear();
+ return count;
+}
+
+//////////////////////////////////////////////////
+void SystemManager::AddSystem(const SystemPluginPtr &_system,
+ Entity _entity,
+ std::shared_ptr _sdf)
+{
+ this->AddSystemImpl(SystemInternal(_system), _entity, _sdf);
+}
+
+//////////////////////////////////////////////////
+void SystemManager::AddSystem(
+ const std::shared_ptr &_system,
+ Entity _entity,
+ std::shared_ptr _sdf)
+{
+ this->AddSystemImpl(SystemInternal(_system), _entity, _sdf);
+}
+
+//////////////////////////////////////////////////
+void SystemManager::AddSystemImpl(
+ SystemInternal _system,
+ Entity _entity,
+ std::shared_ptr _sdf)
+{
+ // Configure the system, if necessary
+ if (_system.configure && this->entityCompMgr && this->eventMgr)
+ {
+ _system.configure->Configure(_entity, _sdf,
+ *this->entityCompMgr,
+ *this->eventMgr);
+ }
+
+ // Update callbacks will be handled later, add to queue
+ std::lock_guard lock(this->pendingSystemsMutex);
+ this->pendingSystems.push_back(_system);
+}
+
+//////////////////////////////////////////////////
+const std::vector& SystemManager::SystemsConfigure()
+{
+ return this->systemsConfigure;
+}
+
+//////////////////////////////////////////////////
+const std::vector& SystemManager::SystemsPreUpdate()
+{
+ return this->systemsPreupdate;
+}
+
+//////////////////////////////////////////////////
+const std::vector& SystemManager::SystemsUpdate()
+{
+ return this->systemsUpdate;
+}
+
+//////////////////////////////////////////////////
+const std::vector& SystemManager::SystemsPostUpdate()
+{
+ return this->systemsPostupdate;
+}
diff --git a/src/SystemManager.hh b/src/SystemManager.hh
new file mode 100644
index 0000000000..9c38505922
--- /dev/null
+++ b/src/SystemManager.hh
@@ -0,0 +1,153 @@
+/*
+ * Copyright (C) 2022 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 IGNITION_GAZEBO_SYSTEMMANAGER_HH_
+#define IGNITION_GAZEBO_SYSTEMMANAGER_HH_
+
+#include
+#include
+#include
+
+#include "ignition/gazebo/config.hh"
+#include "ignition/gazebo/EntityComponentManager.hh"
+#include "ignition/gazebo/Export.hh"
+#include "ignition/gazebo/SystemLoader.hh"
+#include "ignition/gazebo/Types.hh"
+
+#include "SystemInternal.hh"
+
+namespace ignition
+{
+ namespace gazebo
+ {
+ // Inline bracket to help doxygen filtering.
+ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+
+ /// \brief Used to load / unload sysetms as well as iterate over them.
+ class IGNITION_GAZEBO_VISIBLE SystemManager
+ {
+ /// \brief Constructor
+ /// \param[in] _systemLoader A pointer to a SystemLoader to load plugins
+ /// from files
+ /// \param[in] _entityCompMgr Pointer to the entity component manager to
+ /// be used when configuring new systems
+ /// \param[in] _eventMgr Pointer to the event manager to be used when
+ /// configuring new systems
+ public: explicit SystemManager(const SystemLoaderPtr &_systemLoader,
+ EntityComponentManager *_entityCompMgr = nullptr,
+ EventManager *_eventMgr = nullptr);
+
+ /// \brief Load system plugin for a given entity.
+ /// \param[in] _entity Entity
+ /// \param[in] _fname Filename of the plugin library
+ /// \param[in] _name Name of the plugin
+ /// \param[in] _sdf SDF element (content of plugin tag)
+ public: void LoadPlugin(const Entity _entity,
+ const std::string &_fname,
+ const std::string &_name,
+ const sdf::ElementPtr &_sdf);
+
+ /// \brief Add a system to the manager
+ /// \param[in] _system SystemPluginPtr to be added
+ /// \param[in] _entity Entity that system is attached to.
+ /// \param[in] _sdf Pointer to the SDF of the entity.
+ public: void AddSystem(const SystemPluginPtr &_system,
+ Entity _entity,
+ std::shared_ptr _sdf);
+
+ /// \brief Add a system to the manager
+ /// \param[in] _system SystemPluginPtr to be added
+ /// \param[in] _entity Entity that system is attached to.
+ /// \param[in] _sdf Pointer to the SDF of the entity.
+ public: void AddSystem(const std::shared_ptr &_system,
+ Entity _entity,
+ std::shared_ptr _sdf);
+
+ /// \brief Get the count of currently active systems.
+ /// \return The active systems count.
+ public: size_t ActiveCount() const;
+
+ /// \brief Get the count of currently pending systems.
+ /// \return The pending systems count.
+ public: size_t PendingCount() const;
+
+ /// \brief Get the count of all (pending + active) managed systems
+ /// \return The count.
+ public: size_t TotalCount() const;
+
+ /// \brief Move all "pending" systems to "active" state
+ /// \return The number of newly-active systems
+ public: size_t ActivatePendingSystems();
+
+ /// \brief Get an vector of all systems implementing "Configure"
+ public: const std::vector& SystemsConfigure();
+
+ /// \brief Get an vector of all systems implementing "PreUpdate"
+ public: const std::vector& SystemsPreUpdate();
+
+ /// \brief Get an vector of all systems implementing "Update"
+ public: const std::vector& SystemsUpdate();
+
+ /// \brief Get an vector of all systems implementing "PostUpdate"
+ public: const std::vector& SystemsPostUpdate();
+
+ /// \brief Implementation for AddSystem functions. This only adds systems
+ /// to a queue, the actual addition is performed by `AddSystemToRunner` at
+ /// the appropriate time.
+ /// \param[in] _system Generic representation of a system.
+ /// \param[in] _entity Entity received from AddSystem.
+ /// \param[in] _sdf SDF received from AddSystem.
+ private: void AddSystemImpl(SystemInternal _system,
+ Entity _entity,
+ std::shared_ptr _sdf);
+
+ /// \brief All the systems.
+ private: std::vector systems;
+
+ /// \brief Pending systems to be added to systems.
+ private: std::vector pendingSystems;
+
+ /// \brief Mutex to protect pendingSystems
+ private: mutable std::mutex pendingSystemsMutex;
+
+ /// \brief Systems implementing Configure
+ private: std::vector systemsConfigure;
+
+ /// \brief Systems implementing PreUpdate
+ private: std::vector systemsPreupdate;
+
+ /// \brief Systems implementing Update
+ private: std::vector systemsUpdate;
+
+ /// \brief Systems implementing PostUpdate
+ private: std::vector systemsPostupdate;
+
+ /// \brief System loader, for loading system plugins.
+ private: SystemLoaderPtr systemLoader;
+
+ /// \brief Mutex to protect systemLoader
+ private: std::mutex systemLoaderMutex;
+
+ /// \brief Pointer to associated entity component manager
+ private: EntityComponentManager *entityCompMgr;
+
+ /// \brief Pointer to associated event manager
+ private: EventManager *eventMgr;
+ };
+ }
+ } // namespace gazebo
+} // namespace ignition
+#endif // IGNITION_GAZEBO_SYSTEMINTERNAL_HH_
diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc
new file mode 100644
index 0000000000..4fc7288580
--- /dev/null
+++ b/src/SystemManager_TEST.cc
@@ -0,0 +1,198 @@
+/*
+ * Copyright (C) 2022 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 "ignition/gazebo/EntityComponentManager.hh"
+#include "ignition/gazebo/System.hh"
+#include "ignition/gazebo/SystemLoader.hh"
+#include "ignition/gazebo/Types.hh"
+#include "ignition/gazebo/test_config.hh" // NOLINT(build/include)
+
+#include "SystemManager.hh"
+
+using namespace ignition::gazebo;
+
+/////////////////////////////////////////////////
+class SystemWithConfigure:
+ public System,
+ public ISystemConfigure
+{
+ // Documentation inherited
+ public: void Configure(
+ const Entity &,
+ const std::shared_ptr &,
+ EntityComponentManager &,
+ EventManager &) override { configured++; };
+
+ public: int configured = 0;
+};
+
+/////////////////////////////////////////////////
+class SystemWithUpdates:
+ public System,
+ public ISystemPreUpdate,
+ public ISystemUpdate,
+ public ISystemPostUpdate
+{
+ // Documentation inherited
+ public: void PreUpdate(const UpdateInfo &,
+ EntityComponentManager &) override {};
+
+ // Documentation inherited
+ public: void Update(const UpdateInfo &,
+ EntityComponentManager &) override {};
+
+ // Documentation inherited
+ public: void PostUpdate(const UpdateInfo &,
+ const EntityComponentManager &) override {};
+};
+
+/////////////////////////////////////////////////
+TEST(SystemManager, Constructor)
+{
+ auto loader = std::make_shared();
+ SystemManager systemMgr(loader);
+
+ ASSERT_EQ(0u, systemMgr.ActiveCount());
+ ASSERT_EQ(0u, systemMgr.PendingCount());
+ ASSERT_EQ(0u, systemMgr.TotalCount());
+
+ ASSERT_EQ(0u, systemMgr.SystemsConfigure().size());
+ ASSERT_EQ(0u, systemMgr.SystemsPreUpdate().size());
+ ASSERT_EQ(0u, systemMgr.SystemsUpdate().size());
+ ASSERT_EQ(0u, systemMgr.SystemsPostUpdate().size());
+}
+
+/////////////////////////////////////////////////
+TEST(SystemManager, AddSystemNoEcm)
+{
+ auto loader = std::make_shared();
+ SystemManager systemMgr(loader);
+
+ EXPECT_EQ(0u, systemMgr.ActiveCount());
+ EXPECT_EQ(0u, systemMgr.PendingCount());
+ EXPECT_EQ(0u, systemMgr.TotalCount());
+ EXPECT_EQ(0u, systemMgr.SystemsConfigure().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
+
+ auto configSystem = std::make_shared();
+ systemMgr.AddSystem(configSystem, kNullEntity, nullptr);
+
+ // SystemManager without an ECM/EventmManager will mean no config occurs
+ EXPECT_EQ(0, configSystem->configured);
+
+ EXPECT_EQ(0u, systemMgr.ActiveCount());
+ EXPECT_EQ(1u, systemMgr.PendingCount());
+ EXPECT_EQ(1u, systemMgr.TotalCount());
+ EXPECT_EQ(0u, systemMgr.SystemsConfigure().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
+
+ systemMgr.ActivatePendingSystems();
+ EXPECT_EQ(1u, systemMgr.ActiveCount());
+ EXPECT_EQ(0u, systemMgr.PendingCount());
+ EXPECT_EQ(1u, systemMgr.TotalCount());
+ EXPECT_EQ(1u, systemMgr.SystemsConfigure().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
+
+ auto updateSystem = std::make_shared();
+ systemMgr.AddSystem(updateSystem, kNullEntity, nullptr);
+ EXPECT_EQ(1u, systemMgr.ActiveCount());
+ EXPECT_EQ(1u, systemMgr.PendingCount());
+ EXPECT_EQ(2u, systemMgr.TotalCount());
+ EXPECT_EQ(1u, systemMgr.SystemsConfigure().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
+
+ systemMgr.ActivatePendingSystems();
+ EXPECT_EQ(2u, systemMgr.ActiveCount());
+ EXPECT_EQ(0u, systemMgr.PendingCount());
+ EXPECT_EQ(2u, systemMgr.TotalCount());
+ EXPECT_EQ(1u, systemMgr.SystemsConfigure().size());
+ EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().size());
+ EXPECT_EQ(1u, systemMgr.SystemsUpdate().size());
+ EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size());
+}
+
+/////////////////////////////////////////////////
+TEST(SystemManager, AddSystemEcm)
+{
+ auto loader = std::make_shared();
+
+ auto ecm = EntityComponentManager();
+ auto eventManager = EventManager();
+
+ SystemManager systemMgr(loader, &ecm, &eventManager);
+
+ EXPECT_EQ(0u, systemMgr.ActiveCount());
+ EXPECT_EQ(0u, systemMgr.PendingCount());
+ EXPECT_EQ(0u, systemMgr.TotalCount());
+ EXPECT_EQ(0u, systemMgr.SystemsConfigure().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
+
+ auto configSystem = std::make_shared();
+ systemMgr.AddSystem(configSystem, kNullEntity, nullptr);
+
+ // Configure called during AddSystem
+ EXPECT_EQ(1, configSystem->configured);
+
+ EXPECT_EQ(0u, systemMgr.ActiveCount());
+ EXPECT_EQ(1u, systemMgr.PendingCount());
+ EXPECT_EQ(1u, systemMgr.TotalCount());
+ EXPECT_EQ(0u, systemMgr.SystemsConfigure().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
+
+ systemMgr.ActivatePendingSystems();
+ EXPECT_EQ(1u, systemMgr.ActiveCount());
+ EXPECT_EQ(0u, systemMgr.PendingCount());
+ EXPECT_EQ(1u, systemMgr.TotalCount());
+ EXPECT_EQ(1u, systemMgr.SystemsConfigure().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
+
+ auto updateSystem = std::make_shared();
+ systemMgr.AddSystem(updateSystem, kNullEntity, nullptr);
+ EXPECT_EQ(1u, systemMgr.ActiveCount());
+ EXPECT_EQ(1u, systemMgr.PendingCount());
+ EXPECT_EQ(2u, systemMgr.TotalCount());
+ EXPECT_EQ(1u, systemMgr.SystemsConfigure().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
+ EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
+
+ systemMgr.ActivatePendingSystems();
+ EXPECT_EQ(2u, systemMgr.ActiveCount());
+ EXPECT_EQ(0u, systemMgr.PendingCount());
+ EXPECT_EQ(2u, systemMgr.TotalCount());
+ EXPECT_EQ(1u, systemMgr.SystemsConfigure().size());
+ EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().size());
+ EXPECT_EQ(1u, systemMgr.SystemsUpdate().size());
+ EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size());
+}
+
diff --git a/src/Util.cc b/src/Util.cc
index ec6efd57dc..e46e1ffb5a 100644
--- a/src/Util.cc
+++ b/src/Util.cc
@@ -81,7 +81,7 @@ math::Pose3d worldPose(const Entity &_entity,
if (!parentPose)
break;
// transform pose
- pose = pose + parentPose->Data();
+ pose = parentPose->Data() * pose;
// keep going up the tree
p = _ecm.Component