diff --git a/rcljava/include/org_ros2_rcljava_client_ClientImpl.h b/rcljava/include/org_ros2_rcljava_client_ClientImpl.h index c59635e0..9a3b20e5 100644 --- a/rcljava/include/org_ros2_rcljava_client_ClientImpl.h +++ b/rcljava/include/org_ros2_rcljava_client_ClientImpl.h @@ -25,9 +25,9 @@ extern "C" { * Method: nativeSendClientRequest * Signature: (JJJJJLorg/ros2/rcljava/interfaces/MessageDefinition;)V */ -JNIEXPORT void +JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_client_ClientImpl_nativeSendClientRequest( - JNIEnv *, jclass, jlong, jlong, jlong, jlong, jlong, jobject); + JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject); /* * Class: org_ros2_rcljava_client_ClientImpl diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp index 33646342..842b7d99 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp @@ -34,9 +34,9 @@ using rcljava_common::exceptions::rcljava_throw_rclexception; using rcljava_common::signatures::convert_from_java_signature; using rcljava_common::signatures::destroy_ros_message_signature; -JNIEXPORT void JNICALL +JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_client_ClientImpl_nativeSendClientRequest( - JNIEnv * env, jclass, jlong client_handle, jlong sequence_number, + JNIEnv * env, jclass, jlong client_handle, jlong jrequest_from_java_converter_handle, jlong jrequest_to_java_converter_handle, jlong jrequest_destructor_handle, jobject jrequest_msg) { @@ -53,6 +53,7 @@ Java_org_ros2_rcljava_client_ClientImpl_nativeSendClientRequest( void * request_msg = convert_from_java(jrequest_msg, nullptr); + int64_t sequence_number; rcl_ret_t ret = rcl_send_request(client, request_msg, &sequence_number); destroy_ros_message_signature destroy_ros_message = @@ -65,6 +66,7 @@ Java_org_ros2_rcljava_client_ClientImpl_nativeSendClientRequest( rcl_reset_error(); rcljava_throw_rclexception(env, ret, msg); } + return static_cast(sequence_number); } JNIEXPORT void JNICALL diff --git a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java index b8274cec..c5189b49 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java @@ -17,6 +17,7 @@ import java.time.Duration; import java.lang.ref.WeakReference; +import java.lang.IllegalStateException; import java.lang.InterruptedException; import java.lang.Long; import java.util.AbstractMap; @@ -52,7 +53,6 @@ public class ClientImpl implements Client { private final WeakReference nodeReference; private long handle; private final String serviceName; - private long sequenceNumber = 0; private Map> pendingRequests; private final Class requestType; @@ -79,8 +79,8 @@ public void accept(Future input) {} public final Future asyncSendRequest(final U request, final Consumer> callback) { synchronized (pendingRequests) { - sequenceNumber++; - nativeSendClientRequest(handle, sequenceNumber, request.getFromJavaConverterInstance(), + long sequenceNumber = nativeSendClientRequest( + handle, request.getFromJavaConverterInstance(), request.getToJavaConverterInstance(), request.getDestructorInstance(), request); RCLFuture future = new RCLFuture(this.nodeReference); @@ -96,15 +96,20 @@ public final void handleResponse( synchronized (pendingRequests) { long sequenceNumber = header.sequenceNumber; Map.Entry entry = pendingRequests.remove(sequenceNumber); - Consumer callback = entry.getKey(); - RCLFuture future = entry.getValue(); - future.set(response); - callback.accept(future); + if (entry != null) { + Consumer callback = entry.getKey(); + RCLFuture future = entry.getValue(); + future.set(response); + callback.accept(future); + return; + } + throw new IllegalStateException( + "No request made with the given sequence number: " + sequenceNumber); } } - private static native void nativeSendClientRequest(long handle, long sequenceNumber, - long requestFromJavaConverterHandle, long requestToJavaConverterHandle, + private static native long nativeSendClientRequest( + long handle, long requestFromJavaConverterHandle, long requestToJavaConverterHandle, long requestDestructorHandle, MessageDefinition requestMessage); public final Class getRequestType() {