diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index 9fa10643..a2a5862f 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -264,14 +264,14 @@ if(BUILD_TESTING) "src/test/java/org/ros2/rcljava/RCLJavaTest.java" "src/test/java/org/ros2/rcljava/SpinTest.java" "src/test/java/org/ros2/rcljava/TimeTest.java" - # "src/test/java/org/ros2/rcljava/client/ClientTest.java" + "src/test/java/org/ros2/rcljava/client/ClientTest.java" "src/test/java/org/ros2/rcljava/contexts/ContextTest.java" "src/test/java/org/ros2/rcljava/node/NodeOptionsTest.java" "src/test/java/org/ros2/rcljava/node/NodeParametersTest.java" "src/test/java/org/ros2/rcljava/node/NodeUndeclaredParametersTest.java" "src/test/java/org/ros2/rcljava/node/NodeTest.java" - # "src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java" - # "src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java" + "src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java" + "src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java" "src/test/java/org/ros2/rcljava/publisher/PublisherTest.java" "src/test/java/org/ros2/rcljava/qos/QoSProfileTest.java" "src/test/java/org/ros2/rcljava/subscription/SubscriptionTest.java" @@ -283,13 +283,13 @@ if(BUILD_TESTING) "org.ros2.rcljava.RCLJavaTest" "org.ros2.rcljava.SpinTest" "org.ros2.rcljava.TimeTest" - # "org.ros2.rcljava.client.ClientTest" + "org.ros2.rcljava.client.ClientTest" "org.ros2.rcljava.contexts.ContextTest" "org.ros2.rcljava.node.NodeOptionsTest" "org.ros2.rcljava.node.NodeParametersTest" "org.ros2.rcljava.node.NodeUndeclaredParametersTest" "org.ros2.rcljava.node.NodeTest" - # "org.ros2.rcljava.parameters.SyncParametersClientTest" + "org.ros2.rcljava.parameters.SyncParametersClientTest" "org.ros2.rcljava.publisher.PublisherTest" "org.ros2.rcljava.qos.QoSProfileTest" "org.ros2.rcljava.subscription.SubscriptionTest" diff --git a/rcljava/include/org_ros2_rcljava_client_ClientImpl.h b/rcljava/include/org_ros2_rcljava_client_ClientImpl.h index ab71b022..c59635e0 100644 --- a/rcljava/include/org_ros2_rcljava_client_ClientImpl.h +++ b/rcljava/include/org_ros2_rcljava_client_ClientImpl.h @@ -37,6 +37,16 @@ JNICALL Java_org_ros2_rcljava_client_ClientImpl_nativeSendClientRequest( JNIEXPORT void JNICALL Java_org_ros2_rcljava_client_ClientImpl_nativeDispose(JNIEnv *, jclass, jlong, jlong); +/* + * Class: org_ros2_rcljava_client_ClientImpl + * Method: nativeIsServiceAvailable + * Signature: (JJ)Z + */ +JNIEXPORT jboolean +JNICALL Java_org_ros2_rcljava_client_ClientImpl_nativeIsServiceAvailable( + JNIEnv *, jclass, jlong, jlong); + + #ifdef __cplusplus } #endif 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 f950506f..33646342 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp @@ -19,6 +19,7 @@ #include #include "rcl/error_handling.h" +#include "rcl/graph.h" #include "rcl/node.h" #include "rcl/rcl.h" #include "rmw/rmw.h" @@ -96,3 +97,29 @@ Java_org_ros2_rcljava_client_ClientImpl_nativeDispose( rcljava_throw_rclexception(env, ret, msg); } } + +JNIEXPORT jboolean JNICALL +Java_org_ros2_rcljava_client_ClientImpl_nativeIsServiceAvailable( + JNIEnv * env, jclass, jlong node_handle, jlong client_handle) +{ + rcl_node_t * node = reinterpret_cast(node_handle); + assert(node != NULL); + rcl_client_t * client = reinterpret_cast(client_handle); + assert(client != NULL); + + bool is_ready; + rcl_ret_t ret = rcl_service_server_is_available(node, client, &is_ready); + if (RCL_RET_NODE_INVALID == ret) { + if (node && !rcl_context_is_valid(node->context)) { + // context is shutdown, do a soft failure + return false; + } + } + if (ret != RCL_RET_OK) { + std::string msg = + "Failed to check if service is available: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } + return is_ready; +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/client/Client.java b/rcljava/src/main/java/org/ros2/rcljava/client/Client.java index 79ef137e..774ecec6 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/client/Client.java +++ b/rcljava/src/main/java/org/ros2/rcljava/client/Client.java @@ -15,6 +15,7 @@ package org.ros2.rcljava.client; +import java.time.Duration; import java.util.concurrent.Future; import org.ros2.rcljava.concurrent.RCLFuture; @@ -37,5 +38,34 @@ Future asyncSendRe Future asyncSendRequest( final U request, final Consumer> callback); + /** + * Check if the service server is available. + * + * @return true if the client can talk to the service, false otherwise. + */ + boolean isServiceAvailable(); + + /** + * Wait for the service server to be available. + * + * Blocks until the service is available or the ROS context is invalidated. + * + * @return true if the service is available, false if the ROS context was shutdown. + */ + boolean waitForService(); + + /** + * Wait for the service server to be available. + * + * Blocks until the service is available or a timeout occurs. + * Also returns if the ROS context is invalidated. + * + * @param timeout Time to wait for the service to be available. + * A zero value causes this method to check if the service is available and return immediately. + * A negative value is treated as an infinite timeout. + * @return true if the service is available, false otherwise. + */ + boolean waitForService(Duration timeout); + String getServiceName(); } 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 1269de83..b8274cec 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java @@ -15,11 +15,15 @@ package org.ros2.rcljava.client; +import java.time.Duration; import java.lang.ref.WeakReference; +import java.lang.InterruptedException; +import java.lang.Long; import java.util.AbstractMap; import java.util.HashMap; import java.util.Map; import java.util.concurrent.Future; +import java.util.concurrent.TimeUnit; import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.common.JNIUtils; @@ -142,6 +146,58 @@ public final long getHandle() { return this.handle; } + private static native boolean nativeIsServiceAvailable(long nodeHandle, long handle); + + /** + * {@inheritDoc} + */ + public boolean isServiceAvailable() { + Node node = this.nodeReference.get(); + if (node == null) { + return false; + } + return nativeIsServiceAvailable(node.getHandle(), this.handle); + } + + /** + * {@inheritDoc} + */ + public final boolean waitForService() { + return waitForService(Duration.ofNanos(-1)); + } + + /** + * {@inheritDoc} + */ + public final boolean waitForService(Duration timeout) { + long timeoutNano = timeout.toNanos(); + if (0L == timeoutNano) { + return isServiceAvailable(); + } + long startTime = System.nanoTime(); + long timeToWait = (timeoutNano >= 0L) ? timeoutNano : Long.MAX_VALUE; + while (RCLJava.ok() && timeToWait > 0L) { + // TODO(jacobperron): Wake up whenever graph changes instead of sleeping for a fixed duration + try { + TimeUnit.MILLISECONDS.sleep(10); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + return false; + } + + if (isServiceAvailable()) { + return true; + } + + // If timeout is negative, timeToWait will always be greater than zero + if (timeoutNano > 0L) { + timeToWait = timeoutNano - (System.nanoTime() - startTime); + } + } + + return false; + } + public String getServiceName() { return this.serviceName; } diff --git a/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java index dd41e228..df7d2a6c 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java @@ -26,9 +26,12 @@ import org.junit.Test; import java.lang.ref.WeakReference; +import java.time.Duration; import java.util.Arrays; import java.util.List; +import java.util.concurrent.Future; +import java.util.concurrent.TimeUnit; import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.concurrent.RCLFuture; @@ -81,10 +84,10 @@ public static void tearDownOnce() { @Test public final void testAdd() throws Exception { - RCLFuture future = + RCLFuture consumerFuture = new RCLFuture(new WeakReference(node)); - TestClientConsumer clientConsumer = new TestClientConsumer(future); + TestClientConsumer clientConsumer = new TestClientConsumer(consumerFuture); Service service = node.createService( rcljava.srv.AddTwoInts.class, "add_two_ints", clientConsumer); @@ -96,12 +99,19 @@ public final void testAdd() throws Exception { Client client = node.createClient(rcljava.srv.AddTwoInts.class, "add_two_ints"); - while (RCLJava.ok() && !future.isDone()) { - client.asyncSendRequest(request); - RCLJava.spinOnce(node); - } + assertTrue(client.waitForService(Duration.ofSeconds(10))); + + Future responseFuture = client.asyncSendRequest(request); + + rcljava.srv.AddTwoInts_Response response = responseFuture.get(10, TimeUnit.SECONDS); + + // Check that the message was received by the service + assertTrue(consumerFuture.isDone()); + + // Check the contents of the response + assertEquals(5, response.getSum()); - assertEquals(5, future.get().getSum()); + // Cleanup client.dispose(); assertEquals(0, client.getHandle()); service.dispose();