diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py index 4d70be100..bd1d5dbc4 100644 --- a/rclpy/rclpy/client.py +++ b/rclpy/rclpy/client.py @@ -44,11 +44,10 @@ def run(self): if sigint_gc_handle in guard_condition_ready_list: rclpy.utilities.shutdown() return - response = _rclpy.rclpy_take_response( + seq, response = _rclpy.rclpy_take_response( self.client.client_handle, - self.client.srv_type.Response, - self.client.sequence_number) - if response: + self.client.srv_type.Response) + if seq is not None and seq == self.client.sequence_number: self.client.response = response diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index 79422a032..0dd46638d 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -218,12 +218,11 @@ async def _execute_subscription(self, sub, msg): await await_or_execute(sub.callback, msg) def _take_client(self, client): - response = _rclpy.rclpy_take_response( - client.client_handle, client.srv_type.Response, client.sequence_number) - return response + return _rclpy.rclpy_take_response(client.client_handle, client.srv_type.Response) - async def _execute_client(self, client, response): - if response: + async def _execute_client(self, client, seq_and_response): + sequence, response = seq_and_response + if sequence is not None and sequence == client.sequence_number: # clients spawn their own thread to wait for a response in the # wait_for_future function. Users can either use this mechanism or monitor # the content of client.response to check if a response has been received diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c index 729b0e6bd..60e0a0519 100644 --- a/rclpy/src/rclpy/_rclpy.c +++ b/rclpy/src/rclpy/_rclpy.c @@ -2164,16 +2164,15 @@ rclpy_take_request(PyObject * Py_UNUSED(self), PyObject * args) * * \param[in] pyclient Capsule pointing to the client to process the response * \param[in] pyresponse_type Instance of the message type to take - * \return Python response message with all fields populated with received response + * \return 2-tuple sequence number and received response or None, None if there is no response */ static PyObject * rclpy_take_response(PyObject * Py_UNUSED(self), PyObject * args) { PyObject * pyclient; PyObject * pyresponse_type; - PY_LONG_LONG sequence_number; - if (!PyArg_ParseTuple(args, "OOK", &pyclient, &pyresponse_type, &sequence_number)) { + if (!PyArg_ParseTuple(args, "OO", &pyclient, &pyresponse_type)) { return NULL; } rcl_client_t * client = @@ -2209,10 +2208,16 @@ rclpy_take_response(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } rmw_request_id_t * header = (rmw_request_id_t *)PyMem_Malloc(sizeof(rmw_request_id_t)); - header->sequence_number = sequence_number; rcl_ret_t ret = rcl_take_response(client, header, taken_response); + int64_t sequence = header->sequence_number; PyMem_Free(header); + // Create the tuple to return + PyObject * pytuple = PyTuple_New(2); + if (!pytuple) { + return NULL; + } + if (ret != RCL_RET_CLIENT_TAKE_FAILED) { PyObject * pyconvert_to_py = PyObject_GetAttrString(pyresponse_type, "_CONVERT_TO_PY"); @@ -2224,13 +2229,25 @@ rclpy_take_response(PyObject * Py_UNUSED(self), PyObject * args) destroy_ros_message(taken_response); if (!pytaken_response) { // the function has set the Python error + Py_DECREF(pytuple); return NULL; } - return pytaken_response; - } - // if take_response failed, just do nothing - Py_RETURN_NONE; + PyObject * pysequence = PyLong_FromLongLong(sequence); + if (!pysequence) { + Py_DECREF(pytaken_response); + Py_DECREF(pytuple); + return NULL; + } + PyTuple_SET_ITEM(pytuple, 0, pysequence); + PyTuple_SET_ITEM(pytuple, 1, pytaken_response); + return pytuple; + } + Py_INCREF(Py_None); + PyTuple_SET_ITEM(pytuple, 0, Py_None); + Py_INCREF(Py_None); + PyTuple_SET_ITEM(pytuple, 1, Py_None); + return pytuple; } /// Status of the the client library