diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index c1798b5cb..7d1f80c84 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -153,6 +153,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED src/rclpy/_rclpy_logging.cpp src/rclpy/_rclpy_pybind11.cpp src/rclpy/_rclpy_pycapsule.cpp + src/rclpy/action_client.cpp src/rclpy/action_goal_handle.cpp src/rclpy/client.cpp src/rclpy/clock.cpp diff --git a/rclpy/rclpy/action/client.py b/rclpy/rclpy/action/client.py index 0532ea24b..22ca2d79f 100644 --- a/rclpy/rclpy/action/client.py +++ b/rclpy/rclpy/action/client.py @@ -150,7 +150,7 @@ def __init__( self._action_type = action_type self._action_name = action_name with node.handle as node_capsule: - self._client_handle = _rclpy.rclpy_action_create_client( + self._client_handle = _rclpy.ActionClient( node_capsule, action_type, action_name, @@ -220,9 +220,7 @@ def _remove_pending_result_request(self, future): # Start Waitable API def is_ready(self, wait_set): """Return True if one or more entities are ready in the wait set.""" - ready_entities = _rclpy.rclpy_action_wait_set_is_ready( - self._client_handle, - wait_set) + ready_entities = self._client_handle.is_ready(wait_set) self._is_feedback_ready = ready_entities[0] self._is_status_ready = ready_entities[1] self._is_goal_response_ready = ready_entities[2] @@ -234,36 +232,36 @@ def take_data(self): """Take stuff from lower level so the wait set doesn't immediately wake again.""" data = {} if self._is_goal_response_ready: - taken_data = _rclpy.rclpy_action_take_goal_response( - self._client_handle, self._action_type.Impl.SendGoalService.Response) + taken_data = self._client_handle.take_goal_response( + self._action_type.Impl.SendGoalService.Response) # If take fails, then we get (None, None) if all(taken_data): data['goal'] = taken_data if self._is_cancel_response_ready: - taken_data = _rclpy.rclpy_action_take_cancel_response( - self._client_handle, self._action_type.Impl.CancelGoalService.Response) + taken_data = self._client_handle.take_cancel_response( + self._action_type.Impl.CancelGoalService.Response) # If take fails, then we get (None, None) if all(taken_data): data['cancel'] = taken_data if self._is_result_response_ready: - taken_data = _rclpy.rclpy_action_take_result_response( - self._client_handle, self._action_type.Impl.GetResultService.Response) + taken_data = self._client_handle.take_result_response( + self._action_type.Impl.GetResultService.Response) # If take fails, then we get (None, None) if all(taken_data): data['result'] = taken_data if self._is_feedback_ready: - taken_data = _rclpy.rclpy_action_take_feedback( - self._client_handle, self._action_type.Impl.FeedbackMessage) + taken_data = self._client_handle.take_feedback( + self._action_type.Impl.FeedbackMessage) # If take fails, then we get None if taken_data is not None: data['feedback'] = taken_data if self._is_status_ready: - taken_data = _rclpy.rclpy_action_take_status( - self._client_handle, self._action_type.Impl.GoalStatusMessage) + taken_data = self._client_handle.take_status( + self._action_type.Impl.GoalStatusMessage) # If take fails, then we get None if taken_data is not None: data['status'] = taken_data @@ -347,12 +345,12 @@ async def execute(self, taken_data): def get_num_entities(self): """Return number of each type of entity used in the wait set.""" - num_entities = _rclpy.rclpy_action_wait_set_get_num_entities(self._client_handle) + num_entities = self._client_handle.get_num_entities() return NumberOfEntities(*num_entities) def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - _rclpy.rclpy_action_wait_set_add(self._client_handle, wait_set) + self._client_handle.add_to_waitset(wait_set) # End Waitable API def send_goal(self, goal, **kwargs): @@ -423,8 +421,7 @@ def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None): request = self._action_type.Impl.SendGoalService.Request() request.goal_id = self._generate_random_uuid() if goal_uuid is None else goal_uuid request.goal = goal - sequence_number = _rclpy.rclpy_action_send_goal_request( - self._client_handle, request) + sequence_number = self._client_handle.send_goal_request(request) if sequence_number in self._pending_goal_requests: raise RuntimeError( 'Sequence ({}) conflicts with pending goal request'.format(sequence_number)) @@ -482,9 +479,7 @@ def _cancel_goal_async(self, goal_handle): cancel_request = CancelGoal.Request() cancel_request.goal_info.goal_id = goal_handle.goal_id - sequence_number = _rclpy.rclpy_action_send_cancel_request( - self._client_handle, - cancel_request) + sequence_number = self._client_handle.send_cancel_request(cancel_request) if sequence_number in self._pending_cancel_requests: raise RuntimeError( 'Sequence ({}) conflicts with pending cancel request'.format(sequence_number)) @@ -536,9 +531,7 @@ def _get_result_async(self, goal_handle): result_request = self._action_type.Impl.GetResultService.Request() result_request.goal_id = goal_handle.goal_id - sequence_number = _rclpy.rclpy_action_send_result_request( - self._client_handle, - result_request) + sequence_number = self._client_handle.send_result_request(result_request) if sequence_number in self._pending_result_requests: raise RuntimeError( 'Sequence ({}) conflicts with pending result request'.format(sequence_number)) @@ -557,10 +550,8 @@ def server_is_ready(self): :return: True if an action server is ready, False otherwise. """ - with self._node.handle as node_capsule: - return _rclpy.rclpy_action_server_is_available( - node_capsule, - self._client_handle) + with self._node.handle: + return self._client_handle.is_action_server_available() def wait_for_server(self, timeout_sec=None): """ @@ -587,8 +578,8 @@ def destroy(self): """Destroy the underlying action client handle.""" if self._client_handle is None: return - with self._node.handle as node_capsule: - _rclpy.rclpy_action_destroy_entity(self._client_handle, node_capsule) + with self._node.handle: + self._client_handle.destroy_when_not_in_use() self._node.remove_waitable(self) self._client_handle = None diff --git a/rclpy/src/rclpy/_rclpy_action.cpp b/rclpy/src/rclpy/_rclpy_action.cpp index cb239147d..2f55d1856 100644 --- a/rclpy/src/rclpy/_rclpy_action.cpp +++ b/rclpy/src/rclpy/_rclpy_action.cpp @@ -26,8 +26,9 @@ #include "rclpy_common/common.h" #include "rclpy_common/handle.h" -#include "clock.hpp" +#include "action_client.hpp" #include "action_goal_handle.hpp" +#include "clock.hpp" namespace py = pybind11; @@ -66,11 +67,7 @@ rclpy_action_destroy_entity(py::capsule pyentity, py::capsule pynode) } rcl_ret_t ret; - if (0 == std::strcmp("rcl_action_client_t", pyentity.name())) { - auto action_client = get_pointer(pyentity, "rcl_action_client_t"); - ret = rcl_action_client_fini(action_client, node); - PyMem_Free(action_client); - } else if (0 == std::strcmp("rcl_action_server_t", pyentity.name())) { + if (0 == std::strcmp("rcl_action_server_t", pyentity.name())) { auto action_server = get_pointer(pyentity, "rcl_action_server_t"); ret = rcl_action_server_fini(action_server, node); PyMem_Free(action_server); @@ -127,10 +124,7 @@ rclpy_action_wait_set_add(py::capsule pyentity, py::capsule pywait_set) auto wait_set = get_pointer(pywait_set, "rcl_wait_set_t"); rcl_ret_t ret; - if (0 == strcmp(pyentity.name(), "rcl_action_client_t")) { - auto action_client = get_pointer(pyentity, "rcl_action_client_t"); - ret = rcl_action_wait_set_add_action_client(wait_set, action_client, NULL, NULL); - } else if (0 == strcmp(pyentity.name(), "rcl_action_server_t")) { + if (0 == strcmp(pyentity.name(), "rcl_action_server_t")) { auto action_server = get_pointer(pyentity, "rcl_action_server_t"); ret = rcl_action_wait_set_add_action_server(wait_set, action_server, NULL); } else { @@ -168,17 +162,7 @@ rclpy_action_wait_set_get_num_entities(py::capsule pyentity) size_t num_services = 0u; rcl_ret_t ret; - if (0 == strcmp(pyentity.name(), "rcl_action_client_t")) { - auto action_client = get_pointer(pyentity, "rcl_action_client_t"); - - ret = rcl_action_client_wait_set_get_num_entities( - action_client, - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); - } else if (0 == strcmp(pyentity.name(), "rcl_action_server_t")) { + if (0 == strcmp(pyentity.name(), "rcl_action_server_t")) { auto action_server = get_pointer(pyentity, "rcl_action_server_t"); ret = rcl_action_server_wait_set_get_num_entities( @@ -503,22 +487,6 @@ rclpy_action_server_is_available(py::capsule pynode, py::capsule pyaction_client return is_available; } -#define SEND_SERVICE_REQUEST(Type) \ - auto action_client = get_pointer(pyaction_client, "rcl_action_client_t"); \ - destroy_ros_message_signature * destroy_ros_message = NULL; \ - void * raw_ros_request = rclpy_convert_from_py(pyrequest.ptr(), &destroy_ros_message); \ - if (!raw_ros_request) { \ - throw py::error_already_set(); \ - } \ - int64_t sequence_number; \ - rcl_ret_t ret = rcl_action_send_ ## Type ## _request( \ - action_client, raw_ros_request, &sequence_number); \ - destroy_ros_message(raw_ros_request); \ - if (ret != RCL_RET_OK) { \ - throw rclpy::RCLError("Failed to send " #Type " request"); \ - } \ - return sequence_number; - #define SEND_SERVICE_RESPONSE(Type) \ auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); \ destroy_ros_message_signature * destroy_ros_message = NULL; \ @@ -559,50 +527,6 @@ rclpy_action_server_is_available(py::capsule pynode, py::capsule pyaction_client rclpy_convert_to_py(taken_msg_ptr.get(), pymsg_type.ptr())); \ return pytuple; -#define TAKE_SERVICE_RESPONSE(Type) \ - auto action_client = get_pointer(pyaction_client, "rcl_action_client_t"); \ - destroy_ros_message_signature * destroy_ros_message = NULL; \ - /* taken_msg is always destroyed in this function */ \ - void * taken_msg = rclpy_create_from_py(pymsg_type.ptr(), &destroy_ros_message); \ - if (!taken_msg) { \ - throw py::error_already_set(); \ - } \ - auto taken_msg_ptr = \ - std::unique_ptr(taken_msg, destroy_ros_message); \ - rmw_request_id_t header; \ - rcl_ret_t ret = rcl_action_take_ ## Type ## _response(action_client, &header, taken_msg); \ - int64_t sequence = header.sequence_number; \ - /* Create the tuple to return */ \ - py::tuple pytuple(2); \ - if (ret == RCL_RET_ACTION_CLIENT_TAKE_FAILED || ret == RCL_RET_ACTION_SERVER_TAKE_FAILED) { \ - pytuple[0] = py::none(); \ - pytuple[1] = py::none(); \ - return pytuple; \ - } else if (ret != RCL_RET_OK) { \ - throw rclpy::RCLError("Failed to take " #Type); \ - } \ - pytuple[0] = py::int_(sequence); \ - pytuple[1] = py::reinterpret_steal( \ - rclpy_convert_to_py(taken_msg_ptr.release(), pymsg_type.ptr())); \ - return pytuple; \ - - -/// Send an action goal request. -/** - * Raises AttributeError if there is an issue parsing the pygoal_request. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pygoal_request The request message to send. - * \return sequence_number PyLong object representing the index of the sent request, or - * \return NULL if there is a failure. - */ -int64_t -rclpy_action_send_goal_request(py::capsule pyaction_client, py::object pyrequest) -{ - SEND_SERVICE_REQUEST(goal) -} - /// Take an action goal request. /** * Raises AttributeError if there is an issue parsing the pygoal_request_type. @@ -639,39 +563,6 @@ rclpy_action_send_goal_response( SEND_SERVICE_RESPONSE(goal) } -/// Take an action goal response. -/** - * Raises AttributeError if there is an issue parsing the pygoal_response_type. - * Raises RuntimeError if the underlying rcl library returns an error when taking the response. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pygoal_response_type An instance of the response message type to take. - * \return 2-tuple (sequence number, received response), or - * \return 2-tuple (None, None) if there is no response, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_goal_response(py::capsule pyaction_client, py::object pymsg_type) -{ - TAKE_SERVICE_RESPONSE(goal) -} - -/// Send an action result request. -/** - * Raises AttributeError if there is an issue parsing the pyresult_request. - * Raises RuntimeError if the underlying rcl library returns an error when sending the request. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pyresult_request The request message to send. - * \return sequence_number PyLong object representing the index of the sent request, or - * \return NULL if there is a failure. - */ -int64_t -rclpy_action_send_result_request(py::capsule pyaction_client, py::object pyrequest) -{ - SEND_SERVICE_REQUEST(result); -} - /// Take an action result request. /** * Raises AttributeError if there is an issue parsing the pyresult_request_type. @@ -708,39 +599,6 @@ rclpy_action_send_result_response( SEND_SERVICE_RESPONSE(result) } -/// Take an action result response. -/** - * Raises AttributeError if there is an issue parsing the pyresult_response_type. - * Raises RuntimeError if the underlying rcl library returns an error when taking the response. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pyresult_response_type An instance of the response message type to take. - * \return 2-tuple (sequence number, received response), or - * \return 2-tuple (None, None) if there is no response, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_result_response(py::capsule pyaction_client, py::object pymsg_type) -{ - TAKE_SERVICE_RESPONSE(result); -} - -/// Send an action cancel request. -/** - * Raises AttributeError if there is an issue parsing the pycancel_request. - * Raises RuntimeError if the underlying rcl library returns an error when sending the request. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pycancel_request The request message to send. - * \return sequence_number PyLong object representing the index of the sent request, or - * \return NULL if there is a failure. - */ -int64_t -rclpy_action_send_cancel_request(py::capsule pyaction_client, py::object pyrequest) -{ - SEND_SERVICE_REQUEST(cancel) -} - /// Take an action cancel request. /** * Raises AttributeError if there is an issue parsing the pycancel_request_type. @@ -777,41 +635,6 @@ rclpy_action_send_cancel_response( SEND_SERVICE_RESPONSE(cancel) } -/// Take an action cancel response. -/** - * Raises AttributeError if there is an issue parsing the pycancel_response_type. - * Raises RuntimeError if the underlying rcl library returns an error when taking the response. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pycancel_response_type An instance of the response message type to take. - * \return 2-tuple (sequence number, received response), or - * \return 2-tuple (None, None) if there is no response, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_cancel_response(py::capsule pyaction_client, py::object pymsg_type) -{ - TAKE_SERVICE_RESPONSE(cancel) -} - -#define TAKE_MESSAGE(Type) \ - auto action_client = get_pointer(pyaction_client, "rcl_action_client_t"); \ - destroy_ros_message_signature * destroy_ros_message = NULL; \ - void * taken_msg = rclpy_create_from_py(pymsg_type.ptr(), &destroy_ros_message); \ - if (!taken_msg) { \ - throw py::error_already_set(); \ - } \ - auto taken_msg_ptr = std::unique_ptr( \ - taken_msg, destroy_ros_message); \ - rcl_ret_t ret = rcl_action_take_ ## Type(action_client, taken_msg); \ - if (ret != RCL_RET_OK) { \ - if (ret == RCL_RET_ACTION_CLIENT_TAKE_FAILED) { \ - /* if take failed, just do nothing */ \ - return py::none(); \ - } \ - throw rclpy::RCLError("Failed to take " #Type " with an action client"); \ - } \ - return py::reinterpret_steal(rclpy_convert_to_py(taken_msg, pymsg_type.ptr())); /// Publish a feedback message from a given action server. /** @@ -839,24 +662,6 @@ rclpy_action_publish_feedback(py::capsule pyaction_server, py::object pymsg) } } -/// Take a feedback message from a given action client. -/** - * Raises AttributeError if there is an issue parsing the pyfeedback_type. - * Raises RuntimeError on failure while taking a feedback message. Note, this does not include - * the case where there are no messages available. - * - * \param[in] pyaction_client Capsule pointing to the action client to process the message. - * \param[in] pyfeedback_type Instance of the feedback message type to take. - * \return Python message with all fields populated with received message, or - * \return None if there is nothing to take, or - * \return NULL if there is a failure. - */ -py::object -rclpy_action_take_feedback(py::capsule pyaction_client, py::object pymsg_type) -{ - TAKE_MESSAGE(feedback) -} - /// Publish a status message from a given action server. /** * Raises RuntimeError on failure while publishing a status message. @@ -882,24 +687,6 @@ rclpy_action_publish_status(py::capsule pyaction_server) } } -/// Take a status message from a given action client. -/** - * Raises AttributeError if there is an issue parsing the pystatus_type. - * Raises RuntimeError on failure while taking a status message. Note, this does not include - * the case where there are no messages available. - * - * \param[in] pyaction_client Capsule pointing to the action client to process the message. - * \param[in] pystatus_type Instance of the status message type to take. - * \return Python message with all fields populated with received message, or - * \return None if there is nothing to take, or - * \return NULL if there is a failure. - */ -py::object -rclpy_action_take_status(py::capsule pyaction_client, py::object pymsg_type) -{ - TAKE_MESSAGE(status) -} - rclpy::ActionGoalHandle rclpy_action_accept_new_goal(py::capsule pyaction_server, py::object pygoal_info_msg) { @@ -1120,63 +907,36 @@ define_action_api(py::module m) m.def( "rclpy_action_wait_set_is_ready", &rclpy_action_wait_set_is_ready, "Check if an action entity has any sub-entities ready in a wait set."); - m.def( - "rclpy_action_create_client", &rclpy_action_create_client, - "Create an action client."); m.def( "rclpy_action_create_server", &rclpy_action_create_server, "Create an action server."); m.def( "rclpy_action_server_is_available", &rclpy_action_server_is_available, "Check if an action server is available for a given client."); - m.def( - "rclpy_action_send_goal_request", &rclpy_action_send_goal_request, - "Send a goal request."); m.def( "rclpy_action_take_goal_request", &rclpy_action_take_goal_request, "Take a goal request."); m.def( "rclpy_action_send_goal_response", &rclpy_action_send_goal_response, "Send a goal response."); - m.def( - "rclpy_action_take_goal_response", &rclpy_action_take_goal_response, - "Take a goal response."); - m.def( - "rclpy_action_send_result_request", &rclpy_action_send_result_request, - "Send a result request."); m.def( "rclpy_action_take_result_request", &rclpy_action_take_result_request, "Take a result request."); m.def( "rclpy_action_send_result_response", &rclpy_action_send_result_response, "Send a result response."); - m.def( - "rclpy_action_take_result_response", &rclpy_action_take_result_response, - "Take a result response."); - m.def( - "rclpy_action_send_cancel_request", &rclpy_action_send_cancel_request, - "Send a cancel request."); m.def( "rclpy_action_take_cancel_request", &rclpy_action_take_cancel_request, "Take a cancel request."); m.def( "rclpy_action_send_cancel_response", &rclpy_action_send_cancel_response, "Send a cancel response."); - m.def( - "rclpy_action_take_cancel_response", &rclpy_action_take_cancel_response, - "Take a cancel response."); m.def( "rclpy_action_publish_feedback", &rclpy_action_publish_feedback, "Publish a feedback message."); - m.def( - "rclpy_action_take_feedback", &rclpy_action_take_feedback, - "Take a feedback message."); m.def( "rclpy_action_publish_status", &rclpy_action_publish_status, "Publish a status message."); - m.def( - "rclpy_action_take_status", &rclpy_action_take_status, - "Take a status message."); m.def( "rclpy_action_accept_new_goal", &rclpy_action_accept_new_goal, "Accept a new goal using an action server."); @@ -1206,5 +966,7 @@ define_action_api(py::module m) m.def( "rclpy_action_get_names_and_types", &rclpy_action_get_names_and_types, "Get action names and types."); + + rclpy::define_action_client(m); } } // namespace rclpy diff --git a/rclpy/src/rclpy/action_client.cpp b/rclpy/src/rclpy/action_client.cpp new file mode 100644 index 000000000..68f78d421 --- /dev/null +++ b/rclpy/src/rclpy/action_client.cpp @@ -0,0 +1,330 @@ +// 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 pybind11 before rclpy_common/handle.h includes Python.h +#include + +#include + +#include +#include + +#include "rclpy_common/common.h" +#include "rclpy_common/exceptions.hpp" +#include "rclpy_common/handle.h" + +#include "action_client.hpp" +#include "utils.hpp" + +namespace rclpy +{ + +void +ActionClient::destroy() +{ + rcl_action_client_.reset(); + node_handle_.reset(); +} + +ActionClient::ActionClient( + py::capsule pynode, + py::object pyaction_type, + const char * action_name, + const rmw_qos_profile_t & goal_service_qos, + const rmw_qos_profile_t & result_service_qos, + const rmw_qos_profile_t & cancel_service_qos, + const rmw_qos_profile_t & feedback_topic_qos, + const rmw_qos_profile_t & status_topic_qos) +: node_handle_(std::make_shared(pynode)) +{ + auto node = node_handle_->cast("rcl_node_t"); + + rosidl_action_type_support_t * ts = + static_cast(rclpy_common_get_type_support( + pyaction_type.ptr())); + if (!ts) { + throw py::error_already_set(); + } + + rcl_action_client_options_t action_client_ops = rcl_action_client_get_default_options(); + + action_client_ops.goal_service_qos = goal_service_qos; + action_client_ops.result_service_qos = result_service_qos; + action_client_ops.cancel_service_qos = cancel_service_qos; + action_client_ops.feedback_topic_qos = feedback_topic_qos; + action_client_ops.status_topic_qos = status_topic_qos; + + rcl_action_client_ = std::shared_ptr( + new rcl_action_client_t, + [this](rcl_action_client_t * action_client) + { + auto node = node_handle_->cast_or_warn("rcl_node_t"); + + rcl_ret_t ret = rcl_action_client_fini(action_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 publisher: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete action_client; + }); + + *rcl_action_client_ = rcl_action_get_zero_initialized_client(); + + rcl_ret_t ret = rcl_action_client_init( + rcl_action_client_.get(), + node, + ts, + action_name, + &action_client_ops); + if (RCL_RET_ACTION_NAME_INVALID == ret) { + std::string error_text{"Failed to create action client due to invalid topic name '"}; + error_text += action_name; + error_text += "' : "; + error_text += rcl_get_error_string().str; + rcl_reset_error(); + throw py::value_error(error_text); + } + if (RCL_RET_OK != ret) { + std::string error_text{"Failed to create action client: "}; + error_text += rcl_get_error_string().str; + rcl_reset_error(); + throw py::value_error(error_text); + } +} + +#define TAKE_SERVICE_RESPONSE(Type) \ + /* taken_msg is always destroyed in this function */ \ + auto taken_msg = create_from_py(pymsg_type); \ + rmw_request_id_t header; \ + rcl_ret_t ret = rcl_action_take_ ## Type ## _response( \ + rcl_action_client_.get(), &header, taken_msg.get()); \ + int64_t sequence = header.sequence_number; \ + /* Create the tuple to return */ \ + if (RCL_RET_ACTION_CLIENT_TAKE_FAILED == ret || RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { \ + return py::make_tuple(py::none(), py::none()); \ + } else if (RCL_RET_OK != ret) { \ + throw rclpy::RCLError("Failed to take " #Type); \ + } \ + return py::make_tuple(sequence, convert_to_py(taken_msg.get(), pymsg_type)); \ + +py::tuple +ActionClient::take_goal_response(py::object pymsg_type) +{ + TAKE_SERVICE_RESPONSE(goal) +} + +#define SEND_SERVICE_REQUEST(Type) \ + auto ros_request = convert_from_py(pyrequest); \ + int64_t sequence_number; \ + rcl_ret_t ret = rcl_action_send_ ## Type ## _request( \ + rcl_action_client_.get(), ros_request.get(), &sequence_number); \ + if (RCL_RET_OK != ret) { \ + throw rclpy::RCLError("Failed to send " #Type " request"); \ + } \ + return sequence_number; + +int64_t +ActionClient::send_result_request(py::object pyrequest) +{ + SEND_SERVICE_REQUEST(result); +} + +py::tuple +ActionClient::take_cancel_response(py::object pymsg_type) +{ + TAKE_SERVICE_RESPONSE(cancel) +} + +#define TAKE_MESSAGE(Type) \ + auto taken_msg = create_from_py(pymsg_type); \ + rcl_ret_t ret = rcl_action_take_ ## Type(rcl_action_client_.get(), taken_msg.get()); \ + if (RCL_RET_OK != ret) { \ + if (RCL_RET_ACTION_CLIENT_TAKE_FAILED == ret) { \ + /* if take failed, just do nothing */ \ + return py::none(); \ + } \ + throw rclpy::RCLError("Failed to take " #Type " with an action client"); \ + } \ + return convert_to_py(taken_msg.get(), pymsg_type); + +py::object +ActionClient::take_feedback(py::object pymsg_type) +{ + TAKE_MESSAGE(feedback) +} + +py::object +ActionClient::take_status(py::object pymsg_type) +{ + TAKE_MESSAGE(status) +} + +int64_t +ActionClient::send_cancel_request(py::object pyrequest) +{ + SEND_SERVICE_REQUEST(cancel) +} + +int64_t +ActionClient::send_goal_request(py::object pyrequest) +{ + SEND_SERVICE_REQUEST(goal) +} + +py::tuple +ActionClient::take_result_response(py::object pymsg_type) +{ + TAKE_SERVICE_RESPONSE(result); +} + +py::tuple +ActionClient::get_num_entities() +{ + size_t num_subscriptions = 0u; + size_t num_guard_conditions = 0u; + size_t num_timers = 0u; + size_t num_clients = 0u; + size_t num_services = 0u; + + rcl_ret_t ret; + ret = rcl_action_client_wait_set_get_num_entities( + rcl_action_client_.get(), + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + if (RCL_RET_OK != ret) { + std::string error_text{"Failed to get number of entities for 'rcl_action_client_t'"}; + throw rclpy::RCLError(error_text); + } + + return py::make_tuple( + num_subscriptions, num_guard_conditions, num_timers, + num_clients, num_services); +} + +bool +ActionClient::is_action_server_available() +{ + auto node = node_handle_->cast("rcl_node_t"); + + bool is_available = false; + rcl_ret_t ret = rcl_action_server_is_available( + node, rcl_action_client_.get(), &is_available); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to check if action server is available"); + } + return is_available; +} + +void +ActionClient::add_to_waitset(py::capsule pywait_set) +{ + auto wait_set = static_cast(pywait_set); + + rcl_ret_t ret = rcl_action_wait_set_add_action_client( + wait_set, rcl_action_client_.get(), NULL, NULL); + if (RCL_RET_OK != ret) { + std::string error_text{"Failed to add 'rcl_action_client_t' to wait set"}; + throw rclpy::RCLError(error_text); + } +} + +py::tuple +ActionClient::is_ready(py::capsule pywait_set) +{ + auto wait_set = static_cast(pywait_set); + + bool is_feedback_ready = false; + bool is_status_ready = false; + bool is_goal_response_ready = false; + bool is_cancel_response_ready = false; + bool is_result_response_ready = false; + rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( + wait_set, + rcl_action_client_.get(), + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get number of ready entities for action client"); + } + + py::tuple result_tuple(5); + result_tuple[0] = py::bool_(is_feedback_ready); + result_tuple[1] = py::bool_(is_status_ready); + result_tuple[2] = py::bool_(is_goal_response_ready); + result_tuple[3] = py::bool_(is_cancel_response_ready); + result_tuple[4] = py::bool_(is_result_response_ready); + return result_tuple; +} + +void +define_action_client(py::object module) +{ + py::class_>(module, "ActionClient") + .def( + py::init()) + .def_property_readonly( + "pointer", [](const ActionClient & action_client) { + return reinterpret_cast(action_client.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "take_goal_response", &ActionClient::take_goal_response, + "Take an action goal response.") + .def( + "send_result_request", &ActionClient::send_result_request, + "Send an action result request.") + .def( + "take_cancel_response", &ActionClient::take_cancel_response, + "Take an action cancel response.") + .def( + "take_feedback", &ActionClient::take_feedback, + "Take a feedback message from a given action client.") + .def( + "send_cancel_request", &ActionClient::send_cancel_request, + "Send an action cancel request.") + .def( + "send_goal_request", &ActionClient::send_goal_request, + "Send an action goal request.") + .def( + "take_result_response", &ActionClient::take_result_response, + "Take an action result response.") + .def( + "get_num_entities", &ActionClient::get_num_entities, + "Get the number of wait set entities that make up an action entity.") + .def( + "is_action_server_available", &ActionClient::is_action_server_available, + "Check if an action server is available for the given action client.") + .def( + "add_to_waitset", &ActionClient::add_to_waitset, + "Add an action entity to a wait set.") + .def( + "is_ready", &ActionClient::is_ready, + "Check if an action entity has any ready wait set entities.") + .def( + "take_status", &ActionClient::take_status, + "Take an action status response."); +} +} // namespace rclpy diff --git a/rclpy/src/rclpy/action_client.hpp b/rclpy/src/rclpy/action_client.hpp new file mode 100644 index 000000000..7ce59659f --- /dev/null +++ b/rclpy/src/rclpy/action_client.hpp @@ -0,0 +1,238 @@ +// 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__ACTION_CLIENT_HPP_ +#define RCLPY__ACTION_CLIENT_HPP_ + +#include + +#include + +#include + +#include "destroyable.hpp" +#include "handle.hpp" + +namespace py = pybind11; + +namespace rclpy +{ +/** + * This class will create an action client for the given action name. + * This client will use the typesupport defined in the action module + * provided as pyaction_type to send messages. + */ +class ActionClient : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create an action client. + /* + * Raises ValueError if action name is invalid. + * Raises RuntimeError if the action client could not be created. + * + * \param[in] pynode Capsule pointing to the node to add the action client to. + * \param[in] pyaction_type Action module associated with the action client. + * \param[in] pyaction_name Python object containing the action name. + * \param[in] goal_service_qos rmw_qos_profile_t object for the goal service. + * \param[in] result_service_qos rmw_qos_profile_t object for the result service. + * \param[in] cancel_service_qos rmw_qos_profile_t object for the cancel service. + * \param[in] feedback_qos rmw_qos_profile_t object for the feedback subscriber. + * \param[in] status_qos rmw_qos_profile_t object for the status subscriber. + */ + ActionClient( + py::capsule pynode, + py::object pyaction_type, + const char * action_name, + const rmw_qos_profile_t & goal_service_qos, + const rmw_qos_profile_t & result_service_qos, + const rmw_qos_profile_t & cancel_service_qos, + const rmw_qos_profile_t & feedback_topic_qos, + const rmw_qos_profile_t & status_topic_qos); + + /// Take an action goal response. + /** + * Raises AttributeError if there is an issue parsing the pygoal_response_type. + * Raises RuntimeError if the underlying rcl library returns an error when taking the response. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pygoal_response_type An instance of the response message type to take. + * \return 2-tuple (sequence number, received response), or + * \return 2-tuple (None, None) if there is no response, or + * \return (None, None) if there is a failure in the rcl API call. + */ + py::tuple + take_goal_response(py::object pymsg_type); + + /// Send an action result request. + /** + * Raises AttributeError if there is an issue parsing the pyresult_request. + * Raises RuntimeError if the underlying rcl library returns an error when sending the request. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pyresult_request The request message to send. + * \return sequence_number the index of the sent request + */ + int64_t + send_result_request(py::object pyrequest); + + /// Take an action cancel response. + /** + * Raises AttributeError if there is an issue parsing the pycancel_response_type. + * Raises RuntimeError if the underlying rcl library returns an error when taking the response. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pycancel_response_type An instance of the response message type to take. + * \return 2-tuple (sequence number, received response), or + * \return 2-tuple (None, None) if there is no response, or + * \return (None, None) if there is a failure in the rcl API call. + */ + py::tuple + take_cancel_response(py::object pymsg_type); + + /// Send an action cancel request. + /** + * Raises AttributeError if there is an issue parsing the pycancel_request. + * Raises RuntimeError if the underlying rcl library returns an error when sending the request. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pycancel_request The request message to send. + * \return sequence_number the index of the sent request + */ + int64_t + send_cancel_request(py::object pyrequest); + + /// Take a feedback message from a given action client. + /** + * Raises AttributeError if there is an issue parsing the pyfeedback_type. + * Raises RuntimeError on failure while taking a feedback message. Note, this does not include + * the case where there are no messages available. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pyfeedback_type Instance of the feedback message type to take. + * \return Python message with all fields populated with received message, or + * \return None if there is nothing to take, or + * \return None if there is a failure in the rcl API call. + */ + py::object + take_feedback(py::object pymsg_type); + + /// Take a status message from a given action client. + /** + * Raises AttributeError if there is an issue parsing the pystatus_type. + * Raises RuntimeError on failure while taking a status message. Note, this does not include + * the case where there are no messages available. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pystatus_type Instance of the status message type to take. + * \return Python message with all fields populated with received message, or + * \return None if there is nothing to take, or + * \return None if there is a failure in the rcl API call. + */ + py::object + take_status(py::object pymsg_type); + + /// Send an action goal request. + /** + * Raises AttributeError if there is an issue parsing the pygoal_request. + * Raises RuntimeError on failure. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pygoal_request The request message to send. + * \return sequence_number PyLong object representing the index of the sent request + */ + int64_t + send_goal_request(py::object pyrequest); + + /// Take an action result response. + /** + * Raises AttributeError if there is an issue parsing the pyresult_response_type. + * Raises RuntimeError if the underlying rcl library returns an error when taking the response. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pyresult_response_type An instance of the response message type to take. + * \return 2-tuple (sequence number, received response), or + * \return 2-tuple (None, None) if there is no response, or + * \return (None, None) if there is a failure in the rcl API call. + */ + py::tuple + take_result_response(py::object pymsg_type); + + /// Get the number of wait set entities that make up an action entity. + /** + * \return Tuple containing the number of wait set entities: + * (num_subscriptions, + * num_guard_conditions, + * num_timers, + * num_clients, + * num_services) + */ + py::tuple + get_num_entities(); + + /// Check if an action server is available for the given action client. + /** + * Raises RuntimeError on failure. + * + * \return True if an action server is available, False otherwise. + */ + bool + is_action_server_available(); + + /// Check if an action entity has any ready wait set entities. + /** + * This must be called after waiting on the wait set. + * Raises RuntimeError on failure. + * + * \param[in] pywait_set Capsule pointing to the wait set structure. + * \return A tuple of booleans representing the sub-entities ready: + * (is_feedback_ready, + * is_status_ready, + * is_goal_response_ready, + * is_cancel_response_ready, + * is_result_response_ready) + */ + py::tuple + is_ready(py::capsule pywait_set); + + /// Add an action entitiy to a wait set. + /** + * Raises RuntimeError on failure. + * \param[in] pywait_set Capsule pointer to an rcl_wait_set_t. + */ + void + add_to_waitset(py::capsule pywait_set); + + /// Get rcl_client_t pointer + rcl_action_client_t * + rcl_ptr() const + { + return rcl_action_client_.get(); + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + std::shared_ptr node_handle_; + std::shared_ptr rcl_action_client_; +}; +/// Define a pybind11 wrapper for an rcl_time_point_t +/** + * \param[in] module a pybind11 module to add the definition to + */ +void define_action_client(py::object module); +} // namespace rclpy + +#endif // RCLPY__ACTION_CLIENT_HPP_ diff --git a/rclpy/src/rclpy/utils.cpp b/rclpy/src/rclpy/utils.cpp index 4aca238e8..39adef07a 100644 --- a/rclpy/src/rclpy/utils.cpp +++ b/rclpy/src/rclpy/utils.cpp @@ -81,6 +81,31 @@ create_from_py(py::object pymessage) void, destroy_ros_message_function *>(message, destroy_ros_message); } +std::unique_ptr +convert_from_py(py::object pymessage) +{ + typedef bool convert_from_py_signature (PyObject *, void *); + + std::unique_ptr message = + create_from_py(pymessage); + + py::object pymetaclass = pymessage.attr("__class__"); + + auto capsule_ptr = static_cast( + pymetaclass.attr("_CONVERT_FROM_PY").cast()); + auto convert = + reinterpret_cast(capsule_ptr); + if (!convert) { + throw py::error_already_set(); + } + + if (!convert(pymessage.ptr(), message.get())) { + throw py::error_already_set(); + } + + return message; +} + py::object convert_to_py(void * message, py::object pyclass) { diff --git a/rclpy/src/rclpy/utils.hpp b/rclpy/src/rclpy/utils.hpp index de86d697e..1520005a1 100644 --- a/rclpy/src/rclpy/utils.hpp +++ b/rclpy/src/rclpy/utils.hpp @@ -49,6 +49,16 @@ convert_to_py_names_and_types(const rcl_names_and_types_t * topic_names_and_type std::unique_ptr create_from_py(py::object pyclass); +/// Convert a ROS message from a Python type to a C type. +/** + * Raises AttributeError if the Python message type is missing a required attribute. + * + * \param[in] pyclass ROS message Python type to extract data from. + * \return unique pointer with the C version of the input ROS message. + */ +std::unique_ptr +convert_from_py(py::object pyclass); + /// Convert a ROS message from a C type to a Python type. /** * Raises AttributeError if \p pyclass is missing a required attribute.