From 310ede363fbe00bcc9df75d3533745b9b7dfea9b Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 9 Apr 2021 11:28:57 +0200 Subject: [PATCH 1/4] Convert ActionServer to use C++ Classes Signed-off-by: ahcorde --- rclpy/CMakeLists.txt | 1 + rclpy/rclpy/action/server.py | 62 +--- rclpy/src/rclpy/_rclpy_action.cpp | 496 +------------------------ rclpy/src/rclpy/action_goal_handle.cpp | 29 +- rclpy/src/rclpy/action_goal_handle.hpp | 8 +- rclpy/src/rclpy/action_server.cpp | 438 ++++++++++++++++++++++ rclpy/src/rclpy/action_server.hpp | 252 +++++++++++++ 7 files changed, 724 insertions(+), 562 deletions(-) create mode 100644 rclpy/src/rclpy/action_server.cpp create mode 100644 rclpy/src/rclpy/action_server.hpp diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index c1798b5cb..23a032aa5 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -154,6 +154,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED src/rclpy/_rclpy_pybind11.cpp src/rclpy/_rclpy_pycapsule.cpp src/rclpy/action_goal_handle.cpp + src/rclpy/action_server.cpp src/rclpy/client.cpp src/rclpy/clock.cpp src/rclpy/context.cpp diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py index aa27f9160..ef9ded1d7 100644 --- a/rclpy/rclpy/action/server.py +++ b/rclpy/rclpy/action/server.py @@ -110,7 +110,7 @@ def _update_state(self, event): self._goal.update_goal_state(event) # Publish state change - _rclpy.rclpy_action_publish_status(self._action_server._handle) + self._action_server._handle.publish_status() # If it's a terminal state, then also notify the action server if not self._goal.is_active(): @@ -140,8 +140,7 @@ def publish_feedback(self, feedback): feedback_message.feedback = feedback # Publish - _rclpy.rclpy_action_publish_feedback( - self._action_server._handle, feedback_message) + self._action_server._handle.publish_feedback(feedback_message) def succeed(self): self._update_state(_rclpy.GoalEvent.SUCCEED) @@ -239,7 +238,7 @@ def __init__( self._node = node self._action_type = action_type with node.handle as node_capsule, node.get_clock().handle: - self._handle = _rclpy.rclpy_action_create_server( + self._handle = _rclpy.ActionServer( node_capsule, node.get_clock().handle, action_type, @@ -268,7 +267,7 @@ async def _execute_goal_request(self, request_header_and_message): # Check if goal ID is already being tracked by this action server with self._lock: - goal_id_exists = _rclpy.rclpy_action_server_goal_exists(self._handle, goal_info) + goal_id_exists = self._handle.goal_exists(goal_info) accepted = False if not goal_id_exists: @@ -299,11 +298,7 @@ async def _execute_goal_request(self, request_header_and_message): response_msg = self._action_type.Impl.SendGoalService.Response() response_msg.accepted = accepted response_msg.stamp = goal_info.stamp - _rclpy.rclpy_action_send_goal_response( - self._handle, - request_header, - response_msg, - ) + self._handle.send_goal_response(request_header, response_msg) if not accepted: self._node.get_logger().debug('New goal rejected: {0}'.format(goal_uuid.uuid)) @@ -349,8 +344,8 @@ async def _execute_cancel_request(self, request_header_and_message): with self._lock: # Get list of goals that are requested to be canceled - cancel_response = _rclpy.rclpy_action_process_cancel_request( - self._handle, cancel_request, self._action_type.Impl.CancelGoalService.Response) + cancel_response = self._handle.process_cancel_request( + cancel_request, self._action_type.Impl.CancelGoalService.Response) for goal_info in cancel_response.goals_canceling: goal_uuid = bytes(goal_info.goal_id.uuid) @@ -369,11 +364,7 @@ async def _execute_cancel_request(self, request_header_and_message): # Remove from response cancel_response.goals_canceling.remove(goal_info) - _rclpy.rclpy_action_send_cancel_response( - self._handle, - request_header, - cancel_response, - ) + self._handle.send_cancel_response(request_header, cancel_response) async def _execute_get_result_request(self, request_header_and_message): request_header, result_request = request_header_and_message @@ -388,11 +379,7 @@ async def _execute_get_result_request(self, request_header_and_message): 'Sending result response for unknown goal ID: {0}'.format(goal_uuid)) result_response = self._action_type.Impl.GetResultService.Response() result_response.status = GoalStatus.STATUS_UNKNOWN - _rclpy.rclpy_action_send_result_response( - self._handle, - request_header, - result_response, - ) + self._handle.send_result_response(request_header, result_response) return # There is an accepted goal matching the goal ID, register a callback to send the @@ -406,11 +393,7 @@ async def _execute_expire_goals(self, expired_goals): del self._goal_handles[goal_uuid] def _send_result_response(self, request_header, future): - _rclpy.rclpy_action_send_result_response( - self._handle, - request_header, - future.result(), - ) + self._handle.send_result_response(request_header, future.result()) @property def action_type(self): @@ -420,7 +403,7 @@ def action_type(self): def is_ready(self, wait_set): """Return True if one or more entities are ready in the wait set.""" with self._lock: - ready_entities = _rclpy.rclpy_action_wait_set_is_ready(self._handle, wait_set) + ready_entities = self._handle.is_ready(wait_set) self._is_goal_request_ready = ready_entities[0] self._is_cancel_request_ready = ready_entities[1] self._is_result_request_ready = ready_entities[2] @@ -432,8 +415,7 @@ def take_data(self): data = {} if self._is_goal_request_ready: with self._lock: - taken_data = _rclpy.rclpy_action_take_goal_request( - self._handle, + taken_data = self._handle.take_goal_request( self._action_type.Impl.SendGoalService.Request, ) # If take fails, then we get (None, None) @@ -442,8 +424,7 @@ def take_data(self): if self._is_cancel_request_ready: with self._lock: - taken_data = _rclpy.rclpy_action_take_cancel_request( - self._handle, + taken_data = self._handle.take_cancel_request( self._action_type.Impl.CancelGoalService.Request, ) # If take fails, then we get (None, None) @@ -452,8 +433,7 @@ def take_data(self): if self._is_result_request_ready: with self._lock: - taken_data = _rclpy.rclpy_action_take_result_request( - self._handle, + taken_data = self._handle.take_result_request( self._action_type.Impl.GetResultService.Request, ) # If take fails, then we get (None, None) @@ -462,10 +442,7 @@ def take_data(self): if self._is_goal_expired: with self._lock: - data['expired'] = _rclpy.rclpy_action_expire_goals( - self._handle, - len(self._goal_handles), - ) + data['expired'] = self._handle.expire_goals(len(self._goal_handles)) return data @@ -490,7 +467,7 @@ 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._handle) + num_entities = self._handle.get_num_entities() return NumberOfEntities( num_entities[0], num_entities[1], @@ -501,7 +478,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" with self._lock: - _rclpy.rclpy_action_wait_set_add(self._handle, wait_set) + self._handle.add_to_waitset(wait_set) # End Waitable API def notify_execute(self, goal_handle, execute_callback): @@ -516,7 +493,7 @@ def notify_execute(self, goal_handle, execute_callback): def notify_goal_done(self): with self._lock: - _rclpy.rclpy_action_notify_goal_done(self._handle) + self._handle.notify_goal_done() def register_handle_accepted_callback(self, handle_accepted_callback): """ @@ -604,8 +581,7 @@ def destroy(self): for goal_handle in self._goal_handles.values(): goal_handle.destroy() - with self._node.handle as node_capsule: - _rclpy.rclpy_action_destroy_entity(self._handle, node_capsule) + self._handle.destroy_when_not_in_use() self._node.remove_waitable(self) self._handle = None diff --git a/rclpy/src/rclpy/_rclpy_action.cpp b/rclpy/src/rclpy/_rclpy_action.cpp index cb239147d..f42807ea6 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_goal_handle.hpp" +#include "action_server.hpp" +#include "clock.hpp" namespace py = pybind11; @@ -70,10 +71,6 @@ rclpy_action_destroy_entity(py::capsule pyentity, py::capsule pynode) 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())) { - auto action_server = get_pointer(pyentity, "rcl_action_server_t"); - ret = rcl_action_server_fini(action_server, node); - PyMem_Free(action_server); } else { std::string entity_name = pyentity.name(); throw std::runtime_error(entity_name + " is not a known entity"); @@ -178,16 +175,6 @@ rclpy_action_wait_set_get_num_entities(py::capsule pyentity) &num_timers, &num_clients, &num_services); - } else 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( - action_server, - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); } else { std::string error_text{"Unknown entity: "}; error_text += pyentity.name(); @@ -263,29 +250,6 @@ rclpy_action_wait_set_is_ready(py::capsule pyentity, py::capsule pywait_set) result_tuple[3] = py::bool_(is_cancel_response_ready); result_tuple[4] = py::bool_(is_result_response_ready); return result_tuple; - } else if (0 == strcmp(pyentity.name(), "rcl_action_server_t")) { - auto action_server = get_pointer(pyentity, "rcl_action_server_t"); - bool is_goal_request_ready = false; - bool is_cancel_request_ready = false; - bool is_result_request_ready = false; - bool is_goal_expired = false; - rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( - wait_set, - action_server, - &is_goal_request_ready, - &is_cancel_request_ready, - &is_result_request_ready, - &is_goal_expired); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get number of ready entities for action server"); - } - - py::tuple result_tuple(4); - result_tuple[0] = py::bool_(is_goal_request_ready); - result_tuple[1] = py::bool_(is_cancel_request_ready); - result_tuple[2] = py::bool_(is_result_request_ready); - result_tuple[3] = py::bool_(is_goal_expired); - return result_tuple; } std::string error_text{"Unknown entity: "}; @@ -381,101 +345,6 @@ rclpy_action_create_client( return py::capsule(action_client.release(), "rcl_action_client_t"); } -/// Create an action server. -/** - * This function will create an action server for the given action name. - * This server will use the typesupport defined in the action module - * provided as pyaction_type to send messages over the wire. - * - * On a successful call a capsule referencing the created rcl_action_server_t structure - * is returned. - * - * Raises AttributeError if action type is invalid - * Raises ValueError if action name is invalid - * Raises RuntimeError if the action server could not be created. - * - * \remark Call rclpy_action_destroy_entity() to destroy an action server. - * \param[in] pynode Capsule pointing to the node to add the action server to. - * \param[in] pyaction_type Action module associated with the action server. - * \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. - * \return Capsule named 'rcl_action_server_t', or - * \return NULL on failure. - */ -py::capsule -rclpy_action_create_server( - py::capsule pynode, - const rclpy::Clock & rclpy_clock, - 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, - double result_timeout) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rcl_clock_t * clock = rclpy_clock.rcl_ptr(); - - 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_server_options_t action_server_ops = rcl_action_server_get_default_options(); - - action_server_ops.goal_service_qos = goal_service_qos; - action_server_ops.result_service_qos = result_service_qos; - action_server_ops.cancel_service_qos = cancel_service_qos; - action_server_ops.feedback_topic_qos = feedback_topic_qos; - action_server_ops.status_topic_qos = status_topic_qos; - action_server_ops.result_timeout.nanoseconds = (rcl_duration_value_t)RCL_S_TO_NS(result_timeout); - - auto deleter = [](rcl_action_server_t * ptr) {PyMem_Free(ptr);}; - auto action_server = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rcl_action_server_t))), - deleter); - if (!action_server) { - throw std::bad_alloc(); - } - - *action_server = rcl_action_get_zero_initialized_server(); - rcl_ret_t ret = rcl_action_server_init( - action_server.get(), - node, - clock, - ts, - action_name, - &action_server_ops); - if (ret == RCL_RET_ACTION_NAME_INVALID) { - std::string error_text{"Failed to create action server 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); - } else if (ret != RCL_RET_OK) { - std::string error_text{"Failed to create action server: "}; - error_text += rcl_get_error_string().str; - rcl_reset_error(); - throw py::value_error(error_text); - } - - return py::capsule(action_server.release(), "rcl_action_server_t"); -} - - /// Check if an action server is available for the given action client. /** * Raises RuntimeError on failure. @@ -518,47 +387,6 @@ rclpy_action_server_is_available(py::capsule pynode, py::capsule pyaction_client 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; \ - void * raw_ros_response = rclpy_convert_from_py(pyresponse.ptr(), &destroy_ros_message); \ - if (!raw_ros_response) { \ - throw py::error_already_set(); \ - } \ - rcl_ret_t ret = rcl_action_send_ ## Type ## _response(action_server, header, raw_ros_response); \ - destroy_ros_message(raw_ros_response); \ - if (ret != RCL_RET_OK) { \ - throw rclpy::RCLError("Failed to send " #Type " response"); \ - } - -#define TAKE_SERVICE_REQUEST(Type) \ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_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 ## _request(action_server, &header, taken_msg_ptr.get()); \ - /* 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] = header; \ - pytuple[1] = py::reinterpret_steal( \ - 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; \ @@ -603,42 +431,6 @@ 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. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when taking the request. - * \param[in] pygoal_request_type An instance of the type of request message to take. - * \return 2-tuple (header, received request message) where the header is an - * "rclpy.rmw_request_id_t" type, or - * \return 2-tuple (None, None) if there as no message to take, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_goal_request(py::capsule pyaction_server, py::object pymsg_type) -{ - TAKE_SERVICE_REQUEST(goal) -} - -/// Send an action goal response. -/** - * Raises AttributeError if there is an issue parsing the pygoal_response. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when sending the response. - * \param[in] header Pointer to the message header. - * \param[in] pygoal_response The response message to send. - * \return None - * \return NULL if there is a failure. - */ -void -rclpy_action_send_goal_response( - py::capsule pyaction_server, rmw_request_id_t * header, py::object pyresponse) -{ - SEND_SERVICE_RESPONSE(goal) -} - /// Take an action goal response. /** * Raises AttributeError if there is an issue parsing the pygoal_response_type. @@ -672,42 +464,6 @@ rclpy_action_send_result_request(py::capsule pyaction_client, py::object pyreque SEND_SERVICE_REQUEST(result); } -/// Take an action result request. -/** - * Raises AttributeError if there is an issue parsing the pyresult_request_type. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when taking the request. - * \param[in] pyresult_request_type An instance of the type of request message to take. - * \return 2-tuple (header, received request message) where the header is an - * "rclpy.rmw_request_id_t" type, or - * \return 2-tuple (None, None) if there as no message to take, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_result_request(py::capsule pyaction_server, py::object pymsg_type) -{ - TAKE_SERVICE_REQUEST(result) -} - -/// Send an action result response. -/** - * Raises AttributeError if there is an issue parsing the pyresult_response. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when sending the response. - * \param[in] pyheader Pointer to the message header. - * \param[in] pyresult_response The response message to send. - * \return None - * \return NULL if there is a failure. - */ -void -rclpy_action_send_result_response( - py::capsule pyaction_server, rmw_request_id_t * header, py::object pyresponse) -{ - SEND_SERVICE_RESPONSE(result) -} - /// Take an action result response. /** * Raises AttributeError if there is an issue parsing the pyresult_response_type. @@ -741,42 +497,6 @@ rclpy_action_send_cancel_request(py::capsule pyaction_client, py::object pyreque SEND_SERVICE_REQUEST(cancel) } -/// Take an action cancel request. -/** - * Raises AttributeError if there is an issue parsing the pycancel_request_type. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when taking the request. - * \param[in] pycancel_request_type An instance of the type of request message to take. - * \return 2-tuple (header, received request message) where the header is an - * "rmw_request_id_t" type, or - * \return 2-tuple (None, None) if there as no message to take, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_cancel_request(py::capsule pyaction_server, py::object pymsg_type) -{ - TAKE_SERVICE_REQUEST(cancel) -} - -/// Send an action cancel response. -/** - * Raises AttributeError if there is an issue parsing the pycancel_response. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when sending the response. - * \param[in] pyheader Pointer to the message header. - * \param[in] pycancel_response The response message to send. - * \return sequence_number PyLong object representing the index of the sent response, or - * \return NULL if there is a failure. - */ -void -rclpy_action_send_cancel_response( - py::capsule pyaction_server, rmw_request_id_t * header, py::object pyresponse) -{ - SEND_SERVICE_RESPONSE(cancel) -} - /// Take an action cancel response. /** * Raises AttributeError if there is an issue parsing the pycancel_response_type. @@ -813,32 +533,6 @@ rclpy_action_take_cancel_response(py::capsule pyaction_client, py::object pymsg_ } \ return py::reinterpret_steal(rclpy_convert_to_py(taken_msg, pymsg_type.ptr())); -/// Publish a feedback message from a given action server. -/** - * Raises AttributeError if there is an issue parsing the pyfeedback_msg. - * Raises RuntimeError on failure while publishing a feedback message. - * - * \param[in] pyaction_server Capsule pointing to the action server to publish the message. - * \param[in] pyfeedback_msg The feedback message to publish. - * \return None - */ -void -rclpy_action_publish_feedback(py::capsule pyaction_server, py::object pymsg) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - destroy_ros_message_signature * destroy_ros_message = NULL; - void * raw_ros_message = rclpy_convert_from_py(pymsg.ptr(), &destroy_ros_message); - if (!raw_ros_message) { - throw py::error_already_set(); - } - auto raw_ros_message_ptr = std::unique_ptr( - raw_ros_message, destroy_ros_message); - rcl_ret_t ret = rcl_action_publish_feedback(action_server, raw_ros_message); - if (ret != RCL_RET_OK) { - throw rclpy::RCLError("Failed to publish feedback with an action server"); - } -} - /// Take a feedback message from a given action client. /** * Raises AttributeError if there is an issue parsing the pyfeedback_type. @@ -857,31 +551,6 @@ 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. - * - * \param[in] pyaction_server Capsule pointing to the action server to publish the message. - * \return None - */ -void -rclpy_action_publish_status(py::capsule pyaction_server) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - rcl_action_goal_status_array_t status_message = - rcl_action_get_zero_initialized_goal_status_array(); - rcl_ret_t ret = rcl_action_get_goal_status_array(action_server, &status_message); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed get goal status array"); - } - - ret = rcl_action_publish_status(action_server, &status_message); - - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed publish goal status array"); - } -} - /// Take a status message from a given action client. /** * Raises AttributeError if there is an issue parsing the pystatus_type. @@ -900,121 +569,6 @@ 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) -{ - return rclpy::ActionGoalHandle(pyaction_server, pygoal_info_msg); -} - -void -rclpy_action_notify_goal_done(py::capsule pyaction_server) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - rcl_ret_t ret = rcl_action_notify_goal_done(action_server); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to notfiy action server of goal done"); - } -} - -bool -rclpy_action_server_goal_exists(py::capsule pyaction_server, py::object pygoal_info) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - destroy_ros_message_signature * destroy_ros_message = NULL; - rcl_action_goal_info_t * goal_info = static_cast( - rclpy_convert_from_py(pygoal_info.ptr(), &destroy_ros_message)); - if (!goal_info) { - throw py::error_already_set(); - } - - auto goal_info_ptr = std::unique_ptr( - goal_info, destroy_ros_message); - - return rcl_action_server_goal_exists(action_server, goal_info); -} - -py::object -rclpy_action_process_cancel_request( - py::capsule pyaction_server, py::object pycancel_request, py::object pycancel_response_type) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - - destroy_ros_message_signature * destroy_cancel_request = NULL; - rcl_action_cancel_request_t * cancel_request = static_cast( - rclpy_convert_from_py(pycancel_request.ptr(), &destroy_cancel_request)); - if (!cancel_request) { - throw py::error_already_set(); - } - auto cancel_request_ptr = - std::unique_ptr( - cancel_request, destroy_cancel_request); - - rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); - rcl_ret_t ret = rcl_action_process_cancel_request( - action_server, cancel_request, &cancel_response); - - if (RCL_RET_OK != ret) { - std::string error_text{"Failed to process cancel request: "}; - error_text += rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_action_cancel_response_fini(&cancel_response); - if (RCL_RET_OK != ret) { - error_text += ". Also failed to cleanup response: "; - error_text += rcl_get_error_string().str; - rcl_reset_error(); - } - throw std::runtime_error(error_text); - } - - PyObject * pycancel_response = - rclpy_convert_to_py(&cancel_response.msg, pycancel_response_type.ptr()); - if (!pycancel_response) { - rcl_ret_t ignore = rcl_action_cancel_response_fini(&cancel_response); - (void) ignore; - throw py::error_already_set(); - } - py::object return_value = py::reinterpret_steal(pycancel_response); - - ret = rcl_action_cancel_response_fini(&cancel_response); - - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to finalize cancel response"); - } - return return_value; -} - -py::tuple -rclpy_action_expire_goals(py::capsule pyaction_server, int64_t max_num_goals) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - - auto expired_goals = - std::unique_ptr(new rcl_action_goal_info_t[max_num_goals]); - size_t num_expired; - rcl_ret_t ret = rcl_action_expire_goals( - action_server, expired_goals.get(), max_num_goals, &num_expired); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to expire goals"); - } - - // Get Python GoalInfo type - py::module pyaction_msgs_module = py::module::import("action_msgs.msg"); - py::object pygoal_info_class = pyaction_msgs_module.attr("GoalInfo"); - py::object pygoal_info_type = pygoal_info_class(); - - // Create a tuple of GoalInfo instances to return - py::tuple result_tuple(num_expired); - - for (size_t i = 0; i < num_expired; ++i) { - result_tuple[i] = py::reinterpret_steal( - rclpy_convert_to_py(&(expired_goals.get()[i]), pygoal_info_type.ptr())); - } - - return result_tuple; -} - - py::object rclpy_action_get_client_names_and_types_by_node( py::capsule pynode, const char * remote_node_name, const char * remote_node_namespace) @@ -1123,78 +677,34 @@ define_action_api(py::module m) 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."); - m.def( - "rclpy_action_notify_goal_done", &rclpy_action_notify_goal_done, - "Notify and action server that a goal has reached a terminal state."); define_action_goal_handle(m); + define_action_server(m); - m.def( - "rclpy_action_server_goal_exists", &rclpy_action_server_goal_exists, - "Check if a goal being tracked by an action server."); - m.def( - "rclpy_action_process_cancel_request", &rclpy_action_process_cancel_request, - "Process a cancel request to determine what goals should be canceled."); - m.def( - "rclpy_action_expire_goals", &rclpy_action_expire_goals, - "Expire goals associated with an action server."); m.def( "rclpy_action_get_client_names_and_types_by_node", &rclpy_action_get_client_names_and_types_by_node, diff --git a/rclpy/src/rclpy/action_goal_handle.cpp b/rclpy/src/rclpy/action_goal_handle.cpp index 9ca6b840e..45904043c 100644 --- a/rclpy/src/rclpy/action_goal_handle.cpp +++ b/rclpy/src/rclpy/action_goal_handle.cpp @@ -29,28 +29,9 @@ namespace py = pybind11; namespace rclpy { -template -T -get_pointer(py::capsule & capsule, const char * name) -{ - if (strcmp(name, capsule.name())) { - std::string error_text{"Expected capusle with name '"}; - error_text += name; - error_text += "' but got '"; - error_text += capsule.name(); - error_text += "'"; - throw py::value_error(error_text); - } - // TODO(sloretz) use get_pointer() in pybind11 2.6+ - return static_cast(capsule); -} - ActionGoalHandle::ActionGoalHandle( - py::capsule pyaction_server, py::object pygoal_info_msg) + rclpy::ActionServer & action_server, py::object pygoal_info_msg) { - auto action_server = get_pointer( - pyaction_server, "rcl_action_server_t"); - destroy_ros_message_signature * destroy_ros_message = NULL; auto goal_info_msg = static_cast( rclpy_convert_from_py(pygoal_info_msg.ptr(), &destroy_ros_message)); @@ -62,7 +43,8 @@ ActionGoalHandle::ActionGoalHandle( auto goal_info_msg_ptr = std::unique_ptr( goal_info_msg, destroy_ros_message); - auto rcl_handle = rcl_action_accept_new_goal(action_server, goal_info_msg); + auto rcl_handle = rcl_action_accept_new_goal( + action_server.rcl_ptr(), goal_info_msg); if (!rcl_handle) { throw rclpy::RCLError("Failed to accept new goal"); } @@ -117,9 +99,8 @@ void define_action_goal_handle(py::module module) { py::class_>( - module, - "ActionGoalHandle") - .def(py::init()) + module, "ActionGoalHandle") + .def(py::init()) .def_property_readonly( "pointer", [](const ActionGoalHandle & handle) { return reinterpret_cast(handle.rcl_ptr()); diff --git a/rclpy/src/rclpy/action_goal_handle.hpp b/rclpy/src/rclpy/action_goal_handle.hpp index e2c054a35..b1e1bfc94 100644 --- a/rclpy/src/rclpy/action_goal_handle.hpp +++ b/rclpy/src/rclpy/action_goal_handle.hpp @@ -21,6 +21,7 @@ #include +#include "action_server.hpp" #include "destroyable.hpp" #include "handle.hpp" @@ -28,7 +29,10 @@ namespace py = pybind11; namespace rclpy { -class ActionGoalHandle : public Destroyable + +class ActionServer; + +class ActionGoalHandle : public Destroyable, public std::enable_shared_from_this { public: /// Create an action goal handle @@ -42,7 +46,7 @@ class ActionGoalHandle : public Destroyable * \param[in] pyaction_server handle to the action server that is accepting the goal * \param[in] pygoal_info_msg a message containing info about the goal being accepted */ - ActionGoalHandle(py::capsule pyaction_server, py::object pygoal_info_msg); + ActionGoalHandle(rclpy::ActionServer & action_server, py::object pygoal_info_msg); ~ActionGoalHandle() = default; diff --git a/rclpy/src/rclpy/action_server.cpp b/rclpy/src/rclpy/action_server.cpp new file mode 100644 index 000000000..dc64fccab --- /dev/null +++ b/rclpy/src/rclpy/action_server.cpp @@ -0,0 +1,438 @@ +// 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_server.hpp" + +namespace rclpy +{ + +void +ActionServer::destroy() +{ + rcl_action_server_.reset(); + node_handle_.reset(); +} + +ActionServer::ActionServer( + py::capsule pynode, + const rclpy::Clock & rclpy_clock, + 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, + double result_timeout) +: node_handle_(std::make_shared(pynode)) +{ + auto node = node_handle_->cast("rcl_node_t"); + + rcl_clock_t * clock = rclpy_clock.rcl_ptr(); + + 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_server_options_t action_server_ops = rcl_action_server_get_default_options(); + + action_server_ops.goal_service_qos = goal_service_qos; + action_server_ops.result_service_qos = result_service_qos; + action_server_ops.cancel_service_qos = cancel_service_qos; + action_server_ops.feedback_topic_qos = feedback_topic_qos; + action_server_ops.status_topic_qos = status_topic_qos; + action_server_ops.result_timeout.nanoseconds = (rcl_duration_value_t)RCL_S_TO_NS(result_timeout); + + rcl_action_server_ = std::shared_ptr( + new rcl_action_server_t, + [this](rcl_action_server_t * action_server) + { + auto node = node_handle_->cast_or_warn("rcl_node_t"); + + rcl_ret_t ret = rcl_action_server_fini(action_server, 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_server; + }); + + *rcl_action_server_ = rcl_action_get_zero_initialized_server(); + + rcl_ret_t ret = rcl_action_server_init( + rcl_action_server_.get(), + node, + clock, + ts, + action_name, + &action_server_ops); + if (RCL_RET_ACTION_NAME_INVALID == ret) { + std::string error_text{"Failed to create action server 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); + } else if (RCL_RET_OK != ret) { + std::string error_text{"Failed to create action server: "}; + error_text += rcl_get_error_string().str; + rcl_reset_error(); + throw py::value_error(error_text); + } +} + +#define TAKE_SERVICE_REQUEST(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 ## _request(rcl_action_server_.get(), &header, taken_msg.get()); \ + /* Create the tuple to return */ \ + if (ret == RCL_RET_ACTION_CLIENT_TAKE_FAILED || ret == RCL_RET_ACTION_SERVER_TAKE_FAILED) { \ + 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(header, convert_to_py(taken_msg.get(), pymsg_type)); \ + +py::tuple +ActionServer::take_goal_request(py::object pymsg_type) +{ + TAKE_SERVICE_REQUEST(goal) +} + +py::tuple +ActionServer::take_result_request(py::object pymsg_type) +{ + TAKE_SERVICE_REQUEST(result) +} + +#define SEND_SERVICE_RESPONSE(Type) \ + destroy_ros_message_signature * destroy_ros_message = NULL; \ + void * raw_ros_response = rclpy_convert_from_py(pyresponse.ptr(), &destroy_ros_message); \ + if (!raw_ros_response) { \ + throw py::error_already_set(); \ + } \ + rcl_ret_t ret = rcl_action_send_ ## Type ## _response( \ + rcl_action_server_.get(), header, raw_ros_response); \ + destroy_ros_message(raw_ros_response); \ + if (ret != RCL_RET_OK) { \ + throw rclpy::RCLError("Failed to send " #Type " response"); \ + } + +void +ActionServer::send_goal_response( + rmw_request_id_t * header, py::object pyresponse) +{ + SEND_SERVICE_RESPONSE(goal) +} + +void +ActionServer::send_result_response( + rmw_request_id_t * header, py::object pyresponse) +{ + SEND_SERVICE_RESPONSE(result) +} + +py::tuple +ActionServer::take_cancel_request(py::object pymsg_type) +{ + TAKE_SERVICE_REQUEST(cancel) +} + +void +ActionServer::send_cancel_response( + rmw_request_id_t * header, py::object pyresponse) +{ + SEND_SERVICE_RESPONSE(cancel) +} + +void +ActionServer::publish_feedback(py::object pymsg) +{ + auto ros_message = create_from_py(pymsg); + rcl_ret_t ret = rcl_action_publish_feedback(rcl_action_server_.get(), ros_message.get()); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to publish feedback with an action server"); + } +} + +void +ActionServer::publish_status() +{ + rcl_action_goal_status_array_t status_message = + rcl_action_get_zero_initialized_goal_status_array(); + rcl_ret_t ret = rcl_action_get_goal_status_array(rcl_action_server_.get(), &status_message); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed get goal status array"); + } + + ret = rcl_action_publish_status(rcl_action_server_.get(), &status_message); + + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed publish goal status array"); + } +} + +void +ActionServer::notify_goal_done() +{ + rcl_ret_t ret = rcl_action_notify_goal_done(rcl_action_server_.get()); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to notfiy action server of goal done"); + } +} + +bool +ActionServer::goal_exists(py::object pygoal_info) +{ + destroy_ros_message_signature * destroy_ros_message = NULL; + rcl_action_goal_info_t * goal_info = static_cast( + rclpy_convert_from_py(pygoal_info.ptr(), &destroy_ros_message)); + if (!goal_info) { + throw py::error_already_set(); + } + + auto goal_info_ptr = std::unique_ptr( + goal_info, destroy_ros_message); + + return rcl_action_server_goal_exists(rcl_action_server_.get(), goal_info); +} + +py::tuple +ActionServer::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 = rcl_action_server_wait_set_get_num_entities( + rcl_action_server_.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_server_t'"}; + throw rclpy::RCLError(error_text); + } + + py::tuple result_tuple(5); + result_tuple[0] = py::int_(num_subscriptions); + result_tuple[1] = py::int_(num_guard_conditions); + result_tuple[2] = py::int_(num_timers); + result_tuple[3] = py::int_(num_clients); + result_tuple[4] = py::int_(num_services); + return result_tuple; +} + +py::tuple +ActionServer::is_ready(py::capsule pywait_set) +{ + auto wait_set = static_cast(pywait_set); + bool is_goal_request_ready = false; + bool is_cancel_request_ready = false; + bool is_result_request_ready = false; + bool is_goal_expired = false; + rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( + wait_set, + rcl_action_server_.get(), + &is_goal_request_ready, + &is_cancel_request_ready, + &is_result_request_ready, + &is_goal_expired); + + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get number of ready entities for action server"); + } + + py::tuple result_tuple(4); + result_tuple[0] = py::bool_(is_goal_request_ready); + result_tuple[1] = py::bool_(is_cancel_request_ready); + result_tuple[2] = py::bool_(is_result_request_ready); + result_tuple[3] = py::bool_(is_goal_expired); + return result_tuple; +} + +void +ActionServer::add_to_waitset(py::capsule pywait_set) +{ + auto wait_set = static_cast(pywait_set); + rcl_ret_t ret = rcl_action_wait_set_add_action_server(wait_set, rcl_action_server_.get(), NULL); + if (RCL_RET_OK != ret) { + std::string error_text{"Failed to add 'rcl_action_server_t' to wait set"}; + throw rclpy::RCLError(error_text); + } +} + +py::object +ActionServer::process_cancel_request( + py::object pycancel_request, py::object pycancel_response_type) +{ + destroy_ros_message_signature * destroy_cancel_request = NULL; + rcl_action_cancel_request_t * cancel_request = static_cast( + rclpy_convert_from_py(pycancel_request.ptr(), &destroy_cancel_request)); + if (!cancel_request) { + throw py::error_already_set(); + } + auto cancel_request_ptr = + std::unique_ptr( + cancel_request, destroy_cancel_request); + + rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); + rcl_ret_t ret = rcl_action_process_cancel_request( + rcl_action_server_.get(), cancel_request, &cancel_response); + + if (RCL_RET_OK != ret) { + std::string error_text{"Failed to process cancel request: "}; + error_text += rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_cancel_response_fini(&cancel_response); + if (RCL_RET_OK != ret) { + error_text += ". Also failed to cleanup response: "; + error_text += rcl_get_error_string().str; + rcl_reset_error(); + } + throw std::runtime_error(error_text); + } + + PyObject * pycancel_response = + rclpy_convert_to_py(&cancel_response.msg, pycancel_response_type.ptr()); + if (!pycancel_response) { + rcl_ret_t ignore = rcl_action_cancel_response_fini(&cancel_response); + (void) ignore; + throw py::error_already_set(); + } + py::object return_value = py::reinterpret_steal(pycancel_response); + + ret = rcl_action_cancel_response_fini(&cancel_response); + + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to finalize cancel response"); + } + return return_value; +} + +py::tuple +ActionServer::expire_goals(int64_t max_num_goals) +{ + auto expired_goals = + std::unique_ptr(new rcl_action_goal_info_t[max_num_goals]); + size_t num_expired; + rcl_ret_t ret = rcl_action_expire_goals( + rcl_action_server_.get(), expired_goals.get(), max_num_goals, &num_expired); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to expire goals"); + } + + // Get Python GoalInfo type + py::module pyaction_msgs_module = py::module::import("action_msgs.msg"); + py::object pygoal_info_class = pyaction_msgs_module.attr("GoalInfo"); + py::object pygoal_info_type = pygoal_info_class(); + + // Create a tuple of GoalInfo instances to return + py::tuple result_tuple(num_expired); + + for (size_t i = 0; i < num_expired; ++i) { + result_tuple[i] = py::reinterpret_steal( + rclpy_convert_to_py(&(expired_goals.get()[i]), pygoal_info_type.ptr())); + } + + return result_tuple; +} + +void +define_action_server(py::object module) +{ + py::class_>(module, "ActionServer") + .def( + py::init()) + .def_property_readonly( + "pointer", [](const ActionServer & action_server) { + return reinterpret_cast(action_server.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "take_goal_request", &ActionServer::take_goal_request, + "Take an action goal request.") + .def( + "send_goal_response", &ActionServer::send_goal_response, + "Send an action goal response.") + .def( + "send_result_response", &ActionServer::send_result_response, + "Send an action result response.") + .def( + "take_cancel_request", &ActionServer::take_cancel_request, + "Take an action cancel request.") + .def( + "take_result_request", &ActionServer::take_result_request, + "Take an action result request.") + .def( + "send_cancel_response", &ActionServer::send_cancel_response, + "Send an action cancel response.") + .def( + "publish_feedback", &ActionServer::publish_feedback, + " Publish a feedback message from a given action server.") + .def( + "publish_status", &ActionServer::publish_status, + "Publish a status message from a given action server.") + .def( + "notify_goal_done", &ActionServer::notify_goal_done, + "Notify goal is done.") + .def( + "goal_exists", &ActionServer::goal_exists, + "Check is a goal exists in the server.") + .def( + "process_cancel_request", &ActionServer::process_cancel_request, + "Process a cancel request") + .def( + "expire_goals", &ActionServer::expire_goals, + "Expired goals.") + .def( + "get_num_entities", &ActionServer::get_num_entities, + "Get the number of wait set entities that make up an action entity.") + .def( + "is_ready", &ActionServer::is_ready, + "Check if an action entity has any ready wait set entities.") + .def( + "add_to_waitset", &ActionServer::add_to_waitset, + "Add an action entitiy to a wait set."); +} + +} // namespace rclpy diff --git a/rclpy/src/rclpy/action_server.hpp b/rclpy/src/rclpy/action_server.hpp new file mode 100644 index 000000000..ab3ccfeff --- /dev/null +++ b/rclpy/src/rclpy/action_server.hpp @@ -0,0 +1,252 @@ +// 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_SERVER_HPP_ +#define RCLPY__ACTION_SERVER_HPP_ + +#include + +#include + +#include + +#include "clock.hpp" +#include "destroyable.hpp" +#include "handle.hpp" + +namespace py = pybind11; + +namespace rclpy +{ +class ActionServer : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create an action client. + /** + * This function 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 over the wire. + * + * Raises AttributeError if action type is invalid + * Raises ValueError if action name is invalid + * Raises RuntimeError if the action server could not be created. + * + * \remark Call rclpy_action_destroy_entity() to destroy an action server. + * \param[in] pynode Capsule pointing to the node to add the action server to. + * \param[in] pyaction_type Action module associated with the action server. + * \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. + * \return Capsule named 'rcl_action_server_t', or + * \return NULL on failure. + */ + ActionServer( + py::capsule pynode, + const rclpy::Clock & rclpy_clock, + 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, + double result_timeout); + + /// Take an action goal request. + /** + * Raises AttributeError if there is an issue parsing the pygoal_request_type. + * Raises RuntimeError on failure. + * + * \param[in] pygoal_request_type An instance of the type of request message to take. + * \return 2-tuple (header, received request message) where the header is an + * "rclpy.rmw_request_id_t" type, or + * \return 2-tuple (None, None) if there as no message to take, or + * \return (None, None) if there is a failure. + */ + py::tuple + take_goal_request(py::object pymsg_type); + + /// Send an action goal response. + /** + * Raises AttributeError if there is an issue parsing the pygoal_response. + * Raises RuntimeError on failure. + * + * \param[in] header Pointer to the message header. + * \param[in] pygoal_response The response message to send. + * \return None + * \return NULL if there is a failure. + */ + void + send_goal_response( + rmw_request_id_t * header, py::object pyresponse); + + /// Send an action result response. + /** + * Raises AttributeError if there is an issue parsing the pyresult_response. + * Raises RuntimeError on failure. + * + * \param[in] pyheader Pointer to the message header. + * \param[in] pyresult_response The response message to send. + * \return None + * \return NULL if there is a failure. + */ + void + send_result_response( + rmw_request_id_t * header, py::object pyresponse); + + /// Take an action cancel request. + /** + * Raises AttributeError if there is an issue parsing the pycancel_request_type. + * Raises RuntimeError on failure. + * + * \param[in] pycancel_request_type An instance of the type of request message to take. + * \return 2-tuple (header, received request message) where the header is an + * "rmw_request_id_t" type, or + * \return 2-tuple (None, None) if there as no message to take, or + * \return (None, None) if there is a failure. + */ + py::tuple + take_cancel_request(py::object pymsg_type); + + /// Take an action result request. + /** + * Raises AttributeError if there is an issue parsing the pyresult_request_type. + * Raises RuntimeError on failure. + * + * \param[in] pyresult_request_type An instance of the type of request message to take. + * \return 2-tuple (header, received request message) where the header is an + * "rclpy.rmw_request_id_t" type, or + * \return 2-tuple (None, None) if there as no message to take, or + * \return (None, None) if there is a failure. + */ + py::tuple + take_result_request(py::object pymsg_type); + + /// Send an action cancel response. + /** + * Raises AttributeError if there is an issue parsing the pycancel_response. + * Raises RuntimeError on failure. + * + * \param[in] pyheader Pointer to the message header. + * \param[in] pycancel_response The response message to send. + * \return sequence_number PyLong object representing the index of the sent response, or + * \return NULL if there is a failure. + */ + void + send_cancel_response( + rmw_request_id_t * header, py::object pyresponse); + + /// Publish a feedback message from a given action server. + /** + * Raises AttributeError if there is an issue parsing the pyfeedback_msg. + * Raises RuntimeError on failure while publishing a feedback message. + * + * \param[in] pyfeedback_msg The feedback message to publish. + * \return None + */ + void + publish_feedback(py::object pymsg); + + /// Publish a status message from a given action server. + /** + * Raises RuntimeError on failure while publishing a status message. + * + * \return None + */ + void + publish_status(); + + void + notify_goal_done(); + + bool + goal_exists(py::object pygoal_info); + + py::object + process_cancel_request( + py::object pycancel_request, py::object pycancel_response_type); + + py::tuple + expire_goals(int64_t max_num_goals); + + /// 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 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 Bool representing the ready sub-entities. + * (is_goal_request_ready, + * is_cancel_request_ready, + * is_result_request_ready, + * is_goal_expired) + */ + py::tuple + is_ready(py::capsule pywait_set); + + /// Add an action entitiy to a wait set. + /** + * Raises RuntimeError on failure. + * \param[in] pyentity Capsule pointer to an action entity + * (rcl_action_client_t or rcl_action_server_t). + * \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_server_t * + rcl_ptr() const + { + return rcl_action_server_.get(); + } + + /// Get rcl_client_t pointer + std::shared_ptr + get_rcl_shared_ptr() + { + return rcl_action_server_; + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + std::shared_ptr node_handle_; + std::shared_ptr rcl_action_server_; +}; +/// Define a pybind11 wrapper for an rcl_time_point_t +/** + * \param[in] module a pybind11 module to add the definition to + */ +void define_action_server(py::object module); +} // namespace rclpy + +#endif // RCLPY__ACTION_SERVER_HPP_ From c726671b99239920a061616edd769ae7b0e720d6 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 9 Apr 2021 14:10:03 +0200 Subject: [PATCH 2/4] Fixed Test_action_server Signed-off-by: ahcorde --- rclpy/src/rclpy/action_server.cpp | 52 ++++++++++--------------------- rclpy/src/rclpy/utils.cpp | 25 +++++++++++++++ rclpy/src/rclpy/utils.hpp | 10 ++++++ 3 files changed, 51 insertions(+), 36 deletions(-) diff --git a/rclpy/src/rclpy/action_server.cpp b/rclpy/src/rclpy/action_server.cpp index dc64fccab..b3c76908b 100644 --- a/rclpy/src/rclpy/action_server.cpp +++ b/rclpy/src/rclpy/action_server.cpp @@ -137,15 +137,10 @@ ActionServer::take_result_request(py::object pymsg_type) } #define SEND_SERVICE_RESPONSE(Type) \ - destroy_ros_message_signature * destroy_ros_message = NULL; \ - void * raw_ros_response = rclpy_convert_from_py(pyresponse.ptr(), &destroy_ros_message); \ - if (!raw_ros_response) { \ - throw py::error_already_set(); \ - } \ + auto ros_response = convert_from_py(pyresponse); \ rcl_ret_t ret = rcl_action_send_ ## Type ## _response( \ - rcl_action_server_.get(), header, raw_ros_response); \ - destroy_ros_message(raw_ros_response); \ - if (ret != RCL_RET_OK) { \ + rcl_action_server_.get(), header, ros_response.get()); \ + if (RCL_RET_OK != ret) { \ throw rclpy::RCLError("Failed to send " #Type " response"); \ } @@ -179,7 +174,7 @@ ActionServer::send_cancel_response( void ActionServer::publish_feedback(py::object pymsg) { - auto ros_message = create_from_py(pymsg); + auto ros_message = convert_from_py(pymsg); rcl_ret_t ret = rcl_action_publish_feedback(rcl_action_server_.get(), ros_message.get()); if (RCL_RET_OK != ret) { throw rclpy::RCLError("Failed to publish feedback with an action server"); @@ -215,17 +210,12 @@ ActionServer::notify_goal_done() bool ActionServer::goal_exists(py::object pygoal_info) { - destroy_ros_message_signature * destroy_ros_message = NULL; - rcl_action_goal_info_t * goal_info = static_cast( - rclpy_convert_from_py(pygoal_info.ptr(), &destroy_ros_message)); - if (!goal_info) { + auto goal_info = convert_from_py(pygoal_info); + rcl_action_goal_info_t * goal_info_type = static_cast(goal_info.get()); + if (!goal_info_type) { throw py::error_already_set(); } - - auto goal_info_ptr = std::unique_ptr( - goal_info, destroy_ros_message); - - return rcl_action_server_goal_exists(rcl_action_server_.get(), goal_info); + return rcl_action_server_goal_exists(rcl_action_server_.get(), goal_info_type); } py::tuple @@ -302,19 +292,16 @@ py::object ActionServer::process_cancel_request( py::object pycancel_request, py::object pycancel_response_type) { - destroy_ros_message_signature * destroy_cancel_request = NULL; - rcl_action_cancel_request_t * cancel_request = static_cast( - rclpy_convert_from_py(pycancel_request.ptr(), &destroy_cancel_request)); - if (!cancel_request) { + auto cancel_request = convert_from_py(pycancel_request); + rcl_action_cancel_request_t * cancel_request_type = static_cast( + cancel_request.get()); + if (!cancel_request_type) { throw py::error_already_set(); } - auto cancel_request_ptr = - std::unique_ptr( - cancel_request, destroy_cancel_request); rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); rcl_ret_t ret = rcl_action_process_cancel_request( - rcl_action_server_.get(), cancel_request, &cancel_response); + rcl_action_server_.get(), cancel_request_type, &cancel_response); if (RCL_RET_OK != ret) { std::string error_text{"Failed to process cancel request: "}; @@ -330,14 +317,7 @@ ActionServer::process_cancel_request( throw std::runtime_error(error_text); } - PyObject * pycancel_response = - rclpy_convert_to_py(&cancel_response.msg, pycancel_response_type.ptr()); - if (!pycancel_response) { - rcl_ret_t ignore = rcl_action_cancel_response_fini(&cancel_response); - (void) ignore; - throw py::error_already_set(); - } - py::object return_value = py::reinterpret_steal(pycancel_response); + py::object return_value = convert_to_py(&cancel_response.msg, pycancel_response_type); ret = rcl_action_cancel_response_fini(&cancel_response); @@ -368,8 +348,8 @@ ActionServer::expire_goals(int64_t max_num_goals) py::tuple result_tuple(num_expired); for (size_t i = 0; i < num_expired; ++i) { - result_tuple[i] = py::reinterpret_steal( - rclpy_convert_to_py(&(expired_goals.get()[i]), pygoal_info_type.ptr())); + result_tuple[i] = + convert_to_py(&(expired_goals.get()[i]), pygoal_info_type); } return result_tuple; diff --git a/rclpy/src/rclpy/utils.cpp b/rclpy/src/rclpy/utils.cpp index 4aca238e8..cc9e2a26f 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())) { + 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. From 64b7984f7f756733862ba287357d50acc1b746a0 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 12 Apr 2021 19:37:20 +0200 Subject: [PATCH 3/4] Improved docs Signed-off-by: ahcorde --- rclpy/src/rclpy/action_server.hpp | 84 ++++++++++++++++++------------- 1 file changed, 50 insertions(+), 34 deletions(-) diff --git a/rclpy/src/rclpy/action_server.hpp b/rclpy/src/rclpy/action_server.hpp index ab3ccfeff..b7e33aa7f 100644 --- a/rclpy/src/rclpy/action_server.hpp +++ b/rclpy/src/rclpy/action_server.hpp @@ -29,21 +29,22 @@ namespace py = pybind11; namespace rclpy { +/* + * This class will create an action server for the given action name. + * This client will use the typesupport defined in the action module + * provided as pyaction_type to send messages. + */ class ActionServer : public Destroyable, public std::enable_shared_from_this { public: - /// Create an action client. + /// Create an action server. /** - * This function 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 over the wire. - * * Raises AttributeError if action type is invalid * Raises ValueError if action name is invalid * Raises RuntimeError if the action server could not be created. * - * \remark Call rclpy_action_destroy_entity() to destroy an action server. * \param[in] pynode Capsule pointing to the node to add the action server to. + * \param[in] rclpy_clock Clock use to create the action server. * \param[in] pyaction_type Action module associated with the action server. * \param[in] pyaction_name Python object containing the action name. * \param[in] goal_service_qos rmw_qos_profile_t object for the goal service. @@ -51,8 +52,6 @@ class ActionServer : public Destroyable, public std::enable_shared_from_this Date: Wed, 14 Apr 2021 09:51:10 +0200 Subject: [PATCH 4/4] Improved docs and minor fixes Signed-off-by: ahcorde --- rclpy/src/rclpy/action_server.cpp | 26 +++++++++++--------------- rclpy/src/rclpy/action_server.hpp | 12 ++++-------- 2 files changed, 15 insertions(+), 23 deletions(-) diff --git a/rclpy/src/rclpy/action_server.cpp b/rclpy/src/rclpy/action_server.cpp index b3c76908b..5bc791999 100644 --- a/rclpy/src/rclpy/action_server.cpp +++ b/rclpy/src/rclpy/action_server.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -212,9 +213,6 @@ ActionServer::goal_exists(py::object pygoal_info) { auto goal_info = convert_from_py(pygoal_info); rcl_action_goal_info_t * goal_info_type = static_cast(goal_info.get()); - if (!goal_info_type) { - throw py::error_already_set(); - } return rcl_action_server_goal_exists(rcl_action_server_.get(), goal_info_type); } @@ -283,8 +281,7 @@ ActionServer::add_to_waitset(py::capsule pywait_set) auto wait_set = static_cast(pywait_set); rcl_ret_t ret = rcl_action_wait_set_add_action_server(wait_set, rcl_action_server_.get(), NULL); if (RCL_RET_OK != ret) { - std::string error_text{"Failed to add 'rcl_action_server_t' to wait set"}; - throw rclpy::RCLError(error_text); + throw rclpy::RCLError("Failed to add 'rcl_action_server_t' to wait set"); } } @@ -293,15 +290,12 @@ ActionServer::process_cancel_request( py::object pycancel_request, py::object pycancel_response_type) { auto cancel_request = convert_from_py(pycancel_request); - rcl_action_cancel_request_t * cancel_request_type = static_cast( + rcl_action_cancel_request_t * cancel_request_tmp = static_cast( cancel_request.get()); - if (!cancel_request_type) { - throw py::error_already_set(); - } rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); rcl_ret_t ret = rcl_action_process_cancel_request( - rcl_action_server_.get(), cancel_request_type, &cancel_response); + rcl_action_server_.get(), cancel_request_tmp, &cancel_response); if (RCL_RET_OK != ret) { std::string error_text{"Failed to process cancel request: "}; @@ -318,12 +312,14 @@ ActionServer::process_cancel_request( } py::object return_value = convert_to_py(&cancel_response.msg, pycancel_response_type); + RCPPUTILS_SCOPE_EXIT( + { + ret = rcl_action_cancel_response_fini(&cancel_response); - ret = rcl_action_cancel_response_fini(&cancel_response); - - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to finalize cancel response"); - } + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to finalize cancel response"); + } + }); return return_value; } diff --git a/rclpy/src/rclpy/action_server.hpp b/rclpy/src/rclpy/action_server.hpp index b7e33aa7f..f5008a9dd 100644 --- a/rclpy/src/rclpy/action_server.hpp +++ b/rclpy/src/rclpy/action_server.hpp @@ -73,8 +73,7 @@ class ActionServer : public Destroyable, public std::enable_shared_from_this