diff --git a/rclpy/rclpy/action_client.py b/rclpy/rclpy/action_client.py index f14f202a7..6fe35c688 100644 --- a/rclpy/rclpy/action_client.py +++ b/rclpy/rclpy/action_client.py @@ -135,8 +135,13 @@ def _remove_pending_request(self, future, pending_requests): Remove a future from the list of pending requests. This prevents a future from receiving a request and executing its done callbacks. - :param future: a future returned from :meth:`call_async` + :param future: a future returned from one of :meth:`send_goal_async`, + :meth:`cancel_goal_async`, or :meth:`get_result_async`. :type future: rclpy.task.Future + :param pending_requests: The list of pending requests. + :type pending_requests: dict + :return: The sequence number associated with the removed future, or + None if the future was not found in the list. """ for seq, req_future in pending_requests.items(): if future == req_future: @@ -160,7 +165,7 @@ def _remove_pending_result_request(self, future): # Start Waitable API def is_ready(self, wait_set): - """Return True if entities are ready in the wait set.""" + """Return True if one or more entities are ready in the wait set.""" ready_entities = _rclpy_action.rclpy_action_wait_set_is_ready(self.client_handle, wait_set) self._is_feedback_ready = ready_entities[0] self._is_status_ready = ready_entities[1] @@ -202,7 +207,12 @@ def take_data(self): return data async def execute(self, taken_data): - """Execute work after data has been taken from a ready wait set.""" + """ + Execute work after data has been taken from a ready wait set. + + This will set results for Future objects for any received service responses and + call any user-defined callbacks (e.g. feedback). + """ if 'feedback' in taken_data: feedback_msg = taken_data['feedback'] goal_uuid = uuid.UUID(bytes=bytes(feedback_msg.action_goal_id.uuid)) @@ -247,7 +257,7 @@ async def execute(self, taken_data): self._pending_result_requests[sequence_number].set_result(result_response) def get_num_entities(self): - """Return number of each type of entity used.""" + """Return number of each type of entity used in the wait set.""" return _rclpy_action.rclpy_action_wait_set_get_num_entities(self.client_handle) def add_to_wait_set(self, wait_set): @@ -261,8 +271,12 @@ def send_goal(self, goal, **kwargs): Do not call this method in a callback or a deadlock may occur. - :param goal: The goal request - :return: The result response + See :meth:`send_goal_async` for more info about keyword arguments. + + :param goal: The goal request. + :type goal: action_type.Goal + :return: The result response. + :rtype: action_type.Result """ future = self.send_goal_async(goal, kwargs) while self.node.context.ok() and not future.done(): @@ -273,7 +287,16 @@ def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None): """ Send a goal and asynchronously get the result. - :param goal: The goal request + The result of the returned Future is set to a ClientGoalHandle when receipt of the goal + is acknowledged by an action server. + + :param goal: The goal request. + :type goal: action_type.Goal + :param feedback_callback: Callback function for feedback associated with the goal. + :type feedback_callback: function + :param goal_uuid: Universally unique identifier for the goal. + If None, then a random UUID is generated. + :type: unique_identifier_msgs.UUID :return: a Future instance to a goal handle that completes when the goal request has been accepted or rejected. :rtype: :class:`rclpy.task.Future` instance @@ -303,6 +326,7 @@ def cancel_goal(self, goal_handle): Do not call this method in a callback or a deadlock may occur. :param goal_handle: Handle to the goal to cancel. + :type goal_handle: :class:`ClientGoalHandle` :return: The cancel response. """ pass @@ -312,6 +336,7 @@ def cancel_goal_async(self, goal_handle): Send a cancel request for an active goal and asynchronously get the result. :param goal_handle: Handle to the goal to cancel. + :type goal_handle: :class:`ClientGoalHandle` :return: a Future instance that completes when the cancel request has been processed. :rtype: :class:`rclpy.task.Future` instance """ @@ -341,6 +366,7 @@ def get_result(self, goal_handle): Do not call this method in a callback or a deadlock may occur. :param goal_handle: Handle to the goal to get the result for. + :type goal_handle: :class:`ClientGoalHandle` :return: The result response. """ pass @@ -350,6 +376,7 @@ def get_result_async(self, goal_handle): Request the result for an active goal asynchronously. :param goal_handle: Handle to the goal to cancel. + :type goal_handle: :class:`ClientGoalHandle` :return: a Future instance that completes when the get result request has been processed. :rtype: :class:`rclpy.task.Future` instance """ diff --git a/rclpy/src/rclpy/_rclpy_action.c b/rclpy/src/rclpy/_rclpy_action.c index cc8c88891..0dbbed5d7 100644 --- a/rclpy/src/rclpy/_rclpy_action.c +++ b/rclpy/src/rclpy/_rclpy_action.c @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -// TODO(jacobperron): Review/complete documentation - #include #include @@ -43,13 +41,11 @@ rclpy_action_destroy_entity(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - if (!PyCapsule_CheckExact(pyentity)) { - PyErr_Format(PyExc_ValueError, "Object is not a capsule"); + rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + if (!node) { return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); - rcl_ret_t ret; if (PyCapsule_IsValid(pyentity, "rcl_action_client_t")) { rcl_action_client_t * action_client = @@ -82,22 +78,20 @@ rclpy_action_destroy_entity(PyObject * Py_UNUSED(self), PyObject * args) Py_RETURN_NONE; } -/// Fetch a predefined qos_profile from rcl_action and convert it to a Python QoSProfile Object. +/// Fetch a predefined qos_profile from rcl_action and convert it to a Python QoSProfile object. /** - * Raises RuntimeError if there is an rcl error. + * Raises RuntimeError if the QoS profile is unknown. * * This function takes a string defining a rmw_qos_profile_t and returns the * corresponding Python QoSProfile object. - * \param[in] string with the name of the profile to load. + * \param[in] pyrmw_profile String with the name of the profile to load. * \return QoSProfile object. */ static PyObject * rclpy_action_get_rmw_qos_profile(PyObject * Py_UNUSED(self), PyObject * args) { const char * pyrmw_profile; - if (!PyArg_ParseTuple( - args, "z", &pyrmw_profile)) - { + if (!PyArg_ParseTuple(args, "z", &pyrmw_profile)) { return NULL; } @@ -105,17 +99,15 @@ rclpy_action_get_rmw_qos_profile(PyObject * Py_UNUSED(self), PyObject * args) if (0 == strcmp(pyrmw_profile, "rcl_action_qos_profile_status_default")) { pyqos_profile = rclpy_convert_to_py_qos_policy((void *)&rcl_action_qos_profile_status_default); } else { - PyErr_Format(PyExc_RuntimeError, - "Requested unknown rmw_qos_profile: '%s'", pyrmw_profile); - return NULL; + return PyErr_Format(PyExc_RuntimeError, + "Requested unknown rmw_qos_profile: '%s'", pyrmw_profile); } return pyqos_profile; } /// Add an action entitiy to a wait set. /** - * ValueError if TODO - * RuntimeError if TODO + * 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. @@ -135,45 +127,36 @@ rclpy_action_wait_set_add(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } + rcl_ret_t ret; if (PyCapsule_IsValid(pyentity, "rcl_action_client_t")) { rcl_action_client_t * action_client = (rcl_action_client_t *)PyCapsule_GetPointer(pyentity, "rcl_action_client_t"); - rcl_ret_t ret = rcl_action_wait_set_add_action_client(wait_set, action_client, NULL, NULL); - - if (RCL_RET_OK != ret) { - PyErr_Format( - PyExc_RuntimeError, - "Failed to add action client to wait set: %s", - rcl_get_error_string().str); - rcl_reset_error(); - return NULL; - } + ret = rcl_action_wait_set_add_action_client(wait_set, action_client, NULL, NULL); } else if (PyCapsule_IsValid(pyentity, "rcl_action_server_t")) { rcl_action_server_t * action_server = (rcl_action_server_t *)PyCapsule_GetPointer(pyentity, "rcl_action_server_t"); - - rcl_ret_t ret = rcl_action_wait_set_add_action_server(wait_set, action_server, NULL); - - if (RCL_RET_OK != ret) { - PyErr_Format( - PyExc_RuntimeError, - "Failed to add action server to wait set: %s", - rcl_get_error_string().str); - rcl_reset_error(); - return NULL; - } + ret = rcl_action_wait_set_add_action_server(wait_set, action_server, NULL); } else { + ret = RCL_RET_ERROR; // to avoid linter warning PyErr_Format(PyExc_RuntimeError, "'%s' is not a known entity", PyCapsule_GetName(pyentity)); return NULL; } + if (RCL_RET_OK != ret) { + PyErr_Format(PyExc_RuntimeError, "Failed to add '%s' to wait set: %s", + PyCapsule_GetName(pyentity), rcl_get_error_string().str); + rcl_reset_error(); + return NULL; + } + Py_RETURN_NONE; } -/// Get number of wait set entities an action entity has +/// Get the number of wait set entities that make up an action entity. /** - * \param[in] pyentity Capsule pointer to an action entity (action client or action server). + * \param[in] pyentity Capsule pointer to an action entity + * (rcl_action_client_t or rcl_action_server_t). * \return NumberOfEntities object. */ static PyObject * @@ -191,49 +174,44 @@ rclpy_action_wait_set_get_num_entities(PyObject * Py_UNUSED(self), PyObject * ar size_t num_clients = 0u; size_t num_services = 0u; + rcl_ret_t ret; if (PyCapsule_IsValid(pyentity, "rcl_action_client_t")) { rcl_action_client_t * action_client = (rcl_action_client_t *)PyCapsule_GetPointer(pyentity, "rcl_action_client_t"); - rcl_ret_t ret = rcl_action_client_wait_set_get_num_entities( + ret = rcl_action_client_wait_set_get_num_entities( action_client, &num_subscriptions, &num_guard_conditions, &num_timers, &num_clients, &num_services); - if (RCL_RET_OK != ret) { - PyErr_Format( - PyExc_RuntimeError, - "Failed to get number of entities for action client: %s", - rcl_get_error_string().str); - rcl_reset_error(); - return NULL; - } } else if (PyCapsule_IsValid(pyentity, "rcl_action_server_t")) { rcl_action_server_t * action_server = (rcl_action_server_t *)PyCapsule_GetPointer(pyentity, "rcl_action_server_t"); - rcl_ret_t ret = rcl_action_server_wait_set_get_num_entities( + ret = rcl_action_server_wait_set_get_num_entities( action_server, &num_subscriptions, &num_guard_conditions, &num_timers, &num_clients, &num_services); - if (RCL_RET_OK != ret) { - PyErr_Format( - PyExc_RuntimeError, - "Failed to get number of entities for action server: %s", - rcl_get_error_string().str); - rcl_reset_error(); - return NULL; - } } else { + ret = RCL_RET_ERROR; // to avoid linter warning PyErr_Format(PyExc_RuntimeError, "'%s' is not a known entity", PyCapsule_GetName(pyentity)); return NULL; } + if (RCL_RET_OK != ret) { + PyErr_Format(PyExc_RuntimeError, + "Failed to get number of entities for '%s': %s", + PyCapsule_GetName(pyentity), + rcl_get_error_string().str); + rcl_reset_error(); + return NULL; + } + // Convert rcl result into Python NumberOfEntities object to return PyObject * pywaitable_module = PyImport_ImportModule("rclpy.waitable"); PyObject * pynum_of_entities_class = PyObject_GetAttrString( @@ -259,13 +237,24 @@ rclpy_action_wait_set_get_num_entities(PyObject * Py_UNUSED(self), PyObject * ar /// Check if an action entity has any ready wait set entities. /** * This must be called after waiting on the wait set. - * Raises RuntimeError if the entity type is unknown. - * Raises IndexError if the given index is beyond the number of entities in the set. + * Raises RuntimeError on failure. * - * \param[in] entity Capsule pointing to the action entity (action client or action server). + * \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. - * \param[in] pyindex Location in the wait set of the entity to check. - * \return True if the action entity has at least one sub-entity that is ready (ie. not NULL). + * \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) */ static PyObject * rclpy_action_wait_set_is_ready(PyObject * Py_UNUSED(self), PyObject * args) @@ -299,8 +288,7 @@ rclpy_action_wait_set_is_ready(PyObject * Py_UNUSED(self), PyObject * args) &is_cancel_response_ready, &is_result_response_ready); if (RCL_RET_OK != ret) { - PyErr_Format( - PyExc_RuntimeError, + PyErr_Format(PyExc_RuntimeError, "Failed to get number of ready entities for action client: %s", rcl_get_error_string().str); rcl_reset_error(); @@ -329,8 +317,7 @@ rclpy_action_wait_set_is_ready(PyObject * Py_UNUSED(self), PyObject * args) &is_result_request_ready, &is_goal_expired); if (RCL_RET_OK != ret) { - PyErr_Format( - PyExc_RuntimeError, + PyErr_Format(PyExc_RuntimeError, "Failed to get number of ready entities for action server: %s", rcl_get_error_string().str); rcl_reset_error(); @@ -367,12 +354,10 @@ rclpy_action_wait_set_is_ready(PyObject * Py_UNUSED(self), PyObject * args) * 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 list with two elements is returned: + * On a successful call a capsule referencing the created rcl_action_client_t structure + * is returned. * - * - a Capsule pointing to the pointer of the created rcl_action_client_t * structure. - * - an integer representing the memory address of the created rcl_action_client_t. - * - * Raises ValueError if the capsules are not the correct types. + * 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. @@ -384,7 +369,7 @@ rclpy_action_wait_set_is_ready(PyObject * Py_UNUSED(self), PyObject * args) * \param[in] pycancel_service_qos QoSProfile Python object for the cancel service. * \param[in] pyfeedback_qos QoSProfile Python object for the feedback subscriber. * \param[in] pystatus_qos QoSProfile Python object for the status subscriber. - * \return capsule and memory address, or + * \return Capsule named 'rcl_action_client_t', or * \return NULL on failure. */ static PyObject * @@ -416,20 +401,31 @@ rclpy_action_create_client(PyObject * Py_UNUSED(self), PyObject * args) } if (!PyUnicode_Check(pyaction_name)) { - PyErr_Format(PyExc_TypeError, "Argument pyaction_name is not a PyUnicode object"); - return NULL; + return PyErr_Format(PyExc_TypeError, "Argument pyaction_name is not a PyUnicode object"); } char * action_name = (char *)PyUnicode_1BYTE_DATA(pyaction_name); rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + if (!node) { + return NULL; + } PyObject * pymetaclass = PyObject_GetAttrString(pyaction_type, "__class__"); + if (!pymetaclass) { + return PyErr_Format(PyExc_AttributeError, "Action type has no '__class__' attribute"); + } PyObject * pyts = PyObject_GetAttrString(pymetaclass, "_TYPE_SUPPORT"); + if (!pyts) { + return PyErr_Format(PyExc_AttributeError, "Action type has no '_TYPE_SUPPORT' attribute"); + } rosidl_action_type_support_t * ts = (rosidl_action_type_support_t *)PyCapsule_GetPointer(pyts, NULL); + if (!ts) { + return NULL; + } rcl_action_client_options_t action_client_ops = rcl_action_client_get_default_options(); @@ -467,8 +463,7 @@ rclpy_action_create_client(PyObject * Py_UNUSED(self), PyObject * args) /// Check if an action server is available for the given action client. /** - * Raises ValueError if one or more capsules are not the correct type. - * Raises RuntimeError if the underlying rcl library returns an error. + * 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. @@ -519,9 +514,15 @@ rclpy_action_server_is_available(PyObject * Py_UNUSED(self), PyObject * args) return NULL; \ } \ PyObject * pyrequest_type = PyObject_GetAttrString(pyrequest, "__class__"); \ - assert(pyrequest_type != NULL); \ + if (!pyrequest_type) { \ + return PyErr_Format(PyExc_AttributeError, \ + "Action " #Type " request type has no '__class__' attribute"); \ + } \ PyObject * pymetaclass = PyObject_GetAttrString(pyrequest_type, "__class__"); \ - assert(pymetaclass != NULL); \ + if (!pymetaclass) { \ + return PyErr_Format(PyExc_AttributeError, \ + "Action " #Type " request type's metaclass has no '__class__' attribute"); \ + } \ create_ros_message_signature * create_ros_message = get_capsule_pointer( \ pymetaclass, "_CREATE_ROS_MESSAGE"); \ assert(create_ros_message != NULL && \ @@ -568,6 +569,10 @@ rclpy_action_server_is_available(PyObject * Py_UNUSED(self), PyObject * args) return NULL; \ } \ PyObject * pymetaclass = PyObject_GetAttrString(pyresponse_type, "__class__"); \ + if (!pymetaclass) { \ + return PyErr_Format(PyExc_AttributeError, \ + "Action " #Type " response type has no '__class__' attribute"); \ + } \ create_ros_message_signature * create_ros_message = get_capsule_pointer( \ pymetaclass, "_CREATE_ROS_MESSAGE"); \ assert(create_ros_message != NULL && \ @@ -621,8 +626,8 @@ rclpy_action_server_is_available(PyObject * Py_UNUSED(self), PyObject * args) /// Send an action goal request. /** - * Raises ValueError if pyaction_client does not have the correct capsule type. - * Raises RuntimeError if the underlying rcl library returns an error when sending the request. + * Raises AttributeError if there is an issue parsing the pygoal_request type. + * Raises RuntimeError on failure. * * \param[in] pyaction_client The action client to use when sending the request. * \param[in] pygoal_request The request message to send. @@ -637,7 +642,7 @@ rclpy_action_send_goal_request(PyObject * Py_UNUSED(self), PyObject * args) /// Take an action goal response. /** - * Raises ValueError if pyaction_client does not have the correct capsule type. + * Raises AttributeError if there is an issue parsing the pygoal_response type. * Raises RuntimeError if the underlying rcl library returns an error when taking the response. * * \param[in] pyaction_client The action client to use when sending the request. @@ -654,7 +659,7 @@ rclpy_action_take_goal_response(PyObject * Py_UNUSED(self), PyObject * args) /// Send an action result request. /** - * Raises ValueError if pyaction_client does not have the correct capsule type. + * Raises AttributeError if there is an issue parsing the pyresult_request type. * Raises RuntimeError if the underlying rcl library returns an error when sending the request. * * \param[in] pyaction_client The action client to use when sending the request. @@ -670,7 +675,7 @@ rclpy_action_send_result_request(PyObject * Py_UNUSED(self), PyObject * args) /// Take an action result response. /** - * Raises ValueError if pyaction_client does not have the correct capsule type. + * Raises AttributeError if there is an issue parsing the pyresult_response type. * Raises RuntimeError if the underlying rcl library returns an error when taking the response. * * \param[in] pyaction_client The action client to use when sending the request. @@ -687,7 +692,7 @@ rclpy_action_take_result_response(PyObject * Py_UNUSED(self), PyObject * args) /// Send an action cancel request. /** - * Raises ValueError if pyaction_client does not have the correct capsule type. + * Raises AttributeError if there is an issue parsing the pycancel_request type. * Raises RuntimeError if the underlying rcl library returns an error when sending the request. * * \param[in] pyaction_client The action client to use when sending the request. @@ -703,7 +708,7 @@ rclpy_action_send_cancel_request(PyObject * Py_UNUSED(self), PyObject * args) /// Take an action cancel response. /** - * Raises ValueError if pyaction_client does not have the correct capsule type. + * Raises AttributeError if there is an issue parsing the pycancel_response type. * Raises RuntimeError if the underlying rcl library returns an error when taking the response. * * \param[in] pyaction_client The action client to use when sending the request. @@ -730,14 +735,18 @@ rclpy_action_take_cancel_response(PyObject * Py_UNUSED(self), PyObject * args) return NULL; \ } \ PyObject * pymetaclass = PyObject_GetAttrString(pymsg_type, "__class__"); \ + if (!pymetaclass) { \ + return PyErr_Format(PyExc_AttributeError, \ + "Action " #Type " message type has no '__class__' attribute"); \ + } \ create_ros_message_signature * create_ros_message = get_capsule_pointer( \ pymetaclass, "_CREATE_ROS_MESSAGE"); \ assert(create_ros_message != NULL && \ - "unable to retrieve create_ros_message function, type_support mustn't have been imported"); \ + "unable to retrieve create_ros_message function, type_support must not have been imported"); \ destroy_ros_message_signature * destroy_ros_message = get_capsule_pointer( \ pymetaclass, "_DESTROY_ROS_MESSAGE"); \ assert(destroy_ros_message != NULL && \ - "unable to retrieve destroy_ros_message function, type_support mustn't have been imported"); \ + "unable to retrieve destroy_ros_message function, type_support must not have been imported"); \ void * taken_msg = create_ros_message(); \ if (!taken_msg) { \ Py_DECREF(pymetaclass); \ @@ -770,6 +779,10 @@ rclpy_action_take_cancel_response(PyObject * Py_UNUSED(self), PyObject * args) /// Take a feedback message from a given action client. /** + * Raises AttributeError if there is an issue parsing the pyfeedback_type. + * Raises RuntimeError on failure while taking a feedback message. Note, this does not include + * the case where there are no messages available. + * * \param[in] pyaction_client Capsule pointing to the action client to process the message. * \param[in] pyfeedback_type Instance of the feedback message type to take. * \return Python message with all fields populated with received message, or @@ -784,6 +797,10 @@ rclpy_action_take_feedback(PyObject * Py_UNUSED(self), PyObject * args) /// Take a status message from a given action client. /** + * Raises AttributeError if there is an issue parsing the pystatus_type. + * Raises RuntimeError on failure while taking a status message. Note, this does not include + * the case where there are no messages available. + * * \param[in] pyaction_client Capsule pointing to the action client to process the message. * \param[in] pystatus_type Instance of the status message type to take. * \return Python message with all fields populated with received message, or diff --git a/rclpy/src/rclpy/impl/common.h b/rclpy/src/rclpy/impl/common.h index 4f5f39ee7..48bcf7022 100644 --- a/rclpy/src/rclpy/impl/common.h +++ b/rclpy/src/rclpy/impl/common.h @@ -51,7 +51,8 @@ get_capsule_pointer(PyObject * pymetaclass, const char * attr) { PyObject * pyattr = PyObject_GetAttrString(pymetaclass, attr); if (!pyattr) { - return NULL; + return PyErr_Format(PyExc_AttributeError, + "Failed to get attribute '%s' in get_capsule_pointer()", attr); } void * ptr = PyCapsule_GetPointer(pyattr, NULL); Py_DECREF(pyattr);