From cd14e6a03d5351bc724c86b2fdc3fe5e4c9edb02 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 19 Nov 2021 17:23:14 +0100 Subject: [PATCH 01/21] Added Python interfaces to same Ignition Gazebo methods Signed-off-by: ahcorde --- CMakeLists.txt | 3 + examples/python/gravity.sdf | 22 ++ examples/python/helperFixture.py | 62 +++++ python/CMakeLists.txt | 99 ++++++++ .../common/_ignition_common_pybind11.cc | 27 +++ python/src/ignition/common/console.cc | 31 +++ python/src/ignition/common/console.hh | 33 +++ .../gazebo/_ignition_gazebo_pybind11.cc | 44 ++++ python/src/ignition/gazebo/destroyable.cc | 90 ++++++++ python/src/ignition/gazebo/destroyable.hh | 71 ++++++ .../gazebo/entity_component_manager.cc | 57 +++++ .../gazebo/entity_component_manager.hh | 72 ++++++ python/src/ignition/gazebo/event_manager.cc | 35 +++ python/src/ignition/gazebo/event_manager.hh | 39 ++++ python/src/ignition/gazebo/exceptions.hh | 35 +++ python/src/ignition/gazebo/helper_system.cc | 217 ++++++++++++++++++ python/src/ignition/gazebo/helper_system.hh | 159 +++++++++++++ python/src/ignition/gazebo/server.cc | 46 ++++ python/src/ignition/gazebo/server.hh | 45 ++++ python/src/ignition/gazebo/server_config.cc | 35 +++ python/src/ignition/gazebo/server_config.hh | 42 ++++ python/src/ignition/gazebo/update_info.cc | 41 ++++ python/src/ignition/gazebo/update_info.hh | 43 ++++ python/src/ignition/gazebo/world.cc | 75 ++++++ python/src/ignition/gazebo/world.hh | 75 ++++++ 25 files changed, 1498 insertions(+) create mode 100644 examples/python/gravity.sdf create mode 100644 examples/python/helperFixture.py create mode 100644 python/CMakeLists.txt create mode 100644 python/src/ignition/common/_ignition_common_pybind11.cc create mode 100644 python/src/ignition/common/console.cc create mode 100644 python/src/ignition/common/console.hh create mode 100644 python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc create mode 100644 python/src/ignition/gazebo/destroyable.cc create mode 100644 python/src/ignition/gazebo/destroyable.hh create mode 100644 python/src/ignition/gazebo/entity_component_manager.cc create mode 100644 python/src/ignition/gazebo/entity_component_manager.hh create mode 100644 python/src/ignition/gazebo/event_manager.cc create mode 100644 python/src/ignition/gazebo/event_manager.hh create mode 100644 python/src/ignition/gazebo/exceptions.hh create mode 100644 python/src/ignition/gazebo/helper_system.cc create mode 100644 python/src/ignition/gazebo/helper_system.hh create mode 100644 python/src/ignition/gazebo/server.cc create mode 100644 python/src/ignition/gazebo/server.hh create mode 100644 python/src/ignition/gazebo/server_config.cc create mode 100644 python/src/ignition/gazebo/server_config.hh create mode 100644 python/src/ignition/gazebo/update_info.cc create mode 100644 python/src/ignition/gazebo/update_info.hh create mode 100644 python/src/ignition/gazebo/world.cc create mode 100644 python/src/ignition/gazebo/world.hh diff --git a/CMakeLists.txt b/CMakeLists.txt index 94162ecd4a..4b2ca91ddd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 @@ -186,6 +187,8 @@ add_subdirectory(examples) #============================================================================ ign_create_packages() +add_subdirectory(python) + #============================================================================ # Configure documentation #============================================================================ diff --git a/examples/python/gravity.sdf b/examples/python/gravity.sdf new file mode 100644 index 0000000000..ee1161fa5f --- /dev/null +++ b/examples/python/gravity.sdf @@ -0,0 +1,22 @@ + + + + + + + + + + + 0.4 + 0.4 + 0.4 + + 1.0 + + + + + diff --git a/examples/python/helperFixture.py b/examples/python/helperFixture.py new file mode 100644 index 0000000000..d80ed249d1 --- /dev/null +++ b/examples/python/helperFixture.py @@ -0,0 +1,62 @@ +#!/usr/bin/python3 + +import os +import time + +from ignition.common import set_verbosity +from ignition.gazebo import HelperFixture, World +from ignition.math import Vector3d + + +set_verbosity(4) + +file_path = os.path.dirname(os.path.realpath(__file__)) + +helper = HelperFixture(os.path.join(file_path, 'gravity.sdf')) + +post_iterations = 0 +iterations = 0 +pre_iterations = 0 + + +def on_configure_cb(worldEntity, _ecm): + print('World entity is ', worldEntity) + w = World(worldEntity) + v = w.gravity(_ecm) + print('Gravity ', v) + modelEntity = w.model_by_name(_ecm, 'falling') + print('Entity for falling model is: ', modelEntity) + + +def on_post_udpate_cb(_info, _ecm): + global post_iterations + post_iterations += 1 + # print(_info.sim_time) + + +def on_pre_udpate_cb(_info, _ecm): + global pre_iterations + pre_iterations += 1 + + +def on_udpate_cb(_info, _ecm): + global iterations + iterations += 1 + + +helper = helper.on_post_update(on_post_udpate_cb) +helper = helper.on_update(on_udpate_cb) +helper = helper.on_pre_update(on_pre_udpate_cb) +helper = helper.on_configure(on_configure_cb) + +helper = 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/python/CMakeLists.txt b/python/CMakeLists.txt new file mode 100644 index 0000000000..bc7c2519af --- /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}) + else() + # custom cmake command is returning dist-packages + string(REPLACE "dist-packages" "site-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(IGN_PYTHON_INSTALL_PATH "${IGN_PYTHON_INSTALL_PATH}/ignition") + +set(PYBIND11_PYTHON_VERSION 3) + +find_package(pybind11 2.2 QUIET) + +if (NOT ${pybind11_FOUND}) + if(${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.11.0") + include(FetchContent) + FetchContent_Declare( + pybind11 + GIT_REPOSITORY https://github.com/pybind/pybind11 + GIT_TAG v2.8.1 + ) + + FetchContent_GetProperties(pybind11) + if(NOT pybind11_POPULATED) + FetchContent_Populate(pybind11) + add_subdirectory(${pybind11_SOURCE_DIR} ${pybind11_BINARY_DIR}) + endif() + set(pybind11_FOUND TRUE) + endif() +endif() + +if (${pybind11_FOUND}) + + # 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}/" + ) + endfunction() + + pybind11_add_module(gazebo SHARED + src/ignition/gazebo/_ignition_gazebo_pybind11.cc + src/ignition/gazebo/destroyable.cc + src/ignition/gazebo/entity_component_manager + src/ignition/gazebo/event_manager.cc + src/ignition/gazebo/helper_system.cc + src/ignition/gazebo/server_config.cc + src/ignition/gazebo/server.cc + src/ignition/gazebo/update_info.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} + ) + + configure_build_install_location(gazebo) + configure_build_install_location(common) + +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..40c90d31ef --- /dev/null +++ b/python/src/ignition/common/_ignition_common_pybind11.cc @@ -0,0 +1,27 @@ +// 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 py = pybind11; + +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/common/console.cc b/python/src/ignition/common/console.cc new file mode 100644 index 0000000000..a6f1361575 --- /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..ba0597f19a --- /dev/null +++ b/python/src/ignition/common/console.hh @@ -0,0 +1,33 @@ +// 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_HPP_ +#define IGNITION_GAZEBO_PYTHON__CONSOLE_HPP_ + +#include + +namespace py = pybind11; + +namespace ignition +{ + namespace common + { + namespace python + { + void SetVerbosity(int _verbosity); + } + } +} + +#endif 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..15c0860dda --- /dev/null +++ b/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc @@ -0,0 +1,44 @@ +// 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 "destroyable.hh" +#include "entity_component_manager.hh" +#include "event_manager.hh" +#include "exceptions.hh" +#include "helper_system.hh" +#include "server.hh" +#include "server_config.hh" +#include "update_info.hh" +#include "world.hh" + +namespace py = pybind11; + +PYBIND11_MODULE(gazebo, m) { + m.doc() = "Ignition Gazebo Python Library."; + + ignition::utils::python::define_destroyable(m); + + py::register_exception( + m, "InvalidHandle", PyExc_RuntimeError); + + ignition::gazebo::python::define_gazebo_entity_component_manager(m); + ignition::gazebo::python::define_gazebo_event_manager(m); + ignition::gazebo::python::define_gazebo_helper_fixture(m); + ignition::gazebo::python::define_gazebo_server(m); + ignition::gazebo::python::define_gazebo_update_info(m); + ignition::gazebo::python::define_gazebo_world(m); + ignition::gazebo::python::define_server_config(m); +} diff --git a/python/src/ignition/gazebo/destroyable.cc b/python/src/ignition/gazebo/destroyable.cc new file mode 100644 index 0000000000..55833eeed0 --- /dev/null +++ b/python/src/ignition/gazebo/destroyable.cc @@ -0,0 +1,90 @@ +// 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 + +#include "destroyable.hh" +#include "exceptions.hh" + +namespace ignition +{ +namespace utils +{ +namespace python +{ +Destroyable::Destroyable(const Destroyable &) +{ + // When a destroyable is copied, it does not matter if someone asked + // to destroy the original. The copy has its own lifetime. + use_count = 0; + please_destroy_ = false; +} + +void +Destroyable::enter() +{ + if (please_destroy_) { + throw InvalidHandle("cannot use Destroyable because destruction was requested"); + } + ++use_count; +} + +void +Destroyable::exit(py::object, py::object, py::object) +{ + if (0u == use_count) { + throw std::runtime_error("Internal error: Destroyable use_count would be negative"); + } + + --use_count; + if (please_destroy_ && 0u == use_count) { + destroy(); + } +} + +void +Destroyable::destroy() +{ + // Normally would be pure virtual, but then pybind11 can't create bindings for this class + throw std::runtime_error("Internal error: Destroyable subclass didn't override destroy()"); +} + +void +Destroyable::destroy_when_not_in_use() +{ + if (please_destroy_) { + // already asked to destroy + return; + } + please_destroy_ = true; + if (0u == use_count) { + destroy(); + } +} + +void +define_destroyable(py::object module) +{ + py::class_>(module, "Destroyable") + .def("__enter__", &Destroyable::enter) + .def("__exit__", &Destroyable::exit) + .def( + "destroy_when_not_in_use", &Destroyable::destroy_when_not_in_use, + "Forcefully destroy the rcl object as soon as it's not actively being used"); +} +} // namespace python +} // namespace utils +} // namespace ignition diff --git a/python/src/ignition/gazebo/destroyable.hh b/python/src/ignition/gazebo/destroyable.hh new file mode 100644 index 0000000000..fd66a98226 --- /dev/null +++ b/python/src/ignition/gazebo/destroyable.hh @@ -0,0 +1,71 @@ +// 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__DESTROYABLE_HPP_ +#define IGNITION_GAZEBO_PYTHON__DESTROYABLE_HPP_ + +#include + +namespace py = pybind11; + +namespace ignition +{ +namespace utils +{ +namespace python +{ +/// This class blocks destruction when in use +class Destroyable +{ +public: + /// Default constructor + Destroyable() = default; + + /// Copy constructor + Destroyable(const Destroyable & other); + + /// Context manager __enter__ - block destruction + void + enter(); + + /// Context manager __exit__ - unblock destruction + void + exit(py::object pytype, py::object pyvalue, py::object pytraceback); + + /// Signal that the object should be destroyed as soon as it's not in use + void + destroy_when_not_in_use(); + + /// Override this to destroy an object + virtual + void + destroy(); + + virtual + ~Destroyable() = default; + +private: + size_t use_count = 0u; + bool please_destroy_ = false; +}; + +/// Define a pybind11 wrapper for an rclpy::Destroyable +/** + * \param[in] module a pybind11 module to add the definition to + */ +void define_destroyable(py::object module); +} // namespace python +} // namespace ignition +} // namespace gazebo +#endif // IGNITION_GAZEBO_PYTHON__DESTROYABLE_HPP_ diff --git a/python/src/ignition/gazebo/entity_component_manager.cc b/python/src/ignition/gazebo/entity_component_manager.cc new file mode 100644 index 0000000000..d3388aad85 --- /dev/null +++ b/python/src/ignition/gazebo/entity_component_manager.cc @@ -0,0 +1,57 @@ +// 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 "entity_component_manager.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +///////////////////////////////////////////////// +EntityComponentManager::EntityComponentManager(const ignition::gazebo::EntityComponentManager &_ecm) +{ + _entity_component_manager = &_ecm; +} + +///////////////////////////////////////////////// +EntityComponentManager::EntityComponentManager(ignition::gazebo::EntityComponentManager &_ecm) +{ + _entity_component_manager_no_const = &_ecm; +} + +///////////////////////////////////////////////// +EntityComponentManager::~EntityComponentManager() +{ +} + +///////////////////////////////////////////////// +void EntityComponentManager::destroy() +{ +} + +///////////////////////////////////////////////// +void define_gazebo_entity_component_manager(py::object module) +{ + py::class_>(module, "EntityComponentManager") + .def(py::init()); +} +} // namespace python +} // namespace gazebo +} // namespace ignition diff --git a/python/src/ignition/gazebo/entity_component_manager.hh b/python/src/ignition/gazebo/entity_component_manager.hh new file mode 100644 index 0000000000..569010b243 --- /dev/null +++ b/python/src/ignition/gazebo/entity_component_manager.hh @@ -0,0 +1,72 @@ +// 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__ENTITY_COMPONENT_MANAGER_HPP_ +#define IGNITION_GAZEBO_PYTHON__ENTITY_COMPONENT_MANAGER_HPP_ + +#include + +#include "ignition/gazebo/EntityComponentManager.hh" + +#include "destroyable.hh" + +namespace py = pybind11; + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ + + class EntityComponentManager : public ignition::utils::python::Destroyable, + public std::enable_shared_from_this + { + /// \brief Constructor + public: EntityComponentManager(const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Constructor + public: EntityComponentManager(ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Destructor + public: ~EntityComponentManager(); + + /// Force an early destruction of this object + void + destroy() override; + + /// Get rcl_client_t pointer + ignition::gazebo::EntityComponentManager * + rcl_ptr() const + { + return _entity_component_manager_no_const; + } + + private: + const ignition::gazebo::EntityComponentManager * _entity_component_manager; + ignition::gazebo::EntityComponentManager * _entity_component_manager_no_const; + }; + +/// Define a pybind11 wrapper for an ignition::gazebo::EntityComponentManager +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +define_gazebo_entity_component_manager(py::object module); + +} // namespace python +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PYTHON__ENTITY_COMPONENT_MANAGER_HPP_ diff --git a/python/src/ignition/gazebo/event_manager.cc b/python/src/ignition/gazebo/event_manager.cc new file mode 100644 index 0000000000..0c8a2f6858 --- /dev/null +++ b/python/src/ignition/gazebo/event_manager.cc @@ -0,0 +1,35 @@ +// 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 "ignition/gazebo/EventManager.hh" + +#include "event_manager.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +///////////////////////////////////////////////// +void define_gazebo_event_manager(py::object module) +{ + py::class_(module, "EventManager") + .def(py::init<>()); +} +} // namespace python +} // namespace gazebo +} // namespace ignition diff --git a/python/src/ignition/gazebo/event_manager.hh b/python/src/ignition/gazebo/event_manager.hh new file mode 100644 index 0000000000..38c0566b8d --- /dev/null +++ b/python/src/ignition/gazebo/event_manager.hh @@ -0,0 +1,39 @@ +// 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__EVENT_MANAGER_HPP_ +#define IGNITION_GAZEBO_PYTHON__EVENT_MANAGER_HPP_ + +#include + +namespace py = pybind11; + +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 +define_gazebo_event_manager(py::object module); + +} // namespace python +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PYTHON__EVENT_MANAGER_HPP_ diff --git a/python/src/ignition/gazebo/exceptions.hh b/python/src/ignition/gazebo/exceptions.hh new file mode 100644 index 0000000000..53a5df2275 --- /dev/null +++ b/python/src/ignition/gazebo/exceptions.hh @@ -0,0 +1,35 @@ +// 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__EXCEPTIONS_HPP_ +#define IGNITION_GAZEBO_PYTHON__EXCEPTIONS_HPP_ + +#include +#include + +namespace ignition +{ +namespace utils +{ +namespace python +{ +class InvalidHandle : public std::runtime_error +{ + using std::runtime_error::runtime_error; +}; +} // namespace utils +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PYTHON__EXCEPTIONS_HPP_ diff --git a/python/src/ignition/gazebo/helper_system.cc b/python/src/ignition/gazebo/helper_system.cc new file mode 100644 index 0000000000..3acf73f28e --- /dev/null +++ b/python/src/ignition/gazebo/helper_system.cc @@ -0,0 +1,217 @@ +/* + * 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 "helper_system.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +///////////////////////////////////////////////// +void HelperSystem::Configure( + const Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + EventManager &_eventMgr) +{ + if (this->configureCallback_internal) + { + auto ecm = ignition::gazebo::python::EntityComponentManager(_ecm); + this->configureCallback_internal(_entity, ecm); + } +} + +///////////////////////////////////////////////// +void HelperSystem::PreUpdate(const UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + if (this->preUpdateCallback_internal) + { + auto ecm = ignition::gazebo::python::EntityComponentManager(_ecm); + this->preUpdateCallback_internal(_info, ecm); + } +} + +///////////////////////////////////////////////// +void HelperSystem::Update(const UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + if (this->updateCallback_internal) + { + auto ecm = ignition::gazebo::python::EntityComponentManager(_ecm); + this->updateCallback_internal(_info, ecm); + } +} + +///////////////////////////////////////////////// +void HelperSystem::PostUpdate(const UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + if (this->postUpdateCallback_internal) + this->postUpdateCallback_internal(_info, ignition::gazebo::python::EntityComponentManager(_ecm)); +} + +////////////////////////////////////////////////// +class HelperFixturePrivate +{ + /// \brief Initialize fixture + /// \param[in] _config Server config + public: void Init(const ServerConfig &_config); + + /// \brief Pointer to underlying server + public: std::shared_ptr server{nullptr}; + + /// \brief Pointer to underlying Helper interface + public: std::shared_ptr helperSystem{nullptr}; + + /// \brief Flag to make sure Finalize is only called once + public: bool finalized{false}; +}; + +////////////////////////////////////////////////// +HelperFixture::HelperFixture(const std::string &_path) + : dataPtr(std::make_shared()) +{ + ServerConfig config; + config.SetSdfFile(_path); + this->dataPtr->Init(config); +} + +////////////////////////////////////////////////// +HelperFixture::HelperFixture(const ServerConfig &_config) + : dataPtr(new HelperFixturePrivate()) +{ + this->dataPtr->Init(_config); +} + +////////////////////////////////////////////////// +HelperFixture::~HelperFixture() +{ + dataPtr = nullptr; +} + +void +HelperFixture::destroy() +{ + dataPtr.reset(); +} + +////////////////////////////////////////////////// +void HelperFixturePrivate::Init(const ServerConfig &_config) +{ + this->helperSystem = std::make_shared(); + this->server = std::make_shared(_config); +} + +////////////////////////////////////////////////// +HelperFixture &HelperFixture::Finalize() +{ + if (this->dataPtr->finalized) + { + ignwarn << "Fixture has already been finalized, this only needs to be done" + << " once." << std::endl; + return *this; + } + + this->dataPtr->server->AddSystem(this->dataPtr->helperSystem); + + this->dataPtr->finalized = true; + return *this; +} + +////////////////////////////////////////////////// +HelperFixture &HelperFixture::OnConfigure(std::function _cb) +{ + if (nullptr != this->dataPtr->helperSystem) + this->dataPtr->helperSystem->configureCallback_internal = std::move(_cb); + return *this; +} + +////////////////////////////////////////////////// +HelperFixture &HelperFixture::OnPreUpdate(std::function _cb) +{ + if (nullptr != this->dataPtr->helperSystem) + this->dataPtr->helperSystem->preUpdateCallback_internal = std::move(_cb); + return *this; +} + +////////////////////////////////////////////////// +HelperFixture &HelperFixture::OnUpdate(std::function _cb) +{ + if (nullptr != this->dataPtr->helperSystem) + this->dataPtr->helperSystem->updateCallback_internal = std::move(_cb); + return *this; +} + +////////////////////////////////////////////////// +HelperFixture &HelperFixture::OnPostUpdate(std::function _cb) +{ + if (nullptr != this->dataPtr->helperSystem) + this->dataPtr->helperSystem->postUpdateCallback_internal = std::move(_cb); + return *this; +} + +////////////////////////////////////////////////// +std::shared_ptr HelperFixture::Server() const +{ + return this->dataPtr->server; +} + +void +define_gazebo_helper_fixture(py::object module) +{ + py::class_>(module, "HelperFixture") + .def(py::init()) + .def( + "server", &HelperFixture::Server, + "Get pointer to underlying server." + ) + .def( + "finalize", &HelperFixture::Finalize, + "Finalize all the functions and add fixture to server." + ) + .def( + "on_pre_update", &HelperFixture::OnPreUpdate, + "Wrapper around a system's pre-update callback" + ) + .def( + "on_update", &HelperFixture::OnUpdate, + "Wrapper around a system's update callback" + ) + .def( + "on_post_update", &HelperFixture::OnPostUpdate, + "Wrapper around a system's post-update callback" + ) + .def( + "on_configure", &HelperFixture::OnConfigure, + "Wrapper around a system's pre-update callback" + ); +} +} +} +} diff --git a/python/src/ignition/gazebo/helper_system.hh b/python/src/ignition/gazebo/helper_system.hh new file mode 100644 index 0000000000..b413bb2443 --- /dev/null +++ b/python/src/ignition/gazebo/helper_system.hh @@ -0,0 +1,159 @@ +/* + * 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__HELPER_SYSTEM_HPP_ +#define IGNITION_GAZEBO_PYTHON__HELPER_SYSTEM_HPP_ + +#include +#include +#include + +#include +#include +#include + +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Export.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/ServerConfig.hh" + +#include "destroyable.hh" +#include "entity_component_manager.hh" + +namespace py = pybind11; + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ + class HelperFixturePrivate; + + class HelperFixture : public ignition::utils::python::Destroyable, public std::enable_shared_from_this + { + /// \brief Constructor + /// \param[in] _path Path to SDF file. + public: explicit HelperFixture(const std::string &_path); + + /// \brief Constructor + /// \param[in] _config Server config file + public: explicit HelperFixture(const ServerConfig &_config); + + /// \brief Destructor + public: virtual ~HelperFixture(); + + /// Force an early destruction of this object + void + destroy() override; + + /// \brief Wrapper around a system's pre-update callback + /// \param[in] _cb Function to be called every pre-update + /// The _entity and _sdf will correspond to the world entity. + /// \return Reference to self. + public: HelperFixture &OnConfigure(std::function _cb); + + /// \brief Wrapper around a system's pre-update callback + /// \param[in] _cb Function to be called every pre-update + /// \return Reference to self. + public: HelperFixture &OnPreUpdate(std::function _cb); + + /// \brief Wrapper around a system's update callback + /// \param[in] _cb Function to be called every update + /// \return Reference to self. + public: HelperFixture &OnUpdate(std::function _cb); + + /// \brief Wrapper around a system's post-update callback + /// \param[in] _cb Function to be called every post-update + /// \return Reference to self. + public: HelperFixture &OnPostUpdate(std::function _cb); + + /// \brief Finalize all the functions and add fixture to server. + /// Finalize must be called before running the server, otherwise none of the + /// `On*` functions will be called. + /// The `OnConfigure` callback is called immediately on finalize. + public: HelperFixture &Finalize(); + + /// \brief Get pointer to underlying server. + public: std::shared_ptr Server() const; + + /// \internal + /// \brief Pointer to private data. + // TODO(chapulina) Use IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) when porting to v6 + private: std::shared_ptr dataPtr; + }; + +/// \brief System that is inserted into the simulation loop to observe the ECM. +class HelperSystem : + public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemUpdate, + public ISystemPostUpdate +{ + // Documentation inherited + public: void Configure( + const Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate(const UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + // Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Function to call every time we configure a world + public: std::function + configureCallback_internal; + + /// \brief Function to call every pre-update + public: std::function + preUpdateCallback_internal; + + /// \brief Function to call every update + public: std::function + updateCallback_internal; + + /// \brief Function to call every post-update + public: std::function postUpdateCallback_internal; +}; + +/// Define a pybind11 wrapper for an ignition::gazebo::HelperFixture +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +define_gazebo_helper_fixture(py::object module); +} +} +} + +#endif diff --git a/python/src/ignition/gazebo/server.cc b/python/src/ignition/gazebo/server.cc new file mode 100644 index 0000000000..318111b370 --- /dev/null +++ b/python/src/ignition/gazebo/server.cc @@ -0,0 +1,46 @@ +// 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 "server.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +void define_gazebo_server(py::object module) +{ + py::class_(module, "Server") + .def(py::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", + py::overload_cast<>(&ignition::gazebo::Server::Running, py::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..2353891f69 --- /dev/null +++ b/python/src/ignition/gazebo/server.hh @@ -0,0 +1,45 @@ +// 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__SERVER_HPP_ +#define IGNITION_GAZEBO_PYTHON__SERVER_HPP_ + +#include + +#include + +#include + +#include "server_config.hh" + +namespace py = pybind11; + +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 +define_gazebo_server(py::object module); + +} // namespace python +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HPP_ diff --git a/python/src/ignition/gazebo/server_config.cc b/python/src/ignition/gazebo/server_config.cc new file mode 100644 index 0000000000..2cf5c9b8af --- /dev/null +++ b/python/src/ignition/gazebo/server_config.cc @@ -0,0 +1,35 @@ +// 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 "server_config.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +void define_server_config(py::object module) +{ + py::class_(module, "ServerConfig") + .def(py::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/server_config.hh b/python/src/ignition/gazebo/server_config.hh new file mode 100644 index 0000000000..61882c50df --- /dev/null +++ b/python/src/ignition/gazebo/server_config.hh @@ -0,0 +1,42 @@ +// 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__SERVER_CONFIG_HPP_ +#define IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HPP_ + +#include + +#include + +#include + +namespace py = pybind11; + +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 +define_server_config(py::object module); +} // namespace python +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HPP_ diff --git a/python/src/ignition/gazebo/update_info.cc b/python/src/ignition/gazebo/update_info.cc new file mode 100644 index 0000000000..a2bd0c5553 --- /dev/null +++ b/python/src/ignition/gazebo/update_info.cc @@ -0,0 +1,41 @@ +// 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 + +#include + +#include "update_info.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +void define_gazebo_update_info(py::object module) +{ + py::class_(module, "UpdateInfo") + .def(py::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/update_info.hh b/python/src/ignition/gazebo/update_info.hh new file mode 100644 index 0000000000..9e93b014a1 --- /dev/null +++ b/python/src/ignition/gazebo/update_info.hh @@ -0,0 +1,43 @@ +// 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__UPDATE_INFO_HPP_ +#define IGNITION_GAZEBO_PYTHON__UPDATE_INFO_HPP_ + +#include + +#include + +#include + +namespace py = pybind11; + +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 +define_gazebo_update_info(py::object module); + +} // namespace python +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HPP_ diff --git a/python/src/ignition/gazebo/world.cc b/python/src/ignition/gazebo/world.cc new file mode 100644 index 0000000000..d8b34e0331 --- /dev/null +++ b/python/src/ignition/gazebo/world.cc @@ -0,0 +1,75 @@ +// 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 + +#include "world.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +World::World(ignition::gazebo::Entity _entity) +{ + _world = std::make_shared(_entity); +} + +World::~World() +{ +} + +void World::destroy() +{ + _world.reset(); +} + +ignition::math::Vector3 World::Gravity(const EntityComponentManager &_ecm) +{ + std::optional gravity = _world->Gravity(*_ecm.rcl_ptr()); + if (gravity.has_value()) + { + return gravity.value(); + } + return ignition::math::Vector3d::NaN; +} + +ignition::gazebo::Entity World::ModelByName( + ignition::gazebo::python::EntityComponentManager &_ecm, + std::string _name) +{ + return _world->ModelByName(*_ecm.rcl_ptr(), _name); +} + +void define_gazebo_world(py::object module) +{ + py::class_>(module, "World") + .def(py::init()) + .def( + "model_by_name", &ignition::gazebo::python::World::ModelByName, + "Get the ID of a model entity which is an immediate child of " + " this world.") + .def( + "gravity", &ignition::gazebo::python::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..6a84d6d86a --- /dev/null +++ b/python/src/ignition/gazebo/world.hh @@ -0,0 +1,75 @@ +// 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__WORLD_HPP_ +#define IGNITION_GAZEBO_PYTHON__WORLD_HPP_ + +#include + +#include +#include + +#include "entity_component_manager.hh" + +#include + +namespace py = pybind11; + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +class World : public ignition::utils::python::Destroyable, + public std::enable_shared_from_this +{ + public: World(ignition::gazebo::Entity _entity = kNullEntity); + public: ~World(); + + /// \brief Get the gravity in m/s^2. + /// \param[in] _ecm Entity-component manager. + /// \return Gravity vector or nullopt if the entity does not + /// have a components::Gravity component. + public: ignition::math::Vector3 + Gravity(const EntityComponentManager &_ecm); + + /// \brief Get the ID of a model entity which is an immediate child of + /// this world. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _name Model name. + /// \return Model entity. + public: ignition::gazebo::Entity ModelByName( + ignition::gazebo::python::EntityComponentManager &_ecm, + std::string _name); + + /// Force an early destruction of this object + void destroy() override; + +private: + std::shared_ptr _world; +}; + +/// Define a pybind11 wrapper for an ignition::gazebo::Server +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +define_gazebo_world(py::object module); + +} // namespace python +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HPP_ From 3bc8e374f32031daec88c104f625797fdc7e6368 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 25 Nov 2021 10:35:34 +0100 Subject: [PATCH 02/21] Added feedback Signed-off-by: ahcorde --- examples/python/helperFixture.py | 13 +++++ .../common/_ignition_common_pybind11.cc | 2 - python/src/ignition/common/console.hh | 2 - .../gazebo/_ignition_gazebo_pybind11.cc | 6 +-- python/src/ignition/gazebo/destroyable.cc | 24 ++++++---- python/src/ignition/gazebo/destroyable.hh | 12 ++--- .../gazebo/entity_component_manager.cc | 17 ++++--- .../gazebo/entity_component_manager.hh | 24 +++++----- python/src/ignition/gazebo/event_manager.cc | 6 +-- python/src/ignition/gazebo/event_manager.hh | 4 +- python/src/ignition/gazebo/exceptions.hh | 2 +- python/src/ignition/gazebo/helper_system.cc | 18 +++---- python/src/ignition/gazebo/helper_system.hh | 48 +++++++++++-------- python/src/ignition/gazebo/server.cc | 11 +++-- python/src/ignition/gazebo/server.hh | 10 +--- python/src/ignition/gazebo/server_config.cc | 8 ++-- python/src/ignition/gazebo/server_config.hh | 8 +--- python/src/ignition/gazebo/update_info.cc | 8 ++-- python/src/ignition/gazebo/update_info.hh | 8 +--- python/src/ignition/gazebo/world.cc | 14 +++--- python/src/ignition/gazebo/world.hh | 6 +-- 21 files changed, 130 insertions(+), 121 deletions(-) diff --git a/examples/python/helperFixture.py b/examples/python/helperFixture.py index d80ed249d1..0ba7f11d34 100644 --- a/examples/python/helperFixture.py +++ b/examples/python/helperFixture.py @@ -1,4 +1,17 @@ #!/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. import os import time diff --git a/python/src/ignition/common/_ignition_common_pybind11.cc b/python/src/ignition/common/_ignition_common_pybind11.cc index 40c90d31ef..9122350b25 100644 --- a/python/src/ignition/common/_ignition_common_pybind11.cc +++ b/python/src/ignition/common/_ignition_common_pybind11.cc @@ -16,8 +16,6 @@ #include "console.hh" -namespace py = pybind11; - PYBIND11_MODULE(common, m) { m.doc() = "Ignition Common Python Library."; diff --git a/python/src/ignition/common/console.hh b/python/src/ignition/common/console.hh index ba0597f19a..ce05dafb80 100644 --- a/python/src/ignition/common/console.hh +++ b/python/src/ignition/common/console.hh @@ -17,8 +17,6 @@ #include -namespace py = pybind11; - namespace ignition { namespace common diff --git a/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc b/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc index 15c0860dda..b6c2d24518 100644 --- a/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc +++ b/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc @@ -24,14 +24,12 @@ #include "update_info.hh" #include "world.hh" -namespace py = pybind11; - PYBIND11_MODULE(gazebo, m) { m.doc() = "Ignition Gazebo Python Library."; - ignition::utils::python::define_destroyable(m); + ignition::gazebo::python::define_destroyable(m); - py::register_exception( + pybind11::register_exception( m, "InvalidHandle", PyExc_RuntimeError); ignition::gazebo::python::define_gazebo_entity_component_manager(m); diff --git a/python/src/ignition/gazebo/destroyable.cc b/python/src/ignition/gazebo/destroyable.cc index 55833eeed0..04e2f83407 100644 --- a/python/src/ignition/gazebo/destroyable.cc +++ b/python/src/ignition/gazebo/destroyable.cc @@ -21,7 +21,7 @@ namespace ignition { -namespace utils +namespace gazebo { namespace python { @@ -43,10 +43,11 @@ Destroyable::enter() } void -Destroyable::exit(py::object, py::object, py::object) +Destroyable::exit(pybind11::object, pybind11::object, pybind11::object) { if (0u == use_count) { - throw std::runtime_error("Internal error: Destroyable use_count would be negative"); + throw std::runtime_error("Internal error: " + "Destroyable use_count would be negative"); } --use_count; @@ -58,8 +59,10 @@ Destroyable::exit(py::object, py::object, py::object) void Destroyable::destroy() { - // Normally would be pure virtual, but then pybind11 can't create bindings for this class - throw std::runtime_error("Internal error: Destroyable subclass didn't override destroy()"); + // Normally would be pure virtual, but then pybind11 can't + // create bindings for this class + throw std::runtime_error("Internal error : " + "Destroyable subclass didn't override destroy()"); } void @@ -76,14 +79,17 @@ Destroyable::destroy_when_not_in_use() } void -define_destroyable(py::object module) +define_destroyable(pybind11::object module) { - py::class_>(module, "Destroyable") + pybind11::class_>( + module, "Destroyable") .def("__enter__", &Destroyable::enter) .def("__exit__", &Destroyable::exit) .def( - "destroy_when_not_in_use", &Destroyable::destroy_when_not_in_use, - "Forcefully destroy the rcl object as soon as it's not actively being used"); + "destroy_when_not_in_use", + &Destroyable::destroy_when_not_in_use, + "Forcefully destroy the rcl object as soon as it's not actively " + "being used"); } } // namespace python } // namespace utils diff --git a/python/src/ignition/gazebo/destroyable.hh b/python/src/ignition/gazebo/destroyable.hh index fd66a98226..32f25b5c2c 100644 --- a/python/src/ignition/gazebo/destroyable.hh +++ b/python/src/ignition/gazebo/destroyable.hh @@ -17,11 +17,9 @@ #include -namespace py = pybind11; - namespace ignition { -namespace utils +namespace gazebo { namespace python { @@ -41,7 +39,9 @@ public: /// Context manager __exit__ - unblock destruction void - exit(py::object pytype, py::object pyvalue, py::object pytraceback); + exit(pybind11::object pytype, + pybind11::object pyvalue, + pybind11::object pytraceback); /// Signal that the object should be destroyed as soon as it's not in use void @@ -60,11 +60,11 @@ private: bool please_destroy_ = false; }; -/// Define a pybind11 wrapper for an rclpy::Destroyable +/// Define a pybind11 wrapper for an rclpybind11::Destroyable /** * \param[in] module a pybind11 module to add the definition to */ -void define_destroyable(py::object module); +void define_destroyable(pybind11::object module); } // namespace python } // namespace ignition } // namespace gazebo diff --git a/python/src/ignition/gazebo/entity_component_manager.cc b/python/src/ignition/gazebo/entity_component_manager.cc index d3388aad85..9631b018f1 100644 --- a/python/src/ignition/gazebo/entity_component_manager.cc +++ b/python/src/ignition/gazebo/entity_component_manager.cc @@ -23,13 +23,15 @@ namespace gazebo namespace python { ///////////////////////////////////////////////// -EntityComponentManager::EntityComponentManager(const ignition::gazebo::EntityComponentManager &_ecm) +EntityComponentManager::EntityComponentManager( + const ignition::gazebo::EntityComponentManager &_ecm) { _entity_component_manager = &_ecm; } ///////////////////////////////////////////////// -EntityComponentManager::EntityComponentManager(ignition::gazebo::EntityComponentManager &_ecm) +EntityComponentManager::EntityComponentManager( + ignition::gazebo::EntityComponentManager &_ecm) { _entity_component_manager_no_const = &_ecm; } @@ -45,12 +47,13 @@ void EntityComponentManager::destroy() } ///////////////////////////////////////////////// -void define_gazebo_entity_component_manager(py::object module) +void define_gazebo_entity_component_manager(pybind11::object module) { - py::class_>(module, "EntityComponentManager") - .def(py::init()); + pybind11::class_>( + module, "EntityComponentManager") + .def(pybind11::init()); } } // namespace python } // namespace gazebo diff --git a/python/src/ignition/gazebo/entity_component_manager.hh b/python/src/ignition/gazebo/entity_component_manager.hh index 569010b243..7bb2a792bb 100644 --- a/python/src/ignition/gazebo/entity_component_manager.hh +++ b/python/src/ignition/gazebo/entity_component_manager.hh @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IGNITION_GAZEBO_PYTHON__ENTITY_COMPONENT_MANAGER_HPP_ -#define IGNITION_GAZEBO_PYTHON__ENTITY_COMPONENT_MANAGER_HPP_ +#ifndef IGNITION_GAZEBO_PYTHON__ENTITY_COMPONENT_MANAGER_HH_ +#define IGNITION_GAZEBO_PYTHON__ENTITY_COMPONENT_MANAGER_HH_ #include @@ -21,8 +21,6 @@ #include "destroyable.hh" -namespace py = pybind11; - namespace ignition { namespace gazebo @@ -30,14 +28,17 @@ namespace gazebo namespace python { - class EntityComponentManager : public ignition::utils::python::Destroyable, - public std::enable_shared_from_this + class EntityComponentManager : + public ignition::gazebo::python::Destroyable, + public std::enable_shared_from_this { /// \brief Constructor - public: EntityComponentManager(const ignition::gazebo::EntityComponentManager &_ecm); + public: EntityComponentManager( + const ignition::gazebo::EntityComponentManager &_ecm); /// \brief Constructor - public: EntityComponentManager(ignition::gazebo::EntityComponentManager &_ecm); + public: EntityComponentManager( + ignition::gazebo::EntityComponentManager &_ecm); /// \brief Destructor public: ~EntityComponentManager(); @@ -55,7 +56,8 @@ namespace python private: const ignition::gazebo::EntityComponentManager * _entity_component_manager; - ignition::gazebo::EntityComponentManager * _entity_component_manager_no_const; + ignition::gazebo::EntityComponentManager * + _entity_component_manager_no_const; }; /// Define a pybind11 wrapper for an ignition::gazebo::EntityComponentManager @@ -63,10 +65,10 @@ namespace python * \param[in] module a pybind11 module to add the definition to */ void -define_gazebo_entity_component_manager(py::object module); +define_gazebo_entity_component_manager(pybind11::object module); } // namespace python } // namespace gazebo } // namespace ignition -#endif // IGNITION_GAZEBO_PYTHON__ENTITY_COMPONENT_MANAGER_HPP_ +#endif // IGNITION_GAZEBO_PYTHON__ENTITY_COMPONENT_MANAGER_HH_ diff --git a/python/src/ignition/gazebo/event_manager.cc b/python/src/ignition/gazebo/event_manager.cc index 0c8a2f6858..fbd3bfb2a0 100644 --- a/python/src/ignition/gazebo/event_manager.cc +++ b/python/src/ignition/gazebo/event_manager.cc @@ -25,10 +25,10 @@ namespace gazebo namespace python { ///////////////////////////////////////////////// -void define_gazebo_event_manager(py::object module) +void define_gazebo_event_manager(pybind11::object module) { - py::class_(module, "EventManager") - .def(py::init<>()); + pybind11::class_(module, "EventManager") + .def(pybind11::init<>()); } } // namespace python } // namespace gazebo diff --git a/python/src/ignition/gazebo/event_manager.hh b/python/src/ignition/gazebo/event_manager.hh index 38c0566b8d..f07c157094 100644 --- a/python/src/ignition/gazebo/event_manager.hh +++ b/python/src/ignition/gazebo/event_manager.hh @@ -17,8 +17,6 @@ #include -namespace py = pybind11; - namespace ignition { namespace gazebo @@ -30,7 +28,7 @@ namespace python * \param[in] module a pybind11 module to add the definition to */ void -define_gazebo_event_manager(py::object module); +define_gazebo_event_manager(pybind11::object module); } // namespace python } // namespace gazebo diff --git a/python/src/ignition/gazebo/exceptions.hh b/python/src/ignition/gazebo/exceptions.hh index 53a5df2275..6c69479d23 100644 --- a/python/src/ignition/gazebo/exceptions.hh +++ b/python/src/ignition/gazebo/exceptions.hh @@ -20,7 +20,7 @@ namespace ignition { -namespace utils +namespace gazebo { namespace python { diff --git a/python/src/ignition/gazebo/helper_system.cc b/python/src/ignition/gazebo/helper_system.cc index 3acf73f28e..caa8aa5783 100644 --- a/python/src/ignition/gazebo/helper_system.cc +++ b/python/src/ignition/gazebo/helper_system.cc @@ -66,7 +66,8 @@ void HelperSystem::PostUpdate(const UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm) { if (this->postUpdateCallback_internal) - this->postUpdateCallback_internal(_info, ignition::gazebo::python::EntityComponentManager(_ecm)); + this->postUpdateCallback_internal(_info, + ignition::gazebo::python::EntityComponentManager(_ecm)); } ////////////////////////////////////////////////// @@ -149,7 +150,7 @@ HelperFixture &HelperFixture::OnConfigure(std::function _cb) + const UpdateInfo &, ignition::gazebo::python::EntityComponentManager &)> _cb) { if (nullptr != this->dataPtr->helperSystem) this->dataPtr->helperSystem->preUpdateCallback_internal = std::move(_cb); @@ -158,7 +159,7 @@ HelperFixture &HelperFixture::OnPreUpdate(std::function _cb) + const UpdateInfo &, ignition::gazebo::python::EntityComponentManager &)> _cb) { if (nullptr != this->dataPtr->helperSystem) this->dataPtr->helperSystem->updateCallback_internal = std::move(_cb); @@ -167,7 +168,8 @@ HelperFixture &HelperFixture::OnUpdate(std::function _cb) + const UpdateInfo &, + const ignition::gazebo::python::EntityComponentManager &)> _cb) { if (nullptr != this->dataPtr->helperSystem) this->dataPtr->helperSystem->postUpdateCallback_internal = std::move(_cb); @@ -181,12 +183,12 @@ std::shared_ptr HelperFixture::Server() const } void -define_gazebo_helper_fixture(py::object module) +define_gazebo_helper_fixture(pybind11::object module) { - py::class_>(module, "HelperFixture") - .def(py::init()) + .def(pybind11::init()) .def( "server", &HelperFixture::Server, "Get pointer to underlying server." diff --git a/python/src/ignition/gazebo/helper_system.hh b/python/src/ignition/gazebo/helper_system.hh index b413bb2443..cd5a15faa1 100644 --- a/python/src/ignition/gazebo/helper_system.hh +++ b/python/src/ignition/gazebo/helper_system.hh @@ -33,8 +33,6 @@ #include "destroyable.hh" #include "entity_component_manager.hh" -namespace py = pybind11; - namespace ignition { namespace gazebo @@ -43,7 +41,9 @@ namespace python { class HelperFixturePrivate; - class HelperFixture : public ignition::utils::python::Destroyable, public std::enable_shared_from_this + class HelperFixture : + public ignition::gazebo::python::Destroyable, + public std::enable_shared_from_this { /// \brief Constructor /// \param[in] _path Path to SDF file. @@ -72,23 +72,26 @@ namespace python /// \param[in] _cb Function to be called every pre-update /// \return Reference to self. public: HelperFixture &OnPreUpdate(std::function _cb); + const UpdateInfo &, + ignition::gazebo::python::EntityComponentManager &)> _cb); /// \brief Wrapper around a system's update callback /// \param[in] _cb Function to be called every update /// \return Reference to self. public: HelperFixture &OnUpdate(std::function _cb); + const UpdateInfo &, + ignition::gazebo::python::EntityComponentManager &)> _cb); /// \brief Wrapper around a system's post-update callback /// \param[in] _cb Function to be called every post-update /// \return Reference to self. public: HelperFixture &OnPostUpdate(std::function _cb); + const UpdateInfo &, + const ignition::gazebo::python::EntityComponentManager &)> _cb); /// \brief Finalize all the functions and add fixture to server. - /// Finalize must be called before running the server, otherwise none of the - /// `On*` functions will be called. + /// Finalize must be called before running the server, otherwise none of + /// the `On*` functions will be called. /// The `OnConfigure` callback is called immediately on finalize. public: HelperFixture &Finalize(); @@ -97,7 +100,7 @@ namespace python /// \internal /// \brief Pointer to private data. - // TODO(chapulina) Use IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) when porting to v6 + // TODO(ahcorde) Use IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) when porting to v6 private: std::shared_ptr dataPtr; }; @@ -111,39 +114,42 @@ class HelperSystem : { // Documentation inherited public: void Configure( - const Entity &_entity, - const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &_eventMgr) override; + const Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) override; // Documentation inherited public: void PreUpdate(const UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) override; + ignition::gazebo::EntityComponentManager &_ecm) override; // Documentation inherited public: void Update(const UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) override; + ignition::gazebo::EntityComponentManager &_ecm) override; // Documentation inherited public: void PostUpdate(const UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) override; + const ignition::gazebo::EntityComponentManager &_ecm) override; /// \brief Function to call every time we configure a world public: std::function + ignition::gazebo::python::EntityComponentManager &_ecm)> configureCallback_internal; /// \brief Function to call every pre-update - public: std::function + public: std::function preUpdateCallback_internal; /// \brief Function to call every update - public: std::function + public: std::function updateCallback_internal; /// \brief Function to call every post-update public: std::function postUpdateCallback_internal; + const ignition::gazebo::python::EntityComponentManager &)> + postUpdateCallback_internal; }; /// Define a pybind11 wrapper for an ignition::gazebo::HelperFixture @@ -151,7 +157,7 @@ class HelperSystem : * \param[in] module a pybind11 module to add the definition to */ void -define_gazebo_helper_fixture(py::object module); +define_gazebo_helper_fixture(pybind11::object module); } } } diff --git a/python/src/ignition/gazebo/server.cc b/python/src/ignition/gazebo/server.cc index 318111b370..64a2128441 100644 --- a/python/src/ignition/gazebo/server.cc +++ b/python/src/ignition/gazebo/server.cc @@ -14,6 +14,9 @@ #include +#include +#include + #include "server.hh" namespace ignition @@ -22,10 +25,10 @@ namespace gazebo { namespace python { -void define_gazebo_server(py::object module) +void define_gazebo_server(pybind11::object module) { - py::class_(module, "Server") - .def(py::init()) + pybind11::class_(module, "Server") + .def(pybind11::init()) .def( "run", &ignition::gazebo::Server::Run, "Run the server. By default this is a non-blocking call, " @@ -37,7 +40,7 @@ void define_gazebo_server(py::object module) "Return true if the specified world has an entity with the provided name.") .def( "is_running", - py::overload_cast<>(&ignition::gazebo::Server::Running, py::const_), + pybind11::overload_cast<>(&ignition::gazebo::Server::Running, pybind11::const_), "Get whether the server is running."); } diff --git a/python/src/ignition/gazebo/server.hh b/python/src/ignition/gazebo/server.hh index 2353891f69..91358e0879 100644 --- a/python/src/ignition/gazebo/server.hh +++ b/python/src/ignition/gazebo/server.hh @@ -17,14 +17,6 @@ #include -#include - -#include - -#include "server_config.hh" - -namespace py = pybind11; - namespace ignition { namespace gazebo @@ -36,7 +28,7 @@ namespace python * \param[in] module a pybind11 module to add the definition to */ void -define_gazebo_server(py::object module); +define_gazebo_server(pybind11::object module); } // namespace python } // namespace gazebo diff --git a/python/src/ignition/gazebo/server_config.cc b/python/src/ignition/gazebo/server_config.cc index 2cf5c9b8af..15f76cfac7 100644 --- a/python/src/ignition/gazebo/server_config.cc +++ b/python/src/ignition/gazebo/server_config.cc @@ -14,6 +14,8 @@ #include +#include + #include "server_config.hh" namespace ignition @@ -22,10 +24,10 @@ namespace gazebo { namespace python { -void define_server_config(py::object module) +void define_server_config(pybind11::object module) { - py::class_(module, "ServerConfig") - .def(py::init<>()) + 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."); diff --git a/python/src/ignition/gazebo/server_config.hh b/python/src/ignition/gazebo/server_config.hh index 61882c50df..9cfa4984fd 100644 --- a/python/src/ignition/gazebo/server_config.hh +++ b/python/src/ignition/gazebo/server_config.hh @@ -17,12 +17,6 @@ #include -#include - -#include - -namespace py = pybind11; - namespace ignition { namespace gazebo @@ -34,7 +28,7 @@ namespace python * \param[in] module a pybind11 module to add the definition to */ void -define_server_config(py::object module); +define_server_config(pybind11::object module); } // namespace python } // namespace gazebo } // namespace ignition diff --git a/python/src/ignition/gazebo/update_info.cc b/python/src/ignition/gazebo/update_info.cc index a2bd0c5553..736ae8d510 100644 --- a/python/src/ignition/gazebo/update_info.cc +++ b/python/src/ignition/gazebo/update_info.cc @@ -15,7 +15,7 @@ #include #include -#include +#include #include "update_info.hh" @@ -25,10 +25,10 @@ namespace gazebo { namespace python { -void define_gazebo_update_info(py::object module) +void define_gazebo_update_info(pybind11::object module) { - py::class_(module, "UpdateInfo") - .def(py::init<>()) + 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) diff --git a/python/src/ignition/gazebo/update_info.hh b/python/src/ignition/gazebo/update_info.hh index 9e93b014a1..c8484e4c97 100644 --- a/python/src/ignition/gazebo/update_info.hh +++ b/python/src/ignition/gazebo/update_info.hh @@ -17,12 +17,6 @@ #include -#include - -#include - -namespace py = pybind11; - namespace ignition { namespace gazebo @@ -34,7 +28,7 @@ namespace python * \param[in] module a pybind11 module to add the definition to */ void -define_gazebo_update_info(py::object module); +define_gazebo_update_info(pybind11::object module); } // namespace python } // namespace gazebo diff --git a/python/src/ignition/gazebo/world.cc b/python/src/ignition/gazebo/world.cc index d8b34e0331..79b18136d6 100644 --- a/python/src/ignition/gazebo/world.cc +++ b/python/src/ignition/gazebo/world.cc @@ -38,9 +38,11 @@ void World::destroy() _world.reset(); } -ignition::math::Vector3 World::Gravity(const EntityComponentManager &_ecm) +ignition::math::Vector3 World::Gravity( + const EntityComponentManager &_ecm) { - std::optional gravity = _world->Gravity(*_ecm.rcl_ptr()); + std::optional gravity = + _world->Gravity(*_ecm.rcl_ptr()); if (gravity.has_value()) { return gravity.value(); @@ -55,12 +57,12 @@ ignition::gazebo::Entity World::ModelByName( return _world->ModelByName(*_ecm.rcl_ptr(), _name); } -void define_gazebo_world(py::object module) +void define_gazebo_world(pybind11::object module) { - py::class_>(module, "World") - .def(py::init()) + .def(pybind11::init()) .def( "model_by_name", &ignition::gazebo::python::World::ModelByName, "Get the ID of a model entity which is an immediate child of " diff --git a/python/src/ignition/gazebo/world.hh b/python/src/ignition/gazebo/world.hh index 6a84d6d86a..48aa654579 100644 --- a/python/src/ignition/gazebo/world.hh +++ b/python/src/ignition/gazebo/world.hh @@ -24,15 +24,13 @@ #include -namespace py = pybind11; - namespace ignition { namespace gazebo { namespace python { -class World : public ignition::utils::python::Destroyable, +class World : public ignition::gazebo::python::Destroyable, public std::enable_shared_from_this { public: World(ignition::gazebo::Entity _entity = kNullEntity); @@ -66,7 +64,7 @@ private: * \param[in] module a pybind11 module to add the definition to */ void -define_gazebo_world(py::object module); +define_gazebo_world(pybind11::object module); } // namespace python } // namespace gazebo From 4106c2b9b7b89f4a0b36ef304979afb0d7ad7b4c Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 25 Nov 2021 10:41:49 +0100 Subject: [PATCH 03/21] Fixed License style Signed-off-by: ahcorde --- .../gazebo/_ignition_gazebo_pybind11.cc | 28 +++++++++--------- python/src/ignition/gazebo/destroyable.cc | 29 ++++++++++--------- python/src/ignition/gazebo/destroyable.hh | 28 +++++++++--------- .../gazebo/entity_component_manager.cc | 29 ++++++++++--------- .../gazebo/entity_component_manager.hh | 28 +++++++++--------- python/src/ignition/gazebo/event_manager.cc | 28 +++++++++--------- python/src/ignition/gazebo/event_manager.hh | 29 ++++++++++--------- python/src/ignition/gazebo/exceptions.hh | 29 ++++++++++--------- python/src/ignition/gazebo/server.cc | 29 ++++++++++--------- python/src/ignition/gazebo/server.hh | 29 ++++++++++--------- python/src/ignition/gazebo/server_config.cc | 29 ++++++++++--------- python/src/ignition/gazebo/server_config.hh | 29 ++++++++++--------- python/src/ignition/gazebo/update_info.cc | 29 ++++++++++--------- python/src/ignition/gazebo/update_info.hh | 29 ++++++++++--------- python/src/ignition/gazebo/world.cc | 29 ++++++++++--------- python/src/ignition/gazebo/world.hh | 28 +++++++++--------- 16 files changed, 249 insertions(+), 210 deletions(-) diff --git a/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc b/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc index b6c2d24518..0a7a0343f5 100644 --- a/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc +++ b/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc @@ -1,16 +1,18 @@ -// 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. +/* + * 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 diff --git a/python/src/ignition/gazebo/destroyable.cc b/python/src/ignition/gazebo/destroyable.cc index 04e2f83407..d209fc1b65 100644 --- a/python/src/ignition/gazebo/destroyable.cc +++ b/python/src/ignition/gazebo/destroyable.cc @@ -1,17 +1,18 @@ -// 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. - +/* + * 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 diff --git a/python/src/ignition/gazebo/destroyable.hh b/python/src/ignition/gazebo/destroyable.hh index 32f25b5c2c..a174c9aafe 100644 --- a/python/src/ignition/gazebo/destroyable.hh +++ b/python/src/ignition/gazebo/destroyable.hh @@ -1,16 +1,18 @@ -// 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. +/* + * 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__DESTROYABLE_HPP_ #define IGNITION_GAZEBO_PYTHON__DESTROYABLE_HPP_ diff --git a/python/src/ignition/gazebo/entity_component_manager.cc b/python/src/ignition/gazebo/entity_component_manager.cc index 9631b018f1..29dc7580a9 100644 --- a/python/src/ignition/gazebo/entity_component_manager.cc +++ b/python/src/ignition/gazebo/entity_component_manager.cc @@ -1,17 +1,18 @@ -// 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. - +/* + * 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 "entity_component_manager.hh" diff --git a/python/src/ignition/gazebo/entity_component_manager.hh b/python/src/ignition/gazebo/entity_component_manager.hh index 7bb2a792bb..23f6b5a7e2 100644 --- a/python/src/ignition/gazebo/entity_component_manager.hh +++ b/python/src/ignition/gazebo/entity_component_manager.hh @@ -1,16 +1,18 @@ -// 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. +/* + * 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_ diff --git a/python/src/ignition/gazebo/event_manager.cc b/python/src/ignition/gazebo/event_manager.cc index fbd3bfb2a0..d6fe78c2eb 100644 --- a/python/src/ignition/gazebo/event_manager.cc +++ b/python/src/ignition/gazebo/event_manager.cc @@ -1,16 +1,18 @@ -// 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. +/* + * 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 diff --git a/python/src/ignition/gazebo/event_manager.hh b/python/src/ignition/gazebo/event_manager.hh index f07c157094..6121ee10db 100644 --- a/python/src/ignition/gazebo/event_manager.hh +++ b/python/src/ignition/gazebo/event_manager.hh @@ -1,16 +1,19 @@ -// 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. +/* + * 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_HPP_ #define IGNITION_GAZEBO_PYTHON__EVENT_MANAGER_HPP_ diff --git a/python/src/ignition/gazebo/exceptions.hh b/python/src/ignition/gazebo/exceptions.hh index 6c69479d23..fd45e3c0a2 100644 --- a/python/src/ignition/gazebo/exceptions.hh +++ b/python/src/ignition/gazebo/exceptions.hh @@ -1,16 +1,19 @@ -// 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. +/* + * 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__EXCEPTIONS_HPP_ #define IGNITION_GAZEBO_PYTHON__EXCEPTIONS_HPP_ diff --git a/python/src/ignition/gazebo/server.cc b/python/src/ignition/gazebo/server.cc index 64a2128441..d5c7d97b0a 100644 --- a/python/src/ignition/gazebo/server.cc +++ b/python/src/ignition/gazebo/server.cc @@ -1,16 +1,19 @@ -// 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. +/* + * 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 diff --git a/python/src/ignition/gazebo/server.hh b/python/src/ignition/gazebo/server.hh index 91358e0879..2b7dbdda1a 100644 --- a/python/src/ignition/gazebo/server.hh +++ b/python/src/ignition/gazebo/server.hh @@ -1,16 +1,19 @@ -// 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. +/* + * 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_HPP_ #define IGNITION_GAZEBO_PYTHON__SERVER_HPP_ diff --git a/python/src/ignition/gazebo/server_config.cc b/python/src/ignition/gazebo/server_config.cc index 15f76cfac7..479b84aa79 100644 --- a/python/src/ignition/gazebo/server_config.cc +++ b/python/src/ignition/gazebo/server_config.cc @@ -1,16 +1,19 @@ -// 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. +/* + * 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 diff --git a/python/src/ignition/gazebo/server_config.hh b/python/src/ignition/gazebo/server_config.hh index 9cfa4984fd..c591b5792d 100644 --- a/python/src/ignition/gazebo/server_config.hh +++ b/python/src/ignition/gazebo/server_config.hh @@ -1,16 +1,19 @@ -// 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. +/* + * 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_HPP_ #define IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HPP_ diff --git a/python/src/ignition/gazebo/update_info.cc b/python/src/ignition/gazebo/update_info.cc index 736ae8d510..1fe07325e3 100644 --- a/python/src/ignition/gazebo/update_info.cc +++ b/python/src/ignition/gazebo/update_info.cc @@ -1,16 +1,19 @@ -// 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. +/* + * 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 diff --git a/python/src/ignition/gazebo/update_info.hh b/python/src/ignition/gazebo/update_info.hh index c8484e4c97..916db1d54f 100644 --- a/python/src/ignition/gazebo/update_info.hh +++ b/python/src/ignition/gazebo/update_info.hh @@ -1,16 +1,19 @@ -// 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. +/* + * 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_HPP_ #define IGNITION_GAZEBO_PYTHON__UPDATE_INFO_HPP_ diff --git a/python/src/ignition/gazebo/world.cc b/python/src/ignition/gazebo/world.cc index 79b18136d6..2aed785f2b 100644 --- a/python/src/ignition/gazebo/world.cc +++ b/python/src/ignition/gazebo/world.cc @@ -1,16 +1,19 @@ -// 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. +/* + * 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 diff --git a/python/src/ignition/gazebo/world.hh b/python/src/ignition/gazebo/world.hh index 48aa654579..d7a3130da4 100644 --- a/python/src/ignition/gazebo/world.hh +++ b/python/src/ignition/gazebo/world.hh @@ -1,16 +1,18 @@ -// 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. +/* + * 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_HPP_ #define IGNITION_GAZEBO_PYTHON__WORLD_HPP_ From 7a54950ece05c3c4d029eca71be5d1b6ff12cbd2 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 25 Nov 2021 10:48:30 +0100 Subject: [PATCH 04/21] Fix style in destroyable class Signed-off-by: ahcorde --- python/src/ignition/gazebo/destroyable.cc | 18 +++++++++--------- python/src/ignition/gazebo/destroyable.hh | 12 +++++++----- .../gazebo/entity_component_manager.cc | 2 +- .../gazebo/entity_component_manager.hh | 2 +- python/src/ignition/gazebo/helper_system.cc | 6 +++--- python/src/ignition/gazebo/helper_system.hh | 2 +- python/src/ignition/gazebo/world.cc | 2 +- python/src/ignition/gazebo/world.hh | 2 +- 8 files changed, 24 insertions(+), 22 deletions(-) diff --git a/python/src/ignition/gazebo/destroyable.cc b/python/src/ignition/gazebo/destroyable.cc index d209fc1b65..77165a720b 100644 --- a/python/src/ignition/gazebo/destroyable.cc +++ b/python/src/ignition/gazebo/destroyable.cc @@ -35,7 +35,7 @@ Destroyable::Destroyable(const Destroyable &) } void -Destroyable::enter() +Destroyable::Enter() { if (please_destroy_) { throw InvalidHandle("cannot use Destroyable because destruction was requested"); @@ -44,7 +44,7 @@ Destroyable::enter() } void -Destroyable::exit(pybind11::object, pybind11::object, pybind11::object) +Destroyable::Exit(pybind11::object, pybind11::object, pybind11::object) { if (0u == use_count) { throw std::runtime_error("Internal error: " @@ -53,12 +53,12 @@ Destroyable::exit(pybind11::object, pybind11::object, pybind11::object) --use_count; if (please_destroy_ && 0u == use_count) { - destroy(); + Destroy(); } } void -Destroyable::destroy() +Destroyable::Destroy() { // Normally would be pure virtual, but then pybind11 can't // create bindings for this class @@ -67,7 +67,7 @@ Destroyable::destroy() } void -Destroyable::destroy_when_not_in_use() +Destroyable::DestroyWhenNotInUse() { if (please_destroy_) { // already asked to destroy @@ -75,7 +75,7 @@ Destroyable::destroy_when_not_in_use() } please_destroy_ = true; if (0u == use_count) { - destroy(); + Destroy(); } } @@ -84,11 +84,11 @@ define_destroyable(pybind11::object module) { pybind11::class_>( module, "Destroyable") - .def("__enter__", &Destroyable::enter) - .def("__exit__", &Destroyable::exit) + .def("__enter__", &Destroyable::Enter) + .def("__exit__", &Destroyable::Exit) .def( "destroy_when_not_in_use", - &Destroyable::destroy_when_not_in_use, + &Destroyable::DestroyWhenNotInUse, "Forcefully destroy the rcl object as soon as it's not actively " "being used"); } diff --git a/python/src/ignition/gazebo/destroyable.hh b/python/src/ignition/gazebo/destroyable.hh index a174c9aafe..a066bd85a2 100644 --- a/python/src/ignition/gazebo/destroyable.hh +++ b/python/src/ignition/gazebo/destroyable.hh @@ -25,7 +25,9 @@ namespace gazebo { namespace python { -/// This class blocks destruction when in use +/// This class blocks destruction when in use. +/// This class is required to handle the lifecycle of the +/// object. class Destroyable { public: @@ -37,22 +39,22 @@ public: /// Context manager __enter__ - block destruction void - enter(); + Enter(); /// Context manager __exit__ - unblock destruction void - exit(pybind11::object pytype, + Exit(pybind11::object pytype, pybind11::object pyvalue, pybind11::object pytraceback); /// Signal that the object should be destroyed as soon as it's not in use void - destroy_when_not_in_use(); + DestroyWhenNotInUse(); /// Override this to destroy an object virtual void - destroy(); + Destroy(); virtual ~Destroyable() = default; diff --git a/python/src/ignition/gazebo/entity_component_manager.cc b/python/src/ignition/gazebo/entity_component_manager.cc index 29dc7580a9..b54331931d 100644 --- a/python/src/ignition/gazebo/entity_component_manager.cc +++ b/python/src/ignition/gazebo/entity_component_manager.cc @@ -43,7 +43,7 @@ EntityComponentManager::~EntityComponentManager() } ///////////////////////////////////////////////// -void EntityComponentManager::destroy() +void EntityComponentManager::Destroy() { } diff --git a/python/src/ignition/gazebo/entity_component_manager.hh b/python/src/ignition/gazebo/entity_component_manager.hh index 23f6b5a7e2..ad6f042e30 100644 --- a/python/src/ignition/gazebo/entity_component_manager.hh +++ b/python/src/ignition/gazebo/entity_component_manager.hh @@ -47,7 +47,7 @@ namespace python /// Force an early destruction of this object void - destroy() override; + Destroy() override; /// Get rcl_client_t pointer ignition::gazebo::EntityComponentManager * diff --git a/python/src/ignition/gazebo/helper_system.cc b/python/src/ignition/gazebo/helper_system.cc index caa8aa5783..faae6d33a0 100644 --- a/python/src/ignition/gazebo/helper_system.cc +++ b/python/src/ignition/gazebo/helper_system.cc @@ -28,9 +28,9 @@ namespace python ///////////////////////////////////////////////// void HelperSystem::Configure( const Entity &_entity, - const std::shared_ptr &_sdf, + const std::shared_ptr &/*_sdf*/, ignition::gazebo::EntityComponentManager &_ecm, - EventManager &_eventMgr) + EventManager &/*_eventMgr*/) { if (this->configureCallback_internal) { @@ -110,7 +110,7 @@ HelperFixture::~HelperFixture() } void -HelperFixture::destroy() +HelperFixture::Destroy() { dataPtr.reset(); } diff --git a/python/src/ignition/gazebo/helper_system.hh b/python/src/ignition/gazebo/helper_system.hh index cd5a15faa1..4be66f4c9f 100644 --- a/python/src/ignition/gazebo/helper_system.hh +++ b/python/src/ignition/gazebo/helper_system.hh @@ -58,7 +58,7 @@ namespace python /// Force an early destruction of this object void - destroy() override; + Destroy() override; /// \brief Wrapper around a system's pre-update callback /// \param[in] _cb Function to be called every pre-update diff --git a/python/src/ignition/gazebo/world.cc b/python/src/ignition/gazebo/world.cc index 2aed785f2b..6291a5dc0c 100644 --- a/python/src/ignition/gazebo/world.cc +++ b/python/src/ignition/gazebo/world.cc @@ -36,7 +36,7 @@ World::~World() { } -void World::destroy() +void World::Destroy() { _world.reset(); } diff --git a/python/src/ignition/gazebo/world.hh b/python/src/ignition/gazebo/world.hh index d7a3130da4..149dcdd2a6 100644 --- a/python/src/ignition/gazebo/world.hh +++ b/python/src/ignition/gazebo/world.hh @@ -55,7 +55,7 @@ class World : public ignition::gazebo::python::Destroyable, std::string _name); /// Force an early destruction of this object - void destroy() override; + void Destroy() override; private: std::shared_ptr _world; From dbe9c08e435079c6d4ca47d8980a2be07f47e326 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 30 Nov 2021 16:54:51 +0100 Subject: [PATCH 05/21] return optional Signed-off-by: ahcorde --- python/src/ignition/gazebo/world.cc | 9 +++------ python/src/ignition/gazebo/world.hh | 2 +- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/python/src/ignition/gazebo/world.cc b/python/src/ignition/gazebo/world.cc index 6291a5dc0c..b7a8dd6cbd 100644 --- a/python/src/ignition/gazebo/world.cc +++ b/python/src/ignition/gazebo/world.cc @@ -16,6 +16,7 @@ #include +#include #include @@ -41,16 +42,12 @@ void World::Destroy() _world.reset(); } -ignition::math::Vector3 World::Gravity( +std::optional> World::Gravity( const EntityComponentManager &_ecm) { std::optional gravity = _world->Gravity(*_ecm.rcl_ptr()); - if (gravity.has_value()) - { - return gravity.value(); - } - return ignition::math::Vector3d::NaN; + return gravity; } ignition::gazebo::Entity World::ModelByName( diff --git a/python/src/ignition/gazebo/world.hh b/python/src/ignition/gazebo/world.hh index 149dcdd2a6..885a6bf20a 100644 --- a/python/src/ignition/gazebo/world.hh +++ b/python/src/ignition/gazebo/world.hh @@ -42,7 +42,7 @@ class World : public ignition::gazebo::python::Destroyable, /// \param[in] _ecm Entity-component manager. /// \return Gravity vector or nullopt if the entity does not /// have a components::Gravity component. - public: ignition::math::Vector3 + public: std::optional> Gravity(const EntityComponentManager &_ecm); /// \brief Get the ID of a model entity which is an immediate child of From b2b9f8c22141ddb4aa90ce5caea5aa96034d0c6b Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 10 Dec 2021 10:44:04 +0100 Subject: [PATCH 06/21] Follow code conventions Signed-off-by: ahcorde --- python/CMakeLists.txt | 18 +++++----- .../common/{console.cc => Console.cc} | 2 +- .../common/{console.hh => Console.hh} | 0 .../common/_ignition_common_pybind11.cc | 2 +- .../gazebo/{destroyable.cc => Destroyable.cc} | 6 ++-- .../gazebo/{destroyable.hh => Destroyable.hh} | 2 +- ...t_manager.cc => EntityComponentManager.cc} | 4 +-- ...t_manager.hh => EntityComponentManager.hh} | 5 ++- .../{event_manager.cc => EventManager.cc} | 4 +-- .../{event_manager.hh => EventManager.hh} | 3 +- .../gazebo/{exceptions.hh => Exceptions.hh} | 0 .../{helper_system.cc => HelperSystem.cc} | 4 +-- .../{helper_system.hh => HelperSystem.hh} | 6 ++-- .../ignition/gazebo/{server.cc => Server.cc} | 5 ++- .../ignition/gazebo/{server.hh => Server.hh} | 3 +- .../{server_config.cc => ServerConfig.cc} | 4 +-- .../{server_config.hh => ServerConfig.hh} | 2 +- .../gazebo/{update_info.cc => UpdateInfo.cc} | 5 ++- .../gazebo/{update_info.hh => UpdateInfo.hh} | 3 +- .../ignition/gazebo/{world.cc => World.cc} | 5 ++- .../ignition/gazebo/{world.hh => World.hh} | 5 ++- .../gazebo/_ignition_gazebo_pybind11.cc | 34 +++++++++---------- 22 files changed, 57 insertions(+), 65 deletions(-) rename python/src/ignition/common/{console.cc => Console.cc} (97%) rename python/src/ignition/common/{console.hh => Console.hh} (100%) rename python/src/ignition/gazebo/{destroyable.cc => Destroyable.cc} (95%) rename python/src/ignition/gazebo/{destroyable.hh => Destroyable.hh} (97%) rename python/src/ignition/gazebo/{entity_component_manager.cc => EntityComponentManager.cc} (94%) rename python/src/ignition/gazebo/{entity_component_manager.hh => EntityComponentManager.hh} (95%) rename python/src/ignition/gazebo/{event_manager.cc => EventManager.cc} (91%) rename python/src/ignition/gazebo/{event_manager.hh => EventManager.hh} (95%) rename python/src/ignition/gazebo/{exceptions.hh => Exceptions.hh} (100%) rename python/src/ignition/gazebo/{helper_system.cc => HelperSystem.cc} (98%) rename python/src/ignition/gazebo/{helper_system.hh => HelperSystem.hh} (97%) rename python/src/ignition/gazebo/{server.cc => Server.cc} (95%) rename python/src/ignition/gazebo/{server.hh => Server.hh} (95%) rename python/src/ignition/gazebo/{server_config.cc => ServerConfig.cc} (92%) rename python/src/ignition/gazebo/{server_config.hh => ServerConfig.hh} (95%) rename python/src/ignition/gazebo/{update_info.cc => UpdateInfo.cc} (93%) rename python/src/ignition/gazebo/{update_info.hh => UpdateInfo.hh} (95%) rename python/src/ignition/gazebo/{world.cc => World.cc} (96%) rename python/src/ignition/gazebo/{world.hh => World.hh} (96%) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index bc7c2519af..b06b186056 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -67,14 +67,14 @@ if (${pybind11_FOUND}) pybind11_add_module(gazebo SHARED src/ignition/gazebo/_ignition_gazebo_pybind11.cc - src/ignition/gazebo/destroyable.cc - src/ignition/gazebo/entity_component_manager - src/ignition/gazebo/event_manager.cc - src/ignition/gazebo/helper_system.cc - src/ignition/gazebo/server_config.cc - src/ignition/gazebo/server.cc - src/ignition/gazebo/update_info.cc - src/ignition/gazebo/world.cc + src/ignition/gazebo/Destroyable.cc + src/ignition/gazebo/EntityComponentManager + src/ignition/gazebo/EventManager.cc + src/ignition/gazebo/HelperSystem.cc + src/ignition/gazebo/Server.cc + src/ignition/gazebo/ServerConfig.cc + src/ignition/gazebo/UpdateInfo.cc + src/ignition/gazebo/World.cc ) target_link_libraries(gazebo PRIVATE @@ -86,7 +86,7 @@ if (${pybind11_FOUND}) # 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 + src/ignition/common/Console.cc ) target_link_libraries(common PRIVATE diff --git a/python/src/ignition/common/console.cc b/python/src/ignition/common/Console.cc similarity index 97% rename from python/src/ignition/common/console.cc rename to python/src/ignition/common/Console.cc index a6f1361575..8d904a4487 100644 --- a/python/src/ignition/common/console.cc +++ b/python/src/ignition/common/Console.cc @@ -14,7 +14,7 @@ #include -#include "console.hh" +#include "Console.hh" namespace ignition { diff --git a/python/src/ignition/common/console.hh b/python/src/ignition/common/Console.hh similarity index 100% rename from python/src/ignition/common/console.hh rename to python/src/ignition/common/Console.hh diff --git a/python/src/ignition/common/_ignition_common_pybind11.cc b/python/src/ignition/common/_ignition_common_pybind11.cc index 9122350b25..12a67670fc 100644 --- a/python/src/ignition/common/_ignition_common_pybind11.cc +++ b/python/src/ignition/common/_ignition_common_pybind11.cc @@ -14,7 +14,7 @@ #include -#include "console.hh" +#include "Console.hh" PYBIND11_MODULE(common, m) { m.doc() = "Ignition Common Python Library."; diff --git a/python/src/ignition/gazebo/destroyable.cc b/python/src/ignition/gazebo/Destroyable.cc similarity index 95% rename from python/src/ignition/gazebo/destroyable.cc rename to python/src/ignition/gazebo/Destroyable.cc index 77165a720b..0ec1daf092 100644 --- a/python/src/ignition/gazebo/destroyable.cc +++ b/python/src/ignition/gazebo/Destroyable.cc @@ -17,8 +17,8 @@ #include -#include "destroyable.hh" -#include "exceptions.hh" +#include "Destroyable.hh" +#include "Exceptions.hh" namespace ignition { @@ -80,7 +80,7 @@ Destroyable::DestroyWhenNotInUse() } void -define_destroyable(pybind11::object module) +defineDestroyable(pybind11::object module) { pybind11::class_>( module, "Destroyable") diff --git a/python/src/ignition/gazebo/destroyable.hh b/python/src/ignition/gazebo/Destroyable.hh similarity index 97% rename from python/src/ignition/gazebo/destroyable.hh rename to python/src/ignition/gazebo/Destroyable.hh index a066bd85a2..8e19aa512c 100644 --- a/python/src/ignition/gazebo/destroyable.hh +++ b/python/src/ignition/gazebo/Destroyable.hh @@ -68,7 +68,7 @@ private: /** * \param[in] module a pybind11 module to add the definition to */ -void define_destroyable(pybind11::object module); +void defineDestroyable(pybind11::object module); } // namespace python } // namespace ignition } // namespace gazebo diff --git a/python/src/ignition/gazebo/entity_component_manager.cc b/python/src/ignition/gazebo/EntityComponentManager.cc similarity index 94% rename from python/src/ignition/gazebo/entity_component_manager.cc rename to python/src/ignition/gazebo/EntityComponentManager.cc index b54331931d..fa97bf83ae 100644 --- a/python/src/ignition/gazebo/entity_component_manager.cc +++ b/python/src/ignition/gazebo/EntityComponentManager.cc @@ -15,7 +15,7 @@ */ #include -#include "entity_component_manager.hh" +#include "EntityComponentManager.hh" namespace ignition { @@ -48,7 +48,7 @@ void EntityComponentManager::Destroy() } ///////////////////////////////////////////////// -void define_gazebo_entity_component_manager(pybind11::object module) +void defineGazeboEntityComponentManager(pybind11::object module) { pybind11::class_(module, "EventManager") .def(pybind11::init<>()); diff --git a/python/src/ignition/gazebo/event_manager.hh b/python/src/ignition/gazebo/EventManager.hh similarity index 95% rename from python/src/ignition/gazebo/event_manager.hh rename to python/src/ignition/gazebo/EventManager.hh index 6121ee10db..94a3907a38 100644 --- a/python/src/ignition/gazebo/event_manager.hh +++ b/python/src/ignition/gazebo/EventManager.hh @@ -31,8 +31,7 @@ namespace python * \param[in] module a pybind11 module to add the definition to */ void -define_gazebo_event_manager(pybind11::object module); - +defineGazeboEventManager(pybind11::object module); } // namespace python } // namespace gazebo } // namespace ignition diff --git a/python/src/ignition/gazebo/exceptions.hh b/python/src/ignition/gazebo/Exceptions.hh similarity index 100% rename from python/src/ignition/gazebo/exceptions.hh rename to python/src/ignition/gazebo/Exceptions.hh diff --git a/python/src/ignition/gazebo/helper_system.cc b/python/src/ignition/gazebo/HelperSystem.cc similarity index 98% rename from python/src/ignition/gazebo/helper_system.cc rename to python/src/ignition/gazebo/HelperSystem.cc index faae6d33a0..fec54c6211 100644 --- a/python/src/ignition/gazebo/helper_system.cc +++ b/python/src/ignition/gazebo/HelperSystem.cc @@ -17,7 +17,7 @@ #include #include -#include "helper_system.hh" +#include "HelperSystem.hh" namespace ignition { @@ -183,7 +183,7 @@ std::shared_ptr HelperFixture::Server() const } void -define_gazebo_helper_fixture(pybind11::object module) +defineGazeboHelperFixture(pybind11::object module) { pybind11::class_ #include -#include "server.hh" +#include "Server.hh" namespace ignition { @@ -28,7 +28,7 @@ namespace gazebo { namespace python { -void define_gazebo_server(pybind11::object module) +void defineGazeboServer(pybind11::object module) { pybind11::class_(module, "Server") .def(pybind11::init()) @@ -46,7 +46,6 @@ void define_gazebo_server(pybind11::object module) 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 similarity index 95% rename from python/src/ignition/gazebo/server.hh rename to python/src/ignition/gazebo/Server.hh index 2b7dbdda1a..2393e1cd2e 100644 --- a/python/src/ignition/gazebo/server.hh +++ b/python/src/ignition/gazebo/Server.hh @@ -31,8 +31,7 @@ namespace python * \param[in] module a pybind11 module to add the definition to */ void -define_gazebo_server(pybind11::object module); - +defineGazeboServer(pybind11::object module); } // namespace python } // namespace gazebo } // namespace ignition diff --git a/python/src/ignition/gazebo/server_config.cc b/python/src/ignition/gazebo/ServerConfig.cc similarity index 92% rename from python/src/ignition/gazebo/server_config.cc rename to python/src/ignition/gazebo/ServerConfig.cc index 479b84aa79..d8b650aff1 100644 --- a/python/src/ignition/gazebo/server_config.cc +++ b/python/src/ignition/gazebo/ServerConfig.cc @@ -19,7 +19,7 @@ #include -#include "server_config.hh" +#include "ServerConfig.hh" namespace ignition { @@ -27,7 +27,7 @@ namespace gazebo { namespace python { -void define_server_config(pybind11::object module) +void defineGazeboServerConfig(pybind11::object module) { pybind11::class_(module, "ServerConfig") .def(pybind11::init<>()) diff --git a/python/src/ignition/gazebo/server_config.hh b/python/src/ignition/gazebo/ServerConfig.hh similarity index 95% rename from python/src/ignition/gazebo/server_config.hh rename to python/src/ignition/gazebo/ServerConfig.hh index c591b5792d..82fbcb5c99 100644 --- a/python/src/ignition/gazebo/server_config.hh +++ b/python/src/ignition/gazebo/ServerConfig.hh @@ -31,7 +31,7 @@ namespace python * \param[in] module a pybind11 module to add the definition to */ void -define_server_config(pybind11::object module); +defineGazeboServerConfig(pybind11::object module); } // namespace python } // namespace gazebo } // namespace ignition diff --git a/python/src/ignition/gazebo/update_info.cc b/python/src/ignition/gazebo/UpdateInfo.cc similarity index 93% rename from python/src/ignition/gazebo/update_info.cc rename to python/src/ignition/gazebo/UpdateInfo.cc index 1fe07325e3..e31dc7cb92 100644 --- a/python/src/ignition/gazebo/update_info.cc +++ b/python/src/ignition/gazebo/UpdateInfo.cc @@ -20,7 +20,7 @@ #include -#include "update_info.hh" +#include "UpdateInfo.hh" namespace ignition { @@ -28,7 +28,7 @@ namespace gazebo { namespace python { -void define_gazebo_update_info(pybind11::object module) +void defineGazeboUpdateInfo(pybind11::object module) { pybind11::class_(module, "UpdateInfo") .def(pybind11::init<>()) @@ -38,7 +38,6 @@ void define_gazebo_update_info(pybind11::object module) .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/update_info.hh b/python/src/ignition/gazebo/UpdateInfo.hh similarity index 95% rename from python/src/ignition/gazebo/update_info.hh rename to python/src/ignition/gazebo/UpdateInfo.hh index 916db1d54f..83cd23ee89 100644 --- a/python/src/ignition/gazebo/update_info.hh +++ b/python/src/ignition/gazebo/UpdateInfo.hh @@ -31,8 +31,7 @@ namespace python * \param[in] module a pybind11 module to add the definition to */ void -define_gazebo_update_info(pybind11::object module); - +defineGazeboUpdateInfo(pybind11::object module); } // namespace python } // namespace gazebo } // namespace ignition diff --git a/python/src/ignition/gazebo/world.cc b/python/src/ignition/gazebo/World.cc similarity index 96% rename from python/src/ignition/gazebo/world.cc rename to python/src/ignition/gazebo/World.cc index b7a8dd6cbd..c8d1c47c30 100644 --- a/python/src/ignition/gazebo/world.cc +++ b/python/src/ignition/gazebo/World.cc @@ -20,7 +20,7 @@ #include -#include "world.hh" +#include "World.hh" namespace ignition { @@ -57,7 +57,7 @@ ignition::gazebo::Entity World::ModelByName( return _world->ModelByName(*_ecm.rcl_ptr(), _name); } -void define_gazebo_world(pybind11::object module) +void defineGazeboWorld(pybind11::object module) { pybind11::class_ #include -#include "entity_component_manager.hh" +#include "EntityComponentManager.hh" #include @@ -66,8 +66,7 @@ private: * \param[in] module a pybind11 module to add the definition to */ void -define_gazebo_world(pybind11::object module); - +defineGazeboWorld(pybind11::object module); } // namespace python } // namespace gazebo } // namespace ignition diff --git a/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc b/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc index 0a7a0343f5..dc72178493 100644 --- a/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc +++ b/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc @@ -16,29 +16,29 @@ #include -#include "destroyable.hh" -#include "entity_component_manager.hh" -#include "event_manager.hh" -#include "exceptions.hh" -#include "helper_system.hh" -#include "server.hh" -#include "server_config.hh" -#include "update_info.hh" -#include "world.hh" +#include "Destroyable.hh" +#include "EntityComponentManager.hh" +#include "EventManager.hh" +#include "Exceptions.hh" +#include "HelperSystem.hh" +#include "Server.hh" +#include "ServerConfig.hh" +#include "UpdateInfo.hh" +#include "World.hh" PYBIND11_MODULE(gazebo, m) { m.doc() = "Ignition Gazebo Python Library."; - ignition::gazebo::python::define_destroyable(m); + ignition::gazebo::python::defineDestroyable(m); pybind11::register_exception( m, "InvalidHandle", PyExc_RuntimeError); - ignition::gazebo::python::define_gazebo_entity_component_manager(m); - ignition::gazebo::python::define_gazebo_event_manager(m); - ignition::gazebo::python::define_gazebo_helper_fixture(m); - ignition::gazebo::python::define_gazebo_server(m); - ignition::gazebo::python::define_gazebo_update_info(m); - ignition::gazebo::python::define_gazebo_world(m); - ignition::gazebo::python::define_server_config(m); + ignition::gazebo::python::defineGazeboEntityComponentManager(m); + ignition::gazebo::python::defineGazeboEventManager(m); + ignition::gazebo::python::defineGazeboHelperFixture(m); + ignition::gazebo::python::defineGazeboServer(m); + ignition::gazebo::python::defineGazeboServerConfig(m); + ignition::gazebo::python::defineGazeboUpdateInfo(m); + ignition::gazebo::python::defineGazeboWorld(m); } From 4682207f1eb6c71abeda071501c727d39451a6c8 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 10 Dec 2021 11:10:44 +0100 Subject: [PATCH 07/21] Fix Signed-off-by: ahcorde --- python/src/ignition/common/Console.hh | 4 ++-- python/src/ignition/gazebo/Destroyable.hh | 6 +++--- python/src/ignition/gazebo/EventManager.hh | 6 +++--- python/src/ignition/gazebo/Exceptions.hh | 6 +++--- python/src/ignition/gazebo/HelperSystem.hh | 6 +++--- python/src/ignition/gazebo/Server.hh | 6 +++--- python/src/ignition/gazebo/ServerConfig.hh | 6 +++--- python/src/ignition/gazebo/UpdateInfo.hh | 6 +++--- python/src/ignition/gazebo/World.hh | 6 +++--- 9 files changed, 26 insertions(+), 26 deletions(-) diff --git a/python/src/ignition/common/Console.hh b/python/src/ignition/common/Console.hh index ce05dafb80..4b84fbf514 100644 --- a/python/src/ignition/common/Console.hh +++ b/python/src/ignition/common/Console.hh @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IGNITION_GAZEBO_PYTHON__CONSOLE_HPP_ -#define IGNITION_GAZEBO_PYTHON__CONSOLE_HPP_ +#ifndef IGNITION_GAZEBO_PYTHON__CONSOLE_HH_ +#define IGNITION_GAZEBO_PYTHON__CONSOLE_HH_ #include diff --git a/python/src/ignition/gazebo/Destroyable.hh b/python/src/ignition/gazebo/Destroyable.hh index 8e19aa512c..6c8ef55d5a 100644 --- a/python/src/ignition/gazebo/Destroyable.hh +++ b/python/src/ignition/gazebo/Destroyable.hh @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_PYTHON__DESTROYABLE_HPP_ -#define IGNITION_GAZEBO_PYTHON__DESTROYABLE_HPP_ +#ifndef IGNITION_GAZEBO_PYTHON__DESTROYABLE_HH_ +#define IGNITION_GAZEBO_PYTHON__DESTROYABLE_HH_ #include @@ -72,4 +72,4 @@ void defineDestroyable(pybind11::object module); } // namespace python } // namespace ignition } // namespace gazebo -#endif // IGNITION_GAZEBO_PYTHON__DESTROYABLE_HPP_ +#endif // IGNITION_GAZEBO_PYTHON__DESTROYABLE_HH_ diff --git a/python/src/ignition/gazebo/EventManager.hh b/python/src/ignition/gazebo/EventManager.hh index 94a3907a38..bcd177edcb 100644 --- a/python/src/ignition/gazebo/EventManager.hh +++ b/python/src/ignition/gazebo/EventManager.hh @@ -15,8 +15,8 @@ */ -#ifndef IGNITION_GAZEBO_PYTHON__EVENT_MANAGER_HPP_ -#define IGNITION_GAZEBO_PYTHON__EVENT_MANAGER_HPP_ +#ifndef IGNITION_GAZEBO_PYTHON__EVENT_MANAGER_HH_ +#define IGNITION_GAZEBO_PYTHON__EVENT_MANAGER_HH_ #include @@ -36,4 +36,4 @@ defineGazeboEventManager(pybind11::object module); } // namespace gazebo } // namespace ignition -#endif // IGNITION_GAZEBO_PYTHON__EVENT_MANAGER_HPP_ +#endif // IGNITION_GAZEBO_PYTHON__EVENT_MANAGER_HH_ diff --git a/python/src/ignition/gazebo/Exceptions.hh b/python/src/ignition/gazebo/Exceptions.hh index fd45e3c0a2..0da1426a16 100644 --- a/python/src/ignition/gazebo/Exceptions.hh +++ b/python/src/ignition/gazebo/Exceptions.hh @@ -15,8 +15,8 @@ */ -#ifndef IGNITION_GAZEBO_PYTHON__EXCEPTIONS_HPP_ -#define IGNITION_GAZEBO_PYTHON__EXCEPTIONS_HPP_ +#ifndef IGNITION_GAZEBO_PYTHON__EXCEPTIONS_HH_ +#define IGNITION_GAZEBO_PYTHON__EXCEPTIONS_HH_ #include #include @@ -35,4 +35,4 @@ class InvalidHandle : public std::runtime_error } // namespace gazebo } // namespace ignition -#endif // IGNITION_GAZEBO_PYTHON__EXCEPTIONS_HPP_ +#endif // IGNITION_GAZEBO_PYTHON__EXCEPTIONS_HH_ diff --git a/python/src/ignition/gazebo/HelperSystem.hh b/python/src/ignition/gazebo/HelperSystem.hh index 80ae3cdf0c..ba225a77b9 100644 --- a/python/src/ignition/gazebo/HelperSystem.hh +++ b/python/src/ignition/gazebo/HelperSystem.hh @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef IGNITION_GAZEBO_PYTHON__HELPER_SYSTEM_HPP_ -#define IGNITION_GAZEBO_PYTHON__HELPER_SYSTEM_HPP_ +#ifndef IGNITION_GAZEBO_PYTHON__HELPER_SYSTEM_HH_ +#define IGNITION_GAZEBO_PYTHON__HELPER_SYSTEM_HH_ #include #include @@ -162,4 +162,4 @@ defineGazeboHelperFixture(pybind11::object module); } } -#endif +#endif // IGNITION_GAZEBO_PYTHON__HELPER_SYSTEM_HH_ diff --git a/python/src/ignition/gazebo/Server.hh b/python/src/ignition/gazebo/Server.hh index 2393e1cd2e..6e969a035b 100644 --- a/python/src/ignition/gazebo/Server.hh +++ b/python/src/ignition/gazebo/Server.hh @@ -15,8 +15,8 @@ */ -#ifndef IGNITION_GAZEBO_PYTHON__SERVER_HPP_ -#define IGNITION_GAZEBO_PYTHON__SERVER_HPP_ +#ifndef IGNITION_GAZEBO_PYTHON__SERVER_HH_ +#define IGNITION_GAZEBO_PYTHON__SERVER_HH_ #include @@ -36,4 +36,4 @@ defineGazeboServer(pybind11::object module); } // namespace gazebo } // namespace ignition -#endif // IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HPP_ +#endif // IGNITION_GAZEBO_PYTHON__SERVER_HH_ diff --git a/python/src/ignition/gazebo/ServerConfig.hh b/python/src/ignition/gazebo/ServerConfig.hh index 82fbcb5c99..d955bd5f49 100644 --- a/python/src/ignition/gazebo/ServerConfig.hh +++ b/python/src/ignition/gazebo/ServerConfig.hh @@ -15,8 +15,8 @@ */ -#ifndef IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HPP_ -#define IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HPP_ +#ifndef IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HH_ +#define IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HH_ #include @@ -36,4 +36,4 @@ defineGazeboServerConfig(pybind11::object module); } // namespace gazebo } // namespace ignition -#endif // IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HPP_ +#endif // IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HH_ diff --git a/python/src/ignition/gazebo/UpdateInfo.hh b/python/src/ignition/gazebo/UpdateInfo.hh index 83cd23ee89..f2ec4e910d 100644 --- a/python/src/ignition/gazebo/UpdateInfo.hh +++ b/python/src/ignition/gazebo/UpdateInfo.hh @@ -15,8 +15,8 @@ */ -#ifndef IGNITION_GAZEBO_PYTHON__UPDATE_INFO_HPP_ -#define IGNITION_GAZEBO_PYTHON__UPDATE_INFO_HPP_ +#ifndef IGNITION_GAZEBO_PYTHON__UPDATE_INFO_HH_ +#define IGNITION_GAZEBO_PYTHON__UPDATE_INFO_HH_ #include @@ -36,4 +36,4 @@ defineGazeboUpdateInfo(pybind11::object module); } // namespace gazebo } // namespace ignition -#endif // IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HPP_ +#endif // IGNITION_GAZEBO_PYTHON__UPDATE_INFO_HH_ diff --git a/python/src/ignition/gazebo/World.hh b/python/src/ignition/gazebo/World.hh index a7ee2a3e0d..dd9cf08487 100644 --- a/python/src/ignition/gazebo/World.hh +++ b/python/src/ignition/gazebo/World.hh @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_PYTHON__WORLD_HPP_ -#define IGNITION_GAZEBO_PYTHON__WORLD_HPP_ +#ifndef IGNITION_GAZEBO_PYTHON__WORLD_HH_ +#define IGNITION_GAZEBO_PYTHON__WORLD_HH_ #include @@ -71,4 +71,4 @@ defineGazeboWorld(pybind11::object module); } // namespace gazebo } // namespace ignition -#endif // IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HPP_ +#endif // IGNITION_GAZEBO_PYTHON__WORLD_HH_ From 0b89181c02ef55f896752dcd0ece0bbf1a160493 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Fri, 31 Dec 2021 00:06:57 +0100 Subject: [PATCH 08/21] Feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- examples/python/helperFixture.py | 10 ++--- .../ignition/gazebo/EntityComponentManager.cc | 30 +------------- .../ignition/gazebo/EntityComponentManager.hh | 34 +--------------- python/src/ignition/gazebo/EventManager.hh | 1 + python/src/ignition/gazebo/HelperSystem.cc | 39 +++++++------------ python/src/ignition/gazebo/HelperSystem.hh | 27 ++++++------- python/src/ignition/gazebo/Server.hh | 1 + python/src/ignition/gazebo/ServerConfig.hh | 1 + python/src/ignition/gazebo/UpdateInfo.hh | 1 + python/src/ignition/gazebo/World.cc | 37 ++---------------- python/src/ignition/gazebo/World.hh | 32 +-------------- 11 files changed, 46 insertions(+), 167 deletions(-) diff --git a/examples/python/helperFixture.py b/examples/python/helperFixture.py index 0ba7f11d34..1b2cb5aebd 100644 --- a/examples/python/helperFixture.py +++ b/examples/python/helperFixture.py @@ -57,12 +57,12 @@ def on_udpate_cb(_info, _ecm): iterations += 1 -helper = helper.on_post_update(on_post_udpate_cb) -helper = helper.on_update(on_udpate_cb) -helper = helper.on_pre_update(on_pre_udpate_cb) -helper = helper.on_configure(on_configure_cb) +helper.on_post_update(on_post_udpate_cb) +helper.on_update(on_udpate_cb) +helper.on_pre_update(on_pre_udpate_cb) +helper.on_configure(on_configure_cb) -helper = helper.finalize() +helper.finalize() server = helper.server() server.run(False, 1000, False) diff --git a/python/src/ignition/gazebo/EntityComponentManager.cc b/python/src/ignition/gazebo/EntityComponentManager.cc index fa97bf83ae..1786ba387c 100644 --- a/python/src/ignition/gazebo/EntityComponentManager.cc +++ b/python/src/ignition/gazebo/EntityComponentManager.cc @@ -23,38 +23,12 @@ namespace gazebo { namespace python { -///////////////////////////////////////////////// -EntityComponentManager::EntityComponentManager( - const ignition::gazebo::EntityComponentManager &_ecm) -{ - _entity_component_manager = &_ecm; -} - -///////////////////////////////////////////////// -EntityComponentManager::EntityComponentManager( - ignition::gazebo::EntityComponentManager &_ecm) -{ - _entity_component_manager_no_const = &_ecm; -} - -///////////////////////////////////////////////// -EntityComponentManager::~EntityComponentManager() -{ -} - -///////////////////////////////////////////////// -void EntityComponentManager::Destroy() -{ -} - ///////////////////////////////////////////////// void defineGazeboEntityComponentManager(pybind11::object module) { - pybind11::class_>( + pybind11::class_( module, "EntityComponentManager") - .def(pybind11::init()); + .def(pybind11::init<>()); } } // namespace python } // namespace gazebo diff --git a/python/src/ignition/gazebo/EntityComponentManager.hh b/python/src/ignition/gazebo/EntityComponentManager.hh index 1b102fe2dc..670a9389ae 100644 --- a/python/src/ignition/gazebo/EntityComponentManager.hh +++ b/python/src/ignition/gazebo/EntityComponentManager.hh @@ -29,42 +29,10 @@ namespace gazebo { namespace python { - - class EntityComponentManager : - public ignition::gazebo::python::Destroyable, - public std::enable_shared_from_this - { - /// \brief Constructor - public: EntityComponentManager( - const ignition::gazebo::EntityComponentManager &_ecm); - - /// \brief Constructor - public: EntityComponentManager( - ignition::gazebo::EntityComponentManager &_ecm); - - /// \brief Destructor - public: ~EntityComponentManager(); - - /// Force an early destruction of this object - void - Destroy() override; - - /// Get rcl_client_t pointer - ignition::gazebo::EntityComponentManager * - rcl_ptr() const - { - return _entity_component_manager_no_const; - } - - private: - const ignition::gazebo::EntityComponentManager * _entity_component_manager; - ignition::gazebo::EntityComponentManager * - _entity_component_manager_no_const; - }; - /// Define a pybind11 wrapper for an ignition::gazebo::EntityComponentManager /** * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python */ void defineGazeboEntityComponentManager(pybind11::object module); diff --git a/python/src/ignition/gazebo/EventManager.hh b/python/src/ignition/gazebo/EventManager.hh index bcd177edcb..15f2f8c37b 100644 --- a/python/src/ignition/gazebo/EventManager.hh +++ b/python/src/ignition/gazebo/EventManager.hh @@ -29,6 +29,7 @@ namespace python /// Define a pybind11 wrapper for an ignition::gazebo::EventManager /** * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python */ void defineGazeboEventManager(pybind11::object module); diff --git a/python/src/ignition/gazebo/HelperSystem.cc b/python/src/ignition/gazebo/HelperSystem.cc index fec54c6211..334d3983a6 100644 --- a/python/src/ignition/gazebo/HelperSystem.cc +++ b/python/src/ignition/gazebo/HelperSystem.cc @@ -34,8 +34,7 @@ void HelperSystem::Configure( { if (this->configureCallback_internal) { - auto ecm = ignition::gazebo::python::EntityComponentManager(_ecm); - this->configureCallback_internal(_entity, ecm); + this->configureCallback_internal(_entity, &_ecm); } } @@ -45,8 +44,7 @@ void HelperSystem::PreUpdate(const UpdateInfo &_info, { if (this->preUpdateCallback_internal) { - auto ecm = ignition::gazebo::python::EntityComponentManager(_ecm); - this->preUpdateCallback_internal(_info, ecm); + this->preUpdateCallback_internal(_info, &_ecm); } } @@ -56,8 +54,7 @@ void HelperSystem::Update(const UpdateInfo &_info, { if (this->updateCallback_internal) { - auto ecm = ignition::gazebo::python::EntityComponentManager(_ecm); - this->updateCallback_internal(_info, ecm); + this->updateCallback_internal(_info, &_ecm); } } @@ -66,8 +63,7 @@ void HelperSystem::PostUpdate(const UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm) { if (this->postUpdateCallback_internal) - this->postUpdateCallback_internal(_info, - ignition::gazebo::python::EntityComponentManager(_ecm)); + this->postUpdateCallback_internal(_info, &_ecm); } ////////////////////////////////////////////////// @@ -123,57 +119,52 @@ void HelperFixturePrivate::Init(const ServerConfig &_config) } ////////////////////////////////////////////////// -HelperFixture &HelperFixture::Finalize() +void HelperFixture::Finalize() { if (this->dataPtr->finalized) { ignwarn << "Fixture has already been finalized, this only needs to be done" << " once." << std::endl; - return *this; } - + std::cerr << "AddSystem1" << '\n'; this->dataPtr->server->AddSystem(this->dataPtr->helperSystem); + std::cerr << "AddSystem2" << '\n'; this->dataPtr->finalized = true; - return *this; } ////////////////////////////////////////////////// -HelperFixture &HelperFixture::OnConfigure(std::function _cb) + ignition::gazebo::EntityComponentManager *_ecm)> _cb) { if (nullptr != this->dataPtr->helperSystem) this->dataPtr->helperSystem->configureCallback_internal = std::move(_cb); - return *this; } ////////////////////////////////////////////////// -HelperFixture &HelperFixture::OnPreUpdate(std::function _cb) +void HelperFixture::OnPreUpdate(std::function _cb) { if (nullptr != this->dataPtr->helperSystem) this->dataPtr->helperSystem->preUpdateCallback_internal = std::move(_cb); - return *this; } ////////////////////////////////////////////////// -HelperFixture &HelperFixture::OnUpdate(std::function _cb) +void HelperFixture::OnUpdate(std::function _cb) { if (nullptr != this->dataPtr->helperSystem) this->dataPtr->helperSystem->updateCallback_internal = std::move(_cb); - return *this; } ////////////////////////////////////////////////// -HelperFixture &HelperFixture::OnPostUpdate(std::function _cb) + const ignition::gazebo::EntityComponentManager *)> _cb) { if (nullptr != this->dataPtr->helperSystem) this->dataPtr->helperSystem->postUpdateCallback_internal = std::move(_cb); - return *this; } ////////////////////////////////////////////////// diff --git a/python/src/ignition/gazebo/HelperSystem.hh b/python/src/ignition/gazebo/HelperSystem.hh index ba225a77b9..1f736f45f1 100644 --- a/python/src/ignition/gazebo/HelperSystem.hh +++ b/python/src/ignition/gazebo/HelperSystem.hh @@ -64,36 +64,36 @@ namespace python /// \param[in] _cb Function to be called every pre-update /// The _entity and _sdf will correspond to the world entity. /// \return Reference to self. - public: HelperFixture &OnConfigure(std::function _cb); + ignition::gazebo::EntityComponentManager *_ecm)> _cb); /// \brief Wrapper around a system's pre-update callback /// \param[in] _cb Function to be called every pre-update /// \return Reference to self. - public: HelperFixture &OnPreUpdate(std::function _cb); + ignition::gazebo::EntityComponentManager *)> _cb); /// \brief Wrapper around a system's update callback /// \param[in] _cb Function to be called every update /// \return Reference to self. - public: HelperFixture &OnUpdate(std::function _cb); + ignition::gazebo::EntityComponentManager *)> _cb); /// \brief Wrapper around a system's post-update callback /// \param[in] _cb Function to be called every post-update /// \return Reference to self. - public: HelperFixture &OnPostUpdate(std::function _cb); + const ignition::gazebo::EntityComponentManager *)> _cb); /// \brief Finalize all the functions and add fixture to server. /// Finalize must be called before running the server, otherwise none of /// the `On*` functions will be called. /// The `OnConfigure` callback is called immediately on finalize. - public: HelperFixture &Finalize(); + public: void Finalize(); /// \brief Get pointer to underlying server. public: std::shared_ptr Server() const; @@ -133,28 +133,29 @@ class HelperSystem : /// \brief Function to call every time we configure a world public: std::function + ignition::gazebo::EntityComponentManager *_ecm)> configureCallback_internal; /// \brief Function to call every pre-update public: std::function + ignition::gazebo::EntityComponentManager *)> preUpdateCallback_internal; /// \brief Function to call every update public: std::function + ignition::gazebo::EntityComponentManager *)> updateCallback_internal; /// \brief Function to call every post-update public: std::function + const ignition::gazebo::EntityComponentManager *)> postUpdateCallback_internal; }; /// Define a pybind11 wrapper for an ignition::gazebo::HelperFixture /** * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python */ void defineGazeboHelperFixture(pybind11::object module); diff --git a/python/src/ignition/gazebo/Server.hh b/python/src/ignition/gazebo/Server.hh index 6e969a035b..e03172beaa 100644 --- a/python/src/ignition/gazebo/Server.hh +++ b/python/src/ignition/gazebo/Server.hh @@ -29,6 +29,7 @@ namespace python /// Define a pybind11 wrapper for an ignition::gazebo::Server /** * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python */ void defineGazeboServer(pybind11::object module); diff --git a/python/src/ignition/gazebo/ServerConfig.hh b/python/src/ignition/gazebo/ServerConfig.hh index d955bd5f49..c54087e985 100644 --- a/python/src/ignition/gazebo/ServerConfig.hh +++ b/python/src/ignition/gazebo/ServerConfig.hh @@ -29,6 +29,7 @@ namespace python /// Define a pybind11 wrapper for an ignition::gazebo::ServerConfig /** * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python */ void defineGazeboServerConfig(pybind11::object module); diff --git a/python/src/ignition/gazebo/UpdateInfo.hh b/python/src/ignition/gazebo/UpdateInfo.hh index f2ec4e910d..52f1496e6a 100644 --- a/python/src/ignition/gazebo/UpdateInfo.hh +++ b/python/src/ignition/gazebo/UpdateInfo.hh @@ -29,6 +29,7 @@ namespace python /// Define a pybind11 wrapper for an ignition::gazebo::UpdateInfo /** * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python */ void defineGazeboUpdateInfo(pybind11::object module); diff --git a/python/src/ignition/gazebo/World.cc b/python/src/ignition/gazebo/World.cc index c8d1c47c30..6822c12f8f 100644 --- a/python/src/ignition/gazebo/World.cc +++ b/python/src/ignition/gazebo/World.cc @@ -28,47 +28,16 @@ namespace gazebo { namespace python { -World::World(ignition::gazebo::Entity _entity) -{ - _world = std::make_shared(_entity); -} - -World::~World() -{ -} - -void World::Destroy() -{ - _world.reset(); -} - -std::optional> World::Gravity( - const EntityComponentManager &_ecm) -{ - std::optional gravity = - _world->Gravity(*_ecm.rcl_ptr()); - return gravity; -} - -ignition::gazebo::Entity World::ModelByName( - ignition::gazebo::python::EntityComponentManager &_ecm, - std::string _name) -{ - return _world->ModelByName(*_ecm.rcl_ptr(), _name); -} - void defineGazeboWorld(pybind11::object module) { - pybind11::class_>(module, "World") + pybind11::class_(module, "World") .def(pybind11::init()) .def( - "model_by_name", &ignition::gazebo::python::World::ModelByName, + "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::python::World::Gravity, + "gravity", &ignition::gazebo::World::Gravity, "Get the gravity in m/s^2."); } } // namespace python diff --git a/python/src/ignition/gazebo/World.hh b/python/src/ignition/gazebo/World.hh index dd9cf08487..0c65f087d5 100644 --- a/python/src/ignition/gazebo/World.hh +++ b/python/src/ignition/gazebo/World.hh @@ -32,38 +32,10 @@ namespace gazebo { namespace python { -class World : public ignition::gazebo::python::Destroyable, - public std::enable_shared_from_this -{ - public: World(ignition::gazebo::Entity _entity = kNullEntity); - public: ~World(); - - /// \brief Get the gravity in m/s^2. - /// \param[in] _ecm Entity-component manager. - /// \return Gravity vector or nullopt if the entity does not - /// have a components::Gravity component. - public: std::optional> - Gravity(const EntityComponentManager &_ecm); - - /// \brief Get the ID of a model entity which is an immediate child of - /// this world. - /// \param[in] _ecm Entity-component manager. - /// \param[in] _name Model name. - /// \return Model entity. - public: ignition::gazebo::Entity ModelByName( - ignition::gazebo::python::EntityComponentManager &_ecm, - std::string _name); - - /// Force an early destruction of this object - void Destroy() override; - -private: - std::shared_ptr _world; -}; - -/// Define a pybind11 wrapper for an ignition::gazebo::Server +/// Define a pybind11 wrapper for an ignition::gazebo::World /** * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python */ void defineGazeboWorld(pybind11::object module); From 517f1106844492f3bbaba13d5605a39f856c981a Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 5 Jan 2022 18:10:04 +0100 Subject: [PATCH 09/21] Added tutorial Signed-off-by: ahcorde --- tutorials.md.in | 1 + tutorials/python_interfaces.md | 79 ++++++++++++++++++++++++++++++++++ 2 files changed, 80 insertions(+) create mode 100644 tutorials/python_interfaces.md diff --git a/tutorials.md.in b/tutorials.md.in index 395f1a9c20..a41255775a 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -37,6 +37,7 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage model_command "Model Command": Use the CLI to get information about the models in a simulation. * \subpage test_fixture "Test Fixture": Writing automated CI tests * \subpage spherical_coordinates "Spherical coordinates": Working with latitude and longitude +* \subpage python_interfaces Python interfaces **Migration from Gazebo classic** diff --git a/tutorials/python_interfaces.md b/tutorials/python_interfaces.md new file mode 100644 index 0000000000..314e65d9d7 --- /dev/null +++ b/tutorials/python_interfaces.md @@ -0,0 +1,79 @@ +\page python_interfaces Python interfaces + +# Overview + +Ignition Gazebo provides a Python API to interact with world. + +For now, we provide a `HelperFixture` class that allows to load a world file, +step simulation and check entities and components. + + - **Step 1**: Load a world with a fixture + + ```python + file_path = os.path.dirname(os.path.realpath(__file__)) + helper = HelperFixture(os.path.join(file_path, 'gravity.sdf')) + ``` + + - **Step 2**: Write your `configure`, `pretupdate`, `update` or `postupdate` code: + ```python + def on_post_udpate_cb(_info, _ecm): + # + ... + ``` + + - **Step 3**: Register the function. + ```python + helper.on_post_update(on_post_udpate_cb) + ``` + + + - **Step 4**: Be sure to call finalize before running the server. + ```python + helper.finalize() + ``` + + - **Step 5**: Run the server + ```python + server.run(False, 1000, False) + while(server.is_running()): + time.sleep(0.1) + ``` + +# Run the example + +In the `examples/python/helperFixture.py` folder there is a Python script that +shows make use of this API. If you compiled Ignition Gazebo from source you should modify your `PYTHONPATH`: + +```bash +export PYTHONPATH=$PYTHONPATH:/install/lib/python +``` + +Now you can run the example: + +```bash +$ python3 examples/python/helperFixture.py +[Msg] Loading SDF world file[/home/ahcorde/ignition_fortress/src/ign-gazebo/examples/python/gravity.sdf]. +[Dbg] [Physics.cc:789] Loaded [ignition::physics::dartsim::Plugin] from library [/home/ahcorde/ignition_fortress/install/lib/ign-physics-5/engine-plugins/libignition-physics-dartsim-plugin.so] +[Dbg] [SimulationRunner.cc:909] Loaded system [ignition::gazebo::systems::Physics] for entity [1] +[Msg] Loaded level [3] +[Msg] Serving world controls on [/world/gravity/control], [/world/gravity/control/state] and [/world/gravity/playback/control] +[Msg] Serving GUI information on [/world/gravity/gui/info] +[Msg] World [gravity] initialized with [default_physics] physics profile. +[Msg] Serving world SDF generation service on [/world/gravity/generate_world_sdf] +[Msg] Serving world names on [/gazebo/worlds] +[Msg] Resource path add service on [/gazebo/resource_paths/add]. +[Msg] Resource path get service on [/gazebo/resource_paths/get]. +[Msg] Resource paths published on [/gazebo/resource_paths]. +AddSystem1 +World entity is 1 +Gravity 0 0 -9.8 +Entity for falling model is: 4 +AddSystem2 +[Msg] Found no publishers on /stats, adding root stats topic +[Msg] Found no publishers on /clock, adding root clock topic +[Dbg] [SimulationRunner.cc:524] Creating PostUpdate worker threads: 2 +[Dbg] [SimulationRunner.cc:537] Creating postupdate worker thread (0) +iterations 1000 +post_iterations 1000 +pre_iterations 1000 +``` From 81a732af3b1e94fb40a5fdd4e684c8082bf752e1 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 11 Jan 2022 13:48:06 +0100 Subject: [PATCH 10/21] CMake changes Signed-off-by: ahcorde --- .github/ci/packages.apt | 2 + CMakeLists.txt | 25 ++++++- python/CMakeLists.txt | 116 ++++++++++++++------------------- tutorials/python_interfaces.md | 2 +- 4 files changed, 76 insertions(+), 69 deletions(-) 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 9ff24f8d83..25946f2094 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -167,6 +167,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,8 +207,9 @@ add_subdirectory(examples) #============================================================================ ign_create_packages() -add_subdirectory(python) - +if (${pybind11_FOUND}) + add_subdirectory(python) +endif() #============================================================================ # Configure documentation #============================================================================ diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index b06b186056..86ac12631f 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -22,9 +22,6 @@ if(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION) if(USE_DIST_PACKAGES_FOR_PYTHON) string(REPLACE "site-packages" "dist-packages" IGN_PYTHON_INSTALL_PATH ${Python3_SITEARCH}) - else() - # custom cmake command is returning dist-packages - string(REPLACE "dist-packages" "site-packages" IGN_PYTHON_INSTALL_PATH ${Python3_SITEARCH}) endif() else() # If not a system installation, respect local paths @@ -33,67 +30,54 @@ endif() set(IGN_PYTHON_INSTALL_PATH "${IGN_PYTHON_INSTALL_PATH}/ignition") -set(PYBIND11_PYTHON_VERSION 3) - -find_package(pybind11 2.2 QUIET) - -if (NOT ${pybind11_FOUND}) - if(${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.11.0") - include(FetchContent) - FetchContent_Declare( - pybind11 - GIT_REPOSITORY https://github.com/pybind/pybind11 - GIT_TAG v2.8.1 - ) - - FetchContent_GetProperties(pybind11) - if(NOT pybind11_POPULATED) - FetchContent_Populate(pybind11) - add_subdirectory(${pybind11_SOURCE_DIR} ${pybind11_BINARY_DIR}) - endif() - set(pybind11_FOUND TRUE) - endif() -endif() - -if (${pybind11_FOUND}) - - # 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}/" - ) - endfunction() - - pybind11_add_module(gazebo SHARED - src/ignition/gazebo/_ignition_gazebo_pybind11.cc - src/ignition/gazebo/Destroyable.cc - src/ignition/gazebo/EntityComponentManager - src/ignition/gazebo/EventManager.cc - src/ignition/gazebo/HelperSystem.cc - src/ignition/gazebo/Server.cc - src/ignition/gazebo/ServerConfig.cc - src/ignition/gazebo/UpdateInfo.cc - src/ignition/gazebo/World.cc +# 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}/" ) - - 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} - ) - - configure_build_install_location(gazebo) - configure_build_install_location(common) - -endif() +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/HelperSystem.cc + src/ignition/gazebo/Server.cc + src/ignition/gazebo/ServerConfig.cc + src/ignition/gazebo/UpdateInfo.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_LIB_INSTALL_DIR}/python/" +) + +configure_build_install_location(gazebo) +configure_build_install_location(common) diff --git a/tutorials/python_interfaces.md b/tutorials/python_interfaces.md index 314e65d9d7..2d15e0d76e 100644 --- a/tutorials/python_interfaces.md +++ b/tutorials/python_interfaces.md @@ -41,7 +41,7 @@ step simulation and check entities and components. # Run the example -In the `examples/python/helperFixture.py` folder there is a Python script that +In the [examples/python](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo6/examples/python) folder there is a Python script shows make use of this API. If you compiled Ignition Gazebo from source you should modify your `PYTHONPATH`: ```bash From 673ad9bd024f89847daf460fd298859ee941720c Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 13 Jan 2022 21:22:04 +0100 Subject: [PATCH 11/21] Added feedback Signed-off-by: ahcorde --- examples/python/helperFixture.py | 9 +- .../ignition/gazebo/EntityComponentManager.hh | 4 +- include/ignition/gazebo/EventManager.hh | 4 +- python/CMakeLists.txt | 4 +- python/src/ignition/gazebo/Destroyable.cc | 97 -------- python/src/ignition/gazebo/Destroyable.hh | 75 ------- .../ignition/gazebo/EntityComponentManager.hh | 3 - python/src/ignition/gazebo/EventManager.hh | 1 - python/src/ignition/gazebo/HelperSystem.cc | 210 ------------------ python/src/ignition/gazebo/HelperSystem.hh | 166 -------------- python/src/ignition/gazebo/Server.hh | 1 - python/src/ignition/gazebo/ServerConfig.hh | 1 - python/src/ignition/gazebo/TestFixture.cc | 69 ++++++ python/src/ignition/gazebo/TestFixture.hh | 38 ++++ python/src/ignition/gazebo/UpdateInfo.hh | 1 - python/src/ignition/gazebo/World.hh | 6 - .../gazebo/_ignition_gazebo_pybind11.cc | 7 +- python/src/ignition/sdformat/Element.cc | 36 +++ python/src/ignition/sdformat/Element.hh | 38 ++++ .../ignition/sdformat/_sdformat_pybind11.cc | 23 ++ 20 files changed, 218 insertions(+), 575 deletions(-) delete mode 100644 python/src/ignition/gazebo/Destroyable.cc delete mode 100644 python/src/ignition/gazebo/Destroyable.hh delete mode 100644 python/src/ignition/gazebo/HelperSystem.cc delete mode 100644 python/src/ignition/gazebo/HelperSystem.hh create mode 100644 python/src/ignition/gazebo/TestFixture.cc create mode 100644 python/src/ignition/gazebo/TestFixture.hh create mode 100644 python/src/ignition/sdformat/Element.cc create mode 100644 python/src/ignition/sdformat/Element.hh create mode 100644 python/src/ignition/sdformat/_sdformat_pybind11.cc diff --git a/examples/python/helperFixture.py b/examples/python/helperFixture.py index 1b2cb5aebd..ed0c37162a 100644 --- a/examples/python/helperFixture.py +++ b/examples/python/helperFixture.py @@ -17,22 +17,22 @@ import time from ignition.common import set_verbosity -from ignition.gazebo import HelperFixture, World +from ignition.gazebo import TestFixture, World from ignition.math import Vector3d - +from sdformat import Element set_verbosity(4) file_path = os.path.dirname(os.path.realpath(__file__)) -helper = HelperFixture(os.path.join(file_path, 'gravity.sdf')) +helper = TestFixture(os.path.join(file_path, 'gravity.sdf')) post_iterations = 0 iterations = 0 pre_iterations = 0 -def on_configure_cb(worldEntity, _ecm): +def on_configure_cb(worldEntity, _sdf, _ecm, _eventManager): print('World entity is ', worldEntity) w = World(worldEntity) v = w.gravity(_ecm) @@ -61,7 +61,6 @@ def on_udpate_cb(_info, _ecm): helper.on_update(on_udpate_cb) helper.on_pre_update(on_pre_udpate_cb) helper.on_configure(on_configure_cb) - helper.finalize() server = helper.server() diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index e699a65fe7..6715f757ba 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -768,8 +768,10 @@ namespace ignition Entity _entity, const std::unordered_set &_types = {}) const; + /// This was originally a unique_ptr, but pybind11 was failing because it + /// was not able to copy the class /// \brief Private data pointer. - private: std::unique_ptr dataPtr; + private: std::shared_ptr dataPtr; /// \brief Add an entity and its components to a serialized state message. /// \param[out] _msg The state message. diff --git a/include/ignition/gazebo/EventManager.hh b/include/ignition/gazebo/EventManager.hh index bd1331cfbc..9e22584acb 100644 --- a/include/ignition/gazebo/EventManager.hh +++ b/include/ignition/gazebo/EventManager.hh @@ -140,9 +140,11 @@ namespace ignition } }; + /// This was originally a unique_ptr, but pybind11 was failing because it + /// was not able to copy the class /// \brief Container of used signals. private: std::unordered_map, + std::shared_ptr, Hasher, EqualTo> events; }; } diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 86ac12631f..cef2a2e290 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -40,9 +40,9 @@ endfunction() pybind11_add_module(gazebo SHARED src/ignition/gazebo/_ignition_gazebo_pybind11.cc - src/ignition/gazebo/EntityComponentManager.cc + src/ignition/gazebo/EntityComponentManager.cc src/ignition/gazebo/EventManager.cc - src/ignition/gazebo/HelperSystem.cc + src/ignition/gazebo/TestFixture.cc src/ignition/gazebo/Server.cc src/ignition/gazebo/ServerConfig.cc src/ignition/gazebo/UpdateInfo.cc diff --git a/python/src/ignition/gazebo/Destroyable.cc b/python/src/ignition/gazebo/Destroyable.cc deleted file mode 100644 index 0ec1daf092..0000000000 --- a/python/src/ignition/gazebo/Destroyable.cc +++ /dev/null @@ -1,97 +0,0 @@ -/* - * 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 "Destroyable.hh" -#include "Exceptions.hh" - -namespace ignition -{ -namespace gazebo -{ -namespace python -{ -Destroyable::Destroyable(const Destroyable &) -{ - // When a destroyable is copied, it does not matter if someone asked - // to destroy the original. The copy has its own lifetime. - use_count = 0; - please_destroy_ = false; -} - -void -Destroyable::Enter() -{ - if (please_destroy_) { - throw InvalidHandle("cannot use Destroyable because destruction was requested"); - } - ++use_count; -} - -void -Destroyable::Exit(pybind11::object, pybind11::object, pybind11::object) -{ - if (0u == use_count) { - throw std::runtime_error("Internal error: " - "Destroyable use_count would be negative"); - } - - --use_count; - if (please_destroy_ && 0u == use_count) { - Destroy(); - } -} - -void -Destroyable::Destroy() -{ - // Normally would be pure virtual, but then pybind11 can't - // create bindings for this class - throw std::runtime_error("Internal error : " - "Destroyable subclass didn't override destroy()"); -} - -void -Destroyable::DestroyWhenNotInUse() -{ - if (please_destroy_) { - // already asked to destroy - return; - } - please_destroy_ = true; - if (0u == use_count) { - Destroy(); - } -} - -void -defineDestroyable(pybind11::object module) -{ - pybind11::class_>( - module, "Destroyable") - .def("__enter__", &Destroyable::Enter) - .def("__exit__", &Destroyable::Exit) - .def( - "destroy_when_not_in_use", - &Destroyable::DestroyWhenNotInUse, - "Forcefully destroy the rcl object as soon as it's not actively " - "being used"); -} -} // namespace python -} // namespace utils -} // namespace ignition diff --git a/python/src/ignition/gazebo/Destroyable.hh b/python/src/ignition/gazebo/Destroyable.hh deleted file mode 100644 index 6c8ef55d5a..0000000000 --- a/python/src/ignition/gazebo/Destroyable.hh +++ /dev/null @@ -1,75 +0,0 @@ -/* - * 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__DESTROYABLE_HH_ -#define IGNITION_GAZEBO_PYTHON__DESTROYABLE_HH_ - -#include - -namespace ignition -{ -namespace gazebo -{ -namespace python -{ -/// This class blocks destruction when in use. -/// This class is required to handle the lifecycle of the -/// object. -class Destroyable -{ -public: - /// Default constructor - Destroyable() = default; - - /// Copy constructor - Destroyable(const Destroyable & other); - - /// Context manager __enter__ - block destruction - void - Enter(); - - /// Context manager __exit__ - unblock destruction - void - Exit(pybind11::object pytype, - pybind11::object pyvalue, - pybind11::object pytraceback); - - /// Signal that the object should be destroyed as soon as it's not in use - void - DestroyWhenNotInUse(); - - /// Override this to destroy an object - virtual - void - Destroy(); - - virtual - ~Destroyable() = default; - -private: - size_t use_count = 0u; - bool please_destroy_ = false; -}; - -/// Define a pybind11 wrapper for an rclpybind11::Destroyable -/** - * \param[in] module a pybind11 module to add the definition to - */ -void defineDestroyable(pybind11::object module); -} // namespace python -} // namespace ignition -} // namespace gazebo -#endif // IGNITION_GAZEBO_PYTHON__DESTROYABLE_HH_ diff --git a/python/src/ignition/gazebo/EntityComponentManager.hh b/python/src/ignition/gazebo/EntityComponentManager.hh index 670a9389ae..b982483631 100644 --- a/python/src/ignition/gazebo/EntityComponentManager.hh +++ b/python/src/ignition/gazebo/EntityComponentManager.hh @@ -21,8 +21,6 @@ #include "ignition/gazebo/EntityComponentManager.hh" -#include "Destroyable.hh" - namespace ignition { namespace gazebo @@ -32,7 +30,6 @@ namespace python /// Define a pybind11 wrapper for an ignition::gazebo::EntityComponentManager /** * \param[in] module a pybind11 module to add the definition to - * \param[in] typestr name of the type used by Python */ void defineGazeboEntityComponentManager(pybind11::object module); diff --git a/python/src/ignition/gazebo/EventManager.hh b/python/src/ignition/gazebo/EventManager.hh index 15f2f8c37b..bcd177edcb 100644 --- a/python/src/ignition/gazebo/EventManager.hh +++ b/python/src/ignition/gazebo/EventManager.hh @@ -29,7 +29,6 @@ namespace python /// Define a pybind11 wrapper for an ignition::gazebo::EventManager /** * \param[in] module a pybind11 module to add the definition to - * \param[in] typestr name of the type used by Python */ void defineGazeboEventManager(pybind11::object module); diff --git a/python/src/ignition/gazebo/HelperSystem.cc b/python/src/ignition/gazebo/HelperSystem.cc deleted file mode 100644 index 334d3983a6..0000000000 --- a/python/src/ignition/gazebo/HelperSystem.cc +++ /dev/null @@ -1,210 +0,0 @@ -/* - * 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 "HelperSystem.hh" - -namespace ignition -{ -namespace gazebo -{ -namespace python -{ -///////////////////////////////////////////////// -void HelperSystem::Configure( - const Entity &_entity, - const std::shared_ptr &/*_sdf*/, - ignition::gazebo::EntityComponentManager &_ecm, - EventManager &/*_eventMgr*/) -{ - if (this->configureCallback_internal) - { - this->configureCallback_internal(_entity, &_ecm); - } -} - -///////////////////////////////////////////////// -void HelperSystem::PreUpdate(const UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) -{ - if (this->preUpdateCallback_internal) - { - this->preUpdateCallback_internal(_info, &_ecm); - } -} - -///////////////////////////////////////////////// -void HelperSystem::Update(const UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) -{ - if (this->updateCallback_internal) - { - this->updateCallback_internal(_info, &_ecm); - } -} - -///////////////////////////////////////////////// -void HelperSystem::PostUpdate(const UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) -{ - if (this->postUpdateCallback_internal) - this->postUpdateCallback_internal(_info, &_ecm); -} - -////////////////////////////////////////////////// -class HelperFixturePrivate -{ - /// \brief Initialize fixture - /// \param[in] _config Server config - public: void Init(const ServerConfig &_config); - - /// \brief Pointer to underlying server - public: std::shared_ptr server{nullptr}; - - /// \brief Pointer to underlying Helper interface - public: std::shared_ptr helperSystem{nullptr}; - - /// \brief Flag to make sure Finalize is only called once - public: bool finalized{false}; -}; - -////////////////////////////////////////////////// -HelperFixture::HelperFixture(const std::string &_path) - : dataPtr(std::make_shared()) -{ - ServerConfig config; - config.SetSdfFile(_path); - this->dataPtr->Init(config); -} - -////////////////////////////////////////////////// -HelperFixture::HelperFixture(const ServerConfig &_config) - : dataPtr(new HelperFixturePrivate()) -{ - this->dataPtr->Init(_config); -} - -////////////////////////////////////////////////// -HelperFixture::~HelperFixture() -{ - dataPtr = nullptr; -} - -void -HelperFixture::Destroy() -{ - dataPtr.reset(); -} - -////////////////////////////////////////////////// -void HelperFixturePrivate::Init(const ServerConfig &_config) -{ - this->helperSystem = std::make_shared(); - this->server = std::make_shared(_config); -} - -////////////////////////////////////////////////// -void HelperFixture::Finalize() -{ - if (this->dataPtr->finalized) - { - ignwarn << "Fixture has already been finalized, this only needs to be done" - << " once." << std::endl; - } - std::cerr << "AddSystem1" << '\n'; - this->dataPtr->server->AddSystem(this->dataPtr->helperSystem); - std::cerr << "AddSystem2" << '\n'; - - this->dataPtr->finalized = true; -} - -////////////////////////////////////////////////// -void HelperFixture::OnConfigure(std::function _cb) -{ - if (nullptr != this->dataPtr->helperSystem) - this->dataPtr->helperSystem->configureCallback_internal = std::move(_cb); -} - -////////////////////////////////////////////////// -void HelperFixture::OnPreUpdate(std::function _cb) -{ - if (nullptr != this->dataPtr->helperSystem) - this->dataPtr->helperSystem->preUpdateCallback_internal = std::move(_cb); -} - -////////////////////////////////////////////////// -void HelperFixture::OnUpdate(std::function _cb) -{ - if (nullptr != this->dataPtr->helperSystem) - this->dataPtr->helperSystem->updateCallback_internal = std::move(_cb); -} - -////////////////////////////////////////////////// -void HelperFixture::OnPostUpdate(std::function _cb) -{ - if (nullptr != this->dataPtr->helperSystem) - this->dataPtr->helperSystem->postUpdateCallback_internal = std::move(_cb); -} - -////////////////////////////////////////////////// -std::shared_ptr HelperFixture::Server() const -{ - return this->dataPtr->server; -} - -void -defineGazeboHelperFixture(pybind11::object module) -{ - pybind11::class_>(module, "HelperFixture") - .def(pybind11::init()) - .def( - "server", &HelperFixture::Server, - "Get pointer to underlying server." - ) - .def( - "finalize", &HelperFixture::Finalize, - "Finalize all the functions and add fixture to server." - ) - .def( - "on_pre_update", &HelperFixture::OnPreUpdate, - "Wrapper around a system's pre-update callback" - ) - .def( - "on_update", &HelperFixture::OnUpdate, - "Wrapper around a system's update callback" - ) - .def( - "on_post_update", &HelperFixture::OnPostUpdate, - "Wrapper around a system's post-update callback" - ) - .def( - "on_configure", &HelperFixture::OnConfigure, - "Wrapper around a system's pre-update callback" - ); -} -} -} -} diff --git a/python/src/ignition/gazebo/HelperSystem.hh b/python/src/ignition/gazebo/HelperSystem.hh deleted file mode 100644 index 1f736f45f1..0000000000 --- a/python/src/ignition/gazebo/HelperSystem.hh +++ /dev/null @@ -1,166 +0,0 @@ -/* - * 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__HELPER_SYSTEM_HH_ -#define IGNITION_GAZEBO_PYTHON__HELPER_SYSTEM_HH_ - -#include -#include -#include - -#include -#include -#include - -#include "ignition/gazebo/EntityComponentManager.hh" -#include "ignition/gazebo/Export.hh" -#include "ignition/gazebo/Server.hh" -#include "ignition/gazebo/ServerConfig.hh" - -#include "Destroyable.hh" -#include "EntityComponentManager.hh" - -namespace ignition -{ -namespace gazebo -{ -namespace python -{ - class HelperFixturePrivate; - - class HelperFixture : - public ignition::gazebo::python::Destroyable, - public std::enable_shared_from_this - { - /// \brief Constructor - /// \param[in] _path Path to SDF file. - public: explicit HelperFixture(const std::string &_path); - - /// \brief Constructor - /// \param[in] _config Server config file - public: explicit HelperFixture(const ServerConfig &_config); - - /// \brief Destructor - public: virtual ~HelperFixture(); - - /// Force an early destruction of this object - void - Destroy() override; - - /// \brief Wrapper around a system's pre-update callback - /// \param[in] _cb Function to be called every pre-update - /// The _entity and _sdf will correspond to the world entity. - /// \return Reference to self. - public: void OnConfigure(std::function _cb); - - /// \brief Wrapper around a system's pre-update callback - /// \param[in] _cb Function to be called every pre-update - /// \return Reference to self. - public: void OnPreUpdate(std::function _cb); - - /// \brief Wrapper around a system's update callback - /// \param[in] _cb Function to be called every update - /// \return Reference to self. - public: void OnUpdate(std::function _cb); - - /// \brief Wrapper around a system's post-update callback - /// \param[in] _cb Function to be called every post-update - /// \return Reference to self. - public: void OnPostUpdate(std::function _cb); - - /// \brief Finalize all the functions and add fixture to server. - /// Finalize must be called before running the server, otherwise none of - /// the `On*` functions will be called. - /// The `OnConfigure` callback is called immediately on finalize. - public: void Finalize(); - - /// \brief Get pointer to underlying server. - public: std::shared_ptr Server() const; - - /// \internal - /// \brief Pointer to private data. - // TODO(ahcorde) Use IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) when porting to v6 - private: std::shared_ptr dataPtr; - }; - -/// \brief System that is inserted into the simulation loop to observe the ECM. -class HelperSystem : - public System, - public ISystemConfigure, - public ISystemPreUpdate, - public ISystemUpdate, - public ISystemPostUpdate -{ - // Documentation inherited - public: void Configure( - const Entity &_entity, - const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &_eventMgr) override; - - // Documentation inherited - public: void PreUpdate(const UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) override; - - // Documentation inherited - public: void Update(const UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) override; - - // Documentation inherited - public: void PostUpdate(const UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) override; - - /// \brief Function to call every time we configure a world - public: std::function - configureCallback_internal; - - /// \brief Function to call every pre-update - public: std::function - preUpdateCallback_internal; - - /// \brief Function to call every update - public: std::function - updateCallback_internal; - - /// \brief Function to call every post-update - public: std::function - postUpdateCallback_internal; -}; - -/// Define a pybind11 wrapper for an ignition::gazebo::HelperFixture -/** - * \param[in] module a pybind11 module to add the definition to - * \param[in] typestr name of the type used by Python - */ -void -defineGazeboHelperFixture(pybind11::object module); -} -} -} - -#endif // IGNITION_GAZEBO_PYTHON__HELPER_SYSTEM_HH_ diff --git a/python/src/ignition/gazebo/Server.hh b/python/src/ignition/gazebo/Server.hh index e03172beaa..6e969a035b 100644 --- a/python/src/ignition/gazebo/Server.hh +++ b/python/src/ignition/gazebo/Server.hh @@ -29,7 +29,6 @@ namespace python /// Define a pybind11 wrapper for an ignition::gazebo::Server /** * \param[in] module a pybind11 module to add the definition to - * \param[in] typestr name of the type used by Python */ void defineGazeboServer(pybind11::object module); diff --git a/python/src/ignition/gazebo/ServerConfig.hh b/python/src/ignition/gazebo/ServerConfig.hh index c54087e985..d955bd5f49 100644 --- a/python/src/ignition/gazebo/ServerConfig.hh +++ b/python/src/ignition/gazebo/ServerConfig.hh @@ -29,7 +29,6 @@ namespace python /// Define a pybind11 wrapper for an ignition::gazebo::ServerConfig /** * \param[in] module a pybind11 module to add the definition to - * \param[in] typestr name of the type used by Python */ void defineGazeboServerConfig(pybind11::object module); diff --git a/python/src/ignition/gazebo/TestFixture.cc b/python/src/ignition/gazebo/TestFixture.cc new file mode 100644 index 0000000000..be871830fd --- /dev/null +++ b/python/src/ignition/gazebo/TestFixture.cc @@ -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. + * +*/ +#include +#include + +#include "TestFixture.hh" + +#include "ignition/gazebo/TestFixture.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, + "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", &TestFixture::OnPreUpdate, + pybind11::return_value_policy::reference, + "Wrapper around a system's pre-update callback" + ) + .def( + "on_update", &TestFixture::OnUpdate, + pybind11::return_value_policy::reference_internal, + "Wrapper around a system's update callback" + ) + .def( + "on_post_update", &TestFixture::OnPostUpdate, + pybind11::return_value_policy::reference_internal, + "Wrapper around a system's post-update callback" + ) + .def( + "on_configure", &TestFixture::OnConfigure, + pybind11::return_value_policy::reference, + "Wrapper around a system's pre-update 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.hh b/python/src/ignition/gazebo/UpdateInfo.hh index 52f1496e6a..f2ec4e910d 100644 --- a/python/src/ignition/gazebo/UpdateInfo.hh +++ b/python/src/ignition/gazebo/UpdateInfo.hh @@ -29,7 +29,6 @@ namespace python /// Define a pybind11 wrapper for an ignition::gazebo::UpdateInfo /** * \param[in] module a pybind11 module to add the definition to - * \param[in] typestr name of the type used by Python */ void defineGazeboUpdateInfo(pybind11::object module); diff --git a/python/src/ignition/gazebo/World.hh b/python/src/ignition/gazebo/World.hh index 0c65f087d5..e77e42b4b6 100644 --- a/python/src/ignition/gazebo/World.hh +++ b/python/src/ignition/gazebo/World.hh @@ -20,11 +20,6 @@ #include #include -#include - -#include "EntityComponentManager.hh" - -#include namespace ignition { @@ -35,7 +30,6 @@ namespace python /// Define a pybind11 wrapper for an ignition::gazebo::World /** * \param[in] module a pybind11 module to add the definition to - * \param[in] typestr name of the type used by Python */ void defineGazeboWorld(pybind11::object module); diff --git a/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc b/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc index dc72178493..89340724bd 100644 --- a/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc +++ b/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc @@ -16,29 +16,26 @@ #include -#include "Destroyable.hh" #include "EntityComponentManager.hh" #include "EventManager.hh" #include "Exceptions.hh" -#include "HelperSystem.hh" #include "Server.hh" #include "ServerConfig.hh" +#include "TestFixture.hh" #include "UpdateInfo.hh" #include "World.hh" PYBIND11_MODULE(gazebo, m) { m.doc() = "Ignition Gazebo Python Library."; - ignition::gazebo::python::defineDestroyable(m); - pybind11::register_exception( m, "InvalidHandle", PyExc_RuntimeError); ignition::gazebo::python::defineGazeboEntityComponentManager(m); ignition::gazebo::python::defineGazeboEventManager(m); - ignition::gazebo::python::defineGazeboHelperFixture(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); } 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); +} From 999d6a6704b0df86aab7c895f605ed6e212d14ad Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 14 Jan 2022 18:03:05 +0100 Subject: [PATCH 12/21] Fixed python issue Signed-off-by: ahcorde --- .../{helperFixture.py => testFixture.py} | 0 .../ignition/gazebo/EntityComponentManager.hh | 2 +- python/src/ignition/gazebo/Exceptions.hh | 38 --- python/src/ignition/gazebo/TestFixture.cc | 107 ++++++- .../gazebo/_ignition_gazebo_pybind11.cc | 4 - python/src/ignition/gazebo/wrap_functions.hh | 270 ++++++++++++++++++ 6 files changed, 372 insertions(+), 49 deletions(-) rename examples/python/{helperFixture.py => testFixture.py} (100%) delete mode 100644 python/src/ignition/gazebo/Exceptions.hh create mode 100644 python/src/ignition/gazebo/wrap_functions.hh diff --git a/examples/python/helperFixture.py b/examples/python/testFixture.py similarity index 100% rename from examples/python/helperFixture.py rename to examples/python/testFixture.py diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 6715f757ba..1539b668a4 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -771,7 +771,7 @@ namespace ignition /// This was originally a unique_ptr, but pybind11 was failing because it /// was not able to copy the class /// \brief Private data pointer. - private: std::shared_ptr dataPtr; + private: std::unique_ptr dataPtr; /// \brief Add an entity and its components to a serialized state message. /// \param[out] _msg The state message. diff --git a/python/src/ignition/gazebo/Exceptions.hh b/python/src/ignition/gazebo/Exceptions.hh deleted file mode 100644 index 0da1426a16..0000000000 --- a/python/src/ignition/gazebo/Exceptions.hh +++ /dev/null @@ -1,38 +0,0 @@ -/* - * 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__EXCEPTIONS_HH_ -#define IGNITION_GAZEBO_PYTHON__EXCEPTIONS_HH_ - -#include -#include - -namespace ignition -{ -namespace gazebo -{ -namespace python -{ -class InvalidHandle : public std::runtime_error -{ - using std::runtime_error::runtime_error; -}; -} // namespace utils -} // namespace gazebo -} // namespace ignition - -#endif // IGNITION_GAZEBO_PYTHON__EXCEPTIONS_HH_ diff --git a/python/src/ignition/gazebo/TestFixture.cc b/python/src/ignition/gazebo/TestFixture.cc index be871830fd..98ae3c8a40 100644 --- a/python/src/ignition/gazebo/TestFixture.cc +++ b/python/src/ignition/gazebo/TestFixture.cc @@ -21,6 +21,74 @@ #include "ignition/gazebo/TestFixture.hh" +#include "wrap_functions.hh" + +// 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 + +// 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. + + +// 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)); +} + namespace ignition { namespace gazebo @@ -44,22 +112,49 @@ defineGazeboTestFixture(pybind11::object module) "Finalize all the functions and add fixture to server." ) .def( - "on_pre_update", &TestFixture::OnPreUpdate, + "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", &TestFixture::OnUpdate, - pybind11::return_value_policy::reference_internal, + "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", &TestFixture::OnPostUpdate, - pybind11::return_value_policy::reference_internal, + "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" ) .def( - "on_configure", &TestFixture::OnConfigure, + "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 pre-update callback" ); diff --git a/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc b/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc index 89340724bd..d7d0e28a43 100644 --- a/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc +++ b/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc @@ -18,7 +18,6 @@ #include "EntityComponentManager.hh" #include "EventManager.hh" -#include "Exceptions.hh" #include "Server.hh" #include "ServerConfig.hh" #include "TestFixture.hh" @@ -28,9 +27,6 @@ PYBIND11_MODULE(gazebo, m) { m.doc() = "Ignition Gazebo Python Library."; - pybind11::register_exception( - m, "InvalidHandle", PyExc_RuntimeError); - ignition::gazebo::python::defineGazeboEntityComponentManager(m); ignition::gazebo::python::defineGazeboEventManager(m); ignition::gazebo::python::defineGazeboServer(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..a79523bb44 --- /dev/null +++ b/python/src/ignition/gazebo/wrap_functions.hh @@ -0,0 +1,270 @@ +// 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