diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt
index a1658d5..d000a38 100644
--- a/rcl/CMakeLists.txt
+++ b/rcl/CMakeLists.txt
@@ -12,8 +12,8 @@ find_package(rosidl_generator_c REQUIRED)
 include_directories(include)
 
 if(NOT WIN32)
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra")
   set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra")
 endif()
 
 set(${PROJECT_NAME}_sources
@@ -47,6 +47,12 @@ macro(target)
     ARCHIVE DESTINATION lib
     LIBRARY DESTINATION lib
     RUNTIME DESTINATION bin)
+
+  # rcl_lib_dir is passed as APPEND_LIBRARY_DIRS for each ament_add_gtest call so
+  # the librcl that they link against is on the library path.
+  # This is especially important on Windows.
+  # This is overwritten each loop, but which one it points to doesn't really matter.
+  set(rcl_lib_dir "$<TARGET_FILE_DIR:${PROJECT_NAME}${target_suffix}>")
 endmacro()
 
 call_for_each_rmw_implementation(target GENERATE_DEFAULT)
diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt
index a3d8edc..f586b27 100644
--- a/rcl/test/CMakeLists.txt
+++ b/rcl/test/CMakeLists.txt
@@ -1,117 +1,91 @@
 find_package(ament_cmake_gtest REQUIRED)
+find_package(std_msgs REQUIRED)
+
+include(rcl_add_custom_gtest.cmake)
 
 set(extra_test_libraries)
-set(extra_memory_tools_env PHONY=)  # Use a phony env var so there is always at least one.
-# rcl_lib_dir is passed as APPEND_LIBRARY_DIRS for each ament_add_gtest call so
-# the librcl that they link against is on the library path.
-# This is especially improtant on Windows.
-set(rcl_lib_dir $<TARGET_FILE_DIR:${PROJECT_NAME}>)
-ament_find_gtest()  # For GTEST_LIBRARIES
-if(APPLE)
-  add_library(${PROJECT_NAME}_memory_tools_interpose SHARED memory_tools_osx_interpose.cpp)
-  target_link_libraries(${PROJECT_NAME}_memory_tools_interpose ${GTEST_LIBRARIES})
-  set_target_properties(${PROJECT_NAME}_memory_tools_interpose
-    PROPERTIES COMPILE_FLAGS "-std=c++11")
-  list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools_interpose)
-  list(APPEND extra_memory_tools_env
-    DYLD_INSERT_LIBRARIES=$<TARGET_FILE:${PROJECT_NAME}_memory_tools_interpose>)
-endif()
-add_library(${PROJECT_NAME}_memory_tools SHARED memory_tools.cpp)
-if(NOT WIN32)
-  set_target_properties(${PROJECT_NAME}_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11")
-endif()
-if(UNIX AND NOT APPLE)
-  list(APPEND extra_test_libraries dl)
-  list(APPEND extra_memory_tools_env DL_PRELOAD=$<TARGET_FILE:${PROJECT_NAME}_memory_tools>)
-endif()
-target_link_libraries(${PROJECT_NAME}_memory_tools ${extra_test_libraries})
-target_compile_definitions(${PROJECT_NAME}_memory_tools
-  PRIVATE "RCL_MEMORY_TOOLS_BUILDING_DLL")
-list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools)
+set(extra_test_env)
 
-if(NOT WIN32)
-  ament_add_gtest(test_memory_tools test_memory_tools.cpp
-    ENV ${extra_memory_tools_env} APPEND_LIBRARY_DIRS ${rcl_lib_dir})
-  if(TARGET test_memory_tools)
-    target_include_directories(test_memory_tools PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-    )
-    if(NOT WIN32)
-      set_target_properties(test_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11")
-    endif()
-    target_link_libraries(test_memory_tools ${PROJECT_NAME} ${extra_test_libraries})
+# This subdirectory extends both extra_test_libraries and extra_test_env.
+add_subdirectory(memory_tools)
+
+macro(test_target)
+  find_package(${rmw_implementation} REQUIRED)
+  test_target_function()
+endmacro()
+
+function(test_target_function)
+  # TODO(wjwwood): remove these when C type support is supported by typesupport_introspection.
+  #                after removing, we can also recombine the function and macro
+  if("${rmw_implementation} " STREQUAL "rmw_connext_dynamic_cpp ")
+    message(STATUS "Skipping tests for '${rmw_implementation}'")
+    return()
+  endif()
+  if("${rmw_implementation} " STREQUAL "rmw_fastrtps_cpp ")
+    message(STATUS "Skipping tests for '${rmw_implementation}'")
+    return()
   endif()
-endif()
 
-ament_add_gtest(test_allocator rcl/test_allocator.cpp
-  ENV ${extra_memory_tools_env} APPEND_LIBRARY_DIRS ${rcl_lib_dir})
-if(TARGET test_allocator)
-  target_include_directories(test_allocator PUBLIC
-    ${rcl_interfaces_INCLUDE_DIRS}
-    ${rmw_INCLUDE_DIRS}
+  message(STATUS "Creating tests for '${rmw_implementation}'")
+
+  rcl_add_custom_gtest(test_allocator${target_suffix}
+    SRCS rcl/test_allocator.cpp
+    ENV ${extra_test_env}
+    APPEND_LIBRARY_DIRS "${rcl_lib_dir}"
+    LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries}
+    AMENT_DEPENDENCIES ${rmw_implementation}
   )
-  if(NOT WIN32)
-    set_target_properties(test_allocator PROPERTIES COMPILE_FLAGS "-std=c++11")
-  endif()
-  target_link_libraries(test_allocator ${PROJECT_NAME} ${extra_test_libraries})
-endif()
 
-ament_add_gtest(test_time rcl/test_time.cpp
-  ENV ${extra_memory_tools_env} APPEND_LIBRARY_DIRS ${rcl_lib_dir})
-if(TARGET test_time)
-  target_include_directories(test_time PUBLIC
-    ${rcl_interfaces_INCLUDE_DIRS}
-    ${rmw_INCLUDE_DIRS}
+  rcl_add_custom_gtest(test_time${target_suffix}
+    SRCS rcl/test_time.cpp
+    ENV ${extra_test_env}
+    APPEND_LIBRARY_DIRS "${rcl_lib_dir}"
+    LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries}
+    AMENT_DEPENDENCIES ${rmw_implementation}
   )
-  if(NOT WIN32)
-    set_target_properties(test_time PROPERTIES COMPILE_FLAGS "-std=c++11")
-  endif()
-  target_link_libraries(test_time ${PROJECT_NAME} ${extra_test_libraries})
-endif()
 
-ament_add_gtest(test_common
-  rcl/test_common.cpp
-  ENV
-    ${extra_memory_tools_env}
-    EMPTY_TEST=
-    NORMAL_TEST=foo
-  APPEND_LIBRARY_DIRS
-    ${rcl_lib_dir}
-)
-if(TARGET test_common)
-  target_include_directories(test_common PUBLIC
-    ${rcl_interfaces_INCLUDE_DIRS}
-    ${rmw_INCLUDE_DIRS}
+  rcl_add_custom_gtest(test_common${target_suffix}
+    SRCS rcl/test_common.cpp
+    ENV
+      ${extra_test_env}
+      EMPTY_TEST=
+      NORMAL_TEST=foo
+    APPEND_LIBRARY_DIRS "${rcl_lib_dir}"
+    LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries}
+    AMENT_DEPENDENCIES ${rmw_implementation}
   )
-  if(NOT WIN32)
-    set_target_properties(test_common PROPERTIES COMPILE_FLAGS "-std=c++11")
-  endif()
-  target_link_libraries(test_common ${PROJECT_NAME} ${extra_test_libraries})
-endif()
 
-ament_add_gtest(test_rcl rcl/test_rcl.cpp
-  ENV ${extra_memory_tools_env} APPEND_LIBRARY_DIRS ${rcl_lib_dir})
-if(TARGET test_rcl)
-  target_include_directories(test_rcl PUBLIC
-    ${rcl_interfaces_INCLUDE_DIRS}
-    ${rmw_INCLUDE_DIRS}
+  rcl_add_custom_gtest(test_rcl${target_suffix}
+    SRCS rcl/test_rcl.cpp
+    ENV ${extra_test_env}
+    APPEND_LIBRARY_DIRS "${rcl_lib_dir}"
+    LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries}
+    AMENT_DEPENDENCIES ${rmw_implementation}
   )
-  if(NOT WIN32)
-    set_target_properties(test_rcl PROPERTIES COMPILE_FLAGS "-std=c++11")
-  endif()
-  target_link_libraries(test_rcl ${PROJECT_NAME} ${extra_test_libraries})
-endif()
 
-ament_add_gtest(test_node rcl/test_node.cpp
-  ENV ${extra_memory_tools_env} APPEND_LIBRARY_DIRS ${rcl_lib_dir})
-if(TARGET test_node)
-  target_include_directories(test_node PUBLIC
-    ${rcl_interfaces_INCLUDE_DIRS}
-    ${rmw_INCLUDE_DIRS}
+  rcl_add_custom_gtest(test_node${target_suffix}
+    SRCS rcl/test_node.cpp
+    ENV ${extra_test_env}
+    APPEND_LIBRARY_DIRS "${rcl_lib_dir}"
+    LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries}
+    AMENT_DEPENDENCIES ${rmw_implementation}
   )
-  if(NOT WIN32)
-    set_target_properties(test_node PROPERTIES COMPILE_FLAGS "-std=c++11")
-  endif()
-  target_link_libraries(test_node ${PROJECT_NAME} ${extra_test_libraries})
-endif()
+
+  rcl_add_custom_gtest(test_publisher${target_suffix}
+    SRCS rcl/test_publisher.cpp
+    ENV ${extra_test_env}
+    APPEND_LIBRARY_DIRS "${rcl_lib_dir}"
+    LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries}
+    AMENT_DEPENDENCIES ${rmw_implementation} "std_msgs"
+  )
+
+  rcl_add_custom_gtest(test_subscription${target_suffix}
+    SRCS rcl/test_subscription.cpp
+    ENV ${extra_test_env}
+    APPEND_LIBRARY_DIRS "${rcl_lib_dir}"
+    LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries}
+    AMENT_DEPENDENCIES ${rmw_implementation} "std_msgs"
+  )
+endfunction()
+
+call_for_each_rmw_implementation(test_target)
diff --git a/rcl/test/memory_tools/CMakeLists.txt b/rcl/test/memory_tools/CMakeLists.txt
new file mode 100644
index 0000000..1f394ea
--- /dev/null
+++ b/rcl/test/memory_tools/CMakeLists.txt
@@ -0,0 +1,36 @@
+ament_find_gtest()  # For GTEST_LIBRARIES
+
+# Create the memory_tools library which is used by the tests. (rmw implementation agnostic)
+add_library(${PROJECT_NAME}_memory_tools SHARED memory_tools.cpp)
+if(APPLE)
+  # Create an OS X specific version of the memory tools that does interposing.
+  # See: http://toves.freeshell.org/interpose/
+  add_library(${PROJECT_NAME}_memory_tools_interpose SHARED memory_tools_osx_interpose.cpp)
+  target_link_libraries(${PROJECT_NAME}_memory_tools_interpose ${GTEST_LIBRARIES})
+  list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools_interpose)
+  list(APPEND extra_test_env
+    DYLD_INSERT_LIBRARIES=$<TARGET_FILE:${PROJECT_NAME}_memory_tools_interpose>)
+endif()
+if(UNIX AND NOT APPLE)
+  # On Linux like systems, add dl and use the normal library and DL_PRELOAD.
+  list(APPEND extra_test_libraries dl)
+  list(APPEND extra_test_env DL_PRELOAD=$<TARGET_FILE:${PROJECT_NAME}_memory_tools>)
+endif()
+target_link_libraries(${PROJECT_NAME}_memory_tools ${extra_test_libraries})
+target_compile_definitions(${PROJECT_NAME}_memory_tools
+  PRIVATE "RCL_MEMORY_TOOLS_BUILDING_DLL")
+list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools)
+
+# Create tests for the memory tools library.
+if(NOT WIN32)  # (memory tools doesn't do anything on Windows)
+  include(../rcl_add_custom_gtest.cmake)
+
+  rcl_add_custom_gtest(test_memory_tools
+    SRCS test_memory_tools.cpp
+    ENV ${extra_test_env}
+    LIBRARIES ${extra_test_libraries}
+  )
+endif()
+
+set(extra_test_libraries ${extra_test_libraries} PARENT_SCOPE)
+set(extra_test_env ${extra_test_env} PARENT_SCOPE)
diff --git a/rcl/test/memory_tools.cpp b/rcl/test/memory_tools/memory_tools.cpp
similarity index 95%
rename from rcl/test/memory_tools.cpp
rename to rcl/test/memory_tools/memory_tools.cpp
index 17fd508..37c7b82 100644
--- a/rcl/test/memory_tools.cpp
+++ b/rcl/test/memory_tools/memory_tools.cpp
@@ -55,14 +55,16 @@ free(void * pointer)
 
 void start_memory_checking()
 {
-  printf("starting memory checking...\n");
-  enabled.store(true);
+  if (!enabled.exchange(true)) {
+    printf("starting memory checking...\n");
+  }
 }
 
 void stop_memory_checking()
 {
-  printf("stopping memory checking...\n");
-  enabled.store(false);
+  if (enabled.exchange(false)) {
+    printf("stopping memory checking...\n");
+  }
 }
 
 /******************************************************************************
diff --git a/rcl/test/memory_tools.hpp b/rcl/test/memory_tools/memory_tools.hpp
similarity index 85%
rename from rcl/test/memory_tools.hpp
rename to rcl/test/memory_tools/memory_tools.hpp
index 001c110..ef92e4a 100644
--- a/rcl/test/memory_tools.hpp
+++ b/rcl/test/memory_tools/memory_tools.hpp
@@ -16,8 +16,8 @@
 //   https://dxr.mozilla.org/mozilla-central/rev/
 //    cc9c6cd756cb744596ba039dcc5ad3065a7cc3ea/memory/build/replace_malloc.c
 
-#ifndef MEMORY_TOOLS_HPP_
-#define MEMORY_TOOLS_HPP_
+#ifndef RCL__TEST__MEMORY_TOOLS__MEMORY_TOOLS_HPP_
+#define RCL__TEST__MEMORY_TOOLS__MEMORY_TOOLS_HPP_
 
 #include <stddef.h>
 
@@ -104,4 +104,29 @@ RCL_MEMORY_TOOLS_PUBLIC
 void
 memory_checking_thread_init();
 
-#endif  // MEMORY_TOOLS_HPP_
+// What follows is a set of failing allocator functions, used for testing.
+void *
+failing_malloc(size_t size, void * state)
+{
+  (void)size;
+  (void)state;
+  return nullptr;
+}
+
+void *
+failing_realloc(void * pointer, size_t size, void * state)
+{
+  (void)pointer;
+  (void)size;
+  (void)state;
+  return nullptr;
+}
+
+void
+failing_free(void * pointer, void * state)
+{
+  (void)pointer;
+  (void)state;
+}
+
+#endif  // RCL__TEST__MEMORY_TOOLS__MEMORY_TOOLS_HPP_
diff --git a/rcl/test/memory_tools_common.cpp b/rcl/test/memory_tools/memory_tools_common.cpp
similarity index 90%
rename from rcl/test/memory_tools_common.cpp
rename to rcl/test/memory_tools/memory_tools_common.cpp
index c7ea833..8f8d51f 100644
--- a/rcl/test/memory_tools_common.cpp
+++ b/rcl/test/memory_tools/memory_tools_common.cpp
@@ -25,7 +25,7 @@
 #endif  // defined(__APPLE__)
 
 #include "./memory_tools.hpp"
-#include "./scope_exit.hpp"
+#include "../scope_exit.hpp"
 
 static std::atomic<bool> enabled(false);
 
@@ -65,9 +65,11 @@ custom_malloc(size_t size)
   }
   void * memory = malloc(size);
   uint64_t fw_size = size;
-  MALLOC_PRINTF(
-    " malloc (%s) %p %" PRIu64 "\n",
-    malloc_expected ? "    expected" : "not expected", memory, fw_size);
+  if (!malloc_expected) {
+    MALLOC_PRINTF(
+      " malloc (%s) %p %" PRIu64 "\n",
+      malloc_expected ? "    expected" : "not expected", memory, fw_size);
+  }
   return memory;
 }
 
@@ -107,9 +109,11 @@ custom_realloc(void * memory_in, size_t size)
   }
   void * memory = realloc(memory_in, size);
   uint64_t fw_size = size;
-  MALLOC_PRINTF(
-    "realloc (%s) %p %p %" PRIu64 "\n",
-    malloc_expected ? "    expected" : "not expected", memory_in, memory, fw_size);
+  if (!realloc_expected) {
+    MALLOC_PRINTF(
+      "realloc (%s) %p %p %" PRIu64 "\n",
+      realloc_expected ? "    expected" : "not expected", memory_in, memory, fw_size);
+  }
   return memory;
 }
 
@@ -147,8 +151,10 @@ custom_free(void * memory)
       (*unexpected_free_callback)();
     }
   }
-  MALLOC_PRINTF(
-    "   free (%s) %p\n", malloc_expected ? "    expected" : "not expected", memory);
+  if (!free_expected) {
+    MALLOC_PRINTF(
+      "   free (%s) %p\n", free_expected ? "    expected" : "not expected", memory);
+  }
   free(memory);
 }
 
diff --git a/rcl/test/memory_tools_osx_interpose.cpp b/rcl/test/memory_tools/memory_tools_osx_interpose.cpp
similarity index 89%
rename from rcl/test/memory_tools_osx_interpose.cpp
rename to rcl/test/memory_tools/memory_tools_osx_interpose.cpp
index 91579cf..d9397df 100644
--- a/rcl/test/memory_tools_osx_interpose.cpp
+++ b/rcl/test/memory_tools/memory_tools_osx_interpose.cpp
@@ -40,14 +40,16 @@ typedef struct interpose_s
 void osx_start_memory_checking()
 {
   // No loading required, it is handled by DYLD_INSERT_LIBRARIES and dynamic library interposing.
-  MALLOC_PRINTF("starting memory checking...\n");
-  enabled.store(true);
+  if (!enabled.exchange(true)) {
+    MALLOC_PRINTF("starting memory checking...\n");
+  }
 }
 
 void osx_stop_memory_checking()
 {
-  MALLOC_PRINTF("stopping memory checking...\n");
-  enabled.store(false);
+  if (enabled.exchange(false)) {
+    MALLOC_PRINTF("stopping memory checking...\n");
+  }
 }
 
 OSX_INTERPOSE(custom_malloc, malloc);
diff --git a/rcl/test/test_memory_tools.cpp b/rcl/test/memory_tools/test_memory_tools.cpp
similarity index 100%
rename from rcl/test/test_memory_tools.cpp
rename to rcl/test/memory_tools/test_memory_tools.cpp
diff --git a/rcl/test/rcl/test_allocator.cpp b/rcl/test/rcl/test_allocator.cpp
index dfe41e6..d32fbf6 100644
--- a/rcl/test/rcl/test_allocator.cpp
+++ b/rcl/test/rcl/test_allocator.cpp
@@ -16,7 +16,7 @@
 
 #include "rcl/allocator.h"
 
-#include "../memory_tools.hpp"
+#include "../memory_tools/memory_tools.hpp"
 
 class TestAllocatorFixture : public ::testing::Test
 {
diff --git a/rcl/test/rcl/test_node.cpp b/rcl/test/rcl/test_node.cpp
index c8befcd..7632ece 100644
--- a/rcl/test/rcl/test_node.cpp
+++ b/rcl/test/rcl/test_node.cpp
@@ -20,7 +20,7 @@
 #include "rcl/node.h"
 #include "rmw/rmw.h"  // For rmw_get_implementation_identifier.
 
-#include "../memory_tools.hpp"
+#include "../memory_tools/memory_tools.hpp"
 #include "../scope_exit.hpp"
 #include "rcl/error_handling.h"
 
@@ -47,30 +47,6 @@ public:
   }
 };
 
-void *
-failing_malloc(size_t size, void * state)
-{
-  (void)(size);
-  (void)(state);
-  return nullptr;
-}
-
-void *
-failing_realloc(void * pointer, size_t size, void * state)
-{
-  (void)(pointer);
-  (void)(size);
-  (void)(state);
-  return nullptr;
-}
-
-void
-failing_free(void * pointer, void * state)
-{
-  (void)pointer;
-  (void)state;
-}
-
 bool is_opensplice =
   std::string(rmw_get_implementation_identifier()).find("opensplice") != std::string::npos;
 #if defined(WIN32)
diff --git a/rcl/test/rcl/test_rcl.cpp b/rcl/test/rcl/test_rcl.cpp
index c88f601..e0c4c7b 100644
--- a/rcl/test/rcl/test_rcl.cpp
+++ b/rcl/test/rcl/test_rcl.cpp
@@ -16,7 +16,7 @@
 
 #include "rcl/rcl.h"
 
-#include "../memory_tools.hpp"
+#include "../memory_tools/memory_tools.hpp"
 #include "rcl/error_handling.h"
 
 class TestRCLFixture : public ::testing::Test
@@ -42,30 +42,6 @@ public:
   }
 };
 
-void *
-failing_malloc(size_t size, void * state)
-{
-  (void)(size);
-  (void)(state);
-  return nullptr;
-}
-
-void *
-failing_realloc(void * pointer, size_t size, void * state)
-{
-  (void)(pointer);
-  (void)(size);
-  (void)(state);
-  return nullptr;
-}
-
-void
-failing_free(void * pointer, void * state)
-{
-  (void)pointer;
-  (void)state;
-}
-
 struct FakeTestArgv
 {
   FakeTestArgv()
@@ -97,6 +73,9 @@ struct FakeTestArgv
 
   int argc;
   char ** argv;
+
+private:
+  FakeTestArgv(const FakeTestArgv &) = delete;
 };
 
 /* Tests the rcl_init(), rcl_ok(), and rcl_shutdown() functions.
diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp
index 1fda410..18c5a61 100644
--- a/rcl/test/rcl/test_time.cpp
+++ b/rcl/test/rcl/test_time.cpp
@@ -22,7 +22,7 @@
 #include "rcl/error_handling.h"
 #include "rcl/time.h"
 
-#include "../memory_tools.hpp"
+#include "../memory_tools/memory_tools.hpp"
 
 class TestTimeFixture : public ::testing::Test
 {
diff --git a/rcl/test/rcl_add_custom_gtest.cmake b/rcl/test/rcl_add_custom_gtest.cmake
new file mode 100644
index 0000000..895aad3
--- /dev/null
+++ b/rcl/test/rcl_add_custom_gtest.cmake
@@ -0,0 +1,100 @@
+# Copyright 2016 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+if(rcl_add_custom_gtest_INCLUDED)
+  return()
+endif()
+set(rcl_add_custom_gtest_INCLUDED TRUE)
+
+# include CMake functions
+include(CMakeParseArguments)
+
+#
+# Custom macro for adding a gtest in rcl.
+#
+# It also takes some of the arguments of ament_add_gtest as well as
+# INCLUDE_DIRS, LIBRARIES, and AMENT_DEPENDENCIES which are passed to
+# target_include_directories(), target_link_libraries(), and
+# ament_target_dependencies() respectively.
+#
+# :param target: the target name which will also be used as the test name
+# :type target: string
+# :param SRCS: list of source files used to create the gtest
+# :type SRCS: list of strings
+# :param ENV: list of env vars to set; listed as ``VAR=value``
+# :type ENV: list of strings
+# :param APPEND_ENV: list of env vars to append if already set, otherwise set;
+#   listed as ``VAR=value``
+# :type APPEND_ENV: list of strings
+# :param APPEND_LIBRARY_DIRS: list of library dirs to append to the appropriate
+#   OS specific env var, a la LD_LIBRARY_PATH
+# :type APPEND_LIBRARY_DIRS: list of strings
+# :param INCLUDE_DIRS: list of include directories to add to the target
+# :type INCLUDE_DIRS: list of strings
+# :param LIBRARIES: list of libraries to link to the target
+# :type LIBRARIES: list of strings
+# :param AMENT_DEPENDENCIES: list of depends to pass ament_target_dependencies
+# :type AMENT_DEPENDENCIES: list of strings
+#
+# @public
+#
+macro(rcl_add_custom_gtest target)
+  cmake_parse_arguments(_ARG
+    "TRACE"
+    ""
+    "SRCS;ENV;APPEND_ENV;APPEND_LIBRARY_DIRS;INCLUDE_DIRS;LIBRARIES;AMENT_DEPENDENCIES"
+    ${ARGN})
+  if(_ARG_UNPARSED_ARGUMENTS)
+    message(FATAL_ERROR "rcl_add_custom_gtest() called with unused arguments: ${_ARG_UNPARSED_ARGUMENTS}")
+  endif()
+  if(_ARG_ENV)
+    set(_ARG_ENV "ENV" ${_ARG_ENV})
+  endif()
+  if(_ARG_APPEND_ENV)
+    set(_ARG_APPEND_ENV "APPEND_ENV" ${_ARG_APPEND_ENV})
+  endif()
+  if(_ARG_APPEND_LIBRARY_DIRS)
+    set(_ARG_APPEND_LIBRARY_DIRS "APPEND_LIBRARY_DIRS" ${_ARG_APPEND_LIBRARY_DIRS})
+  endif()
+
+  # Pass args along to ament_add_gtest().
+  ament_add_gtest(${target} ${_ARG_SRCS} ${_ARG_ENV} ${_ARG_APPEND_ENV} ${_ARG_APPEND_LIBRARY_DIRS})
+  # Check if the target was actually created.
+  if(TARGET ${target})
+    if(_ARG_TRACE)
+      message(STATUS "rcl_add_custom_gtest() Target '${target}':")
+    endif()
+    # Add extra include directories, if any.
+    if(_ARG_INCLUDE_DIRS)
+      if(_ARG_TRACE)
+        message(STATUS "  rcl_add_custom_gtest() INCLUDE_DIRS: ${_ARG_INCLUDE_DIRS}")
+      endif()
+      target_include_directories(${target} PUBLIC ${_ARG_INCLUDE_DIRS})
+    endif()
+    # Add extra link libraries, if any.
+    if(_ARG_LIBRARIES)
+      if(_ARG_TRACE)
+        message(STATUS "  rcl_add_custom_gtest() LIBRARIES: ${_ARG_LIBRARIES}")
+      endif()
+      target_link_libraries(${target} ${_ARG_LIBRARIES})
+    endif()
+    # Add extra ament dependencies, if any.
+    if(_ARG_AMENT_DEPENDENCIES)
+      if(_ARG_TRACE)
+        message(STATUS "  rcl_add_custom_gtest() AMENT_DEPENDENCIES: ${_ARG_AMENT_DEPENDENCIES}")
+      endif()
+      ament_target_dependencies(${target} ${_ARG_AMENT_DEPENDENCIES})
+    endif()
+  endif()
+endmacro()