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 <string>
 
 #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<rcl_node_t *>(node_handle);
+  assert(node != NULL);
+  rcl_client_t * client = reinterpret_cast<rcl_client_t *>(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/RCLJava.java b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
index 87d7ebe8..6c820ed9 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
@@ -82,31 +82,14 @@ private RCLJava() {}
 
   private static void cleanup() {
     for (Node node : nodes) {
-      for (Subscription subscription : node.getSubscriptions()) {
-        subscription.dispose();
-      }
-
-      for (Publisher publisher : node.getPublishers()) {
-        publisher.dispose();
-      }
-
-      for (Timer timer : node.getTimers()) {
-        timer.dispose();
-      }
-
-      for (Service service : node.getServices()) {
-        service.dispose();
-      }
-
-      for (Client client : node.getClients()) {
-        client.dispose();
-      }
-
       node.dispose();
     }
+    nodes.clear();
+
     for (Context context : contexts) {
       context.dispose();
     }
+    contexts.clear();
   }
 
   static {
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 @@ <U extends MessageDefinition, V extends MessageDefinition> Future<V> asyncSendRe
   <U extends MessageDefinition, V extends MessageDefinition> Future<V> asyncSendRequest(
       final U request, final Consumer<Future<V>> 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/main/java/org/ros2/rcljava/node/NodeImpl.java b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java
index 4e649913..5c11aed8 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java
@@ -26,6 +26,7 @@
 import org.ros2.rcljava.contexts.Context;
 import org.ros2.rcljava.graph.EndpointInfo;
 import org.ros2.rcljava.graph.NameAndTypes;
+import org.ros2.rcljava.interfaces.Disposable;
 import org.ros2.rcljava.interfaces.MessageDefinition;
 import org.ros2.rcljava.interfaces.ServiceDefinition;
 import org.ros2.rcljava.node.NodeOptions;
@@ -418,10 +419,26 @@ public final Collection<Client> getClients() {
    */
   private static native void nativeDispose(long handle);
 
+  private <T extends Disposable> void cleanupDisposables(Collection<T> disposables) {
+    for (Disposable disposable : disposables) {
+      disposable.dispose();
+    }
+    disposables.clear();
+  }
+
+  private void cleanup() {
+    cleanupDisposables(subscriptions);
+    cleanupDisposables(publishers);
+    cleanupDisposables(timers);
+    cleanupDisposables(services);
+    cleanupDisposables(clients);
+  }
+
   /**
    * {@inheritDoc}
    */
   public final void dispose() {
+    cleanup();
     nativeDispose(this.handle);
     this.handle = 0;
   }
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<rcljava.srv.AddTwoInts_Response> future =
+    RCLFuture<rcljava.srv.AddTwoInts_Response> consumerFuture =
         new RCLFuture<rcljava.srv.AddTwoInts_Response>(new WeakReference<Node>(node));
 
-    TestClientConsumer clientConsumer = new TestClientConsumer(future);
+    TestClientConsumer clientConsumer = new TestClientConsumer(consumerFuture);
 
     Service<rcljava.srv.AddTwoInts> service = node.<rcljava.srv.AddTwoInts>createService(
         rcljava.srv.AddTwoInts.class, "add_two_ints", clientConsumer);
@@ -96,12 +99,19 @@ public final void testAdd() throws Exception {
     Client<rcljava.srv.AddTwoInts> client =
         node.<rcljava.srv.AddTwoInts>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<rcljava.srv.AddTwoInts_Response> 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();
diff --git a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java
index 21573efb..358c9e7f 100644
--- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java
+++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java
@@ -858,7 +858,7 @@ public Node getNode() {
     executor.addNode(composableSubscriptionNodeOne);
     executor.addNode(composableSubscriptionNodeTwo);
 
-    while (RCLJava.ok() && !futureOne.isDone() && !futureTwo.isDone()) {
+    while (RCLJava.ok() && !(futureOne.isDone() && futureTwo.isDone())) {
       publisher.publish(msg);
       executor.spinSome();
     }
diff --git a/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java
index a458c6a2..67154902 100644
--- a/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java
+++ b/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java
@@ -37,6 +37,7 @@
 import org.ros2.rcljava.concurrent.RCLFuture;
 import org.ros2.rcljava.consumers.Consumer;
 import org.ros2.rcljava.node.Node;
+import org.ros2.rcljava.node.NodeOptions;
 import org.ros2.rcljava.parameters.ParameterVariant;
 import org.ros2.rcljava.parameters.client.AsyncParametersClient;
 import org.ros2.rcljava.parameters.client.AsyncParametersClientImpl;
@@ -74,7 +75,9 @@ public static void setupOnce() throws Exception {
 
   @Before
   public void setUp() throws Exception {
-    node = RCLJava.createNode("test_node");
+    NodeOptions opts = new NodeOptions();
+    opts.setAllowUndeclaredParameters(true);
+    node = RCLJava.createNode("test_node", "", opts);
     parameterService = new ParameterServiceImpl(node);
     parametersClient = new AsyncParametersClientImpl(node);
   }
diff --git a/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java
index 8ec4266a..8248f1dd 100644
--- a/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java
+++ b/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java
@@ -37,6 +37,7 @@
 import org.ros2.rcljava.concurrent.RCLFuture;
 import org.ros2.rcljava.consumers.Consumer;
 import org.ros2.rcljava.node.Node;
+import org.ros2.rcljava.node.NodeOptions;
 import org.ros2.rcljava.parameters.ParameterVariant;
 import org.ros2.rcljava.parameters.client.SyncParametersClient;
 import org.ros2.rcljava.parameters.client.SyncParametersClientImpl;
@@ -57,7 +58,9 @@ public static void setupOnce() throws Exception {
 
   @Before
   public void setUp() throws Exception {
-    node = RCLJava.createNode("test_node");
+    NodeOptions opts = new NodeOptions();
+    opts.setAllowUndeclaredParameters(true);
+    node = RCLJava.createNode("test_node", "", opts);
     parameterService = new ParameterServiceImpl(node);
     parametersClient = new SyncParametersClientImpl(node);
   }
diff --git a/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake b/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake
index b6779281..e8c9e433 100644
--- a/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake
+++ b/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake
@@ -81,11 +81,23 @@ foreach(_abs_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES})
       "${_output_path}/${_parent_folder}/${_idl_name}_Goal.java"
       "${_output_path}/${_parent_folder}/${_idl_name}_Result.java"
       "${_output_path}/${_parent_folder}/${_idl_name}_Feedback.java"
+      "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal.java"
+      "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal_Request.java"
+      "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal_Response.java"
+      "${_output_path}/${_parent_folder}/${_idl_name}_GetResult.java"
+      "${_output_path}/${_parent_folder}/${_idl_name}_GetResult_Request.java"
+      "${_output_path}/${_parent_folder}/${_idl_name}_GetResult_Response.java"
     )
     foreach(_typesupport_impl ${_typesupport_impls})
       list(APPEND _generated_extension_${_typesupport_impl}_files "${_output_path}/${_parent_folder}/${_idl_name}_Goal.ep.${_typesupport_impl}.cpp")
       list(APPEND _generated_extension_${_typesupport_impl}_files "${_output_path}/${_parent_folder}/${_idl_name}_Result.ep.${_typesupport_impl}.cpp")
       list(APPEND _generated_extension_${_typesupport_impl}_files "${_output_path}/${_parent_folder}/${_idl_name}_Feedback.ep.${_typesupport_impl}.cpp")
+      list(APPEND _generated_extension_${_typesupport_impl}_files "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal.ep.${_typesupport_impl}.cpp")
+      list(APPEND _generated_extension_${_typesupport_impl}_files "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal_Request.ep.${_typesupport_impl}.cpp")
+      list(APPEND _generated_extension_${_typesupport_impl}_files "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal_Response.ep.${_typesupport_impl}.cpp")
+      list(APPEND _generated_extension_${_typesupport_impl}_files "${_output_path}/${_parent_folder}/${_idl_name}_GetResult.ep.${_typesupport_impl}.cpp")
+      list(APPEND _generated_extension_${_typesupport_impl}_files "${_output_path}/${_parent_folder}/${_idl_name}_GetResult_Request.ep.${_typesupport_impl}.cpp")
+      list(APPEND _generated_extension_${_typesupport_impl}_files "${_output_path}/${_parent_folder}/${_idl_name}_GetResult_Response.ep.${_typesupport_impl}.cpp")
     endforeach()
   endif()
 
diff --git a/rosidl_generator_java/resource/action.cpp.em b/rosidl_generator_java/resource/action.cpp.em
index 0abc7138..0d3492e4 100644
--- a/rosidl_generator_java/resource/action.cpp.em
+++ b/rosidl_generator_java/resource/action.cpp.em
@@ -58,21 +58,28 @@ expand_template(
     data,
     output_file)
 
+data = {
+    'package_name': package_name,
+    'output_dir': output_dir,
+    'template_basepath': template_basepath,
+    'typesupport_impl': typesupport_impl,
+}
+
 # Generate SendGoal service type
 data.update({'service': action.send_goal_service})
 output_file = os.path.join(
     output_dir, *namespaces[1:], '{0}.ep.{1}.cpp'.format(send_goal_type_name, typesupport_impl))
 expand_template(
-    'msg.cpp.em',
+    'srv.cpp.em',
     data,
     output_file)
 
-# Generate SendGoal service type
+# Generate GetResult service type
 data.update({'service': action.get_result_service})
 output_file = os.path.join(
     output_dir, *namespaces[1:], '{0}.ep.{1}.cpp'.format(get_result_type_name, typesupport_impl))
 expand_template(
-    'msg.cpp.em',
+    'srv.cpp.em',
     data,
     output_file)
 }@
diff --git a/rosidl_generator_java/resource/action.java.em b/rosidl_generator_java/resource/action.java.em
index 070ddd61..5e7ebbe9 100644
--- a/rosidl_generator_java/resource/action.java.em
+++ b/rosidl_generator_java/resource/action.java.em
@@ -13,6 +13,8 @@ type_name = action.namespaced_type.name
 goal_type_name = action.goal.structure.namespaced_type.name
 result_type_name = action.result.structure.namespaced_type.name
 feedback_type_name = action.feedback.structure.namespaced_type.name
+send_goal_type_name = action.send_goal_service.namespaced_type.name
+get_result_type_name = action.get_result_service.namespaced_type.name
 
 data = {
     'package_name': package_name,
@@ -44,6 +46,22 @@ expand_template(
     output_file,
     template_basepath=template_basepath)
 
+data.update({'service': action.send_goal_service})
+output_file = os.path.join(output_dir, *namespaces[1:], send_goal_type_name + '.java')
+expand_template(
+    'srv.java.em',
+    data,
+    output_file,
+    template_basepath=template_basepath)
+
+data.update({'service': action.get_result_service})
+output_file = os.path.join(output_dir, *namespaces[1:], get_result_type_name + '.java')
+expand_template(
+    'srv.java.em',
+    data,
+    output_file,
+    template_basepath=template_basepath)
+
 action_imports = [
     'org.ros2.rcljava.common.JNIUtils',
     'org.ros2.rcljava.interfaces.ActionDefinition',
diff --git a/rosidl_generator_java/resource/msg.cpp.em b/rosidl_generator_java/resource/msg.cpp.em
index 6810a67c..9458b285 100644
--- a/rosidl_generator_java/resource/msg.cpp.em
+++ b/rosidl_generator_java/resource/msg.cpp.em
@@ -62,6 +62,7 @@ for member in message.structure.members:
         namespaced_types.add(get_jni_type(type_))
         include_prefix = idl_structure_type_to_c_include_prefix(type_)
         # TODO(jacobperron): Remove this logic after https://github.com/ros2/rosidl/pull/432 (Foxy)
+        #                    and https://github.com/ros2/rosidl/pull/538
         # Strip off any service or action suffix
         # There are several types that actions and services are composed of, but they are included
         # a common header that is based on the action or service name
@@ -76,10 +77,15 @@ for member in message.structure.members:
             include_prefix = include_prefix[:-8]
         elif include_prefix.endswith('__feedback'):
             include_prefix = include_prefix[:-10]
+        elif include_prefix.endswith('__send_goal'):
+            include_prefix = include_prefix[:-11]
+        elif include_prefix.endswith('__get_result'):
+            include_prefix = include_prefix[:-12]
         member_includes.add(include_prefix + '.h')
 }@
 @{
 # TODO(jacobperron): Remove this logic after https://github.com/ros2/rosidl/pull/432 (Foxy)
+#                    and https://github.com/ros2/rosidl/pull/538
 message_c_include_prefix = idl_structure_type_to_c_include_prefix(message.structure.namespaced_type)
 # Strip off any service or action suffix
 if message_c_include_prefix.endswith('__request'):
@@ -92,6 +98,10 @@ elif message_c_include_prefix.endswith('__result'):
     message_c_include_prefix = message_c_include_prefix[:-8]
 elif message_c_include_prefix.endswith('__feedback'):
     message_c_include_prefix = message_c_include_prefix[:-10]
+elif message_c_include_prefix.endswith('__send_goal'):
+    message_c_include_prefix = message_c_include_prefix[:-11]
+elif message_c_include_prefix.endswith('__get_result'):
+    message_c_include_prefix = message_c_include_prefix[:-12]
 }@
 
 #include <jni.h>
diff --git a/rosidl_generator_java/resource/srv.cpp.em b/rosidl_generator_java/resource/srv.cpp.em
index f22bf408..4c13f6b5 100644
--- a/rosidl_generator_java/resource/srv.cpp.em
+++ b/rosidl_generator_java/resource/srv.cpp.em
@@ -40,7 +40,20 @@ expand_template(
 
 #include "rosidl_runtime_c/service_type_support_struct.h"
 
-#include "@(idl_structure_type_to_c_include_prefix(service.namespaced_type)).h"
+@{
+include_prefix = idl_structure_type_to_c_include_prefix(service.namespaced_type)
+# TODO(jacobperron): Remove this logic after https://github.com/ros2/rosidl/pull/538
+# Strip off any service suffix
+# There are a couple service types that actions are composed of, but they are included
+# a common header that is based on the action name
+# ie. there are not separate headers for each type
+if include_prefix.endswith('__send_goal'):
+    include_prefix = include_prefix[:-11]
+elif include_prefix.endswith('__get_result'):
+    include_prefix = include_prefix[:-12]
+}@
+
+#include "@(include_prefix).h"
 
 // Ensure that a jlong is big enough to store raw pointers
 static_assert(sizeof(jlong) >= sizeof(std::intptr_t), "jlong must be able to store pointers");