diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index df8ba7df7..2189cb0f7 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -166,9 +166,11 @@ pybind11_add_module(_rclpy_pybind11 SHARED src/rclpy/client.cpp src/rclpy/clock.cpp src/rclpy/context.cpp + src/rclpy/destroyable.cpp src/rclpy/duration.cpp src/rclpy/graph.cpp src/rclpy/guard_condition.cpp + src/rclpy/handle.cpp src/rclpy/init.cpp src/rclpy/logging.cpp src/rclpy/names.cpp diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py index f3274738e..d875a0528 100644 --- a/rclpy/rclpy/client.py +++ b/rclpy/rclpy/client.py @@ -33,7 +33,7 @@ class Client: def __init__( self, context: Context, - client_handle, + client_impl: _rclpy.Client, srv_type: SrvType, srv_name: str, qos_profile: QoSProfile, @@ -46,7 +46,7 @@ def __init__( should call :meth:`.Node.create_client`. :param context: The context associated with the service client. - :param client_handle: :class:`Handle` wrapping the underlying ``rcl_client_t`` object. + :param client_impl: :class:`_rclpy.Client` wrapping the underlying ``rcl_client_t`` object. :param srv_type: The service type. :param srv_name: The name of the service. :param qos_profile: The quality of service profile to apply the service client. @@ -54,7 +54,7 @@ def __init__( nodes default callback group is used. """ self.context = context - self.__handle = client_handle + self.__client = client_impl self.srv_type = srv_type self.srv_name = srv_name self.qos_profile = qos_profile @@ -122,8 +122,8 @@ def call_async(self, request: SrvTypeRequest) -> Future: if not isinstance(request, self.srv_type.Request): raise TypeError() - with self.handle as capsule: - sequence_number = _rclpy.rclpy_send_request(capsule, request) + with self.handle: + sequence_number = self.__client.send_request(request) if sequence_number in self._pending_requests: raise RuntimeError('Sequence (%r) conflicts with pending request' % sequence_number) @@ -140,8 +140,8 @@ def service_is_ready(self) -> bool: :return: ``True`` if a server is ready, ``False`` otherwise. """ - with self.handle as capsule: - return _rclpy.rclpy_service_server_is_available(capsule) + with self.handle: + return self.__client.service_server_is_available() def wait_for_service(self, timeout_sec: float = None) -> bool: """ @@ -165,7 +165,7 @@ def wait_for_service(self, timeout_sec: float = None) -> bool: @property def handle(self): - return self.__handle + return self.__client def destroy(self): - self.handle.destroy() + self.__client.destroy_when_not_in_use() diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index 05d681255..c0c6cc034 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -340,8 +340,8 @@ async def _execute_subscription(self, sub, msg): await await_or_execute(sub.callback, msg) def _take_client(self, client): - with client.handle as capsule: - return _rclpy.rclpy_take_response(capsule, client.srv_type.Response) + with client.handle: + return client.handle.take_response(client.srv_type.Response) async def _execute_client(self, client, seq_and_response): header, response = seq_and_response @@ -519,10 +519,11 @@ def _wait_for_ready_callbacks( except InvalidHandle: entity_count.num_subscriptions -= 1 - client_capsules = [] + client_handles = [] for cli in clients: try: - client_capsules.append(context_stack.enter_context(cli.handle)) + context_stack.enter_context(cli.handle) + client_handles.append(cli.handle) except InvalidHandle: entity_count.num_clients -= 1 @@ -561,8 +562,8 @@ def _wait_for_ready_callbacks( _rclpy.rclpy_wait_set_clear_entities(wait_set) for sub_capsule in sub_capsules: _rclpy.rclpy_wait_set_add_entity('subscription', wait_set, sub_capsule) - for cli_capsule in client_capsules: - _rclpy.rclpy_wait_set_add_entity('client', wait_set, cli_capsule) + for cli_handle in client_handles: + _rclpy.rclpy_wait_set_add_client(wait_set, cli_handle) for srv_capsule in service_capsules: _rclpy.rclpy_wait_set_add_entity('service', wait_set, srv_capsule) for tmr_capsule in timer_capsules: diff --git a/rclpy/rclpy/handle.py b/rclpy/rclpy/handle.py index e46cc433a..c5077632a 100644 --- a/rclpy/rclpy/handle.py +++ b/rclpy/rclpy/handle.py @@ -15,11 +15,11 @@ from threading import Lock from rclpy.impl.implementation_singleton import rclpy_handle_implementation as _rclpy_handle +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.impl.implementation_singleton import rclpy_pycapsule_implementation as _rclpy_capsule -class InvalidHandle(Exception): - pass +InvalidHandle = _rclpy.InvalidHandle class Handle: diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index d80d24417..9f517f0dd 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -1383,7 +1383,7 @@ def create_client( failed = False try: with self.handle as node_capsule: - client_capsule = _rclpy.rclpy_create_client( + client_impl = _rclpy.Client( node_capsule, srv_type, srv_name, @@ -1393,11 +1393,9 @@ def create_client( if failed: self._validate_topic_or_service_name(srv_name, is_service=True) - client_handle = Handle(client_capsule) - client = Client( self.context, - client_handle, srv_type, srv_name, qos_profile, + client_impl, srv_type, srv_name, qos_profile, callback_group) self.__clients.append(client) callback_group.add_entity(client) diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index 24fd1bc2a..76ce8f380 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -19,6 +19,7 @@ #include "client.hpp" #include "clock.hpp" #include "context.hpp" +#include "destroyable.hpp" #include "duration.hpp" #include "graph.hpp" #include "guard_condition.hpp" @@ -43,6 +44,8 @@ namespace py = pybind11; PYBIND11_MODULE(_rclpy_pybind11, m) { m.doc() = "ROS 2 Python client library."; + rclpy::define_destroyable(m); + py::enum_(m, "ClockType") .value("UNINITIALIZED", RCL_CLOCK_UNINITIALIZED) .value("ROS_TIME", RCL_ROS_TIME) @@ -100,6 +103,8 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { m, "UnsupportedEventTypeError", rclerror.ptr()); py::register_exception( m, "NotImplementedError", PyExc_NotImplementedError); + py::register_exception( + m, "InvalidHandle", PyExc_RuntimeError); m.def( "rclpy_init", &rclpy::init, @@ -108,18 +113,7 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { "rclpy_shutdown", &rclpy::shutdown, "rclpy_shutdown."); - m.def( - "rclpy_create_client", &rclpy::client_create, - "Create a Client"); - m.def( - "rclpy_send_request", &rclpy::client_send_request, - "Send a request"); - m.def( - "rclpy_service_server_is_available", &rclpy::client_service_server_is_available, - "Return true if the service server is available"); - m.def( - "rclpy_take_response", &rclpy::client_take_response, - "rclpy_take_response"); + rclpy::define_client(m); m.def( "rclpy_context_get_domain_id", &rclpy::context_get_domain_id, @@ -268,6 +262,9 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { m.def( "rclpy_wait_set_add_entity", &rclpy::wait_set_add_entity, "rclpy_wait_set_add_entity."); + m.def( + "rclpy_wait_set_add_client", &rclpy::wait_set_add_client, + "Add a client to the wait set."); m.def( "rclpy_wait_set_is_ready", &rclpy::wait_set_is_ready, "rclpy_wait_set_is_ready."); diff --git a/rclpy/src/rclpy/client.cpp b/rclpy/src/rclpy/client.cpp index 6357a9328..a171d568e 100644 --- a/rclpy/src/rclpy/client.cpp +++ b/rclpy/src/rclpy/client.cpp @@ -21,48 +21,26 @@ #include #include "rclpy_common/common.h" -#include "rclpy_common/handle.h" - -#include "rclpy_common/exceptions.hpp" #include "client.hpp" +#include "rclpy_common/exceptions.hpp" +#include "utils.hpp" namespace rclpy { -static void -_rclpy_destroy_client(void * p) -{ - auto cli = static_cast(p); - if (!cli) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_client got NULL pointer"); - return; - } - rcl_ret_t ret = rcl_client_fini(&(cli->client), cli->node); - if (RCL_RET_OK != ret) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "Failed to fini client: %s", - rcl_get_error_string().str); - rcl_reset_error(); - } - PyMem_Free(cli); +void +Client::destroy() +{ + rcl_client_.reset(); + node_handle_.reset(); } -py::capsule -client_create( - py::capsule pynode, py::object pysrv_type, std::string service_name, - py::capsule pyqos_profile) +Client::Client( + py::capsule pynode, py::object pysrv_type, const char * service_name, py::capsule pyqos_profile) +: node_handle_(std::make_shared(pynode)) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } + auto node = node_handle_->cast("rcl_node_t"); auto srv_type = static_cast( rclpy_common_get_type_support(pysrv_type.ptr())); @@ -80,22 +58,31 @@ client_create( client_ops.qos = *qos_profile; } - // Use smart pointer to make sure memory is free'd on error - auto deleter = [](rclpy_client_t * ptr) {_rclpy_destroy_client(ptr);}; - auto cli = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rclpy_client_t))), - deleter); - if (!cli) { - throw std::bad_alloc(); - } - cli->client = rcl_get_zero_initialized_client(); - cli->node = node; + // Create a client + rcl_client_ = std::shared_ptr( + new rcl_client_t, + [this](rcl_client_t * client) + { + auto node = node_handle_->cast_or_warn("rcl_node_t"); + + rcl_ret_t ret = rcl_client_fini(client, node); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, "Failed to fini client: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete client; + }); + + *rcl_client_ = rcl_get_zero_initialized_client(); rcl_ret_t ret = rcl_client_init( - &(cli->client), node, srv_type, - service_name.c_str(), &client_ops); - if (ret != RCL_RET_OK) { - if (ret == RCL_RET_SERVICE_NAME_INVALID) { + rcl_client_.get(), node, srv_type, service_name, &client_ops); + if (RCL_RET_OK != ret) { + if (RCL_RET_SERVICE_NAME_INVALID == ret) { std::string error_text{"failed to create client due to invalid service name '"}; error_text += service_name; error_text += "': "; @@ -105,35 +92,11 @@ client_create( } throw RCLError("failed to create client"); } - - PyObject * pycli_c = - rclpy_create_handle_capsule(cli.get(), "rclpy_client_t", _rclpy_destroy_client); - if (!pycli_c) { - throw py::error_already_set(); - } - auto pycli = py::reinterpret_steal(pycli_c); - // pycli now owns the rclpy_client_t - cli.release(); - - auto cli_handle = static_cast(pycli); - auto node_handle = static_cast(pynode); - _rclpy_handle_add_dependency(cli_handle, node_handle); - if (PyErr_Occurred()) { - throw py::error_already_set(); - } - - return pycli; } int64_t -client_send_request(py::capsule pyclient, py::object pyrequest) +Client::send_request(py::object pyrequest) { - auto client = static_cast( - rclpy_handle_get_pointer_from_capsule(pyclient.ptr(), "rclpy_client_t")); - if (!client) { - throw py::error_already_set(); - } - destroy_ros_message_signature * destroy_ros_message = nullptr; void * raw_ros_request = rclpy_convert_from_py(pyrequest.ptr(), &destroy_ros_message); if (!raw_ros_request) { @@ -141,9 +104,9 @@ client_send_request(py::capsule pyclient, py::object pyrequest) } int64_t sequence_number; - rcl_ret_t ret = rcl_send_request(&(client->client), raw_ros_request, &sequence_number); + rcl_ret_t ret = rcl_send_request(rcl_client_.get(), raw_ros_request, &sequence_number); destroy_ros_message(raw_ros_request); - if (ret != RCL_RET_OK) { + if (RCL_RET_OK != ret) { throw RCLError("failed to send request"); } @@ -151,17 +114,12 @@ client_send_request(py::capsule pyclient, py::object pyrequest) } bool -client_service_server_is_available(py::capsule pyclient) +Client::service_server_is_available() { - auto client = static_cast( - rclpy_handle_get_pointer_from_capsule(pyclient.ptr(), "rclpy_client_t")); - if (!client) { - throw py::error_already_set(); - } - + auto node = node_handle_->cast("rcl_node_t"); bool is_ready; - rcl_ret_t ret = rcl_service_server_is_available(client->node, &(client->client), &is_ready); - if (ret != RCL_RET_OK) { + rcl_ret_t ret = rcl_service_server_is_available(node, rcl_client_.get(), &is_ready); + if (RCL_RET_OK != ret) { throw RCLError("failed to check service availability"); } @@ -175,22 +133,9 @@ _rclpy_destroy_service_info(PyObject * pycapsule) } py::tuple -client_take_response(py::capsule pyclient, py::object pyresponse_type) +Client::take_response(py::object pyresponse_type) { - auto client = static_cast( - rclpy_handle_get_pointer_from_capsule(pyclient.ptr(), "rclpy_client_t")); - if (!client) { - throw py::error_already_set(); - } - - destroy_ros_message_signature * destroy_ros_message = nullptr; - void * taken_response_ptr = rclpy_create_from_py(pyresponse_type.ptr(), &destroy_ros_message); - if (!taken_response_ptr) { - throw py::error_already_set(); - } - auto message_deleter = [destroy_ros_message](void * ptr) {destroy_ros_message(ptr);}; - auto taken_response = std::unique_ptr( - taken_response_ptr, message_deleter); + auto taken_response = create_from_py(pyresponse_type); auto deleter = [](rmw_service_info_t * ptr) {PyMem_Free(ptr);}; auto header = std::unique_ptr( @@ -202,24 +147,44 @@ client_take_response(py::capsule pyclient, py::object pyresponse_type) py::tuple result_tuple(2); rcl_ret_t ret = rcl_take_response_with_info( - &(client->client), header.get(), taken_response.get()); + rcl_client_.get(), header.get(), taken_response.get()); if (ret == RCL_RET_CLIENT_TAKE_FAILED) { result_tuple[0] = py::none(); result_tuple[1] = py::none(); return result_tuple; } + if (RCL_RET_OK != ret) { + throw RCLError("encountered error when taking client response"); + } result_tuple[0] = py::capsule( header.release(), "rmw_service_info_t", _rclpy_destroy_service_info); - PyObject * pytaken_response_c = rclpy_convert_to_py(taken_response.get(), pyresponse_type.ptr()); - if (!pytaken_response_c) { - throw py::error_already_set(); - } - result_tuple[1] = py::reinterpret_steal(pytaken_response_c); + result_tuple[1] = convert_to_py(taken_response.get(), pyresponse_type); // result_tuple now owns the message taken_response.release(); return result_tuple; } + +void +define_client(py::object module) +{ + py::class_(module, "Client") + .def(py::init()) + .def_property_readonly( + "pointer", [](const Client & client) { + return reinterpret_cast(client.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "send_request", &Client::send_request, + "Send a request") + .def( + "service_server_is_available", &Client::service_server_is_available, + "Return true if the service server is available") + .def( + "take_response", &Client::take_response, + "Take a received response from an earlier request"); +} } // namespace rclpy diff --git a/rclpy/src/rclpy/client.hpp b/rclpy/src/rclpy/client.hpp index ecdf82ad1..0600fca6a 100644 --- a/rclpy/src/rclpy/client.hpp +++ b/rclpy/src/rclpy/client.hpp @@ -17,67 +17,91 @@ #include -#include +#include + +#include + +#include "destroyable.hpp" +#include "handle.hpp" namespace py = pybind11; namespace rclpy { -/// Create a client -/** - * This function will create a client for the given service name. - * This client will use the typesupport defined in the service module - * provided as pysrv_type to send messages over the wire. - * - * On a successful call a Capsule pointing to the pointer of the created - * rclpy_client_t * is returned. - * - * Raises ValueError if the capsules are not the correct types - * Raises RuntimeError if the client could not be created - * - * \param[in] pynode Capsule pointing to the node to add the client to - * \param[in] pysrv_type Service module associated with the client - * \param[in] service_name Python object containing the service name - * \param[in] pyqos_profile QoSProfile Python object for this client - * \return capsule containing the rclpy_client_t - */ -py::capsule -client_create( - py::capsule pynode, py::object pysrv_type, std::string service_name, - py::capsule pyqos_profile); +class Client : public Destroyable +{ +public: + /// Create a client + /** + * This function will create a client for the given service name. + * This client will use the typesupport defined in the service module + * provided as pysrv_type to send messages. + * + * Raises ValueError if the capsules are not the correct types + * Raises RuntimeError if the client could not be created + * + * \param[in] pynode Capsule pointing to the node to add the client to + * \param[in] pysrv_type Service module associated with the client + * \param[in] service_name Python object containing the service name + * \param[in] pyqos_profile QoSProfile Python object for this client + */ + Client(py::capsule pynode, py::object pysrv_type, const char * service_name, py::capsule pyqos); -/// Publish a request message -/** - * Raises ValueError if pyclient is not a client capsule - * Raises RuntimeError if the request could not be sent - * - * \param[in] pyclient Capsule pointing to the client - * \param[in] pyrequest request message to send - * \return sequence_number Index of the sent request - */ -int64_t -client_send_request(py::capsule pyclient, py::object pyrequest); + ~Client() = default; -/// Check if a service server is available -/** - * Raises ValueError if the arguments are not capsules - * - * \param[in] pyclient Capsule pointing to the client - * \return True if the service server is available - */ -bool -client_service_server_is_available(py::capsule pyclient); + /// Publish a request message + /** + * Raises ValueError if pyclient is not a client capsule + * Raises RuntimeError if the request could not be sent + * + * \param[in] pyrequest request message to send + * \return sequence_number Index of the sent request + */ + int64_t + send_request(py::object pyrequest); + + /// Check if a service server is available + /** + * Raises ValueError if the arguments are not capsules + * + * \return True if the service server is available + */ + bool + service_server_is_available(); + + /// Take a response from a given client + /** + * Raises ValueError if pyclient is not a client capsule + * + * \param[in] pyresponse_type Instance of the message type to take + * \return 2-tuple sequence number and received response, or None if there is no response + */ + py::tuple + take_response(py::object pyresponse_type); + + /// Get rcl_client_t pointer + rcl_client_t * + rcl_ptr() const + { + return rcl_client_.get(); + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + // TODO(sloretz) replace with std::shared_ptr when rclpy::Node exists + std::shared_ptr node_handle_; + std::shared_ptr rcl_client_; +}; -/// Take a response from a given client +/// Define a pybind11 wrapper for an rclpy::Client /** - * Raises ValueError if pyclient is not a client capsule - * - * \param[in] pyclient Capsule pointing to the client to process the response - * \param[in] pyresponse_type Instance of the message type to take - * \return 2-tuple sequence number and received response or None, None if there is no response + * \param[in] module a pybind11 module to add the definition to */ -py::tuple -client_take_response(py::capsule pyclient, py::object pyresponse_type); +void +define_client(py::object module); } // namespace rclpy #endif // RCLPY__CLIENT_HPP_ diff --git a/rclpy/src/rclpy/destroyable.cpp b/rclpy/src/rclpy/destroyable.cpp new file mode 100644 index 000000000..fa6ced3a1 --- /dev/null +++ b/rclpy/src/rclpy/destroyable.cpp @@ -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. + +#include + +#include + +#include "destroyable.hpp" +#include "rclpy_common/exceptions.hpp" + +namespace rclpy +{ +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 NotImplementedError("Internal error: Destroyable subclass didn't override destroy()"); +} + +void +Destroyable::destroy_when_not_in_use() +{ + 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 rclpy diff --git a/rclpy/src/rclpy/destroyable.hpp b/rclpy/src/rclpy/destroyable.hpp new file mode 100644 index 000000000..333db0916 --- /dev/null +++ b/rclpy/src/rclpy/destroyable.hpp @@ -0,0 +1,60 @@ +// 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 RCLPY__DESTROYABLE_HPP_ +#define RCLPY__DESTROYABLE_HPP_ + +#include + +namespace py = pybind11; + +namespace rclpy +{ +/// This class blocks destruction when in use +class Destroyable +{ +public: + /// 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 rclpy + +#endif // RCLPY__DESTROYABLE_HPP_ diff --git a/rclpy/src/rclpy/handle.cpp b/rclpy/src/rclpy/handle.cpp new file mode 100644 index 000000000..91b7ac20a --- /dev/null +++ b/rclpy/src/rclpy/handle.cpp @@ -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. + +#include + +#include "rclpy_common/handle.h" + +#include "handle.hpp" + +namespace rclpy +{ +Handle::Handle(py::capsule pyhandle) +: pyhandle_(pyhandle) +{ + inc_ref(); +} + +Handle::~Handle() +{ + dec_ref(); +} + +Handle & +Handle::operator=(const Handle & other) +{ + dec_ref(); + + pyhandle_ = other.pyhandle_; + + inc_ref(); + + return *this; +} + +void Handle::inc_ref() +{ + if (pyhandle_.ptr()) { + // Forced to assume this uses rclpy_handle_t because pycapsule name gives no clues + auto c_handle = static_cast(pyhandle_); + // Increment the reference count + _rclpy_handle_inc_ref(c_handle); + } +} + +void Handle::dec_ref() +{ + if (pyhandle_.ptr()) { + // Forced to assume this uses rclpy_handle_t because pycapsule name gives no clues + auto c_handle = static_cast(pyhandle_); + // Decrement the reference count + _rclpy_handle_dec_ref(c_handle); + } +} + +void * +Handle::rcl_ptr(const char * capsule_name) noexcept +{ + return rclpy_handle_get_pointer_from_capsule(pyhandle_.ptr(), capsule_name); +} +} // namespace rclpy diff --git a/rclpy/src/rclpy/handle.hpp b/rclpy/src/rclpy/handle.hpp new file mode 100644 index 000000000..1f742f5bb --- /dev/null +++ b/rclpy/src/rclpy/handle.hpp @@ -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 RCLPY__HANDLE_HPP_ +#define RCLPY__HANDLE_HPP_ + +#include + +namespace py = pybind11; + +namespace rclpy +{ +/// RAII wrapper around rclpy_handle_t that keeps it alive +class Handle +{ +public: + Handle() = default; + + /// Create a handle instance wrapping a pycapsule to a type that uses rclpy_handle_t + explicit Handle(py::capsule pyhandle); + + ~Handle(); + + /// assignment operator + Handle & operator=(const Handle & other); + + /// Get the rcl pointer belonging to the handle or throw an exception + template + T + cast(const char * capsule_name) + { + void * ptr = rcl_ptr(capsule_name); + if (!ptr) { + throw py::error_already_set(); + } + return static_cast(ptr); + } + + /// Get the rcl pointer belonging to the handle or print a warning + template + T + cast_or_warn(const char * capsule_name) noexcept + { + void * ptr = rcl_ptr(capsule_name); + if (!ptr) { + PyErr_Clear(); + int stack_level = 1; + PyErr_WarnFormat(PyExc_RuntimeWarning, stack_level, "Failed to get rclpy_handle_t pointer"); + } + return static_cast(ptr); + } + +private: + py::capsule pyhandle_; + + void inc_ref(); + void dec_ref(); + + void * + rcl_ptr(const char * capsule_name) noexcept; +}; +} // namespace rclpy + +#endif // RCLPY__HANDLE_HPP_ diff --git a/rclpy/src/rclpy/wait_set.cpp b/rclpy/src/rclpy/wait_set.cpp index 60b51f53b..73ccd31cc 100644 --- a/rclpy/src/rclpy/wait_set.cpp +++ b/rclpy/src/rclpy/wait_set.cpp @@ -135,13 +135,6 @@ wait_set_add_entity(const std::string & entity_type, py::capsule pywait_set, py: throw py::error_already_set(); } ret = rcl_wait_set_add_subscription(wait_set, &(sub->subscription), &index); - } else if ("client" == entity_type) { - auto client = static_cast( - rclpy_handle_get_pointer_from_capsule(pyentity.ptr(), "rclpy_client_t")); - if (!client) { - throw py::error_already_set(); - } - ret = rcl_wait_set_add_client(wait_set, &(client->client), &index); } else if ("service" == entity_type) { auto srv = static_cast( rclpy_handle_get_pointer_from_capsule(pyentity.ptr(), "rclpy_service_t")); @@ -185,6 +178,22 @@ wait_set_add_entity(const std::string & entity_type, py::capsule pywait_set, py: return index; } +size_t +wait_set_add_client(const py::capsule pywait_set, const Client & client) +{ + if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { + throw py::value_error("capsule is not an rcl_wait_set_t"); + } + auto wait_set = static_cast(pywait_set); + + size_t index; + rcl_ret_t ret = rcl_wait_set_add_client(wait_set, client.rcl_ptr(), &index); + if (RCL_RET_OK != ret) { + throw RCLError("failed to add client to wait set"); + } + return index; +} + bool wait_set_is_ready(const std::string & entity_type, py::capsule pywait_set, size_t index) { diff --git a/rclpy/src/rclpy/wait_set.hpp b/rclpy/src/rclpy/wait_set.hpp index eae3f22d9..da7ce1268 100644 --- a/rclpy/src/rclpy/wait_set.hpp +++ b/rclpy/src/rclpy/wait_set.hpp @@ -17,8 +17,11 @@ #include +#include #include +#include "client.hpp" + namespace py = pybind11; namespace rclpy @@ -73,6 +76,17 @@ wait_set_clear_entities(py::capsule pywait_set); size_t wait_set_add_entity(const std::string & entity_type, py::capsule pywait_set, py::capsule pyentity); +/// Add a client to the wait set structure +/** + * Raises RCLError if any lower level error occurs + * + * \param[in] pywait_set Capsule pointing to the wait set structure + * \param[in] client a client to add to the wait set + * \return Index in waitset entity was added at + */ +size_t +wait_set_add_client(const py::capsule pywait_set, const Client & client); + /// Check if an entity in the wait set is ready by its index /** * This must be called after waiting on the wait set. diff --git a/rclpy/src/rclpy_common/include/rclpy_common/exceptions.hpp b/rclpy/src/rclpy_common/include/rclpy_common/exceptions.hpp index 2ff0afeaa..d40513174 100644 --- a/rclpy/src/rclpy_common/include/rclpy_common/exceptions.hpp +++ b/rclpy/src/rclpy_common/include/rclpy_common/exceptions.hpp @@ -86,6 +86,11 @@ class NotImplementedError : public RCLError using RCLError::RCLError; }; +class InvalidHandle : public std::runtime_error +{ + using std::runtime_error::runtime_error; +}; + } // namespace rclpy #endif // RCLPY_COMMON__EXCEPTIONS_HPP_ diff --git a/rclpy/src/rclpy_common/include/rclpy_common/handle.h b/rclpy/src/rclpy_common/include/rclpy_common/handle.h index 47039ed43..44c9d85f5 100644 --- a/rclpy/src/rclpy_common/include/rclpy_common/handle.h +++ b/rclpy/src/rclpy_common/include/rclpy_common/handle.h @@ -112,6 +112,14 @@ RCLPY_COMMON_PUBLIC void _rclpy_handle_add_dependency(rclpy_handle_t * dependent, rclpy_handle_t * dependency); +/// Increments the reference count of a handle. +/** + * If a reference count is manually incremented, then it must later be manually decremented. + */ +RCLPY_COMMON_PUBLIC +void +_rclpy_handle_inc_ref(rclpy_handle_t * handle); + /// Decrements the reference count of a handle. /** * The reference count of `handle` is decremented. diff --git a/rclpy/src/rclpy_common/src/handle.c b/rclpy/src/rclpy_common/src/handle.c index 52be0f88b..7d0f8d526 100644 --- a/rclpy/src/rclpy_common/src/handle.c +++ b/rclpy/src/rclpy_common/src/handle.c @@ -76,6 +76,16 @@ _rclpy_handle_add_dependency(rclpy_handle_t * dependent, rclpy_handle_t * depend dependency->ref_count++; } +/// Increments the reference count of a handle. +void +_rclpy_handle_inc_ref(rclpy_handle_t * handle) +{ + if (!handle) { + return; + } + ++handle->ref_count; +} + /// Decrements the reference count of a handle. /** * The reference count of `handle` is decremented. diff --git a/rclpy/test/test_destruction.py b/rclpy/test/test_destruction.py index 3113a84dd..e8737f0fb 100644 --- a/rclpy/test/test_destruction.py +++ b/rclpy/test/test_destruction.py @@ -189,14 +189,10 @@ def test_destroy_client_asap(): with client.handle: pass - with client.handle: - node.destroy_client(client) - # handle valid because it's still being used - with client.handle: - pass + node.destroy_client(client) with pytest.raises(InvalidHandle): - # handle invalid because it was destroyed when no one was using it + # handle invalid because it was destroyed with client.handle: pass finally: diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index bba94f341..353493923 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -42,7 +42,7 @@ def __init__(self, node): super().__init__(ReentrantCallbackGroup()) with node.handle as node_capsule: - self.client = _rclpy.rclpy_create_client( + self.client = _rclpy.Client( node_capsule, EmptySrv, 'test_client', QoSProfile(depth=10).get_c_qos_profile()) self.client_index = None self.client_is_ready = False @@ -60,7 +60,7 @@ def take_data(self): """Take stuff from lower level so the wait set doesn't immediately wake again.""" if self.client_is_ready: self.client_is_ready = False - return _rclpy.rclpy_take_response(self.client, EmptySrv.Response) + return self.client.take_response(EmptySrv.Response) return None async def execute(self, taken_data): @@ -76,7 +76,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.client_index = _rclpy.rclpy_wait_set_add_entity('client', wait_set, self.client) + self.client_index = _rclpy.rclpy_wait_set_add_client(wait_set, self.client) class ServerWaitable(Waitable): @@ -317,11 +317,11 @@ def test_waitable_with_client(self): server = self.node.create_service(EmptySrv, 'test_client', lambda req, resp: resp) - while not _rclpy.rclpy_service_server_is_available(self.waitable.client): + while not self.waitable.client.service_server_is_available(): time.sleep(0.1) thr = self.start_spin_thread(self.waitable) - _rclpy.rclpy_send_request(self.waitable.client, EmptySrv.Request()) + self.waitable.client.send_request(EmptySrv.Request()) thr.join() assert self.waitable.future.done()