diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index 7d1f80c84..b1abfd719 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -155,6 +155,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED src/rclpy/_rclpy_pycapsule.cpp src/rclpy/action_client.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 2f55d1856..8b5121921 100644 --- a/rclpy/src/rclpy/_rclpy_action.cpp +++ b/rclpy/src/rclpy/_rclpy_action.cpp @@ -28,6 +28,7 @@ #include "action_client.hpp" #include "action_goal_handle.hpp" +#include "action_server.hpp" #include "clock.hpp" namespace py = pybind11; @@ -49,45 +50,6 @@ get_pointer(py::capsule & capsule, const char * name) return static_cast(capsule); } - -/// Destroy an rcl_action entity. -/** - * Raises RuntimeError on failure. - * - * \param[in] pyentity Capsule pointing to the entity to destroy. - * \param[in] pynode Capsule pointing to the node the action client was added to. - */ -void -rclpy_action_destroy_entity(py::capsule pyentity, py::capsule pynode) -{ - 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_ret_t ret; - 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"); - } - - if (ret != RCL_RET_OK) { - std::string error_text = "Failed to fini '"; - error_text += pyentity.name(); - error_text += "'"; - throw rclpy::RCLError(error_text); - } - - if (PyCapsule_SetName(pyentity.ptr(), "_destroyed_by_rclpy_action_destroy_entity_")) { - throw py::error_already_set(); - } -} - /// Fetch a predefined qos_profile from rcl_action and convert it to a Python QoSProfile object. /** * Raises RuntimeError if the QoS profile is unknown. @@ -111,697 +73,6 @@ rclpy_action_get_rmw_qos_profile(const char * rmw_profile) return py::reinterpret_steal(pyqos_profile); } -/// 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 -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_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 { - std::string error_text{"Unknown entity: "}; - error_text += pyentity.name(); - throw std::runtime_error(error_text); - } - - if (RCL_RET_OK != ret) { - std::string error_text{"Failed to add '"}; - error_text += pyentity.name(); - error_text += "' to wait set"; - throw rclpy::RCLError(error_text); - } -} - -/// Get the number of wait set entities that make up an action entity. -/** - * \param[in] pyentity Capsule pointer to an action entity - * (rcl_action_client_t or rcl_action_server_t). - * \return Tuple containing the number of wait set entities: - * (num_subscriptions, - * num_guard_conditions, - * num_timers, - * num_clients, - * num_services) - */ -py::tuple -rclpy_action_wait_set_get_num_entities(py::capsule pyentity) -{ - 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; - 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(); - throw std::runtime_error(error_text); - } - - if (RCL_RET_OK != ret) { - std::string error_text{"Failed to get number of entities for '"}; - error_text += pyentity.name(); - error_text += "'"; - 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; -} - -/// 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] entity Capsule pointing to the action entity - * (rcl_action_client_t or rcl_action_server_t). - * \param[in] pywait_set Capsule pointing to the wait set structure. - * \return A tuple of Bool representing the ready sub-entities. - * For a rcl_action_client_t: - * (is_feedback_ready, - * is_status_ready, - * is_goal_response_ready, - * is_cancel_response_ready, - * is_result_response_ready) - * - * For a rcl_action_server_t: - * (is_goal_request_ready, - * is_cancel_request_ready, - * is_result_request_ready, - * is_goal_expired) - */ -py::tuple -rclpy_action_wait_set_is_ready(py::capsule pyentity, py::capsule pywait_set) -{ - auto wait_set = get_pointer(pywait_set, "rcl_wait_set_t"); - - if (0 == strcmp(pyentity.name(), "rcl_action_client_t")) { - auto action_client = get_pointer(pyentity, "rcl_action_client_t"); - 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, - action_client, - &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; - } 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: "}; - error_text += pyentity.name(); - throw std::runtime_error(error_text); -} - -/// 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. - * - * On a successful call a capsule referencing the created rcl_action_client_t structure - * is returned. - * - * Raises ValueError if action name is invalid - * Raises RuntimeError if the action client could not be created. - * - * \remark Call rclpy_action_destroy_entity() to destroy an action client. - * \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. - * \return Capsule named 'rcl_action_client_t', or - * \return NULL on failure. - */ -py::capsule -rclpy_action_create_client( - 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) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - 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; - - auto deleter = [](rcl_action_client_t * ptr) {PyMem_Free(ptr);}; - auto action_client = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rcl_action_client_t))), - deleter); - if (!action_client) { - throw std::bad_alloc(); - } - - *action_client = rcl_action_get_zero_initialized_client(); - rcl_ret_t ret = rcl_action_client_init( - action_client.get(), - node, - ts, - action_name, - &action_client_ops); - if (ret == RCL_RET_ACTION_NAME_INVALID) { - 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); - } else if (ret != RCL_RET_OK) { - 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); - } - - 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. - * - * \param[in] pynode Capsule pointing to the node to associated with the action client. - * \param[in] pyaction_client The action client to use when checking for an available server. - * \return True if an action server is available, False otherwise. - */ -bool -rclpy_action_server_is_available(py::capsule pynode, py::capsule pyaction_client) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - auto action_client = get_pointer(pyaction_client, "rcl_action_client_t"); - - bool is_available = false; - rcl_ret_t ret = rcl_action_server_is_available(node, action_client, &is_available); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to check if action server is available"); - } - return is_available; -} - -#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; - -/// 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 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 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) -} - - -/// 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"); - } -} - -/// 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"); - } -} - -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) @@ -892,69 +163,12 @@ namespace rclpy void define_action_api(py::module m) { - m.def( - "rclpy_action_destroy_entity", &rclpy_action_destroy_entity, - "Destroy a rclpy_action entity."); m.def( "rclpy_action_get_rmw_qos_profile", &rclpy_action_get_rmw_qos_profile, "Get an action RMW QoS profile."); - m.def( - "rclpy_action_wait_set_add", &rclpy_action_wait_set_add, - "Add an action entitiy to a wait set."); - m.def( - "rclpy_action_wait_set_get_num_entities", &rclpy_action_wait_set_get_num_entities, - "Get the number of wait set entities for an action entitity."); - 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_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_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_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_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_publish_feedback", &rclpy_action_publish_feedback, - "Publish a feedback message."); - m.def( - "rclpy_action_publish_status", &rclpy_action_publish_status, - "Publish 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..5bc791999 --- /dev/null +++ b/rclpy/src/rclpy/action_server.cpp @@ -0,0 +1,414 @@ +// 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 + +#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) \ + auto ros_response = convert_from_py(pyresponse); \ + rcl_ret_t ret = rcl_action_send_ ## Type ## _response( \ + rcl_action_server_.get(), header, ros_response.get()); \ + if (RCL_RET_OK != ret) { \ + 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 = 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"); + } +} + +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) +{ + auto goal_info = convert_from_py(pygoal_info); + rcl_action_goal_info_t * goal_info_type = static_cast(goal_info.get()); + return rcl_action_server_goal_exists(rcl_action_server_.get(), goal_info_type); +} + +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) { + throw rclpy::RCLError("Failed to add 'rcl_action_server_t' to wait set"); + } +} + +py::object +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_tmp = static_cast( + cancel_request.get()); + + 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_tmp, &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); + } + + py::object return_value = convert_to_py(&cancel_response.msg, pycancel_response_type); + RCPPUTILS_SCOPE_EXIT( + { + 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] = + convert_to_py(&(expired_goals.get()[i]), pygoal_info_type); + } + + 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..f5008a9dd --- /dev/null +++ b/rclpy/src/rclpy/action_server.hpp @@ -0,0 +1,264 @@ +// 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 +{ +/* + * 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 server. + /** + * Raises AttributeError if action type is invalid + * Raises ValueError if action name is invalid + * Raises RuntimeError if the action server could not be created. + * + * \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. + * \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. + */ + 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 RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] pymsg_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 + */ + py::tuple + take_goal_request(py::object pymsg_type); + + /// Send an action goal response. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] header Pointer to the message header. + * \param[in] pyresponse The response message to send. + */ + void + send_goal_response( + rmw_request_id_t * header, py::object pyresponse); + + /// Send an action result response. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] pyheader Pointer to the message header. + * \param[in] pyresponse The response message to send. + */ + void + send_result_response( + rmw_request_id_t * header, py::object pyresponse); + + /// Take an action cancel request. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] pymsg_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 + */ + py::tuple + take_cancel_request(py::object pymsg_type); + + /// Take an action result request. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] pymsg_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 + */ + py::tuple + take_result_request(py::object pymsg_type); + + /// Send an action cancel response. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] pyheader Pointer to the message header. + * \param[in] pyresponse The response message to send. + */ + void + send_cancel_response( + rmw_request_id_t * header, py::object pyresponse); + + /// Publish a feedback message from a given action server. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure while publishing a feedback message. + * + * \param[in] pymsg The feedback message to publish. + */ + void + publish_feedback(py::object pymsg); + + /// Publish a status message from a given action server. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure while publishing a status message. + */ + void + publish_status(); + + /// Notifies action server that a goal handle reached a terminal state. + /** + * Raises RCLError if an error occurs in rcl + */ + void + notify_goal_done(); + + /// Check if a goal is already being tracked by an action server. + /* + * Raises AttributeError if there is an issue parsing the pygoal_info. + * + * \param[in] pygoal_info the identifiers of goals that expired, or set to `NULL` if unused + * \return True if the goal exists, false otherwise. + */ + bool + goal_exists(py::object pygoal_info); + + /// Process a cancel request using an action server. + /** + * This is a non-blocking call. + * + * Raises RuntimeError on failure while publishing a status message. + * Raises RCLError if an error occurs in rcl + * + * \return the cancel response message + */ + py::object + process_cancel_request( + py::object pycancel_request, py::object pycancel_response_type); + + /// Expires goals associated with an action server. + py::tuple + expire_goals(int64_t max_num_goals); + + /// Get the number of wait set entities that make up an action entity. + /** + * Raises RCLError if an error occurs in rcl + * + * \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. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pywait_set Capsule pointing to the wait set structure. + * \return A tuple of booleans representing 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. + * Raises RCLError if an error occurs in rcl + * + * \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_