From 2db2efd01a4ce4f8af5e182bb0cebaf3bef9c939 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 12 Jan 2018 09:43:38 -0800 Subject: [PATCH] Proof of concept WaitSet with Pybind11 --- rclpy/CMakeLists.txt | 18 +- rclpy/cmake/FindPYBIND11.cmake | 37 +++ rclpy/rclpy/impl/implementation_singleton.py | 1 + rclpy/src/rclpy/_rclpy_wait_set.cpp | 244 +++++++++++++++++++ 4 files changed, 299 insertions(+), 1 deletion(-) create mode 100644 rclpy/cmake/FindPYBIND11.cmake create mode 100644 rclpy/src/rclpy/_rclpy_wait_set.cpp diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index c52b7c0ba..4e58b7d72 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.5) -project(rclpy C) +project(rclpy C CXX) # Default to C11 if(NOT CMAKE_C_STANDARD) @@ -90,6 +90,22 @@ ament_target_dependencies(rclpy_logging "rcutils" ) +set( CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_LIST_DIR}/cmake ) +find_package(PYBIND11 REQUIRED) + +add_library( + rclpy_wait_set + SHARED src/rclpy/_rclpy_wait_set.cpp +) +configure_python_c_extension_library(rclpy_wait_set) +set_target_properties(rclpy_wait_set PROPERTIES CXX_VISIBILITY_PRESET "hidden") +set_target_properties(rclpy_wait_set PROPERTIES CXX_STANDARD 11) +target_include_directories(rclpy_wait_set PRIVATE ${PYBIND11_INCLUDE_DIR}) +ament_target_dependencies(rclpy_wait_set + "rcl" + "rcutils" +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/rclpy/cmake/FindPYBIND11.cmake b/rclpy/cmake/FindPYBIND11.cmake new file mode 100644 index 000000000..2f29dd9fc --- /dev/null +++ b/rclpy/cmake/FindPYBIND11.cmake @@ -0,0 +1,37 @@ +# Copyright 2018 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(FindPackageHandleStandardArgs) + +# Look for pybind11 outright +find_path(PYBIND11_INCLUDE_DIR + NAMES pybind11/pybind11.h +) + +if(NOT PYBIND11_INCLUDE_DIR) + # Try using python code to get the include path + set(Python_ADDITIONAL_VERSIONS 3) + include(CMakeFindDependencyMacro) + find_dependency(PythonInterp) + + if(PYTHONINTERP_FOUND) + execute_process( + COMMAND ${PYTHON_EXECUTABLE} -c "import pybind11; print(pybind11.get_include())" + OUTPUT_VARIABLE PYBIND11_INCLUDE_DIR + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + endif() +endif() + +find_package_handle_standard_args(PYBIND11 DEFAULT_MSG PYBIND11_INCLUDE_DIR) diff --git a/rclpy/rclpy/impl/implementation_singleton.py b/rclpy/rclpy/impl/implementation_singleton.py index 1f984bd66..b4dafc4ec 100644 --- a/rclpy/rclpy/impl/implementation_singleton.py +++ b/rclpy/rclpy/impl/implementation_singleton.py @@ -30,3 +30,4 @@ rclpy_implementation = _import('._rclpy') rclpy_logging_implementation = _import('._rclpy_logging') +rclpy_wait_set = _import('._rclpy_wait_set') diff --git a/rclpy/src/rclpy/_rclpy_wait_set.cpp b/rclpy/src/rclpy/_rclpy_wait_set.cpp new file mode 100644 index 000000000..a2fbeaa28 --- /dev/null +++ b/rclpy/src/rclpy/_rclpy_wait_set.cpp @@ -0,0 +1,244 @@ +// Copyright 2018 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 +#include +#include +#include + +namespace py = pybind11; + + +namespace rclpy +{ +class WaitSet +{ +public: + void add_subscription(py::object subscription); + void add_guard_condition(py::object guard_condition); + void add_timer(py::object timer); + void add_client(py::object client); + void add_service(py::object service); + void wait(int timeout); + bool is_subscription_ready(py::object subscription); + bool is_guard_condition_ready(py::object guard_condition); + bool is_timer_ready(py::object timer); + bool is_client_ready(py::object client); + bool is_service_ready(py::object service); + +private: + std::vector pysubs; + std::vector pygcs; + std::vector pytmrs; + std::vector pyclis; + std::vector pysrvs; +}; + +template +RCL_TYPE * get_handle(py::object entity, const std::string & type) +{ + py::capsule capsule = py::getattr(entity, (type + std::string("_handle")).c_str()); + + void * handle = PyCapsule_GetPointer( + capsule.ptr(), (std::string("rcl_") + type + std::string("_t")).c_str()); + if (handle == NULL) { + throw pybind11::error_already_set(); + } + return static_cast(handle); +} + +void WaitSet::add_subscription(py::object subscription) +{ + pysubs.push_back(get_handle(subscription, "subscription")); +} + +void WaitSet::add_guard_condition(py::object guard_condition) +{ + pygcs.push_back(get_handle(guard_condition, "guard_condition")); +} + +void WaitSet::add_timer(py::object timer) +{ + pytmrs.push_back(get_handle(timer, "timer")); +} + +void WaitSet::add_client(py::object client) +{ + pyclis.push_back(get_handle(client, "client")); +} + +void WaitSet::add_service(py::object service) +{ + pysrvs.push_back(get_handle(service, "service")); +} + +void WaitSet::wait(int timeout) +{ + rcl_ret_t ret; + + // Initialize the wait set + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init( + &wait_set, pysubs.size(), pygcs.size(), pytmrs.size(), pyclis.size(), pysrvs.size(), + rcl_get_default_allocator()); + + // Populate the wait set + for (std::size_t i = 0; i < pysubs.size(); ++i) { + wait_set.subscriptions[i] = pysubs[i]; + } + for (std::size_t i = 0; i < pygcs.size(); ++i) { + wait_set.guard_conditions[i] = pygcs[i]; + } + for (std::size_t i = 0; i < pytmrs.size(); ++i) { + wait_set.timers[i] = pytmrs[i]; + } + for (std::size_t i = 0; i < pyclis.size(); ++i) { + wait_set.clients[i] = pyclis[i]; + } + for (std::size_t i = 0; i < pysrvs.size(); ++i) { + wait_set.services[i] = pysrvs[i]; + } + + { // Release the GIL and wait + py::gil_scoped_release release; + ret = rcl_wait(&wait_set, timeout); + } + if (ret != RCL_RET_OK) { + // becomes RuntimeError in python + auto e = std::runtime_error( + std::string("Failed to wait: ") + std::string(rcl_get_error_string_safe())); + rcl_reset_error(); + throw e; + } + + // Get the result from the wait set + std::vector ready_subscriptions; + std::vector ready_guard_conditions; + std::vector ready_timers; + std::vector ready_clients; + std::vector ready_services; + if (ret != RCL_RET_TIMEOUT) { + for (std::size_t e = 0; e < pysubs.size(); ++e) { + for (std::size_t i = 0; i < wait_set.size_of_subscriptions; ++i) { + if (wait_set.subscriptions[i] == pysubs[e]) { + ready_subscriptions.push_back(pysubs[e]); + break; + } + } + } + for (std::size_t e = 0; e < pygcs.size(); ++e) { + for (std::size_t i = 0; i < wait_set.size_of_guard_conditions; ++i) { + if (wait_set.guard_conditions[i] == pygcs[e]) { + ready_guard_conditions.push_back(pygcs[e]); + break; + } + } + } + for (std::size_t e = 0; e < pytmrs.size(); ++e) { + for (std::size_t i = 0; i < wait_set.size_of_timers; ++i) { + if (wait_set.timers[i] == pytmrs[e]) { + ready_timers.push_back(pytmrs[e]); + break; + } + } + } + for (std::size_t e = 0; e < pyclis.size(); ++e) { + for (std::size_t i = 0; i < wait_set.size_of_clients; ++i) { + if (wait_set.clients[i] == pyclis[e]) { + ready_clients.push_back(pyclis[e]); + break; + } + } + } + for (std::size_t e = 0; e < pysrvs.size(); ++e) { + for (std::size_t i = 0; i < wait_set.size_of_services; ++i) { + if (wait_set.services[i] == pysrvs[e]) { + ready_services.push_back(pysrvs[e]); + break; + } + } + } + } + pysubs = std::move(ready_subscriptions); + pygcs = std::move(ready_guard_conditions); + pytmrs = std::move(ready_timers); + pyclis = std::move(ready_clients); + pysrvs = std::move(ready_services); +} + +bool WaitSet::is_subscription_ready(py::object subscription) +{ + return std::find(pysubs.begin(), pysubs.end(), + get_handle(subscription, "subscription")) != pysubs.end(); +} + +bool WaitSet::is_guard_condition_ready(py::object guard_condition) +{ + return std::find(pygcs.begin(), pygcs.end(), + get_handle(guard_condition, "guard_condition")) != pygcs.end(); +} + +bool WaitSet::is_timer_ready(py::object timer) +{ + return std::find(pytmrs.begin(), pytmrs.end(), + get_handle(timer, "timer")) != pytmrs.end(); +} + +bool WaitSet::is_client_ready(py::object client) +{ + return std::find(pyclis.begin(), pyclis.end(), + get_handle(client, "client")) != pyclis.end(); +} + +bool WaitSet::is_service_ready(py::object service) +{ + return std::find(pysrvs.begin(), pysrvs.end(), + get_handle(service, "service")) != pysrvs.end(); +} +} // namespace rclpy + + +PYBIND11_MODULE(_rclpy_wait_set, m) { + m.doc() = + R"pbdoc( + _rclpy_wait_set + ----------------------- + Implements code to interface python and rcl. + )pbdoc"; + + py::class_(m, "WaitSet") + .def(py::init<>()) + .def("add_subscription", &rclpy::WaitSet::add_subscription) + .def("add_guard_condition", &rclpy::WaitSet::add_guard_condition) + .def("add_timer", &rclpy::WaitSet::add_timer) + .def("add_client", &rclpy::WaitSet::add_client) + .def("add_service", &rclpy::WaitSet::add_service) + .def("wait", &rclpy::WaitSet::wait, "this is a doc string", py::arg("timeout") = -1) + .def("is_subscription_ready", &rclpy::WaitSet::is_subscription_ready) + .def("is_guard_condition_ready", &rclpy::WaitSet::is_guard_condition_ready) + .def("is_timer_ready", &rclpy::WaitSet::is_timer_ready) + .def("is_client_ready", &rclpy::WaitSet::is_client_ready) + .def("is_service_ready", &rclpy::WaitSet::is_service_ready); + + // TODO(sloretz) target_compile_definisions VERSION_INFO +#ifdef VERSION_INFO + m.attr("__version__") = VERSION_INFO; +#else + m.attr("__version__") = "dev"; +#endif +}