diff --git a/repositories/repositories.bzl b/repositories/repositories.bzl
index db736cdf..99d1c8cf 100644
--- a/repositories/repositories.bzl
+++ b/repositories/repositories.bzl
@@ -131,6 +131,14 @@ def ros2_workspace_repositories():
         urls = ["https://github.com/zaphoyd/websocketpp/archive/refs/tags/0.8.2.tar.gz"],
     )
 
+    maybe(
+        http_archive,
+        name = "with_cfg.bzl",
+        sha256 = "d83f99ac39cd9940848ea11a51a60159cf09cda2ba30545036041551aae73ab4",
+        strip_prefix = "with_cfg.bzl-0.6.1",
+        url = "https://github.com/fmeum/with_cfg.bzl/releases/download/v0.6.1/with_cfg.bzl-v0.6.1.tar.gz",
+    )
+
     maybe(
         http_archive,
         name = "asio",
diff --git a/repositories/rmw_implementation.BUILD.bazel b/repositories/rmw_implementation.BUILD.bazel
index 7549c9cb..f1151ef9 100644
--- a/repositories/rmw_implementation.BUILD.bazel
+++ b/repositories/rmw_implementation.BUILD.bazel
@@ -15,9 +15,7 @@ ros2_cpp_library(
         "@ros2_rmw_cyclonedds//:rmw_cyclonedds",
     ],
     includes = ["include"],
-    local_defines = [
-        "RMW_LIBRARY_PATH=\\\"$(rootpath @ros2_rmw_cyclonedds//:rmw_cyclonedds)\\\"",
-    ],
+    local_defines = ['RMW_LIBRARY_PATH=\\"../ros2_rmw_cyclonedds/librmw_cyclonedds.so\\"'],
     visibility = ["//visibility:public"],
     deps = [
         "@ros2_rcpputils//:rcpputils",
diff --git a/ros2/test/BUILD.bazel b/ros2/test/BUILD.bazel
index 2f32ebdc..686649db 100644
--- a/ros2/test/BUILD.bazel
+++ b/ros2/test/BUILD.bazel
@@ -7,6 +7,7 @@ load(
     "py_ros2_interface_library",
     "ros2_interface_library",
 )
+load("@com_github_mvukov_rules_ros2//ros2/test:ros2_cpp_opt_test.bzl", "ros2_cpp_opt_test")
 load("@rules_python//python:defs.bzl", "py_test")
 
 py_test(
@@ -66,3 +67,10 @@ ros2_cpp_test(
         "@ros2_rclcpp//:rclcpp",
     ],
 )
+
+ros2_cpp_opt_test(
+    name = "test_rclcpp_opt",
+    size = "small",
+    srcs = ["test_rclcpp.cpp"],
+    deps = ["@ros2_rclcpp//:rclcpp"],
+)
diff --git a/ros2/test/ros2_cpp_opt_test.bzl b/ros2/test/ros2_cpp_opt_test.bzl
new file mode 100644
index 00000000..d10db7a1
--- /dev/null
+++ b/ros2/test/ros2_cpp_opt_test.bzl
@@ -0,0 +1,4 @@
+load("@com_github_mvukov_rules_ros2//ros2:cc_defs.bzl", "ros2_cpp_test")
+load("@with_cfg.bzl", "with_cfg")
+
+ros2_cpp_opt_test, _ros2_cpp_opt_test_internal = with_cfg(ros2_cpp_test).set("compilation_mode", "opt").build()