Use the sequence number correctly when sending a request (#63)

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

Author: ivanpauno

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

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<jlong>(sequence_number);
 }
 
 JNIEXPORT void JNICALL

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<T extends ServiceDefinition> implements Client<T> {
   private final WeakReference<Node> nodeReference;
   private long handle;
   private final String serviceName;
-  private long sequenceNumber = 0;
   private Map<Long, Map.Entry<Consumer, RCLFuture>> pendingRequests;
 
   private final Class<MessageDefinition> requestType;
@@ -79,8 +79,8 @@ public void accept(Future<V> input) {}
   public final <U extends MessageDefinition, V extends MessageDefinition> Future<V>
   asyncSendRequest(final U request, final Consumer<Future<V>> callback) {
     synchronized (pendingRequests) {
-      sequenceNumber++;
-      nativeSendClientRequest(handle, sequenceNumber, request.getFromJavaConverterInstance(),
+      long sequenceNumber = nativeSendClientRequest(
+          handle, request.getFromJavaConverterInstance(),
           request.getToJavaConverterInstance(), request.getDestructorInstance(), request);
       RCLFuture<V> future = new RCLFuture<V>(this.nodeReference);
 
@@ -96,15 +96,20 @@ public final <U extends MessageDefinition> void handleResponse(
     synchronized (pendingRequests) {
       long sequenceNumber = header.sequenceNumber;
       Map.Entry<Consumer, RCLFuture> entry = pendingRequests.remove(sequenceNumber);
-      Consumer<Future> callback = entry.getKey();
-      RCLFuture<U> future = entry.getValue();
-      future.set(response);
-      callback.accept(future);
+      if (entry != null) {
+        Consumer<Future> callback = entry.getKey();
+        RCLFuture<U> 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<MessageDefinition> getRequestType() {