rclpy_take_response checks sequence number

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
 
 

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

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