Skip to content

Commit

Permalink
Fix rclpy_take_response sequence number bug
Browse files Browse the repository at this point in the history
rcl_take_response outputs the sequence number. rclpy_take_response was
incorrectly assuming that the sequence number was an input. This fixes
multiple pending requests getting mismatched.
  • Loading branch information
sloretz committed Jan 18, 2018
1 parent 4a3482b commit 93bbea4
Show file tree
Hide file tree
Showing 2 changed files with 21 additions and 9 deletions.
8 changes: 4 additions & 4 deletions rclpy/rclpy/executors.py
Original file line number Diff line number Diff line change
Expand Up @@ -219,10 +219,10 @@ async def _execute_subscription(self, sub, msg):

def _take_client(self, client):
for sequence, future in client._pending_requests.items():
response = _rclpy.rclpy_take_response(
client.client_handle, client.srv_type.Response, sequence)
if response is not None:
return sequence, response
seq_and_response = _rclpy.rclpy_take_response(
client.client_handle, client.srv_type.Response)
if seq_and_response is not None:
return seq_and_response
# The request was cancelled
return None, None

Expand Down
22 changes: 17 additions & 5 deletions rclpy/src/rclpy/_rclpy.c
Original file line number Diff line number Diff line change
Expand Up @@ -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 if there is no response to take
*/
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 =
Expand Down Expand Up @@ -2209,8 +2208,8 @@ 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);

if (ret != RCL_RET_CLIENT_TAKE_FAILED) {
Expand All @@ -2227,7 +2226,20 @@ rclpy_take_response(PyObject * Py_UNUSED(self), PyObject * args)
return NULL;
}

return pytaken_response;
PyObject * pysequence = PyLong_FromLongLong(sequence);
if (!pysequence) {
Py_DECREF(pytaken_response);
return NULL;
}
PyObject * pytuple = PyTuple_New(2);
if (!pytuple) {
Py_DECREF(pysequence);
Py_DECREF(pytaken_response);
return NULL;
}
PyTuple_SET_ITEM(pytuple, 0, pysequence);
PyTuple_SET_ITEM(pytuple, 1, pytaken_response);
return pytuple;
}
// if take_response failed, just do nothing
Py_RETURN_NONE;
Expand Down

0 comments on commit 93bbea4

Please sign in to comment.