From 14f20b5004d810475d05ac5fefa1dc30675ad4b0 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Fri, 18 Jul 2025 16:45:34 +0800
Subject: [PATCH 01/20] Remove lockfiles from being tracked

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 .gitignore                                 |   1 +
 examples/message_demo/Cargo.lock           | 652 -----------------
 examples/minimal_client_service/Cargo.lock | 632 -----------------
 examples/minimal_pub_sub/Cargo.lock        | 599 ----------------
 examples/rust_pubsub/Cargo.lock            | 463 ------------
 rclrs/Cargo.lock                           | 782 ---------------------
 6 files changed, 1 insertion(+), 3128 deletions(-)
 delete mode 100644 examples/message_demo/Cargo.lock
 delete mode 100644 examples/minimal_client_service/Cargo.lock
 delete mode 100644 examples/minimal_pub_sub/Cargo.lock
 delete mode 100644 examples/rust_pubsub/Cargo.lock
 delete mode 100644 rclrs/Cargo.lock

diff --git a/.gitignore b/.gitignore
index 83c7a4d7..35a1460f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -214,6 +214,7 @@ dmypy.json
 # Generated by Cargo
 # will have compiled files and executables
 /target/
+Cargo.lock
 
 # These are backup files generated by rustfmt
 **/*.rs.bk
diff --git a/examples/message_demo/Cargo.lock b/examples/message_demo/Cargo.lock
deleted file mode 100644
index 97c18c0d..00000000
--- a/examples/message_demo/Cargo.lock
+++ /dev/null
@@ -1,652 +0,0 @@
-# This file is automatically @generated by Cargo.
-# It is not intended for manual editing.
-version = 3
-
-[[package]]
-name = "addr2line"
-version = "0.24.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "dfbe277e56a376000877090da837660b4427aad530e3028d44e0bffe4f89a1c1"
-dependencies = [
- "gimli",
-]
-
-[[package]]
-name = "adler2"
-version = "2.0.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "512761e0bb2578dd7380c6baaa0f4ce03e84f95e960231d1dec8bf4d7d6e2627"
-
-[[package]]
-name = "aho-corasick"
-version = "1.1.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8e60d3430d3a69478ad0993f19238d2df97c507009a52b3c10addcd7f6bcb916"
-dependencies = [
- "memchr",
-]
-
-[[package]]
-name = "anyhow"
-version = "1.0.98"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e16d2d3311acee920a9eb8d33b8cbc1787ce4a264e85f964c2404b969bdcd487"
-dependencies = [
- "backtrace",
-]
-
-[[package]]
-name = "autocfg"
-version = "1.4.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26"
-
-[[package]]
-name = "backtrace"
-version = "0.3.74"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8d82cb332cdfaed17ae235a638438ac4d4839913cc2af585c3c6746e8f8bee1a"
-dependencies = [
- "addr2line",
- "cfg-if",
- "libc",
- "miniz_oxide",
- "object",
- "rustc-demangle",
- "windows-targets 0.52.6",
-]
-
-[[package]]
-name = "bindgen"
-version = "0.70.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f49d8fed880d473ea71efb9bf597651e77201bdd4893efe54c9e5d65ae04ce6f"
-dependencies = [
- "bitflags",
- "cexpr",
- "clang-sys",
- "itertools",
- "log",
- "prettyplease",
- "proc-macro2",
- "quote",
- "regex",
- "rustc-hash",
- "shlex",
- "syn",
-]
-
-[[package]]
-name = "bitflags"
-version = "2.9.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1b8e56985ec62d17e9c1001dc89c88ecd7dc08e47eba5ec7c29c7b5eeecde967"
-
-[[package]]
-name = "cexpr"
-version = "0.6.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6fac387a98bb7c37292057cffc56d62ecb629900026402633ae9160df93a8766"
-dependencies = [
- "nom",
-]
-
-[[package]]
-name = "cfg-if"
-version = "1.0.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd"
-
-[[package]]
-name = "clang-sys"
-version = "1.8.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0b023947811758c97c59bf9d1c188fd619ad4718dcaa767947df1cadb14f39f4"
-dependencies = [
- "glob",
- "libc",
- "libloading",
-]
-
-[[package]]
-name = "either"
-version = "1.15.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719"
-
-[[package]]
-name = "examples_rclrs_message_demo"
-version = "0.4.1"
-dependencies = [
- "anyhow",
- "backtrace",
- "rclrs",
- "rclrs_example_msgs",
- "rosidl_runtime_rs",
- "serde_json",
-]
-
-[[package]]
-name = "futures"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "65bc07b1a8bc7c85c5f2e110c476c7389b4554ba72af57d8445ea63a576b0876"
-dependencies = [
- "futures-channel",
- "futures-core",
- "futures-executor",
- "futures-io",
- "futures-sink",
- "futures-task",
- "futures-util",
-]
-
-[[package]]
-name = "futures-channel"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2dff15bf788c671c1934e366d07e30c1814a8ef514e1af724a602e8a2fbe1b10"
-dependencies = [
- "futures-core",
- "futures-sink",
-]
-
-[[package]]
-name = "futures-core"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "05f29059c0c2090612e8d742178b0580d2dc940c837851ad723096f87af6663e"
-
-[[package]]
-name = "futures-executor"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1e28d1d997f585e54aebc3f97d39e72338912123a67330d723fdbb564d646c9f"
-dependencies = [
- "futures-core",
- "futures-task",
- "futures-util",
-]
-
-[[package]]
-name = "futures-io"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9e5c1b78ca4aae1ac06c48a526a655760685149f0d465d21f37abfe57ce075c6"
-
-[[package]]
-name = "futures-macro"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "162ee34ebcb7c64a8abebc059ce0fee27c2262618d7b60ed8faf72fef13c3650"
-dependencies = [
- "proc-macro2",
- "quote",
- "syn",
-]
-
-[[package]]
-name = "futures-sink"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e575fab7d1e0dcb8d0c7bcf9a63ee213816ab51902e6d244a95819acacf1d4f7"
-
-[[package]]
-name = "futures-task"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f90f7dce0722e95104fcb095585910c0977252f286e354b5e3bd38902cd99988"
-
-[[package]]
-name = "futures-util"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9fa08315bb612088cc391249efdc3bc77536f16c91f6cf495e6fbe85b20a4a81"
-dependencies = [
- "futures-channel",
- "futures-core",
- "futures-io",
- "futures-macro",
- "futures-sink",
- "futures-task",
- "memchr",
- "pin-project-lite",
- "pin-utils",
- "slab",
-]
-
-[[package]]
-name = "gimli"
-version = "0.31.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "07e28edb80900c19c28f1072f2e8aeca7fa06b23cd4169cefe1af5aa3260783f"
-
-[[package]]
-name = "glob"
-version = "0.3.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a8d1add55171497b4705a648c6b583acafb01d58050a51727785f0b2c8e0a2b2"
-
-[[package]]
-name = "itertools"
-version = "0.13.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "413ee7dfc52ee1a4949ceeb7dbc8a33f2d6c088194d9f922fb8318faf1f01186"
-dependencies = [
- "either",
-]
-
-[[package]]
-name = "itoa"
-version = "1.0.15"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4a5f13b858c8d314ee3e8f639011f7ccefe71f97f96e50151fb991f267928e2c"
-
-[[package]]
-name = "libc"
-version = "0.2.172"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d750af042f7ef4f724306de029d18836c26c1765a54a6a3f094cbd23a7267ffa"
-
-[[package]]
-name = "libloading"
-version = "0.8.8"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "07033963ba89ebaf1584d767badaa2e8fcec21aedea6b8c0346d487d49c28667"
-dependencies = [
- "cfg-if",
- "windows-targets 0.53.0",
-]
-
-[[package]]
-name = "log"
-version = "0.4.27"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94"
-
-[[package]]
-name = "memchr"
-version = "2.7.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "78ca9ab1a0babb1e7d5695e3530886289c18cf2f87ec19a575a0abdce112e3a3"
-
-[[package]]
-name = "minimal-lexical"
-version = "0.2.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a"
-
-[[package]]
-name = "miniz_oxide"
-version = "0.8.8"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3be647b768db090acb35d5ec5db2b0e1f1de11133ca123b9eacf5137868f892a"
-dependencies = [
- "adler2",
-]
-
-[[package]]
-name = "nom"
-version = "7.1.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d273983c5a657a70a3e8f2a01329822f3b8c8172b73826411a55751e404a0a4a"
-dependencies = [
- "memchr",
- "minimal-lexical",
-]
-
-[[package]]
-name = "object"
-version = "0.36.7"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "62948e14d923ea95ea2c7c86c71013138b66525b86bdc08d2dcc262bdb497b87"
-dependencies = [
- "memchr",
-]
-
-[[package]]
-name = "pin-project-lite"
-version = "0.2.16"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3b3cff922bd51709b605d9ead9aa71031d81447142d828eb4a6eba76fe619f9b"
-
-[[package]]
-name = "pin-utils"
-version = "0.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"
-
-[[package]]
-name = "prettyplease"
-version = "0.2.32"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "664ec5419c51e34154eec046ebcba56312d5a2fc3b09a06da188e1ad21afadf6"
-dependencies = [
- "proc-macro2",
- "syn",
-]
-
-[[package]]
-name = "proc-macro2"
-version = "1.0.95"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "02b3e5e68a3a1a02aad3ec490a98007cbc13c37cbe84a3cd7b8e406d76e7f778"
-dependencies = [
- "unicode-ident",
-]
-
-[[package]]
-name = "quote"
-version = "1.0.40"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d"
-dependencies = [
- "proc-macro2",
-]
-
-[[package]]
-name = "rclrs"
-version = "0.4.1"
-dependencies = [
- "bindgen",
- "cfg-if",
- "futures",
- "rosidl_runtime_rs",
-]
-
-[[package]]
-name = "rclrs_example_msgs"
-version = "0.4.1"
-dependencies = [
- "rosidl_runtime_rs",
- "serde",
- "serde-big-array",
-]
-
-[[package]]
-name = "regex"
-version = "1.11.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b544ef1b4eac5dc2db33ea63606ae9ffcfac26c1416a2806ae0bf5f56b201191"
-dependencies = [
- "aho-corasick",
- "memchr",
- "regex-automata",
- "regex-syntax",
-]
-
-[[package]]
-name = "regex-automata"
-version = "0.4.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "809e8dc61f6de73b46c85f4c96486310fe304c434cfa43669d7b40f711150908"
-dependencies = [
- "aho-corasick",
- "memchr",
- "regex-syntax",
-]
-
-[[package]]
-name = "regex-syntax"
-version = "0.8.5"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2b15c43186be67a4fd63bee50d0303afffcef381492ebe2c5d87f324e1b8815c"
-
-[[package]]
-name = "rosidl_runtime_rs"
-version = "0.4.1"
-dependencies = [
- "cfg-if",
- "serde",
-]
-
-[[package]]
-name = "rustc-demangle"
-version = "0.1.24"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "719b953e2095829ee67db738b3bfa9fa368c94900df327b3f07fe6e794d2fe1f"
-
-[[package]]
-name = "rustc-hash"
-version = "1.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2"
-
-[[package]]
-name = "ryu"
-version = "1.0.20"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "28d3b2b1366ec20994f1fd18c3c594f05c5dd4bc44d8bb0c1c632c8d6829481f"
-
-[[package]]
-name = "serde"
-version = "1.0.219"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5f0e2c6ed6606019b4e29e69dbaba95b11854410e5347d525002456dbbb786b6"
-dependencies = [
- "serde_derive",
-]
-
-[[package]]
-name = "serde-big-array"
-version = "0.5.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "11fc7cc2c76d73e0f27ee52abbd64eec84d46f370c88371120433196934e4b7f"
-dependencies = [
- "serde",
-]
-
-[[package]]
-name = "serde_derive"
-version = "1.0.219"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5b0276cf7f2c73365f7157c8123c21cd9a50fbbd844757af28ca1f5925fc2a00"
-dependencies = [
- "proc-macro2",
- "quote",
- "syn",
-]
-
-[[package]]
-name = "serde_json"
-version = "1.0.140"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "20068b6e96dc6c9bd23e01df8827e6c7e1f2fddd43c21810382803c136b99373"
-dependencies = [
- "itoa",
- "memchr",
- "ryu",
- "serde",
-]
-
-[[package]]
-name = "shlex"
-version = "1.3.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64"
-
-[[package]]
-name = "slab"
-version = "0.4.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8f92a496fb766b417c996b9c5e57daf2f7ad3b0bebe1ccfca4856390e3d3bb67"
-dependencies = [
- "autocfg",
-]
-
-[[package]]
-name = "syn"
-version = "2.0.101"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8ce2b7fc941b3a24138a0a7cf8e858bfc6a992e7978a068a5c760deb0ed43caf"
-dependencies = [
- "proc-macro2",
- "quote",
- "unicode-ident",
-]
-
-[[package]]
-name = "unicode-ident"
-version = "1.0.18"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512"
-
-[[package]]
-name = "windows-targets"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973"
-dependencies = [
- "windows_aarch64_gnullvm 0.52.6",
- "windows_aarch64_msvc 0.52.6",
- "windows_i686_gnu 0.52.6",
- "windows_i686_gnullvm 0.52.6",
- "windows_i686_msvc 0.52.6",
- "windows_x86_64_gnu 0.52.6",
- "windows_x86_64_gnullvm 0.52.6",
- "windows_x86_64_msvc 0.52.6",
-]
-
-[[package]]
-name = "windows-targets"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b1e4c7e8ceaaf9cb7d7507c974735728ab453b67ef8f18febdd7c11fe59dca8b"
-dependencies = [
- "windows_aarch64_gnullvm 0.53.0",
- "windows_aarch64_msvc 0.53.0",
- "windows_i686_gnu 0.53.0",
- "windows_i686_gnullvm 0.53.0",
- "windows_i686_msvc 0.53.0",
- "windows_x86_64_gnu 0.53.0",
- "windows_x86_64_gnullvm 0.53.0",
- "windows_x86_64_msvc 0.53.0",
-]
-
-[[package]]
-name = "windows_aarch64_gnullvm"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3"
-
-[[package]]
-name = "windows_aarch64_gnullvm"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "86b8d5f90ddd19cb4a147a5fa63ca848db3df085e25fee3cc10b39b6eebae764"
-
-[[package]]
-name = "windows_aarch64_msvc"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469"
-
-[[package]]
-name = "windows_aarch64_msvc"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c7651a1f62a11b8cbd5e0d42526e55f2c99886c77e007179efff86c2b137e66c"
-
-[[package]]
-name = "windows_i686_gnu"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b"
-
-[[package]]
-name = "windows_i686_gnu"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c1dc67659d35f387f5f6c479dc4e28f1d4bb90ddd1a5d3da2e5d97b42d6272c3"
-
-[[package]]
-name = "windows_i686_gnullvm"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66"
-
-[[package]]
-name = "windows_i686_gnullvm"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9ce6ccbdedbf6d6354471319e781c0dfef054c81fbc7cf83f338a4296c0cae11"
-
-[[package]]
-name = "windows_i686_msvc"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66"
-
-[[package]]
-name = "windows_i686_msvc"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "581fee95406bb13382d2f65cd4a908ca7b1e4c2f1917f143ba16efe98a589b5d"
-
-[[package]]
-name = "windows_x86_64_gnu"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78"
-
-[[package]]
-name = "windows_x86_64_gnu"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2e55b5ac9ea33f2fc1716d1742db15574fd6fc8dadc51caab1c16a3d3b4190ba"
-
-[[package]]
-name = "windows_x86_64_gnullvm"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d"
-
-[[package]]
-name = "windows_x86_64_gnullvm"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0a6e035dd0599267ce1ee132e51c27dd29437f63325753051e71dd9e42406c57"
-
-[[package]]
-name = "windows_x86_64_msvc"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec"
-
-[[package]]
-name = "windows_x86_64_msvc"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486"
-
-[[patch.unused]]
-name = "action_msgs"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "builtin_interfaces"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "example_interfaces"
-version = "0.9.3"
-
-[[patch.unused]]
-name = "rcl_interfaces"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "rosgraph_msgs"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "std_msgs"
-version = "4.8.0"
-
-[[patch.unused]]
-name = "test_msgs"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "unique_identifier_msgs"
-version = "2.2.1"
diff --git a/examples/minimal_client_service/Cargo.lock b/examples/minimal_client_service/Cargo.lock
deleted file mode 100644
index 17c40061..00000000
--- a/examples/minimal_client_service/Cargo.lock
+++ /dev/null
@@ -1,632 +0,0 @@
-# This file is automatically @generated by Cargo.
-# It is not intended for manual editing.
-version = 3
-
-[[package]]
-name = "action_msgs"
-version = "1.2.1"
-dependencies = [
- "builtin_interfaces",
- "rosidl_runtime_rs",
- "unique_identifier_msgs",
-]
-
-[[package]]
-name = "addr2line"
-version = "0.24.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "dfbe277e56a376000877090da837660b4427aad530e3028d44e0bffe4f89a1c1"
-dependencies = [
- "gimli",
-]
-
-[[package]]
-name = "adler2"
-version = "2.0.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "512761e0bb2578dd7380c6baaa0f4ce03e84f95e960231d1dec8bf4d7d6e2627"
-
-[[package]]
-name = "aho-corasick"
-version = "1.1.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8e60d3430d3a69478ad0993f19238d2df97c507009a52b3c10addcd7f6bcb916"
-dependencies = [
- "memchr",
-]
-
-[[package]]
-name = "anyhow"
-version = "1.0.98"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e16d2d3311acee920a9eb8d33b8cbc1787ce4a264e85f964c2404b969bdcd487"
-dependencies = [
- "backtrace",
-]
-
-[[package]]
-name = "autocfg"
-version = "1.4.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26"
-
-[[package]]
-name = "backtrace"
-version = "0.3.74"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8d82cb332cdfaed17ae235a638438ac4d4839913cc2af585c3c6746e8f8bee1a"
-dependencies = [
- "addr2line",
- "cfg-if",
- "libc",
- "miniz_oxide",
- "object",
- "rustc-demangle",
- "windows-targets 0.52.6",
-]
-
-[[package]]
-name = "bindgen"
-version = "0.70.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f49d8fed880d473ea71efb9bf597651e77201bdd4893efe54c9e5d65ae04ce6f"
-dependencies = [
- "bitflags",
- "cexpr",
- "clang-sys",
- "itertools",
- "log",
- "prettyplease",
- "proc-macro2",
- "quote",
- "regex",
- "rustc-hash",
- "shlex",
- "syn",
-]
-
-[[package]]
-name = "bitflags"
-version = "2.9.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1b8e56985ec62d17e9c1001dc89c88ecd7dc08e47eba5ec7c29c7b5eeecde967"
-
-[[package]]
-name = "builtin_interfaces"
-version = "1.2.1"
-dependencies = [
- "rosidl_runtime_rs",
-]
-
-[[package]]
-name = "cexpr"
-version = "0.6.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6fac387a98bb7c37292057cffc56d62ecb629900026402633ae9160df93a8766"
-dependencies = [
- "nom",
-]
-
-[[package]]
-name = "cfg-if"
-version = "1.0.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd"
-
-[[package]]
-name = "clang-sys"
-version = "1.8.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0b023947811758c97c59bf9d1c188fd619ad4718dcaa767947df1cadb14f39f4"
-dependencies = [
- "glob",
- "libc",
- "libloading",
-]
-
-[[package]]
-name = "either"
-version = "1.15.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719"
-
-[[package]]
-name = "example_interfaces"
-version = "0.9.3"
-dependencies = [
- "action_msgs",
- "builtin_interfaces",
- "rosidl_runtime_rs",
- "unique_identifier_msgs",
-]
-
-[[package]]
-name = "examples_rclrs_minimal_client_service"
-version = "0.4.1"
-dependencies = [
- "anyhow",
- "backtrace",
- "example_interfaces",
- "rclrs",
- "rosidl_runtime_rs",
- "tokio",
-]
-
-[[package]]
-name = "futures"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "65bc07b1a8bc7c85c5f2e110c476c7389b4554ba72af57d8445ea63a576b0876"
-dependencies = [
- "futures-channel",
- "futures-core",
- "futures-executor",
- "futures-io",
- "futures-sink",
- "futures-task",
- "futures-util",
-]
-
-[[package]]
-name = "futures-channel"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2dff15bf788c671c1934e366d07e30c1814a8ef514e1af724a602e8a2fbe1b10"
-dependencies = [
- "futures-core",
- "futures-sink",
-]
-
-[[package]]
-name = "futures-core"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "05f29059c0c2090612e8d742178b0580d2dc940c837851ad723096f87af6663e"
-
-[[package]]
-name = "futures-executor"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1e28d1d997f585e54aebc3f97d39e72338912123a67330d723fdbb564d646c9f"
-dependencies = [
- "futures-core",
- "futures-task",
- "futures-util",
-]
-
-[[package]]
-name = "futures-io"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9e5c1b78ca4aae1ac06c48a526a655760685149f0d465d21f37abfe57ce075c6"
-
-[[package]]
-name = "futures-macro"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "162ee34ebcb7c64a8abebc059ce0fee27c2262618d7b60ed8faf72fef13c3650"
-dependencies = [
- "proc-macro2",
- "quote",
- "syn",
-]
-
-[[package]]
-name = "futures-sink"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e575fab7d1e0dcb8d0c7bcf9a63ee213816ab51902e6d244a95819acacf1d4f7"
-
-[[package]]
-name = "futures-task"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f90f7dce0722e95104fcb095585910c0977252f286e354b5e3bd38902cd99988"
-
-[[package]]
-name = "futures-util"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9fa08315bb612088cc391249efdc3bc77536f16c91f6cf495e6fbe85b20a4a81"
-dependencies = [
- "futures-channel",
- "futures-core",
- "futures-io",
- "futures-macro",
- "futures-sink",
- "futures-task",
- "memchr",
- "pin-project-lite",
- "pin-utils",
- "slab",
-]
-
-[[package]]
-name = "gimli"
-version = "0.31.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "07e28edb80900c19c28f1072f2e8aeca7fa06b23cd4169cefe1af5aa3260783f"
-
-[[package]]
-name = "glob"
-version = "0.3.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a8d1add55171497b4705a648c6b583acafb01d58050a51727785f0b2c8e0a2b2"
-
-[[package]]
-name = "itertools"
-version = "0.13.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "413ee7dfc52ee1a4949ceeb7dbc8a33f2d6c088194d9f922fb8318faf1f01186"
-dependencies = [
- "either",
-]
-
-[[package]]
-name = "libc"
-version = "0.2.172"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d750af042f7ef4f724306de029d18836c26c1765a54a6a3f094cbd23a7267ffa"
-
-[[package]]
-name = "libloading"
-version = "0.8.8"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "07033963ba89ebaf1584d767badaa2e8fcec21aedea6b8c0346d487d49c28667"
-dependencies = [
- "cfg-if",
- "windows-targets 0.53.0",
-]
-
-[[package]]
-name = "log"
-version = "0.4.27"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94"
-
-[[package]]
-name = "memchr"
-version = "2.7.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "78ca9ab1a0babb1e7d5695e3530886289c18cf2f87ec19a575a0abdce112e3a3"
-
-[[package]]
-name = "minimal-lexical"
-version = "0.2.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a"
-
-[[package]]
-name = "miniz_oxide"
-version = "0.8.8"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3be647b768db090acb35d5ec5db2b0e1f1de11133ca123b9eacf5137868f892a"
-dependencies = [
- "adler2",
-]
-
-[[package]]
-name = "nom"
-version = "7.1.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d273983c5a657a70a3e8f2a01329822f3b8c8172b73826411a55751e404a0a4a"
-dependencies = [
- "memchr",
- "minimal-lexical",
-]
-
-[[package]]
-name = "object"
-version = "0.36.7"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "62948e14d923ea95ea2c7c86c71013138b66525b86bdc08d2dcc262bdb497b87"
-dependencies = [
- "memchr",
-]
-
-[[package]]
-name = "pin-project-lite"
-version = "0.2.16"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3b3cff922bd51709b605d9ead9aa71031d81447142d828eb4a6eba76fe619f9b"
-
-[[package]]
-name = "pin-utils"
-version = "0.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"
-
-[[package]]
-name = "prettyplease"
-version = "0.2.32"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "664ec5419c51e34154eec046ebcba56312d5a2fc3b09a06da188e1ad21afadf6"
-dependencies = [
- "proc-macro2",
- "syn",
-]
-
-[[package]]
-name = "proc-macro2"
-version = "1.0.95"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "02b3e5e68a3a1a02aad3ec490a98007cbc13c37cbe84a3cd7b8e406d76e7f778"
-dependencies = [
- "unicode-ident",
-]
-
-[[package]]
-name = "quote"
-version = "1.0.40"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d"
-dependencies = [
- "proc-macro2",
-]
-
-[[package]]
-name = "rclrs"
-version = "0.4.1"
-dependencies = [
- "bindgen",
- "cfg-if",
- "futures",
- "rosidl_runtime_rs",
-]
-
-[[package]]
-name = "regex"
-version = "1.11.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b544ef1b4eac5dc2db33ea63606ae9ffcfac26c1416a2806ae0bf5f56b201191"
-dependencies = [
- "aho-corasick",
- "memchr",
- "regex-automata",
- "regex-syntax",
-]
-
-[[package]]
-name = "regex-automata"
-version = "0.4.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "809e8dc61f6de73b46c85f4c96486310fe304c434cfa43669d7b40f711150908"
-dependencies = [
- "aho-corasick",
- "memchr",
- "regex-syntax",
-]
-
-[[package]]
-name = "regex-syntax"
-version = "0.8.5"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2b15c43186be67a4fd63bee50d0303afffcef381492ebe2c5d87f324e1b8815c"
-
-[[package]]
-name = "rosidl_runtime_rs"
-version = "0.4.1"
-dependencies = [
- "cfg-if",
-]
-
-[[package]]
-name = "rustc-demangle"
-version = "0.1.24"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "719b953e2095829ee67db738b3bfa9fa368c94900df327b3f07fe6e794d2fe1f"
-
-[[package]]
-name = "rustc-hash"
-version = "1.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2"
-
-[[package]]
-name = "shlex"
-version = "1.3.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64"
-
-[[package]]
-name = "slab"
-version = "0.4.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8f92a496fb766b417c996b9c5e57daf2f7ad3b0bebe1ccfca4856390e3d3bb67"
-dependencies = [
- "autocfg",
-]
-
-[[package]]
-name = "syn"
-version = "2.0.101"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8ce2b7fc941b3a24138a0a7cf8e858bfc6a992e7978a068a5c760deb0ed43caf"
-dependencies = [
- "proc-macro2",
- "quote",
- "unicode-ident",
-]
-
-[[package]]
-name = "tokio"
-version = "1.45.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "75ef51a33ef1da925cea3e4eb122833cb377c61439ca401b770f54902b806779"
-dependencies = [
- "backtrace",
- "pin-project-lite",
- "tokio-macros",
-]
-
-[[package]]
-name = "tokio-macros"
-version = "2.5.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6e06d43f1345a3bcd39f6a56dbb7dcab2ba47e68e8ac134855e7e2bdbaf8cab8"
-dependencies = [
- "proc-macro2",
- "quote",
- "syn",
-]
-
-[[package]]
-name = "unicode-ident"
-version = "1.0.18"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512"
-
-[[package]]
-name = "unique_identifier_msgs"
-version = "2.2.1"
-dependencies = [
- "rosidl_runtime_rs",
-]
-
-[[package]]
-name = "windows-targets"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973"
-dependencies = [
- "windows_aarch64_gnullvm 0.52.6",
- "windows_aarch64_msvc 0.52.6",
- "windows_i686_gnu 0.52.6",
- "windows_i686_gnullvm 0.52.6",
- "windows_i686_msvc 0.52.6",
- "windows_x86_64_gnu 0.52.6",
- "windows_x86_64_gnullvm 0.52.6",
- "windows_x86_64_msvc 0.52.6",
-]
-
-[[package]]
-name = "windows-targets"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b1e4c7e8ceaaf9cb7d7507c974735728ab453b67ef8f18febdd7c11fe59dca8b"
-dependencies = [
- "windows_aarch64_gnullvm 0.53.0",
- "windows_aarch64_msvc 0.53.0",
- "windows_i686_gnu 0.53.0",
- "windows_i686_gnullvm 0.53.0",
- "windows_i686_msvc 0.53.0",
- "windows_x86_64_gnu 0.53.0",
- "windows_x86_64_gnullvm 0.53.0",
- "windows_x86_64_msvc 0.53.0",
-]
-
-[[package]]
-name = "windows_aarch64_gnullvm"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3"
-
-[[package]]
-name = "windows_aarch64_gnullvm"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "86b8d5f90ddd19cb4a147a5fa63ca848db3df085e25fee3cc10b39b6eebae764"
-
-[[package]]
-name = "windows_aarch64_msvc"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469"
-
-[[package]]
-name = "windows_aarch64_msvc"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c7651a1f62a11b8cbd5e0d42526e55f2c99886c77e007179efff86c2b137e66c"
-
-[[package]]
-name = "windows_i686_gnu"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b"
-
-[[package]]
-name = "windows_i686_gnu"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c1dc67659d35f387f5f6c479dc4e28f1d4bb90ddd1a5d3da2e5d97b42d6272c3"
-
-[[package]]
-name = "windows_i686_gnullvm"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66"
-
-[[package]]
-name = "windows_i686_gnullvm"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9ce6ccbdedbf6d6354471319e781c0dfef054c81fbc7cf83f338a4296c0cae11"
-
-[[package]]
-name = "windows_i686_msvc"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66"
-
-[[package]]
-name = "windows_i686_msvc"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "581fee95406bb13382d2f65cd4a908ca7b1e4c2f1917f143ba16efe98a589b5d"
-
-[[package]]
-name = "windows_x86_64_gnu"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78"
-
-[[package]]
-name = "windows_x86_64_gnu"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2e55b5ac9ea33f2fc1716d1742db15574fd6fc8dadc51caab1c16a3d3b4190ba"
-
-[[package]]
-name = "windows_x86_64_gnullvm"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d"
-
-[[package]]
-name = "windows_x86_64_gnullvm"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0a6e035dd0599267ce1ee132e51c27dd29437f63325753051e71dd9e42406c57"
-
-[[package]]
-name = "windows_x86_64_msvc"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec"
-
-[[package]]
-name = "windows_x86_64_msvc"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486"
-
-[[patch.unused]]
-name = "rcl_interfaces"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "rclrs_example_msgs"
-version = "0.4.1"
-
-[[patch.unused]]
-name = "rosgraph_msgs"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "std_msgs"
-version = "4.8.0"
-
-[[patch.unused]]
-name = "test_msgs"
-version = "1.2.1"
diff --git a/examples/minimal_pub_sub/Cargo.lock b/examples/minimal_pub_sub/Cargo.lock
deleted file mode 100644
index 8dce706f..00000000
--- a/examples/minimal_pub_sub/Cargo.lock
+++ /dev/null
@@ -1,599 +0,0 @@
-# This file is automatically @generated by Cargo.
-# It is not intended for manual editing.
-version = 3
-
-[[package]]
-name = "addr2line"
-version = "0.24.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "dfbe277e56a376000877090da837660b4427aad530e3028d44e0bffe4f89a1c1"
-dependencies = [
- "gimli",
-]
-
-[[package]]
-name = "adler2"
-version = "2.0.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "512761e0bb2578dd7380c6baaa0f4ce03e84f95e960231d1dec8bf4d7d6e2627"
-
-[[package]]
-name = "aho-corasick"
-version = "1.1.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8e60d3430d3a69478ad0993f19238d2df97c507009a52b3c10addcd7f6bcb916"
-dependencies = [
- "memchr",
-]
-
-[[package]]
-name = "anyhow"
-version = "1.0.98"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e16d2d3311acee920a9eb8d33b8cbc1787ce4a264e85f964c2404b969bdcd487"
-dependencies = [
- "backtrace",
-]
-
-[[package]]
-name = "autocfg"
-version = "1.4.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26"
-
-[[package]]
-name = "backtrace"
-version = "0.3.74"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8d82cb332cdfaed17ae235a638438ac4d4839913cc2af585c3c6746e8f8bee1a"
-dependencies = [
- "addr2line",
- "cfg-if",
- "libc",
- "miniz_oxide",
- "object",
- "rustc-demangle",
- "windows-targets 0.52.6",
-]
-
-[[package]]
-name = "bindgen"
-version = "0.70.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f49d8fed880d473ea71efb9bf597651e77201bdd4893efe54c9e5d65ae04ce6f"
-dependencies = [
- "bitflags",
- "cexpr",
- "clang-sys",
- "itertools",
- "log",
- "prettyplease",
- "proc-macro2",
- "quote",
- "regex",
- "rustc-hash",
- "shlex",
- "syn",
-]
-
-[[package]]
-name = "bitflags"
-version = "2.9.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1b8e56985ec62d17e9c1001dc89c88ecd7dc08e47eba5ec7c29c7b5eeecde967"
-
-[[package]]
-name = "builtin_interfaces"
-version = "1.2.1"
-dependencies = [
- "rosidl_runtime_rs",
-]
-
-[[package]]
-name = "cexpr"
-version = "0.6.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6fac387a98bb7c37292057cffc56d62ecb629900026402633ae9160df93a8766"
-dependencies = [
- "nom",
-]
-
-[[package]]
-name = "cfg-if"
-version = "1.0.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd"
-
-[[package]]
-name = "clang-sys"
-version = "1.8.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0b023947811758c97c59bf9d1c188fd619ad4718dcaa767947df1cadb14f39f4"
-dependencies = [
- "glob",
- "libc",
- "libloading",
-]
-
-[[package]]
-name = "either"
-version = "1.15.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719"
-
-[[package]]
-name = "examples_rclrs_minimal_pub_sub"
-version = "0.4.1"
-dependencies = [
- "anyhow",
- "backtrace",
- "rclrs",
- "rosidl_runtime_rs",
- "std_msgs",
-]
-
-[[package]]
-name = "futures"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "65bc07b1a8bc7c85c5f2e110c476c7389b4554ba72af57d8445ea63a576b0876"
-dependencies = [
- "futures-channel",
- "futures-core",
- "futures-executor",
- "futures-io",
- "futures-sink",
- "futures-task",
- "futures-util",
-]
-
-[[package]]
-name = "futures-channel"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2dff15bf788c671c1934e366d07e30c1814a8ef514e1af724a602e8a2fbe1b10"
-dependencies = [
- "futures-core",
- "futures-sink",
-]
-
-[[package]]
-name = "futures-core"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "05f29059c0c2090612e8d742178b0580d2dc940c837851ad723096f87af6663e"
-
-[[package]]
-name = "futures-executor"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1e28d1d997f585e54aebc3f97d39e72338912123a67330d723fdbb564d646c9f"
-dependencies = [
- "futures-core",
- "futures-task",
- "futures-util",
-]
-
-[[package]]
-name = "futures-io"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9e5c1b78ca4aae1ac06c48a526a655760685149f0d465d21f37abfe57ce075c6"
-
-[[package]]
-name = "futures-macro"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "162ee34ebcb7c64a8abebc059ce0fee27c2262618d7b60ed8faf72fef13c3650"
-dependencies = [
- "proc-macro2",
- "quote",
- "syn",
-]
-
-[[package]]
-name = "futures-sink"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e575fab7d1e0dcb8d0c7bcf9a63ee213816ab51902e6d244a95819acacf1d4f7"
-
-[[package]]
-name = "futures-task"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f90f7dce0722e95104fcb095585910c0977252f286e354b5e3bd38902cd99988"
-
-[[package]]
-name = "futures-util"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9fa08315bb612088cc391249efdc3bc77536f16c91f6cf495e6fbe85b20a4a81"
-dependencies = [
- "futures-channel",
- "futures-core",
- "futures-io",
- "futures-macro",
- "futures-sink",
- "futures-task",
- "memchr",
- "pin-project-lite",
- "pin-utils",
- "slab",
-]
-
-[[package]]
-name = "gimli"
-version = "0.31.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "07e28edb80900c19c28f1072f2e8aeca7fa06b23cd4169cefe1af5aa3260783f"
-
-[[package]]
-name = "glob"
-version = "0.3.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a8d1add55171497b4705a648c6b583acafb01d58050a51727785f0b2c8e0a2b2"
-
-[[package]]
-name = "itertools"
-version = "0.13.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "413ee7dfc52ee1a4949ceeb7dbc8a33f2d6c088194d9f922fb8318faf1f01186"
-dependencies = [
- "either",
-]
-
-[[package]]
-name = "libc"
-version = "0.2.172"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d750af042f7ef4f724306de029d18836c26c1765a54a6a3f094cbd23a7267ffa"
-
-[[package]]
-name = "libloading"
-version = "0.8.8"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "07033963ba89ebaf1584d767badaa2e8fcec21aedea6b8c0346d487d49c28667"
-dependencies = [
- "cfg-if",
- "windows-targets 0.53.0",
-]
-
-[[package]]
-name = "log"
-version = "0.4.27"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94"
-
-[[package]]
-name = "memchr"
-version = "2.7.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "78ca9ab1a0babb1e7d5695e3530886289c18cf2f87ec19a575a0abdce112e3a3"
-
-[[package]]
-name = "minimal-lexical"
-version = "0.2.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a"
-
-[[package]]
-name = "miniz_oxide"
-version = "0.8.8"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3be647b768db090acb35d5ec5db2b0e1f1de11133ca123b9eacf5137868f892a"
-dependencies = [
- "adler2",
-]
-
-[[package]]
-name = "nom"
-version = "7.1.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d273983c5a657a70a3e8f2a01329822f3b8c8172b73826411a55751e404a0a4a"
-dependencies = [
- "memchr",
- "minimal-lexical",
-]
-
-[[package]]
-name = "object"
-version = "0.36.7"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "62948e14d923ea95ea2c7c86c71013138b66525b86bdc08d2dcc262bdb497b87"
-dependencies = [
- "memchr",
-]
-
-[[package]]
-name = "pin-project-lite"
-version = "0.2.16"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3b3cff922bd51709b605d9ead9aa71031d81447142d828eb4a6eba76fe619f9b"
-
-[[package]]
-name = "pin-utils"
-version = "0.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"
-
-[[package]]
-name = "prettyplease"
-version = "0.2.32"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "664ec5419c51e34154eec046ebcba56312d5a2fc3b09a06da188e1ad21afadf6"
-dependencies = [
- "proc-macro2",
- "syn",
-]
-
-[[package]]
-name = "proc-macro2"
-version = "1.0.95"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "02b3e5e68a3a1a02aad3ec490a98007cbc13c37cbe84a3cd7b8e406d76e7f778"
-dependencies = [
- "unicode-ident",
-]
-
-[[package]]
-name = "quote"
-version = "1.0.40"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d"
-dependencies = [
- "proc-macro2",
-]
-
-[[package]]
-name = "rclrs"
-version = "0.4.1"
-dependencies = [
- "bindgen",
- "cfg-if",
- "futures",
- "rosidl_runtime_rs",
-]
-
-[[package]]
-name = "regex"
-version = "1.11.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b544ef1b4eac5dc2db33ea63606ae9ffcfac26c1416a2806ae0bf5f56b201191"
-dependencies = [
- "aho-corasick",
- "memchr",
- "regex-automata",
- "regex-syntax",
-]
-
-[[package]]
-name = "regex-automata"
-version = "0.4.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "809e8dc61f6de73b46c85f4c96486310fe304c434cfa43669d7b40f711150908"
-dependencies = [
- "aho-corasick",
- "memchr",
- "regex-syntax",
-]
-
-[[package]]
-name = "regex-syntax"
-version = "0.8.5"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2b15c43186be67a4fd63bee50d0303afffcef381492ebe2c5d87f324e1b8815c"
-
-[[package]]
-name = "rosidl_runtime_rs"
-version = "0.4.1"
-dependencies = [
- "cfg-if",
-]
-
-[[package]]
-name = "rustc-demangle"
-version = "0.1.24"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "719b953e2095829ee67db738b3bfa9fa368c94900df327b3f07fe6e794d2fe1f"
-
-[[package]]
-name = "rustc-hash"
-version = "1.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2"
-
-[[package]]
-name = "shlex"
-version = "1.3.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64"
-
-[[package]]
-name = "slab"
-version = "0.4.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8f92a496fb766b417c996b9c5e57daf2f7ad3b0bebe1ccfca4856390e3d3bb67"
-dependencies = [
- "autocfg",
-]
-
-[[package]]
-name = "std_msgs"
-version = "4.8.0"
-dependencies = [
- "builtin_interfaces",
- "rosidl_runtime_rs",
-]
-
-[[package]]
-name = "syn"
-version = "2.0.101"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8ce2b7fc941b3a24138a0a7cf8e858bfc6a992e7978a068a5c760deb0ed43caf"
-dependencies = [
- "proc-macro2",
- "quote",
- "unicode-ident",
-]
-
-[[package]]
-name = "unicode-ident"
-version = "1.0.18"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512"
-
-[[package]]
-name = "windows-targets"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973"
-dependencies = [
- "windows_aarch64_gnullvm 0.52.6",
- "windows_aarch64_msvc 0.52.6",
- "windows_i686_gnu 0.52.6",
- "windows_i686_gnullvm 0.52.6",
- "windows_i686_msvc 0.52.6",
- "windows_x86_64_gnu 0.52.6",
- "windows_x86_64_gnullvm 0.52.6",
- "windows_x86_64_msvc 0.52.6",
-]
-
-[[package]]
-name = "windows-targets"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b1e4c7e8ceaaf9cb7d7507c974735728ab453b67ef8f18febdd7c11fe59dca8b"
-dependencies = [
- "windows_aarch64_gnullvm 0.53.0",
- "windows_aarch64_msvc 0.53.0",
- "windows_i686_gnu 0.53.0",
- "windows_i686_gnullvm 0.53.0",
- "windows_i686_msvc 0.53.0",
- "windows_x86_64_gnu 0.53.0",
- "windows_x86_64_gnullvm 0.53.0",
- "windows_x86_64_msvc 0.53.0",
-]
-
-[[package]]
-name = "windows_aarch64_gnullvm"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3"
-
-[[package]]
-name = "windows_aarch64_gnullvm"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "86b8d5f90ddd19cb4a147a5fa63ca848db3df085e25fee3cc10b39b6eebae764"
-
-[[package]]
-name = "windows_aarch64_msvc"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469"
-
-[[package]]
-name = "windows_aarch64_msvc"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c7651a1f62a11b8cbd5e0d42526e55f2c99886c77e007179efff86c2b137e66c"
-
-[[package]]
-name = "windows_i686_gnu"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b"
-
-[[package]]
-name = "windows_i686_gnu"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c1dc67659d35f387f5f6c479dc4e28f1d4bb90ddd1a5d3da2e5d97b42d6272c3"
-
-[[package]]
-name = "windows_i686_gnullvm"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66"
-
-[[package]]
-name = "windows_i686_gnullvm"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9ce6ccbdedbf6d6354471319e781c0dfef054c81fbc7cf83f338a4296c0cae11"
-
-[[package]]
-name = "windows_i686_msvc"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66"
-
-[[package]]
-name = "windows_i686_msvc"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "581fee95406bb13382d2f65cd4a908ca7b1e4c2f1917f143ba16efe98a589b5d"
-
-[[package]]
-name = "windows_x86_64_gnu"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78"
-
-[[package]]
-name = "windows_x86_64_gnu"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2e55b5ac9ea33f2fc1716d1742db15574fd6fc8dadc51caab1c16a3d3b4190ba"
-
-[[package]]
-name = "windows_x86_64_gnullvm"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d"
-
-[[package]]
-name = "windows_x86_64_gnullvm"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0a6e035dd0599267ce1ee132e51c27dd29437f63325753051e71dd9e42406c57"
-
-[[package]]
-name = "windows_x86_64_msvc"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec"
-
-[[package]]
-name = "windows_x86_64_msvc"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486"
-
-[[patch.unused]]
-name = "action_msgs"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "example_interfaces"
-version = "0.9.3"
-
-[[patch.unused]]
-name = "rcl_interfaces"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "rclrs_example_msgs"
-version = "0.4.1"
-
-[[patch.unused]]
-name = "rosgraph_msgs"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "test_msgs"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "unique_identifier_msgs"
-version = "2.2.1"
diff --git a/examples/rust_pubsub/Cargo.lock b/examples/rust_pubsub/Cargo.lock
deleted file mode 100644
index fb94ffeb..00000000
--- a/examples/rust_pubsub/Cargo.lock
+++ /dev/null
@@ -1,463 +0,0 @@
-# This file is automatically @generated by Cargo.
-# It is not intended for manual editing.
-version = 3
-
-[[package]]
-name = "aho-corasick"
-version = "1.1.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8e60d3430d3a69478ad0993f19238d2df97c507009a52b3c10addcd7f6bcb916"
-dependencies = [
- "memchr",
-]
-
-[[package]]
-name = "autocfg"
-version = "1.4.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26"
-
-[[package]]
-name = "bindgen"
-version = "0.70.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f49d8fed880d473ea71efb9bf597651e77201bdd4893efe54c9e5d65ae04ce6f"
-dependencies = [
- "bitflags",
- "cexpr",
- "clang-sys",
- "itertools",
- "log",
- "prettyplease",
- "proc-macro2",
- "quote",
- "regex",
- "rustc-hash",
- "shlex",
- "syn",
-]
-
-[[package]]
-name = "bitflags"
-version = "2.9.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1b8e56985ec62d17e9c1001dc89c88ecd7dc08e47eba5ec7c29c7b5eeecde967"
-
-[[package]]
-name = "builtin_interfaces"
-version = "1.2.1"
-dependencies = [
- "rosidl_runtime_rs",
-]
-
-[[package]]
-name = "cexpr"
-version = "0.6.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6fac387a98bb7c37292057cffc56d62ecb629900026402633ae9160df93a8766"
-dependencies = [
- "nom",
-]
-
-[[package]]
-name = "cfg-if"
-version = "1.0.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd"
-
-[[package]]
-name = "clang-sys"
-version = "1.8.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0b023947811758c97c59bf9d1c188fd619ad4718dcaa767947df1cadb14f39f4"
-dependencies = [
- "glob",
- "libc",
- "libloading",
-]
-
-[[package]]
-name = "either"
-version = "1.15.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719"
-
-[[package]]
-name = "futures"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "65bc07b1a8bc7c85c5f2e110c476c7389b4554ba72af57d8445ea63a576b0876"
-dependencies = [
- "futures-channel",
- "futures-core",
- "futures-executor",
- "futures-io",
- "futures-sink",
- "futures-task",
- "futures-util",
-]
-
-[[package]]
-name = "futures-channel"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2dff15bf788c671c1934e366d07e30c1814a8ef514e1af724a602e8a2fbe1b10"
-dependencies = [
- "futures-core",
- "futures-sink",
-]
-
-[[package]]
-name = "futures-core"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "05f29059c0c2090612e8d742178b0580d2dc940c837851ad723096f87af6663e"
-
-[[package]]
-name = "futures-executor"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1e28d1d997f585e54aebc3f97d39e72338912123a67330d723fdbb564d646c9f"
-dependencies = [
- "futures-core",
- "futures-task",
- "futures-util",
-]
-
-[[package]]
-name = "futures-io"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9e5c1b78ca4aae1ac06c48a526a655760685149f0d465d21f37abfe57ce075c6"
-
-[[package]]
-name = "futures-macro"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "162ee34ebcb7c64a8abebc059ce0fee27c2262618d7b60ed8faf72fef13c3650"
-dependencies = [
- "proc-macro2",
- "quote",
- "syn",
-]
-
-[[package]]
-name = "futures-sink"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e575fab7d1e0dcb8d0c7bcf9a63ee213816ab51902e6d244a95819acacf1d4f7"
-
-[[package]]
-name = "futures-task"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f90f7dce0722e95104fcb095585910c0977252f286e354b5e3bd38902cd99988"
-
-[[package]]
-name = "futures-util"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9fa08315bb612088cc391249efdc3bc77536f16c91f6cf495e6fbe85b20a4a81"
-dependencies = [
- "futures-channel",
- "futures-core",
- "futures-io",
- "futures-macro",
- "futures-sink",
- "futures-task",
- "memchr",
- "pin-project-lite",
- "pin-utils",
- "slab",
-]
-
-[[package]]
-name = "glob"
-version = "0.3.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a8d1add55171497b4705a648c6b583acafb01d58050a51727785f0b2c8e0a2b2"
-
-[[package]]
-name = "itertools"
-version = "0.13.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "413ee7dfc52ee1a4949ceeb7dbc8a33f2d6c088194d9f922fb8318faf1f01186"
-dependencies = [
- "either",
-]
-
-[[package]]
-name = "libc"
-version = "0.2.172"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d750af042f7ef4f724306de029d18836c26c1765a54a6a3f094cbd23a7267ffa"
-
-[[package]]
-name = "libloading"
-version = "0.8.8"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "07033963ba89ebaf1584d767badaa2e8fcec21aedea6b8c0346d487d49c28667"
-dependencies = [
- "cfg-if",
- "windows-targets",
-]
-
-[[package]]
-name = "log"
-version = "0.4.27"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94"
-
-[[package]]
-name = "memchr"
-version = "2.7.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "78ca9ab1a0babb1e7d5695e3530886289c18cf2f87ec19a575a0abdce112e3a3"
-
-[[package]]
-name = "minimal-lexical"
-version = "0.2.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a"
-
-[[package]]
-name = "nom"
-version = "7.1.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d273983c5a657a70a3e8f2a01329822f3b8c8172b73826411a55751e404a0a4a"
-dependencies = [
- "memchr",
- "minimal-lexical",
-]
-
-[[package]]
-name = "pin-project-lite"
-version = "0.2.16"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3b3cff922bd51709b605d9ead9aa71031d81447142d828eb4a6eba76fe619f9b"
-
-[[package]]
-name = "pin-utils"
-version = "0.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"
-
-[[package]]
-name = "prettyplease"
-version = "0.2.32"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "664ec5419c51e34154eec046ebcba56312d5a2fc3b09a06da188e1ad21afadf6"
-dependencies = [
- "proc-macro2",
- "syn",
-]
-
-[[package]]
-name = "proc-macro2"
-version = "1.0.95"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "02b3e5e68a3a1a02aad3ec490a98007cbc13c37cbe84a3cd7b8e406d76e7f778"
-dependencies = [
- "unicode-ident",
-]
-
-[[package]]
-name = "quote"
-version = "1.0.40"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d"
-dependencies = [
- "proc-macro2",
-]
-
-[[package]]
-name = "rclrs"
-version = "0.4.1"
-dependencies = [
- "bindgen",
- "cfg-if",
- "futures",
- "rosidl_runtime_rs",
-]
-
-[[package]]
-name = "regex"
-version = "1.11.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b544ef1b4eac5dc2db33ea63606ae9ffcfac26c1416a2806ae0bf5f56b201191"
-dependencies = [
- "aho-corasick",
- "memchr",
- "regex-automata",
- "regex-syntax",
-]
-
-[[package]]
-name = "regex-automata"
-version = "0.4.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "809e8dc61f6de73b46c85f4c96486310fe304c434cfa43669d7b40f711150908"
-dependencies = [
- "aho-corasick",
- "memchr",
- "regex-syntax",
-]
-
-[[package]]
-name = "regex-syntax"
-version = "0.8.5"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2b15c43186be67a4fd63bee50d0303afffcef381492ebe2c5d87f324e1b8815c"
-
-[[package]]
-name = "rosidl_runtime_rs"
-version = "0.4.1"
-dependencies = [
- "cfg-if",
-]
-
-[[package]]
-name = "rust_pubsub"
-version = "0.1.0"
-dependencies = [
- "rclrs",
- "std_msgs",
-]
-
-[[package]]
-name = "rustc-hash"
-version = "1.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2"
-
-[[package]]
-name = "shlex"
-version = "1.3.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64"
-
-[[package]]
-name = "slab"
-version = "0.4.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8f92a496fb766b417c996b9c5e57daf2f7ad3b0bebe1ccfca4856390e3d3bb67"
-dependencies = [
- "autocfg",
-]
-
-[[package]]
-name = "std_msgs"
-version = "4.8.0"
-dependencies = [
- "builtin_interfaces",
- "rosidl_runtime_rs",
-]
-
-[[package]]
-name = "syn"
-version = "2.0.101"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8ce2b7fc941b3a24138a0a7cf8e858bfc6a992e7978a068a5c760deb0ed43caf"
-dependencies = [
- "proc-macro2",
- "quote",
- "unicode-ident",
-]
-
-[[package]]
-name = "unicode-ident"
-version = "1.0.18"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512"
-
-[[package]]
-name = "windows-targets"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b1e4c7e8ceaaf9cb7d7507c974735728ab453b67ef8f18febdd7c11fe59dca8b"
-dependencies = [
- "windows_aarch64_gnullvm",
- "windows_aarch64_msvc",
- "windows_i686_gnu",
- "windows_i686_gnullvm",
- "windows_i686_msvc",
- "windows_x86_64_gnu",
- "windows_x86_64_gnullvm",
- "windows_x86_64_msvc",
-]
-
-[[package]]
-name = "windows_aarch64_gnullvm"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "86b8d5f90ddd19cb4a147a5fa63ca848db3df085e25fee3cc10b39b6eebae764"
-
-[[package]]
-name = "windows_aarch64_msvc"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c7651a1f62a11b8cbd5e0d42526e55f2c99886c77e007179efff86c2b137e66c"
-
-[[package]]
-name = "windows_i686_gnu"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c1dc67659d35f387f5f6c479dc4e28f1d4bb90ddd1a5d3da2e5d97b42d6272c3"
-
-[[package]]
-name = "windows_i686_gnullvm"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9ce6ccbdedbf6d6354471319e781c0dfef054c81fbc7cf83f338a4296c0cae11"
-
-[[package]]
-name = "windows_i686_msvc"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "581fee95406bb13382d2f65cd4a908ca7b1e4c2f1917f143ba16efe98a589b5d"
-
-[[package]]
-name = "windows_x86_64_gnu"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2e55b5ac9ea33f2fc1716d1742db15574fd6fc8dadc51caab1c16a3d3b4190ba"
-
-[[package]]
-name = "windows_x86_64_gnullvm"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0a6e035dd0599267ce1ee132e51c27dd29437f63325753051e71dd9e42406c57"
-
-[[package]]
-name = "windows_x86_64_msvc"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486"
-
-[[patch.unused]]
-name = "action_msgs"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "example_interfaces"
-version = "0.9.3"
-
-[[patch.unused]]
-name = "rcl_interfaces"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "rclrs_example_msgs"
-version = "0.4.1"
-
-[[patch.unused]]
-name = "rosgraph_msgs"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "test_msgs"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "unique_identifier_msgs"
-version = "2.2.1"
diff --git a/rclrs/Cargo.lock b/rclrs/Cargo.lock
deleted file mode 100644
index 5fc5e7bd..00000000
--- a/rclrs/Cargo.lock
+++ /dev/null
@@ -1,782 +0,0 @@
-# This file is automatically @generated by Cargo.
-# It is not intended for manual editing.
-version = 3
-
-[[package]]
-name = "action_msgs"
-version = "1.2.1"
-dependencies = [
- "builtin_interfaces",
- "rosidl_runtime_rs",
- "unique_identifier_msgs",
-]
-
-[[package]]
-name = "addr2line"
-version = "0.24.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "dfbe277e56a376000877090da837660b4427aad530e3028d44e0bffe4f89a1c1"
-dependencies = [
- "gimli",
-]
-
-[[package]]
-name = "adler2"
-version = "2.0.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "512761e0bb2578dd7380c6baaa0f4ce03e84f95e960231d1dec8bf4d7d6e2627"
-
-[[package]]
-name = "aho-corasick"
-version = "1.1.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8e60d3430d3a69478ad0993f19238d2df97c507009a52b3c10addcd7f6bcb916"
-dependencies = [
- "memchr",
-]
-
-[[package]]
-name = "ament_rs"
-version = "0.2.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b901da946b7b1620d44563e10c7634681af855a7f5fb59bd09b6eb801dcf6e49"
-dependencies = [
- "itertools 0.8.2",
- "walkdir",
-]
-
-[[package]]
-name = "autocfg"
-version = "1.4.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26"
-
-[[package]]
-name = "backtrace"
-version = "0.3.75"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6806a6321ec58106fea15becdad98371e28d92ccbc7c8f1b3b6dd724fe8f1002"
-dependencies = [
- "addr2line",
- "cfg-if",
- "libc",
- "miniz_oxide",
- "object",
- "rustc-demangle",
- "windows-targets 0.52.6",
-]
-
-[[package]]
-name = "bindgen"
-version = "0.70.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f49d8fed880d473ea71efb9bf597651e77201bdd4893efe54c9e5d65ae04ce6f"
-dependencies = [
- "bitflags",
- "cexpr",
- "clang-sys",
- "itertools 0.13.0",
- "log",
- "prettyplease",
- "proc-macro2",
- "quote",
- "regex",
- "rustc-hash",
- "shlex",
- "syn",
-]
-
-[[package]]
-name = "bitflags"
-version = "2.9.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1b8e56985ec62d17e9c1001dc89c88ecd7dc08e47eba5ec7c29c7b5eeecde967"
-
-[[package]]
-name = "builtin_interfaces"
-version = "1.2.1"
-dependencies = [
- "rosidl_runtime_rs",
-]
-
-[[package]]
-name = "cexpr"
-version = "0.6.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6fac387a98bb7c37292057cffc56d62ecb629900026402633ae9160df93a8766"
-dependencies = [
- "nom",
-]
-
-[[package]]
-name = "cfg-if"
-version = "1.0.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd"
-
-[[package]]
-name = "clang-sys"
-version = "1.8.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0b023947811758c97c59bf9d1c188fd619ad4718dcaa767947df1cadb14f39f4"
-dependencies = [
- "glob",
- "libc",
- "libloading",
-]
-
-[[package]]
-name = "either"
-version = "1.15.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719"
-
-[[package]]
-name = "errno"
-version = "0.3.12"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "cea14ef9355e3beab063703aa9dab15afd25f0667c341310c1e5274bb1d0da18"
-dependencies = [
- "libc",
- "windows-sys",
-]
-
-[[package]]
-name = "fastrand"
-version = "2.3.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "37909eebbb50d72f9059c3b6d82c0463f2ff062c9e95845c43a6c9c0355411be"
-
-[[package]]
-name = "futures"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "65bc07b1a8bc7c85c5f2e110c476c7389b4554ba72af57d8445ea63a576b0876"
-dependencies = [
- "futures-channel",
- "futures-core",
- "futures-executor",
- "futures-io",
- "futures-sink",
- "futures-task",
- "futures-util",
-]
-
-[[package]]
-name = "futures-channel"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2dff15bf788c671c1934e366d07e30c1814a8ef514e1af724a602e8a2fbe1b10"
-dependencies = [
- "futures-core",
- "futures-sink",
-]
-
-[[package]]
-name = "futures-core"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "05f29059c0c2090612e8d742178b0580d2dc940c837851ad723096f87af6663e"
-
-[[package]]
-name = "futures-executor"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1e28d1d997f585e54aebc3f97d39e72338912123a67330d723fdbb564d646c9f"
-dependencies = [
- "futures-core",
- "futures-task",
- "futures-util",
-]
-
-[[package]]
-name = "futures-io"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9e5c1b78ca4aae1ac06c48a526a655760685149f0d465d21f37abfe57ce075c6"
-
-[[package]]
-name = "futures-macro"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "162ee34ebcb7c64a8abebc059ce0fee27c2262618d7b60ed8faf72fef13c3650"
-dependencies = [
- "proc-macro2",
- "quote",
- "syn",
-]
-
-[[package]]
-name = "futures-sink"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e575fab7d1e0dcb8d0c7bcf9a63ee213816ab51902e6d244a95819acacf1d4f7"
-
-[[package]]
-name = "futures-task"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f90f7dce0722e95104fcb095585910c0977252f286e354b5e3bd38902cd99988"
-
-[[package]]
-name = "futures-util"
-version = "0.3.31"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9fa08315bb612088cc391249efdc3bc77536f16c91f6cf495e6fbe85b20a4a81"
-dependencies = [
- "futures-channel",
- "futures-core",
- "futures-io",
- "futures-macro",
- "futures-sink",
- "futures-task",
- "memchr",
- "pin-project-lite",
- "pin-utils",
- "slab",
-]
-
-[[package]]
-name = "getrandom"
-version = "0.3.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "26145e563e54f2cadc477553f1ec5ee650b00862f0a58bcd12cbdc5f0ea2d2f4"
-dependencies = [
- "cfg-if",
- "libc",
- "r-efi",
- "wasi",
-]
-
-[[package]]
-name = "gimli"
-version = "0.31.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "07e28edb80900c19c28f1072f2e8aeca7fa06b23cd4169cefe1af5aa3260783f"
-
-[[package]]
-name = "glob"
-version = "0.3.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a8d1add55171497b4705a648c6b583acafb01d58050a51727785f0b2c8e0a2b2"
-
-[[package]]
-name = "itertools"
-version = "0.8.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f56a2d0bc861f9165be4eb3442afd3c236d8a98afd426f65d92324ae1091a484"
-dependencies = [
- "either",
-]
-
-[[package]]
-name = "itertools"
-version = "0.13.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "413ee7dfc52ee1a4949ceeb7dbc8a33f2d6c088194d9f922fb8318faf1f01186"
-dependencies = [
- "either",
-]
-
-[[package]]
-name = "libc"
-version = "0.2.172"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d750af042f7ef4f724306de029d18836c26c1765a54a6a3f094cbd23a7267ffa"
-
-[[package]]
-name = "libloading"
-version = "0.8.8"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "07033963ba89ebaf1584d767badaa2e8fcec21aedea6b8c0346d487d49c28667"
-dependencies = [
- "cfg-if",
- "windows-targets 0.53.0",
-]
-
-[[package]]
-name = "linux-raw-sys"
-version = "0.9.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "cd945864f07fe9f5371a27ad7b52a172b4b499999f1d97574c9fa68373937e12"
-
-[[package]]
-name = "log"
-version = "0.4.27"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94"
-
-[[package]]
-name = "memchr"
-version = "2.7.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "78ca9ab1a0babb1e7d5695e3530886289c18cf2f87ec19a575a0abdce112e3a3"
-
-[[package]]
-name = "minimal-lexical"
-version = "0.2.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a"
-
-[[package]]
-name = "miniz_oxide"
-version = "0.8.8"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3be647b768db090acb35d5ec5db2b0e1f1de11133ca123b9eacf5137868f892a"
-dependencies = [
- "adler2",
-]
-
-[[package]]
-name = "nom"
-version = "7.1.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d273983c5a657a70a3e8f2a01329822f3b8c8172b73826411a55751e404a0a4a"
-dependencies = [
- "memchr",
- "minimal-lexical",
-]
-
-[[package]]
-name = "object"
-version = "0.36.7"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "62948e14d923ea95ea2c7c86c71013138b66525b86bdc08d2dcc262bdb497b87"
-dependencies = [
- "memchr",
-]
-
-[[package]]
-name = "once_cell"
-version = "1.21.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d"
-
-[[package]]
-name = "pin-project-lite"
-version = "0.2.16"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3b3cff922bd51709b605d9ead9aa71031d81447142d828eb4a6eba76fe619f9b"
-
-[[package]]
-name = "pin-utils"
-version = "0.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"
-
-[[package]]
-name = "prettyplease"
-version = "0.2.32"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "664ec5419c51e34154eec046ebcba56312d5a2fc3b09a06da188e1ad21afadf6"
-dependencies = [
- "proc-macro2",
- "syn",
-]
-
-[[package]]
-name = "proc-macro2"
-version = "1.0.95"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "02b3e5e68a3a1a02aad3ec490a98007cbc13c37cbe84a3cd7b8e406d76e7f778"
-dependencies = [
- "unicode-ident",
-]
-
-[[package]]
-name = "quote"
-version = "1.0.40"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d"
-dependencies = [
- "proc-macro2",
-]
-
-[[package]]
-name = "r-efi"
-version = "5.2.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "74765f6d916ee2faa39bc8e68e4f3ed8949b48cccdac59983d287a7cb71ce9c5"
-
-[[package]]
-name = "rclrs"
-version = "0.4.1"
-dependencies = [
- "ament_rs",
- "bindgen",
- "cfg-if",
- "futures",
- "libloading",
- "rosidl_runtime_rs",
- "serde",
- "serde-big-array",
- "tempfile",
- "test_msgs",
- "tokio",
-]
-
-[[package]]
-name = "regex"
-version = "1.11.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b544ef1b4eac5dc2db33ea63606ae9ffcfac26c1416a2806ae0bf5f56b201191"
-dependencies = [
- "aho-corasick",
- "memchr",
- "regex-automata",
- "regex-syntax",
-]
-
-[[package]]
-name = "regex-automata"
-version = "0.4.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "809e8dc61f6de73b46c85f4c96486310fe304c434cfa43669d7b40f711150908"
-dependencies = [
- "aho-corasick",
- "memchr",
- "regex-syntax",
-]
-
-[[package]]
-name = "regex-syntax"
-version = "0.8.5"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2b15c43186be67a4fd63bee50d0303afffcef381492ebe2c5d87f324e1b8815c"
-
-[[package]]
-name = "rosidl_runtime_rs"
-version = "0.4.1"
-dependencies = [
- "cfg-if",
- "serde",
-]
-
-[[package]]
-name = "rustc-demangle"
-version = "0.1.24"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "719b953e2095829ee67db738b3bfa9fa368c94900df327b3f07fe6e794d2fe1f"
-
-[[package]]
-name = "rustc-hash"
-version = "1.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2"
-
-[[package]]
-name = "rustix"
-version = "1.0.7"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c71e83d6afe7ff64890ec6b71d6a69bb8a610ab78ce364b3352876bb4c801266"
-dependencies = [
- "bitflags",
- "errno",
- "libc",
- "linux-raw-sys",
- "windows-sys",
-]
-
-[[package]]
-name = "same-file"
-version = "1.0.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "93fc1dc3aaa9bfed95e02e6eadabb4baf7e3078b0bd1b4d7b6b0b68378900502"
-dependencies = [
- "winapi-util",
-]
-
-[[package]]
-name = "serde"
-version = "1.0.219"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5f0e2c6ed6606019b4e29e69dbaba95b11854410e5347d525002456dbbb786b6"
-dependencies = [
- "serde_derive",
-]
-
-[[package]]
-name = "serde-big-array"
-version = "0.5.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "11fc7cc2c76d73e0f27ee52abbd64eec84d46f370c88371120433196934e4b7f"
-dependencies = [
- "serde",
-]
-
-[[package]]
-name = "serde_derive"
-version = "1.0.219"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5b0276cf7f2c73365f7157c8123c21cd9a50fbbd844757af28ca1f5925fc2a00"
-dependencies = [
- "proc-macro2",
- "quote",
- "syn",
-]
-
-[[package]]
-name = "shlex"
-version = "1.3.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64"
-
-[[package]]
-name = "slab"
-version = "0.4.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8f92a496fb766b417c996b9c5e57daf2f7ad3b0bebe1ccfca4856390e3d3bb67"
-dependencies = [
- "autocfg",
-]
-
-[[package]]
-name = "syn"
-version = "2.0.101"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8ce2b7fc941b3a24138a0a7cf8e858bfc6a992e7978a068a5c760deb0ed43caf"
-dependencies = [
- "proc-macro2",
- "quote",
- "unicode-ident",
-]
-
-[[package]]
-name = "tempfile"
-version = "3.20.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e8a64e3985349f2441a1a9ef0b853f869006c3855f2cda6862a94d26ebb9d6a1"
-dependencies = [
- "fastrand",
- "getrandom",
- "once_cell",
- "rustix",
- "windows-sys",
-]
-
-[[package]]
-name = "test_msgs"
-version = "1.2.1"
-dependencies = [
- "action_msgs",
- "builtin_interfaces",
- "rosidl_runtime_rs",
- "unique_identifier_msgs",
-]
-
-[[package]]
-name = "tokio"
-version = "1.45.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "75ef51a33ef1da925cea3e4eb122833cb377c61439ca401b770f54902b806779"
-dependencies = [
- "backtrace",
- "pin-project-lite",
- "tokio-macros",
-]
-
-[[package]]
-name = "tokio-macros"
-version = "2.5.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6e06d43f1345a3bcd39f6a56dbb7dcab2ba47e68e8ac134855e7e2bdbaf8cab8"
-dependencies = [
- "proc-macro2",
- "quote",
- "syn",
-]
-
-[[package]]
-name = "unicode-ident"
-version = "1.0.18"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512"
-
-[[package]]
-name = "unique_identifier_msgs"
-version = "2.2.1"
-dependencies = [
- "rosidl_runtime_rs",
-]
-
-[[package]]
-name = "walkdir"
-version = "2.5.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "29790946404f91d9c5d06f9874efddea1dc06c5efe94541a7d6863108e3a5e4b"
-dependencies = [
- "same-file",
- "winapi-util",
-]
-
-[[package]]
-name = "wasi"
-version = "0.14.2+wasi-0.2.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9683f9a5a998d873c0d21fcbe3c083009670149a8fab228644b8bd36b2c48cb3"
-dependencies = [
- "wit-bindgen-rt",
-]
-
-[[package]]
-name = "winapi-util"
-version = "0.1.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "cf221c93e13a30d793f7645a0e7762c55d169dbb0a49671918a2319d289b10bb"
-dependencies = [
- "windows-sys",
-]
-
-[[package]]
-name = "windows-sys"
-version = "0.59.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1e38bc4d79ed67fd075bcc251a1c39b32a1776bbe92e5bef1f0bf1f8c531853b"
-dependencies = [
- "windows-targets 0.52.6",
-]
-
-[[package]]
-name = "windows-targets"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973"
-dependencies = [
- "windows_aarch64_gnullvm 0.52.6",
- "windows_aarch64_msvc 0.52.6",
- "windows_i686_gnu 0.52.6",
- "windows_i686_gnullvm 0.52.6",
- "windows_i686_msvc 0.52.6",
- "windows_x86_64_gnu 0.52.6",
- "windows_x86_64_gnullvm 0.52.6",
- "windows_x86_64_msvc 0.52.6",
-]
-
-[[package]]
-name = "windows-targets"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b1e4c7e8ceaaf9cb7d7507c974735728ab453b67ef8f18febdd7c11fe59dca8b"
-dependencies = [
- "windows_aarch64_gnullvm 0.53.0",
- "windows_aarch64_msvc 0.53.0",
- "windows_i686_gnu 0.53.0",
- "windows_i686_gnullvm 0.53.0",
- "windows_i686_msvc 0.53.0",
- "windows_x86_64_gnu 0.53.0",
- "windows_x86_64_gnullvm 0.53.0",
- "windows_x86_64_msvc 0.53.0",
-]
-
-[[package]]
-name = "windows_aarch64_gnullvm"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3"
-
-[[package]]
-name = "windows_aarch64_gnullvm"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "86b8d5f90ddd19cb4a147a5fa63ca848db3df085e25fee3cc10b39b6eebae764"
-
-[[package]]
-name = "windows_aarch64_msvc"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469"
-
-[[package]]
-name = "windows_aarch64_msvc"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c7651a1f62a11b8cbd5e0d42526e55f2c99886c77e007179efff86c2b137e66c"
-
-[[package]]
-name = "windows_i686_gnu"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b"
-
-[[package]]
-name = "windows_i686_gnu"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c1dc67659d35f387f5f6c479dc4e28f1d4bb90ddd1a5d3da2e5d97b42d6272c3"
-
-[[package]]
-name = "windows_i686_gnullvm"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66"
-
-[[package]]
-name = "windows_i686_gnullvm"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9ce6ccbdedbf6d6354471319e781c0dfef054c81fbc7cf83f338a4296c0cae11"
-
-[[package]]
-name = "windows_i686_msvc"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66"
-
-[[package]]
-name = "windows_i686_msvc"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "581fee95406bb13382d2f65cd4a908ca7b1e4c2f1917f143ba16efe98a589b5d"
-
-[[package]]
-name = "windows_x86_64_gnu"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78"
-
-[[package]]
-name = "windows_x86_64_gnu"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2e55b5ac9ea33f2fc1716d1742db15574fd6fc8dadc51caab1c16a3d3b4190ba"
-
-[[package]]
-name = "windows_x86_64_gnullvm"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d"
-
-[[package]]
-name = "windows_x86_64_gnullvm"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0a6e035dd0599267ce1ee132e51c27dd29437f63325753051e71dd9e42406c57"
-
-[[package]]
-name = "windows_x86_64_msvc"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec"
-
-[[package]]
-name = "windows_x86_64_msvc"
-version = "0.53.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486"
-
-[[package]]
-name = "wit-bindgen-rt"
-version = "0.39.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6f42320e61fe2cfd34354ecb597f86f413484a798ba44a8ca1165c58d42da6c1"
-dependencies = [
- "bitflags",
-]
-
-[[patch.unused]]
-name = "rcl_interfaces"
-version = "1.2.1"
-
-[[patch.unused]]
-name = "rosgraph_msgs"
-version = "1.2.1"

From 8cc895997c076b9a1edccd2a8048eb303b440fec Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Fri, 18 Jul 2025 17:34:56 +0800
Subject: [PATCH 02/20] Port draft action implementation from #410

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/src/action.rs                    |  57 ++
 rclrs/src/action/client.rs             | 258 ++++++++
 rclrs/src/action/server.rs             | 783 +++++++++++++++++++++++++
 rclrs/src/action/server_goal_handle.rs | 241 ++++++++
 rclrs/src/drop_guard.rs                |  48 ++
 rclrs/src/error.rs                     |  38 ++
 rclrs/src/lib.rs                       |   4 +
 rclrs/src/node.rs                      |  51 ++
 rclrs/src/qos.rs                       |  19 +
 rclrs/src/rcl_bindings.rs              |   2 +
 rclrs/src/rcl_wrapper.h                |   2 +
 11 files changed, 1503 insertions(+)
 create mode 100644 rclrs/src/action.rs
 create mode 100644 rclrs/src/action/client.rs
 create mode 100644 rclrs/src/action/server.rs
 create mode 100644 rclrs/src/action/server_goal_handle.rs
 create mode 100644 rclrs/src/drop_guard.rs

diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs
new file mode 100644
index 00000000..4432beac
--- /dev/null
+++ b/rclrs/src/action.rs
@@ -0,0 +1,57 @@
+pub(crate) mod client;
+pub(crate) mod server;
+mod server_goal_handle;
+
+use crate::rcl_bindings::RCL_ACTION_UUID_SIZE;
+use std::fmt;
+
+pub use client::{ActionClient, ActionClientBase, ActionClientOptions, ActionClientState};
+pub use server::{ActionServer, ActionServerBase, ActionServerOptions, ActionServerState};
+pub use server_goal_handle::ServerGoalHandle;
+
+/// A unique identifier for a goal request.
+#[derive(Copy, Clone, Debug, Default, PartialEq, Eq, PartialOrd, Ord, Hash)]
+pub struct GoalUuid(pub [u8; RCL_ACTION_UUID_SIZE]);
+
+impl fmt::Display for GoalUuid {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> {
+        write!(f, "{:02x}{:02x}{:02x}{:02x}-{:02x}{:02x}-{:02x}{:02x}-{:02x}{:02x}-{:02x}{:02x}{:02x}{:02x}{:02x}{:02x}",
+               self.0[0],
+               self.0[1],
+               self.0[2],
+               self.0[3],
+               self.0[4],
+               self.0[5],
+               self.0[6],
+               self.0[7],
+               self.0[8],
+               self.0[9],
+               self.0[10],
+               self.0[11],
+               self.0[12],
+               self.0[13],
+               self.0[14],
+               self.0[15],
+               )
+    }
+}
+
+/// The response returned by an [`ActionServer`]'s goal callback when a goal request is received.
+#[derive(PartialEq, Eq)]
+pub enum GoalResponse {
+    /// The goal is rejected and will not be executed.
+    Reject = 1,
+    /// The server accepts the goal and will begin executing it immediately.
+    AcceptAndExecute = 2,
+    /// The server accepts the goal and will begin executing it later.
+    AcceptAndDefer = 3,
+}
+
+/// The response returned by an [`ActionServer`]'s cancel callback when a goal is requested to be cancelled.
+#[derive(PartialEq, Eq)]
+pub enum CancelResponse {
+    /// The server will not try to cancel the goal.
+    Reject = 1,
+    /// The server will try to cancel the goal.
+    Accept = 2,
+}
diff --git a/rclrs/src/action/client.rs b/rclrs/src/action/client.rs
new file mode 100644
index 00000000..556fd1a0
--- /dev/null
+++ b/rclrs/src/action/client.rs
@@ -0,0 +1,258 @@
+use crate::{
+    error::ToResult, rcl_bindings::*, wait::WaitableNumEntities, Node, NodeHandle, QoSProfile,
+    RclrsError, ENTITY_LIFECYCLE_MUTEX,
+};
+use std::{
+    borrow::Borrow,
+    ffi::CString,
+    marker::PhantomData,
+    sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard},
+};
+
+// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
+// they are running in. Therefore, this type can be safely sent to another thread.
+unsafe impl Send for rcl_action_client_t {}
+
+/// Manage the lifecycle of an `rcl_action_client_t`, including managing its dependencies
+/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are
+/// [dropped after][1] the `rcl_action_client_t`.
+///
+/// [1]: <https://doc.rust-lang.org/reference/destructors.html>
+pub struct ActionClientHandle {
+    rcl_action_client: Mutex<rcl_action_client_t>,
+    node_handle: Arc<NodeHandle>,
+    pub(crate) in_use_by_wait_set: Arc<AtomicBool>,
+}
+
+impl ActionClientHandle {
+    pub(crate) fn lock(&self) -> MutexGuard<rcl_action_client_t> {
+        self.rcl_action_client.lock().unwrap()
+    }
+}
+
+impl Drop for ActionClientHandle {
+    fn drop(&mut self) {
+        let rcl_action_client = self.rcl_action_client.get_mut().unwrap();
+        let mut rcl_node = self.node_handle.rcl_node.lock().unwrap();
+        let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap();
+        // SAFETY: The entity lifecycle mutex is locked to protect against the risk of
+        // global variables in the rmw implementation being unsafely modified during cleanup.
+        unsafe {
+            rcl_action_client_fini(rcl_action_client, &mut *rcl_node);
+        }
+    }
+}
+
+/// Trait to be implemented by concrete ActionClient structs.
+///
+/// See [`ActionClient<T>`] for an example
+pub trait ActionClientBase: Send + Sync {
+    /// Internal function to get a reference to the `rcl` handle.
+    fn handle(&self) -> &ActionClientHandle;
+    /// Returns the number of underlying entities for the action client.
+    fn num_entities(&self) -> &WaitableNumEntities;
+    /// Tries to run the callback for the given readiness mode.
+    fn execute(&self, mode: ReadyMode) -> Result<(), RclrsError>;
+}
+
+pub(crate) enum ReadyMode {
+    Feedback,
+    Status,
+    GoalResponse,
+    CancelResponse,
+    ResultResponse,
+}
+
+///
+/// Main class responsible for sending goals to a ROS action server.
+///
+/// Create a client using [`Node::create_action_client`][1].
+///
+/// Receiving feedback and results requires the node's executor to [spin][2].
+///
+/// [1]: crate::NodeState::create_action_client
+/// [2]: crate::spin
+pub type ActionClient<ActionT> = Arc<ActionClientState<ActionT>>;
+
+/// The inner state of an [`ActionClient`].
+///
+/// This is public so that you can choose to create a [`Weak`][1] reference to it
+/// if you want to be able to refer to an [`ActionClient`] in a non-owning way. It is
+/// generally recommended to manage the `ActionClientState` inside of an [`Arc`],
+/// and [`ActionClient`] is provided as a convenience alias for that.
+///
+/// The public API of the [`ActionClient`] type is implemented via `ActionClientState`.
+///
+/// [1]: std::sync::Weak
+pub struct ActionClientState<ActionT>
+where
+    ActionT: rosidl_runtime_rs::Action,
+{
+    _marker: PhantomData<fn() -> ActionT>,
+    pub(crate) handle: Arc<ActionClientHandle>,
+    num_entities: WaitableNumEntities,
+    /// Ensure the parent node remains alive as long as the subscription is held.
+    /// This implementation will change in the future.
+    #[allow(unused)]
+    node: Node,
+}
+
+impl<T> ActionClientState<T>
+where
+    T: rosidl_runtime_rs::Action,
+{
+    /// Creates a new action client.
+    pub(crate) fn new<'a>(
+        node: &Node,
+        options: impl Into<ActionClientOptions<'a>>,
+    ) -> Result<Self, RclrsError>
+    where
+        T: rosidl_runtime_rs::Action,
+    {
+        let options = options.into();
+        // SAFETY: Getting a zero-initialized value is always safe.
+        let mut rcl_action_client = unsafe { rcl_action_get_zero_initialized_client() };
+        let type_support = T::get_type_support() as *const rosidl_action_type_support_t;
+        let action_name_c_string =
+            CString::new(options.action_name).map_err(|err| RclrsError::StringContainsNul {
+                err,
+                s: options.action_name.into(),
+            })?;
+
+        // SAFETY: No preconditions for this function.
+        let action_client_options = unsafe { rcl_action_client_get_default_options() };
+
+        {
+            let mut rcl_node = node.handle.rcl_node.lock().unwrap();
+            let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap();
+
+            // SAFETY:
+            // * The rcl_action_client was zero-initialized as expected by this function.
+            // * The rcl_node is kept alive by the NodeHandle because it is a dependency of the action client.
+            // * The action name and the options are copied by this function, so they can be dropped
+            //   afterwards.
+            // * The entity lifecycle mutex is locked to protect against the risk of global
+            //   variables in the rmw implementation being unsafely modified during initialization.
+            unsafe {
+                rcl_action_client_init(
+                    &mut rcl_action_client,
+                    &mut *rcl_node,
+                    type_support,
+                    action_name_c_string.as_ptr(),
+                    &action_client_options,
+                )
+                .ok()?;
+            }
+        }
+
+        let handle = Arc::new(ActionClientHandle {
+            rcl_action_client: Mutex::new(rcl_action_client),
+            node_handle: Arc::clone(&node.handle),
+            in_use_by_wait_set: Arc::new(AtomicBool::new(false)),
+        });
+
+        let mut num_entities = WaitableNumEntities::default();
+        unsafe {
+            rcl_action_client_wait_set_get_num_entities(
+                &*handle.lock(),
+                &mut num_entities.num_subscriptions,
+                &mut num_entities.num_guard_conditions,
+                &mut num_entities.num_timers,
+                &mut num_entities.num_clients,
+                &mut num_entities.num_services,
+            )
+            .ok()?;
+        }
+
+        Ok(Self {
+            _marker: Default::default(),
+            handle,
+            num_entities,
+            node: Arc::clone(node),
+        })
+    }
+
+    fn execute_feedback(&self) -> Result<(), RclrsError> {
+        todo!()
+    }
+
+    fn execute_status(&self) -> Result<(), RclrsError> {
+        todo!()
+    }
+
+    fn execute_goal_response(&self) -> Result<(), RclrsError> {
+        todo!()
+    }
+
+    fn execute_cancel_response(&self) -> Result<(), RclrsError> {
+        todo!()
+    }
+
+    fn execute_result_response(&self) -> Result<(), RclrsError> {
+        todo!()
+    }
+}
+
+impl<T> ActionClientBase for ActionClientState<T>
+where
+    T: rosidl_runtime_rs::Action,
+{
+    fn handle(&self) -> &ActionClientHandle {
+        &self.handle
+    }
+
+    fn num_entities(&self) -> &WaitableNumEntities {
+        &self.num_entities
+    }
+
+    fn execute(&self, mode: ReadyMode) -> Result<(), RclrsError> {
+        match mode {
+            ReadyMode::Feedback => self.execute_feedback(),
+            ReadyMode::Status => self.execute_status(),
+            ReadyMode::GoalResponse => self.execute_goal_response(),
+            ReadyMode::CancelResponse => self.execute_cancel_response(),
+            ReadyMode::ResultResponse => self.execute_result_response(),
+        }
+    }
+}
+
+/// `ActionClientOptions` are used by [`Node::create_action_client`][1] to initialize an
+/// [`ActionClient`].
+///
+/// [1]: crate::Node::create_action_client
+#[derive(Debug, Clone)]
+#[non_exhaustive]
+pub struct ActionClientOptions<'a> {
+    /// The name of the action that this client will send requests to
+    pub action_name: &'a str,
+    /// The quality of service profile for the goal service
+    pub goal_service_qos: QoSProfile,
+    /// The quality of service profile for the result service
+    pub result_service_qos: QoSProfile,
+    /// The quality of service profile for the cancel service
+    pub cancel_service_qos: QoSProfile,
+    /// The quality of service profile for the feedback topic
+    pub feedback_topic_qos: QoSProfile,
+    /// The quality of service profile for the status topic
+    pub status_topic_qos: QoSProfile,
+}
+
+impl<'a> ActionClientOptions<'a> {
+    /// Initialize a new [`ActionClientOptions`] with default settings.
+    pub fn new(action_name: &'a str) -> Self {
+        Self {
+            action_name,
+            goal_service_qos: QoSProfile::services_default(),
+            result_service_qos: QoSProfile::services_default(),
+            cancel_service_qos: QoSProfile::services_default(),
+            feedback_topic_qos: QoSProfile::topics_default(),
+            status_topic_qos: QoSProfile::action_status_default(),
+        }
+    }
+}
+
+impl<'a, T: Borrow<str> + ?Sized + 'a> From<&'a T> for ActionClientOptions<'a> {
+    fn from(value: &'a T) -> Self {
+        Self::new(value.borrow())
+    }
+}
diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs
new file mode 100644
index 00000000..949af77f
--- /dev/null
+++ b/rclrs/src/action/server.rs
@@ -0,0 +1,783 @@
+use crate::{
+    action::{CancelResponse, GoalResponse, GoalUuid, ServerGoalHandle},
+    error::{RclReturnCode, ToResult},
+    rcl_bindings::*,
+    wait::WaitableNumEntities,
+    Clock, DropGuard, Node, NodeHandle, QoSProfile, RclrsError, ENTITY_LIFECYCLE_MUTEX,
+};
+use rosidl_runtime_rs::{Action, ActionImpl, Message, Service};
+use std::{
+    borrow::Borrow,
+    collections::HashMap,
+    ffi::CString,
+    sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard},
+};
+
+// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
+// they are running in. Therefore, this type can be safely sent to another thread.
+unsafe impl Send for rcl_action_server_t {}
+
+/// Manage the lifecycle of an `rcl_action_server_t`, including managing its dependencies
+/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are
+/// [dropped after][1] the `rcl_action_server_t`.
+///
+/// [1]: <https://doc.rust-lang.org/reference/destructors.html>
+pub struct ActionServerHandle {
+    rcl_action_server: Mutex<rcl_action_server_t>,
+    node_handle: Arc<NodeHandle>,
+    pub(crate) in_use_by_wait_set: Arc<AtomicBool>,
+}
+
+impl ActionServerHandle {
+    pub(crate) fn lock(&self) -> MutexGuard<rcl_action_server_t> {
+        self.rcl_action_server.lock().unwrap()
+    }
+}
+
+impl Drop for ActionServerHandle {
+    fn drop(&mut self) {
+        let rcl_action_server = self.rcl_action_server.get_mut().unwrap();
+        let mut rcl_node = self.node_handle.rcl_node.lock().unwrap();
+        let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap();
+        // SAFETY: The entity lifecycle mutex is locked to protect against the risk of
+        // global variables in the rmw implementation being unsafely modified during cleanup.
+        unsafe {
+            rcl_action_server_fini(rcl_action_server, &mut *rcl_node);
+        }
+    }
+}
+
+/// Trait to be implemented by concrete ActionServer structs.
+///
+/// See [`ActionServer<T>`] for an example
+pub trait ActionServerBase: Send + Sync {
+    /// Internal function to get a reference to the `rcl` handle.
+    fn handle(&self) -> &ActionServerHandle;
+    /// Returns the number of underlying entities for the action server.
+    fn num_entities(&self) -> &WaitableNumEntities;
+    /// Tries to run the callback for the given readiness mode.
+    fn execute(self: Arc<Self>, mode: ReadyMode) -> Result<(), RclrsError>;
+}
+
+pub(crate) enum ReadyMode {
+    GoalRequest,
+    CancelRequest,
+    ResultRequest,
+    GoalExpired,
+}
+
+pub type GoalCallback<ActionT> = dyn Fn(GoalUuid, <ActionT as rosidl_runtime_rs::Action>::Goal) -> GoalResponse + 'static + Send + Sync;
+pub type CancelCallback<ActionT> = dyn Fn(Arc<ServerGoalHandle<ActionT>>) -> CancelResponse + 'static + Send + Sync;
+pub type AcceptedCallback<ActionT> = dyn Fn(Arc<ServerGoalHandle<ActionT>>) + 'static + Send + Sync;
+
+/// An action server that can respond to requests sent by ROS action clients.
+///
+/// Create an action server using [`Node::create_action_server`][1].
+///
+/// ROS only supports having one server for any given fully-qualified
+/// action name. "Fully-qualified" means the namespace is also taken into account
+/// for uniqueness. A clone of an `ActionServer` will refer to the same server
+/// instance as the original. The underlying instance is tied to [`ActionServerState`]
+/// which implements the [`ActionServer`] API.
+///
+/// Responding to requests requires the node's executor to [spin][2].
+///
+/// [1]: crate::NodeState::create_action_server
+/// [2]: crate::spin
+pub type ActionServer<ActionT> = Arc<ActionServerState<ActionT>>;
+
+/// The inner state of an [`ActionServer`].
+///
+/// This is public so that you can choose to create a [`Weak`][1] reference to it
+/// if you want to be able to refer to a [`ActionServer`] in a non-owning way. It is
+/// generally recommended to manage the `ActionServerState` inside of an [`Arc`],
+/// and [`ActionServer`] is provided as a convenience alias for that.
+///
+/// The public API of the [`ActionServer`] type is implemented via `ActionServerState`.
+///
+/// [1]: std::sync::Weak
+pub struct ActionServerState<ActionT>
+where
+    ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
+{
+    pub(crate) handle: Arc<ActionServerHandle>,
+    num_entities: WaitableNumEntities,
+    goal_callback: Box<GoalCallback<ActionT>>,
+    cancel_callback: Box<CancelCallback<ActionT>>,
+    accepted_callback: Box<AcceptedCallback<ActionT>>,
+    // TODO(nwn): Audit these three mutexes to ensure there's no deadlocks or broken invariants. We
+    // may want to join them behind a shared mutex, at least for the `goal_results` and `result_requests`.
+    goal_handles: Mutex<HashMap<GoalUuid, Arc<ServerGoalHandle<ActionT>>>>,
+    goal_results: Mutex<HashMap<GoalUuid, <<ActionT::GetResultService as Service>::Response as Message>::RmwMsg>>,
+    result_requests: Mutex<HashMap<GoalUuid, Vec<rmw_request_id_t>>>,
+    /// Ensure the parent node remains alive as long as the subscription is held.
+    /// This implementation will change in the future.
+    #[allow(unused)]
+    node: Node,
+}
+
+impl<T> ActionServerState<T>
+where
+    T: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
+{
+    /// Creates a new action server.
+    pub(crate) fn new<'a>(
+        node: &Node,
+        options: impl Into<ActionServerOptions<'a>>,
+        goal_callback: impl Fn(GoalUuid, T::Goal) -> GoalResponse + 'static + Send + Sync,
+        cancel_callback: impl Fn(Arc<ServerGoalHandle<T>>) -> CancelResponse + 'static + Send + Sync,
+        accepted_callback: impl Fn(Arc<ServerGoalHandle<T>>) + 'static + Send + Sync,
+    ) -> Result<Self, RclrsError>
+    where
+        T: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
+    {
+        let options = options.into();
+        // SAFETY: Getting a zero-initialized value is always safe.
+        let mut rcl_action_server = unsafe { rcl_action_get_zero_initialized_server() };
+        let type_support = T::get_type_support() as *const rosidl_action_type_support_t;
+        let action_name_c_string =
+            CString::new(options.action_name).map_err(|err| RclrsError::StringContainsNul {
+                err,
+                s: options.action_name.into(),
+            })?;
+
+        // SAFETY: No preconditions for this function.
+        let action_server_options = unsafe { rcl_action_server_get_default_options() };
+
+        {
+            let mut rcl_node = node.handle.rcl_node.lock().unwrap();
+            let clock = node.get_clock();
+            let rcl_clock = clock.rcl_clock();
+            let mut rcl_clock = rcl_clock.lock().unwrap();
+            let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap();
+
+            // SAFETY:
+            // * The rcl_action_server is zero-initialized as mandated by this function.
+            // * The rcl_node is kept alive by the NodeHandle because it is a dependency of the action server.
+            // * The action name and the options are copied by this function, so they can be dropped
+            //   afterwards.
+            // * The entity lifecycle mutex is locked to protect against the risk of global
+            //   variables in the rmw implementation being unsafely modified during initialization.
+            unsafe {
+                rcl_action_server_init(
+                    &mut rcl_action_server,
+                    &mut *rcl_node,
+                    &mut *rcl_clock,
+                    type_support,
+                    action_name_c_string.as_ptr(),
+                    &action_server_options,
+                )
+                .ok()?;
+            }
+        }
+
+        let handle = Arc::new(ActionServerHandle {
+            rcl_action_server: Mutex::new(rcl_action_server),
+            node_handle: Arc::clone(&node.handle),
+            in_use_by_wait_set: Arc::new(AtomicBool::new(false)),
+        });
+
+        let mut num_entities = WaitableNumEntities::default();
+        unsafe {
+            rcl_action_server_wait_set_get_num_entities(
+                &*handle.lock(),
+                &mut num_entities.num_subscriptions,
+                &mut num_entities.num_guard_conditions,
+                &mut num_entities.num_timers,
+                &mut num_entities.num_clients,
+                &mut num_entities.num_services,
+            )
+            .ok()?;
+        }
+
+        Ok(Self {
+            handle,
+            num_entities,
+            goal_callback: Box::new(goal_callback),
+            cancel_callback: Box::new(cancel_callback),
+            accepted_callback: Box::new(accepted_callback),
+            goal_handles: Mutex::new(HashMap::new()),
+            goal_results: Mutex::new(HashMap::new()),
+            result_requests: Mutex::new(HashMap::new()),
+            node: node.clone(),
+        })
+    }
+
+    fn take_goal_request(&self) -> Result<(<<T::SendGoalService as Service>::Request as Message>::RmwMsg, rmw_request_id_t), RclrsError> {
+        let mut request_id = rmw_request_id_t {
+            writer_guid: [0; 16],
+            sequence_number: 0,
+        };
+        type RmwRequest<T> = <<<T as ActionImpl>::SendGoalService as Service>::Request as Message>::RmwMsg;
+        let mut request_rmw = RmwRequest::<T>::default();
+        let handle = &*self.handle.lock();
+        unsafe {
+            // SAFETY: The action server is locked by the handle. The request_id is a
+            // zero-initialized rmw_request_id_t, and the request_rmw is a default-initialized
+            // SendGoalService request message.
+            rcl_action_take_goal_request(
+                handle,
+                &mut request_id,
+                &mut request_rmw as *mut RmwRequest<T> as *mut _,
+            )
+        }
+        .ok()?;
+
+        Ok((request_rmw, request_id))
+    }
+
+    fn send_goal_response(
+        &self,
+        mut request_id: rmw_request_id_t,
+        accepted: bool,
+    ) -> Result<(), RclrsError> {
+        let mut response_rmw = <T as ActionImpl>::create_goal_response(accepted, (0, 0));
+        let handle = &*self.handle.lock();
+        let result = unsafe {
+            // SAFETY: The action server handle is locked and so synchronized with other
+            // functions. The request_id and response message are uniquely owned, and so will
+            // not mutate during this function call.
+            // Also, when appropriate, `rcl_action_accept_new_goal()` has been called beforehand,
+            // as specified in the `rcl_action` docs.
+            rcl_action_send_goal_response(
+                handle,
+                &mut request_id,
+                &mut response_rmw as *mut _ as *mut _,
+            )
+        }
+        .ok();
+        match result {
+            Ok(()) => Ok(()),
+            Err(RclrsError::RclError {
+                code: RclReturnCode::Timeout,
+                ..
+            }) => {
+                // TODO(nwn): Log an error and continue.
+                // (See https://github.com/ros2/rclcpp/pull/2215 for reasoning.)
+                Ok(())
+            }
+            _ => result,
+        }
+    }
+
+    fn execute_goal_request(self: Arc<Self>) -> Result<(), RclrsError> {
+        let (request, request_id) = match self.take_goal_request() {
+            Ok(res) => res,
+            Err(RclrsError::RclError {
+                code: RclReturnCode::ServiceTakeFailed,
+                ..
+            }) => {
+                // Spurious wakeup – this may happen even when a waitset indicated that this
+                // action was ready, so it shouldn't be an error.
+                return Ok(());
+            }
+            Err(err) => return Err(err),
+        };
+
+        let uuid = GoalUuid(*<T as ActionImpl>::get_goal_request_uuid(&request));
+
+        let response: GoalResponse = {
+            todo!("Optionally convert request to an idiomatic type for the user's callback.");
+            todo!("Call self.goal_callback(uuid, request)");
+        };
+
+        // Don't continue if the goal was rejected by the user.
+        if response == GoalResponse::Reject {
+            return self.send_goal_response(request_id, false);
+        }
+
+        let goal_handle = {
+            // SAFETY: No preconditions
+            let mut goal_info = unsafe { rcl_action_get_zero_initialized_goal_info() };
+            // Only populate the goal UUID; the timestamp will be set internally by
+            // rcl_action_accept_new_goal().
+            goal_info.goal_id.uuid = uuid.0;
+
+            let server_handle = &mut *self.handle.lock();
+            let goal_handle_ptr = unsafe {
+                // SAFETY: The action server handle is locked and so synchronized with other
+                // functions. The request_id and response message are uniquely owned, and so will
+                // not mutate during this function call. The returned goal handle pointer should be
+                // valid unless it is null.
+                rcl_action_accept_new_goal(server_handle, &goal_info)
+            };
+            if goal_handle_ptr.is_null() {
+                // Other than rcl_get_error_string(), there's no indication what happened.
+                panic!("Failed to accept goal");
+            } else {
+                Arc::new(ServerGoalHandle::<T>::new(
+                    goal_handle_ptr,
+                    Arc::downgrade(&self),
+                    todo!("Create an Arc holding the goal message"),
+                    uuid,
+                ))
+            }
+        };
+
+        self.send_goal_response(request_id, true)?;
+
+        self.goal_handles
+            .lock()
+            .unwrap()
+            .insert(uuid, Arc::clone(&goal_handle));
+
+        if response == GoalResponse::AcceptAndExecute {
+            goal_handle.execute()?;
+        }
+
+        self.publish_status()?;
+
+        // TODO: Call the user's goal_accepted callback.
+        todo!("Call self.accepted_callback(goal_handle)");
+
+        Ok(())
+    }
+
+    fn take_cancel_request(&self) -> Result<(action_msgs__srv__CancelGoal_Request, rmw_request_id_t), RclrsError> {
+        let mut request_id = rmw_request_id_t {
+            writer_guid: [0; 16],
+            sequence_number: 0,
+        };
+        // SAFETY: No preconditions
+        let mut request_rmw = unsafe { rcl_action_get_zero_initialized_cancel_request() };
+        let handle = &*self.handle.lock();
+        unsafe {
+            // SAFETY: The action server is locked by the handle. The request_id is a
+            // zero-initialized rmw_request_id_t, and the request_rmw is a zero-initialized
+            // action_msgs__srv__CancelGoal_Request.
+            rcl_action_take_cancel_request(
+                handle,
+                &mut request_id,
+                &mut request_rmw as *mut _ as *mut _,
+            )
+        }
+        .ok()?;
+
+        Ok((request_rmw, request_id))
+    }
+
+    fn send_cancel_response(
+        &self,
+        mut request_id: rmw_request_id_t,
+        response_rmw: &mut action_msgs__srv__CancelGoal_Response,
+    ) -> Result<(), RclrsError> {
+        let handle = &*self.handle.lock();
+        let result = unsafe {
+            // SAFETY: The action server handle is locked and so synchronized with other functions.
+            // The request_id and response are both uniquely owned or borrowed, and so neither will
+            // mutate during this function call.
+            rcl_action_send_cancel_response(
+                handle,
+                &mut request_id,
+                response_rmw as *mut _ as *mut _,
+            )
+        }
+        .ok();
+        match result {
+            Ok(()) => Ok(()),
+            Err(RclrsError::RclError {
+                code: RclReturnCode::Timeout,
+                ..
+            }) => {
+                // TODO(nwn): Log an error and continue.
+                // (See https://github.com/ros2/rclcpp/pull/2215 for reasoning.)
+                Ok(())
+            }
+            _ => result,
+        }
+    }
+
+    fn execute_cancel_request(&self) -> Result<(), RclrsError> {
+        let (request, request_id) = match self.take_cancel_request() {
+            Ok(res) => res,
+            Err(RclrsError::RclError {
+                code: RclReturnCode::ServiceTakeFailed,
+                ..
+            }) => {
+                // Spurious wakeup – this may happen even when a waitset indicated that this
+                // action was ready, so it shouldn't be an error.
+                return Ok(());
+            }
+            Err(err) => return Err(err),
+        };
+
+        let mut response_rmw = {
+            // SAFETY: No preconditions
+            let mut response_rmw = unsafe { rcl_action_get_zero_initialized_cancel_response() };
+            unsafe {
+                // SAFETY: The action server is locked by the handle. The request was initialized
+                // by rcl_action, and the response is a zero-initialized
+                // rcl_action_cancel_response_t.
+                rcl_action_process_cancel_request(
+                    &*self.handle.lock(),
+                    &request,
+                    &mut response_rmw as *mut _,
+                )
+            }
+            .ok()?;
+
+            DropGuard::new(response_rmw, |mut response_rmw| unsafe {
+                // SAFETY: The response was initialized by rcl_action_process_cancel_request().
+                // Later modifications only truncate the size of the array and shift elements,
+                // without modifying the data pointer or capacity.
+                rcl_action_cancel_response_fini(&mut response_rmw);
+            })
+        };
+
+        let num_candidates = response_rmw.msg.goals_canceling.size;
+        let mut num_accepted = 0;
+        for idx in 0..response_rmw.msg.goals_canceling.size {
+            let goal_info = unsafe {
+                // SAFETY: The array pointed to by response_rmw.msg.goals_canceling.data is
+                // guaranteed to contain at least response_rmw.msg.goals_canceling.size members.
+                &*response_rmw.msg.goals_canceling.data.add(idx)
+            };
+            let goal_uuid = GoalUuid(goal_info.goal_id.uuid);
+
+            let response = {
+                if let Some(goal_handle) = self.goal_handles.lock().unwrap().get(&goal_uuid) {
+                    let response: CancelResponse = todo!("Call self.cancel_callback(goal_handle)");
+                    if response == CancelResponse::Accept {
+                        // Still reject the request if the goal is no longer cancellable.
+                        if goal_handle.cancel().is_ok() {
+                            CancelResponse::Accept
+                        } else {
+                            CancelResponse::Reject
+                        }
+                    } else {
+                        CancelResponse::Reject
+                    }
+                } else {
+                    CancelResponse::Reject
+                }
+            };
+
+            if response == CancelResponse::Accept {
+                // Shift the accepted entry back to the first rejected slot, if necessary.
+                if num_accepted < idx {
+                    let goal_info_slot = unsafe {
+                        // SAFETY: The array pointed to by response_rmw.msg.goals_canceling.data is
+                        // guaranteed to contain at least response_rmw.msg.goals_canceling.size
+                        // members. Since `num_accepted` is strictly less than `idx`, it is a
+                        // distinct element of the array, so there is no mutable aliasing.
+                        &mut *response_rmw.msg.goals_canceling.data.add(num_accepted)
+                    };
+                }
+                num_accepted += 1;
+            }
+        }
+        response_rmw.msg.goals_canceling.size = num_accepted;
+
+        // If the user rejects all individual cancel requests, consider the entire request as
+        // having been rejected.
+        if num_accepted == 0 && num_candidates > 0 {
+            // TODO(nwn): Include action_msgs__srv__CancelGoal_Response__ERROR_REJECTED in the rcl
+            // bindings.
+            response_rmw.msg.return_code = 1;
+        }
+
+        // If any goal states changed, publish a status update.
+        if num_accepted > 0 {
+            self.publish_status()?;
+        }
+
+        self.send_cancel_response(request_id, &mut response_rmw.msg)?;
+
+        Ok(())
+    }
+
+    fn take_result_request(&self) -> Result<(<<T::GetResultService as Service>::Request as Message>::RmwMsg, rmw_request_id_t), RclrsError> {
+        let mut request_id = rmw_request_id_t {
+            writer_guid: [0; 16],
+            sequence_number: 0,
+        };
+        type RmwRequest<T> = <<<T as ActionImpl>::GetResultService as Service>::Request as Message>::RmwMsg;
+        let mut request_rmw = RmwRequest::<T>::default();
+        let handle = &*self.handle.lock();
+        unsafe {
+            // SAFETY: The action server is locked by the handle. The request_id is a
+            // zero-initialized rmw_request_id_t, and the request_rmw is a default-initialized
+            // GetResultService request message.
+            rcl_action_take_result_request(
+                handle,
+                &mut request_id,
+                &mut request_rmw as *mut RmwRequest<T> as *mut _,
+            )
+        }
+        .ok()?;
+
+        Ok((request_rmw, request_id))
+    }
+
+    fn send_result_response(
+        &self,
+        mut request_id: rmw_request_id_t,
+        response_rmw: &mut <<<T as ActionImpl>::GetResultService as rosidl_runtime_rs::Service>::Response as Message>::RmwMsg,
+    ) -> Result<(), RclrsError> {
+        let handle = &*self.handle.lock();
+        let result = unsafe {
+            // SAFETY: The action server handle is locked and so synchronized with other functions.
+            // The request_id and response are both uniquely owned or borrowed, and so neither will
+            // mutate during this function call.
+            rcl_action_send_result_response(
+                handle,
+                &mut request_id,
+                response_rmw as *mut _ as *mut _,
+            )
+        }
+        .ok();
+        match result {
+            Ok(()) => Ok(()),
+            Err(RclrsError::RclError {
+                code: RclReturnCode::Timeout,
+                ..
+            }) => {
+                // TODO(nwn): Log an error and continue.
+                // (See https://github.com/ros2/rclcpp/pull/2215 for reasoning.)
+                Ok(())
+            }
+            _ => result,
+        }
+    }
+
+    fn execute_result_request(&self) -> Result<(), RclrsError> {
+        let (request, request_id) = match self.take_result_request() {
+            Ok(res) => res,
+            Err(RclrsError::RclError {
+                code: RclReturnCode::ServiceTakeFailed,
+                ..
+            }) => {
+                // Spurious wakeup – this may happen even when a waitset indicated that this
+                // action was ready, so it shouldn't be an error.
+                return Ok(());
+            }
+            Err(err) => return Err(err),
+        };
+
+        let uuid = GoalUuid(*<T as ActionImpl>::get_result_request_uuid(&request));
+
+        let goal_exists = unsafe {
+            // SAFETY: No preconditions
+            let mut goal_info = rcl_action_get_zero_initialized_goal_info();
+            goal_info.goal_id.uuid = uuid.0;
+
+            // SAFETY: The action server is locked through the handle. The `goal_info`
+            // argument points to a rcl_action_goal_info_t with the desired UUID.
+            rcl_action_server_goal_exists(&*self.handle.lock(), &goal_info)
+        };
+
+        if goal_exists {
+            if let Some(result) = self.goal_results.lock().unwrap().get_mut(&uuid) {
+                // Respond immediately if the goal already has a response.
+                self.send_result_response(request_id, result)?;
+            } else {
+                // Queue up the request for a response once the goal terminates.
+                self.result_requests.lock().unwrap().entry(uuid).or_insert(vec![]).push(request_id);
+            }
+        } else {
+            // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_UNKNOWN in the rcl
+            // bindings.
+            let null_response = <T::Result as Message>::RmwMsg::default();
+            let mut response_rmw = <T as ActionImpl>::create_result_response(0, null_response);
+            self.send_result_response(request_id, &mut response_rmw)?;
+        }
+
+        Ok(())
+    }
+
+    fn execute_goal_expired(&self) -> Result<(), RclrsError> {
+        // We assume here that only one goal expires at a time. If not, the only consequence is
+        // that we'll call rcl_action_expire_goals() more than necessary.
+
+        // SAFETY: No preconditions
+        let mut expired_goal = unsafe { rcl_action_get_zero_initialized_goal_info() };
+        let mut num_expired = 1;
+
+        loop {
+            unsafe {
+                // SAFETY: The action server is locked through the handle. The `expired_goal`
+                // argument points to an array of one rcl_action_goal_info_t and num_expired points
+                // to a `size_t`.
+                rcl_action_expire_goals(&*self.handle.lock(), &mut expired_goal, 1, &mut num_expired)
+            }
+            .ok()?;
+
+            if num_expired > 0 {
+                // Clean up the expired goal.
+                let uuid = GoalUuid(expired_goal.goal_id.uuid);
+                self.goal_results.lock().unwrap().remove(&uuid);
+                self.result_requests.lock().unwrap().remove(&uuid);
+                self.goal_handles.lock().unwrap().remove(&uuid);
+            } else {
+                break;
+            }
+        }
+
+        Ok(())
+    }
+
+    // TODO(nwn): Replace `status` with a "properly typed" action_msgs::msg::GoalStatus enum.
+    pub(crate) fn terminate_goal(&self, goal_id: &GoalUuid, status: i8, result: <T::Result as Message>::RmwMsg) -> Result<(), RclrsError> {
+        let response_rmw = <T as ActionImpl>::create_result_response(status, result);
+
+        // Publish the result to anyone listening.
+        self.publish_result(goal_id, response_rmw);
+
+        // Publish the state change.
+        self.publish_status();
+
+        // Notify rcl that a goal has terminated and to therefore recalculate the expired goal timer.
+        unsafe {
+            // SAFETY: The action server is locked and valid. No other preconditions.
+            rcl_action_notify_goal_done(&*self.handle.lock())
+        }
+        .ok()?;
+
+        // Release ownership of the goal handle. It will persist until the user also drops it.
+        self.goal_handles.lock().unwrap().remove(&goal_id);
+
+        Ok(())
+    }
+
+    pub(crate) fn publish_status(&self) -> Result<(), RclrsError> {
+        let mut goal_statuses = DropGuard::new(
+            unsafe {
+                // SAFETY: No preconditions
+                rcl_action_get_zero_initialized_goal_status_array()
+            },
+            |mut goal_statuses| unsafe {
+                // SAFETY: The goal_status array is either zero-initialized and empty or populated by
+                // `rcl_action_get_goal_status_array`. In either case, it can be safely finalized.
+                rcl_action_goal_status_array_fini(&mut goal_statuses);
+            },
+        );
+
+        unsafe {
+            // SAFETY: The action server is locked through the handle and goal_statuses is
+            // zero-initialized.
+            rcl_action_get_goal_status_array(&*self.handle.lock(), &mut *goal_statuses)
+        }
+        .ok()?;
+
+        unsafe {
+            // SAFETY: The action server is locked through the handle and goal_statuses.msg is a
+            // valid `action_msgs__msg__GoalStatusArray` by construction.
+            rcl_action_publish_status(
+                &*self.handle.lock(),
+                &goal_statuses.msg as *const _ as *const std::ffi::c_void,
+            )
+        }
+        .ok()
+    }
+
+    pub(crate) fn publish_feedback(&self, goal_id: &GoalUuid, feedback: &<T as rosidl_runtime_rs::Action>::Feedback) -> Result<(), RclrsError> {
+        let feedback_rmw = <<T as rosidl_runtime_rs::Action>::Feedback as Message>::into_rmw_message(std::borrow::Cow::Borrowed(feedback));
+        let mut feedback_msg = <T as rosidl_runtime_rs::ActionImpl>::create_feedback_message(&goal_id.0, feedback_rmw.into_owned());
+        unsafe {
+            // SAFETY: The action server is locked through the handle, meaning that no other
+            // non-thread-safe functions can be called on it at the same time. The feedback_msg is
+            // exclusively owned here, ensuring that it won't be modified during the call.
+            // rcl_action_publish_feedback() guarantees that it won't modify `feedback_msg`.
+            rcl_action_publish_feedback(
+                &*self.handle.lock(),
+                &mut feedback_msg as *mut _ as *mut std::ffi::c_void,
+            )
+        }
+        .ok()
+    }
+
+    fn publish_result(&self, goal_id: &GoalUuid, mut result: <<<T as ActionImpl>::GetResultService as Service>::Response as Message>::RmwMsg) -> Result<(), RclrsError> {
+        let goal_exists = unsafe {
+            // SAFETY: No preconditions
+            let mut goal_info = rcl_action_get_zero_initialized_goal_info();
+            goal_info.goal_id.uuid = goal_id.0;
+
+            // SAFETY: The action server is locked through the handle. The `goal_info`
+            // argument points to a rcl_action_goal_info_t with the desired UUID.
+            rcl_action_server_goal_exists(&*self.handle.lock(), &goal_info)
+        };
+        if !goal_exists {
+            panic!("Cannot publish result for unknown goal")
+        }
+
+        // TODO(nwn): Fix synchronization problem between goal_results and result_requests.
+        // Currently, there is a gap between the request queue being drained and the result being
+        // stored for future requests. Any requests received during that gap would never receive a
+        // response. Fixing this means we'll need combined locking over these two hash maps.
+
+        // Respond to all queued requests.
+        if let Some(result_requests) = self.result_requests.lock().unwrap().remove(&goal_id) {
+            for mut result_request in result_requests {
+                self.send_result_response(result_request, &mut result)?;
+            }
+        }
+
+        self.goal_results.lock().unwrap().insert(*goal_id, result);
+
+        Ok(())
+    }
+}
+
+impl<T> ActionServerBase for ActionServerState<T>
+where
+    T: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
+{
+    fn handle(&self) -> &ActionServerHandle {
+        &self.handle
+    }
+
+    fn num_entities(&self) -> &WaitableNumEntities {
+        &self.num_entities
+    }
+
+    fn execute(self: Arc<Self>, mode: ReadyMode) -> Result<(), RclrsError> {
+        match mode {
+            ReadyMode::GoalRequest => self.execute_goal_request(),
+            ReadyMode::CancelRequest => self.execute_cancel_request(),
+            ReadyMode::ResultRequest => self.execute_result_request(),
+            ReadyMode::GoalExpired => self.execute_goal_expired(),
+        }
+    }
+}
+
+/// `ActionServerOptions` are used by [`Node::create_action_server`][1] to initialize an
+/// [`ActionServer`].
+///
+/// [1]: crate::Node::create_action_server
+#[derive(Debug, Clone)]
+#[non_exhaustive]
+pub struct ActionServerOptions<'a> {
+    /// The name of the action implemented by this server
+    pub action_name: &'a str,
+    /// The quality of service profile for the goal service
+    pub goal_service_qos: QoSProfile,
+    /// The quality of service profile for the result service
+    pub result_service_qos: QoSProfile,
+    /// The quality of service profile for the cancel service
+    pub cancel_service_qos: QoSProfile,
+    /// The quality of service profile for the feedback topic
+    pub feedback_topic_qos: QoSProfile,
+    /// The quality of service profile for the status topic
+    pub status_topic_qos: QoSProfile,
+    // TODO(nwn): result_timeout
+}
+
+impl<'a> ActionServerOptions<'a> {
+    /// Initialize a new [`ActionServerOptions`] with default settings.
+    pub fn new(action_name: &'a str) -> Self {
+        Self {
+            action_name,
+            goal_service_qos: QoSProfile::services_default(),
+            result_service_qos: QoSProfile::services_default(),
+            cancel_service_qos: QoSProfile::services_default(),
+            feedback_topic_qos: QoSProfile::topics_default(),
+            status_topic_qos: QoSProfile::action_status_default(),
+        }
+    }
+}
+
+impl<'a, T: Borrow<str> + ?Sized + 'a> From<&'a T> for ActionServerOptions<'a> {
+    fn from(value: &'a T) -> Self {
+        Self::new(value.borrow())
+    }
+}
diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs
new file mode 100644
index 00000000..6910f8d5
--- /dev/null
+++ b/rclrs/src/action/server_goal_handle.rs
@@ -0,0 +1,241 @@
+use crate::{action::ActionServerState, rcl_bindings::*, GoalUuid, RclrsError, ToResult};
+use std::sync::{Arc, Mutex, Weak};
+
+// Values defined by `action_msgs/msg/GoalStatus`
+#[repr(i8)]
+#[derive(Debug, Clone, Hash, PartialEq, Eq)]
+enum GoalStatus {
+    Unknown = 0,
+    Accepted = 1,
+    Executing = 2,
+    Canceling = 3,
+    Succeeded = 4,
+    Canceled = 5,
+    Aborted = 6,
+}
+
+/// Handle to interact with goals on a server.
+///
+/// Use this to check the status of a goal and to set its result.
+///
+/// This type will only be created by an [`ActionServer`] when a goal is accepted and will be
+/// passed to the user in the associated `handle_accepted` callback.
+pub struct ServerGoalHandle<ActionT>
+where
+    ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
+{
+    rcl_handle: Mutex<*mut rcl_action_goal_handle_t>,
+    action_server: Weak<ActionServerState<ActionT>>,
+    goal_request: Arc<ActionT::Goal>,
+    uuid: GoalUuid,
+}
+
+// SAFETY: Send/Sync are not automatically implemented due to the contained raw pointer
+// (specifically, `*mut rcl_action_goal_handle_t`). However, the `rcl_handle` field is wrapped in a
+// mutex, guaranteeing that the underlying data is never simultaneously accessed on the rclrs side
+// by multiple threads. Moreover, the rcl_action functions taking these handles are able to be run
+// from any thread.
+unsafe impl<ActionT> Send for ServerGoalHandle<ActionT> where ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl {}
+unsafe impl<ActionT> Sync for ServerGoalHandle<ActionT> where ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl {}
+
+impl<ActionT> ServerGoalHandle<ActionT>
+where
+    ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
+{
+    pub(crate) fn new(
+        rcl_handle: *mut rcl_action_goal_handle_t,
+        action_server: Weak<ActionServerState<ActionT>>,
+        goal_request: Arc<ActionT::Goal>,
+        uuid: GoalUuid,
+    ) -> Self {
+        Self {
+            rcl_handle: Mutex::new(rcl_handle),
+            action_server,
+            goal_request: Arc::clone(&goal_request),
+            uuid,
+        }
+    }
+
+    /// Returns the goal state.
+    fn get_state(&self) -> Result<GoalStatus, RclrsError> {
+        let mut state = GoalStatus::Unknown as rcl_action_goal_state_t;
+        {
+            let rcl_handle = self.rcl_handle.lock().unwrap();
+            // SAFETY: The provided goal handle is properly initialized by construction.
+            unsafe { rcl_action_goal_handle_get_status(*rcl_handle, &mut state).ok()? }
+        }
+        // SAFETY: state is initialized to a valid GoalStatus value and will only ever by set by
+        // rcl_action_goal_handle_get_status to a valid GoalStatus value.
+        Ok(unsafe { std::mem::transmute(state) })
+    }
+
+    /// Returns whether the client has requested that this goal be cancelled.
+    pub fn is_canceling(&self) -> bool {
+        self.get_state().unwrap() == GoalStatus::Canceling
+    }
+
+    /// Returns true if the goal is either pending or executing, or false if it has reached a
+    /// terminal state.
+    pub fn is_active(&self) -> bool {
+        let rcl_handle = self.rcl_handle.lock().unwrap();
+        // SAFETY: The provided goal handle is properly initialized by construction.
+        unsafe { rcl_action_goal_handle_is_active(*rcl_handle) }
+    }
+
+    /// Returns whether the goal is executing.
+    pub fn is_executing(&self) -> bool {
+        self.get_state().unwrap() == GoalStatus::Executing
+    }
+
+    /// Attempt to perform the given goal state transition.
+    fn update_state(&self, event: rcl_action_goal_event_t) -> Result<(), RclrsError> {
+        let mut rcl_handle = self.rcl_handle.lock().unwrap();
+        // SAFETY: The provided goal handle is properly initialized by construction.
+        unsafe { rcl_action_update_goal_state(*rcl_handle, event).ok() }
+    }
+
+    /// Indicate that the goal is being cancelled.
+    ///
+    /// This is called when a cancel request for the goal has been accepted.
+    ///
+    /// Returns an error if the goal is in any state other than accepted or executing.
+    pub(crate) fn cancel(&self) -> Result<(), RclrsError> {
+        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCEL_GOAL)
+    }
+
+    /// Indicate that the goal could not be reached and has been aborted.
+    ///
+    /// Only call this if the goal is executing but cannot be completed. This is a terminal state,
+    /// so no more methods may be called on a goal handle after this is called.
+    ///
+    /// Returns an error if the goal is in any state other than executing.
+    pub fn abort(&self, result: &ActionT::Result) -> Result<(), RclrsError> {
+        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_ABORT)?;
+
+        if let Some(action_server) = self.action_server.upgrade() {
+            let result_rmw = <ActionT::Result as rosidl_runtime_rs::Message>::into_rmw_message(std::borrow::Cow::Borrowed(result)).into_owned();
+            // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_ABORTED in the rcl
+            // bindings.
+            action_server.terminate_goal(&self.uuid, 6, result_rmw)?;
+        }
+        Ok(())
+    }
+
+    /// Indicate that the goal has succeeded.
+    ///
+    /// Only call this if the goal is executing and has reached the desired final state. This is a
+    /// terminal state, so no more methods may be called on a goal handle after this is called.
+    ///
+    /// Returns an error if the goal is in any state other than executing.
+    pub fn succeed(&self, result: &ActionT::Result) -> Result<(), RclrsError> {
+        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_SUCCEED)?;
+
+        if let Some(action_server) = self.action_server.upgrade() {
+            let result_rmw = <ActionT::Result as rosidl_runtime_rs::Message>::into_rmw_message(std::borrow::Cow::Borrowed(result)).into_owned();
+            // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_SUCCEEDED in the rcl
+            // bindings.
+            action_server.terminate_goal(&self.uuid, 4, result_rmw)?;
+        }
+        Ok(())
+    }
+
+    /// Indicate that the goal has been cancelled.
+    ///
+    /// Only call this if the goal is executing or pending, but has been cancelled. This is a
+    /// terminal state, so no more methods may be called on a goal handle after this is called.
+    ///
+    /// Returns an error if the goal is in any state other than executing or pending.
+    pub fn canceled(&self, result: &ActionT::Result) -> Result<(), RclrsError> {
+        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCELED)?;
+
+        if let Some(action_server) = self.action_server.upgrade() {
+            let result_rmw = <ActionT::Result as rosidl_runtime_rs::Message>::into_rmw_message(std::borrow::Cow::Borrowed(result)).into_owned();
+            // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_CANCELED in the rcl
+            // bindings.
+            action_server.terminate_goal(&self.uuid, 5, result_rmw)?;
+        }
+        Ok(())
+    }
+
+    /// Indicate that the server is starting to execute the goal.
+    ///
+    /// Only call this if the goal is pending. This is a terminal state, so no more methods may be
+    /// called on a goal handle after this is called.
+    ///
+    /// Returns an error if the goal is in any state other than pending.
+    pub fn execute(&self) -> Result<(), RclrsError> {
+        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_EXECUTE)?;
+
+        // Publish the state change.
+        if let Some(action_server) = self.action_server.upgrade() {
+            action_server.publish_status()?;
+        }
+        Ok(())
+    }
+
+    /// Try canceling the goal if possible.
+    fn try_canceling(&mut self) -> Result<bool, RclrsError> {
+        let rcl_handle = self.rcl_handle.lock().unwrap();
+
+        // If the goal is in a cancelable state, transition to canceling.
+        // SAFETY: The provided goal handle is properly initialized by construction.
+        let is_cancelable = unsafe { rcl_action_goal_handle_is_cancelable(*rcl_handle) };
+        if is_cancelable {
+            self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCEL_GOAL)?;
+        }
+
+        // If the goal is canceling, transition to canceled.
+        if self.get_state()? == GoalStatus::Canceling {
+            self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCELED)?;
+            Ok(true)
+        } else {
+            Ok(false)
+        }
+    }
+
+    /// Get the unique identifier of the goal.
+    pub fn goal_id(&self) -> GoalUuid {
+        self.uuid
+    }
+
+    /// Get the user-provided message describing the goal.
+    pub fn goal(&self) -> Arc<ActionT::Goal> {
+        Arc::clone(&self.goal_request)
+    }
+
+    /// Send an update about the goal's progress.
+    ///
+    /// This may only be called when the goal is executing.
+    ///
+    /// Returns an error if the goal is in any state other than executing.
+    pub fn publish_feedback(&self, feedback: &ActionT::Feedback) -> Result<(), RclrsError> {
+        // If the action server no longer exists, simply drop the message.
+        if let Some(action_server) = self.action_server.upgrade() {
+            action_server.publish_feedback(&self.uuid, feedback)?;
+        }
+        Ok(())
+    }
+}
+
+impl<ActionT> Drop for ServerGoalHandle<ActionT>
+where
+    ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
+{
+    /// Cancel the goal if its handle is dropped without reaching a terminal state.
+    fn drop(&mut self) {
+        if self.try_canceling() == Ok(true) {
+            if let Some(action_server) = self.action_server.upgrade() {
+                let response_rmw = <ActionT::Result as rosidl_runtime_rs::Message>::RmwMsg::default();
+                // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_CANCELED in the rcl
+                // bindings.
+                action_server.terminate_goal(&self.uuid, 5, response_rmw);
+            }
+        }
+        {
+            let rcl_handle = self.rcl_handle.lock().unwrap();
+            // SAFETY: The provided goal handle is properly initialized by construction. It will
+            // not be accessed beyond this point.
+            unsafe { rcl_action_goal_handle_fini(*rcl_handle); }
+        }
+    }
+}
diff --git a/rclrs/src/drop_guard.rs b/rclrs/src/drop_guard.rs
new file mode 100644
index 00000000..f4e47b2d
--- /dev/null
+++ b/rclrs/src/drop_guard.rs
@@ -0,0 +1,48 @@
+use std::{
+    mem::ManuallyDrop,
+    ops::{Deref, DerefMut, Drop, Fn},
+};
+
+/// A wrapper providing additional drop-logic for the contained value.
+///
+/// When this wrapper is dropped, the contained value will be passed into the given function before
+/// being destructed.
+pub(crate) struct DropGuard<T, F: Fn(T)> {
+    value: ManuallyDrop<T>,
+    drop_fn: F,
+}
+
+impl<T, F: Fn(T)> DropGuard<T, F> {
+    /// Create a new `DropGuard` with the given value and drop function.
+    pub fn new(value: T, drop_fn: F) -> Self {
+        Self {
+            value: ManuallyDrop::new(value),
+            drop_fn,
+        }
+    }
+}
+
+impl<T, F: Fn(T)> Deref for DropGuard<T, F> {
+    type Target = T;
+
+    fn deref(&self) -> &T {
+        &*self.value
+    }
+}
+
+impl<T, F: Fn(T)> DerefMut for DropGuard<T, F> {
+    fn deref_mut(&mut self) -> &mut T {
+        &mut *self.value
+    }
+}
+
+impl<T, F: Fn(T)> Drop for DropGuard<T, F> {
+    fn drop(&mut self) {
+        // SAFETY: ManuallyDrop::take() leaves `self.value` in an uninitialized state, meaning that
+        // it must not be accessed further. This is guaranteed since `self` is being dropped and
+        // cannot be accessed after this function completes. Moreover, the strict ownership of
+        // `self.value` means that it cannot be accessed by `self.drop_fn`'s drop function either.
+        let value = unsafe { ManuallyDrop::take(&mut self.value) };
+        (self.drop_fn)(value);
+    }
+}
diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs
index b177aaaf..1293457e 100644
--- a/rclrs/src/error.rs
+++ b/rclrs/src/error.rs
@@ -251,6 +251,26 @@ pub enum RclReturnCode {
     EventInvalid = 2000,
     /// Failed to take an event from the event handle
     EventTakeFailed = 2001,
+    // ====== 2XXX: action-specific errors ======
+    /// Action name does not pass validation
+    // TODO(nwn): Consult with upstream about this reused error code.
+    // ActionNameInvalid = 2000,
+    /// Action goal accepted
+    ActionGoalAccepted = 2100,
+    /// Action goal rejected
+    ActionGoalRejected = 2101,
+    /// Action client is invalid
+    ActionClientInvalid = 2102,
+    /// Action client failed to take response
+    ActionClientTakeFailed = 2103,
+    /// Action server is invalid
+    ActionServerInvalid = 2200,
+    /// Action server failed to take request
+    ActionServerTakeFailed = 2201,
+    /// Action goal handle invalid
+    ActionGoalHandleInvalid = 2300,
+    /// Action invalid event
+    ActionGoalEventInvalid = 2301,
     // ====== 30XX: lifecycle-specific errors ======
     /// `rcl_lifecycle` state registered
     LifecycleStateRegistered = 3000,
@@ -305,6 +325,15 @@ impl TryFrom<i32> for RclReturnCode {
             x if x == Self::InvalidLogLevelRule as i32 => Self::InvalidLogLevelRule,
             x if x == Self::EventInvalid as i32 => Self::EventInvalid,
             x if x == Self::EventTakeFailed as i32 => Self::EventTakeFailed,
+            // x if x == Self::ActionNameInvalid as i32 => Self::ActionNameInvalid,
+            x if x == Self::ActionGoalAccepted as i32 => Self::ActionGoalAccepted,
+            x if x == Self::ActionGoalRejected as i32 => Self::ActionGoalRejected,
+            x if x == Self::ActionClientInvalid as i32 => Self::ActionClientInvalid,
+            x if x == Self::ActionClientTakeFailed as i32 => Self::ActionClientTakeFailed,
+            x if x == Self::ActionServerInvalid as i32 => Self::ActionServerInvalid,
+            x if x == Self::ActionServerTakeFailed as i32 => Self::ActionServerTakeFailed,
+            x if x == Self::ActionGoalHandleInvalid as i32 => Self::ActionGoalHandleInvalid,
+            x if x == Self::ActionGoalEventInvalid as i32 => Self::ActionGoalEventInvalid,
             x if x == Self::LifecycleStateRegistered as i32 => Self::LifecycleStateRegistered,
             x if x == Self::LifecycleStateNotRegistered as i32 => Self::LifecycleStateNotRegistered,
             other => {
@@ -386,6 +415,15 @@ impl Display for RclReturnCode {
             Self::EventTakeFailed => {
                 "Failed to take an event from the event handle (RCL_RET_EVENT_TAKE_FAILED)."
             }
+            // Self::ActionNameInvalid => "Action name does not pass validation (RCL_RET_ACTION_NAME_INVALID).",
+            Self::ActionGoalAccepted => "Action goal accepted (RCL_RET_ACTION_GOAL_ACCEPTED).",
+            Self::ActionGoalRejected => "Action goal rejected (RCL_RET_ACTION_GOAL_REJECTED).",
+            Self::ActionClientInvalid => "Action client is invalid (RCL_RET_ACTION_CLIENT_INVALID).",
+            Self::ActionClientTakeFailed => "Action client failed to take response (RCL_RET_ACTION_CLIENT_TAKE_FAILED).",
+            Self::ActionServerInvalid => "Action server is invalid (RCL_RET_ACTION_SERVER_INVALID).",
+            Self::ActionServerTakeFailed => "Action server failed to take request (RCL_RET_ACTION_SERVER_TAKE_FAILED).",
+            Self::ActionGoalHandleInvalid => "Action goal handle invalid (RCL_RET_ACTION_GOAL_HANDLE_INVALID).",
+            Self::ActionGoalEventInvalid => "Action invalid event (RCL_RET_ACTION_GOAL_EVENT_INVALID).",
             Self::LifecycleStateRegistered => {
                 "`rcl_lifecycle` state registered (RCL_RET_LIFECYCLE_STATE_REGISTERED)."
             }
diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs
index 366e499b..13bbafcb 100644
--- a/rclrs/src/lib.rs
+++ b/rclrs/src/lib.rs
@@ -174,10 +174,12 @@
 //! # Ok::<(), RclrsError>(())
 //! ```
 
+mod action;
 mod arguments;
 mod client;
 mod clock;
 mod context;
+mod drop_guard;
 mod error;
 mod executor;
 mod logging;
@@ -201,10 +203,12 @@ mod rcl_bindings;
 #[cfg(feature = "dyn_msg")]
 pub mod dynamic_message;
 
+pub use action::*;
 pub use arguments::*;
 pub use client::*;
 pub use clock::*;
 pub use context::*;
+use drop_guard::DropGuard;
 pub use error::*;
 pub use executor::*;
 pub use logging::*;
diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index dd01d060..3bbce0f6 100644
--- a/rclrs/src/node.rs
+++ b/rclrs/src/node.rs
@@ -349,6 +349,57 @@ impl NodeState {
     {
         ClientState::<T>::create(options, self)
     }
+    /// Creates an [`ActionClient`][1].
+    ///
+    /// [1]: crate::ActionClient
+    // TODO: make action client's lifetime depend on node's lifetime
+    pub fn create_action_client<'a, T>(
+        self: &Arc<Self>,
+        options: impl Into<ActionClientOptions<'a>>,
+    ) -> Result<ActionClient<T>, RclrsError>
+    where
+        T: rosidl_runtime_rs::Action,
+    {
+        let action_client = Arc::new(ActionClientState::<T>::new(self, options)?);
+        self.action_clients_mtx
+            .lock()
+            .unwrap()
+            .push(Arc::downgrade(&action_client) as Weak<dyn ActionClientBase>);
+        Ok(action_client)
+    }
+
+    /// Creates an [`ActionServer`][1].
+    ///
+    /// [1]: crate::ActionServer
+    // TODO: make action server's lifetime depend on node's lifetime
+    pub fn create_action_server<'a, ActionT, GoalCallback, CancelCallback, AcceptedCallback>(
+        self: &Arc<Self>,
+        options: impl Into<ActionServerOptions<'a>>,
+        handle_goal: GoalCallback,
+        handle_cancel: CancelCallback,
+        handle_accepted: AcceptedCallback,
+    ) -> Result<ActionServer<ActionT>, RclrsError>
+    where
+        ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
+        GoalCallback: Fn(GoalUuid, <ActionT as rosidl_runtime_rs::Action>::Goal) -> GoalResponse + 'static + Send + Sync,
+        CancelCallback: Fn(Arc<ServerGoalHandle<ActionT>>) -> CancelResponse + 'static + Send + Sync,
+        AcceptedCallback: Fn(Arc<ServerGoalHandle<ActionT>>) + 'static + Send + Sync,
+    {
+        let action_server = Arc::new(ActionServerState::<ActionT>::new(
+            self,
+            options,
+            handle_goal,
+            handle_cancel,
+            handle_accepted,
+        )?);
+        self.action_servers_mtx
+            .lock()
+            .unwrap()
+            .push(Arc::downgrade(&action_server) as Weak<dyn ActionServerBase>);
+        Ok(action_server)
+    }
+
+
 
     /// Creates a [`Publisher`].
     ///
diff --git a/rclrs/src/qos.rs b/rclrs/src/qos.rs
index dcde5902..dcaa0b30 100644
--- a/rclrs/src/qos.rs
+++ b/rclrs/src/qos.rs
@@ -295,6 +295,11 @@ impl QoSProfile {
     pub fn system_default() -> Self {
         QOS_PROFILE_SYSTEM_DEFAULT
     }
+
+    /// Get the default QoS profile for action status topics.
+    pub fn action_status_default() -> Self {
+        QOS_PROFILE_ACTION_STATUS_DEFAULT
+    }
 }
 
 impl From<QoSHistoryPolicy> for rmw_qos_history_policy_t {
@@ -478,3 +483,17 @@ pub const QOS_PROFILE_SYSTEM_DEFAULT: QoSProfile = QoSProfile {
     liveliness_lease: QoSDuration::SystemDefault,
     avoid_ros_namespace_conventions: false,
 };
+
+/// Equivalent to `rcl_action_qos_profile_status_default` from the [`rcl_action` package][1].
+///
+/// [1]: https://github.com/ros2/rcl/blob/rolling/rcl_action/include/rcl_action/default_qos.h
+pub const QOS_PROFILE_ACTION_STATUS_DEFAULT: QoSProfile = QoSProfile {
+    history: QoSHistoryPolicy::KeepLast { depth: 1 },
+    reliability: QoSReliabilityPolicy::Reliable,
+    durability: QoSDurabilityPolicy::TransientLocal,
+    deadline: QoSDuration::SystemDefault,
+    lifespan: QoSDuration::SystemDefault,
+    liveliness: QoSLivelinessPolicy::SystemDefault,
+    liveliness_lease: QoSDuration::SystemDefault,
+    avoid_ros_namespace_conventions: false,
+};
diff --git a/rclrs/src/rcl_bindings.rs b/rclrs/src/rcl_bindings.rs
index dbfb5d5b..e41a3407 100644
--- a/rclrs/src/rcl_bindings.rs
+++ b/rclrs/src/rcl_bindings.rs
@@ -142,6 +142,7 @@ cfg_if::cfg_if! {
         pub struct rosidl_message_type_support_t;
 
         pub const RMW_GID_STORAGE_SIZE: usize = 24;
+        pub const RCL_ACTION_UUID_SIZE: usize = 16;
 
         extern "C" {
             pub fn rcl_context_is_valid(context: *const rcl_context_t) -> bool;
@@ -150,6 +151,7 @@ cfg_if::cfg_if! {
         include!(concat!(env!("OUT_DIR"), "/rcl_bindings_generated.rs"));
 
         pub const RMW_GID_STORAGE_SIZE: usize = rmw_gid_storage_size_constant;
+        pub const RCL_ACTION_UUID_SIZE: usize = rcl_action_uuid_size_constant;
     }
 }
 
diff --git a/rclrs/src/rcl_wrapper.h b/rclrs/src/rcl_wrapper.h
index ececf491..998f4022 100644
--- a/rclrs/src/rcl_wrapper.h
+++ b/rclrs/src/rcl_wrapper.h
@@ -1,5 +1,6 @@
 #include <rcl/graph.h>
 #include <rcl/rcl.h>
+#include <rcl_action/rcl_action.h>
 #include <rcl_yaml_param_parser/parser.h>
 #include <rcutils/error_handling.h>
 #include <rcl/logging.h>
@@ -8,3 +9,4 @@
 #include <rosidl_typesupport_introspection_c/message_introspection.h>
 
 const size_t rmw_gid_storage_size_constant = RMW_GID_STORAGE_SIZE;
+const size_t rcl_action_uuid_size_constant = UUID_SIZE;

From 593a0ef939e4ab4ae48742eb5e400023373edfc7 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Sun, 20 Jul 2025 23:34:46 +0800
Subject: [PATCH 03/20] Rework readiness for waitables

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/Cargo.toml                       |   3 +
 rclrs/src/action.rs                    |   5 +-
 rclrs/src/action/server.rs             |  75 ++++-----------
 rclrs/src/action/server_goal_handle.rs |   9 +-
 rclrs/src/action/server_goal_state.rs  |  34 +++++++
 rclrs/src/client.rs                    |   5 +-
 rclrs/src/clock.rs                     |   5 +
 rclrs/src/error.rs                     |  20 +++-
 rclrs/src/node.rs                      |  12 ++-
 rclrs/src/service.rs                   |   7 +-
 rclrs/src/subscription.rs              |   7 +-
 rclrs/src/wait_set.rs                  |   2 +-
 rclrs/src/wait_set/guard_condition.rs  |   1 +
 rclrs/src/wait_set/rcl_primitive.rs    | 126 ++++++++++++++++++++++++-
 rclrs/src/wait_set/wait_set_runner.rs  |   4 +-
 rclrs/src/wait_set/waitable.rs         |  64 +++++++++----
 16 files changed, 279 insertions(+), 100 deletions(-)
 create mode 100644 rclrs/src/action/server_goal_state.rs

diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml
index a32f12fb..53a2d0f5 100644
--- a/rclrs/Cargo.toml
+++ b/rclrs/Cargo.toml
@@ -36,6 +36,9 @@ rosidl_runtime_rs = "0.4"
 serde = { version = "1", optional = true, features = ["derive"] }
 serde-big-array = { version = "0.5.1", optional = true }
 
+# Needed to watch for the cancel signal for actions
+tokio = { version = "1", features = ["sync"] }
+
 [dev-dependencies]
 # Needed for e.g. writing yaml files in tests
 tempfile = "3.3.0"
diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs
index 4432beac..0285598a 100644
--- a/rclrs/src/action.rs
+++ b/rclrs/src/action.rs
@@ -1,12 +1,13 @@
 pub(crate) mod client;
 pub(crate) mod server;
 mod server_goal_handle;
+mod server_goal_state;
 
 use crate::rcl_bindings::RCL_ACTION_UUID_SIZE;
 use std::fmt;
 
-pub use client::{ActionClient, ActionClientBase, ActionClientOptions, ActionClientState};
-pub use server::{ActionServer, ActionServerBase, ActionServerOptions, ActionServerState};
+pub use client::*;
+pub use server::*;
 pub use server_goal_handle::ServerGoalHandle;
 
 /// A unique identifier for a goal request.
diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs
index 949af77f..0ae0a5ac 100644
--- a/rclrs/src/action/server.rs
+++ b/rclrs/src/action/server.rs
@@ -2,7 +2,6 @@ use crate::{
     action::{CancelResponse, GoalResponse, GoalUuid, ServerGoalHandle},
     error::{RclReturnCode, ToResult},
     rcl_bindings::*,
-    wait::WaitableNumEntities,
     Clock, DropGuard, Node, NodeHandle, QoSProfile, RclrsError, ENTITY_LIFECYCLE_MUTEX,
 };
 use rosidl_runtime_rs::{Action, ActionImpl, Message, Service};
@@ -22,14 +21,13 @@ unsafe impl Send for rcl_action_server_t {}
 /// [dropped after][1] the `rcl_action_server_t`.
 ///
 /// [1]: <https://doc.rust-lang.org/reference/destructors.html>
-pub struct ActionServerHandle {
+pub(crate) struct ActionServerHandle {
     rcl_action_server: Mutex<rcl_action_server_t>,
     node_handle: Arc<NodeHandle>,
-    pub(crate) in_use_by_wait_set: Arc<AtomicBool>,
 }
 
 impl ActionServerHandle {
-    pub(crate) fn lock(&self) -> MutexGuard<rcl_action_server_t> {
+    fn lock(&self) -> MutexGuard<rcl_action_server_t> {
         self.rcl_action_server.lock().unwrap()
     }
 }
@@ -47,18 +45,6 @@ impl Drop for ActionServerHandle {
     }
 }
 
-/// Trait to be implemented by concrete ActionServer structs.
-///
-/// See [`ActionServer<T>`] for an example
-pub trait ActionServerBase: Send + Sync {
-    /// Internal function to get a reference to the `rcl` handle.
-    fn handle(&self) -> &ActionServerHandle;
-    /// Returns the number of underlying entities for the action server.
-    fn num_entities(&self) -> &WaitableNumEntities;
-    /// Tries to run the callback for the given readiness mode.
-    fn execute(self: Arc<Self>, mode: ReadyMode) -> Result<(), RclrsError>;
-}
-
 pub(crate) enum ReadyMode {
     GoalRequest,
     CancelRequest,
@@ -66,9 +52,9 @@ pub(crate) enum ReadyMode {
     GoalExpired,
 }
 
-pub type GoalCallback<ActionT> = dyn Fn(GoalUuid, <ActionT as rosidl_runtime_rs::Action>::Goal) -> GoalResponse + 'static + Send + Sync;
-pub type CancelCallback<ActionT> = dyn Fn(Arc<ServerGoalHandle<ActionT>>) -> CancelResponse + 'static + Send + Sync;
-pub type AcceptedCallback<ActionT> = dyn Fn(Arc<ServerGoalHandle<ActionT>>) + 'static + Send + Sync;
+pub type GoalCallback<A> = dyn Fn(GoalUuid, <A as Action>::Goal) -> GoalResponse + 'static + Send + Sync;
+pub type CancelCallback<A> = dyn Fn(Arc<ServerGoalHandle<A>>) -> CancelResponse + 'static + Send + Sync;
+pub type AcceptedCallback<A> = dyn Fn(Arc<ServerGoalHandle<A>>) + 'static + Send + Sync;
 
 /// An action server that can respond to requests sent by ROS action clients.
 ///
@@ -96,19 +82,16 @@ pub type ActionServer<ActionT> = Arc<ActionServerState<ActionT>>;
 /// The public API of the [`ActionServer`] type is implemented via `ActionServerState`.
 ///
 /// [1]: std::sync::Weak
-pub struct ActionServerState<ActionT>
+pub struct ActionServerState<A>
 where
-    ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
+    A: ActionImpl,
 {
     pub(crate) handle: Arc<ActionServerHandle>,
-    num_entities: WaitableNumEntities,
-    goal_callback: Box<GoalCallback<ActionT>>,
-    cancel_callback: Box<CancelCallback<ActionT>>,
-    accepted_callback: Box<AcceptedCallback<ActionT>>,
+
     // TODO(nwn): Audit these three mutexes to ensure there's no deadlocks or broken invariants. We
     // may want to join them behind a shared mutex, at least for the `goal_results` and `result_requests`.
-    goal_handles: Mutex<HashMap<GoalUuid, Arc<ServerGoalHandle<ActionT>>>>,
-    goal_results: Mutex<HashMap<GoalUuid, <<ActionT::GetResultService as Service>::Response as Message>::RmwMsg>>,
+    goal_handles: Mutex<HashMap<GoalUuid, Arc<ServerGoalHandle<A>>>>,
+    goal_results: Mutex<HashMap<GoalUuid, <<A::GetResultService as Service>::Response as Message>::RmwMsg>>,
     result_requests: Mutex<HashMap<GoalUuid, Vec<rmw_request_id_t>>>,
     /// Ensure the parent node remains alive as long as the subscription is held.
     /// This implementation will change in the future.
@@ -118,19 +101,16 @@ where
 
 impl<T> ActionServerState<T>
 where
-    T: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
+    T: ActionImpl,
 {
     /// Creates a new action server.
-    pub(crate) fn new<'a>(
+    pub(crate) fn create<'a>(
         node: &Node,
         options: impl Into<ActionServerOptions<'a>>,
         goal_callback: impl Fn(GoalUuid, T::Goal) -> GoalResponse + 'static + Send + Sync,
         cancel_callback: impl Fn(Arc<ServerGoalHandle<T>>) -> CancelResponse + 'static + Send + Sync,
         accepted_callback: impl Fn(Arc<ServerGoalHandle<T>>) + 'static + Send + Sync,
-    ) -> Result<Self, RclrsError>
-    where
-        T: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
-    {
+    ) -> Result<Self, RclrsError> {
         let options = options.into();
         // SAFETY: Getting a zero-initialized value is always safe.
         let mut rcl_action_server = unsafe { rcl_action_get_zero_initialized_server() };
@@ -145,9 +125,9 @@ where
         let action_server_options = unsafe { rcl_action_server_get_default_options() };
 
         {
-            let mut rcl_node = node.handle.rcl_node.lock().unwrap();
+            let mut rcl_node = node.handle().rcl_node.lock().unwrap();
             let clock = node.get_clock();
-            let rcl_clock = clock.rcl_clock();
+            let rcl_clock = clock.get_rcl_clock();
             let mut rcl_clock = rcl_clock.lock().unwrap();
             let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap();
 
@@ -173,8 +153,7 @@ where
 
         let handle = Arc::new(ActionServerHandle {
             rcl_action_server: Mutex::new(rcl_action_server),
-            node_handle: Arc::clone(&node.handle),
-            in_use_by_wait_set: Arc::new(AtomicBool::new(false)),
+            node_handle: Arc::clone(&node.handle()),
         });
 
         let mut num_entities = WaitableNumEntities::default();
@@ -718,28 +697,6 @@ where
     }
 }
 
-impl<T> ActionServerBase for ActionServerState<T>
-where
-    T: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
-{
-    fn handle(&self) -> &ActionServerHandle {
-        &self.handle
-    }
-
-    fn num_entities(&self) -> &WaitableNumEntities {
-        &self.num_entities
-    }
-
-    fn execute(self: Arc<Self>, mode: ReadyMode) -> Result<(), RclrsError> {
-        match mode {
-            ReadyMode::GoalRequest => self.execute_goal_request(),
-            ReadyMode::CancelRequest => self.execute_cancel_request(),
-            ReadyMode::ResultRequest => self.execute_result_request(),
-            ReadyMode::GoalExpired => self.execute_goal_expired(),
-        }
-    }
-}
-
 /// `ActionServerOptions` are used by [`Node::create_action_server`][1] to initialize an
 /// [`ActionServer`].
 ///
diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs
index 6910f8d5..3602b3a0 100644
--- a/rclrs/src/action/server_goal_handle.rs
+++ b/rclrs/src/action/server_goal_handle.rs
@@ -1,5 +1,6 @@
 use crate::{action::ActionServerState, rcl_bindings::*, GoalUuid, RclrsError, ToResult};
 use std::sync::{Arc, Mutex, Weak};
+use rosidl_runtime_rs::Action;
 
 // Values defined by `action_msgs/msg/GoalStatus`
 #[repr(i8)]
@@ -20,13 +21,13 @@ enum GoalStatus {
 ///
 /// This type will only be created by an [`ActionServer`] when a goal is accepted and will be
 /// passed to the user in the associated `handle_accepted` callback.
-pub struct ServerGoalHandle<ActionT>
+pub struct ServerGoalHandle<A>
 where
-    ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
+    A: Action,
 {
     rcl_handle: Mutex<*mut rcl_action_goal_handle_t>,
-    action_server: Weak<ActionServerState<ActionT>>,
-    goal_request: Arc<ActionT::Goal>,
+    action_server: Weak<ActionServerState<A>>,
+    goal_request: Arc<A::Goal>,
     uuid: GoalUuid,
 }
 
diff --git a/rclrs/src/action/server_goal_state.rs b/rclrs/src/action/server_goal_state.rs
new file mode 100644
index 00000000..06db4a79
--- /dev/null
+++ b/rclrs/src/action/server_goal_state.rs
@@ -0,0 +1,34 @@
+use std::sync::Arc;
+use tokio::sync::watch::{Sender, Receiver};
+use rosidl_runtime_rs::ActionImpl;
+
+use crate::{
+    ActionServerState, GoalUuid,
+};
+
+struct ServerGoalState<A: ActionImpl> {
+    cancellation: Arc<CancellationState>,
+    server_state: Arc<ActionServerState<A>>,
+}
+
+struct CancellationState<A: ActionImpl> {
+    receiver: Receiver<CancellationMode<A>>,
+    sender: Sender<CancellationMode<A>>,
+}
+
+enum CancellationMode<A: ActionImpl> {
+    None,
+    CancelRequested(CancellationResponder<A>),
+    Cancelling,
+}
+
+struct CancellationResponder<A: ActionImpl> {
+    uuid: GoalUuid,
+    handle: Arc<ActionServerState<A>>,
+}
+
+impl CancellationResponder {
+    fn send_cancel_response(&self) {
+
+    }
+}
diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs
index 971d4f88..29f03fa3 100644
--- a/rclrs/src/client.rs
+++ b/rclrs/src/client.rs
@@ -10,7 +10,7 @@ use rosidl_runtime_rs::Message;
 use crate::{
     error::ToResult, log_fatal, rcl_bindings::*, IntoPrimitiveOptions, MessageCow, Node, Promise,
     QoSProfile, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclReturnCode, RclrsError,
-    ServiceInfo, Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
+    ReadyKind, ServiceInfo, Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
 };
 
 mod client_async_callback;
@@ -415,7 +415,8 @@ impl<T> RclPrimitive for ClientExecutable<T>
 where
     T: rosidl_runtime_rs::Service,
 {
-    unsafe fn execute(&mut self, _: &mut dyn Any) -> Result<(), RclrsError> {
+    unsafe fn execute(&mut self, ready: ReadyKind, _: &mut dyn Any) -> Result<(), RclrsError> {
+        ready.is_basic()?;
         self.board.lock().unwrap().execute(&self.handle)
     }
 
diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index 992cd4b4..8dd54345 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -88,6 +88,11 @@ impl Clock {
         Self { kind, rcl_clock }
     }
 
+    /// Returns the clock's `rcl_clock_t`.
+    pub(crate) fn get_rcl_clock(&self) -> &Arc<Mutex<rcl_clock_t>> {
+        &self.rcl_clock
+    }
+
     /// Returns the clock's `ClockType`.
     pub fn clock_type(&self) -> ClockType {
         self.kind
diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs
index 1293457e..4c25b04d 100644
--- a/rclrs/src/error.rs
+++ b/rclrs/src/error.rs
@@ -4,7 +4,7 @@ use std::{
     fmt::{self, Display},
 };
 
-use crate::{rcl_bindings::*, DeclarationError};
+use crate::{rcl_bindings::*, DeclarationError, ReadyKind};
 
 /// The main error type.
 #[derive(Debug, PartialEq, Eq)]
@@ -51,6 +51,14 @@ pub enum RclrsError {
     ParameterDeclarationError(crate::DeclarationError),
     /// A mutex used internally has been [poisoned][std::sync::PoisonError].
     PoisonedMutex,
+    /// An [`crate::RclPrimitive`] received ready information that is not
+    /// compatible with its type.
+    InvalidReadyInformation {
+        /// The expected format of the ready information (default-initialized)
+        expected: ReadyKind,
+        /// The ready information that was received.
+        received: ReadyKind,
+    }
 }
 
 impl RclrsError {
@@ -118,6 +126,15 @@ impl Display for RclrsError {
             RclrsError::PoisonedMutex => {
                 write!(f, "A mutex used internally has been poisoned")
             }
+            RclrsError::InvalidReadyInformation { expected, received } => {
+                write!(
+                    f,
+                    "Invalid ready information was provided. This suggests an error \
+                    in how the wait set is being used.\
+                    \n - Expected information: {expected:?}\
+                    \n - Actual: {received:?}",
+                )
+            }
         }
     }
 }
@@ -157,6 +174,7 @@ impl Error for RclrsError {
             RclrsError::InvalidPayload { .. } => None,
             RclrsError::ParameterDeclarationError(_) => None,
             RclrsError::PoisonedMutex => None,
+            RclrsError::InvalidReadyInformation { .. } => None,
         }
     }
 }
diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index 3bbce0f6..5b604a0e 100644
--- a/rclrs/src/node.rs
+++ b/rclrs/src/node.rs
@@ -29,11 +29,13 @@ use async_std::future::timeout;
 use rosidl_runtime_rs::Message;
 
 use crate::{
-    rcl_bindings::*, Client, ClientOptions, ClientState, Clock, ContextHandle, ExecutorCommands,
-    IntoAsyncServiceCallback, IntoAsyncSubscriptionCallback, IntoNodeServiceCallback,
-    IntoNodeSubscriptionCallback, LogParams, Logger, ParameterBuilder, ParameterInterface,
-    ParameterVariant, Parameters, Promise, Publisher, PublisherOptions, PublisherState, RclrsError,
-    Service, ServiceOptions, ServiceState, Subscription, SubscriptionOptions, SubscriptionState,
+    rcl_bindings::*, ActionClient, ActionClientOptions, ActionClientState,
+    ActionServer, ActionServerOptions, ActionServerState, Client, ClientOptions,
+    ClientState, Clock, ContextHandle, ExecutorCommands, IntoAsyncServiceCallback,
+    IntoAsyncSubscriptionCallback, IntoNodeServiceCallback, IntoNodeSubscriptionCallback,
+    LogParams, Logger, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters,
+    Promise, Publisher, PublisherOptions, PublisherState, RclrsError, Service,
+    ServiceOptions, ServiceState, Subscription, SubscriptionOptions, SubscriptionState,
     TimeSource, ToLogParams, Worker, WorkerOptions, WorkerState, ENTITY_LIFECYCLE_MUTEX,
 };
 
diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs
index aaa07abc..985827f9 100644
--- a/rclrs/src/service.rs
+++ b/rclrs/src/service.rs
@@ -9,8 +9,8 @@ use rosidl_runtime_rs::{Message, Service as IdlService};
 
 use crate::{
     error::ToResult, rcl_bindings::*, IntoPrimitiveOptions, MessageCow, Node, NodeHandle,
-    QoSProfile, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, Waitable,
-    WaitableLifecycle, WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX,
+    QoSProfile, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, ReadyKind,
+    Waitable, WaitableLifecycle, WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX,
 };
 
 mod any_service_callback;
@@ -249,7 +249,8 @@ where
     T: IdlService,
     Scope: WorkScope,
 {
-    unsafe fn execute(&mut self, payload: &mut dyn Any) -> Result<(), RclrsError> {
+    unsafe fn execute(&mut self, ready: ReadyKind, payload: &mut dyn Any) -> Result<(), RclrsError> {
+        ready.is_basic()?;
         self.callback
             .lock()
             .unwrap()
diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs
index e57c542f..9409fd0a 100644
--- a/rclrs/src/subscription.rs
+++ b/rclrs/src/subscription.rs
@@ -8,8 +8,8 @@ use rosidl_runtime_rs::{Message, RmwMessage};
 
 use crate::{
     error::ToResult, qos::QoSProfile, rcl_bindings::*, IntoPrimitiveOptions, Node, NodeHandle,
-    RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, Waitable, WaitableLifecycle,
-    WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX,
+    RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, ReadyKind, Waitable,
+    WaitableLifecycle, WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX,
 };
 
 mod any_subscription_callback;
@@ -261,7 +261,8 @@ impl<T, Payload: 'static> RclPrimitive for SubscriptionExecutable<T, Payload>
 where
     T: Message,
 {
-    unsafe fn execute(&mut self, payload: &mut dyn Any) -> Result<(), RclrsError> {
+    unsafe fn execute(&mut self, ready: ReadyKind, payload: &mut dyn Any) -> Result<(), RclrsError> {
+        ready.is_basic()?;
         self.callback
             .lock()
             .unwrap()
diff --git a/rclrs/src/wait_set.rs b/rclrs/src/wait_set.rs
index a7951f00..c4d7ca06 100644
--- a/rclrs/src/wait_set.rs
+++ b/rclrs/src/wait_set.rs
@@ -102,7 +102,7 @@ impl WaitSet {
     pub fn wait(
         &mut self,
         timeout: Option<Duration>,
-        mut f: impl FnMut(&mut dyn RclPrimitive) -> Result<(), RclrsError>,
+        mut f: impl FnMut(&mut dyn RclPrimitive, ReadyKind) -> Result<(), RclrsError>,
     ) -> Result<(), RclrsError> {
         let timeout_ns = match timeout.map(|d| d.as_nanos()) {
             None => -1,
diff --git a/rclrs/src/wait_set/guard_condition.rs b/rclrs/src/wait_set/guard_condition.rs
index 8860e23a..86a7c645 100644
--- a/rclrs/src/wait_set/guard_condition.rs
+++ b/rclrs/src/wait_set/guard_condition.rs
@@ -136,6 +136,7 @@ struct GuardConditionHandle {
 /// condition variables are owned at the rclrs layer while others were obtained
 /// from rcl and either have static lifetimes or lifetimes that depend on
 /// something else.
+#[derive(Debug)]
 pub enum InnerGuardConditionHandle {
     /// This variant means the guard condition was created and owned by rclrs.
     /// Its memory is managed by us.
diff --git a/rclrs/src/wait_set/rcl_primitive.rs b/rclrs/src/wait_set/rcl_primitive.rs
index 205926c6..7d5af77a 100644
--- a/rclrs/src/wait_set/rcl_primitive.rs
+++ b/rclrs/src/wait_set/rcl_primitive.rs
@@ -1,6 +1,8 @@
 use std::{any::Any, sync::MutexGuard};
 
-use crate::{rcl_bindings::*, InnerGuardConditionHandle, RclrsError};
+use crate::{
+    log_error, rcl_bindings::*, InnerGuardConditionHandle, RclrsError, ToResult,
+};
 
 /// This provides the public API for executing a waitable item.
 pub trait RclPrimitive: Send + Sync {
@@ -20,7 +22,7 @@ pub trait RclPrimitive: Send + Sync {
     /// [1]: crate::ExecutorChannel
     /// [2]: crate::WorkerChannel
     /// [3]: crate::Worker
-    unsafe fn execute(&mut self, payload: &mut dyn Any) -> Result<(), RclrsError>;
+    unsafe fn execute(&mut self, ready: ReadyKind, payload: &mut dyn Any) -> Result<(), RclrsError>;
 
     /// Indicate what kind of primitive this is.
     fn kind(&self) -> RclPrimitiveKind;
@@ -44,9 +46,12 @@ pub enum RclPrimitiveKind {
     Service,
     /// Event
     Event,
+    /// Action Server
+    ActionServer,
 }
 
 /// Used by the wait set to obtain the handle of a primitive.
+#[derive(Debug)]
 pub enum RclPrimitiveHandle<'a> {
     /// Handle for a subscription
     Subscription(MutexGuard<'a, rcl_subscription_t>),
@@ -60,4 +65,121 @@ pub enum RclPrimitiveHandle<'a> {
     Service(MutexGuard<'a, rcl_service_t>),
     /// Handle for an event
     Event(MutexGuard<'a, rcl_event_t>),
+    /// Handle for an action server
+    ActionServer(MutexGuard<'a, rcl_action_server_t>),
+}
+
+/// Describe the way in which a waitable is ready.
+#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+pub enum ReadyKind {
+    /// The basic readiness is for most wait set primitives, which only have one
+    /// execution path that may be ready.
+    Basic,
+    /// This type of readiness is specific to action servers, which consist of
+    /// multiple primitives. Any combination of those primitives might be ready,
+    /// and we need to know which to execute specifically.
+    ActionServer(ActionServerReady),
+}
+
+impl ReadyKind {
+    /// Convert a pointer's status into a basic ready indicator.
+    pub fn for_ptr<T>(ptr: *const T) -> Option<Self> {
+        if ptr.is_null() {
+            None
+        } else {
+            Some(Self::Basic)
+        }
+    }
+
+    /// This is used by the basic primitive types to validate that they are
+    /// receiving ready information that matches their primitive type.
+    pub fn is_basic(&self) -> Result<(), RclrsError> {
+        match self {
+            Self::Basic => Ok(()),
+            _ => Err(RclrsError::InvalidReadyInformation {
+                expected: Self::Basic,
+                received: *self,
+            })
+        }
+    }
+}
+
+/// This is the ready information for an action server.
+///
+/// Action servers provide multiple services bundled together. When a wait set
+/// wakes up it is possible for any number of those services to be ready for
+/// processing. This struct conveys which of an action's services are ready.
+#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+pub struct ActionServerReady {
+    /// True if there is a goal request message ready to take, false otherwise.
+    pub goal_request: bool,
+    /// True if there is a cancel request message ready to take, false otherwise.
+    pub cancel_request: bool,
+    /// True if there is a result request message ready to take, false otherwise.
+    pub result_request: bool,
+    //// True if a goal has expired, false otherwise.
+    pub goal_expired: bool,
+}
+
+impl Default for ActionServerReady {
+    fn default() -> Self {
+        Self {
+            goal_request: false,
+            cancel_request: false,
+            result_request: false,
+            goal_expired: false,
+        }
+    }
+}
+
+impl ActionServerReady {
+    /// Check whether any primitives in an action server are ready to be processed.
+    ///
+    /// SAFETY: This calls a function which is not thread-safe. The wait set and
+    /// action server handles must not be in used in any other threads. The
+    /// [`MutexGuard`] should ensure this for the action server, but it is up to
+    /// the caller to ensure that the wait set is not being simultaneously accessed
+    /// in any other thread.
+    pub(crate) unsafe fn check(
+        wait_set: &rcl_wait_set_t,
+        action_server: MutexGuard<rcl_action_server_t>,
+    ) -> Option<ReadyKind> {
+        let mut ready = ActionServerReady::default();
+        let r;
+        unsafe {
+            // SAFETY: We give a safety warning to ensure that the wait set is
+            // not being used elsewhere. The action server handle is guarded by
+            // the mutex. With those two requirements met, we do not need to
+            // worry about this function not being thread-safe.
+            r = rcl_action_server_wait_set_get_entities_ready(
+                wait_set,
+                &*action_server,
+                &mut ready.goal_request,
+                &mut ready.cancel_request,
+                &mut ready.result_request,
+                &mut ready.goal_expired,
+            );
+        }
+        if let Err(err) = r.ok() {
+            log_error!(
+                "ActionServerReady.check",
+                "Error while checking action server: {err}",
+            );
+        }
+
+        if ready.any_ready() {
+            Some(ReadyKind::ActionServer(ready))
+        } else {
+            None
+        }
+    }
+
+    /// Check whether any of the primitives of the action server are ready. When
+    /// this is false, we can skip producing a [`ReadyKind`] entirely.
+    pub fn any_ready(&self) -> bool {
+        self.goal_request
+        || self.cancel_request
+        || self.result_request
+        || self.goal_expired
+    }
 }
diff --git a/rclrs/src/wait_set/wait_set_runner.rs b/rclrs/src/wait_set/wait_set_runner.rs
index b60f0705..1eb8e745 100644
--- a/rclrs/src/wait_set/wait_set_runner.rs
+++ b/rclrs/src/wait_set/wait_set_runner.rs
@@ -184,12 +184,12 @@ impl WaitSetRunner {
             });
 
             let mut at_least_one = false;
-            self.wait_set.wait(timeout, |executable| {
+            self.wait_set.wait(timeout, |executable, ready| {
                 at_least_one = true;
                 // SAFETY: The user of WaitSetRunner is responsible for ensuring
                 // the runner has the same payload type as the executables that
                 // are given to it.
-                unsafe { executable.execute(&mut *self.payload) }
+                unsafe { executable.execute(ready, &mut *self.payload) }
             })?;
 
             if at_least_one {
diff --git a/rclrs/src/wait_set/waitable.rs b/rclrs/src/wait_set/waitable.rs
index d510d1f9..4978f45b 100644
--- a/rclrs/src/wait_set/waitable.rs
+++ b/rclrs/src/wait_set/waitable.rs
@@ -4,15 +4,18 @@ use std::sync::{
 };
 
 use crate::{
-    error::ToResult, rcl_bindings::*, GuardCondition, RclPrimitive, RclPrimitiveHandle,
-    RclPrimitiveKind, RclrsError,
+    error::ToResult, log_error, rcl_bindings::*, ActionServerReady, GuardCondition,
+    RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, ReadyKind,
 };
 
+/// How many services are added to a wait set by an action server.
+const SERVICES_PER_ACTION_SERVER: usize = 3;
+
 /// This struct manages the presence of an rcl primitive inside the wait set.
 /// It will keep track of where the primitive is within the wait set as well as
 /// automatically remove the primitive from the wait set once it isn't being
 /// used anymore.
-#[must_use = "If you do not give the Waiter to a WaitSet then it will never be useful"]
+#[must_use = "If you do not give the Waitable to a WaitSet then it will never be useful"]
 pub struct Waitable {
     pub(super) primitive: Box<dyn RclPrimitive>,
     in_use: Arc<AtomicBool>,
@@ -26,7 +29,7 @@ impl Waitable {
         guard_condition: Option<Arc<GuardCondition>>,
     ) -> (Self, WaitableLifecycle) {
         let in_use = Arc::new(AtomicBool::new(true));
-        let waiter = Self {
+        let waitable = Self {
             primitive,
             in_use: Arc::clone(&in_use),
             index_in_wait_set: None,
@@ -36,7 +39,7 @@ impl Waitable {
             in_use,
             guard_condition,
         };
-        (waiter, lifecycle)
+        (waitable, lifecycle)
     }
 
     pub(super) fn in_wait_set(&self) -> bool {
@@ -47,24 +50,49 @@ impl Waitable {
         self.in_use.load(Ordering::Relaxed)
     }
 
-    pub(super) fn is_ready(&self, wait_set: &rcl_wait_set_t) -> bool {
-        self.index_in_wait_set.is_some_and(|index| {
-            let ptr_is_null = unsafe {
+    pub(super) fn is_ready(&self, wait_set: &rcl_wait_set_t) -> Option<ReadyKind> {
+        self.index_in_wait_set.and_then(|index| {
+            unsafe {
                 // SAFETY: Each field in the wait set is an array of points.
                 // The dereferencing that we do is equivalent to obtaining the
                 // element of the array at the index-th position.
                 match self.primitive.kind() {
-                    RclPrimitiveKind::Subscription => wait_set.subscriptions.add(index).is_null(),
+                    RclPrimitiveKind::Subscription => {
+                        ReadyKind::for_ptr(wait_set.subscriptions.add(index))
+                    },
                     RclPrimitiveKind::GuardCondition => {
-                        wait_set.guard_conditions.add(index).is_null()
+                        ReadyKind::for_ptr(wait_set.guard_conditions.add(index))
+                    }
+                    RclPrimitiveKind::Service => {
+                        ReadyKind::for_ptr(wait_set.services.add(index))
+                    }
+                    RclPrimitiveKind::Client => {
+                        ReadyKind::for_ptr(wait_set.clients.add(index))
+                    }
+                    RclPrimitiveKind::Timer => {
+                        ReadyKind::for_ptr(wait_set.timers.add(index))
+                    }
+                    RclPrimitiveKind::Event => {
+                        ReadyKind::for_ptr(wait_set.events.add(index))
+                    }
+                    RclPrimitiveKind::ActionServer => {
+                        match self.primitive.handle() {
+                            RclPrimitiveHandle::ActionServer(handle) => {
+                                ActionServerReady::check(wait_set, handle)
+                            }
+                            handle => {
+                                log_error!(
+                                    "waitable.is_ready",
+                                    "Invalid handle for ActionServer type: {handle:?}. \
+                                    This indicates a bug in the implementation of rclrs. \
+                                    Please report this to the rclrs maintainers.",
+                                );
+                                None
+                            }
+                        }
                     }
-                    RclPrimitiveKind::Service => wait_set.services.add(index).is_null(),
-                    RclPrimitiveKind::Client => wait_set.clients.add(index).is_null(),
-                    RclPrimitiveKind::Timer => wait_set.timers.add(index).is_null(),
-                    RclPrimitiveKind::Event => wait_set.events.add(index).is_null(),
                 }
-            };
-            !ptr_is_null
+            }
         })
     }
 
@@ -95,6 +123,9 @@ impl Waitable {
                 RclPrimitiveHandle::Event(handle) => {
                     rcl_wait_set_add_event(wait_set, &*handle, &mut index)
                 }
+                RclPrimitiveHandle::ActionServer(handle) => {
+                    rcl_action_wait_set_add_action_server(wait_set, &*handle, &mut index)
+                }
             }
         }
         .ok()?;
@@ -153,6 +184,7 @@ impl WaitableCount {
             RclPrimitiveKind::Client => self.clients += count,
             RclPrimitiveKind::Service => self.services += count,
             RclPrimitiveKind::Event => self.events += count,
+            RclPrimitiveKind::ActionServer => self.services += SERVICES_PER_ACTION_SERVER*count,
         }
     }
 

From ac35d169a8898067476e55a7403c93821dfcea31 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Sat, 26 Jul 2025 23:43:16 +0800
Subject: [PATCH 04/20] Reworking action server goal handle lifecycles

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/src/action.rs                           |  25 +-
 .../action/{client.rs => action_client.rs}    |   0
 .../action/{server.rs => action_server.rs}    | 180 +++-----
 rclrs/src/action/action_server_goal_handle.rs | 391 ++++++++++++++++++
 rclrs/src/action/server_goal_handle.rs        | 242 -----------
 rclrs/src/action/server_goal_state.rs         |  34 --
 rclrs/src/time.rs                             |  20 +-
 rclrs/src/wait_set/guard_condition.rs         |   5 +-
 8 files changed, 484 insertions(+), 413 deletions(-)
 rename rclrs/src/action/{client.rs => action_client.rs} (100%)
 rename rclrs/src/action/{server.rs => action_server.rs} (84%)
 create mode 100644 rclrs/src/action/action_server_goal_handle.rs
 delete mode 100644 rclrs/src/action/server_goal_handle.rs
 delete mode 100644 rclrs/src/action/server_goal_state.rs

diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs
index 0285598a..b3b7b060 100644
--- a/rclrs/src/action.rs
+++ b/rclrs/src/action.rs
@@ -1,14 +1,17 @@
-pub(crate) mod client;
-pub(crate) mod server;
-mod server_goal_handle;
-mod server_goal_state;
+use std::ops::Deref;
+
+pub(crate) mod action_client;
+pub(crate) mod action_server;
+mod action_server_goal_handle;
+mod action_server_goal_state;
 
 use crate::rcl_bindings::RCL_ACTION_UUID_SIZE;
 use std::fmt;
 
-pub use client::*;
-pub use server::*;
-pub use server_goal_handle::ServerGoalHandle;
+pub use action_client::*;
+pub use action_server::*;
+use action_server_goal_handle::{LiveActionServerGoalHandle, DroppedActionServerGoalHandle};
+use action_server_goal_state::*;
 
 /// A unique identifier for a goal request.
 #[derive(Copy, Clone, Debug, Default, PartialEq, Eq, PartialOrd, Ord, Hash)]
@@ -37,6 +40,14 @@ impl fmt::Display for GoalUuid {
     }
 }
 
+impl Deref for GoalUuid {
+    type Target = [u8; RCL_ACTION_UUID_SIZE];
+
+    fn deref(&self) -> &Self::Target {
+        &self.0
+    }
+}
+
 /// The response returned by an [`ActionServer`]'s goal callback when a goal request is received.
 #[derive(PartialEq, Eq)]
 pub enum GoalResponse {
diff --git a/rclrs/src/action/client.rs b/rclrs/src/action/action_client.rs
similarity index 100%
rename from rclrs/src/action/client.rs
rename to rclrs/src/action/action_client.rs
diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/action_server.rs
similarity index 84%
rename from rclrs/src/action/server.rs
rename to rclrs/src/action/action_server.rs
index 0ae0a5ac..f42e941e 100644
--- a/rclrs/src/action/server.rs
+++ b/rclrs/src/action/action_server.rs
@@ -1,50 +1,17 @@
 use crate::{
-    action::{CancelResponse, GoalResponse, GoalUuid, ServerGoalHandle},
+    action::{CancelResponse, GoalResponse, GoalUuid, ActionServerGoalHandle},
     error::{RclReturnCode, ToResult},
     rcl_bindings::*,
-    Clock, DropGuard, Node, NodeHandle, QoSProfile, RclrsError, ENTITY_LIFECYCLE_MUTEX,
+    DropGuard, Node, NodeHandle, QoSProfile, RclrsError, ENTITY_LIFECYCLE_MUTEX,
 };
 use rosidl_runtime_rs::{Action, ActionImpl, Message, Service};
 use std::{
     borrow::Borrow,
     collections::HashMap,
     ffi::CString,
-    sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard},
+    sync::{Arc, Mutex, MutexGuard, Weak},
 };
 
-// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
-// they are running in. Therefore, this type can be safely sent to another thread.
-unsafe impl Send for rcl_action_server_t {}
-
-/// Manage the lifecycle of an `rcl_action_server_t`, including managing its dependencies
-/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are
-/// [dropped after][1] the `rcl_action_server_t`.
-///
-/// [1]: <https://doc.rust-lang.org/reference/destructors.html>
-pub(crate) struct ActionServerHandle {
-    rcl_action_server: Mutex<rcl_action_server_t>,
-    node_handle: Arc<NodeHandle>,
-}
-
-impl ActionServerHandle {
-    fn lock(&self) -> MutexGuard<rcl_action_server_t> {
-        self.rcl_action_server.lock().unwrap()
-    }
-}
-
-impl Drop for ActionServerHandle {
-    fn drop(&mut self) {
-        let rcl_action_server = self.rcl_action_server.get_mut().unwrap();
-        let mut rcl_node = self.node_handle.rcl_node.lock().unwrap();
-        let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap();
-        // SAFETY: The entity lifecycle mutex is locked to protect against the risk of
-        // global variables in the rmw implementation being unsafely modified during cleanup.
-        unsafe {
-            rcl_action_server_fini(rcl_action_server, &mut *rcl_node);
-        }
-    }
-}
-
 pub(crate) enum ReadyMode {
     GoalRequest,
     CancelRequest,
@@ -52,10 +19,6 @@ pub(crate) enum ReadyMode {
     GoalExpired,
 }
 
-pub type GoalCallback<A> = dyn Fn(GoalUuid, <A as Action>::Goal) -> GoalResponse + 'static + Send + Sync;
-pub type CancelCallback<A> = dyn Fn(Arc<ServerGoalHandle<A>>) -> CancelResponse + 'static + Send + Sync;
-pub type AcceptedCallback<A> = dyn Fn(Arc<ServerGoalHandle<A>>) + 'static + Send + Sync;
-
 /// An action server that can respond to requests sent by ROS action clients.
 ///
 /// Create an action server using [`Node::create_action_server`][1].
@@ -70,7 +33,7 @@ pub type AcceptedCallback<A> = dyn Fn(Arc<ServerGoalHandle<A>>) + 'static + Send
 ///
 /// [1]: crate::NodeState::create_action_server
 /// [2]: crate::spin
-pub type ActionServer<ActionT> = Arc<ActionServerState<ActionT>>;
+pub type ActionServer<A> = Arc<ActionServerState<A>>;
 
 /// The inner state of an [`ActionServer`].
 ///
@@ -82,23 +45,25 @@ pub type ActionServer<ActionT> = Arc<ActionServerState<ActionT>>;
 /// The public API of the [`ActionServer`] type is implemented via `ActionServerState`.
 ///
 /// [1]: std::sync::Weak
-pub struct ActionServerState<A>
-where
-    A: ActionImpl,
-{
+pub struct ActionServerState<A: ActionImpl> {
     pub(crate) handle: Arc<ActionServerHandle>,
 
-    // TODO(nwn): Audit these three mutexes to ensure there's no deadlocks or broken invariants. We
-    // may want to join them behind a shared mutex, at least for the `goal_results` and `result_requests`.
-    goal_handles: Mutex<HashMap<GoalUuid, Arc<ServerGoalHandle<A>>>>,
-    goal_results: Mutex<HashMap<GoalUuid, <<A::GetResultService as Service>::Response as Message>::RmwMsg>>,
-    result_requests: Mutex<HashMap<GoalUuid, Vec<rmw_request_id_t>>>,
+    board: Mutex<ActionServerGoalBoard<A>>,
+
     /// Ensure the parent node remains alive as long as the subscription is held.
-    /// This implementation will change in the future.
     #[allow(unused)]
     node: Node,
 }
 
+pub struct ActionServerGoalBoard<A: ActionImpl> {
+    /// These goals have a live handle held by the user. We refer to them with a
+    /// Weak to prevent a circular reference. When the user drops the live handle
+    /// it will automatically be moved into the dropped_goals map.
+    live_goals: HashMap<GoalUuid, Weak<LiveActionServerGoalHandle<A>>>,
+    /// These goals have been dropped by the user.
+    dropped_goals: HashMap<GoalUuid, DroppedActionServerGoalHandle<A>>,
+}
+
 impl<T> ActionServerState<T>
 where
     T: ActionImpl,
@@ -107,9 +72,6 @@ where
     pub(crate) fn create<'a>(
         node: &Node,
         options: impl Into<ActionServerOptions<'a>>,
-        goal_callback: impl Fn(GoalUuid, T::Goal) -> GoalResponse + 'static + Send + Sync,
-        cancel_callback: impl Fn(Arc<ServerGoalHandle<T>>) -> CancelResponse + 'static + Send + Sync,
-        accepted_callback: impl Fn(Arc<ServerGoalHandle<T>>) + 'static + Send + Sync,
     ) -> Result<Self, RclrsError> {
         let options = options.into();
         // SAFETY: Getting a zero-initialized value is always safe.
@@ -182,6 +144,10 @@ where
         })
     }
 
+    pub fn node(&self) -> &Node {
+        &self.node
+    }
+
     fn take_goal_request(&self) -> Result<(<<T::SendGoalService as Service>::Request as Message>::RmwMsg, rmw_request_id_t), RclrsError> {
         let mut request_id = rmw_request_id_t {
             writer_guid: [0; 16],
@@ -284,7 +250,7 @@ where
                 // Other than rcl_get_error_string(), there's no indication what happened.
                 panic!("Failed to accept goal");
             } else {
-                Arc::new(ServerGoalHandle::<T>::new(
+                Arc::new(ActionServerGoalHandle::<T>::new(
                     goal_handle_ptr,
                     Arc::downgrade(&self),
                     todo!("Create an Arc holding the goal message"),
@@ -301,7 +267,7 @@ where
             .insert(uuid, Arc::clone(&goal_handle));
 
         if response == GoalResponse::AcceptAndExecute {
-            goal_handle.execute()?;
+            goal_handle.transition_to_execute()?;
         }
 
         self.publish_status()?;
@@ -595,30 +561,44 @@ where
         Ok(())
     }
 
-    // TODO(nwn): Replace `status` with a "properly typed" action_msgs::msg::GoalStatus enum.
-    pub(crate) fn terminate_goal(&self, goal_id: &GoalUuid, status: i8, result: <T::Result as Message>::RmwMsg) -> Result<(), RclrsError> {
-        let response_rmw = <T as ActionImpl>::create_result_response(status, result);
-
-        // Publish the result to anyone listening.
-        self.publish_result(goal_id, response_rmw);
-
-        // Publish the state change.
-        self.publish_status();
-
-        // Notify rcl that a goal has terminated and to therefore recalculate the expired goal timer.
+    pub(crate) fn publish_feedback(&self, goal_id: &GoalUuid, feedback: &<T as rosidl_runtime_rs::Action>::Feedback) -> Result<(), RclrsError> {
+        let feedback_rmw = <<T as rosidl_runtime_rs::Action>::Feedback as Message>::into_rmw_message(std::borrow::Cow::Borrowed(feedback));
+        let mut feedback_msg = <T as rosidl_runtime_rs::ActionImpl>::create_feedback_message(&goal_id.0, feedback_rmw.into_owned());
         unsafe {
-            // SAFETY: The action server is locked and valid. No other preconditions.
-            rcl_action_notify_goal_done(&*self.handle.lock())
+            // SAFETY: The action server is locked through the handle, meaning that no other
+            // non-thread-safe functions can be called on it at the same time. The feedback_msg is
+            // exclusively owned here, ensuring that it won't be modified during the call.
+            // rcl_action_publish_feedback() guarantees that it won't modify `feedback_msg`.
+            rcl_action_publish_feedback(
+                &*self.handle.lock(),
+                &mut feedback_msg as *mut _ as *mut std::ffi::c_void,
+            )
         }
-        .ok()?;
+        .ok()
+    }
+}
 
-        // Release ownership of the goal handle. It will persist until the user also drops it.
-        self.goal_handles.lock().unwrap().remove(&goal_id);
+// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
+// they are running in. Therefore, this type can be safely sent to another thread.
+unsafe impl Send for rcl_action_server_t {}
 
-        Ok(())
+/// Manage the lifecycle of an `rcl_action_server_t`, including managing its dependencies
+/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are
+/// [dropped after][1] the `rcl_action_server_t`.
+///
+/// [1]: <https://doc.rust-lang.org/reference/destructors.html>
+pub(crate) struct ActionServerHandle {
+    rcl_action_server: Mutex<rcl_action_server_t>,
+    /// Ensure the node remains active while the action server is running
+    node_handle: Arc<NodeHandle>,
+}
+
+impl ActionServerHandle {
+    pub(super) fn lock(&self) -> MutexGuard<rcl_action_server_t> {
+        self.rcl_action_server.lock().unwrap()
     }
 
-    pub(crate) fn publish_status(&self) -> Result<(), RclrsError> {
+    pub(super) fn publish_status(&self) -> Result<(), RclrsError> {
         let mut goal_statuses = DropGuard::new(
             unsafe {
                 // SAFETY: No preconditions
@@ -631,10 +611,11 @@ where
             },
         );
 
+        let rcl_handle = self.lock();
         unsafe {
             // SAFETY: The action server is locked through the handle and goal_statuses is
             // zero-initialized.
-            rcl_action_get_goal_status_array(&*self.handle.lock(), &mut *goal_statuses)
+            rcl_action_get_goal_status_array(&*rcl_handle, &mut *goal_statuses)
         }
         .ok()?;
 
@@ -642,59 +623,12 @@ where
             // SAFETY: The action server is locked through the handle and goal_statuses.msg is a
             // valid `action_msgs__msg__GoalStatusArray` by construction.
             rcl_action_publish_status(
-                &*self.handle.lock(),
+                &*rcl_handle,
                 &goal_statuses.msg as *const _ as *const std::ffi::c_void,
             )
         }
         .ok()
     }
-
-    pub(crate) fn publish_feedback(&self, goal_id: &GoalUuid, feedback: &<T as rosidl_runtime_rs::Action>::Feedback) -> Result<(), RclrsError> {
-        let feedback_rmw = <<T as rosidl_runtime_rs::Action>::Feedback as Message>::into_rmw_message(std::borrow::Cow::Borrowed(feedback));
-        let mut feedback_msg = <T as rosidl_runtime_rs::ActionImpl>::create_feedback_message(&goal_id.0, feedback_rmw.into_owned());
-        unsafe {
-            // SAFETY: The action server is locked through the handle, meaning that no other
-            // non-thread-safe functions can be called on it at the same time. The feedback_msg is
-            // exclusively owned here, ensuring that it won't be modified during the call.
-            // rcl_action_publish_feedback() guarantees that it won't modify `feedback_msg`.
-            rcl_action_publish_feedback(
-                &*self.handle.lock(),
-                &mut feedback_msg as *mut _ as *mut std::ffi::c_void,
-            )
-        }
-        .ok()
-    }
-
-    fn publish_result(&self, goal_id: &GoalUuid, mut result: <<<T as ActionImpl>::GetResultService as Service>::Response as Message>::RmwMsg) -> Result<(), RclrsError> {
-        let goal_exists = unsafe {
-            // SAFETY: No preconditions
-            let mut goal_info = rcl_action_get_zero_initialized_goal_info();
-            goal_info.goal_id.uuid = goal_id.0;
-
-            // SAFETY: The action server is locked through the handle. The `goal_info`
-            // argument points to a rcl_action_goal_info_t with the desired UUID.
-            rcl_action_server_goal_exists(&*self.handle.lock(), &goal_info)
-        };
-        if !goal_exists {
-            panic!("Cannot publish result for unknown goal")
-        }
-
-        // TODO(nwn): Fix synchronization problem between goal_results and result_requests.
-        // Currently, there is a gap between the request queue being drained and the result being
-        // stored for future requests. Any requests received during that gap would never receive a
-        // response. Fixing this means we'll need combined locking over these two hash maps.
-
-        // Respond to all queued requests.
-        if let Some(result_requests) = self.result_requests.lock().unwrap().remove(&goal_id) {
-            for mut result_request in result_requests {
-                self.send_result_response(result_request, &mut result)?;
-            }
-        }
-
-        self.goal_results.lock().unwrap().insert(*goal_id, result);
-
-        Ok(())
-    }
 }
 
 /// `ActionServerOptions` are used by [`Node::create_action_server`][1] to initialize an
diff --git a/rclrs/src/action/action_server_goal_handle.rs b/rclrs/src/action/action_server_goal_handle.rs
new file mode 100644
index 00000000..44fced0b
--- /dev/null
+++ b/rclrs/src/action/action_server_goal_handle.rs
@@ -0,0 +1,391 @@
+use crate::{
+    rcl_bindings::*,
+    log_error,
+    GoalUuid, RclrsError, ToResult, ActionServer, ActionServerHandle,
+};
+use std::{
+    borrow::Cow,
+    sync::{Arc, Mutex},
+    hash::Hash,
+    ops::Deref,
+};
+use rosidl_runtime_rs::{ActionImpl, Message, Service};
+use tokio::sync::watch::{Sender, Receiver, channel as watch_channel};
+
+/// Values defined by `action_msgs/msg/GoalStatus`
+#[repr(i8)]
+#[derive(Debug, Clone, Hash, PartialEq, Eq)]
+enum GoalStatus {
+    Unknown = 0,
+    Accepted = 1,
+    Executing = 2,
+    Cancelling = 3,
+    Succeeded = 4,
+    Cancelled = 5,
+    Aborted = 6,
+}
+
+/// Possible status values for terminal states
+#[repr(i8)]
+#[derive(Debug, Clone, Hash, PartialEq, Eq)]
+enum TerminalStatus {
+    Succeeded = 4,
+    Cancelled = 5,
+    Aborted = 6,
+}
+
+/// From rcl documentation for rcl_action_accept_new_goal:
+///
+/// If a failure occurs, `NULL` is returned and an error message is set.
+/// Possible reasons for failure:
+///   - action server is invalid
+///   - goal info is invalid
+///   - goal ID is already being tracked by the action server
+///   - memory allocation failure
+///
+/// We have no way of diagnosing which of these errors caused the failure, so
+/// all we can do is indicate that an error occurred with accepting the goal.
+#[derive(Debug, Clone)]
+pub struct GoalAcceptanceError;
+
+/// A handle for an action goal that has been requested but not accepted yet.
+pub(super) struct RequestedActionServerGoalHandle<A: ActionImpl> {
+    server: ActionServer<A>,
+    goal_request: Arc<A::Goal>,
+    uuid: GoalUuid,
+}
+
+impl<A: ActionImpl> RequestedActionServerGoalHandle<A> {
+    pub(super) fn transition_to_accepted(self) -> Result<ActionServerGoalHandle<A>, GoalAcceptanceError> {
+        let goal_handle = {
+            let mut goal_info = unsafe {
+                // SAFETY: Zero-initialized rcl structs are always safe to make
+                rcl_action_get_zero_initialized_goal_info()
+            };
+            goal_info.goal_id.uuid = *self.uuid;
+            goal_info.stamp = self.server.node().get_clock().now().to_rcl().unwrap_or_default();
+
+            let mut server_handle = self.server.handle.lock();
+            let goal_handle_ptr = unsafe {
+                // SAFETY: The action server handle is locked and so synchronized with other
+                // functions. The request_id and response message are uniquely owned, and so will
+                // not mutate during this function call. The returned goal handle pointer should be
+                // valid unless it is null.
+                rcl_action_accept_new_goal(&mut *server_handle, &goal_info)
+            };
+
+            if goal_handle_ptr.is_null() {
+                return Err(GoalAcceptanceError);
+            }
+
+            let goal_handle = unsafe {
+                // SAFETY: We receive a loose pointer since the function call is
+                // fallible, but the loose pointer we receive is actually managed
+                // internally by rcl_action, so we must not retain its pointer
+                // value. Instead we need to copy its internal impl* into a new
+                // struct which we will take ownership of.
+                //
+                // It remains our responsibility to call `rcl_action_goal_handle_fini`
+                // on the copy of the `rcl_action_goal_handle_t` because rcl_action
+                // will not manage the memory of the inner impl*.
+                *goal_handle_ptr
+            };
+
+            goal_handle
+        };
+
+        Ok(Arc::new(ActionServerGoalHandle {
+            rcl_handle: Mutex::new(goal_handle),
+            goal_request: self.goal_request,
+            uuid: self.uuid,
+            server: self.server.handle,
+            result_response: Default::default(),
+            cancellation: Default::default(),
+        }))
+    }
+}
+
+/// This struct is a minimal bridge from an rclrs action server goal to the rcl
+/// API.
+pub(super) struct ActionServerGoalHandle<A: ActionImpl> {
+    rcl_handle: Mutex<rcl_action_goal_handle_t>,
+    goal_request: Arc<A::Goal>,
+    uuid: GoalUuid,
+    result_response: ResponseState<A>,
+    server: ActionServerHandle,
+    cancellation: CancellationState,
+}
+
+// SAFETY: The functions accessing this type don't care about the thread they are
+// running in. Therefore this type can be safely sent to another thread.
+unsafe impl Send for rcl_action_goal_handle_t {}
+
+impl<A: ActionImpl> ActionServerGoalHandle<A> {
+    /// Returns the goal state.
+    fn get_status(&self) -> Result<GoalStatus, RclrsError> {
+        let mut state = GoalStatus::Unknown as rcl_action_goal_state_t;
+        {
+            let rcl_handle = self.rcl_handle.lock().unwrap();
+            // SAFETY: The provided goal handle is properly initialized by construction.
+            unsafe { rcl_action_goal_handle_get_status(*rcl_handle, &mut state).ok()? }
+        }
+        // SAFETY: state is initialized to a valid GoalStatus value and will only ever by set by
+        // rcl_action_goal_handle_get_status to a valid GoalStatus value.
+        Ok(unsafe { std::mem::transmute(state) })
+    }
+
+    /// Returns whether the client has requested that this goal be cancelled.
+    pub(super) fn is_cancelling(&self) -> bool {
+        self.get_status().is_ok_and(|s| s == GoalStatus::Cancelling)
+    }
+
+    /// Returns true if the goal is either pending or executing, or false if it has reached a
+    /// terminal state.
+    pub(super) fn is_active(&self) -> bool {
+        let rcl_handle = self.rcl_handle.lock().unwrap();
+        // SAFETY: The provided goal handle is properly initialized by construction.
+        unsafe { rcl_action_goal_handle_is_active(*rcl_handle) }
+    }
+
+    /// Returns whether the goal is executing.
+    pub(super) fn is_executing(&self) -> bool {
+        self.get_status().is_ok_and(|s| s == GoalStatus::Executing)
+    }
+
+    /// Get the unique identifier of the goal.
+    pub(super) fn goal_id(&self) -> GoalUuid {
+        self.uuid
+    }
+
+    /// Get the user-provided message describing the goal.
+    pub(super) fn goal(&self) -> Arc<A::Goal> {
+        Arc::clone(&self.goal_request)
+    }
+
+    /// Indicate that the goal is being cancelled.
+    ///
+    /// This is called when a cancel request for the goal has been accepted.
+    ///
+    /// Returns an error if the goal is in any state other than accepted or executing.
+    pub(super) fn transition_to_cancelling(&self) -> Result<(), RclrsError> {
+        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCEL_GOAL)
+    }
+
+    /// Indicate that the goal could not be reached and has been aborted.
+    ///
+    /// Only call this if the goal is executing but cannot be completed. This is a terminal state,
+    /// so no more methods may be called on a goal handle after this is called.
+    ///
+    /// Returns an error if the goal is in any state other than executing.
+    pub(super) fn transition_to_abort(&self, result: &A::Result) -> Result<(), RclrsError> {
+        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_ABORT)?;
+        let result_rmw = <A::Result as Message>::into_rmw_message(Cow::Borrowed(result)).into_owned();
+        self.terminate_goal(&self.uuid, 6, result_rmw)?;
+        Ok(())
+    }
+
+    /// Indicate that the goal has succeeded.
+    ///
+    /// Only call this if the goal is executing and has reached the desired final state. This is a
+    /// terminal state, so no more methods may be called on a goal handle after this is called.
+    ///
+    /// Returns an error if the goal is in any state other than executing.
+    pub(super) fn transition_to_succeed(&self, result: &A::Result) -> Result<(), RclrsError> {
+        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_SUCCEED)?;
+        let result_rmw = <A::Result as Message>::into_rmw_message(Cow::Borrowed(result)).into_owned();
+        self.terminate_goal(&self.uuid, TerminalStatus::Succeeded, result_rmw)?;
+        Ok(())
+    }
+
+    /// Indicate that the goal has been cancelled.
+    ///
+    /// Only call this if the goal is executing or pending, but has been cancelled. This is a
+    /// terminal state, so no more methods may be called on a goal handle after this is called.
+    ///
+    /// Returns an error if the goal is in any state other than executing or pending.
+    pub(super) fn transition_to_cancelled(&self, result: &A::Result) -> Result<(), RclrsError> {
+        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCELED)?;
+        let result_rmw = <A::Result as Message>::into_rmw_message(Cow::Borrowed(result)).into_owned();
+        self.terminate_goal(&self.uuid, TerminalStatus::Cancelled, result_rmw)?;
+        Ok(())
+    }
+
+    /// Indicate that the server is starting to execute the goal.
+    ///
+    /// Only call this if the goal is pending. This is a terminal state, so no more methods may be
+    /// called on a goal handle after this is called.
+    ///
+    /// Returns an error if the goal is in any state other than pending.
+    pub(super) fn transition_to_execute(&self) -> Result<(), RclrsError> {
+        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_EXECUTE)?;
+
+        // Publish the state change.
+        if let Some(action_server) = self.action_server.upgrade() {
+            action_server.publish_status()?;
+        }
+        Ok(())
+    }
+
+    /// Send an update about the goal's progress.
+    ///
+    /// This may only be called when the goal is executing.
+    ///
+    /// Returns an error if the goal is in any state other than executing.
+    pub(super) fn publish_feedback(&self, feedback: &A::Feedback) -> Result<(), RclrsError> {
+        // If the action server no longer exists, simply drop the message.
+        if let Some(action_server) = self.action_server.upgrade() {
+            action_server.publish_feedback(&self.uuid, feedback)?;
+        }
+        Ok(())
+    }
+
+    fn terminate_goal(
+        &self,
+        goal_id: &GoalUuid,
+        status: TerminalStatus,
+        result: <A::Result as Message>::RmwMsg,
+    ) -> Result<(), RclrsError> {
+        let response_rmw = <A as ActionImpl>::create_result_response(status as i8, result);
+
+        // Publish the result to anyone listening.
+        self.publish_result(goal_id, response_rmw);
+
+        // Publish the state change.
+        self.publish_status();
+
+        // Notify rcl that a goal has terminated and to therefore recalculate the expired goal timer.
+        unsafe {
+            // SAFETY: The action server is locked and valid. No other preconditions.
+            rcl_action_notify_goal_done(&*self.handle.lock())
+        }
+        .ok()?;
+
+        // Release ownership of the goal handle. It will persist until the user also drops it.
+        self.goal_handles.lock().unwrap().remove(&goal_id);
+
+        Ok(())
+    }
+
+
+    /// Attempt to perform the given goal state transition.
+    fn update_state(&self, event: rcl_action_goal_event_t) -> Result<(), RclrsError> {
+        let mut rcl_handle = self.rcl_handle.lock().unwrap();
+        // SAFETY: The provided goal handle is properly initialized by construction.
+        unsafe { rcl_action_update_goal_state(*rcl_handle, event).ok() }
+    }
+}
+
+pub(super) type ActionResponseRmw<A> = <<<A as ActionImpl>::GetResultService as Service>::Response as Message>::RmwMsg;
+
+/// Manages the state of a goal's response.
+pub(super) enum ResponseState<A: ActionImpl> {
+    /// The response has not arrived yet. There may be some clients waiting for
+    /// the response, and they'll be listed here.
+    Waiting(Vec<rmw_request_id_t>),
+    /// The response is available.
+    Available(ActionResponseRmw<A>),
+}
+
+impl<A: ActionImpl> ResponseState<A> {
+    fn new() -> Self {
+        Self::Waiting(Vec::new())
+    }
+
+    fn provide_result(
+        &mut self,
+        action_server_handle: &ActionServerHandle,
+        goal_id: &GoalUuid,
+        mut result: ActionResponseRmw<A>,
+    ) -> Result<(), RclrsError> {
+        let result_requests = match self {
+            Self::Waiting(waiting) => waiting,
+            Self::Available(previous) => {
+                log_error!(
+                    "action_server_goal_handle.provide_result",
+                    "Action goal {goal_id} was provided with multiple results, \
+                    which is not allowed by the action server state machine and \
+                    indicates a bug in rclrs. The new result will be discarded.\
+                    \nPrevious result: {previous:?}\
+                    \nNew result: {result:?}"
+                );
+            }
+        };
+
+        if !result_requests.is_empty() {
+            let action_server = action_server_handle.lock();
+
+            // Respond to all queued requests.
+            for mut result_request in result_requests {
+                Self::send_result(action_server, result_request, &mut result)?;
+            }
+        }
+
+        *self = Self::Available(result);
+    }
+
+    fn add_result_request(
+        &mut self,
+        action_server_handle: &ActionServerHandle,
+        goal_id: &GoalUuid,
+        result_request: rmw_request_id_t,
+    ) -> Result<(), RclrsError> {
+        match self {
+            Self::Waiting(waiting) => {
+                waiting.push(result_request);
+            }
+            Self::Available(result) => {
+                let action_server = action_server_handle.lock();
+                Self::send_result(action_server, &mut result_request, result)?;
+            }
+        }
+        Ok(())
+    }
+
+    fn send_result(
+        action_server: &rmw_request_id_t,
+        result_request: &mut rmw_request_id_t,
+        result_response: &mut ActionResponseRmw<A>,
+    ) -> Result<(), RclrsError> {
+        unsafe {
+            // SAFETY: The action server handle is kept valid by the
+            // ActionServerHandle. The compiler ensures we have unique access
+            // to the result_request and result_response structures.
+            rcl_action_send_result_response(
+                action_server,
+                &mut result_request,
+                result_response as *mut _ as *mut _,
+            )
+            .ok()
+        }
+    }
+}
+
+pub(super) struct CancellationState {
+    receiver: Receiver<CancellationMode>,
+    sender: Sender<CancellationMode>,
+}
+
+impl Default for CancellationState {
+    fn default() -> Self {
+        let (sender, receiver) = watch_channel(CancellationMode::None);
+        Self { receiver, sender }
+    }
+}
+
+pub(super) enum CancellationMode {
+    None,
+    CancelRequested(Vec<rmw_request_id_t>),
+    Cancelling,
+}
+
+impl<A: ActionImpl> Drop for ActionServerGoalHandle<A> {
+    fn drop(&mut self) {
+        // SAFETY: There should not be any way for the mutex to be poisoned
+        let mut rcl_handle = self.rcl_handle.lock().unwrap();
+        unsafe {
+            // SAFETY: The goal handle was propertly initialized, and it will
+            // never be accessed again after this.
+            rcl_action_goal_handle_fini(&mut *rcl_handle);
+        }
+    }
+}
diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs
deleted file mode 100644
index 3602b3a0..00000000
--- a/rclrs/src/action/server_goal_handle.rs
+++ /dev/null
@@ -1,242 +0,0 @@
-use crate::{action::ActionServerState, rcl_bindings::*, GoalUuid, RclrsError, ToResult};
-use std::sync::{Arc, Mutex, Weak};
-use rosidl_runtime_rs::Action;
-
-// Values defined by `action_msgs/msg/GoalStatus`
-#[repr(i8)]
-#[derive(Debug, Clone, Hash, PartialEq, Eq)]
-enum GoalStatus {
-    Unknown = 0,
-    Accepted = 1,
-    Executing = 2,
-    Canceling = 3,
-    Succeeded = 4,
-    Canceled = 5,
-    Aborted = 6,
-}
-
-/// Handle to interact with goals on a server.
-///
-/// Use this to check the status of a goal and to set its result.
-///
-/// This type will only be created by an [`ActionServer`] when a goal is accepted and will be
-/// passed to the user in the associated `handle_accepted` callback.
-pub struct ServerGoalHandle<A>
-where
-    A: Action,
-{
-    rcl_handle: Mutex<*mut rcl_action_goal_handle_t>,
-    action_server: Weak<ActionServerState<A>>,
-    goal_request: Arc<A::Goal>,
-    uuid: GoalUuid,
-}
-
-// SAFETY: Send/Sync are not automatically implemented due to the contained raw pointer
-// (specifically, `*mut rcl_action_goal_handle_t`). However, the `rcl_handle` field is wrapped in a
-// mutex, guaranteeing that the underlying data is never simultaneously accessed on the rclrs side
-// by multiple threads. Moreover, the rcl_action functions taking these handles are able to be run
-// from any thread.
-unsafe impl<ActionT> Send for ServerGoalHandle<ActionT> where ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl {}
-unsafe impl<ActionT> Sync for ServerGoalHandle<ActionT> where ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl {}
-
-impl<ActionT> ServerGoalHandle<ActionT>
-where
-    ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
-{
-    pub(crate) fn new(
-        rcl_handle: *mut rcl_action_goal_handle_t,
-        action_server: Weak<ActionServerState<ActionT>>,
-        goal_request: Arc<ActionT::Goal>,
-        uuid: GoalUuid,
-    ) -> Self {
-        Self {
-            rcl_handle: Mutex::new(rcl_handle),
-            action_server,
-            goal_request: Arc::clone(&goal_request),
-            uuid,
-        }
-    }
-
-    /// Returns the goal state.
-    fn get_state(&self) -> Result<GoalStatus, RclrsError> {
-        let mut state = GoalStatus::Unknown as rcl_action_goal_state_t;
-        {
-            let rcl_handle = self.rcl_handle.lock().unwrap();
-            // SAFETY: The provided goal handle is properly initialized by construction.
-            unsafe { rcl_action_goal_handle_get_status(*rcl_handle, &mut state).ok()? }
-        }
-        // SAFETY: state is initialized to a valid GoalStatus value and will only ever by set by
-        // rcl_action_goal_handle_get_status to a valid GoalStatus value.
-        Ok(unsafe { std::mem::transmute(state) })
-    }
-
-    /// Returns whether the client has requested that this goal be cancelled.
-    pub fn is_canceling(&self) -> bool {
-        self.get_state().unwrap() == GoalStatus::Canceling
-    }
-
-    /// Returns true if the goal is either pending or executing, or false if it has reached a
-    /// terminal state.
-    pub fn is_active(&self) -> bool {
-        let rcl_handle = self.rcl_handle.lock().unwrap();
-        // SAFETY: The provided goal handle is properly initialized by construction.
-        unsafe { rcl_action_goal_handle_is_active(*rcl_handle) }
-    }
-
-    /// Returns whether the goal is executing.
-    pub fn is_executing(&self) -> bool {
-        self.get_state().unwrap() == GoalStatus::Executing
-    }
-
-    /// Attempt to perform the given goal state transition.
-    fn update_state(&self, event: rcl_action_goal_event_t) -> Result<(), RclrsError> {
-        let mut rcl_handle = self.rcl_handle.lock().unwrap();
-        // SAFETY: The provided goal handle is properly initialized by construction.
-        unsafe { rcl_action_update_goal_state(*rcl_handle, event).ok() }
-    }
-
-    /// Indicate that the goal is being cancelled.
-    ///
-    /// This is called when a cancel request for the goal has been accepted.
-    ///
-    /// Returns an error if the goal is in any state other than accepted or executing.
-    pub(crate) fn cancel(&self) -> Result<(), RclrsError> {
-        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCEL_GOAL)
-    }
-
-    /// Indicate that the goal could not be reached and has been aborted.
-    ///
-    /// Only call this if the goal is executing but cannot be completed. This is a terminal state,
-    /// so no more methods may be called on a goal handle after this is called.
-    ///
-    /// Returns an error if the goal is in any state other than executing.
-    pub fn abort(&self, result: &ActionT::Result) -> Result<(), RclrsError> {
-        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_ABORT)?;
-
-        if let Some(action_server) = self.action_server.upgrade() {
-            let result_rmw = <ActionT::Result as rosidl_runtime_rs::Message>::into_rmw_message(std::borrow::Cow::Borrowed(result)).into_owned();
-            // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_ABORTED in the rcl
-            // bindings.
-            action_server.terminate_goal(&self.uuid, 6, result_rmw)?;
-        }
-        Ok(())
-    }
-
-    /// Indicate that the goal has succeeded.
-    ///
-    /// Only call this if the goal is executing and has reached the desired final state. This is a
-    /// terminal state, so no more methods may be called on a goal handle after this is called.
-    ///
-    /// Returns an error if the goal is in any state other than executing.
-    pub fn succeed(&self, result: &ActionT::Result) -> Result<(), RclrsError> {
-        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_SUCCEED)?;
-
-        if let Some(action_server) = self.action_server.upgrade() {
-            let result_rmw = <ActionT::Result as rosidl_runtime_rs::Message>::into_rmw_message(std::borrow::Cow::Borrowed(result)).into_owned();
-            // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_SUCCEEDED in the rcl
-            // bindings.
-            action_server.terminate_goal(&self.uuid, 4, result_rmw)?;
-        }
-        Ok(())
-    }
-
-    /// Indicate that the goal has been cancelled.
-    ///
-    /// Only call this if the goal is executing or pending, but has been cancelled. This is a
-    /// terminal state, so no more methods may be called on a goal handle after this is called.
-    ///
-    /// Returns an error if the goal is in any state other than executing or pending.
-    pub fn canceled(&self, result: &ActionT::Result) -> Result<(), RclrsError> {
-        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCELED)?;
-
-        if let Some(action_server) = self.action_server.upgrade() {
-            let result_rmw = <ActionT::Result as rosidl_runtime_rs::Message>::into_rmw_message(std::borrow::Cow::Borrowed(result)).into_owned();
-            // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_CANCELED in the rcl
-            // bindings.
-            action_server.terminate_goal(&self.uuid, 5, result_rmw)?;
-        }
-        Ok(())
-    }
-
-    /// Indicate that the server is starting to execute the goal.
-    ///
-    /// Only call this if the goal is pending. This is a terminal state, so no more methods may be
-    /// called on a goal handle after this is called.
-    ///
-    /// Returns an error if the goal is in any state other than pending.
-    pub fn execute(&self) -> Result<(), RclrsError> {
-        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_EXECUTE)?;
-
-        // Publish the state change.
-        if let Some(action_server) = self.action_server.upgrade() {
-            action_server.publish_status()?;
-        }
-        Ok(())
-    }
-
-    /// Try canceling the goal if possible.
-    fn try_canceling(&mut self) -> Result<bool, RclrsError> {
-        let rcl_handle = self.rcl_handle.lock().unwrap();
-
-        // If the goal is in a cancelable state, transition to canceling.
-        // SAFETY: The provided goal handle is properly initialized by construction.
-        let is_cancelable = unsafe { rcl_action_goal_handle_is_cancelable(*rcl_handle) };
-        if is_cancelable {
-            self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCEL_GOAL)?;
-        }
-
-        // If the goal is canceling, transition to canceled.
-        if self.get_state()? == GoalStatus::Canceling {
-            self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCELED)?;
-            Ok(true)
-        } else {
-            Ok(false)
-        }
-    }
-
-    /// Get the unique identifier of the goal.
-    pub fn goal_id(&self) -> GoalUuid {
-        self.uuid
-    }
-
-    /// Get the user-provided message describing the goal.
-    pub fn goal(&self) -> Arc<ActionT::Goal> {
-        Arc::clone(&self.goal_request)
-    }
-
-    /// Send an update about the goal's progress.
-    ///
-    /// This may only be called when the goal is executing.
-    ///
-    /// Returns an error if the goal is in any state other than executing.
-    pub fn publish_feedback(&self, feedback: &ActionT::Feedback) -> Result<(), RclrsError> {
-        // If the action server no longer exists, simply drop the message.
-        if let Some(action_server) = self.action_server.upgrade() {
-            action_server.publish_feedback(&self.uuid, feedback)?;
-        }
-        Ok(())
-    }
-}
-
-impl<ActionT> Drop for ServerGoalHandle<ActionT>
-where
-    ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
-{
-    /// Cancel the goal if its handle is dropped without reaching a terminal state.
-    fn drop(&mut self) {
-        if self.try_canceling() == Ok(true) {
-            if let Some(action_server) = self.action_server.upgrade() {
-                let response_rmw = <ActionT::Result as rosidl_runtime_rs::Message>::RmwMsg::default();
-                // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_CANCELED in the rcl
-                // bindings.
-                action_server.terminate_goal(&self.uuid, 5, response_rmw);
-            }
-        }
-        {
-            let rcl_handle = self.rcl_handle.lock().unwrap();
-            // SAFETY: The provided goal handle is properly initialized by construction. It will
-            // not be accessed beyond this point.
-            unsafe { rcl_action_goal_handle_fini(*rcl_handle); }
-        }
-    }
-}
diff --git a/rclrs/src/action/server_goal_state.rs b/rclrs/src/action/server_goal_state.rs
deleted file mode 100644
index 06db4a79..00000000
--- a/rclrs/src/action/server_goal_state.rs
+++ /dev/null
@@ -1,34 +0,0 @@
-use std::sync::Arc;
-use tokio::sync::watch::{Sender, Receiver};
-use rosidl_runtime_rs::ActionImpl;
-
-use crate::{
-    ActionServerState, GoalUuid,
-};
-
-struct ServerGoalState<A: ActionImpl> {
-    cancellation: Arc<CancellationState>,
-    server_state: Arc<ActionServerState<A>>,
-}
-
-struct CancellationState<A: ActionImpl> {
-    receiver: Receiver<CancellationMode<A>>,
-    sender: Sender<CancellationMode<A>>,
-}
-
-enum CancellationMode<A: ActionImpl> {
-    None,
-    CancelRequested(CancellationResponder<A>),
-    Cancelling,
-}
-
-struct CancellationResponder<A: ActionImpl> {
-    uuid: GoalUuid,
-    handle: Arc<ActionServerState<A>>,
-}
-
-impl CancellationResponder {
-    fn send_cancel_response(&self) {
-
-    }
-}
diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs
index 540c6248..67132624 100644
--- a/rclrs/src/time.rs
+++ b/rclrs/src/time.rs
@@ -29,13 +29,23 @@ impl Time {
 
     /// Convenience function for converting time to ROS message
     pub fn to_ros_msg(&self) -> Result<builtin_interfaces::msg::Time, TryFromIntError> {
-        let nanosec = self.nsec % 1_000_000_000;
+        let (sec, nanosec) = self.to_sec_nanosec()?;
+        Ok(builtin_interfaces::msg::Time { nanosec, sec })
+    }
+
+    pub fn to_rcl(&self) -> Result<builtin_interfaces__msg__Time, TryFromIntError> {
+        let (sec, nanosec) = self.to_sec_nanosec()?;
+        Ok(builtin_interfaces__msg__Time { sec, nanosec })
+    }
+
+    pub fn to_sec_nanosec(&self) -> Result<(i32, u32), TryFromIntError> {
         let sec = self.nsec / 1_000_000_000;
+        let nanosec = self.nsec % 1_000_000_000;
 
-        Ok(builtin_interfaces::msg::Time {
-            nanosec: nanosec.try_into()?,
-            sec: sec.try_into()?,
-        })
+        Ok((
+            sec.try_into()?,
+            nanosec.try_into()?,
+        ))
     }
 }
 
diff --git a/rclrs/src/wait_set/guard_condition.rs b/rclrs/src/wait_set/guard_condition.rs
index 86a7c645..3cd9ec63 100644
--- a/rclrs/src/wait_set/guard_condition.rs
+++ b/rclrs/src/wait_set/guard_condition.rs
@@ -5,7 +5,7 @@ use std::{
 
 use crate::{
     rcl_bindings::*, ContextHandle, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError,
-    ToResult, Waitable, WaitableLifecycle,
+    ReadyKind, ToResult, Waitable, WaitableLifecycle,
 };
 
 /// A waitable entity used for waking up a wait set manually.
@@ -206,7 +206,8 @@ struct GuardConditionExecutable {
 }
 
 impl RclPrimitive for GuardConditionExecutable {
-    unsafe fn execute(&mut self, _: &mut dyn Any) -> Result<(), RclrsError> {
+    unsafe fn execute(&mut self, ready: ReadyKind, _: &mut dyn Any) -> Result<(), RclrsError> {
+        ready.is_basic()?;
         if let Some(callback) = &mut self.callback {
             callback();
         }

From a61b5f03de3c80b8a1c0a457fb2a8205c1626229 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Sun, 27 Jul 2025 18:30:47 +0800
Subject: [PATCH 05/20] Fleshing out the state machines for action server goals
 -- cancelling is WIP

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/src/action.rs                           |   5 +-
 rclrs/src/action/action_server.rs             |  70 ++-
 .../src/action/action_server/accepted_goal.rs |  44 ++
 .../action_server_goal_handle.rs              |  96 ++++
 .../action/action_server/cancelling_goal.rs   |   8 +
 .../action/action_server/executing_goal.rs    |  13 +
 .../action_server/live_action_server_goal.rs  | 505 ++++++++++++++++++
 .../action/action_server/requested_goal.rs    | 127 +++++
 rclrs/src/action/action_server_goal_handle.rs | 391 --------------
 rclrs/src/drop_guard.rs                       |  16 +-
 rclrs/src/error.rs                            |  22 +-
 11 files changed, 871 insertions(+), 426 deletions(-)
 create mode 100644 rclrs/src/action/action_server/accepted_goal.rs
 create mode 100644 rclrs/src/action/action_server/action_server_goal_handle.rs
 create mode 100644 rclrs/src/action/action_server/cancelling_goal.rs
 create mode 100644 rclrs/src/action/action_server/executing_goal.rs
 create mode 100644 rclrs/src/action/action_server/live_action_server_goal.rs
 create mode 100644 rclrs/src/action/action_server/requested_goal.rs
 delete mode 100644 rclrs/src/action/action_server_goal_handle.rs

diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs
index b3b7b060..f5c1fe98 100644
--- a/rclrs/src/action.rs
+++ b/rclrs/src/action.rs
@@ -3,15 +3,13 @@ use std::ops::Deref;
 pub(crate) mod action_client;
 pub(crate) mod action_server;
 mod action_server_goal_handle;
-mod action_server_goal_state;
 
 use crate::rcl_bindings::RCL_ACTION_UUID_SIZE;
 use std::fmt;
 
 pub use action_client::*;
 pub use action_server::*;
-use action_server_goal_handle::{LiveActionServerGoalHandle, DroppedActionServerGoalHandle};
-use action_server_goal_state::*;
+use action_server_goal_handle::*;
 
 /// A unique identifier for a goal request.
 #[derive(Copy, Clone, Debug, Default, PartialEq, Eq, PartialOrd, Ord, Hash)]
@@ -61,6 +59,7 @@ pub enum GoalResponse {
 
 /// The response returned by an [`ActionServer`]'s cancel callback when a goal is requested to be cancelled.
 #[derive(PartialEq, Eq)]
+#[repr(i8)]
 pub enum CancelResponse {
     /// The server will not try to cancel the goal.
     Reject = 1,
diff --git a/rclrs/src/action/action_server.rs b/rclrs/src/action/action_server.rs
index f42e941e..69eafc82 100644
--- a/rclrs/src/action/action_server.rs
+++ b/rclrs/src/action/action_server.rs
@@ -1,10 +1,10 @@
 use crate::{
-    action::{CancelResponse, GoalResponse, GoalUuid, ActionServerGoalHandle},
+    action::{CancelResponse, GoalResponse, GoalUuid},
     error::{RclReturnCode, ToResult},
     rcl_bindings::*,
     DropGuard, Node, NodeHandle, QoSProfile, RclrsError, ENTITY_LIFECYCLE_MUTEX,
 };
-use rosidl_runtime_rs::{Action, ActionImpl, Message, Service};
+use rosidl_runtime_rs::{ActionImpl, Message, Service};
 use std::{
     borrow::Borrow,
     collections::HashMap,
@@ -12,6 +12,24 @@ use std::{
     sync::{Arc, Mutex, MutexGuard, Weak},
 };
 
+mod accepted_goal;
+pub use accepted_goal::*;
+
+mod action_server_goal_handle;
+use action_server_goal_handle::*;
+
+mod cancelling_goal;
+use cancelling_goal::*;
+
+mod executing_goal;
+pub use executing_goal::*;
+
+mod live_action_server_goal;
+use live_action_server_goal::*;
+
+mod requested_goal;
+pub use requested_goal::*;
+
 pub(crate) enum ReadyMode {
     GoalRequest,
     CancelRequest,
@@ -45,8 +63,8 @@ pub type ActionServer<A> = Arc<ActionServerState<A>>;
 /// The public API of the [`ActionServer`] type is implemented via `ActionServerState`.
 ///
 /// [1]: std::sync::Weak
-pub struct ActionServerState<A: ActionImpl> {
-    pub(crate) handle: Arc<ActionServerHandle>,
+struct ActionServerState<A: ActionImpl> {
+    handle: Arc<ActionServerHandle>,
 
     board: Mutex<ActionServerGoalBoard<A>>,
 
@@ -59,9 +77,7 @@ pub struct ActionServerGoalBoard<A: ActionImpl> {
     /// These goals have a live handle held by the user. We refer to them with a
     /// Weak to prevent a circular reference. When the user drops the live handle
     /// it will automatically be moved into the dropped_goals map.
-    live_goals: HashMap<GoalUuid, Weak<LiveActionServerGoalHandle<A>>>,
-    /// These goals have been dropped by the user.
-    dropped_goals: HashMap<GoalUuid, DroppedActionServerGoalHandle<A>>,
+    live_goals: HashMap<GoalUuid, Weak<LiveActionServerGoal<A>>>,
 }
 
 impl<T> ActionServerState<T>
@@ -561,21 +577,6 @@ where
         Ok(())
     }
 
-    pub(crate) fn publish_feedback(&self, goal_id: &GoalUuid, feedback: &<T as rosidl_runtime_rs::Action>::Feedback) -> Result<(), RclrsError> {
-        let feedback_rmw = <<T as rosidl_runtime_rs::Action>::Feedback as Message>::into_rmw_message(std::borrow::Cow::Borrowed(feedback));
-        let mut feedback_msg = <T as rosidl_runtime_rs::ActionImpl>::create_feedback_message(&goal_id.0, feedback_rmw.into_owned());
-        unsafe {
-            // SAFETY: The action server is locked through the handle, meaning that no other
-            // non-thread-safe functions can be called on it at the same time. The feedback_msg is
-            // exclusively owned here, ensuring that it won't be modified during the call.
-            // rcl_action_publish_feedback() guarantees that it won't modify `feedback_msg`.
-            rcl_action_publish_feedback(
-                &*self.handle.lock(),
-                &mut feedback_msg as *mut _ as *mut std::ffi::c_void,
-            )
-        }
-        .ok()
-    }
 }
 
 // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
@@ -591,6 +592,9 @@ pub(crate) struct ActionServerHandle {
     rcl_action_server: Mutex<rcl_action_server_t>,
     /// Ensure the node remains active while the action server is running
     node_handle: Arc<NodeHandle>,
+    /// Ensure the `impl_*` of the action server goals remain valid until they
+    /// have expired or until the rcl_action_server_t gets fini-ed.
+    pub(super) goals: Mutex<HashMap<GoalUuid, Arc<ActionServerGoalHandle>>>,
 }
 
 impl ActionServerHandle {
@@ -672,3 +676,25 @@ impl<'a, T: Borrow<str> + ?Sized + 'a> From<&'a T> for ActionServerOptions<'a> {
         Self::new(value.borrow())
     }
 }
+
+/// Values defined by `action_msgs/msg/GoalStatus`
+#[repr(i8)]
+#[derive(Debug, Clone, Hash, PartialEq, Eq)]
+enum GoalStatus {
+    Unknown = 0,
+    Accepted = 1,
+    Executing = 2,
+    Cancelling = 3,
+    Succeeded = 4,
+    Cancelled = 5,
+    Aborted = 6,
+}
+
+/// Possible status values for terminal states
+#[repr(i8)]
+#[derive(Debug, Clone, Hash, PartialEq, Eq)]
+enum TerminalStatus {
+    Succeeded = 4,
+    Cancelled = 5,
+    Aborted = 6,
+}
diff --git a/rclrs/src/action/action_server/accepted_goal.rs b/rclrs/src/action/action_server/accepted_goal.rs
new file mode 100644
index 00000000..fcfda858
--- /dev/null
+++ b/rclrs/src/action/action_server/accepted_goal.rs
@@ -0,0 +1,44 @@
+use crate::log_error;
+use super::{CancellingGoal, ExecutingGoal, LiveActionServerGoal};
+use std::sync::Arc;
+use rosidl_runtime_rs::ActionImpl;
+
+pub struct AcceptedGoal<A: ActionImpl> {
+    live: Arc<LiveActionServerGoal<A>>,
+}
+
+impl<A: ActionImpl> AcceptedGoal<A> {
+    /// Transition the goal into the executing state. This will allow you to
+    /// start sending feedback and transition to other states such as cancelling,
+    /// succeedded, and aborted.
+    ///
+    /// You can use this even if an action client has requested a cancellation,
+    /// but the cancellation request will be ignored until you handle it with
+    /// [`ExecutingGoal`].
+    pub fn execute(self) -> ExecutingGoal<A> {
+        if let Err(err) = self.live.transition_to_execute() {
+            log_error!(
+                "AcceptedGoal.execute",
+                "Failed to transition to executing status. This error should \
+                not happen, please report it to the maintainers of rclrs: {err}",
+            );
+        }
+
+        ExecutingGoal::new(self.live)
+    }
+
+    /// Transition the goal into either the executing mode or the cancelling mode
+    /// depending on whether an action client has sent a cancellation request.
+    pub fn begin(self) -> BeginAcceptedGoal<A> {
+
+    }
+
+    pub(super) fn new(live: Arc<LiveActionServerGoal<A>>) -> Self {
+        Self { live }
+    }
+}
+
+pub enum BeginAcceptedGoal<A: ActionImpl> {
+    Execute(ExecutingGoal<A>),
+    Cancel(CancellingGoal<A>),
+}
diff --git a/rclrs/src/action/action_server/action_server_goal_handle.rs b/rclrs/src/action/action_server/action_server_goal_handle.rs
new file mode 100644
index 00000000..64f1e119
--- /dev/null
+++ b/rclrs/src/action/action_server/action_server_goal_handle.rs
@@ -0,0 +1,96 @@
+use crate::{
+    rcl_bindings::*,
+    log_error,
+    GoalUuid, RclrsError, ToResult,
+};
+use super::{GoalStatus};
+use std::sync::{Mutex, MutexGuard};
+
+/// This struct is a minimal bridge to the `rcl_action` API for action server goals.
+/// While the goal is still live, it will be managed by a [`LiveActionServerGoal`][1]
+/// struct. Once the [`LiveActionServerGoal`][1] is dropped, it will remain inside
+/// the [`ActionServerHandle`][2] until the goal is reported as expired by `rcl_action`.
+///
+/// This will be created by [`RequestedActionServerGoal`][3]
+///
+/// [1]: super::LiveActionServerGoal
+/// [2]: super::ActionServerHandle
+/// [3]: super::RequestedGoal::accept
+pub(super) struct ActionServerGoalHandle {
+    rcl_handle: Mutex<rcl_action_goal_handle_t>,
+    uuid: GoalUuid,
+}
+
+impl ActionServerGoalHandle {
+    pub(super) fn new(
+        rcl_handle: rcl_action_goal_handle_s,
+        uuid: GoalUuid,
+    ) -> Self {
+        Self {
+            rcl_handle: Mutex::new(rcl_handle),
+            uuid,
+        }
+    }
+
+    pub(super) fn lock(&self) -> MutexGuard<rcl_action_goal_handle_t> {
+        self.rcl_handle.lock().unwrap()
+    }
+
+    /// Returns the goal state.
+    pub(super) fn get_status(&self) -> GoalStatus {
+        let mut state = GoalStatus::Unknown as rcl_action_goal_state_t;
+        {
+            let rcl_handle = self.rcl_handle.lock().unwrap();
+            // SAFETY: The provided goal handle is properly initialized by construction.
+            let r = unsafe { rcl_action_goal_handle_get_status(&*rcl_handle, &mut state).ok() };
+            if let Err(err) = r {
+                log_error!(
+                    "ActionServerGoalHandle.get_status",
+                    "Unexpected error while getting status: {err}",
+                );
+            }
+        }
+        // SAFETY: state is initialized to a valid GoalStatus value and will only ever by set by
+        // rcl_action_goal_handle_get_status to a valid GoalStatus value.
+        unsafe { std::mem::transmute(state) }
+    }
+
+    /// Returns whether the client has requested that this goal be cancelled.
+    pub(super) fn is_cancelling(&self) -> bool {
+        self.get_status() == GoalStatus::Cancelling
+    }
+
+    /// Returns true if the goal is either pending or executing, or false if it has reached a
+    /// terminal state.
+    pub(super) fn is_active(&self) -> bool {
+        let rcl_handle = self.rcl_handle.lock().unwrap();
+        // SAFETY: The provided goal handle is properly initialized by construction.
+        unsafe { rcl_action_goal_handle_is_active(&*rcl_handle) }
+    }
+
+    /// Returns whether the goal is executing.
+    pub(super) fn is_executing(&self) -> bool {
+        self.get_status() == GoalStatus::Executing
+    }
+
+    /// Get the unique identifier of the goal.
+    pub(super) fn goal_id(&self) -> &GoalUuid {
+        &self.uuid
+    }
+}
+
+impl Drop for ActionServerGoalHandle {
+    fn drop(&mut self) {
+        // SAFETY: There should not be any way for the mutex to be poisoned
+        let mut rcl_handle = self.rcl_handle.lock().unwrap();
+        unsafe {
+            // SAFETY: The goal handle was propertly initialized, and it will
+            // never be accessed again after this.
+            rcl_action_goal_handle_fini(&mut *rcl_handle);
+        }
+    }
+}
+
+// SAFETY: The functions accessing this type don't care about the thread they are
+// running in. Therefore this type can be safely sent to another thread.
+unsafe impl Send for rcl_action_goal_handle_t {}
diff --git a/rclrs/src/action/action_server/cancelling_goal.rs b/rclrs/src/action/action_server/cancelling_goal.rs
new file mode 100644
index 00000000..c4c33efe
--- /dev/null
+++ b/rclrs/src/action/action_server/cancelling_goal.rs
@@ -0,0 +1,8 @@
+use super::LiveActionServerGoal;
+use std::sync::Arc;
+use rosidl_runtime_rs::ActionImpl;
+
+pub struct CancellingGoal<A: ActionImpl> {
+    live: Arc<LiveActionServerGoal<A>>,
+}
+
diff --git a/rclrs/src/action/action_server/executing_goal.rs b/rclrs/src/action/action_server/executing_goal.rs
new file mode 100644
index 00000000..136d5382
--- /dev/null
+++ b/rclrs/src/action/action_server/executing_goal.rs
@@ -0,0 +1,13 @@
+use super::LiveActionServerGoal;
+use std::sync::Arc;
+use rosidl_runtime_rs::ActionImpl;
+
+pub struct ExecutingGoal<A: ActionImpl> {
+    live: Arc<LiveActionServerGoal<A>>,
+}
+
+impl<A: ActionImpl> ExecutingGoal<A> {
+    pub(super) fn new(live: Arc<LiveActionServerGoal<A>>) -> Self {
+        Self { live }
+    }
+}
diff --git a/rclrs/src/action/action_server/live_action_server_goal.rs b/rclrs/src/action/action_server/live_action_server_goal.rs
new file mode 100644
index 00000000..fea6b3b9
--- /dev/null
+++ b/rclrs/src/action/action_server/live_action_server_goal.rs
@@ -0,0 +1,505 @@
+use crate::{
+    rcl_bindings::*,
+    vendor::{
+        builtin_interfaces::msg::Time,
+        action_msgs::{
+            msg::GoalInfo,
+            srv::CancelGoal_Response,
+        },
+        unique_identifier_msgs::msg::UUID,
+    },
+    log_error,
+    CancelResponse, GoalUuid, RclrsError, RclErrorMsg, RclReturnCode, ToResult, Node,
+};
+use super::{
+    ActionServerGoalHandle, ActionServerHandle, TerminalStatus, GoalStatus,
+};
+use std::{
+    borrow::Cow,
+    collections::HashSet,
+    sync::{Arc, Mutex},
+    ops::Deref,
+};
+use rosidl_runtime_rs::{Action, ActionImpl, Message, Service};
+use tokio::sync::watch::{Sender, Receiver, channel as watch_channel};
+
+
+/// This struct is the bridge to the rcl_action API for action server goals that
+/// are still active. It can be used to perform transitions while keeping data in
+/// sync between the rclrs user and the rcl_action library.
+///
+/// When this is dropped, the goal will automatically be transitioned into the
+/// aborted status if the user did not transition the goal into a different
+/// terminal status before dropping it.
+pub(super) struct LiveActionServerGoal<A: ActionImpl> {
+    goal_request: Arc<A::Goal>,
+    result_response: Mutex<ResponseState<A>>,
+    cancellation: CancellationState,
+    handle: Arc<ActionServerGoalHandle>,
+    server: Arc<ActionServerHandle>,
+}
+
+impl<A: ActionImpl> LiveActionServerGoal<A> {
+    pub(super) fn new(
+        handle: Arc<ActionServerGoalHandle>,
+        server: Arc<ActionServerHandle>,
+        goal_request: Arc<A::Goal>,
+    ) -> Self {
+        Self {
+            handle,
+            server,
+            goal_request,
+            result_response: Mutex::new(ResponseState::new()),
+            cancellation: Default::default(),
+        }
+    }
+
+    /// Get the user-provided message describing the goal.
+    pub(super) fn goal(&self) -> Arc<A::Goal> {
+        Arc::clone(&self.goal_request)
+    }
+
+    /// Has a cancellation been requested for this goal.
+    pub(super) fn cancel_requested(&self) -> bool {
+        self.cancellation.receiver.borrow().cancel_requested()
+    }
+
+    /// Indicate that the goal is being cancelled.
+    ///
+    /// This is called when a cancel request for the goal has been accepted.
+    ///
+    /// Returns an error if the goal is in any state other than accepted or executing.
+    pub(super) fn transition_to_cancelling(&self) -> Result<(), RclrsError> {
+        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCEL_GOAL)
+    }
+
+    /// Indicate that the goal could not be reached and has been aborted.
+    ///
+    /// Only call this if the goal is executing but cannot be completed. This is a terminal state,
+    /// so no more methods may be called on a goal handle after this is called.
+    ///
+    /// Returns an error if the goal is in any state other than executing.
+    pub(super) fn transition_to_abort(&self, result: &A::Result) -> Result<(), RclrsError> {
+        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_ABORT)?;
+        self.terminate_goal(TerminalStatus::Aborted, result)?;
+        Ok(())
+    }
+
+    /// Indicate that the goal has succeeded.
+    ///
+    /// Only call this if the goal is executing and has reached the desired final state. This is a
+    /// terminal state, so no more methods may be called on a goal handle after this is called.
+    ///
+    /// Returns an error if the goal is in any state other than executing.
+    pub(super) fn transition_to_succeed(&self, result: &A::Result) -> Result<(), RclrsError> {
+        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_SUCCEED)?;
+        self.terminate_goal(TerminalStatus::Succeeded, result)?;
+        Ok(())
+    }
+
+    /// Indicate that the goal has been cancelled.
+    ///
+    /// Only call this if the goal is executing or pending, but has been cancelled. This is a
+    /// terminal state, so no more methods may be called on a goal handle after this is called.
+    ///
+    /// Returns an error if the goal is in any state other than executing or pending.
+    pub(super) fn transition_to_cancelled(&self, result: &A::Result) -> Result<(), RclrsError> {
+        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCELED)?;
+        self.terminate_goal(TerminalStatus::Cancelled, result)?;
+        Ok(())
+    }
+
+    /// Indicate that the server is starting to execute the goal.
+    ///
+    /// Only call this if the goal is pending. This is a terminal state, so no more methods may be
+    /// called on a goal handle after this is called.
+    ///
+    /// Returns an error if the goal is in any state other than pending.
+    pub(super) fn transition_to_execute(&self) -> Result<(), RclrsError> {
+        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_EXECUTE)?;
+
+        // Publish the state change.
+        self.server.publish_status()
+    }
+
+    /// Send an update about the goal's progress.
+    ///
+    /// This may only be called when the goal is executing.
+    ///
+    /// Returns an error if the goal is in any state other than executing.
+    pub(super) fn publish_feedback(&self, feedback: &A::Feedback) -> Result<(), RclrsError> {
+        let feedback_rmw = <<A as Action>::Feedback as Message>::into_rmw_message(Cow::Borrowed(feedback));
+        let mut feedback_msg = <A as ActionImpl>::create_feedback_message(&*self.goal_id(), feedback_rmw.into_owned());
+        unsafe {
+            // SAFETY: The action server is locked through the handle, meaning that no other
+            // non-thread-safe functions can be called on it at the same time. The feedback_msg is
+            // exclusively owned here, ensuring that it won't be modified during the call.
+            // rcl_action_publish_feedback() guarantees that it won't modify `feedback_msg`.
+            rcl_action_publish_feedback(
+                &*self.server.lock(),
+                &mut feedback_msg as *mut _ as *mut _,
+            )
+            .ok()
+        }
+    }
+
+    fn terminate_goal(
+        &self,
+        status: TerminalStatus,
+        result: &A::Result,
+    ) -> Result<(), RclrsError> {
+        let result_rmw = <A::Result as Message>::into_rmw_message(Cow::Borrowed(result)).into_owned();
+        let response_rmw = <A as ActionImpl>::create_result_response(status as i8, result_rmw);
+
+        // Publish the result to anyone listening.
+        self.result_response.lock().unwrap().provide_result(&self.server, self.goal_id(), response_rmw);
+
+        // Publish the state change.
+        self.server.publish_status();
+
+        // Notify rcl that a goal has terminated and to therefore recalculate the expired goal timer.
+        unsafe {
+            // SAFETY: The action server is locked and valid. No other preconditions.
+            rcl_action_notify_goal_done(&*self.server.lock())
+        }
+        .ok()?;
+
+        Ok(())
+    }
+
+    /// Attempt to perform the given goal state transition.
+    fn update_state(&self, event: rcl_action_goal_event_t) -> Result<(), RclrsError> {
+        let mut rcl_handle = self.handle.lock();
+        // SAFETY: The provided goal handle is properly initialized by construction.
+        unsafe { rcl_action_update_goal_state(&mut *rcl_handle, event).ok() }
+    }
+}
+
+impl<A: ActionImpl> Deref for LiveActionServerGoal<A> {
+    type Target = ActionServerGoalHandle;
+
+    fn deref(&self) -> &Self::Target {
+        self.handle.as_ref()
+    }
+}
+
+impl<A: ActionImpl> Drop for LiveActionServerGoal<A> {
+    fn drop(&mut self) {
+        match self.get_status() {
+            GoalStatus::Accepted => {
+                // Transition into executing and then into aborted to reach a
+                // terminal state.
+                self.transition_to_execute();
+                self.transition_to_abort(&Default::default());
+            }
+            GoalStatus::Cancelling | GoalStatus::Executing => {
+                self.transition_to_abort(&Default::default());
+            }
+            GoalStatus::Succeeded | GoalStatus::Cancelled | GoalStatus::Aborted => {
+                // Already in a terminal state, no need to do anything.
+            }
+            GoalStatus::Unknown => {
+                log_error!(
+                    "LiveActionServerGoal.drop",
+                    "Goal status is unknown. This indicates a bug, please \
+                    report this to the maintainers of rclrs."
+                );
+            }
+        }
+    }
+}
+
+pub(super) type ActionResponseRmw<A> = <<<A as ActionImpl>::GetResultService as Service>::Response as Message>::RmwMsg;
+
+/// Manages the state of a goal's response.
+enum ResponseState<A: ActionImpl> {
+    /// The response has not arrived yet. There may be some clients waiting for
+    /// the response, and they'll be listed here.
+    Waiting(Vec<rmw_request_id_t>),
+    /// The response is available.
+    Available(ActionResponseRmw<A>),
+}
+
+impl<A: ActionImpl> ResponseState<A> {
+    fn new() -> Self {
+        Self::Waiting(Vec::new())
+    }
+
+    fn provide_result(
+        &mut self,
+        action_server_handle: &ActionServerHandle,
+        goal_id: &GoalUuid,
+        mut result: ActionResponseRmw<A>,
+    ) -> Result<(), RclrsError> {
+        let result_requests = match self {
+            Self::Waiting(waiting) => waiting,
+            Self::Available(previous) => {
+                log_error!(
+                    "action_server_goal_handle.provide_result",
+                    "Action goal {goal_id} was provided with multiple results, \
+                    which is not allowed by the action server state machine and \
+                    indicates a bug in rclrs. The new result will be discarded.\
+                    \nPrevious result: {previous:?}\
+                    \nNew result: {result:?}"
+                );
+                return Err(RclrsError::RclError {
+                    code: RclReturnCode::ActionGoalEventInvalid,
+                    msg: Some(RclErrorMsg("action goal response is already set".to_string())),
+                });
+            }
+        };
+
+        if !result_requests.is_empty() {
+            let action_server = action_server_handle.lock();
+
+            // Respond to all queued requests.
+            for mut result_request in result_requests {
+                Self::send_result(&*action_server, &mut result_request, &mut result)?;
+            }
+        }
+
+        *self = Self::Available(result);
+        Ok(())
+    }
+
+    fn add_result_request(
+        &mut self,
+        action_server_handle: &ActionServerHandle,
+        mut result_request: rmw_request_id_t,
+    ) -> Result<(), RclrsError> {
+        match self {
+            Self::Waiting(waiting) => {
+                waiting.push(result_request);
+            }
+            Self::Available(result) => {
+                let action_server = action_server_handle.lock();
+                Self::send_result(&*action_server, &mut result_request, result)?;
+            }
+        }
+        Ok(())
+    }
+
+    fn send_result(
+        action_server: &rcl_action_server_t,
+        result_request: &mut rmw_request_id_t,
+        result_response: &mut ActionResponseRmw<A>,
+    ) -> Result<(), RclrsError> {
+        unsafe {
+            // SAFETY: The action server handle is kept valid by the
+            // ActionServerHandle. The compiler ensures we have unique access
+            // to the result_request and result_response structures.
+            rcl_action_send_result_response(
+                action_server,
+                result_request,
+                result_response as *mut _ as *mut _,
+            )
+            .ok()
+        }
+    }
+}
+
+pub(super) struct CancellationState {
+    receiver: Receiver<bool>,
+    sender: Sender<bool>,
+
+    /// We put a mutex on the mode because when we respond to cancellation
+    /// requests we need to ensure that we update the cancellation mode
+    /// atomically
+    mode: Mutex<CancellationMode>,
+}
+
+impl CancellationState {
+    /// Check if a cancellation is currently being requested.
+    fn cancel_requested(&self) -> bool {
+        *self.receiver.borrow()
+    }
+
+    fn request_cancellation(
+        &self,
+        request: CancellationRequest,
+        uuid: &GoalUuid,
+    ) {
+        let mut mode = self.mode.lock().unwrap();
+        match &mut *mode {
+            CancellationMode::None => {
+                let requests = Vec::from_iter([request]);
+                *mode = CancellationMode::CancelRequested(requests);
+            }
+            CancellationMode::CancelRequested(requests) => {
+                requests.push(request);
+            }
+            CancellationMode::Cancelling => {
+                request.accept(*uuid);
+            }
+        }
+    }
+
+    /// Tell current cancellation requesters that their requests are rejected
+    fn reject_cancellation(&self, uuid: &GoalUuid) {
+        let mut mode = self.mode.lock().unwrap();
+        match &mut *mode {
+            CancellationMode::CancelRequested(requesters) => {
+                for requester in requesters.drain(..) {
+                    requester.reject(*uuid);
+                }
+
+                // Revert to not having any cancellation mode
+                *mode = CancellationMode::None;
+                self.sender.send(false);
+            }
+            CancellationMode::None => {
+                // Do nothing
+            }
+            CancellationMode::Cancelling => {
+                // Do nothing. We will not revert a cancellation state.
+            }
+        }
+    }
+
+    /// Tell current and future cancellation requesters that their requests are
+    /// accepted
+    fn accept_cancellation(&self, uuid: &GoalUuid) {
+        let mut mode = self.mode.lock().unwrap();
+        match &mut *mode {
+            CancellationMode::CancelRequested(requesters) => {
+                for requester in requesters.drain(..) {
+                    requester.accept(*uuid);
+                }
+
+                // Progress to cancelling mode
+                *mode = CancellationMode::Cancelling;
+                // Just in case this signal was never sent, make sure we have
+                // a true value in the cancel requested channel.
+                self.sender.send(true);
+            }
+            CancellationMode::None => {
+                // Skip straight to cancellation mode since the user has accepted
+                // a cancellation even though it wasn't requested externally.
+                *mode = CancellationMode::Cancelling;
+                // Make sure the cancellation is signalled.
+                self.sender.send(true);
+            }
+            CancellationMode::Cancelling => {
+                // Do nothing
+            }
+        }
+    }
+
+}
+
+impl Default for CancellationState {
+    fn default() -> Self {
+        let (sender, receiver) = watch_channel(CancellationMode::None);
+        Self { receiver, sender }
+    }
+}
+
+pub(super) enum CancellationMode {
+    None,
+    CancelRequested(Vec<CancellationRequest>),
+    Cancelling,
+}
+
+/// This struct exists to deal with the fact that a single cancellation request
+/// can trigger multiple goal cancellations at once. This allows us to
+/// asynchronously receive the accept/reject results from all the different goals
+/// and then issue the reply once all are received.
+struct CancellationRequest {
+    inner: Arc<Mutex<CancellationRequestInner>>,
+}
+
+impl CancellationRequest {
+    fn accept(&self, uuid: GoalUuid) {
+        let mut inner = self.inner.lock().unwrap();
+        if !inner.received.insert(uuid) {
+            return;
+        }
+
+        let stamp = inner.node.get_clock().now().to_ros_msg().unwrap_or_default();
+        let info = GoalInfo {
+            goal_id: UUID { uuid: *uuid },
+            stamp,
+        };
+
+        inner.accepted.push(info);
+        inner.respond_if_ready();
+    }
+
+    fn reject(&self, uuid: GoalUuid) {
+        let mut inner = self.inner.lock().unwrap();
+        if !inner.received.insert(uuid) {
+            return;
+        }
+
+        inner.respond_if_ready();
+    }
+
+}
+
+struct CancellationRequestInner {
+    id: rmw_request_id_t,
+    waiting_for: Vec<GoalUuid>,
+    received: HashSet<GoalUuid>,
+    accepted: Vec<GoalInfo>,
+    response_sent: bool,
+    server: Arc<ActionServerHandle>,
+    node: Node,
+}
+
+impl CancellationRequestInner {
+    fn respond_if_ready(&mut self) {
+        for expected in &self.waiting_for {
+            if !self.received.contains(expected) {
+                return;
+            }
+        }
+
+        self.respond();
+    }
+
+    fn respond(&mut self) {
+        if self.response_sent {
+            return;
+        }
+
+        self.response_sent = true;
+
+        let mut response = CancelGoal_Response::default();
+        response.goals_canceling = self.accepted.drain(..).collect();
+        if response.goals_canceling.is_empty() {
+            response.return_code = CancelResponse::Reject as i8;
+        } else {
+            response.return_code = CancelResponse::Accept as i8;
+        }
+
+        let mut response_rmw = CancelGoal_Response::into_rmw_message(Cow::Owned(response)).into_owned();
+        let r = unsafe {
+            // SAFETY: The action server handle is locked and so synchronized with other functions.
+            // The request_id and response are both uniquely owned or borrowed, and so neither will
+            // mutate during this function call.
+            rcl_action_send_cancel_response(
+                &*self.server.lock(),
+                &mut self.id,
+                &mut response_rmw as *mut _ as *mut _,
+            )
+            .ok()
+        };
+
+        if let Err(err) = r {
+            log_error!(
+                "CancellationRequest.respond",
+                "Error occurred while responding to a cancellation request: {err}"
+            )
+        }
+    }
+}
+
+impl Drop for CancellationRequestInner {
+    fn drop(&mut self) {
+        if !self.response_sent {
+            // As a last resort, send the response if all possible responders
+            // have dropped.
+            self.respond();
+        }
+    }
+}
diff --git a/rclrs/src/action/action_server/requested_goal.rs b/rclrs/src/action/action_server/requested_goal.rs
new file mode 100644
index 00000000..77d81610
--- /dev/null
+++ b/rclrs/src/action/action_server/requested_goal.rs
@@ -0,0 +1,127 @@
+use crate::{
+    rcl_bindings::*,
+    log_error,
+    GoalUuid, RclrsError, ToResult, ActionServer,
+};
+use super::{ActionServerGoalHandle, LiveActionServerGoal, AcceptedGoal};
+use std::sync::Arc;
+use rosidl_runtime_rs::ActionImpl;
+
+#[derive(Debug, Clone)]
+pub struct GoalAcceptanceError;
+
+/// An action goal that has been requested but not accepted yet. If this is
+/// dropped without being accepted then the goal request will be rejected.
+pub struct RequestedGoal<A: ActionImpl> {
+    server: ActionServer<A>,
+    goal_request: Arc<A::Goal>,
+    uuid: GoalUuid,
+    goal_request_id: rmw_request_id_t,
+    accepted: bool,
+}
+
+impl<A: ActionImpl> RequestedGoal<A> {
+    /// Accept the requested goal. The action client will be notified that the
+    /// goal was accepted, and you will be able to begin executing.
+    pub fn accept(mut self) -> Result<AcceptedGoal<A>, RclrsError> {
+        let handle = {
+            let mut goal_info = unsafe {
+                // SAFETY: Zero-initialized rcl structs are always safe to make
+                rcl_action_get_zero_initialized_goal_info()
+            };
+            goal_info.goal_id.uuid = *self.uuid;
+            goal_info.stamp = self.server.node().get_clock().now().to_rcl().unwrap_or(
+                builtin_interfaces__msg__Time { sec: 0, nanosec: 0 }
+            );
+
+            let mut server_handle = self.server.handle.lock();
+            let goal_handle_ptr = unsafe {
+                // SAFETY: The action server handle is locked and so synchronized with other
+                // functions. The request_id and response message are uniquely owned, and so will
+                // not mutate during this function call. The returned goal handle pointer should be
+                // valid unless it is null.
+                rcl_action_accept_new_goal(&mut *server_handle, &goal_info)
+            };
+
+            if goal_handle_ptr.is_null() {
+                return Err(RclrsError::GoalAcceptanceError);
+            }
+
+            let mut goal_handle = rcl_action_goal_handle_t {
+                impl_: std::ptr::null_mut(),
+            };
+
+            unsafe {
+                // SAFETY: We receive a loose pointer since the function call is
+                // fallible, but the loose pointer we receive is actually managed
+                // internally by rcl_action, so we must not retain its pointer
+                // value. Instead we need to copy its internal impl* into a new
+                // struct which we will take ownership of.
+                //
+                // It remains our responsibility to call `rcl_action_goal_handle_fini`
+                // on the copy of the `rcl_action_goal_handle_t` because rcl_action
+                // will not manage the memory of the inner impl*.
+                goal_handle.impl_ = (*goal_handle_ptr).impl_;
+            };
+
+            Arc::new(ActionServerGoalHandle::new(goal_handle, self.uuid))
+        };
+
+        // We need to store a strong reference to the goal handle in the action
+        // server handle to ensure the goal handle outlives its use within the
+        // action server.
+        self.server.handle.goals.lock().unwrap().insert(*handle.goal_id(), Arc::clone(&handle));
+
+        self.accepted = true;
+        self.send_goal_response();
+
+        let live = Arc::new(LiveActionServerGoal::new(
+            handle,
+            Arc::clone(&self.server.handle),
+            Arc::clone(&self.goal_request),
+        ));
+
+        // Add the live goal to the goal board so the action server executor can
+        // interact with it.
+        self.server.board.lock().unwrap().live_goals.insert(self.uuid, Arc::downgrade(&live));
+
+        Ok(AcceptedGoal::new(live))
+    }
+
+    /// Reject this goal request.
+    ///
+    /// This is the same as simply allowing the [`RequestedGoal`] to drop without
+    /// accepting.
+    pub fn reject(self) {
+        // This function need to do anything, it will simply drop the RequestedGoal,
+        // and the custom Drop implementation will notify the action server that
+        // the goal is rejected.
+    }
+
+    fn send_goal_response(&mut self) -> Result<(), RclrsError> {
+        let stamp = self.server.node().get_clock().now().to_sec_nanosec().unwrap_or((0, 0));
+        let mut response_rmw = <A as ActionImpl>::create_goal_response(self.accepted, stamp);
+        unsafe {
+            rcl_action_send_goal_response(
+                &*self.server.handle.lock(),
+                &mut self.goal_request_id,
+                &mut response_rmw as *mut _ as *mut _,
+            )
+        }
+        .ok()
+    }
+}
+
+impl<A: ActionImpl> Drop for RequestedGoal<A> {
+    fn drop(&mut self) {
+        if !self.accepted {
+            // We should notify that the goal has been rejected.
+            if let Err(err) = self.send_goal_response() {
+                log_error!(
+                    "RequestedGoal.drop",
+                    "Error occurred while attempting to reject a goal: {err}",
+                );
+            }
+        }
+    }
+}
diff --git a/rclrs/src/action/action_server_goal_handle.rs b/rclrs/src/action/action_server_goal_handle.rs
deleted file mode 100644
index 44fced0b..00000000
--- a/rclrs/src/action/action_server_goal_handle.rs
+++ /dev/null
@@ -1,391 +0,0 @@
-use crate::{
-    rcl_bindings::*,
-    log_error,
-    GoalUuid, RclrsError, ToResult, ActionServer, ActionServerHandle,
-};
-use std::{
-    borrow::Cow,
-    sync::{Arc, Mutex},
-    hash::Hash,
-    ops::Deref,
-};
-use rosidl_runtime_rs::{ActionImpl, Message, Service};
-use tokio::sync::watch::{Sender, Receiver, channel as watch_channel};
-
-/// Values defined by `action_msgs/msg/GoalStatus`
-#[repr(i8)]
-#[derive(Debug, Clone, Hash, PartialEq, Eq)]
-enum GoalStatus {
-    Unknown = 0,
-    Accepted = 1,
-    Executing = 2,
-    Cancelling = 3,
-    Succeeded = 4,
-    Cancelled = 5,
-    Aborted = 6,
-}
-
-/// Possible status values for terminal states
-#[repr(i8)]
-#[derive(Debug, Clone, Hash, PartialEq, Eq)]
-enum TerminalStatus {
-    Succeeded = 4,
-    Cancelled = 5,
-    Aborted = 6,
-}
-
-/// From rcl documentation for rcl_action_accept_new_goal:
-///
-/// If a failure occurs, `NULL` is returned and an error message is set.
-/// Possible reasons for failure:
-///   - action server is invalid
-///   - goal info is invalid
-///   - goal ID is already being tracked by the action server
-///   - memory allocation failure
-///
-/// We have no way of diagnosing which of these errors caused the failure, so
-/// all we can do is indicate that an error occurred with accepting the goal.
-#[derive(Debug, Clone)]
-pub struct GoalAcceptanceError;
-
-/// A handle for an action goal that has been requested but not accepted yet.
-pub(super) struct RequestedActionServerGoalHandle<A: ActionImpl> {
-    server: ActionServer<A>,
-    goal_request: Arc<A::Goal>,
-    uuid: GoalUuid,
-}
-
-impl<A: ActionImpl> RequestedActionServerGoalHandle<A> {
-    pub(super) fn transition_to_accepted(self) -> Result<ActionServerGoalHandle<A>, GoalAcceptanceError> {
-        let goal_handle = {
-            let mut goal_info = unsafe {
-                // SAFETY: Zero-initialized rcl structs are always safe to make
-                rcl_action_get_zero_initialized_goal_info()
-            };
-            goal_info.goal_id.uuid = *self.uuid;
-            goal_info.stamp = self.server.node().get_clock().now().to_rcl().unwrap_or_default();
-
-            let mut server_handle = self.server.handle.lock();
-            let goal_handle_ptr = unsafe {
-                // SAFETY: The action server handle is locked and so synchronized with other
-                // functions. The request_id and response message are uniquely owned, and so will
-                // not mutate during this function call. The returned goal handle pointer should be
-                // valid unless it is null.
-                rcl_action_accept_new_goal(&mut *server_handle, &goal_info)
-            };
-
-            if goal_handle_ptr.is_null() {
-                return Err(GoalAcceptanceError);
-            }
-
-            let goal_handle = unsafe {
-                // SAFETY: We receive a loose pointer since the function call is
-                // fallible, but the loose pointer we receive is actually managed
-                // internally by rcl_action, so we must not retain its pointer
-                // value. Instead we need to copy its internal impl* into a new
-                // struct which we will take ownership of.
-                //
-                // It remains our responsibility to call `rcl_action_goal_handle_fini`
-                // on the copy of the `rcl_action_goal_handle_t` because rcl_action
-                // will not manage the memory of the inner impl*.
-                *goal_handle_ptr
-            };
-
-            goal_handle
-        };
-
-        Ok(Arc::new(ActionServerGoalHandle {
-            rcl_handle: Mutex::new(goal_handle),
-            goal_request: self.goal_request,
-            uuid: self.uuid,
-            server: self.server.handle,
-            result_response: Default::default(),
-            cancellation: Default::default(),
-        }))
-    }
-}
-
-/// This struct is a minimal bridge from an rclrs action server goal to the rcl
-/// API.
-pub(super) struct ActionServerGoalHandle<A: ActionImpl> {
-    rcl_handle: Mutex<rcl_action_goal_handle_t>,
-    goal_request: Arc<A::Goal>,
-    uuid: GoalUuid,
-    result_response: ResponseState<A>,
-    server: ActionServerHandle,
-    cancellation: CancellationState,
-}
-
-// SAFETY: The functions accessing this type don't care about the thread they are
-// running in. Therefore this type can be safely sent to another thread.
-unsafe impl Send for rcl_action_goal_handle_t {}
-
-impl<A: ActionImpl> ActionServerGoalHandle<A> {
-    /// Returns the goal state.
-    fn get_status(&self) -> Result<GoalStatus, RclrsError> {
-        let mut state = GoalStatus::Unknown as rcl_action_goal_state_t;
-        {
-            let rcl_handle = self.rcl_handle.lock().unwrap();
-            // SAFETY: The provided goal handle is properly initialized by construction.
-            unsafe { rcl_action_goal_handle_get_status(*rcl_handle, &mut state).ok()? }
-        }
-        // SAFETY: state is initialized to a valid GoalStatus value and will only ever by set by
-        // rcl_action_goal_handle_get_status to a valid GoalStatus value.
-        Ok(unsafe { std::mem::transmute(state) })
-    }
-
-    /// Returns whether the client has requested that this goal be cancelled.
-    pub(super) fn is_cancelling(&self) -> bool {
-        self.get_status().is_ok_and(|s| s == GoalStatus::Cancelling)
-    }
-
-    /// Returns true if the goal is either pending or executing, or false if it has reached a
-    /// terminal state.
-    pub(super) fn is_active(&self) -> bool {
-        let rcl_handle = self.rcl_handle.lock().unwrap();
-        // SAFETY: The provided goal handle is properly initialized by construction.
-        unsafe { rcl_action_goal_handle_is_active(*rcl_handle) }
-    }
-
-    /// Returns whether the goal is executing.
-    pub(super) fn is_executing(&self) -> bool {
-        self.get_status().is_ok_and(|s| s == GoalStatus::Executing)
-    }
-
-    /// Get the unique identifier of the goal.
-    pub(super) fn goal_id(&self) -> GoalUuid {
-        self.uuid
-    }
-
-    /// Get the user-provided message describing the goal.
-    pub(super) fn goal(&self) -> Arc<A::Goal> {
-        Arc::clone(&self.goal_request)
-    }
-
-    /// Indicate that the goal is being cancelled.
-    ///
-    /// This is called when a cancel request for the goal has been accepted.
-    ///
-    /// Returns an error if the goal is in any state other than accepted or executing.
-    pub(super) fn transition_to_cancelling(&self) -> Result<(), RclrsError> {
-        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCEL_GOAL)
-    }
-
-    /// Indicate that the goal could not be reached and has been aborted.
-    ///
-    /// Only call this if the goal is executing but cannot be completed. This is a terminal state,
-    /// so no more methods may be called on a goal handle after this is called.
-    ///
-    /// Returns an error if the goal is in any state other than executing.
-    pub(super) fn transition_to_abort(&self, result: &A::Result) -> Result<(), RclrsError> {
-        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_ABORT)?;
-        let result_rmw = <A::Result as Message>::into_rmw_message(Cow::Borrowed(result)).into_owned();
-        self.terminate_goal(&self.uuid, 6, result_rmw)?;
-        Ok(())
-    }
-
-    /// Indicate that the goal has succeeded.
-    ///
-    /// Only call this if the goal is executing and has reached the desired final state. This is a
-    /// terminal state, so no more methods may be called on a goal handle after this is called.
-    ///
-    /// Returns an error if the goal is in any state other than executing.
-    pub(super) fn transition_to_succeed(&self, result: &A::Result) -> Result<(), RclrsError> {
-        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_SUCCEED)?;
-        let result_rmw = <A::Result as Message>::into_rmw_message(Cow::Borrowed(result)).into_owned();
-        self.terminate_goal(&self.uuid, TerminalStatus::Succeeded, result_rmw)?;
-        Ok(())
-    }
-
-    /// Indicate that the goal has been cancelled.
-    ///
-    /// Only call this if the goal is executing or pending, but has been cancelled. This is a
-    /// terminal state, so no more methods may be called on a goal handle after this is called.
-    ///
-    /// Returns an error if the goal is in any state other than executing or pending.
-    pub(super) fn transition_to_cancelled(&self, result: &A::Result) -> Result<(), RclrsError> {
-        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCELED)?;
-        let result_rmw = <A::Result as Message>::into_rmw_message(Cow::Borrowed(result)).into_owned();
-        self.terminate_goal(&self.uuid, TerminalStatus::Cancelled, result_rmw)?;
-        Ok(())
-    }
-
-    /// Indicate that the server is starting to execute the goal.
-    ///
-    /// Only call this if the goal is pending. This is a terminal state, so no more methods may be
-    /// called on a goal handle after this is called.
-    ///
-    /// Returns an error if the goal is in any state other than pending.
-    pub(super) fn transition_to_execute(&self) -> Result<(), RclrsError> {
-        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_EXECUTE)?;
-
-        // Publish the state change.
-        if let Some(action_server) = self.action_server.upgrade() {
-            action_server.publish_status()?;
-        }
-        Ok(())
-    }
-
-    /// Send an update about the goal's progress.
-    ///
-    /// This may only be called when the goal is executing.
-    ///
-    /// Returns an error if the goal is in any state other than executing.
-    pub(super) fn publish_feedback(&self, feedback: &A::Feedback) -> Result<(), RclrsError> {
-        // If the action server no longer exists, simply drop the message.
-        if let Some(action_server) = self.action_server.upgrade() {
-            action_server.publish_feedback(&self.uuid, feedback)?;
-        }
-        Ok(())
-    }
-
-    fn terminate_goal(
-        &self,
-        goal_id: &GoalUuid,
-        status: TerminalStatus,
-        result: <A::Result as Message>::RmwMsg,
-    ) -> Result<(), RclrsError> {
-        let response_rmw = <A as ActionImpl>::create_result_response(status as i8, result);
-
-        // Publish the result to anyone listening.
-        self.publish_result(goal_id, response_rmw);
-
-        // Publish the state change.
-        self.publish_status();
-
-        // Notify rcl that a goal has terminated and to therefore recalculate the expired goal timer.
-        unsafe {
-            // SAFETY: The action server is locked and valid. No other preconditions.
-            rcl_action_notify_goal_done(&*self.handle.lock())
-        }
-        .ok()?;
-
-        // Release ownership of the goal handle. It will persist until the user also drops it.
-        self.goal_handles.lock().unwrap().remove(&goal_id);
-
-        Ok(())
-    }
-
-
-    /// Attempt to perform the given goal state transition.
-    fn update_state(&self, event: rcl_action_goal_event_t) -> Result<(), RclrsError> {
-        let mut rcl_handle = self.rcl_handle.lock().unwrap();
-        // SAFETY: The provided goal handle is properly initialized by construction.
-        unsafe { rcl_action_update_goal_state(*rcl_handle, event).ok() }
-    }
-}
-
-pub(super) type ActionResponseRmw<A> = <<<A as ActionImpl>::GetResultService as Service>::Response as Message>::RmwMsg;
-
-/// Manages the state of a goal's response.
-pub(super) enum ResponseState<A: ActionImpl> {
-    /// The response has not arrived yet. There may be some clients waiting for
-    /// the response, and they'll be listed here.
-    Waiting(Vec<rmw_request_id_t>),
-    /// The response is available.
-    Available(ActionResponseRmw<A>),
-}
-
-impl<A: ActionImpl> ResponseState<A> {
-    fn new() -> Self {
-        Self::Waiting(Vec::new())
-    }
-
-    fn provide_result(
-        &mut self,
-        action_server_handle: &ActionServerHandle,
-        goal_id: &GoalUuid,
-        mut result: ActionResponseRmw<A>,
-    ) -> Result<(), RclrsError> {
-        let result_requests = match self {
-            Self::Waiting(waiting) => waiting,
-            Self::Available(previous) => {
-                log_error!(
-                    "action_server_goal_handle.provide_result",
-                    "Action goal {goal_id} was provided with multiple results, \
-                    which is not allowed by the action server state machine and \
-                    indicates a bug in rclrs. The new result will be discarded.\
-                    \nPrevious result: {previous:?}\
-                    \nNew result: {result:?}"
-                );
-            }
-        };
-
-        if !result_requests.is_empty() {
-            let action_server = action_server_handle.lock();
-
-            // Respond to all queued requests.
-            for mut result_request in result_requests {
-                Self::send_result(action_server, result_request, &mut result)?;
-            }
-        }
-
-        *self = Self::Available(result);
-    }
-
-    fn add_result_request(
-        &mut self,
-        action_server_handle: &ActionServerHandle,
-        goal_id: &GoalUuid,
-        result_request: rmw_request_id_t,
-    ) -> Result<(), RclrsError> {
-        match self {
-            Self::Waiting(waiting) => {
-                waiting.push(result_request);
-            }
-            Self::Available(result) => {
-                let action_server = action_server_handle.lock();
-                Self::send_result(action_server, &mut result_request, result)?;
-            }
-        }
-        Ok(())
-    }
-
-    fn send_result(
-        action_server: &rmw_request_id_t,
-        result_request: &mut rmw_request_id_t,
-        result_response: &mut ActionResponseRmw<A>,
-    ) -> Result<(), RclrsError> {
-        unsafe {
-            // SAFETY: The action server handle is kept valid by the
-            // ActionServerHandle. The compiler ensures we have unique access
-            // to the result_request and result_response structures.
-            rcl_action_send_result_response(
-                action_server,
-                &mut result_request,
-                result_response as *mut _ as *mut _,
-            )
-            .ok()
-        }
-    }
-}
-
-pub(super) struct CancellationState {
-    receiver: Receiver<CancellationMode>,
-    sender: Sender<CancellationMode>,
-}
-
-impl Default for CancellationState {
-    fn default() -> Self {
-        let (sender, receiver) = watch_channel(CancellationMode::None);
-        Self { receiver, sender }
-    }
-}
-
-pub(super) enum CancellationMode {
-    None,
-    CancelRequested(Vec<rmw_request_id_t>),
-    Cancelling,
-}
-
-impl<A: ActionImpl> Drop for ActionServerGoalHandle<A> {
-    fn drop(&mut self) {
-        // SAFETY: There should not be any way for the mutex to be poisoned
-        let mut rcl_handle = self.rcl_handle.lock().unwrap();
-        unsafe {
-            // SAFETY: The goal handle was propertly initialized, and it will
-            // never be accessed again after this.
-            rcl_action_goal_handle_fini(&mut *rcl_handle);
-        }
-    }
-}
diff --git a/rclrs/src/drop_guard.rs b/rclrs/src/drop_guard.rs
index f4e47b2d..bd57d707 100644
--- a/rclrs/src/drop_guard.rs
+++ b/rclrs/src/drop_guard.rs
@@ -1,20 +1,20 @@
 use std::{
     mem::ManuallyDrop,
-    ops::{Deref, DerefMut, Drop, Fn},
+    ops::{Deref, DerefMut},
 };
 
 /// A wrapper providing additional drop-logic for the contained value.
 ///
 /// When this wrapper is dropped, the contained value will be passed into the given function before
 /// being destructed.
-pub(crate) struct DropGuard<T, F: Fn(T)> {
+pub(crate) struct DropGuard<T> {
     value: ManuallyDrop<T>,
-    drop_fn: F,
+    drop_fn: fn(T),
 }
 
-impl<T, F: Fn(T)> DropGuard<T, F> {
+impl<T> DropGuard<T> {
     /// Create a new `DropGuard` with the given value and drop function.
-    pub fn new(value: T, drop_fn: F) -> Self {
+    pub fn new(value: T, drop_fn: fn(T)) -> Self {
         Self {
             value: ManuallyDrop::new(value),
             drop_fn,
@@ -22,7 +22,7 @@ impl<T, F: Fn(T)> DropGuard<T, F> {
     }
 }
 
-impl<T, F: Fn(T)> Deref for DropGuard<T, F> {
+impl<T> Deref for DropGuard<T> {
     type Target = T;
 
     fn deref(&self) -> &T {
@@ -30,13 +30,13 @@ impl<T, F: Fn(T)> Deref for DropGuard<T, F> {
     }
 }
 
-impl<T, F: Fn(T)> DerefMut for DropGuard<T, F> {
+impl<T> DerefMut for DropGuard<T> {
     fn deref_mut(&mut self) -> &mut T {
         &mut *self.value
     }
 }
 
-impl<T, F: Fn(T)> Drop for DropGuard<T, F> {
+impl<T> Drop for DropGuard<T> {
     fn drop(&mut self) {
         // SAFETY: ManuallyDrop::take() leaves `self.value` in an uninitialized state, meaning that
         // it must not be accessed further. This is guaranteed since `self` is being dropped and
diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs
index 4c25b04d..731c2bf8 100644
--- a/rclrs/src/error.rs
+++ b/rclrs/src/error.rs
@@ -58,7 +58,19 @@ pub enum RclrsError {
         expected: ReadyKind,
         /// The ready information that was received.
         received: ReadyKind,
-    }
+    },
+    /// From rcl documentation for rcl_action_accept_new_goal:
+    ///
+    /// If a failure occurs, `NULL` is returned and an error message is set.
+    /// Possible reasons for failure:
+    ///   - action server is invalid
+    ///   - goal info is invalid
+    ///   - goal ID is already being tracked by the action server
+    ///   - memory allocation failure
+    ///
+    /// We have no way of diagnosing which of these errors caused the failure, so
+    /// all we can do is indicate that an error occurred with accepting the goal.
+    GoalAcceptanceError,
 }
 
 impl RclrsError {
@@ -135,6 +147,12 @@ impl Display for RclrsError {
                     \n - Actual: {received:?}",
                 )
             }
+            RclrsError::GoalAcceptanceError => {
+                write!(
+                    f,
+                    "An error occurred while trying to accept an action server goal",
+                )
+            }
         }
     }
 }
@@ -150,7 +168,7 @@ impl Display for RclrsError {
 /// [1]: std::error::Error
 /// [2]: crate::RclrsError
 #[derive(Debug, PartialEq, Eq)]
-pub struct RclErrorMsg(String);
+pub struct RclErrorMsg(pub(crate) String);
 
 impl Display for RclErrorMsg {
     fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {

From 7340ec781cb1a39d8d21ca8b11b7f61ef25bd2f8 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Mon, 28 Jul 2025 00:08:42 +0800
Subject: [PATCH 06/20] Finished action server goal state machine API -- need
 to finish action_server.rs

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/src/action/action_server.rs             |   3 +
 .../src/action/action_server/accepted_goal.rs |  67 +++-
 .../action_server_goal_handle.rs              |   2 +-
 .../action_server/cancellation_state.rs       | 262 ++++++++++++++
 .../action/action_server/cancelling_goal.rs   |  35 ++
 .../action/action_server/executing_goal.rs    |  68 +++-
 .../action_server/live_action_server_goal.rs  | 331 +++++-------------
 7 files changed, 510 insertions(+), 258 deletions(-)
 create mode 100644 rclrs/src/action/action_server/cancellation_state.rs

diff --git a/rclrs/src/action/action_server.rs b/rclrs/src/action/action_server.rs
index 69eafc82..b03e7ea5 100644
--- a/rclrs/src/action/action_server.rs
+++ b/rclrs/src/action/action_server.rs
@@ -18,6 +18,9 @@ pub use accepted_goal::*;
 mod action_server_goal_handle;
 use action_server_goal_handle::*;
 
+mod cancellation_state;
+use cancellation_state::*;
+
 mod cancelling_goal;
 use cancelling_goal::*;
 
diff --git a/rclrs/src/action/action_server/accepted_goal.rs b/rclrs/src/action/action_server/accepted_goal.rs
index fcfda858..7b307069 100644
--- a/rclrs/src/action/action_server/accepted_goal.rs
+++ b/rclrs/src/action/action_server/accepted_goal.rs
@@ -1,8 +1,9 @@
-use crate::log_error;
 use super::{CancellingGoal, ExecutingGoal, LiveActionServerGoal};
 use std::sync::Arc;
 use rosidl_runtime_rs::ActionImpl;
 
+/// This manages a goal which has been accepted but has not begun executing yet.
+/// It is allowed to transition into the executing or cancelling state.
 pub struct AcceptedGoal<A: ActionImpl> {
     live: Arc<LiveActionServerGoal<A>>,
 }
@@ -16,21 +17,65 @@ impl<A: ActionImpl> AcceptedGoal<A> {
     /// but the cancellation request will be ignored until you handle it with
     /// [`ExecutingGoal`].
     pub fn execute(self) -> ExecutingGoal<A> {
-        if let Err(err) = self.live.transition_to_execute() {
-            log_error!(
-                "AcceptedGoal.execute",
-                "Failed to transition to executing status. This error should \
-                not happen, please report it to the maintainers of rclrs: {err}",
-            );
-        }
-
         ExecutingGoal::new(self.live)
     }
 
-    /// Transition the goal into either the executing mode or the cancelling mode
-    /// depending on whether an action client has sent a cancellation request.
+    /// Process a [`Future`] until it is finished or until a cancellation request
+    /// is received.
+    ///
+    /// If the [`Future`] finishes, its output will be provided in [`Ok`]. If a
+    /// cancellation request is received before the [`Future`] is finished, you
+    /// will receive an [`Err`] with the current state of the [`Future`], which
+    /// you can continue processing later if you choose.
+    ///
+    /// After the cancellation request is received, you will still need to trigger
+    /// [`Self::begin_cancelling`] or [`Self::reject_cancellation`] to respond to
+    /// the request. Otherwise the cancellation request will not receive a response
+    /// until the goal reaches a terminal state.
+    //
+    // TODO(@mxgrey): Add a doctest and example for this.
+    pub async fn until_cancel_requested<F: Future + Unpin>(&self, f: F) -> Result<F::Output, F> {
+        self.live.cancellation().until_cancel_requested(f).await
+    }
+
+    /// Transition the goal into the cancelling state.
+    ///
+    /// This does not require an action client to request a cancellation. Instead
+    /// you may act as the action client and commanding that the goal transition
+    /// into cancelling.
+    ///
+    /// If there are any open cancellation requests for this goal from any action
+    /// clients, they will all be notified that the cancellation is accepted.
+    ///
+    /// For the goal to reach the cancelled state, you must follow this up with
+    /// [`CancellingGoal::cancelled_with`].
+    pub fn begin_cancelling(self) -> CancellingGoal<A> {
+        CancellingGoal::new(self.live)
+    }
+
+    /// If there are any open cancellation requests for this goal, reject them.
+    /// This does not transition the goal in any way.
+    pub fn reject_cancellation(&self) {
+        self.live.reject_cancellation();
+    }
+
+    /// Transition the goal into either the executing mode or the cancelling mode.
+    ///
+    /// If any action client has sent a cancellation request, this will automatically
+    /// transition the goal into the cancelling state and provide you with a
+    /// [`CancellingGoal`]. Otherwise the goal will transition into the executing
+    /// state and you will receive an [`ExecutingGoal`].
     pub fn begin(self) -> BeginAcceptedGoal<A> {
+        if self.live.cancel_requested() {
+            BeginAcceptedGoal::Execute(self.execute())
+        } else {
+            BeginAcceptedGoal::Cancel(self.begin_cancelling())
+        }
+    }
 
+    /// Publish feedback for action clients to read.
+    pub fn publish_feedback(&self, feedback: &A::Feedback) {
+        self.live.publish_feedback(feedback);
     }
 
     pub(super) fn new(live: Arc<LiveActionServerGoal<A>>) -> Self {
diff --git a/rclrs/src/action/action_server/action_server_goal_handle.rs b/rclrs/src/action/action_server/action_server_goal_handle.rs
index 64f1e119..7c70dd6e 100644
--- a/rclrs/src/action/action_server/action_server_goal_handle.rs
+++ b/rclrs/src/action/action_server/action_server_goal_handle.rs
@@ -1,7 +1,7 @@
 use crate::{
     rcl_bindings::*,
     log_error,
-    GoalUuid, RclrsError, ToResult,
+    GoalUuid, ToResult,
 };
 use super::{GoalStatus};
 use std::sync::{Mutex, MutexGuard};
diff --git a/rclrs/src/action/action_server/cancellation_state.rs b/rclrs/src/action/action_server/cancellation_state.rs
new file mode 100644
index 00000000..4bfb062b
--- /dev/null
+++ b/rclrs/src/action/action_server/cancellation_state.rs
@@ -0,0 +1,262 @@
+use crate::{
+    rcl_bindings::*,
+    vendor::{
+        action_msgs::{
+            msg::GoalInfo,
+            srv::CancelGoal_Response,
+        },
+        unique_identifier_msgs::msg::UUID,
+    },
+    log_error,
+    CancelResponse, GoalUuid, ToResult, Node,
+};
+use super::ActionServerHandle;
+use std::{
+    borrow::Cow,
+    collections::HashSet,
+    sync::{Arc, Mutex},
+    future::Future,
+};
+use futures::{future::{select, Either}, pin_mut};
+use rosidl_runtime_rs::Message;
+use tokio::sync::watch::{Sender, Receiver, channel as watch_channel};
+
+pub(super) struct CancellationState {
+    receiver: Receiver<bool>,
+    sender: Sender<bool>,
+
+    /// We put a mutex on the mode because when we respond to cancellation
+    /// requests we need to ensure that we update the cancellation mode
+    /// atomically
+    mode: Mutex<CancellationMode>,
+}
+
+impl CancellationState {
+    pub(super) fn until_cancel_requested<F: Future + Unpin>(&self, f: F) -> impl Future<Output = Result<F::Output, F>> {
+        let mut watcher = self.receiver.clone();
+        async move {
+            let cancel_requested = watcher.wait_for(|request_received| *request_received);
+            pin_mut!(cancel_requested);
+            match select(f, cancel_requested).await {
+                Either::Left((result, _)) => Ok(result),
+                Either::Right((_, f)) => Err(f),
+            }
+        }
+    }
+
+    /// Check if a cancellation is currently being requested.
+    pub(super) fn cancel_requested(&self) -> bool {
+        *self.receiver.borrow()
+    }
+
+    pub(super) fn request_cancellation(
+        &self,
+        request: CancellationRequest,
+        uuid: &GoalUuid,
+    ) {
+        let mut mode = self.mode.lock().unwrap();
+        match &mut *mode {
+            CancellationMode::None => {
+                let requests = Vec::from_iter([request]);
+                *mode = CancellationMode::CancelRequested(requests);
+            }
+            CancellationMode::CancelRequested(requests) => {
+                requests.push(request);
+            }
+            CancellationMode::Cancelling => {
+                request.accept(*uuid);
+            }
+        }
+    }
+
+    /// Tell current cancellation requesters that their requests are rejected
+    pub(super) fn reject_cancellation(&self, uuid: &GoalUuid) {
+        let mut mode = self.mode.lock().unwrap();
+        match &mut *mode {
+            CancellationMode::CancelRequested(requesters) => {
+                for requester in requesters.drain(..) {
+                    requester.reject(*uuid);
+                }
+
+                // Revert to not having any cancellation mode
+                *mode = CancellationMode::None;
+                self.sender.send(false);
+            }
+            CancellationMode::None => {
+                // Do nothing
+            }
+            CancellationMode::Cancelling => {
+                // Do nothing. We will not revert a cancellation state.
+            }
+        }
+    }
+
+    /// Tell current and future cancellation requesters that their requests are
+    /// accepted
+    pub(super) fn accept_cancellation(&self, uuid: &GoalUuid) {
+        let mut mode = self.mode.lock().unwrap();
+        match &mut *mode {
+            CancellationMode::CancelRequested(requesters) => {
+                for requester in requesters.drain(..) {
+                    requester.accept(*uuid);
+                }
+
+                // Progress to cancelling mode
+                *mode = CancellationMode::Cancelling;
+                // Just in case this signal was never sent, make sure we have
+                // a true value in the cancel requested channel.
+                self.sender.send(true);
+            }
+            CancellationMode::None => {
+                // Skip straight to cancellation mode since the user has accepted
+                // a cancellation even though it wasn't requested externally.
+                *mode = CancellationMode::Cancelling;
+                // Make sure the cancellation is signalled.
+                self.sender.send(true);
+            }
+            CancellationMode::Cancelling => {
+                // Do nothing
+            }
+        }
+    }
+}
+
+impl Default for CancellationState {
+    fn default() -> Self {
+        let (sender, receiver) = watch_channel(false);
+        Self {
+            receiver,
+            sender,
+            mode: Mutex::new(CancellationMode::None),
+        }
+    }
+}
+
+pub(super) enum CancellationMode {
+    None,
+    CancelRequested(Vec<CancellationRequest>),
+    Cancelling,
+}
+
+/// This struct exists to deal with the fact that a single cancellation request
+/// can trigger multiple goal cancellations at once. This allows us to
+/// asynchronously receive the accept/reject results from all the different goals
+/// and then issue the reply once all are received.
+pub(super) struct CancellationRequest {
+    inner: Arc<Mutex<CancellationRequestInner>>,
+}
+
+impl CancellationRequest {
+    pub(super) fn new(
+        id: rmw_request_id_t,
+        waiting_for: Vec<GoalUuid>,
+        server: Arc<ActionServerHandle>,
+        node: Node,
+    ) -> Self {
+        Self {
+            inner: Arc::new(Mutex::new(CancellationRequestInner {
+                id,
+                waiting_for,
+                server,
+                node,
+                received: Default::default(),
+                accepted: Vec::new(),
+                response_sent: false,
+            }))
+        }
+    }
+
+    fn accept(&self, uuid: GoalUuid) {
+        let mut inner = self.inner.lock().unwrap();
+        if !inner.received.insert(uuid) {
+            return;
+        }
+
+        let stamp = inner.node.get_clock().now().to_ros_msg().unwrap_or_default();
+        let info = GoalInfo {
+            goal_id: UUID { uuid: *uuid },
+            stamp,
+        };
+
+        inner.accepted.push(info);
+        inner.respond_if_ready();
+    }
+
+    fn reject(&self, uuid: GoalUuid) {
+        let mut inner = self.inner.lock().unwrap();
+        if !inner.received.insert(uuid) {
+            return;
+        }
+
+        inner.respond_if_ready();
+    }
+
+}
+
+struct CancellationRequestInner {
+    id: rmw_request_id_t,
+    waiting_for: Vec<GoalUuid>,
+    received: HashSet<GoalUuid>,
+    accepted: Vec<GoalInfo>,
+    response_sent: bool,
+    server: Arc<ActionServerHandle>,
+    node: Node,
+}
+
+impl CancellationRequestInner {
+    fn respond_if_ready(&mut self) {
+        for expected in &self.waiting_for {
+            if !self.received.contains(expected) {
+                return;
+            }
+        }
+
+        self.respond();
+    }
+
+    fn respond(&mut self) {
+        if self.response_sent {
+            return;
+        }
+
+        self.response_sent = true;
+
+        let mut response = CancelGoal_Response::default();
+        response.goals_canceling = self.accepted.drain(..).collect();
+        if response.goals_canceling.is_empty() {
+            response.return_code = CancelResponse::Reject as i8;
+        } else {
+            response.return_code = CancelResponse::Accept as i8;
+        }
+
+        let mut response_rmw = CancelGoal_Response::into_rmw_message(Cow::Owned(response)).into_owned();
+        let r = unsafe {
+            // SAFETY: The action server handle is locked and so synchronized with other functions.
+            // The request_id and response are both uniquely owned or borrowed, and so neither will
+            // mutate during this function call.
+            rcl_action_send_cancel_response(
+                &*self.server.lock(),
+                &mut self.id,
+                &mut response_rmw as *mut _ as *mut _,
+            )
+            .ok()
+        };
+
+        if let Err(err) = r {
+            log_error!(
+                "CancellationRequest.respond",
+                "Error occurred while responding to a cancellation request: {err}"
+            )
+        }
+    }
+}
+
+impl Drop for CancellationRequestInner {
+    fn drop(&mut self) {
+        if !self.response_sent {
+            // As a last resort, send the response if all possible responders
+            // have dropped.
+            self.respond();
+        }
+    }
+}
diff --git a/rclrs/src/action/action_server/cancelling_goal.rs b/rclrs/src/action/action_server/cancelling_goal.rs
index c4c33efe..993aabd2 100644
--- a/rclrs/src/action/action_server/cancelling_goal.rs
+++ b/rclrs/src/action/action_server/cancelling_goal.rs
@@ -6,3 +6,38 @@ pub struct CancellingGoal<A: ActionImpl> {
     live: Arc<LiveActionServerGoal<A>>,
 }
 
+impl<A: ActionImpl> CancellingGoal<A> {
+    /// Terminate the goal with a cancelled state and a specific result value.
+    ///
+    /// "Cancelled" is a terminal state, so the state of the goal can no longer
+    /// be changed after this. Publish all relevant feedback before calling this.
+    pub fn cancelled_with(self, result: &A::Result) {
+        self.live.transition_to_cancelled(result);
+    }
+
+    /// Transition the goal into the succeeded state.
+    ///
+    /// "Succeeded" is a terminal state, so the state of the goal can no longer
+    /// be changed after this. Publish all relevant feedback before calling this.
+    pub fn succeeded_with(self, result: &A::Result) {
+        self.live.transition_to_succeed(result);
+    }
+
+    /// Transition the goal into the aborted state.
+    ///
+    /// "Aborted" is a terminal state, so the state of the goal can no longer
+    /// be changed after this. Publish all relevant feedback before calling this.
+    pub fn aborted_with(self, result: &A::Result) {
+        self.live.transition_to_aborted(result);
+    }
+
+    /// Publish feedback for action clients to read.
+    pub fn publish_feedback(&self, feedback: &A::Feedback) {
+        self.live.publish_feedback(feedback);
+    }
+
+    pub(super) fn new(live: Arc<LiveActionServerGoal<A>>) -> Self {
+        live.transition_to_cancelling();
+        Self { live }
+    }
+}
diff --git a/rclrs/src/action/action_server/executing_goal.rs b/rclrs/src/action/action_server/executing_goal.rs
index 136d5382..b4e1eaae 100644
--- a/rclrs/src/action/action_server/executing_goal.rs
+++ b/rclrs/src/action/action_server/executing_goal.rs
@@ -1,5 +1,8 @@
-use super::LiveActionServerGoal;
-use std::sync::Arc;
+use super::{CancellingGoal, LiveActionServerGoal};
+use std::{
+    future::Future,
+    sync::Arc,
+};
 use rosidl_runtime_rs::ActionImpl;
 
 pub struct ExecutingGoal<A: ActionImpl> {
@@ -7,7 +10,68 @@ pub struct ExecutingGoal<A: ActionImpl> {
 }
 
 impl<A: ActionImpl> ExecutingGoal<A> {
+    /// Transition the goal into the succeeded state.
+    ///
+    /// "Succeeded" is a terminal state, so the state of the goal can no longer
+    /// be changed after this. Publish all relevant feedback before calling this.
+    pub fn succeeded_with(self, result: &A::Result) {
+        self.live.transition_to_succeed(result);
+    }
+
+    /// Process a [`Future`] until it is finished or until a cancellation request
+    /// is received.
+    ///
+    /// If the [`Future`] finishes, its output will be provided in [`Ok`]. If a
+    /// cancellation request is received before the [`Future`] is finished, you
+    /// will receive an [`Err`] with the current state of the [`Future`], which
+    /// you can continue processing later if you choose.
+    ///
+    /// After the cancellation request is received, you will still need to trigger
+    /// [`Self::begin_cancelling`] or [`Self::reject_cancellation`] to respond to
+    /// the request. Otherwise the cancellation request will not receive a response
+    /// until the goal reaches a terminal state.
+    //
+    // TODO(@mxgrey): Add a doctest and example for this.
+    pub async fn until_cancel_requested<F: Future + Unpin>(&self, f: F) -> Result<F::Output, F> {
+        self.live.cancellation().until_cancel_requested(f).await
+    }
+
+    /// Transition the goal into the cancelling state.
+    ///
+    /// This does not require an action client to request a cancellation. Instead
+    /// you may act as the action client and commanding that the goal transition
+    /// into cancelling.
+    ///
+    /// If there are any open cancellation requests for this goal from any action
+    /// clients, they will all be notified that the cancellation is accepted.
+    ///
+    /// For the goal to reach the cancelled state, you must follow this up with
+    /// [`CancellingGoal::cancelled_with`].
+    pub fn begin_cancelling(self) -> CancellingGoal<A> {
+        CancellingGoal::new(self.live)
+    }
+
+    /// If there are any open cancellation requests for this goal, reject them.
+    /// This does not transition the goal in any way.
+    pub fn reject_cancellation(&self) {
+        self.live.reject_cancellation();
+    }
+
+    /// Transition the goal into the aborted state.
+    ///
+    /// "Aborted" is a terminal state, so the state of the goal can no longer
+    /// be changed after this. Publish all relevant feedback before calling this.
+    pub fn aborted_with(self, result: &A::Result) {
+        self.live.transition_to_aborted(result);
+    }
+
+    /// Publish feedback for action clients to read.
+    pub fn publish_feedback(&self, feedback: &A::Feedback) {
+        self.live.publish_feedback(feedback);
+    }
+
     pub(super) fn new(live: Arc<LiveActionServerGoal<A>>) -> Self {
+        live.transition_to_executing();
         Self { live }
     }
 }
diff --git a/rclrs/src/action/action_server/live_action_server_goal.rs b/rclrs/src/action/action_server/live_action_server_goal.rs
index fea6b3b9..b32b43f3 100644
--- a/rclrs/src/action/action_server/live_action_server_goal.rs
+++ b/rclrs/src/action/action_server/live_action_server_goal.rs
@@ -1,28 +1,17 @@
 use crate::{
     rcl_bindings::*,
-    vendor::{
-        builtin_interfaces::msg::Time,
-        action_msgs::{
-            msg::GoalInfo,
-            srv::CancelGoal_Response,
-        },
-        unique_identifier_msgs::msg::UUID,
-    },
     log_error,
-    CancelResponse, GoalUuid, RclrsError, RclErrorMsg, RclReturnCode, ToResult, Node,
+    GoalUuid, RclrsError, RclErrorMsg, RclReturnCode, ToResult,
 };
 use super::{
-    ActionServerGoalHandle, ActionServerHandle, TerminalStatus, GoalStatus,
+    ActionServerGoalHandle, ActionServerHandle, CancellationState, GoalStatus, TerminalStatus,
 };
 use std::{
     borrow::Cow,
-    collections::HashSet,
     sync::{Arc, Mutex},
     ops::Deref,
 };
 use rosidl_runtime_rs::{Action, ActionImpl, Message, Service};
-use tokio::sync::watch::{Sender, Receiver, channel as watch_channel};
-
 
 /// This struct is the bridge to the rcl_action API for action server goals that
 /// are still active. It can be used to perform transitions while keeping data in
@@ -34,7 +23,7 @@ use tokio::sync::watch::{Sender, Receiver, channel as watch_channel};
 pub(super) struct LiveActionServerGoal<A: ActionImpl> {
     goal_request: Arc<A::Goal>,
     result_response: Mutex<ResponseState<A>>,
-    cancellation: CancellationState,
+    cancellation: Arc<CancellationState>,
     handle: Arc<ActionServerGoalHandle>,
     server: Arc<ActionServerHandle>,
 }
@@ -61,7 +50,11 @@ impl<A: ActionImpl> LiveActionServerGoal<A> {
 
     /// Has a cancellation been requested for this goal.
     pub(super) fn cancel_requested(&self) -> bool {
-        self.cancellation.receiver.borrow().cancel_requested()
+        self.cancellation.cancel_requested()
+    }
+
+    pub(super) fn cancellation(&self) -> &Arc<CancellationState> {
+        &self.cancellation
     }
 
     /// Indicate that the goal is being cancelled.
@@ -69,8 +62,24 @@ impl<A: ActionImpl> LiveActionServerGoal<A> {
     /// This is called when a cancel request for the goal has been accepted.
     ///
     /// Returns an error if the goal is in any state other than accepted or executing.
-    pub(super) fn transition_to_cancelling(&self) -> Result<(), RclrsError> {
-        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCEL_GOAL)
+    pub(super) fn transition_to_cancelling(&self) {
+        self.cancellation.accept_cancellation(self.goal_id());
+        let r = self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCEL_GOAL);
+        if let Err(err) = r {
+            log_error!(
+                "live_action_server_goal.transition_to_cancelling",
+                "Failed to transition to the cancelling state. This error should \
+                not happen, please report it to the maintainers of rclrs: {err}",
+            );
+        }
+    }
+
+    /// If there are any open cancellation requests, they will be rejected and
+    /// the [`CancellationMode`] will be restored to [`CancellationMode::None`].
+    ///
+    /// This function has no effect if the goal is already in the cancelling state.
+    pub(super) fn reject_cancellation(&self) {
+        self.cancellation.reject_cancellation(self.goal_id());
     }
 
     /// Indicate that the goal could not be reached and has been aborted.
@@ -79,10 +88,18 @@ impl<A: ActionImpl> LiveActionServerGoal<A> {
     /// so no more methods may be called on a goal handle after this is called.
     ///
     /// Returns an error if the goal is in any state other than executing.
-    pub(super) fn transition_to_abort(&self, result: &A::Result) -> Result<(), RclrsError> {
-        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_ABORT)?;
-        self.terminate_goal(TerminalStatus::Aborted, result)?;
-        Ok(())
+    pub(super) fn transition_to_aborted(&self, result: &A::Result) {
+        let r = self
+            .update_state(rcl_action_goal_event_t::GOAL_EVENT_ABORT)
+            .and_then(|_| self.terminate_goal(TerminalStatus::Aborted, result));
+
+        if let Err(err) = r {
+            log_error!(
+                "live_action_server_goal.transition_to_aborted",
+                "Failed to transition to the aborted state. This error should not \
+                happen, please report it to the maintainers of rclrs: {err}",
+            );
+        }
     }
 
     /// Indicate that the goal has succeeded.
@@ -91,10 +108,18 @@ impl<A: ActionImpl> LiveActionServerGoal<A> {
     /// terminal state, so no more methods may be called on a goal handle after this is called.
     ///
     /// Returns an error if the goal is in any state other than executing.
-    pub(super) fn transition_to_succeed(&self, result: &A::Result) -> Result<(), RclrsError> {
-        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_SUCCEED)?;
-        self.terminate_goal(TerminalStatus::Succeeded, result)?;
-        Ok(())
+    pub(super) fn transition_to_succeed(&self, result: &A::Result) {
+        let r = self
+            .update_state(rcl_action_goal_event_t::GOAL_EVENT_SUCCEED)
+            .and_then(|_| self.terminate_goal(TerminalStatus::Succeeded, result));
+
+        if let Err(err) = r {
+            log_error!(
+                "live_action_server_goal.transition_to_succeed",
+                "Failed to transition to the succeeded state. This error should not \
+                happen, please report it to the maintainers of rclrs: {err}",
+            );
+        }
     }
 
     /// Indicate that the goal has been cancelled.
@@ -103,10 +128,18 @@ impl<A: ActionImpl> LiveActionServerGoal<A> {
     /// terminal state, so no more methods may be called on a goal handle after this is called.
     ///
     /// Returns an error if the goal is in any state other than executing or pending.
-    pub(super) fn transition_to_cancelled(&self, result: &A::Result) -> Result<(), RclrsError> {
-        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCELED)?;
-        self.terminate_goal(TerminalStatus::Cancelled, result)?;
-        Ok(())
+    pub(super) fn transition_to_cancelled(&self, result: &A::Result) {
+        let r = self
+            .update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCELED)
+            .and_then(|_| self.terminate_goal(TerminalStatus::Cancelled, result));
+
+        if let Err(err) = r {
+            log_error!(
+                "live_action_server_goal.transition_to_cancelled",
+                "Failed to transition to cancelled state. This error should not \
+                happen, please report it to the maintainers of rclrs: {err}",
+            );
+        }
     }
 
     /// Indicate that the server is starting to execute the goal.
@@ -115,11 +148,18 @@ impl<A: ActionImpl> LiveActionServerGoal<A> {
     /// called on a goal handle after this is called.
     ///
     /// Returns an error if the goal is in any state other than pending.
-    pub(super) fn transition_to_execute(&self) -> Result<(), RclrsError> {
-        self.update_state(rcl_action_goal_event_t::GOAL_EVENT_EXECUTE)?;
+    pub(super) fn transition_to_executing(&self) {
+        let r = self
+            .update_state(rcl_action_goal_event_t::GOAL_EVENT_EXECUTE)
+            .and_then(|_| self.server.publish_status());
 
-        // Publish the state change.
-        self.server.publish_status()
+        if let Err(err) = r {
+            log_error!(
+                "live_action_server_goal.transition_to_executing",
+                "Failed to transition to executing status. This error should \
+                not happen, please report it to the maintainers of rclrs: {err}",
+            );
+        }
     }
 
     /// Send an update about the goal's progress.
@@ -127,10 +167,10 @@ impl<A: ActionImpl> LiveActionServerGoal<A> {
     /// This may only be called when the goal is executing.
     ///
     /// Returns an error if the goal is in any state other than executing.
-    pub(super) fn publish_feedback(&self, feedback: &A::Feedback) -> Result<(), RclrsError> {
+    pub(super) fn publish_feedback(&self, feedback: &A::Feedback) {
         let feedback_rmw = <<A as Action>::Feedback as Message>::into_rmw_message(Cow::Borrowed(feedback));
         let mut feedback_msg = <A as ActionImpl>::create_feedback_message(&*self.goal_id(), feedback_rmw.into_owned());
-        unsafe {
+        let r = unsafe {
             // SAFETY: The action server is locked through the handle, meaning that no other
             // non-thread-safe functions can be called on it at the same time. The feedback_msg is
             // exclusively owned here, ensuring that it won't be modified during the call.
@@ -140,6 +180,15 @@ impl<A: ActionImpl> LiveActionServerGoal<A> {
                 &mut feedback_msg as *mut _ as *mut _,
             )
             .ok()
+        };
+
+        if let Err(err) = r {
+            // This is not an error that should be able to occur.
+            log_error!(
+                "live_action_server_goal.publish_feedback",
+                "Failed to publish feedback for an action server goal. This should \
+                not happen, please report it to the maintainers of rclrs: {err}",
+            );
         }
     }
 
@@ -189,11 +238,11 @@ impl<A: ActionImpl> Drop for LiveActionServerGoal<A> {
             GoalStatus::Accepted => {
                 // Transition into executing and then into aborted to reach a
                 // terminal state.
-                self.transition_to_execute();
-                self.transition_to_abort(&Default::default());
+                self.transition_to_executing();
+                self.transition_to_aborted(&Default::default());
             }
             GoalStatus::Cancelling | GoalStatus::Executing => {
-                self.transition_to_abort(&Default::default());
+                self.transition_to_aborted(&Default::default());
             }
             GoalStatus::Succeeded | GoalStatus::Cancelled | GoalStatus::Aborted => {
                 // Already in a terminal state, no need to do anything.
@@ -297,209 +346,3 @@ impl<A: ActionImpl> ResponseState<A> {
         }
     }
 }
-
-pub(super) struct CancellationState {
-    receiver: Receiver<bool>,
-    sender: Sender<bool>,
-
-    /// We put a mutex on the mode because when we respond to cancellation
-    /// requests we need to ensure that we update the cancellation mode
-    /// atomically
-    mode: Mutex<CancellationMode>,
-}
-
-impl CancellationState {
-    /// Check if a cancellation is currently being requested.
-    fn cancel_requested(&self) -> bool {
-        *self.receiver.borrow()
-    }
-
-    fn request_cancellation(
-        &self,
-        request: CancellationRequest,
-        uuid: &GoalUuid,
-    ) {
-        let mut mode = self.mode.lock().unwrap();
-        match &mut *mode {
-            CancellationMode::None => {
-                let requests = Vec::from_iter([request]);
-                *mode = CancellationMode::CancelRequested(requests);
-            }
-            CancellationMode::CancelRequested(requests) => {
-                requests.push(request);
-            }
-            CancellationMode::Cancelling => {
-                request.accept(*uuid);
-            }
-        }
-    }
-
-    /// Tell current cancellation requesters that their requests are rejected
-    fn reject_cancellation(&self, uuid: &GoalUuid) {
-        let mut mode = self.mode.lock().unwrap();
-        match &mut *mode {
-            CancellationMode::CancelRequested(requesters) => {
-                for requester in requesters.drain(..) {
-                    requester.reject(*uuid);
-                }
-
-                // Revert to not having any cancellation mode
-                *mode = CancellationMode::None;
-                self.sender.send(false);
-            }
-            CancellationMode::None => {
-                // Do nothing
-            }
-            CancellationMode::Cancelling => {
-                // Do nothing. We will not revert a cancellation state.
-            }
-        }
-    }
-
-    /// Tell current and future cancellation requesters that their requests are
-    /// accepted
-    fn accept_cancellation(&self, uuid: &GoalUuid) {
-        let mut mode = self.mode.lock().unwrap();
-        match &mut *mode {
-            CancellationMode::CancelRequested(requesters) => {
-                for requester in requesters.drain(..) {
-                    requester.accept(*uuid);
-                }
-
-                // Progress to cancelling mode
-                *mode = CancellationMode::Cancelling;
-                // Just in case this signal was never sent, make sure we have
-                // a true value in the cancel requested channel.
-                self.sender.send(true);
-            }
-            CancellationMode::None => {
-                // Skip straight to cancellation mode since the user has accepted
-                // a cancellation even though it wasn't requested externally.
-                *mode = CancellationMode::Cancelling;
-                // Make sure the cancellation is signalled.
-                self.sender.send(true);
-            }
-            CancellationMode::Cancelling => {
-                // Do nothing
-            }
-        }
-    }
-
-}
-
-impl Default for CancellationState {
-    fn default() -> Self {
-        let (sender, receiver) = watch_channel(CancellationMode::None);
-        Self { receiver, sender }
-    }
-}
-
-pub(super) enum CancellationMode {
-    None,
-    CancelRequested(Vec<CancellationRequest>),
-    Cancelling,
-}
-
-/// This struct exists to deal with the fact that a single cancellation request
-/// can trigger multiple goal cancellations at once. This allows us to
-/// asynchronously receive the accept/reject results from all the different goals
-/// and then issue the reply once all are received.
-struct CancellationRequest {
-    inner: Arc<Mutex<CancellationRequestInner>>,
-}
-
-impl CancellationRequest {
-    fn accept(&self, uuid: GoalUuid) {
-        let mut inner = self.inner.lock().unwrap();
-        if !inner.received.insert(uuid) {
-            return;
-        }
-
-        let stamp = inner.node.get_clock().now().to_ros_msg().unwrap_or_default();
-        let info = GoalInfo {
-            goal_id: UUID { uuid: *uuid },
-            stamp,
-        };
-
-        inner.accepted.push(info);
-        inner.respond_if_ready();
-    }
-
-    fn reject(&self, uuid: GoalUuid) {
-        let mut inner = self.inner.lock().unwrap();
-        if !inner.received.insert(uuid) {
-            return;
-        }
-
-        inner.respond_if_ready();
-    }
-
-}
-
-struct CancellationRequestInner {
-    id: rmw_request_id_t,
-    waiting_for: Vec<GoalUuid>,
-    received: HashSet<GoalUuid>,
-    accepted: Vec<GoalInfo>,
-    response_sent: bool,
-    server: Arc<ActionServerHandle>,
-    node: Node,
-}
-
-impl CancellationRequestInner {
-    fn respond_if_ready(&mut self) {
-        for expected in &self.waiting_for {
-            if !self.received.contains(expected) {
-                return;
-            }
-        }
-
-        self.respond();
-    }
-
-    fn respond(&mut self) {
-        if self.response_sent {
-            return;
-        }
-
-        self.response_sent = true;
-
-        let mut response = CancelGoal_Response::default();
-        response.goals_canceling = self.accepted.drain(..).collect();
-        if response.goals_canceling.is_empty() {
-            response.return_code = CancelResponse::Reject as i8;
-        } else {
-            response.return_code = CancelResponse::Accept as i8;
-        }
-
-        let mut response_rmw = CancelGoal_Response::into_rmw_message(Cow::Owned(response)).into_owned();
-        let r = unsafe {
-            // SAFETY: The action server handle is locked and so synchronized with other functions.
-            // The request_id and response are both uniquely owned or borrowed, and so neither will
-            // mutate during this function call.
-            rcl_action_send_cancel_response(
-                &*self.server.lock(),
-                &mut self.id,
-                &mut response_rmw as *mut _ as *mut _,
-            )
-            .ok()
-        };
-
-        if let Err(err) = r {
-            log_error!(
-                "CancellationRequest.respond",
-                "Error occurred while responding to a cancellation request: {err}"
-            )
-        }
-    }
-}
-
-impl Drop for CancellationRequestInner {
-    fn drop(&mut self) {
-        if !self.response_sent {
-            // As a last resort, send the response if all possible responders
-            // have dropped.
-            self.respond();
-        }
-    }
-}

From cedbb8f03cba97a9e998f8546089117706f3580c Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Mon, 28 Jul 2025 15:47:35 +0800
Subject: [PATCH 07/20] Progress on action_server implementation

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/src/action/action_client.rs             |   2 +-
 rclrs/src/action/action_server.rs             | 287 ++++++++++--------
 .../src/action/action_server/accepted_goal.rs |  13 +-
 .../action/action_server/cancelling_goal.rs   |  16 +-
 .../action/action_server/executing_goal.rs    |  14 +-
 .../action_server/live_action_server_goal.rs  |   4 +-
 .../action/action_server/requested_goal.rs    |  49 ++-
 .../action/action_server/terminated_goal.rs   |  18 ++
 rclrs/src/client.rs                           |   2 +-
 rclrs/src/service.rs                          |   2 +-
 rclrs/src/subscription.rs                     |   2 +-
 rclrs/src/wait_set/guard_condition.rs         |   2 +-
 rclrs/src/wait_set/rcl_primitive.rs           |  16 +-
 rclrs/src/wait_set/waitable.rs                |  12 +-
 14 files changed, 282 insertions(+), 157 deletions(-)
 create mode 100644 rclrs/src/action/action_server/terminated_goal.rs

diff --git a/rclrs/src/action/action_client.rs b/rclrs/src/action/action_client.rs
index 556fd1a0..1bb44444 100644
--- a/rclrs/src/action/action_client.rs
+++ b/rclrs/src/action/action_client.rs
@@ -1,5 +1,5 @@
 use crate::{
-    error::ToResult, rcl_bindings::*, wait::WaitableNumEntities, Node, NodeHandle, QoSProfile,
+    error::ToResult, rcl_bindings::*, Node, NodeHandle, QoSProfile,
     RclrsError, ENTITY_LIFECYCLE_MUTEX,
 };
 use std::{
diff --git a/rclrs/src/action/action_server.rs b/rclrs/src/action/action_server.rs
index b03e7ea5..ebc4cbde 100644
--- a/rclrs/src/action/action_server.rs
+++ b/rclrs/src/action/action_server.rs
@@ -2,15 +2,19 @@ use crate::{
     action::{CancelResponse, GoalResponse, GoalUuid},
     error::{RclReturnCode, ToResult},
     rcl_bindings::*,
-    DropGuard, Node, NodeHandle, QoSProfile, RclrsError, ENTITY_LIFECYCLE_MUTEX,
+    DropGuard, Node, NodeHandle, QoSProfile, RclPrimitive, RclrsError,
+    RclPrimitiveHandle, RclPrimitiveKind, ReadyKind, Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
 };
 use rosidl_runtime_rs::{ActionImpl, Message, Service};
 use std::{
+    any::Any,
     borrow::Borrow,
     collections::HashMap,
     ffi::CString,
     sync::{Arc, Mutex, MutexGuard, Weak},
 };
+use futures::future::BoxFuture;
+use tokio::sync::mpsc::UnboundedSender;
 
 mod accepted_goal;
 pub use accepted_goal::*;
@@ -33,11 +37,49 @@ use live_action_server_goal::*;
 mod requested_goal;
 pub use requested_goal::*;
 
-pub(crate) enum ReadyMode {
-    GoalRequest,
-    CancelRequest,
-    ResultRequest,
-    GoalExpired,
+mod terminated_goal;
+pub use terminated_goal::*;
+
+/// `ActionServerOptions` are used by [`Node::create_action_server`][1] to initialize an
+/// [`ActionServer`].
+///
+/// [1]: crate::NodeState::create_action_server
+#[derive(Debug, Clone)]
+#[non_exhaustive]
+pub struct ActionServerOptions<'a> {
+    /// The name of the action implemented by this server
+    pub action_name: &'a str,
+    /// The quality of service profile for the goal service
+    pub goal_service_qos: QoSProfile,
+    /// The quality of service profile for the result service
+    pub result_service_qos: QoSProfile,
+    /// The quality of service profile for the cancel service
+    pub cancel_service_qos: QoSProfile,
+    /// The quality of service profile for the feedback topic
+    pub feedback_topic_qos: QoSProfile,
+    /// The quality of service profile for the status topic
+    pub status_topic_qos: QoSProfile,
+    // TODO(nwn): result_timeout
+}
+
+impl<'a> ActionServerOptions<'a> {
+    /// Initialize a new [`ActionServerOptions`] with default settings.
+    pub fn new(action_name: &'a str) -> Self {
+        Self {
+            action_name,
+            goal_service_qos: QoSProfile::services_default(),
+            result_service_qos: QoSProfile::services_default(),
+            cancel_service_qos: QoSProfile::services_default(),
+            feedback_topic_qos: QoSProfile::topics_default(),
+            status_topic_qos: QoSProfile::action_status_default(),
+        }
+    }
+}
+
+impl<'a, T: Borrow<str> + ?Sized + 'a> From<&'a T> for ActionServerOptions<'a> {
+    fn from(value: &'a T) -> Self {
+        Self::new(value.borrow())
+    }
 }
 
 /// An action server that can respond to requests sent by ROS action clients.
@@ -67,35 +109,25 @@ pub type ActionServer<A> = Arc<ActionServerState<A>>;
 ///
 /// [1]: std::sync::Weak
 struct ActionServerState<A: ActionImpl> {
-    handle: Arc<ActionServerHandle>,
-
-    board: Mutex<ActionServerGoalBoard<A>>,
+    board: Arc<ActionServerGoalBoard<A>>,
 
-    /// Ensure the parent node remains alive as long as the subscription is held.
+    /// Holding onto this keeps the waitable for this action server alive in the
+    /// wait set of the executor.
     #[allow(unused)]
-    node: Node,
+    lifecycle: WaitableLifecycle,
 }
 
-pub struct ActionServerGoalBoard<A: ActionImpl> {
-    /// These goals have a live handle held by the user. We refer to them with a
-    /// Weak to prevent a circular reference. When the user drops the live handle
-    /// it will automatically be moved into the dropped_goals map.
-    live_goals: HashMap<GoalUuid, Weak<LiveActionServerGoal<A>>>,
-}
-
-impl<T> ActionServerState<T>
-where
-    T: ActionImpl,
-{
+impl<A: ActionImpl> ActionServerState<A> {
     /// Creates a new action server.
     pub(crate) fn create<'a>(
         node: &Node,
         options: impl Into<ActionServerOptions<'a>>,
+        receiver: GoalReceiver<A>,
     ) -> Result<Self, RclrsError> {
         let options = options.into();
         // SAFETY: Getting a zero-initialized value is always safe.
         let mut rcl_action_server = unsafe { rcl_action_get_zero_initialized_server() };
-        let type_support = T::get_type_support() as *const rosidl_action_type_support_t;
+        let type_support = A::get_type_support() as *const rosidl_action_type_support_t;
         let action_name_c_string =
             CString::new(options.action_name).map_err(|err| RclrsError::StringContainsNul {
                 err,
@@ -135,59 +167,50 @@ where
         let handle = Arc::new(ActionServerHandle {
             rcl_action_server: Mutex::new(rcl_action_server),
             node_handle: Arc::clone(&node.handle()),
+            goals: Default::default(),
         });
 
-        let mut num_entities = WaitableNumEntities::default();
-        unsafe {
-            rcl_action_server_wait_set_get_num_entities(
-                &*handle.lock(),
-                &mut num_entities.num_subscriptions,
-                &mut num_entities.num_guard_conditions,
-                &mut num_entities.num_timers,
-                &mut num_entities.num_clients,
-                &mut num_entities.num_services,
-            )
-            .ok()?;
-        }
+        let board = Arc::new(ActionServerGoalBoard::new(receiver, handle, node));
 
-        Ok(Self {
-            handle,
-            num_entities,
-            goal_callback: Box::new(goal_callback),
-            cancel_callback: Box::new(cancel_callback),
-            accepted_callback: Box::new(accepted_callback),
-            goal_handles: Mutex::new(HashMap::new()),
-            goal_results: Mutex::new(HashMap::new()),
-            result_requests: Mutex::new(HashMap::new()),
-            node: node.clone(),
-        })
-    }
+        let async_commands = node.commands().async_worker_commands();
+        let (waitable, lifecycle) = Waitable::new(
+            Box::new(ActionServerExecutable {
+                board: Arc::clone(&board),
+            }),
+            Some(Arc::clone(async_commands.get_guard_condition())),
+        );
+        async_commands.add_to_wait_set(waitable);
 
-    pub fn node(&self) -> &Node {
-        &self.node
+        Ok(Self { board, lifecycle })
     }
+}
 
-    fn take_goal_request(&self) -> Result<(<<T::SendGoalService as Service>::Request as Message>::RmwMsg, rmw_request_id_t), RclrsError> {
-        let mut request_id = rmw_request_id_t {
-            writer_guid: [0; 16],
-            sequence_number: 0,
-        };
-        type RmwRequest<T> = <<<T as ActionImpl>::SendGoalService as Service>::Request as Message>::RmwMsg;
-        let mut request_rmw = RmwRequest::<T>::default();
-        let handle = &*self.handle.lock();
-        unsafe {
-            // SAFETY: The action server is locked by the handle. The request_id is a
-            // zero-initialized rmw_request_id_t, and the request_rmw is a default-initialized
-            // SendGoalService request message.
-            rcl_action_take_goal_request(
-                handle,
-                &mut request_id,
-                &mut request_rmw as *mut RmwRequest<T> as *mut _,
-            )
+pub struct ActionServerGoalBoard<A: ActionImpl> {
+    /// These goals have a live handle held by the user. We refer to them with a
+    /// Weak to prevent a circular reference. When the user drops the live handle
+    /// it will automatically be moved into the dropped_goals map.
+    live_goals: Mutex<HashMap<GoalUuid, Weak<LiveActionServerGoal<A>>>>,
+    receiver: GoalReceiver<A>,
+    handle: Arc<ActionServerHandle>,
+    node: Node,
+}
+
+impl<A: ActionImpl> ActionServerGoalBoard<A> {
+    fn new(
+        receiver: GoalReceiver<A>,
+        handle: Arc<ActionServerHandle>,
+        node: &Node,
+    ) -> Self {
+        Self {
+            receiver,
+            handle,
+            node: Arc::clone(node),
+            live_goals: Default::default(),
         }
-        .ok()?;
+    }
 
-        Ok((request_rmw, request_id))
+    pub fn node(&self) -> &Node {
+        &self.node
     }
 
     fn send_goal_response(
@@ -195,7 +218,7 @@ where
         mut request_id: rmw_request_id_t,
         accepted: bool,
     ) -> Result<(), RclrsError> {
-        let mut response_rmw = <T as ActionImpl>::create_goal_response(accepted, (0, 0));
+        let mut response_rmw = <A as ActionImpl>::create_goal_response(accepted, (0, 0));
         let handle = &*self.handle.lock();
         let result = unsafe {
             // SAFETY: The action server handle is locked and so synchronized with other
@@ -224,8 +247,8 @@ where
         }
     }
 
-    fn execute_goal_request(self: Arc<Self>) -> Result<(), RclrsError> {
-        let (request, request_id) = match self.take_goal_request() {
+    fn execute_goal_request(self: &Arc<Self>) -> Result<(), RclrsError> {
+        let (request, request_id) = match self.handle.take_goal_request::<A>() {
             Ok(res) => res,
             Err(RclrsError::RclError {
                 code: RclReturnCode::ServiceTakeFailed,
@@ -238,12 +261,8 @@ where
             Err(err) => return Err(err),
         };
 
-        let uuid = GoalUuid(*<T as ActionImpl>::get_goal_request_uuid(&request));
+        let uuid = GoalUuid(*<A as ActionImpl>::get_goal_request_uuid(&request));
 
-        let response: GoalResponse = {
-            todo!("Optionally convert request to an idiomatic type for the user's callback.");
-            todo!("Call self.goal_callback(uuid, request)");
-        };
 
         // Don't continue if the goal was rejected by the user.
         if response == GoalResponse::Reject {
@@ -569,22 +588,61 @@ where
             if num_expired > 0 {
                 // Clean up the expired goal.
                 let uuid = GoalUuid(expired_goal.goal_id.uuid);
-                self.goal_results.lock().unwrap().remove(&uuid);
-                self.result_requests.lock().unwrap().remove(&uuid);
-                self.goal_handles.lock().unwrap().remove(&uuid);
+                self.live_goals.lock().unwrap().remove(&uuid);
+                self.handle.goals.lock().unwrap().remove(&uuid);
             } else {
                 break;
             }
         }
 
+        // Clear any lingering dropped goals from the board to avoid leaks
+        self.live_goals.lock().unwrap().retain(
+            |_, weak| weak.upgrade().is_some()
+        );
+
         Ok(())
     }
+}
 
+struct ActionServerExecutable<A: ActionImpl> {
+    board: Arc<ActionServerGoalBoard<A>>,
 }
 
-// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
-// they are running in. Therefore, this type can be safely sent to another thread.
-unsafe impl Send for rcl_action_server_t {}
+impl<A: ActionImpl> RclPrimitive for ActionServerExecutable<A> {
+    unsafe fn execute(
+        &mut self,
+        ready: ReadyKind,
+        _payload: &mut dyn Any,
+    ) -> Result<(), RclrsError> {
+        let ready = ready.for_action_server()?;
+
+        if ready.goal_request {
+            self.board.execute_goal_request()?;
+        }
+
+        if ready.cancel_request {
+            self.board.execute_cancel_request()?;
+        }
+
+        if ready.result_request {
+            self.board.execute_result_request()?;
+        }
+
+        if ready.goal_expired {
+            self.board.execute_goal_expired()?;
+        }
+
+        Ok(())
+    }
+
+    fn kind(&self) -> crate::RclPrimitiveKind {
+        RclPrimitiveKind::ActionServer
+    }
+
+    fn handle(&self) -> RclPrimitiveHandle {
+        RclPrimitiveHandle::ActionServer(self.board.handle.lock())
+    }
+}
 
 /// Manage the lifecycle of an `rcl_action_server_t`, including managing its dependencies
 /// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are
@@ -600,6 +658,10 @@ pub(crate) struct ActionServerHandle {
     pub(super) goals: Mutex<HashMap<GoalUuid, Arc<ActionServerGoalHandle>>>,
 }
 
+// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
+// they are running in. Therefore, this type can be safely sent to another thread.
+unsafe impl Send for rcl_action_server_t {}
+
 impl ActionServerHandle {
     pub(super) fn lock(&self) -> MutexGuard<rcl_action_server_t> {
         self.rcl_action_server.lock().unwrap()
@@ -636,48 +698,35 @@ impl ActionServerHandle {
         }
         .ok()
     }
-}
 
-/// `ActionServerOptions` are used by [`Node::create_action_server`][1] to initialize an
-/// [`ActionServer`].
-///
-/// [1]: crate::Node::create_action_server
-#[derive(Debug, Clone)]
-#[non_exhaustive]
-pub struct ActionServerOptions<'a> {
-    /// The name of the action implemented by this server
-    pub action_name: &'a str,
-    /// The quality of service profile for the goal service
-    pub goal_service_qos: QoSProfile,
-    /// The quality of service profile for the result service
-    pub result_service_qos: QoSProfile,
-    /// The quality of service profile for the cancel service
-    pub cancel_service_qos: QoSProfile,
-    /// The quality of service profile for the feedback topic
-    pub feedback_topic_qos: QoSProfile,
-    /// The quality of service profile for the status topic
-    pub status_topic_qos: QoSProfile,
-    // TODO(nwn): result_timeout
-}
-
-impl<'a> ActionServerOptions<'a> {
-    /// Initialize a new [`ActionServerOptions`] with default settings.
-    pub fn new(action_name: &'a str) -> Self {
-        Self {
-            action_name,
-            goal_service_qos: QoSProfile::services_default(),
-            result_service_qos: QoSProfile::services_default(),
-            cancel_service_qos: QoSProfile::services_default(),
-            feedback_topic_qos: QoSProfile::topics_default(),
-            status_topic_qos: QoSProfile::action_status_default(),
+    fn take_goal_request<A: ActionImpl>(&self) -> Result<(ActionGoalRequestRmw<A>, rmw_request_id_t), RclrsError> {
+        let mut request_id = rmw_request_id_t {
+            writer_guid: [0; 16],
+            sequence_number: 0,
+        };
+        let mut request_rmw = ActionGoalRequestRmw::<A>::default();
+        let handle = self.lock();
+        unsafe {
+            // SAFETY: The action server is locked by the handle. The request_id is a
+            // zero-initialized rmw_request_id_t, and the request_rmw is a default-initialized
+            // SendGoalService request message.
+            rcl_action_take_goal_request(
+                &*handle,
+                &mut request_id,
+                &mut request_rmw as *mut ActionGoalRequestRmw<A> as *mut _,
+            )
         }
+        .ok()?;
+
+        Ok((request_rmw, request_id))
     }
 }
 
-impl<'a, T: Borrow<str> + ?Sized + 'a> From<&'a T> for ActionServerOptions<'a> {
-    fn from(value: &'a T) -> Self {
-        Self::new(value.borrow())
-    }
+type ActionGoalRequestRmw<A> = <<<A as ActionImpl>::SendGoalService as Service>::Request as Message>::RmwMsg;
+
+enum GoalReceiver<A: ActionImpl> {
+    Callback(Box<dyn FnMut(RequestedGoal<A>) -> BoxFuture<'static, TerminatedGoal> + Send + Sync>),
+    Receiver(UnboundedSender<RequestedGoal<A>>),
 }
 
 /// Values defined by `action_msgs/msg/GoalStatus`
diff --git a/rclrs/src/action/action_server/accepted_goal.rs b/rclrs/src/action/action_server/accepted_goal.rs
index 7b307069..be8fa266 100644
--- a/rclrs/src/action/action_server/accepted_goal.rs
+++ b/rclrs/src/action/action_server/accepted_goal.rs
@@ -1,5 +1,8 @@
 use super::{CancellingGoal, ExecutingGoal, LiveActionServerGoal};
-use std::sync::Arc;
+use std::{
+    future::Future,
+    sync::Arc
+};
 use rosidl_runtime_rs::ActionImpl;
 
 /// This manages a goal which has been accepted but has not begun executing yet.
@@ -9,6 +12,11 @@ pub struct AcceptedGoal<A: ActionImpl> {
 }
 
 impl<A: ActionImpl> AcceptedGoal<A> {
+    /// Get the goal of this action.
+    pub fn goal(&self) -> &Arc<A::Goal> {
+        self.live.goal()
+    }
+
     /// Transition the goal into the executing state. This will allow you to
     /// start sending feedback and transition to other states such as cancelling,
     /// succeedded, and aborted.
@@ -16,6 +24,7 @@ impl<A: ActionImpl> AcceptedGoal<A> {
     /// You can use this even if an action client has requested a cancellation,
     /// but the cancellation request will be ignored until you handle it with
     /// [`ExecutingGoal`].
+    #[must_use]
     pub fn execute(self) -> ExecutingGoal<A> {
         ExecutingGoal::new(self.live)
     }
@@ -49,6 +58,7 @@ impl<A: ActionImpl> AcceptedGoal<A> {
     ///
     /// For the goal to reach the cancelled state, you must follow this up with
     /// [`CancellingGoal::cancelled_with`].
+    #[must_use]
     pub fn begin_cancelling(self) -> CancellingGoal<A> {
         CancellingGoal::new(self.live)
     }
@@ -65,6 +75,7 @@ impl<A: ActionImpl> AcceptedGoal<A> {
     /// transition the goal into the cancelling state and provide you with a
     /// [`CancellingGoal`]. Otherwise the goal will transition into the executing
     /// state and you will receive an [`ExecutingGoal`].
+    #[must_use]
     pub fn begin(self) -> BeginAcceptedGoal<A> {
         if self.live.cancel_requested() {
             BeginAcceptedGoal::Execute(self.execute())
diff --git a/rclrs/src/action/action_server/cancelling_goal.rs b/rclrs/src/action/action_server/cancelling_goal.rs
index 993aabd2..bb89586c 100644
--- a/rclrs/src/action/action_server/cancelling_goal.rs
+++ b/rclrs/src/action/action_server/cancelling_goal.rs
@@ -1,4 +1,4 @@
-use super::LiveActionServerGoal;
+use super::{LiveActionServerGoal, TerminatedGoal};
 use std::sync::Arc;
 use rosidl_runtime_rs::ActionImpl;
 
@@ -7,28 +7,36 @@ pub struct CancellingGoal<A: ActionImpl> {
 }
 
 impl<A: ActionImpl> CancellingGoal<A> {
+    /// Get the goal of this action.
+    pub fn goal(&self) -> &Arc<A::Goal> {
+        self.live.goal()
+    }
+
     /// Terminate the goal with a cancelled state and a specific result value.
     ///
     /// "Cancelled" is a terminal state, so the state of the goal can no longer
     /// be changed after this. Publish all relevant feedback before calling this.
-    pub fn cancelled_with(self, result: &A::Result) {
+    pub fn cancelled_with(self, result: &A::Result) -> TerminatedGoal {
         self.live.transition_to_cancelled(result);
+        TerminatedGoal { uuid: *self.live.goal_id() }
     }
 
     /// Transition the goal into the succeeded state.
     ///
     /// "Succeeded" is a terminal state, so the state of the goal can no longer
     /// be changed after this. Publish all relevant feedback before calling this.
-    pub fn succeeded_with(self, result: &A::Result) {
+    pub fn succeeded_with(self, result: &A::Result) -> TerminatedGoal {
         self.live.transition_to_succeed(result);
+        TerminatedGoal { uuid: *self.live.goal_id() }
     }
 
     /// Transition the goal into the aborted state.
     ///
     /// "Aborted" is a terminal state, so the state of the goal can no longer
     /// be changed after this. Publish all relevant feedback before calling this.
-    pub fn aborted_with(self, result: &A::Result) {
+    pub fn aborted_with(self, result: &A::Result) -> TerminatedGoal {
         self.live.transition_to_aborted(result);
+        TerminatedGoal { uuid: *self.live.goal_id() }
     }
 
     /// Publish feedback for action clients to read.
diff --git a/rclrs/src/action/action_server/executing_goal.rs b/rclrs/src/action/action_server/executing_goal.rs
index b4e1eaae..bfba2fca 100644
--- a/rclrs/src/action/action_server/executing_goal.rs
+++ b/rclrs/src/action/action_server/executing_goal.rs
@@ -1,4 +1,4 @@
-use super::{CancellingGoal, LiveActionServerGoal};
+use super::{CancellingGoal, LiveActionServerGoal, TerminatedGoal};
 use std::{
     future::Future,
     sync::Arc,
@@ -10,12 +10,18 @@ pub struct ExecutingGoal<A: ActionImpl> {
 }
 
 impl<A: ActionImpl> ExecutingGoal<A> {
+    /// Get the goal of this action.
+    pub fn goal(&self) -> &Arc<A::Goal> {
+        self.live.goal()
+    }
+
     /// Transition the goal into the succeeded state.
     ///
     /// "Succeeded" is a terminal state, so the state of the goal can no longer
     /// be changed after this. Publish all relevant feedback before calling this.
-    pub fn succeeded_with(self, result: &A::Result) {
+    pub fn succeeded_with(self, result: &A::Result) -> TerminatedGoal {
         self.live.transition_to_succeed(result);
+        TerminatedGoal { uuid: *self.live.goal_id() }
     }
 
     /// Process a [`Future`] until it is finished or until a cancellation request
@@ -47,6 +53,7 @@ impl<A: ActionImpl> ExecutingGoal<A> {
     ///
     /// For the goal to reach the cancelled state, you must follow this up with
     /// [`CancellingGoal::cancelled_with`].
+    #[must_use]
     pub fn begin_cancelling(self) -> CancellingGoal<A> {
         CancellingGoal::new(self.live)
     }
@@ -61,8 +68,9 @@ impl<A: ActionImpl> ExecutingGoal<A> {
     ///
     /// "Aborted" is a terminal state, so the state of the goal can no longer
     /// be changed after this. Publish all relevant feedback before calling this.
-    pub fn aborted_with(self, result: &A::Result) {
+    pub fn aborted_with(self, result: &A::Result) -> TerminatedGoal {
         self.live.transition_to_aborted(result);
+        TerminatedGoal { uuid: *self.live.goal_id() }
     }
 
     /// Publish feedback for action clients to read.
diff --git a/rclrs/src/action/action_server/live_action_server_goal.rs b/rclrs/src/action/action_server/live_action_server_goal.rs
index b32b43f3..4b85af22 100644
--- a/rclrs/src/action/action_server/live_action_server_goal.rs
+++ b/rclrs/src/action/action_server/live_action_server_goal.rs
@@ -44,8 +44,8 @@ impl<A: ActionImpl> LiveActionServerGoal<A> {
     }
 
     /// Get the user-provided message describing the goal.
-    pub(super) fn goal(&self) -> Arc<A::Goal> {
-        Arc::clone(&self.goal_request)
+    pub(super) fn goal(&self) -> &Arc<A::Goal> {
+        &self.goal_request
     }
 
     /// Has a cancellation been requested for this goal.
diff --git a/rclrs/src/action/action_server/requested_goal.rs b/rclrs/src/action/action_server/requested_goal.rs
index 77d81610..8834bf96 100644
--- a/rclrs/src/action/action_server/requested_goal.rs
+++ b/rclrs/src/action/action_server/requested_goal.rs
@@ -1,9 +1,9 @@
 use crate::{
     rcl_bindings::*,
     log_error,
-    GoalUuid, RclrsError, ToResult, ActionServer,
+    GoalUuid, RclrsError, ToResult, ActionServerGoalBoard,
 };
-use super::{ActionServerGoalHandle, LiveActionServerGoal, AcceptedGoal};
+use super::{ActionServerGoalHandle, LiveActionServerGoal, AcceptedGoal, TerminatedGoal};
 use std::sync::Arc;
 use rosidl_runtime_rs::ActionImpl;
 
@@ -13,7 +13,7 @@ pub struct GoalAcceptanceError;
 /// An action goal that has been requested but not accepted yet. If this is
 /// dropped without being accepted then the goal request will be rejected.
 pub struct RequestedGoal<A: ActionImpl> {
-    server: ActionServer<A>,
+    board: Arc<ActionServerGoalBoard<A>>,
     goal_request: Arc<A::Goal>,
     uuid: GoalUuid,
     goal_request_id: rmw_request_id_t,
@@ -21,6 +21,11 @@ pub struct RequestedGoal<A: ActionImpl> {
 }
 
 impl<A: ActionImpl> RequestedGoal<A> {
+    /// Get the goal of this action.
+    pub fn goal(&self) -> &Arc<A::Goal> {
+        &self.goal_request
+    }
+
     /// Accept the requested goal. The action client will be notified that the
     /// goal was accepted, and you will be able to begin executing.
     pub fn accept(mut self) -> Result<AcceptedGoal<A>, RclrsError> {
@@ -30,11 +35,11 @@ impl<A: ActionImpl> RequestedGoal<A> {
                 rcl_action_get_zero_initialized_goal_info()
             };
             goal_info.goal_id.uuid = *self.uuid;
-            goal_info.stamp = self.server.node().get_clock().now().to_rcl().unwrap_or(
+            goal_info.stamp = self.board.node().get_clock().now().to_rcl().unwrap_or(
                 builtin_interfaces__msg__Time { sec: 0, nanosec: 0 }
             );
 
-            let mut server_handle = self.server.handle.lock();
+            let mut server_handle = self.board.handle.lock();
             let goal_handle_ptr = unsafe {
                 // SAFETY: The action server handle is locked and so synchronized with other
                 // functions. The request_id and response message are uniquely owned, and so will
@@ -70,20 +75,20 @@ impl<A: ActionImpl> RequestedGoal<A> {
         // We need to store a strong reference to the goal handle in the action
         // server handle to ensure the goal handle outlives its use within the
         // action server.
-        self.server.handle.goals.lock().unwrap().insert(*handle.goal_id(), Arc::clone(&handle));
+        self.board.handle.goals.lock().unwrap().insert(*handle.goal_id(), Arc::clone(&handle));
 
         self.accepted = true;
         self.send_goal_response();
 
         let live = Arc::new(LiveActionServerGoal::new(
             handle,
-            Arc::clone(&self.server.handle),
+            Arc::clone(&self.board.handle),
             Arc::clone(&self.goal_request),
         ));
 
         // Add the live goal to the goal board so the action server executor can
         // interact with it.
-        self.server.board.lock().unwrap().live_goals.insert(self.uuid, Arc::downgrade(&live));
+        self.board.live_goals.lock().unwrap().insert(self.uuid, Arc::downgrade(&live));
 
         Ok(AcceptedGoal::new(live))
     }
@@ -92,24 +97,40 @@ impl<A: ActionImpl> RequestedGoal<A> {
     ///
     /// This is the same as simply allowing the [`RequestedGoal`] to drop without
     /// accepting.
-    pub fn reject(self) {
-        // This function need to do anything, it will simply drop the RequestedGoal,
-        // and the custom Drop implementation will notify the action server that
-        // the goal is rejected.
+    pub fn reject(self) -> TerminatedGoal {
+        // This function does not need to do anything to transition the goal. It
+        // will simply drop the RequestedGoal, and the custom Drop implementation
+        // will notify the action server that the goal is rejected.
+        TerminatedGoal { uuid: self.uuid }
     }
 
     fn send_goal_response(&mut self) -> Result<(), RclrsError> {
-        let stamp = self.server.node().get_clock().now().to_sec_nanosec().unwrap_or((0, 0));
+        let stamp = self.board.node().get_clock().now().to_sec_nanosec().unwrap_or((0, 0));
         let mut response_rmw = <A as ActionImpl>::create_goal_response(self.accepted, stamp);
         unsafe {
             rcl_action_send_goal_response(
-                &*self.server.handle.lock(),
+                &*self.board.handle.lock(),
                 &mut self.goal_request_id,
                 &mut response_rmw as *mut _ as *mut _,
             )
         }
         .ok()
     }
+
+    pub(super) fn new(
+        board: Arc<ActionServerGoalBoard<A>>,
+        goal_request: Arc<A::Goal>,
+        uuid: GoalUuid,
+        goal_request_id: rmw_request_id_t,
+    ) -> Self {
+        Self {
+            board,
+            goal_request,
+            uuid,
+            goal_request_id,
+            accepted: false,
+        }
+    }
 }
 
 impl<A: ActionImpl> Drop for RequestedGoal<A> {
diff --git a/rclrs/src/action/action_server/terminated_goal.rs b/rclrs/src/action/action_server/terminated_goal.rs
new file mode 100644
index 00000000..6afc8a6a
--- /dev/null
+++ b/rclrs/src/action/action_server/terminated_goal.rs
@@ -0,0 +1,18 @@
+use crate::GoalUuid;
+
+/// This represents a goal that has reached a terminated state. This struct is
+/// used as the return value for action server callbacks to guide the user to
+/// bring the goal to a terminal state.
+///
+/// It is safe to allow this struct to drop if you are using an action goal
+/// receiver instead of an action server callback.
+pub struct TerminatedGoal {
+    pub(super) uuid: GoalUuid,
+}
+
+impl TerminatedGoal {
+    /// Get the UUID of the goal that was terminated.
+    pub fn uuid(&self) -> &GoalUuid {
+        &self.uuid
+    }
+}
diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs
index 29f03fa3..a4b167ac 100644
--- a/rclrs/src/client.rs
+++ b/rclrs/src/client.rs
@@ -416,7 +416,7 @@ where
     T: rosidl_runtime_rs::Service,
 {
     unsafe fn execute(&mut self, ready: ReadyKind, _: &mut dyn Any) -> Result<(), RclrsError> {
-        ready.is_basic()?;
+        ready.for_basic()?;
         self.board.lock().unwrap().execute(&self.handle)
     }
 
diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs
index 985827f9..04536da4 100644
--- a/rclrs/src/service.rs
+++ b/rclrs/src/service.rs
@@ -250,7 +250,7 @@ where
     Scope: WorkScope,
 {
     unsafe fn execute(&mut self, ready: ReadyKind, payload: &mut dyn Any) -> Result<(), RclrsError> {
-        ready.is_basic()?;
+        ready.for_basic()?;
         self.callback
             .lock()
             .unwrap()
diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs
index 9409fd0a..76369f00 100644
--- a/rclrs/src/subscription.rs
+++ b/rclrs/src/subscription.rs
@@ -262,7 +262,7 @@ where
     T: Message,
 {
     unsafe fn execute(&mut self, ready: ReadyKind, payload: &mut dyn Any) -> Result<(), RclrsError> {
-        ready.is_basic()?;
+        ready.for_basic()?;
         self.callback
             .lock()
             .unwrap()
diff --git a/rclrs/src/wait_set/guard_condition.rs b/rclrs/src/wait_set/guard_condition.rs
index 3cd9ec63..16abd9c0 100644
--- a/rclrs/src/wait_set/guard_condition.rs
+++ b/rclrs/src/wait_set/guard_condition.rs
@@ -207,7 +207,7 @@ struct GuardConditionExecutable {
 
 impl RclPrimitive for GuardConditionExecutable {
     unsafe fn execute(&mut self, ready: ReadyKind, _: &mut dyn Any) -> Result<(), RclrsError> {
-        ready.is_basic()?;
+        ready.for_basic()?;
         if let Some(callback) = &mut self.callback {
             callback();
         }
diff --git a/rclrs/src/wait_set/rcl_primitive.rs b/rclrs/src/wait_set/rcl_primitive.rs
index 7d5af77a..399c95a6 100644
--- a/rclrs/src/wait_set/rcl_primitive.rs
+++ b/rclrs/src/wait_set/rcl_primitive.rs
@@ -83,7 +83,7 @@ pub enum ReadyKind {
 
 impl ReadyKind {
     /// Convert a pointer's status into a basic ready indicator.
-    pub fn for_ptr<T>(ptr: *const T) -> Option<Self> {
+    pub fn from_ptr<T>(ptr: *const T) -> Option<Self> {
         if ptr.is_null() {
             None
         } else {
@@ -93,12 +93,22 @@ impl ReadyKind {
 
     /// This is used by the basic primitive types to validate that they are
     /// receiving ready information that matches their primitive type.
-    pub fn is_basic(&self) -> Result<(), RclrsError> {
+    pub fn for_basic(self) -> Result<(), RclrsError> {
         match self {
             Self::Basic => Ok(()),
             _ => Err(RclrsError::InvalidReadyInformation {
                 expected: Self::Basic,
-                received: *self,
+                received: self,
+            })
+        }
+    }
+
+    pub fn for_action_server(self) -> Result<ActionServerReady, RclrsError> {
+        match self {
+            Self::ActionServer(ready) => Ok(ready),
+            _ => Err(RclrsError::InvalidReadyInformation {
+                expected: Self::ActionServer(Default::default()),
+                received: self,
             })
         }
     }
diff --git a/rclrs/src/wait_set/waitable.rs b/rclrs/src/wait_set/waitable.rs
index 4978f45b..a94fbbc4 100644
--- a/rclrs/src/wait_set/waitable.rs
+++ b/rclrs/src/wait_set/waitable.rs
@@ -58,22 +58,22 @@ impl Waitable {
                 // element of the array at the index-th position.
                 match self.primitive.kind() {
                     RclPrimitiveKind::Subscription => {
-                        ReadyKind::for_ptr(wait_set.subscriptions.add(index))
+                        ReadyKind::from_ptr(wait_set.subscriptions.add(index))
                     },
                     RclPrimitiveKind::GuardCondition => {
-                        ReadyKind::for_ptr(wait_set.guard_conditions.add(index))
+                        ReadyKind::from_ptr(wait_set.guard_conditions.add(index))
                     }
                     RclPrimitiveKind::Service => {
-                        ReadyKind::for_ptr(wait_set.services.add(index))
+                        ReadyKind::from_ptr(wait_set.services.add(index))
                     }
                     RclPrimitiveKind::Client => {
-                        ReadyKind::for_ptr(wait_set.clients.add(index))
+                        ReadyKind::from_ptr(wait_set.clients.add(index))
                     }
                     RclPrimitiveKind::Timer => {
-                        ReadyKind::for_ptr(wait_set.timers.add(index))
+                        ReadyKind::from_ptr(wait_set.timers.add(index))
                     }
                     RclPrimitiveKind::Event => {
-                        ReadyKind::for_ptr(wait_set.events.add(index))
+                        ReadyKind::from_ptr(wait_set.events.add(index))
                     }
                     RclPrimitiveKind::ActionServer => {
                         match self.primitive.handle() {

From 03b44c900b82ac044873ec428acd9697aaea1d8d Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Mon, 28 Jul 2025 18:42:06 +0800
Subject: [PATCH 08/20] Action server finished -- needs testing

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/src/action.rs                           |  12 -
 rclrs/src/action/action_server.rs             | 473 ++++++------------
 .../src/action/action_server/accepted_goal.rs |   8 +-
 .../action_server_goal_handle.rs              | 122 ++++-
 .../action_server/cancellation_state.rs       |  49 +-
 .../action/action_server/cancelling_goal.rs   |   6 +-
 .../action/action_server/executing_goal.rs    |   6 +-
 .../action_server/live_action_server_goal.rs  | 134 +----
 .../action/action_server/requested_goal.rs    |  13 +-
 rclrs/src/error.rs                            |   8 +
 rclrs/src/node.rs                             |   2 +-
 11 files changed, 354 insertions(+), 479 deletions(-)

diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs
index f5c1fe98..b046364e 100644
--- a/rclrs/src/action.rs
+++ b/rclrs/src/action.rs
@@ -46,20 +46,8 @@ impl Deref for GoalUuid {
     }
 }
 
-/// The response returned by an [`ActionServer`]'s goal callback when a goal request is received.
-#[derive(PartialEq, Eq)]
-pub enum GoalResponse {
-    /// The goal is rejected and will not be executed.
-    Reject = 1,
-    /// The server accepts the goal and will begin executing it immediately.
-    AcceptAndExecute = 2,
-    /// The server accepts the goal and will begin executing it later.
-    AcceptAndDefer = 3,
-}
-
 /// The response returned by an [`ActionServer`]'s cancel callback when a goal is requested to be cancelled.
 #[derive(PartialEq, Eq)]
-#[repr(i8)]
 pub enum CancelResponse {
     /// The server will not try to cancel the goal.
     Reject = 1,
diff --git a/rclrs/src/action/action_server.rs b/rclrs/src/action/action_server.rs
index ebc4cbde..7388b3eb 100644
--- a/rclrs/src/action/action_server.rs
+++ b/rclrs/src/action/action_server.rs
@@ -1,16 +1,17 @@
 use crate::{
-    action::{CancelResponse, GoalResponse, GoalUuid},
-    error::{RclReturnCode, ToResult},
+    action::GoalUuid,
+    error::ToResult,
     rcl_bindings::*,
     DropGuard, Node, NodeHandle, QoSProfile, RclPrimitive, RclrsError,
     RclPrimitiveHandle, RclPrimitiveKind, ReadyKind, Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
 };
-use rosidl_runtime_rs::{ActionImpl, Message, Service};
+use rosidl_runtime_rs::{Action, Message, RmwGoalRequest, RmwResultRequest};
 use std::{
     any::Any,
     borrow::Borrow,
     collections::HashMap,
     ffi::CString,
+    panic::UnwindSafe,
     sync::{Arc, Mutex, MutexGuard, Weak},
 };
 use futures::future::BoxFuture;
@@ -108,7 +109,7 @@ pub type ActionServer<A> = Arc<ActionServerState<A>>;
 /// The public API of the [`ActionServer`] type is implemented via `ActionServerState`.
 ///
 /// [1]: std::sync::Weak
-struct ActionServerState<A: ActionImpl> {
+struct ActionServerState<A: Action> {
     board: Arc<ActionServerGoalBoard<A>>,
 
     /// Holding onto this keeps the waitable for this action server alive in the
@@ -117,12 +118,12 @@ struct ActionServerState<A: ActionImpl> {
     lifecycle: WaitableLifecycle,
 }
 
-impl<A: ActionImpl> ActionServerState<A> {
+impl<A: Action> ActionServerState<A> {
     /// Creates a new action server.
     pub(crate) fn create<'a>(
         node: &Node,
         options: impl Into<ActionServerOptions<'a>>,
-        receiver: GoalReceiver<A>,
+        dispatch: GoalDispatch<A>,
     ) -> Result<Self, RclrsError> {
         let options = options.into();
         // SAFETY: Getting a zero-initialized value is always safe.
@@ -170,7 +171,7 @@ impl<A: ActionImpl> ActionServerState<A> {
             goals: Default::default(),
         });
 
-        let board = Arc::new(ActionServerGoalBoard::new(receiver, handle, node));
+        let board = Arc::new(ActionServerGoalBoard::new(dispatch, handle, node));
 
         let async_commands = node.commands().async_worker_commands();
         let (waitable, lifecycle) = Waitable::new(
@@ -185,24 +186,24 @@ impl<A: ActionImpl> ActionServerState<A> {
     }
 }
 
-pub struct ActionServerGoalBoard<A: ActionImpl> {
+pub struct ActionServerGoalBoard<A: Action> {
     /// These goals have a live handle held by the user. We refer to them with a
     /// Weak to prevent a circular reference. When the user drops the live handle
     /// it will automatically be moved into the dropped_goals map.
     live_goals: Mutex<HashMap<GoalUuid, Weak<LiveActionServerGoal<A>>>>,
-    receiver: GoalReceiver<A>,
-    handle: Arc<ActionServerHandle>,
+    dispatch: Mutex<GoalDispatch<A>>,
+    handle: Arc<ActionServerHandle<A>>,
     node: Node,
 }
 
-impl<A: ActionImpl> ActionServerGoalBoard<A> {
+impl<A: Action> ActionServerGoalBoard<A> {
     fn new(
-        receiver: GoalReceiver<A>,
-        handle: Arc<ActionServerHandle>,
+        dispatch: GoalDispatch<A>,
+        handle: Arc<ActionServerHandle<A>>,
         node: &Node,
     ) -> Self {
         Self {
-            receiver,
+            dispatch: Mutex::new(dispatch),
             handle,
             node: Arc::clone(node),
             live_goals: Default::default(),
@@ -213,178 +214,60 @@ impl<A: ActionImpl> ActionServerGoalBoard<A> {
         &self.node
     }
 
-    fn send_goal_response(
-        &self,
-        mut request_id: rmw_request_id_t,
-        accepted: bool,
-    ) -> Result<(), RclrsError> {
-        let mut response_rmw = <A as ActionImpl>::create_goal_response(accepted, (0, 0));
-        let handle = &*self.handle.lock();
-        let result = unsafe {
-            // SAFETY: The action server handle is locked and so synchronized with other
-            // functions. The request_id and response message are uniquely owned, and so will
-            // not mutate during this function call.
-            // Also, when appropriate, `rcl_action_accept_new_goal()` has been called beforehand,
-            // as specified in the `rcl_action` docs.
-            rcl_action_send_goal_response(
-                handle,
-                &mut request_id,
-                &mut response_rmw as *mut _ as *mut _,
-            )
-        }
-        .ok();
-        match result {
-            Ok(()) => Ok(()),
-            Err(RclrsError::RclError {
-                code: RclReturnCode::Timeout,
-                ..
-            }) => {
-                // TODO(nwn): Log an error and continue.
-                // (See https://github.com/ros2/rclcpp/pull/2215 for reasoning.)
-                Ok(())
-            }
-            _ => result,
-        }
-    }
-
     fn execute_goal_request(self: &Arc<Self>) -> Result<(), RclrsError> {
-        let (request, request_id) = match self.handle.take_goal_request::<A>() {
+        let (request, goal_request_id) = match self.handle.take_goal_request() {
             Ok(res) => res,
-            Err(RclrsError::RclError {
-                code: RclReturnCode::ServiceTakeFailed,
-                ..
-            }) => {
-                // Spurious wakeup – this may happen even when a waitset indicated that this
-                // action was ready, so it shouldn't be an error.
-                return Ok(());
-            }
-            Err(err) => return Err(err),
-        };
-
-        let uuid = GoalUuid(*<A as ActionImpl>::get_goal_request_uuid(&request));
-
-
-        // Don't continue if the goal was rejected by the user.
-        if response == GoalResponse::Reject {
-            return self.send_goal_response(request_id, false);
-        }
+            Err(err) => {
+                if err.is_take_failed() {
+                    // Spurious wakeup – this may happen even when a waitset indicated that this
+                    // action was ready, so it shouldn't be an error.
+                    return Ok(());
+                }
 
-        let goal_handle = {
-            // SAFETY: No preconditions
-            let mut goal_info = unsafe { rcl_action_get_zero_initialized_goal_info() };
-            // Only populate the goal UUID; the timestamp will be set internally by
-            // rcl_action_accept_new_goal().
-            goal_info.goal_id.uuid = uuid.0;
-
-            let server_handle = &mut *self.handle.lock();
-            let goal_handle_ptr = unsafe {
-                // SAFETY: The action server handle is locked and so synchronized with other
-                // functions. The request_id and response message are uniquely owned, and so will
-                // not mutate during this function call. The returned goal handle pointer should be
-                // valid unless it is null.
-                rcl_action_accept_new_goal(server_handle, &goal_info)
-            };
-            if goal_handle_ptr.is_null() {
-                // Other than rcl_get_error_string(), there's no indication what happened.
-                panic!("Failed to accept goal");
-            } else {
-                Arc::new(ActionServerGoalHandle::<T>::new(
-                    goal_handle_ptr,
-                    Arc::downgrade(&self),
-                    todo!("Create an Arc holding the goal message"),
-                    uuid,
-                ))
+                return Err(err);
             }
         };
 
-        self.send_goal_response(request_id, true)?;
-
-        self.goal_handles
-            .lock()
-            .unwrap()
-            .insert(uuid, Arc::clone(&goal_handle));
+        let (uuid, request) = <A as Action>::split_goal_request(request);
+        let requested_goal = RequestedGoal::new(
+            Arc::clone(self),
+            Arc::new(Message::from_rmw_message(request)),
+            GoalUuid(uuid),
+            goal_request_id,
+        );
 
-        if response == GoalResponse::AcceptAndExecute {
-            goal_handle.transition_to_execute()?;
+        match &mut *self.dispatch.lock()? {
+            GoalDispatch::Callback(callback) => {
+                let f = callback(requested_goal);
+                let _ = self.node.commands().run(f);
+            }
+            GoalDispatch::Sender(sender) => {
+                // A send error means the user has dropped their receiver, so
+                // the requested goal will be dropped and then the goal will be
+                // automatically rejected, so we don't need to do anyting with
+                // SendErrors from here.
+                let _ = sender.send(requested_goal);
+            }
         }
 
-        self.publish_status()?;
-
-        // TODO: Call the user's goal_accepted callback.
-        todo!("Call self.accepted_callback(goal_handle)");
-
         Ok(())
     }
 
-    fn take_cancel_request(&self) -> Result<(action_msgs__srv__CancelGoal_Request, rmw_request_id_t), RclrsError> {
-        let mut request_id = rmw_request_id_t {
-            writer_guid: [0; 16],
-            sequence_number: 0,
-        };
-        // SAFETY: No preconditions
-        let mut request_rmw = unsafe { rcl_action_get_zero_initialized_cancel_request() };
-        let handle = &*self.handle.lock();
-        unsafe {
-            // SAFETY: The action server is locked by the handle. The request_id is a
-            // zero-initialized rmw_request_id_t, and the request_rmw is a zero-initialized
-            // action_msgs__srv__CancelGoal_Request.
-            rcl_action_take_cancel_request(
-                handle,
-                &mut request_id,
-                &mut request_rmw as *mut _ as *mut _,
-            )
-        }
-        .ok()?;
-
-        Ok((request_rmw, request_id))
-    }
-
-    fn send_cancel_response(
-        &self,
-        mut request_id: rmw_request_id_t,
-        response_rmw: &mut action_msgs__srv__CancelGoal_Response,
-    ) -> Result<(), RclrsError> {
-        let handle = &*self.handle.lock();
-        let result = unsafe {
-            // SAFETY: The action server handle is locked and so synchronized with other functions.
-            // The request_id and response are both uniquely owned or borrowed, and so neither will
-            // mutate during this function call.
-            rcl_action_send_cancel_response(
-                handle,
-                &mut request_id,
-                response_rmw as *mut _ as *mut _,
-            )
-        }
-        .ok();
-        match result {
-            Ok(()) => Ok(()),
-            Err(RclrsError::RclError {
-                code: RclReturnCode::Timeout,
-                ..
-            }) => {
-                // TODO(nwn): Log an error and continue.
-                // (See https://github.com/ros2/rclcpp/pull/2215 for reasoning.)
-                Ok(())
-            }
-            _ => result,
-        }
-    }
-
     fn execute_cancel_request(&self) -> Result<(), RclrsError> {
-        let (request, request_id) = match self.take_cancel_request() {
+        let (request, request_id) = match self.handle.take_cancel_request() {
             Ok(res) => res,
-            Err(RclrsError::RclError {
-                code: RclReturnCode::ServiceTakeFailed,
-                ..
-            }) => {
-                // Spurious wakeup – this may happen even when a waitset indicated that this
-                // action was ready, so it shouldn't be an error.
-                return Ok(());
+            Err(err) => {
+                if err.is_take_failed() {
+                    // Spurious wakeup – this may happen even when a waitset indicated that this
+                    // action was ready, so it shouldn't be an error.
+                    return Ok(());
+                }
+
+                return Err(err);
             }
-            Err(err) => return Err(err),
         };
 
-        let mut response_rmw = {
+        let response_rmw = {
             // SAFETY: No preconditions
             let mut response_rmw = unsafe { rcl_action_get_zero_initialized_cancel_response() };
             unsafe {
@@ -407,8 +290,7 @@ impl<A: ActionImpl> ActionServerGoalBoard<A> {
             })
         };
 
-        let num_candidates = response_rmw.msg.goals_canceling.size;
-        let mut num_accepted = 0;
+        let mut waiting_for = Vec::new();
         for idx in 0..response_rmw.msg.goals_canceling.size {
             let goal_info = unsafe {
                 // SAFETY: The array pointed to by response_rmw.msg.goals_canceling.data is
@@ -416,153 +298,68 @@ impl<A: ActionImpl> ActionServerGoalBoard<A> {
                 &*response_rmw.msg.goals_canceling.data.add(idx)
             };
             let goal_uuid = GoalUuid(goal_info.goal_id.uuid);
+            waiting_for.push(goal_uuid);
+        }
 
-            let response = {
-                if let Some(goal_handle) = self.goal_handles.lock().unwrap().get(&goal_uuid) {
-                    let response: CancelResponse = todo!("Call self.cancel_callback(goal_handle)");
-                    if response == CancelResponse::Accept {
-                        // Still reject the request if the goal is no longer cancellable.
-                        if goal_handle.cancel().is_ok() {
-                            CancelResponse::Accept
-                        } else {
-                            CancelResponse::Reject
-                        }
-                    } else {
-                        CancelResponse::Reject
-                    }
-                } else {
-                    CancelResponse::Reject
-                }
-            };
+        let cancellation_request = CancellationRequest::new(
+            request_id,
+            waiting_for.clone(),
+            Arc::clone(&self.handle),
+            Arc::clone(&self.node),
+        );
 
-            if response == CancelResponse::Accept {
-                // Shift the accepted entry back to the first rejected slot, if necessary.
-                if num_accepted < idx {
-                    let goal_info_slot = unsafe {
-                        // SAFETY: The array pointed to by response_rmw.msg.goals_canceling.data is
-                        // guaranteed to contain at least response_rmw.msg.goals_canceling.size
-                        // members. Since `num_accepted` is strictly less than `idx`, it is a
-                        // distinct element of the array, so there is no mutable aliasing.
-                        &mut *response_rmw.msg.goals_canceling.data.add(num_accepted)
-                    };
+        let live_goals = self.live_goals.lock()?;
+        for goal in waiting_for {
+            if let Some(live_goal) = live_goals.get(&goal).and_then(|goal| goal.upgrade()) {
+                live_goal.request_cancellation(cancellation_request.clone());
+            } else {
+                if let Some(handle) = self.handle.goals.lock()?.get(&goal) {
+                    // If the goal is already cancelled then we will say that we
+                    // accept the cancellation request. There is no need to
+                    // check for the cancelling state since non-live goals must
+                    // be in a terminal state.
+                    if handle.is_cancelled() {
+                        cancellation_request.accept(goal);
+                    }
                 }
-                num_accepted += 1;
             }
         }
-        response_rmw.msg.goals_canceling.size = num_accepted;
-
-        // If the user rejects all individual cancel requests, consider the entire request as
-        // having been rejected.
-        if num_accepted == 0 && num_candidates > 0 {
-            // TODO(nwn): Include action_msgs__srv__CancelGoal_Response__ERROR_REJECTED in the rcl
-            // bindings.
-            response_rmw.msg.return_code = 1;
-        }
-
-        // If any goal states changed, publish a status update.
-        if num_accepted > 0 {
-            self.publish_status()?;
-        }
-
-        self.send_cancel_response(request_id, &mut response_rmw.msg)?;
 
         Ok(())
     }
 
-    fn take_result_request(&self) -> Result<(<<T::GetResultService as Service>::Request as Message>::RmwMsg, rmw_request_id_t), RclrsError> {
-        let mut request_id = rmw_request_id_t {
-            writer_guid: [0; 16],
-            sequence_number: 0,
-        };
-        type RmwRequest<T> = <<<T as ActionImpl>::GetResultService as Service>::Request as Message>::RmwMsg;
-        let mut request_rmw = RmwRequest::<T>::default();
-        let handle = &*self.handle.lock();
-        unsafe {
-            // SAFETY: The action server is locked by the handle. The request_id is a
-            // zero-initialized rmw_request_id_t, and the request_rmw is a default-initialized
-            // GetResultService request message.
-            rcl_action_take_result_request(
-                handle,
-                &mut request_id,
-                &mut request_rmw as *mut RmwRequest<T> as *mut _,
-            )
-        }
-        .ok()?;
-
-        Ok((request_rmw, request_id))
-    }
-
-    fn send_result_response(
-        &self,
-        mut request_id: rmw_request_id_t,
-        response_rmw: &mut <<<T as ActionImpl>::GetResultService as rosidl_runtime_rs::Service>::Response as Message>::RmwMsg,
-    ) -> Result<(), RclrsError> {
-        let handle = &*self.handle.lock();
-        let result = unsafe {
-            // SAFETY: The action server handle is locked and so synchronized with other functions.
-            // The request_id and response are both uniquely owned or borrowed, and so neither will
-            // mutate during this function call.
-            rcl_action_send_result_response(
-                handle,
-                &mut request_id,
-                response_rmw as *mut _ as *mut _,
-            )
-        }
-        .ok();
-        match result {
-            Ok(()) => Ok(()),
-            Err(RclrsError::RclError {
-                code: RclReturnCode::Timeout,
-                ..
-            }) => {
-                // TODO(nwn): Log an error and continue.
-                // (See https://github.com/ros2/rclcpp/pull/2215 for reasoning.)
-                Ok(())
-            }
-            _ => result,
-        }
-    }
-
     fn execute_result_request(&self) -> Result<(), RclrsError> {
-        let (request, request_id) = match self.take_result_request() {
+        let (request, mut request_id) = match self.handle.take_result_request() {
             Ok(res) => res,
-            Err(RclrsError::RclError {
-                code: RclReturnCode::ServiceTakeFailed,
-                ..
-            }) => {
-                // Spurious wakeup – this may happen even when a waitset indicated that this
-                // action was ready, so it shouldn't be an error.
-                return Ok(());
-            }
-            Err(err) => return Err(err),
+            Err(err) => {
+                if err.is_take_failed() {
+                    return Ok(());
+                }
+                return Err(err);
+            },
         };
 
-        let uuid = GoalUuid(*<T as ActionImpl>::get_result_request_uuid(&request));
-
-        let goal_exists = unsafe {
-            // SAFETY: No preconditions
-            let mut goal_info = rcl_action_get_zero_initialized_goal_info();
-            goal_info.goal_id.uuid = uuid.0;
-
-            // SAFETY: The action server is locked through the handle. The `goal_info`
-            // argument points to a rcl_action_goal_info_t with the desired UUID.
-            rcl_action_server_goal_exists(&*self.handle.lock(), &goal_info)
-        };
+        let uuid = GoalUuid(*<A as Action>::get_result_request_uuid(&request));
+        if let Some(goal) = self.handle.goals.lock()?.get(&uuid) {
+            goal.add_result_request(&self.handle, request_id)?;
+        } else {
+            // The goal either never existed or expired, so we give back an
+            // unknown response
+            let result_rmw = <<A::Result as Message>::RmwMsg as Default>::default();
+            let mut response_rmw = A::create_result_response(GoalStatus::Unknown as i8, result_rmw);
 
-        if goal_exists {
-            if let Some(result) = self.goal_results.lock().unwrap().get_mut(&uuid) {
-                // Respond immediately if the goal already has a response.
-                self.send_result_response(request_id, result)?;
-            } else {
-                // Queue up the request for a response once the goal terminates.
-                self.result_requests.lock().unwrap().entry(uuid).or_insert(vec![]).push(request_id);
+            let server = self.handle.lock();
+            unsafe {
+                // SAFETY: The action server handle is kept valid by the mutex.
+                // The compiler ensures we have unique access to the result_request
+                // and result_response structures.
+                rcl_action_send_result_response(
+                    &*server,
+                    &mut request_id,
+                    &mut response_rmw as *mut _ as *mut _
+                )
             }
-        } else {
-            // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_UNKNOWN in the rcl
-            // bindings.
-            let null_response = <T::Result as Message>::RmwMsg::default();
-            let mut response_rmw = <T as ActionImpl>::create_result_response(0, null_response);
-            self.send_result_response(request_id, &mut response_rmw)?;
+            .ok()?;
         }
 
         Ok(())
@@ -604,11 +401,11 @@ impl<A: ActionImpl> ActionServerGoalBoard<A> {
     }
 }
 
-struct ActionServerExecutable<A: ActionImpl> {
+struct ActionServerExecutable<A: Action> {
     board: Arc<ActionServerGoalBoard<A>>,
 }
 
-impl<A: ActionImpl> RclPrimitive for ActionServerExecutable<A> {
+impl<A: Action> RclPrimitive for ActionServerExecutable<A> {
     unsafe fn execute(
         &mut self,
         ready: ReadyKind,
@@ -649,20 +446,20 @@ impl<A: ActionImpl> RclPrimitive for ActionServerExecutable<A> {
 /// [dropped after][1] the `rcl_action_server_t`.
 ///
 /// [1]: <https://doc.rust-lang.org/reference/destructors.html>
-pub(crate) struct ActionServerHandle {
+pub(crate) struct ActionServerHandle<A: Action> {
     rcl_action_server: Mutex<rcl_action_server_t>,
     /// Ensure the node remains active while the action server is running
     node_handle: Arc<NodeHandle>,
     /// Ensure the `impl_*` of the action server goals remain valid until they
     /// have expired or until the rcl_action_server_t gets fini-ed.
-    pub(super) goals: Mutex<HashMap<GoalUuid, Arc<ActionServerGoalHandle>>>,
+    pub(super) goals: Mutex<HashMap<GoalUuid, Arc<ActionServerGoalHandle<A>>>>,
 }
 
 // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
 // they are running in. Therefore, this type can be safely sent to another thread.
 unsafe impl Send for rcl_action_server_t {}
 
-impl ActionServerHandle {
+impl<A: Action> ActionServerHandle<A> {
     pub(super) fn lock(&self) -> MutexGuard<rcl_action_server_t> {
         self.rcl_action_server.lock().unwrap()
     }
@@ -699,12 +496,12 @@ impl ActionServerHandle {
         .ok()
     }
 
-    fn take_goal_request<A: ActionImpl>(&self) -> Result<(ActionGoalRequestRmw<A>, rmw_request_id_t), RclrsError> {
+    fn take_goal_request(&self) -> Result<(RmwGoalRequest<A>, rmw_request_id_t), RclrsError> {
         let mut request_id = rmw_request_id_t {
             writer_guid: [0; 16],
             sequence_number: 0,
         };
-        let mut request_rmw = ActionGoalRequestRmw::<A>::default();
+        let mut request_rmw = RmwGoalRequest::<A>::default();
         let handle = self.lock();
         unsafe {
             // SAFETY: The action server is locked by the handle. The request_id is a
@@ -713,20 +510,64 @@ impl ActionServerHandle {
             rcl_action_take_goal_request(
                 &*handle,
                 &mut request_id,
-                &mut request_rmw as *mut ActionGoalRequestRmw<A> as *mut _,
+                &mut request_rmw as *mut RmwGoalRequest<A> as *mut _,
             )
         }
         .ok()?;
 
         Ok((request_rmw, request_id))
     }
-}
 
-type ActionGoalRequestRmw<A> = <<<A as ActionImpl>::SendGoalService as Service>::Request as Message>::RmwMsg;
+    fn take_cancel_request(&self) -> Result<(action_msgs__srv__CancelGoal_Request, rmw_request_id_t), RclrsError> {
+        let mut request_id = rmw_request_id_t {
+            writer_guid: [0; 16],
+            sequence_number: 0,
+        };
+        // SAFETY: No preconditions
+        let mut request_rmw = unsafe { rcl_action_get_zero_initialized_cancel_request() };
+        let handle = self.lock();
+        unsafe {
+            // SAFETY: The action server is locked by the handle. The request_id is a
+            // zero-initialized rmw_request_id_t, and the request_rmw is a zero-initialized
+            // action_msgs__srv__CancelGoal_Request.
+            rcl_action_take_cancel_request(
+                &*handle,
+                &mut request_id,
+                &mut request_rmw as *mut _ as *mut _,
+            )
+        }
+        .ok()?;
+
+        Ok((request_rmw, request_id))
+    }
+
+    fn take_result_request(&self) -> Result<(RmwResultRequest<A>, rmw_request_id_t), RclrsError> {
+        let mut request_id = rmw_request_id_t {
+            writer_guid: [0; 16],
+            sequence_number: 0,
+        };
+
+        let mut request_rmw = RmwResultRequest::<A>::default();
+        let handle = self.lock();
+        unsafe {
+            // SAFETY: The action server is locked by the handle. The request_id is a
+            // zero-initialized rmw_request_id_t, and the request_rmw is a default-initialized
+            // GetResultService request message.
+            rcl_action_take_result_request(
+                &*handle,
+                &mut request_id,
+                &mut request_rmw as *mut RmwResultRequest<A> as *mut _,
+            )
+        }
+        .ok()?;
+
+        Ok((request_rmw, request_id))
+    }
+}
 
-enum GoalReceiver<A: ActionImpl> {
-    Callback(Box<dyn FnMut(RequestedGoal<A>) -> BoxFuture<'static, TerminatedGoal> + Send + Sync>),
-    Receiver(UnboundedSender<RequestedGoal<A>>),
+enum GoalDispatch<A: Action> {
+    Callback(Box<dyn FnMut(RequestedGoal<A>) -> BoxFuture<'static, TerminatedGoal> + Send + Sync + UnwindSafe>),
+    Sender(UnboundedSender<RequestedGoal<A>>),
 }
 
 /// Values defined by `action_msgs/msg/GoalStatus`
diff --git a/rclrs/src/action/action_server/accepted_goal.rs b/rclrs/src/action/action_server/accepted_goal.rs
index be8fa266..ee2a7011 100644
--- a/rclrs/src/action/action_server/accepted_goal.rs
+++ b/rclrs/src/action/action_server/accepted_goal.rs
@@ -3,15 +3,15 @@ use std::{
     future::Future,
     sync::Arc
 };
-use rosidl_runtime_rs::ActionImpl;
+use rosidl_runtime_rs::Action;
 
 /// This manages a goal which has been accepted but has not begun executing yet.
 /// It is allowed to transition into the executing or cancelling state.
-pub struct AcceptedGoal<A: ActionImpl> {
+pub struct AcceptedGoal<A: Action> {
     live: Arc<LiveActionServerGoal<A>>,
 }
 
-impl<A: ActionImpl> AcceptedGoal<A> {
+impl<A: Action> AcceptedGoal<A> {
     /// Get the goal of this action.
     pub fn goal(&self) -> &Arc<A::Goal> {
         self.live.goal()
@@ -94,7 +94,7 @@ impl<A: ActionImpl> AcceptedGoal<A> {
     }
 }
 
-pub enum BeginAcceptedGoal<A: ActionImpl> {
+pub enum BeginAcceptedGoal<A: Action> {
     Execute(ExecutingGoal<A>),
     Cancel(CancellingGoal<A>),
 }
diff --git a/rclrs/src/action/action_server/action_server_goal_handle.rs b/rclrs/src/action/action_server/action_server_goal_handle.rs
index 7c70dd6e..beb9fdfb 100644
--- a/rclrs/src/action/action_server/action_server_goal_handle.rs
+++ b/rclrs/src/action/action_server/action_server_goal_handle.rs
@@ -1,10 +1,11 @@
 use crate::{
     rcl_bindings::*,
     log_error,
-    GoalUuid, ToResult,
+    GoalUuid, ToResult, RclrsError, RclReturnCode, RclErrorMsg, ActionServerHandle,
 };
 use super::{GoalStatus};
 use std::sync::{Mutex, MutexGuard};
+use rosidl_runtime_rs::{Action, RmwResultResponse};
 
 /// This struct is a minimal bridge to the `rcl_action` API for action server goals.
 /// While the goal is still live, it will be managed by a [`LiveActionServerGoal`][1]
@@ -16,18 +17,20 @@ use std::sync::{Mutex, MutexGuard};
 /// [1]: super::LiveActionServerGoal
 /// [2]: super::ActionServerHandle
 /// [3]: super::RequestedGoal::accept
-pub(super) struct ActionServerGoalHandle {
+pub(super) struct ActionServerGoalHandle<A: Action> {
     rcl_handle: Mutex<rcl_action_goal_handle_t>,
+    result_response: Mutex<ResponseState<A>>,
     uuid: GoalUuid,
 }
 
-impl ActionServerGoalHandle {
+impl<A: Action> ActionServerGoalHandle<A> {
     pub(super) fn new(
         rcl_handle: rcl_action_goal_handle_s,
         uuid: GoalUuid,
     ) -> Self {
         Self {
             rcl_handle: Mutex::new(rcl_handle),
+            result_response: Mutex::new(ResponseState::new()),
             uuid,
         }
     }
@@ -60,6 +63,12 @@ impl ActionServerGoalHandle {
         self.get_status() == GoalStatus::Cancelling
     }
 
+    /// This is used to check if we should respond as accepting a cancellation
+    /// request for this goal after it is no longer live.
+    pub(super) fn is_cancelled(&self) -> bool {
+        self.get_status() == GoalStatus::Cancelled
+    }
+
     /// Returns true if the goal is either pending or executing, or false if it has reached a
     /// terminal state.
     pub(super) fn is_active(&self) -> bool {
@@ -77,9 +86,27 @@ impl ActionServerGoalHandle {
     pub(super) fn goal_id(&self) -> &GoalUuid {
         &self.uuid
     }
+
+    /// Provie the result for this action goal
+    pub(super) fn provide_result(
+        &self,
+        action_server_handle: &ActionServerHandle<A>,
+        result: RmwResultResponse<A>,
+    ) -> Result<(), RclrsError> {
+        self.result_response.lock()?.provide_result(action_server_handle, &self.uuid, result)
+    }
+
+    /// Add a result requester for this action goal
+    pub(super) fn add_result_request(
+        &self,
+        action_server_handle: &ActionServerHandle<A>,
+        result_request: rmw_request_id_t,
+    ) -> Result<(), RclrsError> {
+        self.result_response.lock()?.add_result_request(action_server_handle, result_request)
+    }
 }
 
-impl Drop for ActionServerGoalHandle {
+impl<A: Action> Drop for ActionServerGoalHandle<A> {
     fn drop(&mut self) {
         // SAFETY: There should not be any way for the mutex to be poisoned
         let mut rcl_handle = self.rcl_handle.lock().unwrap();
@@ -94,3 +121,90 @@ impl Drop for ActionServerGoalHandle {
 // SAFETY: The functions accessing this type don't care about the thread they are
 // running in. Therefore this type can be safely sent to another thread.
 unsafe impl Send for rcl_action_goal_handle_t {}
+
+/// Manages the state of a goal's response.
+enum ResponseState<A: Action> {
+    /// The response has not arrived yet. There may be some clients waiting for
+    /// the response, and they'll be listed here.
+    Waiting(Vec<rmw_request_id_t>),
+    /// The response is available.
+    Available(RmwResultResponse<A>),
+}
+
+impl<A: Action> ResponseState<A> {
+    fn new() -> Self {
+        Self::Waiting(Vec::new())
+    }
+
+    fn provide_result(
+        &mut self,
+        action_server_handle: &ActionServerHandle<A>,
+        goal_id: &GoalUuid,
+        mut result: RmwResultResponse<A>,
+    ) -> Result<(), RclrsError> {
+        let result_requests = match self {
+            Self::Waiting(waiting) => waiting,
+            Self::Available(previous) => {
+                log_error!(
+                    "action_server_goal_handle.provide_result",
+                    "Action goal {goal_id} was provided with multiple results, \
+                    which is not allowed by the action server state machine and \
+                    indicates a bug in rclrs. The new result will be discarded.\
+                    \nPrevious result: {previous:?}\
+                    \nNew result: {result:?}"
+                );
+                return Err(RclrsError::RclError {
+                    code: RclReturnCode::ActionGoalEventInvalid,
+                    msg: Some(RclErrorMsg("action goal response is already set".to_string())),
+                });
+            }
+        };
+
+        if !result_requests.is_empty() {
+            let action_server = action_server_handle.lock();
+
+            // Respond to all queued requests.
+            for mut result_request in result_requests {
+                Self::send_result(&*action_server, &mut result_request, &mut result)?;
+            }
+        }
+
+        *self = Self::Available(result);
+        Ok(())
+    }
+
+    fn add_result_request(
+        &mut self,
+        action_server_handle: &ActionServerHandle<A>,
+        mut result_request: rmw_request_id_t,
+    ) -> Result<(), RclrsError> {
+        match self {
+            Self::Waiting(waiting) => {
+                waiting.push(result_request);
+            }
+            Self::Available(result) => {
+                let action_server = action_server_handle.lock();
+                Self::send_result(&*action_server, &mut result_request, result)?;
+            }
+        }
+        Ok(())
+    }
+
+    fn send_result(
+        action_server: &rcl_action_server_t,
+        result_request: &mut rmw_request_id_t,
+        result_response: &mut RmwResultResponse<A>,
+    ) -> Result<(), RclrsError> {
+        unsafe {
+            // SAFETY: The action server handle is kept valid by the
+            // ActionServerHandle. The compiler ensures we have unique access
+            // to the result_request and result_response structures.
+            rcl_action_send_result_response(
+                action_server,
+                result_request,
+                result_response as *mut _ as *mut _,
+            )
+            .ok()
+        }
+    }
+}
diff --git a/rclrs/src/action/action_server/cancellation_state.rs b/rclrs/src/action/action_server/cancellation_state.rs
index 4bfb062b..b610e3c2 100644
--- a/rclrs/src/action/action_server/cancellation_state.rs
+++ b/rclrs/src/action/action_server/cancellation_state.rs
@@ -8,7 +8,7 @@ use crate::{
         unique_identifier_msgs::msg::UUID,
     },
     log_error,
-    CancelResponse, GoalUuid, ToResult, Node,
+    CancelResponse, GoalUuid, ToResult, Node, RclrsErrorFilter,
 };
 use super::ActionServerHandle;
 use std::{
@@ -18,20 +18,20 @@ use std::{
     future::Future,
 };
 use futures::{future::{select, Either}, pin_mut};
-use rosidl_runtime_rs::Message;
+use rosidl_runtime_rs::{Action, Message};
 use tokio::sync::watch::{Sender, Receiver, channel as watch_channel};
 
-pub(super) struct CancellationState {
+pub(super) struct CancellationState<A: Action> {
     receiver: Receiver<bool>,
     sender: Sender<bool>,
 
     /// We put a mutex on the mode because when we respond to cancellation
     /// requests we need to ensure that we update the cancellation mode
     /// atomically
-    mode: Mutex<CancellationMode>,
+    mode: Mutex<CancellationMode<A>>,
 }
 
-impl CancellationState {
+impl<A: Action> CancellationState<A> {
     pub(super) fn until_cancel_requested<F: Future + Unpin>(&self, f: F) -> impl Future<Output = Result<F::Output, F>> {
         let mut watcher = self.receiver.clone();
         async move {
@@ -51,7 +51,7 @@ impl CancellationState {
 
     pub(super) fn request_cancellation(
         &self,
-        request: CancellationRequest,
+        request: CancellationRequest<A>,
         uuid: &GoalUuid,
     ) {
         let mut mode = self.mode.lock().unwrap();
@@ -121,7 +121,7 @@ impl CancellationState {
     }
 }
 
-impl Default for CancellationState {
+impl<A: Action> Default for CancellationState<A> {
     fn default() -> Self {
         let (sender, receiver) = watch_channel(false);
         Self {
@@ -132,9 +132,9 @@ impl Default for CancellationState {
     }
 }
 
-pub(super) enum CancellationMode {
+pub(super) enum CancellationMode<A: Action> {
     None,
-    CancelRequested(Vec<CancellationRequest>),
+    CancelRequested(Vec<CancellationRequest<A>>),
     Cancelling,
 }
 
@@ -142,15 +142,23 @@ pub(super) enum CancellationMode {
 /// can trigger multiple goal cancellations at once. This allows us to
 /// asynchronously receive the accept/reject results from all the different goals
 /// and then issue the reply once all are received.
-pub(super) struct CancellationRequest {
-    inner: Arc<Mutex<CancellationRequestInner>>,
+pub(super) struct CancellationRequest<A: Action> {
+    inner: Arc<Mutex<CancellationRequestInner<A>>>,
 }
 
-impl CancellationRequest {
+impl<A: Action> Clone for CancellationRequest<A> {
+    fn clone(&self) -> Self {
+        Self {
+            inner: Arc::clone(&self.inner)
+        }
+    }
+}
+
+impl<A: Action> CancellationRequest<A> {
     pub(super) fn new(
         id: rmw_request_id_t,
         waiting_for: Vec<GoalUuid>,
-        server: Arc<ActionServerHandle>,
+        server: Arc<ActionServerHandle<A>>,
         node: Node,
     ) -> Self {
         Self {
@@ -166,7 +174,7 @@ impl CancellationRequest {
         }
     }
 
-    fn accept(&self, uuid: GoalUuid) {
+    pub(super) fn accept(&self, uuid: GoalUuid) {
         let mut inner = self.inner.lock().unwrap();
         if !inner.received.insert(uuid) {
             return;
@@ -193,17 +201,17 @@ impl CancellationRequest {
 
 }
 
-struct CancellationRequestInner {
+struct CancellationRequestInner<A: Action> {
     id: rmw_request_id_t,
     waiting_for: Vec<GoalUuid>,
     received: HashSet<GoalUuid>,
     accepted: Vec<GoalInfo>,
     response_sent: bool,
-    server: Arc<ActionServerHandle>,
+    server: Arc<ActionServerHandle<A>>,
     node: Node,
 }
 
-impl CancellationRequestInner {
+impl<A: Action> CancellationRequestInner<A> {
     fn respond_if_ready(&mut self) {
         for expected in &self.waiting_for {
             if !self.received.contains(expected) {
@@ -239,8 +247,9 @@ impl CancellationRequestInner {
                 &mut self.id,
                 &mut response_rmw as *mut _ as *mut _,
             )
-            .ok()
-        };
+        }
+        .ok()
+        .timeout_ok();
 
         if let Err(err) = r {
             log_error!(
@@ -251,7 +260,7 @@ impl CancellationRequestInner {
     }
 }
 
-impl Drop for CancellationRequestInner {
+impl<A: Action> Drop for CancellationRequestInner<A> {
     fn drop(&mut self) {
         if !self.response_sent {
             // As a last resort, send the response if all possible responders
diff --git a/rclrs/src/action/action_server/cancelling_goal.rs b/rclrs/src/action/action_server/cancelling_goal.rs
index bb89586c..fd9c5541 100644
--- a/rclrs/src/action/action_server/cancelling_goal.rs
+++ b/rclrs/src/action/action_server/cancelling_goal.rs
@@ -1,12 +1,12 @@
 use super::{LiveActionServerGoal, TerminatedGoal};
 use std::sync::Arc;
-use rosidl_runtime_rs::ActionImpl;
+use rosidl_runtime_rs::Action;
 
-pub struct CancellingGoal<A: ActionImpl> {
+pub struct CancellingGoal<A: Action> {
     live: Arc<LiveActionServerGoal<A>>,
 }
 
-impl<A: ActionImpl> CancellingGoal<A> {
+impl<A: Action> CancellingGoal<A> {
     /// Get the goal of this action.
     pub fn goal(&self) -> &Arc<A::Goal> {
         self.live.goal()
diff --git a/rclrs/src/action/action_server/executing_goal.rs b/rclrs/src/action/action_server/executing_goal.rs
index bfba2fca..89cd9683 100644
--- a/rclrs/src/action/action_server/executing_goal.rs
+++ b/rclrs/src/action/action_server/executing_goal.rs
@@ -3,13 +3,13 @@ use std::{
     future::Future,
     sync::Arc,
 };
-use rosidl_runtime_rs::ActionImpl;
+use rosidl_runtime_rs::Action;
 
-pub struct ExecutingGoal<A: ActionImpl> {
+pub struct ExecutingGoal<A: Action> {
     live: Arc<LiveActionServerGoal<A>>,
 }
 
-impl<A: ActionImpl> ExecutingGoal<A> {
+impl<A: Action> ExecutingGoal<A> {
     /// Get the goal of this action.
     pub fn goal(&self) -> &Arc<A::Goal> {
         self.live.goal()
diff --git a/rclrs/src/action/action_server/live_action_server_goal.rs b/rclrs/src/action/action_server/live_action_server_goal.rs
index 4b85af22..7b8f9702 100644
--- a/rclrs/src/action/action_server/live_action_server_goal.rs
+++ b/rclrs/src/action/action_server/live_action_server_goal.rs
@@ -1,17 +1,18 @@
 use crate::{
     rcl_bindings::*,
     log_error,
-    GoalUuid, RclrsError, RclErrorMsg, RclReturnCode, ToResult,
+    RclrsError, ToResult,
 };
 use super::{
-    ActionServerGoalHandle, ActionServerHandle, CancellationState, GoalStatus, TerminalStatus,
+    ActionServerGoalHandle, ActionServerHandle, CancellationState,
+    CancellationRequest, GoalStatus, TerminalStatus,
 };
 use std::{
     borrow::Cow,
-    sync::{Arc, Mutex},
+    sync::Arc,
     ops::Deref,
 };
-use rosidl_runtime_rs::{Action, ActionImpl, Message, Service};
+use rosidl_runtime_rs::{Action, Message, Service};
 
 /// This struct is the bridge to the rcl_action API for action server goals that
 /// are still active. It can be used to perform transitions while keeping data in
@@ -20,25 +21,23 @@ use rosidl_runtime_rs::{Action, ActionImpl, Message, Service};
 /// When this is dropped, the goal will automatically be transitioned into the
 /// aborted status if the user did not transition the goal into a different
 /// terminal status before dropping it.
-pub(super) struct LiveActionServerGoal<A: ActionImpl> {
+pub(super) struct LiveActionServerGoal<A: Action> {
     goal_request: Arc<A::Goal>,
-    result_response: Mutex<ResponseState<A>>,
-    cancellation: Arc<CancellationState>,
-    handle: Arc<ActionServerGoalHandle>,
-    server: Arc<ActionServerHandle>,
+    cancellation: Arc<CancellationState<A>>,
+    handle: Arc<ActionServerGoalHandle<A>>,
+    server: Arc<ActionServerHandle<A>>,
 }
 
-impl<A: ActionImpl> LiveActionServerGoal<A> {
+impl<A: Action> LiveActionServerGoal<A> {
     pub(super) fn new(
-        handle: Arc<ActionServerGoalHandle>,
-        server: Arc<ActionServerHandle>,
+        handle: Arc<ActionServerGoalHandle<A>>,
+        server: Arc<ActionServerHandle<A>>,
         goal_request: Arc<A::Goal>,
     ) -> Self {
         Self {
             handle,
             server,
             goal_request,
-            result_response: Mutex::new(ResponseState::new()),
             cancellation: Default::default(),
         }
     }
@@ -53,10 +52,14 @@ impl<A: ActionImpl> LiveActionServerGoal<A> {
         self.cancellation.cancel_requested()
     }
 
-    pub(super) fn cancellation(&self) -> &Arc<CancellationState> {
+    pub(super) fn cancellation(&self) -> &Arc<CancellationState<A>> {
         &self.cancellation
     }
 
+    pub(super) fn request_cancellation(&self, request: CancellationRequest<A>) {
+        self.cancellation.request_cancellation(request, self.goal_id());
+    }
+
     /// Indicate that the goal is being cancelled.
     ///
     /// This is called when a cancel request for the goal has been accepted.
@@ -169,7 +172,7 @@ impl<A: ActionImpl> LiveActionServerGoal<A> {
     /// Returns an error if the goal is in any state other than executing.
     pub(super) fn publish_feedback(&self, feedback: &A::Feedback) {
         let feedback_rmw = <<A as Action>::Feedback as Message>::into_rmw_message(Cow::Borrowed(feedback));
-        let mut feedback_msg = <A as ActionImpl>::create_feedback_message(&*self.goal_id(), feedback_rmw.into_owned());
+        let mut feedback_msg = <A as Action>::create_feedback_message(&*self.goal_id(), feedback_rmw.into_owned());
         let r = unsafe {
             // SAFETY: The action server is locked through the handle, meaning that no other
             // non-thread-safe functions can be called on it at the same time. The feedback_msg is
@@ -198,10 +201,8 @@ impl<A: ActionImpl> LiveActionServerGoal<A> {
         result: &A::Result,
     ) -> Result<(), RclrsError> {
         let result_rmw = <A::Result as Message>::into_rmw_message(Cow::Borrowed(result)).into_owned();
-        let response_rmw = <A as ActionImpl>::create_result_response(status as i8, result_rmw);
-
-        // Publish the result to anyone listening.
-        self.result_response.lock().unwrap().provide_result(&self.server, self.goal_id(), response_rmw);
+        let response_rmw = <A as Action>::create_result_response(status as i8, result_rmw);
+        self.handle.provide_result(self.server.as_ref(), response_rmw)?;
 
         // Publish the state change.
         self.server.publish_status();
@@ -224,15 +225,15 @@ impl<A: ActionImpl> LiveActionServerGoal<A> {
     }
 }
 
-impl<A: ActionImpl> Deref for LiveActionServerGoal<A> {
-    type Target = ActionServerGoalHandle;
+impl<A: Action> Deref for LiveActionServerGoal<A> {
+    type Target = ActionServerGoalHandle<A>;
 
     fn deref(&self) -> &Self::Target {
         self.handle.as_ref()
     }
 }
 
-impl<A: ActionImpl> Drop for LiveActionServerGoal<A> {
+impl<A: Action> Drop for LiveActionServerGoal<A> {
     fn drop(&mut self) {
         match self.get_status() {
             GoalStatus::Accepted => {
@@ -258,91 +259,4 @@ impl<A: ActionImpl> Drop for LiveActionServerGoal<A> {
     }
 }
 
-pub(super) type ActionResponseRmw<A> = <<<A as ActionImpl>::GetResultService as Service>::Response as Message>::RmwMsg;
-
-/// Manages the state of a goal's response.
-enum ResponseState<A: ActionImpl> {
-    /// The response has not arrived yet. There may be some clients waiting for
-    /// the response, and they'll be listed here.
-    Waiting(Vec<rmw_request_id_t>),
-    /// The response is available.
-    Available(ActionResponseRmw<A>),
-}
-
-impl<A: ActionImpl> ResponseState<A> {
-    fn new() -> Self {
-        Self::Waiting(Vec::new())
-    }
-
-    fn provide_result(
-        &mut self,
-        action_server_handle: &ActionServerHandle,
-        goal_id: &GoalUuid,
-        mut result: ActionResponseRmw<A>,
-    ) -> Result<(), RclrsError> {
-        let result_requests = match self {
-            Self::Waiting(waiting) => waiting,
-            Self::Available(previous) => {
-                log_error!(
-                    "action_server_goal_handle.provide_result",
-                    "Action goal {goal_id} was provided with multiple results, \
-                    which is not allowed by the action server state machine and \
-                    indicates a bug in rclrs. The new result will be discarded.\
-                    \nPrevious result: {previous:?}\
-                    \nNew result: {result:?}"
-                );
-                return Err(RclrsError::RclError {
-                    code: RclReturnCode::ActionGoalEventInvalid,
-                    msg: Some(RclErrorMsg("action goal response is already set".to_string())),
-                });
-            }
-        };
-
-        if !result_requests.is_empty() {
-            let action_server = action_server_handle.lock();
-
-            // Respond to all queued requests.
-            for mut result_request in result_requests {
-                Self::send_result(&*action_server, &mut result_request, &mut result)?;
-            }
-        }
-
-        *self = Self::Available(result);
-        Ok(())
-    }
-
-    fn add_result_request(
-        &mut self,
-        action_server_handle: &ActionServerHandle,
-        mut result_request: rmw_request_id_t,
-    ) -> Result<(), RclrsError> {
-        match self {
-            Self::Waiting(waiting) => {
-                waiting.push(result_request);
-            }
-            Self::Available(result) => {
-                let action_server = action_server_handle.lock();
-                Self::send_result(&*action_server, &mut result_request, result)?;
-            }
-        }
-        Ok(())
-    }
-
-    fn send_result(
-        action_server: &rcl_action_server_t,
-        result_request: &mut rmw_request_id_t,
-        result_response: &mut ActionResponseRmw<A>,
-    ) -> Result<(), RclrsError> {
-        unsafe {
-            // SAFETY: The action server handle is kept valid by the
-            // ActionServerHandle. The compiler ensures we have unique access
-            // to the result_request and result_response structures.
-            rcl_action_send_result_response(
-                action_server,
-                result_request,
-                result_response as *mut _ as *mut _,
-            )
-            .ok()
-        }
-    }
-}
+pub(super) type ActionResponseRmw<A> = <<<A as Action>::GetResultService as Service>::Response as Message>::RmwMsg;
diff --git a/rclrs/src/action/action_server/requested_goal.rs b/rclrs/src/action/action_server/requested_goal.rs
index 8834bf96..06965bd1 100644
--- a/rclrs/src/action/action_server/requested_goal.rs
+++ b/rclrs/src/action/action_server/requested_goal.rs
@@ -1,18 +1,18 @@
 use crate::{
     rcl_bindings::*,
     log_error,
-    GoalUuid, RclrsError, ToResult, ActionServerGoalBoard,
+    GoalUuid, RclrsError, RclrsErrorFilter, ToResult, ActionServerGoalBoard,
 };
 use super::{ActionServerGoalHandle, LiveActionServerGoal, AcceptedGoal, TerminatedGoal};
 use std::sync::Arc;
-use rosidl_runtime_rs::ActionImpl;
+use rosidl_runtime_rs::Action;
 
 #[derive(Debug, Clone)]
 pub struct GoalAcceptanceError;
 
 /// An action goal that has been requested but not accepted yet. If this is
 /// dropped without being accepted then the goal request will be rejected.
-pub struct RequestedGoal<A: ActionImpl> {
+pub struct RequestedGoal<A: Action> {
     board: Arc<ActionServerGoalBoard<A>>,
     goal_request: Arc<A::Goal>,
     uuid: GoalUuid,
@@ -20,7 +20,7 @@ pub struct RequestedGoal<A: ActionImpl> {
     accepted: bool,
 }
 
-impl<A: ActionImpl> RequestedGoal<A> {
+impl<A: Action> RequestedGoal<A> {
     /// Get the goal of this action.
     pub fn goal(&self) -> &Arc<A::Goal> {
         &self.goal_request
@@ -106,7 +106,7 @@ impl<A: ActionImpl> RequestedGoal<A> {
 
     fn send_goal_response(&mut self) -> Result<(), RclrsError> {
         let stamp = self.board.node().get_clock().now().to_sec_nanosec().unwrap_or((0, 0));
-        let mut response_rmw = <A as ActionImpl>::create_goal_response(self.accepted, stamp);
+        let mut response_rmw = <A as Action>::create_goal_response(self.accepted, stamp);
         unsafe {
             rcl_action_send_goal_response(
                 &*self.board.handle.lock(),
@@ -115,6 +115,7 @@ impl<A: ActionImpl> RequestedGoal<A> {
             )
         }
         .ok()
+        .timeout_ok()
     }
 
     pub(super) fn new(
@@ -133,7 +134,7 @@ impl<A: ActionImpl> RequestedGoal<A> {
     }
 }
 
-impl<A: ActionImpl> Drop for RequestedGoal<A> {
+impl<A: Action> Drop for RequestedGoal<A> {
     fn drop(&mut self) {
         if !self.accepted {
             // We should notify that the goal has been rejected.
diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs
index 731c2bf8..78a68261 100644
--- a/rclrs/src/error.rs
+++ b/rclrs/src/error.rs
@@ -2,6 +2,7 @@ use std::{
     error::Error,
     ffi::{CStr, NulError},
     fmt::{self, Display},
+    sync::PoisonError,
 };
 
 use crate::{rcl_bindings::*, DeclarationError, ReadyKind};
@@ -193,6 +194,7 @@ impl Error for RclrsError {
             RclrsError::ParameterDeclarationError(_) => None,
             RclrsError::PoisonedMutex => None,
             RclrsError::InvalidReadyInformation { .. } => None,
+            RclrsError::GoalAcceptanceError => None,
         }
     }
 }
@@ -320,6 +322,12 @@ impl From<DeclarationError> for RclrsError {
     }
 }
 
+impl<T> From<PoisonError<T>> for RclrsError {
+    fn from(_: PoisonError<T>) -> Self {
+        RclrsError::PoisonedMutex
+    }
+}
+
 impl TryFrom<i32> for RclReturnCode {
     type Error = i32;
 
diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index 5b604a0e..924d7d1f 100644
--- a/rclrs/src/node.rs
+++ b/rclrs/src/node.rs
@@ -382,7 +382,7 @@ impl NodeState {
         handle_accepted: AcceptedCallback,
     ) -> Result<ActionServer<ActionT>, RclrsError>
     where
-        ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl,
+        ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::Action,
         GoalCallback: Fn(GoalUuid, <ActionT as rosidl_runtime_rs::Action>::Goal) -> GoalResponse + 'static + Send + Sync,
         CancelCallback: Fn(Arc<ServerGoalHandle<ActionT>>) -> CancelResponse + 'static + Send + Sync,
         AcceptedCallback: Fn(Arc<ServerGoalHandle<ActionT>>) + 'static + Send + Sync,

From cd7fd50168534c5eb0b3eed200fb2ac9cc70af04 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Mon, 28 Jul 2025 23:17:10 +0800
Subject: [PATCH 09/20] Creating action server and action goal receiver --
 needs testing

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/src/action.rs                           |  10 +-
 rclrs/src/action/action_goal_receiver.rs      |  74 +++++++++++
 rclrs/src/action/action_server.rs             | 123 ++++++++++++++++--
 .../action_server/live_action_server_goal.rs  |   2 -
 rclrs/src/node.rs                             |  66 ++++------
 5 files changed, 220 insertions(+), 55 deletions(-)
 create mode 100644 rclrs/src/action/action_goal_receiver.rs

diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs
index b046364e..aa922fea 100644
--- a/rclrs/src/action.rs
+++ b/rclrs/src/action.rs
@@ -1,15 +1,17 @@
 use std::ops::Deref;
 
 pub(crate) mod action_client;
+pub use action_client::*;
+
+pub(crate) mod action_goal_receiver;
+pub use action_goal_receiver::*;
+
 pub(crate) mod action_server;
-mod action_server_goal_handle;
+pub use action_server::*;
 
 use crate::rcl_bindings::RCL_ACTION_UUID_SIZE;
 use std::fmt;
 
-pub use action_client::*;
-pub use action_server::*;
-use action_server_goal_handle::*;
 
 /// A unique identifier for a goal request.
 #[derive(Copy, Clone, Debug, Default, PartialEq, Eq, PartialOrd, Ord, Hash)]
diff --git a/rclrs/src/action/action_goal_receiver.rs b/rclrs/src/action/action_goal_receiver.rs
new file mode 100644
index 00000000..0e6ce947
--- /dev/null
+++ b/rclrs/src/action/action_goal_receiver.rs
@@ -0,0 +1,74 @@
+use crate::{
+    ActionServer, ActionServerOptions, ActionServerState, RequestedGoal, TerminatedGoal,
+    Node, RclrsError,
+};
+use rosidl_runtime_rs::Action;
+use std::{
+    future::Future,
+    ops::{Deref, DerefMut},
+    sync::Arc,
+};
+use tokio::sync::mpsc::{unbounded_channel, UnboundedReceiver};
+
+/// This is an alternative tool for implementing an [`ActionServer`]. Instead of
+/// specifying a callback to receive goal requests, you can use this receiver to
+/// obtain incoming goal requests and then process them in an async task.
+///
+/// The [`ActionGoalReceiver`] may have some advantages over the [`ActionServer`]
+/// for action servers that support multiple simultaneous goals which may interact
+/// with each other and therefore need to be processed within the same async task.
+//
+// TODO(@mxgrey): Add usage examples here.
+pub struct ActionGoalReceiver<A: Action> {
+    server: ActionServerState<A>,
+    receiver: UnboundedReceiver<RequestedGoal<A>>,
+}
+
+impl<A: Action> Deref for ActionGoalReceiver<A> {
+    type Target = UnboundedReceiver<RequestedGoal<A>>;
+
+    fn deref(&self) -> &Self::Target {
+        &self.receiver
+    }
+}
+
+impl<A: Action> DerefMut for ActionGoalReceiver<A> {
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        &mut self.receiver
+    }
+}
+
+impl<A: Action> ActionGoalReceiver<A> {
+    /// Change this action goal receiver into a callback-based action server.
+    ///
+    /// It is unusual to switch from an action goal receiver to an action server,
+    /// so consider carefully whether this is what you really want to do. Usually
+    /// an action server is created by [`NodeState::create_action_server`].
+    #[must_use]
+    pub fn into_action_server<Task>(
+        self,
+        callback: impl FnMut(RequestedGoal<A>) -> Task + Send + Sync + 'static,
+    ) -> ActionServer<A>
+    where
+        Task: Future<Output = TerminatedGoal> + Send + Sync + 'static,
+    {
+        let Self { server, receiver } = self;
+        server.drain_receiver_into_callback(receiver, callback);
+        Arc::new(server)
+    }
+
+    pub(crate) fn new<'a>(
+        node: &Node,
+        options: impl Into<ActionServerOptions<'a>>,
+    ) -> Result<Self, RclrsError> {
+        let (sender, receiver) = unbounded_channel();
+        let server = ActionServerState::new_for_receiver(node, options, sender)?;
+        Ok(Self { server, receiver })
+    }
+
+    pub(super) fn from_server(server: ActionServerState<A>) -> Self {
+        let (sender, receiver) = unbounded_channel();
+        server.set_goal_sender(sender);
+        Self { server, receiver }
+    }
+}
diff --git a/rclrs/src/action/action_server.rs b/rclrs/src/action/action_server.rs
index 7388b3eb..9bce4765 100644
--- a/rclrs/src/action/action_server.rs
+++ b/rclrs/src/action/action_server.rs
@@ -2,7 +2,7 @@ use crate::{
     action::GoalUuid,
     error::ToResult,
     rcl_bindings::*,
-    DropGuard, Node, NodeHandle, QoSProfile, RclPrimitive, RclrsError,
+    ActionGoalReceiver, DropGuard, Node, NodeHandle, QoSProfile, RclPrimitive, RclrsError,
     RclPrimitiveHandle, RclPrimitiveKind, ReadyKind, Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
 };
 use rosidl_runtime_rs::{Action, Message, RmwGoalRequest, RmwResultRequest};
@@ -11,11 +11,11 @@ use std::{
     borrow::Borrow,
     collections::HashMap,
     ffi::CString,
-    panic::UnwindSafe,
+    future::Future,
     sync::{Arc, Mutex, MutexGuard, Weak},
 };
 use futures::future::BoxFuture;
-use tokio::sync::mpsc::UnboundedSender;
+use tokio::sync::mpsc::{UnboundedSender, UnboundedReceiver};
 
 mod accepted_goal;
 pub use accepted_goal::*;
@@ -85,7 +85,7 @@ impl<'a, T: Borrow<str> + ?Sized + 'a> From<&'a T> for ActionServerOptions<'a> {
 
 /// An action server that can respond to requests sent by ROS action clients.
 ///
-/// Create an action server using [`Node::create_action_server`][1].
+/// Create an action server using [`NodeState::create_action_server`][1].
 ///
 /// ROS only supports having one server for any given fully-qualified
 /// action name. "Fully-qualified" means the namespace is also taken into account
@@ -95,8 +95,11 @@ impl<'a, T: Borrow<str> + ?Sized + 'a> From<&'a T> for ActionServerOptions<'a> {
 ///
 /// Responding to requests requires the node's executor to [spin][2].
 ///
+/// You may also consider using [`ActionGoalReceiver`] to implement your action
+/// server. It provides different ergonomics which may be useful in some situations.
+///
 /// [1]: crate::NodeState::create_action_server
-/// [2]: crate::spin
+/// [2]: crate::Executor::spin
 pub type ActionServer<A> = Arc<ActionServerState<A>>;
 
 /// The inner state of an [`ActionServer`].
@@ -109,7 +112,7 @@ pub type ActionServer<A> = Arc<ActionServerState<A>>;
 /// The public API of the [`ActionServer`] type is implemented via `ActionServerState`.
 ///
 /// [1]: std::sync::Weak
-struct ActionServerState<A: Action> {
+pub struct ActionServerState<A: Action> {
     board: Arc<ActionServerGoalBoard<A>>,
 
     /// Holding onto this keeps the waitable for this action server alive in the
@@ -119,8 +122,69 @@ struct ActionServerState<A: Action> {
 }
 
 impl<A: Action> ActionServerState<A> {
+    /// Change the callback for this action server.
+    pub fn set_callback<Task>(
+        &self,
+        mut callback: impl FnMut(RequestedGoal<A>) -> Task + Send + Sync + 'static,
+    )
+    where
+        Task: Future<Output = TerminatedGoal> + Send + Sync + 'static,
+    {
+        let callback = Box::new(move |requested_goal| -> BoxFuture<'static, TerminatedGoal> {
+            Box::pin(callback(requested_goal))
+        });
+
+        let mut dispatch = match self.board.dispatch.lock() {
+            Ok(dispatch) => dispatch,
+            Err(poison) => poison.into_inner(),
+        };
+
+        *dispatch = GoalDispatch::Callback(callback);
+        self.board.dispatch.clear_poison();
+    }
+
+    /// Change this action server into an action goal receiver, which may be more
+    /// ergonomic for some implementations of an action server.
+    ///
+    /// Note that you'll need to obtain a uniquely owned instance of the
+    /// [`ActionServerState`] to do this conversion. If you have an [`ActionServer`]
+    /// (which is managed by an [`Arc`]) then you will need to use [`Arc::into_inner`]
+    /// to obtain the unique [`ActionServerState`].
+    ///
+    /// It is unusual to switch from an action server to an action goal receiver,
+    /// so consider carefully whether this is what you really want to do. Usually
+    /// an action goal receiver is created by [`NodeState::create_action_goal_receiver`]
+    /// when the action server is being initialized.
+    #[must_use]
+    pub fn into_goal_receiver(self) -> ActionGoalReceiver<A> {
+        ActionGoalReceiver::from_server(self)
+    }
+
+    pub(crate) fn create<'a, Task>(
+        node: &Node,
+        options: impl Into<ActionServerOptions<'a>>,
+        mut callback: impl FnMut(RequestedGoal<A>) -> Task + Send + Sync + 'static,
+    ) -> Result<ActionServer<A>, RclrsError>
+    where
+        Task: Future<Output = TerminatedGoal> + Send + Sync + 'static,
+    {
+        let callback = Box::new(move |requested_goal| -> BoxFuture<'static, TerminatedGoal> {
+            Box::pin(callback(requested_goal))
+        });
+
+        Ok(Arc::new(Self::new(node, options, GoalDispatch::Callback(callback))?))
+    }
+
+    pub(super) fn new_for_receiver<'a>(
+        node: &Node,
+        options: impl Into<ActionServerOptions<'a>>,
+        sender: UnboundedSender<RequestedGoal<A>>,
+    ) -> Result<Self, RclrsError> {
+        Self::new(node, options, GoalDispatch::Sender(sender))
+    }
+
     /// Creates a new action server.
-    pub(crate) fn create<'a>(
+    fn new<'a>(
         node: &Node,
         options: impl Into<ActionServerOptions<'a>>,
         dispatch: GoalDispatch<A>,
@@ -184,6 +248,49 @@ impl<A: Action> ActionServerState<A> {
 
         Ok(Self { board, lifecycle })
     }
+
+    pub(super) fn set_goal_sender(&self, sender: UnboundedSender<RequestedGoal<A>>) {
+        let mut dispatch = match self.board.dispatch.lock() {
+            Ok(dispatch) => dispatch,
+            Err(poison) => poison.into_inner(),
+        };
+
+        *dispatch = GoalDispatch::Sender(sender);
+        self.board.dispatch.clear_poison();
+    }
+
+    /// Used internally to change a receiver into an action server without the
+    /// risk of dropping buffered any goal requests or receiving goals out of
+    /// their original order.
+    pub(super) fn drain_receiver_into_callback<Task>(
+        &self,
+        mut receiver: UnboundedReceiver<RequestedGoal<A>>,
+        mut callback: impl FnMut(RequestedGoal<A>) -> Task + Send + Sync + 'static,
+    )
+    where
+        Task: Future<Output = TerminatedGoal> + Send + Sync + 'static,
+    {
+        let mut callback = Box::new(move |requested_goal| -> BoxFuture<'static, TerminatedGoal> {
+            Box::pin(callback(requested_goal))
+        });
+
+        let mut dispatch = match self.board.dispatch.lock() {
+            Ok(dispatch) => dispatch,
+            Err(poison) => poison.into_inner(),
+        };
+
+        // The dispatch sender is blocked by the mutex, so once we finish draining
+        // the current values in the receiver, there will never be any more values.
+        // By the time we unlock the dispatch mutex, the sender will be dropped,
+        // replaced by the callback.
+        while let Ok(requested_goal) = receiver.try_recv() {
+            let f = (*callback)(requested_goal);
+            let _ = self.board.node.commands().run(f);
+        }
+
+        *dispatch = GoalDispatch::Callback(callback);
+        self.board.dispatch.clear_poison();
+    }
 }
 
 pub struct ActionServerGoalBoard<A: Action> {
@@ -566,7 +673,7 @@ impl<A: Action> ActionServerHandle<A> {
 }
 
 enum GoalDispatch<A: Action> {
-    Callback(Box<dyn FnMut(RequestedGoal<A>) -> BoxFuture<'static, TerminatedGoal> + Send + Sync + UnwindSafe>),
+    Callback(Box<dyn FnMut(RequestedGoal<A>) -> BoxFuture<'static, TerminatedGoal> + Send + Sync>),
     Sender(UnboundedSender<RequestedGoal<A>>),
 }
 
diff --git a/rclrs/src/action/action_server/live_action_server_goal.rs b/rclrs/src/action/action_server/live_action_server_goal.rs
index 7b8f9702..48fc680f 100644
--- a/rclrs/src/action/action_server/live_action_server_goal.rs
+++ b/rclrs/src/action/action_server/live_action_server_goal.rs
@@ -258,5 +258,3 @@ impl<A: Action> Drop for LiveActionServerGoal<A> {
         }
     }
 }
-
-pub(super) type ActionResponseRmw<A> = <<<A as Action>::GetResultService as Service>::Response as Message>::RmwMsg;
diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index 924d7d1f..6ead5da6 100644
--- a/rclrs/src/node.rs
+++ b/rclrs/src/node.rs
@@ -14,6 +14,7 @@ use std::{
     cmp::PartialEq,
     ffi::CStr,
     fmt,
+    future::Future,
     os::raw::c_char,
     sync::{atomic::AtomicBool, Arc, Mutex},
     time::Duration,
@@ -26,17 +27,17 @@ use futures::{
 
 use async_std::future::timeout;
 
-use rosidl_runtime_rs::Message;
+use rosidl_runtime_rs::{Message, Action};
 
 use crate::{
-    rcl_bindings::*, ActionClient, ActionClientOptions, ActionClientState,
+    rcl_bindings::*, ActionClient, ActionClientOptions, ActionClientState, ActionGoalReceiver,
     ActionServer, ActionServerOptions, ActionServerState, Client, ClientOptions,
     ClientState, Clock, ContextHandle, ExecutorCommands, IntoAsyncServiceCallback,
     IntoAsyncSubscriptionCallback, IntoNodeServiceCallback, IntoNodeSubscriptionCallback,
     LogParams, Logger, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters,
-    Promise, Publisher, PublisherOptions, PublisherState, RclrsError, Service,
+    Promise, Publisher, PublisherOptions, PublisherState, RclrsError, RequestedGoal, Service,
     ServiceOptions, ServiceState, Subscription, SubscriptionOptions, SubscriptionState,
-    TimeSource, ToLogParams, Worker, WorkerOptions, WorkerState, ENTITY_LIFECYCLE_MUTEX,
+    TerminatedGoal, TimeSource, ToLogParams, Worker, WorkerOptions, WorkerState, ENTITY_LIFECYCLE_MUTEX,
 };
 
 /// A processing unit that can communicate with other nodes. See the API of
@@ -355,53 +356,36 @@ impl NodeState {
     ///
     /// [1]: crate::ActionClient
     // TODO: make action client's lifetime depend on node's lifetime
-    pub fn create_action_client<'a, T>(
+    pub fn create_action_client<'a, A: Action>(
         self: &Arc<Self>,
         options: impl Into<ActionClientOptions<'a>>,
-    ) -> Result<ActionClient<T>, RclrsError>
-    where
-        T: rosidl_runtime_rs::Action,
-    {
-        let action_client = Arc::new(ActionClientState::<T>::new(self, options)?);
-        self.action_clients_mtx
-            .lock()
-            .unwrap()
-            .push(Arc::downgrade(&action_client) as Weak<dyn ActionClientBase>);
-        Ok(action_client)
+    ) -> Result<ActionClient<A>, RclrsError> {
+        todo!();
     }
 
-    /// Creates an [`ActionServer`][1].
-    ///
-    /// [1]: crate::ActionServer
-    // TODO: make action server's lifetime depend on node's lifetime
-    pub fn create_action_server<'a, ActionT, GoalCallback, CancelCallback, AcceptedCallback>(
+    /// Creates an [`ActionServer`].
+    //
+    // TODO(@mxgrey): Add extensive documentation and usage examples
+    pub fn create_action_server<'a, A: Action, Task>(
         self: &Arc<Self>,
         options: impl Into<ActionServerOptions<'a>>,
-        handle_goal: GoalCallback,
-        handle_cancel: CancelCallback,
-        handle_accepted: AcceptedCallback,
-    ) -> Result<ActionServer<ActionT>, RclrsError>
+        callback: impl FnMut(RequestedGoal<A>) -> Task + Send + Sync + 'static,
+    ) -> Result<ActionServer<A>, RclrsError>
     where
-        ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::Action,
-        GoalCallback: Fn(GoalUuid, <ActionT as rosidl_runtime_rs::Action>::Goal) -> GoalResponse + 'static + Send + Sync,
-        CancelCallback: Fn(Arc<ServerGoalHandle<ActionT>>) -> CancelResponse + 'static + Send + Sync,
-        AcceptedCallback: Fn(Arc<ServerGoalHandle<ActionT>>) + 'static + Send + Sync,
+        Task: Future<Output = TerminatedGoal> + Send + Sync + 'static,
     {
-        let action_server = Arc::new(ActionServerState::<ActionT>::new(
-            self,
-            options,
-            handle_goal,
-            handle_cancel,
-            handle_accepted,
-        )?);
-        self.action_servers_mtx
-            .lock()
-            .unwrap()
-            .push(Arc::downgrade(&action_server) as Weak<dyn ActionServerBase>);
-        Ok(action_server)
+        ActionServerState::create(self, options, callback)
     }
 
-
+    /// Creates an [`ActionGoalReceiver`].
+    //
+    // TODO(@mxgrey): Add extensive documentation and usage examples
+    pub fn create_goal_receiver<'a, A: Action>(
+        self: &Arc<Self>,
+        options: impl Into<ActionServerOptions<'a>>,
+    ) -> Result<ActionGoalReceiver<A>, RclrsError> {
+        ActionGoalReceiver::new(self, options)
+    }
 
     /// Creates a [`Publisher`].
     ///

From 01ab153742c5034450324d89d6c5dc0f557c0179 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Tue, 29 Jul 2025 00:06:15 +0800
Subject: [PATCH 10/20] Introducing action clients into the wait set processing

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/src/action/action_client.rs   | 114 ++++++++++-------------
 rclrs/src/wait_set/rcl_primitive.rs | 111 +++++++++++++++++++++--
 rclrs/src/wait_set/waitable.rs      | 136 +++++++++++++++++++---------
 3 files changed, 248 insertions(+), 113 deletions(-)

diff --git a/rclrs/src/action/action_client.rs b/rclrs/src/action/action_client.rs
index 1bb44444..b0a49345 100644
--- a/rclrs/src/action/action_client.rs
+++ b/rclrs/src/action/action_client.rs
@@ -8,10 +8,48 @@ use std::{
     marker::PhantomData,
     sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard},
 };
+use rosidl_runtime_rs::Action;
 
-// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
-// they are running in. Therefore, this type can be safely sent to another thread.
-unsafe impl Send for rcl_action_client_t {}
+/// `ActionClientOptions` are used by [`Node::create_action_client`][1] to initialize an
+/// [`ActionClient`].
+///
+/// [1]: crate::Node::create_action_client
+#[derive(Debug, Clone)]
+#[non_exhaustive]
+pub struct ActionClientOptions<'a> {
+    /// The name of the action that this client will send requests to
+    pub action_name: &'a str,
+    /// The quality of service profile for the goal service
+    pub goal_service_qos: QoSProfile,
+    /// The quality of service profile for the result service
+    pub result_service_qos: QoSProfile,
+    /// The quality of service profile for the cancel service
+    pub cancel_service_qos: QoSProfile,
+    /// The quality of service profile for the feedback topic
+    pub feedback_topic_qos: QoSProfile,
+    /// The quality of service profile for the status topic
+    pub status_topic_qos: QoSProfile,
+}
+
+impl<'a> ActionClientOptions<'a> {
+    /// Initialize a new [`ActionClientOptions`] with default settings.
+    pub fn new(action_name: &'a str) -> Self {
+        Self {
+            action_name,
+            goal_service_qos: QoSProfile::services_default(),
+            result_service_qos: QoSProfile::services_default(),
+            cancel_service_qos: QoSProfile::services_default(),
+            feedback_topic_qos: QoSProfile::topics_default(),
+            status_topic_qos: QoSProfile::action_status_default(),
+        }
+    }
+}
+
+impl<'a, T: Borrow<str> + ?Sized + 'a> From<&'a T> for ActionClientOptions<'a> {
+    fn from(value: &'a T) -> Self {
+        Self::new(value.borrow())
+    }
+}
 
 /// Manage the lifecycle of an `rcl_action_client_t`, including managing its dependencies
 /// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are
@@ -90,7 +128,6 @@ where
 {
     _marker: PhantomData<fn() -> ActionT>,
     pub(crate) handle: Arc<ActionClientHandle>,
-    num_entities: WaitableNumEntities,
     /// Ensure the parent node remains alive as long as the subscription is held.
     /// This implementation will change in the future.
     #[allow(unused)]
@@ -147,7 +184,7 @@ where
 
         let handle = Arc::new(ActionClientHandle {
             rcl_action_client: Mutex::new(rcl_action_client),
-            node_handle: Arc::clone(&node.handle),
+            node_handle: Arc::clone(&node.handle()),
             in_use_by_wait_set: Arc::new(AtomicBool::new(false)),
         });
 
@@ -167,7 +204,6 @@ where
         Ok(Self {
             _marker: Default::default(),
             handle,
-            num_entities,
             node: Arc::clone(node),
         })
     }
@@ -193,66 +229,16 @@ where
     }
 }
 
-impl<T> ActionClientBase for ActionClientState<T>
-where
-    T: rosidl_runtime_rs::Action,
-{
-    fn handle(&self) -> &ActionClientHandle {
-        &self.handle
-    }
-
-    fn num_entities(&self) -> &WaitableNumEntities {
-        &self.num_entities
-    }
-
-    fn execute(&self, mode: ReadyMode) -> Result<(), RclrsError> {
-        match mode {
-            ReadyMode::Feedback => self.execute_feedback(),
-            ReadyMode::Status => self.execute_status(),
-            ReadyMode::GoalResponse => self.execute_goal_response(),
-            ReadyMode::CancelResponse => self.execute_cancel_response(),
-            ReadyMode::ResultResponse => self.execute_result_response(),
-        }
-    }
+struct ActionClientGoalBoard<A: Action> {
+    _ignore: std::marker::PhantomData<fn(A)>,
 }
 
-/// `ActionClientOptions` are used by [`Node::create_action_client`][1] to initialize an
-/// [`ActionClient`].
-///
-/// [1]: crate::Node::create_action_client
-#[derive(Debug, Clone)]
-#[non_exhaustive]
-pub struct ActionClientOptions<'a> {
-    /// The name of the action that this client will send requests to
-    pub action_name: &'a str,
-    /// The quality of service profile for the goal service
-    pub goal_service_qos: QoSProfile,
-    /// The quality of service profile for the result service
-    pub result_service_qos: QoSProfile,
-    /// The quality of service profile for the cancel service
-    pub cancel_service_qos: QoSProfile,
-    /// The quality of service profile for the feedback topic
-    pub feedback_topic_qos: QoSProfile,
-    /// The quality of service profile for the status topic
-    pub status_topic_qos: QoSProfile,
+struct ActionClientExecutable<A: Action> {
+    board: Arc<ActionClientGoalBoard<A>>,
 }
 
-impl<'a> ActionClientOptions<'a> {
-    /// Initialize a new [`ActionClientOptions`] with default settings.
-    pub fn new(action_name: &'a str) -> Self {
-        Self {
-            action_name,
-            goal_service_qos: QoSProfile::services_default(),
-            result_service_qos: QoSProfile::services_default(),
-            cancel_service_qos: QoSProfile::services_default(),
-            feedback_topic_qos: QoSProfile::topics_default(),
-            status_topic_qos: QoSProfile::action_status_default(),
-        }
-    }
-}
 
-impl<'a, T: Borrow<str> + ?Sized + 'a> From<&'a T> for ActionClientOptions<'a> {
-    fn from(value: &'a T) -> Self {
-        Self::new(value.borrow())
-    }
-}
+
+// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
+// they are running in. Therefore, this type can be safely sent to another thread.
+unsafe impl Send for rcl_action_client_t {}
diff --git a/rclrs/src/wait_set/rcl_primitive.rs b/rclrs/src/wait_set/rcl_primitive.rs
index 399c95a6..393d23b5 100644
--- a/rclrs/src/wait_set/rcl_primitive.rs
+++ b/rclrs/src/wait_set/rcl_primitive.rs
@@ -48,6 +48,8 @@ pub enum RclPrimitiveKind {
     Event,
     /// Action Server
     ActionServer,
+    /// Action Client
+    ActionClient,
 }
 
 /// Used by the wait set to obtain the handle of a primitive.
@@ -67,6 +69,8 @@ pub enum RclPrimitiveHandle<'a> {
     Event(MutexGuard<'a, rcl_event_t>),
     /// Handle for an action server
     ActionServer(MutexGuard<'a, rcl_action_server_t>),
+    /// Handle for an action client
+    ActionClient(MutexGuard<'a, rcl_action_client_t>),
 }
 
 /// Describe the way in which a waitable is ready.
@@ -79,6 +83,10 @@ pub enum ReadyKind {
     /// multiple primitives. Any combination of those primitives might be ready,
     /// and we need to know which to execute specifically.
     ActionServer(ActionServerReady),
+    /// This type of readiness is specific to action clients, which consist of
+    /// multiple primitives. Any combination of those primitives might be ready,
+    /// and we need to know which to execute specifically.
+    ActionClient(ActionClientReady),
 }
 
 impl ReadyKind {
@@ -103,6 +111,7 @@ impl ReadyKind {
         }
     }
 
+    /// This is used by action servers to get their readiness information.
     pub fn for_action_server(self) -> Result<ActionServerReady, RclrsError> {
         match self {
             Self::ActionServer(ready) => Ok(ready),
@@ -112,6 +121,17 @@ impl ReadyKind {
             })
         }
     }
+
+    /// This is used by action clients to get their readiness information.
+    pub fn for_action_client(self) -> Result<ActionClientReady, RclrsError> {
+        match self {
+            Self::ActionClient(ready) => Ok(ready),
+            _ => Err(RclrsError::InvalidReadyInformation {
+                expected: Self::ActionClient(Default::default()),
+                received: self,
+            })
+        }
+    }
 }
 
 /// This is the ready information for an action server.
@@ -155,25 +175,25 @@ impl ActionServerReady {
         action_server: MutexGuard<rcl_action_server_t>,
     ) -> Option<ReadyKind> {
         let mut ready = ActionServerReady::default();
-        let r;
-        unsafe {
+        let r = unsafe {
             // SAFETY: We give a safety warning to ensure that the wait set is
             // not being used elsewhere. The action server handle is guarded by
             // the mutex. With those two requirements met, we do not need to
             // worry about this function not being thread-safe.
-            r = rcl_action_server_wait_set_get_entities_ready(
+            rcl_action_server_wait_set_get_entities_ready(
                 wait_set,
                 &*action_server,
                 &mut ready.goal_request,
                 &mut ready.cancel_request,
                 &mut ready.result_request,
                 &mut ready.goal_expired,
-            );
-        }
+            )
+        };
+
         if let Err(err) = r.ok() {
             log_error!(
                 "ActionServerReady.check",
-                "Error while checking action server: {err}",
+                "Error while checking readiness for action server: {err}",
             );
         }
 
@@ -193,3 +213,82 @@ impl ActionServerReady {
         || self.goal_expired
     }
 }
+
+/// This is the ready information for an action client.
+///
+/// Action clients contain multiple service clients bundled together, as well as
+/// some subscribers. When a wait set wakes up it is possible for any number of
+/// those services or subscriptions to be ready for processing. This struct
+/// conveys which of an action client's messages are ready.
+#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+pub struct ActionClientReady {
+    /// True if there is a feedback message ready to take, false otherwise.
+    pub feedback: bool,
+    /// True if there is a status message ready to take, false otherwise.
+    pub status: bool,
+    /// True if there is a goal response message ready to take, false otherwise.
+    pub goal_response: bool,
+    /// True if there is a cancel response message ready to take, false otherwise.
+    pub cancel_response: bool,
+    /// True if there is a result response message ready to take, false otherwise.
+    pub result_response: bool,
+}
+
+impl ActionClientReady {
+    pub(crate) unsafe fn check(
+        wait_set: &rcl_wait_set_t,
+        action_client: MutexGuard<rcl_action_client_t>,
+    ) -> Option<ReadyKind> {
+        let mut ready = ActionClientReady::default();
+        let r = unsafe {
+            // SAFETY: We give a safety warning to ensure that the wait set is
+            // not being used elsewhere. The action client handle is guarded by
+            // the mutex. With those two requirements met, we do not need to
+            // worry about this function not being thread-safe.
+            rcl_action_client_wait_set_get_entities_ready(
+                wait_set,
+                &*action_client,
+                &mut ready.feedback,
+                &mut ready.status,
+                &mut ready.goal_response,
+                &mut ready.cancel_response,
+                &mut ready.result_response,
+            )
+        };
+
+        if let Err(err) = r.ok() {
+            log_error!(
+                "ActionClientReady.check",
+                "Error while checking readiness for action client: {err}",
+            );
+        }
+
+        if ready.any_ready() {
+            Some(ReadyKind::ActionClient(ready))
+        } else {
+            None
+        }
+    }
+
+    /// Check whether any of the primitives of the action client are ready. When
+    /// this is false, we can skip producing a [`ReadyKind`] entirely.
+    pub fn any_ready(&self) -> bool {
+        self.feedback
+        || self.status
+        || self.goal_response
+        || self.cancel_response
+        || self.result_response
+    }
+}
+
+impl Default for ActionClientReady {
+    fn default() -> Self {
+        Self {
+            feedback: false,
+            status: false,
+            goal_response: false,
+            cancel_response: false,
+            result_response: false,
+        }
+    }
+}
diff --git a/rclrs/src/wait_set/waitable.rs b/rclrs/src/wait_set/waitable.rs
index a94fbbc4..3206992b 100644
--- a/rclrs/src/wait_set/waitable.rs
+++ b/rclrs/src/wait_set/waitable.rs
@@ -4,13 +4,10 @@ use std::sync::{
 };
 
 use crate::{
-    error::ToResult, log_error, rcl_bindings::*, ActionServerReady, GuardCondition,
+    error::ToResult, log_error, rcl_bindings::*, ActionServerReady, ActionClientReady, GuardCondition,
     RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, ReadyKind,
 };
 
-/// How many services are added to a wait set by an action server.
-const SERVICES_PER_ACTION_SERVER: usize = 3;
-
 /// This struct manages the presence of an rcl primitive inside the wait set.
 /// It will keep track of where the primitive is within the wait set as well as
 /// automatically remove the primitive from the wait set once it isn't being
@@ -51,49 +48,94 @@ impl Waitable {
     }
 
     pub(super) fn is_ready(&self, wait_set: &rcl_wait_set_t) -> Option<ReadyKind> {
-        self.index_in_wait_set.and_then(|index| {
-            unsafe {
-                // SAFETY: Each field in the wait set is an array of points.
-                // The dereferencing that we do is equivalent to obtaining the
-                // element of the array at the index-th position.
-                match self.primitive.kind() {
-                    RclPrimitiveKind::Subscription => {
-                        ReadyKind::from_ptr(wait_set.subscriptions.add(index))
-                    },
-                    RclPrimitiveKind::GuardCondition => {
-                        ReadyKind::from_ptr(wait_set.guard_conditions.add(index))
-                    }
-                    RclPrimitiveKind::Service => {
-                        ReadyKind::from_ptr(wait_set.services.add(index))
-                    }
-                    RclPrimitiveKind::Client => {
-                        ReadyKind::from_ptr(wait_set.clients.add(index))
+        match self.primitive.kind() {
+            RclPrimitiveKind::Subscription => {
+                self.index_in_wait_set.and_then(|index| {
+                    // SAFETY: Each field in the wait set is an array of points.
+                    // The dereferencing that we do is equivalent to obtaining the
+                    // element of the array at the index-th position.
+                    ReadyKind::from_ptr(unsafe { wait_set.subscriptions.add(index) })
+                })
+            },
+            RclPrimitiveKind::GuardCondition => {
+                self.index_in_wait_set.and_then(|index| {
+                    // SAFETY: Each field in the wait set is an array of points.
+                    // The dereferencing that we do is equivalent to obtaining the
+                    // element of the array at the index-th position.
+                    ReadyKind::from_ptr(unsafe { wait_set.guard_conditions.add(index) })
+                })
+            }
+            RclPrimitiveKind::Service => {
+                self.index_in_wait_set.and_then(|index| {
+                    // SAFETY: Each field in the wait set is an array of points.
+                    // The dereferencing that we do is equivalent to obtaining the
+                    // element of the array at the index-th position.
+                    ReadyKind::from_ptr(unsafe { wait_set.services.add(index) })
+                })
+            }
+            RclPrimitiveKind::Client => {
+                self.index_in_wait_set.and_then(|index| {
+                    // SAFETY: Each field in the wait set is an array of points.
+                    // The dereferencing that we do is equivalent to obtaining the
+                    // element of the array at the index-th position.
+                    ReadyKind::from_ptr(unsafe { wait_set.clients.add(index) })
+                })
+            }
+            RclPrimitiveKind::Timer => {
+                self.index_in_wait_set.and_then(|index| {
+                    // SAFETY: Each field in the wait set is an array of points.
+                    // The dereferencing that we do is equivalent to obtaining the
+                    // element of the array at the index-th position.
+                    ReadyKind::from_ptr(unsafe { wait_set.timers.add(index) })
+                })
+            }
+            RclPrimitiveKind::Event => {
+                self.index_in_wait_set.and_then(|index| {
+                    // SAFETY: Each field in the wait set is an array of points.
+                    // The dereferencing that we do is equivalent to obtaining the
+                    // element of the array at the index-th position.
+                    ReadyKind::from_ptr(unsafe { wait_set.events.add(index) })
+                })
+            }
+            RclPrimitiveKind::ActionServer => {
+                match self.primitive.handle() {
+                    RclPrimitiveHandle::ActionServer(handle) => {
+                        // SAFETY: We have exclusive ownership of the wait set
+                        // and the action server handle right now, which satisfies
+                        // the safety requirements of the function.
+                        unsafe { ActionServerReady::check(wait_set, handle) }
                     }
-                    RclPrimitiveKind::Timer => {
-                        ReadyKind::from_ptr(wait_set.timers.add(index))
+                    handle => {
+                        log_error!(
+                            "waitable.is_ready",
+                            "Invalid handle for ActionServer type: {handle:?}. \
+                            This indicates a bug in the implementation of rclrs. \
+                            Please report this to the rclrs maintainers.",
+                        );
+                        None
                     }
-                    RclPrimitiveKind::Event => {
-                        ReadyKind::from_ptr(wait_set.events.add(index))
+                }
+            },
+            RclPrimitiveKind::ActionClient => {
+                match self.primitive.handle() {
+                    RclPrimitiveHandle::ActionClient(handle) => {
+                        // SAFETY: We have exclusive ownership of the wait set
+                        // and the action client handle right now, which satisfies
+                        // the safety requirements of the function.
+                        unsafe { ActionClientReady::check(wait_set, handle) }
                     }
-                    RclPrimitiveKind::ActionServer => {
-                        match self.primitive.handle() {
-                            RclPrimitiveHandle::ActionServer(handle) => {
-                                ActionServerReady::check(wait_set, handle)
-                            }
-                            handle => {
-                                log_error!(
-                                    "waitable.is_ready",
-                                    "Invalid handle for ActionServer type: {handle:?}. \
-                                    This indicates a bug in the implementation of rclrs. \
-                                    Please report this to the rclrs maintainers.",
-                                );
-                                None
-                            }
-                        }
+                    handle => {
+                        log_error!(
+                            "waitable.is_ready",
+                            "Invalid handle for ActionClient type: {handle:?}. \
+                            This indicates a bug in the implementation of rclrs. \
+                            Please report this to the rclrs maintainers.",
+                        );
+                        None
                     }
                 }
             }
-        })
+        }
     }
 
     pub(super) fn add_to_wait_set(
@@ -124,7 +166,10 @@ impl Waitable {
                     rcl_wait_set_add_event(wait_set, &*handle, &mut index)
                 }
                 RclPrimitiveHandle::ActionServer(handle) => {
-                    rcl_action_wait_set_add_action_server(wait_set, &*handle, &mut index)
+                    rcl_action_wait_set_add_action_server(wait_set, &*handle, std::ptr::null_mut())
+                }
+                RclPrimitiveHandle::ActionClient(handle) => {
+                    rcl_action_wait_set_add_action_client(wait_set, &*handle, std::ptr::null_mut(), std::ptr::null_mut())
                 }
             }
         }
@@ -184,7 +229,12 @@ impl WaitableCount {
             RclPrimitiveKind::Client => self.clients += count,
             RclPrimitiveKind::Service => self.services += count,
             RclPrimitiveKind::Event => self.events += count,
-            RclPrimitiveKind::ActionServer => self.services += SERVICES_PER_ACTION_SERVER*count,
+            RclPrimitiveKind::ActionServer => {
+                Use rcl_action_server_wait_set_get_num_entities
+            }
+            RclPrimitiveKind::ActionClient => {
+                Use rcl_action_client_wait_set_get_num_entities
+            }
         }
     }
 

From 7d64c9c9a0852c7cd9e10a642bd882e121066506 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Tue, 29 Jul 2025 16:41:04 +0800
Subject: [PATCH 11/20] Incorporate action clients into wait sets

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/src/action/action_client.rs             |  27 +----
 rclrs/src/action/action_server.rs             |   2 +-
 .../action/action_server/requested_goal.rs    |   9 +-
 rclrs/src/wait_set.rs                         |   8 +-
 rclrs/src/wait_set/wait_set_runner.rs         |   2 +-
 rclrs/src/wait_set/waitable.rs                | 106 ++++++++++++++++--
 6 files changed, 107 insertions(+), 47 deletions(-)

diff --git a/rclrs/src/action/action_client.rs b/rclrs/src/action/action_client.rs
index b0a49345..731eadb4 100644
--- a/rclrs/src/action/action_client.rs
+++ b/rclrs/src/action/action_client.rs
@@ -81,18 +81,6 @@ impl Drop for ActionClientHandle {
     }
 }
 
-/// Trait to be implemented by concrete ActionClient structs.
-///
-/// See [`ActionClient<T>`] for an example
-pub trait ActionClientBase: Send + Sync {
-    /// Internal function to get a reference to the `rcl` handle.
-    fn handle(&self) -> &ActionClientHandle;
-    /// Returns the number of underlying entities for the action client.
-    fn num_entities(&self) -> &WaitableNumEntities;
-    /// Tries to run the callback for the given readiness mode.
-    fn execute(&self, mode: ReadyMode) -> Result<(), RclrsError>;
-}
-
 pub(crate) enum ReadyMode {
     Feedback,
     Status,
@@ -160,7 +148,7 @@ where
         let action_client_options = unsafe { rcl_action_client_get_default_options() };
 
         {
-            let mut rcl_node = node.handle.rcl_node.lock().unwrap();
+            let mut rcl_node = node.handle().rcl_node.lock().unwrap();
             let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap();
 
             // SAFETY:
@@ -188,19 +176,6 @@ where
             in_use_by_wait_set: Arc::new(AtomicBool::new(false)),
         });
 
-        let mut num_entities = WaitableNumEntities::default();
-        unsafe {
-            rcl_action_client_wait_set_get_num_entities(
-                &*handle.lock(),
-                &mut num_entities.num_subscriptions,
-                &mut num_entities.num_guard_conditions,
-                &mut num_entities.num_timers,
-                &mut num_entities.num_clients,
-                &mut num_entities.num_services,
-            )
-            .ok()?;
-        }
-
         Ok(Self {
             _marker: Default::default(),
             handle,
diff --git a/rclrs/src/action/action_server.rs b/rclrs/src/action/action_server.rs
index 9bce4765..e9806127 100644
--- a/rclrs/src/action/action_server.rs
+++ b/rclrs/src/action/action_server.rs
@@ -293,7 +293,7 @@ impl<A: Action> ActionServerState<A> {
     }
 }
 
-pub struct ActionServerGoalBoard<A: Action> {
+struct ActionServerGoalBoard<A: Action> {
     /// These goals have a live handle held by the user. We refer to them with a
     /// Weak to prevent a circular reference. When the user drops the live handle
     /// it will automatically be moved into the dropped_goals map.
diff --git a/rclrs/src/action/action_server/requested_goal.rs b/rclrs/src/action/action_server/requested_goal.rs
index 06965bd1..8e7ea661 100644
--- a/rclrs/src/action/action_server/requested_goal.rs
+++ b/rclrs/src/action/action_server/requested_goal.rs
@@ -1,15 +1,12 @@
 use crate::{
     rcl_bindings::*,
     log_error,
-    GoalUuid, RclrsError, RclrsErrorFilter, ToResult, ActionServerGoalBoard,
+    GoalUuid, RclrsError, RclrsErrorFilter, ToResult,
 };
-use super::{ActionServerGoalHandle, LiveActionServerGoal, AcceptedGoal, TerminatedGoal};
+use super::{ActionServerGoalBoard, ActionServerGoalHandle, LiveActionServerGoal, AcceptedGoal, TerminatedGoal};
 use std::sync::Arc;
 use rosidl_runtime_rs::Action;
 
-#[derive(Debug, Clone)]
-pub struct GoalAcceptanceError;
-
 /// An action goal that has been requested but not accepted yet. If this is
 /// dropped without being accepted then the goal request will be rejected.
 pub struct RequestedGoal<A: Action> {
@@ -78,7 +75,7 @@ impl<A: Action> RequestedGoal<A> {
         self.board.handle.goals.lock().unwrap().insert(*handle.goal_id(), Arc::clone(&handle));
 
         self.accepted = true;
-        self.send_goal_response();
+        self.send_goal_response()?;
 
         let live = Arc::new(LiveActionServerGoal::new(
             handle,
diff --git a/rclrs/src/wait_set.rs b/rclrs/src/wait_set.rs
index c4d7ca06..94450c78 100644
--- a/rclrs/src/wait_set.rs
+++ b/rclrs/src/wait_set.rs
@@ -102,7 +102,7 @@ impl WaitSet {
     pub fn wait(
         &mut self,
         timeout: Option<Duration>,
-        mut f: impl FnMut(&mut dyn RclPrimitive, ReadyKind) -> Result<(), RclrsError>,
+        mut f: impl FnMut(ReadyKind, &mut dyn RclPrimitive) -> Result<(), RclrsError>,
     ) -> Result<(), RclrsError> {
         let timeout_ns = match timeout.map(|d| d.as_nanos()) {
             None => -1,
@@ -138,8 +138,8 @@ impl WaitSet {
         // For the remaining entities, check if they were activated and then run
         // the callback for those that were.
         for waiter in self.primitives.values_mut().flat_map(|v| v) {
-            if waiter.is_ready(&self.handle.rcl_wait_set) {
-                f(&mut *waiter.primitive)?;
+            if let Some(ready) =  waiter.is_ready(&self.handle.rcl_wait_set) {
+                f(ready, &mut *waiter.primitive)?;
             }
         }
 
@@ -167,7 +167,7 @@ impl WaitSet {
     pub fn count(&self) -> WaitableCount {
         let mut c = WaitableCount::new();
         for (kind, collection) in &self.primitives {
-            c.add(*kind, collection.len());
+            c.add_group(kind, collection);
         }
         c
     }
diff --git a/rclrs/src/wait_set/wait_set_runner.rs b/rclrs/src/wait_set/wait_set_runner.rs
index 1eb8e745..6b4ffc81 100644
--- a/rclrs/src/wait_set/wait_set_runner.rs
+++ b/rclrs/src/wait_set/wait_set_runner.rs
@@ -184,7 +184,7 @@ impl WaitSetRunner {
             });
 
             let mut at_least_one = false;
-            self.wait_set.wait(timeout, |executable, ready| {
+            self.wait_set.wait(timeout, |ready, executable| {
                 at_least_one = true;
                 // SAFETY: The user of WaitSetRunner is responsible for ensuring
                 // the runner has the same payload type as the executables that
diff --git a/rclrs/src/wait_set/waitable.rs b/rclrs/src/wait_set/waitable.rs
index 3206992b..2acafb9e 100644
--- a/rclrs/src/wait_set/waitable.rs
+++ b/rclrs/src/wait_set/waitable.rs
@@ -221,19 +221,86 @@ impl WaitableCount {
         Self::default()
     }
 
-    pub(super) fn add(&mut self, kind: RclPrimitiveKind, count: usize) {
+    pub(super) fn add_group(
+        &mut self,
+        kind: &RclPrimitiveKind,
+        waitables: &Vec<Waitable>,
+    ) {
         match kind {
-            RclPrimitiveKind::Subscription => self.subscriptions += count,
-            RclPrimitiveKind::GuardCondition => self.guard_conditions += count,
-            RclPrimitiveKind::Timer => self.timers += count,
-            RclPrimitiveKind::Client => self.clients += count,
-            RclPrimitiveKind::Service => self.services += count,
-            RclPrimitiveKind::Event => self.events += count,
+            RclPrimitiveKind::Subscription => self.subscriptions += waitables.len(),
+            RclPrimitiveKind::GuardCondition => self.guard_conditions += waitables.len(),
+            RclPrimitiveKind::Timer => self.timers += waitables.len(),
+            RclPrimitiveKind::Client => self.clients += waitables.len(),
+            RclPrimitiveKind::Service => self.services += waitables.len(),
+            RclPrimitiveKind::Event => self.events += waitables.len(),
             RclPrimitiveKind::ActionServer => {
-                Use rcl_action_server_wait_set_get_num_entities
+                for waitable in waitables {
+                    self.add_single(&*waitable.primitive);
+                }
             }
             RclPrimitiveKind::ActionClient => {
-                Use rcl_action_client_wait_set_get_num_entities
+                for waitable in waitables {
+                    self.add_single(&*waitable.primitive);
+                }
+            }
+        }
+    }
+
+    fn add_single(&mut self, primitive: &dyn RclPrimitive) {
+        match primitive.handle() {
+            RclPrimitiveHandle::Subscription(_) => self.subscriptions += 1,
+            RclPrimitiveHandle::GuardCondition(_) => self.guard_conditions += 1,
+            RclPrimitiveHandle::Timer(_) => self.timers += 1,
+            RclPrimitiveHandle::Client(_) => self.clients += 1,
+            RclPrimitiveHandle::Service(_) => self.services += 1,
+            RclPrimitiveHandle::Event(_) => self.events += 1,
+            RclPrimitiveHandle::ActionServer(handle) => {
+                let mut count = WaitableCount::new();
+                let r = unsafe {
+                    // SAFETY: The handle is kept safe by the mutex guard, and
+                    // there are no other preconditions.
+                    rcl_action_server_wait_set_get_num_entities(
+                        &*handle,
+                        &mut count.subscriptions,
+                        &mut count.guard_conditions,
+                        &mut count.timers,
+                        &mut count.clients,
+                        &mut count.services,
+                    )
+                };
+                if let Err(err) = r.ok() {
+                    log_error!(
+                        "waitable_count.add_single",
+                        "Error occurred while counting primitives for an action server. \
+                        This should not happen, please report it to the rclrs maintainers: {err}",
+                    );
+                }
+
+                *self += count;
+            }
+            RclPrimitiveHandle::ActionClient(handle) => {
+                let mut count = WaitableCount::new();
+                let r = unsafe {
+                    // SAFETY: The handle is kept safe by the mutex guard, and
+                    // there are no other preconditions.
+                    rcl_action_client_wait_set_get_num_entities(
+                        &*handle,
+                        &mut count.subscriptions,
+                        &mut count.guard_conditions,
+                        &mut count.timers,
+                        &mut count.clients,
+                        &mut count.services,
+                    )
+                };
+                if let Err(err) = r.ok() {
+                    log_error!(
+                        "waitable_count.add_single",
+                        "Error occurred while counting primitives for an action client. \
+                        This should not happen, please report it to the rclrs maintainers: {err}",
+                    );
+                }
+
+                *self += count;
             }
         }
     }
@@ -281,3 +348,24 @@ impl WaitableCount {
         .ok()
     }
 }
+
+impl std::ops::Add for WaitableCount {
+    type Output = Self;
+    fn add(self, rhs: Self) -> Self::Output {
+        Self {
+            subscriptions: self.subscriptions + rhs.subscriptions,
+            guard_conditions: self.guard_conditions + rhs.guard_conditions,
+            timers: self.timers + rhs.timers,
+            clients: self.clients + rhs.clients,
+            services: self.services + rhs.services,
+            events: self.events + rhs.events,
+        }
+    }
+}
+
+impl std::ops::AddAssign for WaitableCount {
+    fn add_assign(&mut self, rhs: Self) {
+        let count = *self + rhs;
+        *self = count;
+    }
+}

From 61dc7090675a44e703b2f5790eedbd8fc71fd81e Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Tue, 29 Jul 2025 23:40:20 +0800
Subject: [PATCH 12/20] Fleshing out implementation of action clients

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/src/action.rs                           |  23 +++
 rclrs/src/action/action_client.rs             | 187 ++++++++++++------
 .../action_client/cancellation_client.rs      |   5 +
 .../action/action_client/feedback_client.rs   |  37 ++++
 rclrs/src/action/action_client/goal_client.rs |  93 +++++++++
 rclrs/src/action/action_server.rs             |  26 +--
 .../src/action/action_server/accepted_goal.rs |  21 +-
 .../action_server_goal_handle.rs              |  18 --
 .../action_server/cancellation_state.rs       |  15 +-
 .../action/action_server/cancelling_goal.rs   |   9 +
 .../action/action_server/executing_goal.rs    |  18 +-
 .../action_server/live_action_server_goal.rs  |   4 +-
 .../action/action_server/requested_goal.rs    |  22 ++-
 rclrs/src/error.rs                            |   2 +
 rclrs/src/time.rs                             |   3 +
 rclrs/src/wait_set/rcl_primitive.rs           |   2 +-
 16 files changed, 365 insertions(+), 120 deletions(-)
 create mode 100644 rclrs/src/action/action_client/cancellation_client.rs
 create mode 100644 rclrs/src/action/action_client/feedback_client.rs
 create mode 100644 rclrs/src/action/action_client/goal_client.rs

diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs
index aa922fea..0381560a 100644
--- a/rclrs/src/action.rs
+++ b/rclrs/src/action.rs
@@ -56,3 +56,26 @@ pub enum CancelResponse {
     /// The server will try to cancel the goal.
     Accept = 2,
 }
+
+/// Values defined by `action_msgs/msg/GoalStatus`
+#[repr(i8)]
+#[derive(Debug, Clone, Hash, PartialEq, Eq)]
+pub enum GoalStatus {
+    /// The goal status has never been initialized. This likely means it has not
+    /// yet been accepted.
+    Unknown = 0,
+    /// The goal was accepted by the action server.
+    Accepted = 1,
+    /// The goal is being executed by the action server.
+    Executing = 2,
+    /// The action server has accepting cancelling the goal and is in the process
+    /// of cancelling it.
+    Cancelling = 3,
+    /// The action server has successfully reached the goal.
+    Succeeded = 4,
+    /// The action server has finished cancelling the goal.
+    Cancelled = 5,
+    /// The action server has aborted the goal. This suggests an error happened
+    /// during execution or cancelling.
+    Aborted = 6,
+}
diff --git a/rclrs/src/action/action_client.rs b/rclrs/src/action/action_client.rs
index 731eadb4..d1500b7d 100644
--- a/rclrs/src/action/action_client.rs
+++ b/rclrs/src/action/action_client.rs
@@ -1,14 +1,27 @@
 use crate::{
-    error::ToResult, rcl_bindings::*, Node, NodeHandle, QoSProfile,
+    rcl_bindings::*,
+    GoalStatus, GoalUuid, ToResult, Node, NodeHandle, QoSProfile,
     RclrsError, ENTITY_LIFECYCLE_MUTEX,
 };
 use std::{
     borrow::Borrow,
+    collections::HashMap,
     ffi::CString,
     marker::PhantomData,
     sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard},
 };
-use rosidl_runtime_rs::Action;
+use tokio::sync::{
+    watch::Sender as WatchSender,
+    mpsc::UnboundedSender,
+    oneshot::Sender,
+};
+use rosidl_runtime_rs::{Action, Message, RmwFeedbackMessage};
+
+mod feedback_client;
+pub use feedback_client::*;
+
+mod goal_client;
+pub use goal_client::*;
 
 /// `ActionClientOptions` are used by [`Node::create_action_client`][1] to initialize an
 /// [`ActionClient`].
@@ -51,45 +64,6 @@ impl<'a, T: Borrow<str> + ?Sized + 'a> From<&'a T> for ActionClientOptions<'a> {
     }
 }
 
-/// Manage the lifecycle of an `rcl_action_client_t`, including managing its dependencies
-/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are
-/// [dropped after][1] the `rcl_action_client_t`.
-///
-/// [1]: <https://doc.rust-lang.org/reference/destructors.html>
-pub struct ActionClientHandle {
-    rcl_action_client: Mutex<rcl_action_client_t>,
-    node_handle: Arc<NodeHandle>,
-    pub(crate) in_use_by_wait_set: Arc<AtomicBool>,
-}
-
-impl ActionClientHandle {
-    pub(crate) fn lock(&self) -> MutexGuard<rcl_action_client_t> {
-        self.rcl_action_client.lock().unwrap()
-    }
-}
-
-impl Drop for ActionClientHandle {
-    fn drop(&mut self) {
-        let rcl_action_client = self.rcl_action_client.get_mut().unwrap();
-        let mut rcl_node = self.node_handle.rcl_node.lock().unwrap();
-        let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap();
-        // SAFETY: The entity lifecycle mutex is locked to protect against the risk of
-        // global variables in the rmw implementation being unsafely modified during cleanup.
-        unsafe {
-            rcl_action_client_fini(rcl_action_client, &mut *rcl_node);
-        }
-    }
-}
-
-pub(crate) enum ReadyMode {
-    Feedback,
-    Status,
-    GoalResponse,
-    CancelResponse,
-    ResultResponse,
-}
-
-///
 /// Main class responsible for sending goals to a ROS action server.
 ///
 /// Create a client using [`Node::create_action_client`][1].
@@ -98,7 +72,7 @@ pub(crate) enum ReadyMode {
 ///
 /// [1]: crate::NodeState::create_action_client
 /// [2]: crate::spin
-pub type ActionClient<ActionT> = Arc<ActionClientState<ActionT>>;
+pub type ActionClient<A> = Arc<ActionClientState<A>>;
 
 /// The inner state of an [`ActionClient`].
 ///
@@ -110,34 +84,23 @@ pub type ActionClient<ActionT> = Arc<ActionClientState<ActionT>>;
 /// The public API of the [`ActionClient`] type is implemented via `ActionClientState`.
 ///
 /// [1]: std::sync::Weak
-pub struct ActionClientState<ActionT>
-where
-    ActionT: rosidl_runtime_rs::Action,
-{
-    _marker: PhantomData<fn() -> ActionT>,
-    pub(crate) handle: Arc<ActionClientHandle>,
-    /// Ensure the parent node remains alive as long as the subscription is held.
-    /// This implementation will change in the future.
-    #[allow(unused)]
-    node: Node,
+pub struct ActionClientState<A: Action> {
+    board: Arc<ActionClientGoalBoard<A>>,
 }
 
-impl<T> ActionClientState<T>
-where
-    T: rosidl_runtime_rs::Action,
-{
+impl<A: Action> ActionClientState<A> {
     /// Creates a new action client.
     pub(crate) fn new<'a>(
         node: &Node,
         options: impl Into<ActionClientOptions<'a>>,
     ) -> Result<Self, RclrsError>
     where
-        T: rosidl_runtime_rs::Action,
+        A: rosidl_runtime_rs::Action,
     {
         let options = options.into();
         // SAFETY: Getting a zero-initialized value is always safe.
         let mut rcl_action_client = unsafe { rcl_action_get_zero_initialized_client() };
-        let type_support = T::get_type_support() as *const rosidl_action_type_support_t;
+        let type_support = A::get_type_support() as *const rosidl_action_type_support_t;
         let action_name_c_string =
             CString::new(options.action_name).map_err(|err| RclrsError::StringContainsNul {
                 err,
@@ -173,18 +136,45 @@ where
         let handle = Arc::new(ActionClientHandle {
             rcl_action_client: Mutex::new(rcl_action_client),
             node_handle: Arc::clone(&node.handle()),
-            in_use_by_wait_set: Arc::new(AtomicBool::new(false)),
         });
 
-        Ok(Self {
-            _marker: Default::default(),
+        let board = Arc::new(ActionClientGoalBoard {
             handle,
             node: Arc::clone(node),
-        })
+            result_clients: Default::default(),
+        });
+
+        Ok(Self { board })
     }
+}
+
+struct ActionClientGoalBoard<A: Action> {
+    feedback_senders: Mutex<HashMap<GoalUuid, Vec<UnboundedSender<A::Feedback>>>>,
+    result_clients: Mutex<HashMap<rmw_request_id_t, Sender<A::Result>>>,
+    handle: Arc<ActionClientHandle>,
+    /// Ensure the parent node remains alive as long as the subscription is held.
+    /// This implementation will change in the future.
+    #[allow(unused)]
+    node: Node,
+}
 
+impl<A: Action> ActionClientGoalBoard<A> {
     fn execute_feedback(&self) -> Result<(), RclrsError> {
-        todo!()
+        let feedback_rmw = self.handle.take_feedback::<A>()?;
+        let (goal_uuid, feedback) = A::split_feedback_message(feedback_rmw);
+        let feedback: A::Feedback = Message::from_rmw_message(feedback);
+        if let Some(senders) = self.feedback_senders.lock().unwrap().get(&GoalUuid(goal_uuid)) {
+            // Avoid making any unnecessary clones
+            for sender in senders.iter().take(senders.len() - 1) {
+                sender.send(feedback.clone());
+            }
+
+            if let Some(last_sender) = senders.last() {
+                last_sender.send(feedback);
+            }
+        }
+
+        Ok(())
     }
 
     fn execute_status(&self) -> Result<(), RclrsError> {
@@ -204,15 +194,82 @@ where
     }
 }
 
-struct ActionClientGoalBoard<A: Action> {
-    _ignore: std::marker::PhantomData<fn(A)>,
+/// Once all of the constituent clients for the goal are dropped, this will
+/// remove the [`GoalClientSender`][super::GoalClientSender] from the goal
+/// board.
+struct GoalClientLifecycle<A: Action> {
+    kind: GoalClientKind,
+    board: Arc<ActionClientGoalBoard<A>>,
+}
+
+enum GoalClientKind {
+    Feedback(GoalUuid),
+    Status(GoalUuid),
+    Result(rmw_request_id_t),
+    Cancellation,
+}
+
+impl<A: Action> Drop for GoalClientLifecycle<A> {
+    fn drop(&mut self) {
+        match self.kind {
+            GoalClientKind::Feedback(goal_uuid) => {
+                let mut all_feedback_senders = self.board.feedback_senders.lock().unwrap();
+
+                let mut empty = false;
+                if let Some(senders) = all_feedback_senders.get_mut(&goal_uuid) {
+                    senders.retain(|sender| !sender.is_closed());
+                    empty = senders.is_empty();
+                }
+                if empty {
+                    all_feedback_senders.remove(&goal_uuid);
+                }
+            }
+        }
+    }
 }
 
 struct ActionClientExecutable<A: Action> {
     board: Arc<ActionClientGoalBoard<A>>,
 }
 
+/// Manage the lifecycle of an `rcl_action_client_t`, including managing its dependencies
+/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are
+/// [dropped after][1] the `rcl_action_client_t`.
+///
+/// [1]: <https://doc.rust-lang.org/reference/destructors.html>
+pub struct ActionClientHandle {
+    rcl_action_client: Mutex<rcl_action_client_t>,
+    node_handle: Arc<NodeHandle>,
+}
+
+impl ActionClientHandle {
+    fn lock(&self) -> MutexGuard<rcl_action_client_t> {
+        self.rcl_action_client.lock().unwrap()
+    }
+
+    fn take_feedback<A: Action>(&self) -> Result<RmwFeedbackMessage<A>, RclrsError> {
+        let mut feedback_rmw = RmwFeedbackMessage::<A>::default();
+        unsafe {
+            let handle = self.lock();
+            rcl_action_take_feedback(&*handle, &mut feedback_rmw as *mut _ as *mut _)
+        }
+        .ok()?;
+        Ok(feedback_rmw)
+    }
+}
 
+impl Drop for ActionClientHandle {
+    fn drop(&mut self) {
+        let rcl_action_client = self.rcl_action_client.get_mut().unwrap();
+        let mut rcl_node = self.node_handle.rcl_node.lock().unwrap();
+        let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap();
+        // SAFETY: The entity lifecycle mutex is locked to protect against the risk of
+        // global variables in the rmw implementation being unsafely modified during cleanup.
+        unsafe {
+            rcl_action_client_fini(rcl_action_client, &mut *rcl_node);
+        }
+    }
+}
 
 // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
 // they are running in. Therefore, this type can be safely sent to another thread.
diff --git a/rclrs/src/action/action_client/cancellation_client.rs b/rclrs/src/action/action_client/cancellation_client.rs
new file mode 100644
index 00000000..971292bc
--- /dev/null
+++ b/rclrs/src/action/action_client/cancellation_client.rs
@@ -0,0 +1,5 @@
+
+
+pub struct CancellationClient<A: Action> {
+    board: Arc<ActionClientGoalBoard<A>>,
+}
diff --git a/rclrs/src/action/action_client/feedback_client.rs b/rclrs/src/action/action_client/feedback_client.rs
new file mode 100644
index 00000000..d2a69a6d
--- /dev/null
+++ b/rclrs/src/action/action_client/feedback_client.rs
@@ -0,0 +1,37 @@
+use crate::{GoalStatus, GoalUuid};
+use super::{ActionClientGoalBoard, GoalClientLifecycle};
+use rosidl_runtime_rs::Action;
+use tokio::sync::{
+    watch::Receiver as Watcher,
+    mpsc::UnboundedReceiver,
+    oneshot::Receiver,
+};
+use std::{
+    ops::{Deref, DerefMut},
+    sync::Arc,
+};
+
+/// This struct allows you to receive feedback messages for the goal. Through the
+/// [`DerefMut`] trait you can use the [`UnboundedReceiver`] API to await new
+/// feedback messages.
+pub struct FeedbackClient<A: Action> {
+    receiver: UnboundedReceiver<A::Feedback>,
+    /// This keeps track of whether any goal client components are still being used.
+    /// This must come after the receiver in the struct to ensure cleanup is done
+    /// correctly.
+    #[allow(unused)]
+    lifecycle: Arc<GoalClientLifecycle<A>>,
+}
+
+impl<A: Action> Deref for FeedbackClient<A> {
+    type Target = UnboundedReceiver<A::Feedback>;
+    fn deref(&self) -> &Self::Target {
+        &self.receiver
+    }
+}
+
+impl<A: Action> DerefMut for FeedbackClient<A> {
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        &mut self.receiver
+    }
+}
diff --git a/rclrs/src/action/action_client/goal_client.rs b/rclrs/src/action/action_client/goal_client.rs
new file mode 100644
index 00000000..38e2bfe9
--- /dev/null
+++ b/rclrs/src/action/action_client/goal_client.rs
@@ -0,0 +1,93 @@
+use crate::{GoalStatus, GoalUuid, FeedbackClient};
+use super::{ActionClientGoalBoard, GoalClientLifecycle};
+use rosidl_runtime_rs::Action;
+use tokio::sync::{
+    watch::Receiver as Watcher,
+    mpsc::UnboundedReceiver,
+    oneshot::Receiver,
+};
+use std::{
+    ops::{Deref, DerefMut},
+    sync::Arc,
+};
+
+/// The goal client bundles a set of receivers that will allow you to await
+/// different information from the action server, such as feedback messages,
+/// status updates, and the final result. It also provides a way to request
+/// cancelling a goal.
+///
+/// This struct is designed to be [destructured][1] so each of its fields can be
+/// used independently. This is important because many of them require mutable
+/// access (or one-time access) to be used, so being blocked behind `&mut self`
+/// would make it impossible to use them independently, e.g. to await more than
+/// one at a time.
+///
+/// [1]: https://doc.rust-lang.org/rust-by-example/flow_control/match/destructuring/destructure_structures.html
+pub struct GoalClient<A: Action> {
+    /// Receive feedback messages for the goal.
+    pub feedback: FeedbackClient<A>,
+    /// Watch the status of the goal.
+    pub status: StatusClient<A>,
+    /// Get the final result of the goal.
+    pub result: Receiver<A::Result>
+}
+
+/// This struct allows you to monitor the status of the goal. Through the
+/// [`DerefMut`] trait you can use the [`watch::Receiver`][Watcher] API to
+/// check the latest status and monitor changes.
+pub struct StatusClient<A: Action> {
+    receiver: Watcher<GoalStatus>,
+    /// This keeps track of whether any goal client components are still being used.
+    /// This must come after the receiver in the struct to ensure cleanup is done
+    /// correctly.
+    #[allow(unused)]
+    lifecycle: Arc<GoalClientLifecycle<A>>,
+}
+
+impl<A: Action> Clone for StatusClient<A> {
+    fn clone(&self) -> Self {
+        Self {
+            receiver: self.receiver.clone(),
+            lifecycle: Arc::clone(&self.lifecycle),
+        }
+    }
+}
+
+impl<A: Action> Deref for StatusClient<A> {
+    type Target = Watcher<GoalStatus>;
+    fn deref(&self) -> &Self::Target {
+        &self.receiver
+    }
+}
+
+impl<A: Action> DerefMut for StatusClient<A> {
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        &mut self.receiver
+    }
+}
+
+/// This struct allows you to receive the result of the goal. Through the [`DerefMut`]
+/// trait you can use the [`oneshot::Receiver`][Receiver] API to await the final
+/// result. Note that it can only be received once.
+pub struct ResultClient<A: Action> {
+    receiver: Receiver<A::Result>,
+    /// This keeps track of whether any goal client components are still being used.
+    /// This must come after the receiver in the struct to ensure cleanup is done
+    /// correctly.
+    #[allow(unused)]
+    lifecycle: Arc<GoalClientLifecycle<A>>,
+}
+
+impl<A: Action> Deref for ResultClient<A> {
+    type Target = Receiver<A::Result>;
+    fn deref(&self) -> &Self::Target {
+        &self.receiver
+    }
+}
+
+impl<A: Action> DerefMut for ResultClient<A> {
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        &mut self.receiver
+    }
+}
+
diff --git a/rclrs/src/action/action_server.rs b/rclrs/src/action/action_server.rs
index e9806127..add53ed5 100644
--- a/rclrs/src/action/action_server.rs
+++ b/rclrs/src/action/action_server.rs
@@ -2,7 +2,7 @@ use crate::{
     action::GoalUuid,
     error::ToResult,
     rcl_bindings::*,
-    ActionGoalReceiver, DropGuard, Node, NodeHandle, QoSProfile, RclPrimitive, RclrsError,
+    ActionGoalReceiver, DropGuard, GoalStatus, Node, NodeHandle, QoSProfile, RclPrimitive, RclrsError,
     RclPrimitiveHandle, RclPrimitiveKind, ReadyKind, Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
 };
 use rosidl_runtime_rs::{Action, Message, RmwGoalRequest, RmwResultRequest};
@@ -555,11 +555,12 @@ impl<A: Action> RclPrimitive for ActionServerExecutable<A> {
 /// [1]: <https://doc.rust-lang.org/reference/destructors.html>
 pub(crate) struct ActionServerHandle<A: Action> {
     rcl_action_server: Mutex<rcl_action_server_t>,
-    /// Ensure the node remains active while the action server is running
+    /// Ensure the node remains active while the action server is running.
+    #[allow(unused)]
     node_handle: Arc<NodeHandle>,
     /// Ensure the `impl_*` of the action server goals remain valid until they
     /// have expired or until the rcl_action_server_t gets fini-ed.
-    pub(super) goals: Mutex<HashMap<GoalUuid, Arc<ActionServerGoalHandle<A>>>>,
+    goals: Mutex<HashMap<GoalUuid, Arc<ActionServerGoalHandle<A>>>>,
 }
 
 // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
@@ -609,8 +610,8 @@ impl<A: Action> ActionServerHandle<A> {
             sequence_number: 0,
         };
         let mut request_rmw = RmwGoalRequest::<A>::default();
-        let handle = self.lock();
         unsafe {
+            let handle = self.lock();
             // SAFETY: The action server is locked by the handle. The request_id is a
             // zero-initialized rmw_request_id_t, and the request_rmw is a default-initialized
             // SendGoalService request message.
@@ -632,8 +633,8 @@ impl<A: Action> ActionServerHandle<A> {
         };
         // SAFETY: No preconditions
         let mut request_rmw = unsafe { rcl_action_get_zero_initialized_cancel_request() };
-        let handle = self.lock();
         unsafe {
+            let handle = self.lock();
             // SAFETY: The action server is locked by the handle. The request_id is a
             // zero-initialized rmw_request_id_t, and the request_rmw is a zero-initialized
             // action_msgs__srv__CancelGoal_Request.
@@ -655,8 +656,8 @@ impl<A: Action> ActionServerHandle<A> {
         };
 
         let mut request_rmw = RmwResultRequest::<A>::default();
-        let handle = self.lock();
         unsafe {
+            let handle = self.lock();
             // SAFETY: The action server is locked by the handle. The request_id is a
             // zero-initialized rmw_request_id_t, and the request_rmw is a default-initialized
             // GetResultService request message.
@@ -677,19 +678,6 @@ enum GoalDispatch<A: Action> {
     Sender(UnboundedSender<RequestedGoal<A>>),
 }
 
-/// Values defined by `action_msgs/msg/GoalStatus`
-#[repr(i8)]
-#[derive(Debug, Clone, Hash, PartialEq, Eq)]
-enum GoalStatus {
-    Unknown = 0,
-    Accepted = 1,
-    Executing = 2,
-    Cancelling = 3,
-    Succeeded = 4,
-    Cancelled = 5,
-    Aborted = 6,
-}
-
 /// Possible status values for terminal states
 #[repr(i8)]
 #[derive(Debug, Clone, Hash, PartialEq, Eq)]
diff --git a/rclrs/src/action/action_server/accepted_goal.rs b/rclrs/src/action/action_server/accepted_goal.rs
index ee2a7011..e40e2596 100644
--- a/rclrs/src/action/action_server/accepted_goal.rs
+++ b/rclrs/src/action/action_server/accepted_goal.rs
@@ -13,6 +13,7 @@ pub struct AcceptedGoal<A: Action> {
 
 impl<A: Action> AcceptedGoal<A> {
     /// Get the goal of this action.
+    #[must_use]
     pub fn goal(&self) -> &Arc<A::Goal> {
         self.live.goal()
     }
@@ -49,12 +50,15 @@ impl<A: Action> AcceptedGoal<A> {
 
     /// Transition the goal into the cancelling state.
     ///
-    /// This does not require an action client to request a cancellation. Instead
-    /// you may act as the action client and commanding that the goal transition
-    /// into cancelling.
+    /// This does not require an action client to request a cancellation. If no
+    /// cancellation was requested, then using this function will have your action
+    /// server act as an action client that is commanding that the goal transition
+    /// to cancel.
     ///
     /// If there are any open cancellation requests for this goal from any action
-    /// clients, they will all be notified that the cancellation is accepted.
+    /// clients, they will all be notified that the cancellation is accepted. Any
+    /// new cancellation requests that arrive for the goal after this will
+    /// automatically be accepted.
     ///
     /// For the goal to reach the cancelled state, you must follow this up with
     /// [`CancellingGoal::cancelled_with`].
@@ -94,7 +98,16 @@ impl<A: Action> AcceptedGoal<A> {
     }
 }
 
+/// While a goal was waiting to be executed, it is possible for cancellation
+/// requests to arrive for it. An accepted goal may transition directly into
+/// either executing or cancelling.
+///
+/// If you use [`AcceptedGoal::begin`] then the goal will be transitioned into
+/// the executing state if no cancellation requests have arrived, or the cancelling
+/// state if a cancellation has arrived. This enum will tell you which has occurred.
 pub enum BeginAcceptedGoal<A: Action> {
+    /// The goal has transitioned into executing.
     Execute(ExecutingGoal<A>),
+    /// The goal has transitioned into cancelling.
     Cancel(CancellingGoal<A>),
 }
diff --git a/rclrs/src/action/action_server/action_server_goal_handle.rs b/rclrs/src/action/action_server/action_server_goal_handle.rs
index beb9fdfb..06c93a7e 100644
--- a/rclrs/src/action/action_server/action_server_goal_handle.rs
+++ b/rclrs/src/action/action_server/action_server_goal_handle.rs
@@ -58,30 +58,12 @@ impl<A: Action> ActionServerGoalHandle<A> {
         unsafe { std::mem::transmute(state) }
     }
 
-    /// Returns whether the client has requested that this goal be cancelled.
-    pub(super) fn is_cancelling(&self) -> bool {
-        self.get_status() == GoalStatus::Cancelling
-    }
-
     /// This is used to check if we should respond as accepting a cancellation
     /// request for this goal after it is no longer live.
     pub(super) fn is_cancelled(&self) -> bool {
         self.get_status() == GoalStatus::Cancelled
     }
 
-    /// Returns true if the goal is either pending or executing, or false if it has reached a
-    /// terminal state.
-    pub(super) fn is_active(&self) -> bool {
-        let rcl_handle = self.rcl_handle.lock().unwrap();
-        // SAFETY: The provided goal handle is properly initialized by construction.
-        unsafe { rcl_action_goal_handle_is_active(&*rcl_handle) }
-    }
-
-    /// Returns whether the goal is executing.
-    pub(super) fn is_executing(&self) -> bool {
-        self.get_status() == GoalStatus::Executing
-    }
-
     /// Get the unique identifier of the goal.
     pub(super) fn goal_id(&self) -> &GoalUuid {
         &self.uuid
diff --git a/rclrs/src/action/action_server/cancellation_state.rs b/rclrs/src/action/action_server/cancellation_state.rs
index b610e3c2..4638229c 100644
--- a/rclrs/src/action/action_server/cancellation_state.rs
+++ b/rclrs/src/action/action_server/cancellation_state.rs
@@ -80,7 +80,9 @@ impl<A: Action> CancellationState<A> {
 
                 // Revert to not having any cancellation mode
                 *mode = CancellationMode::None;
-                self.sender.send(false);
+                // We do not need to worry about errors from sending this state
+                // since it is okay for the receiver to be dropped.
+                let _ = self.sender.send(false);
             }
             CancellationMode::None => {
                 // Do nothing
@@ -104,15 +106,18 @@ impl<A: Action> CancellationState<A> {
                 // Progress to cancelling mode
                 *mode = CancellationMode::Cancelling;
                 // Just in case this signal was never sent, make sure we have
-                // a true value in the cancel requested channel.
-                self.sender.send(true);
+                // a true value in the cancel requested channel. We can ignore
+                // errors from this because it is okay for the receiver to be
+                // dropped.
+                let _ = self.sender.send(true);
             }
             CancellationMode::None => {
                 // Skip straight to cancellation mode since the user has accepted
                 // a cancellation even though it wasn't requested externally.
                 *mode = CancellationMode::Cancelling;
-                // Make sure the cancellation is signalled.
-                self.sender.send(true);
+                // Make sure the cancellation is signalled. We can ignore errors
+                // from this because it is okay for the receiver to be dropped.
+                let _ = self.sender.send(true);
             }
             CancellationMode::Cancelling => {
                 // Do nothing
diff --git a/rclrs/src/action/action_server/cancelling_goal.rs b/rclrs/src/action/action_server/cancelling_goal.rs
index fd9c5541..24d19370 100644
--- a/rclrs/src/action/action_server/cancelling_goal.rs
+++ b/rclrs/src/action/action_server/cancelling_goal.rs
@@ -2,12 +2,21 @@ use super::{LiveActionServerGoal, TerminatedGoal};
 use std::sync::Arc;
 use rosidl_runtime_rs::Action;
 
+/// This represents a goal that is in the Cancelling state. This struct is held
+/// by an action server implementation and is used to provide feedback to the
+/// client and to transition the goal into its next state. There is no need to
+/// listen for cancellation requests since being in this state means all incoming
+/// cancellation requests will automatically be accepted.
+///
+/// If you drop this struct without explicitly transitioning it to its next state,
+/// the goal will report itself as aborted.
 pub struct CancellingGoal<A: Action> {
     live: Arc<LiveActionServerGoal<A>>,
 }
 
 impl<A: Action> CancellingGoal<A> {
     /// Get the goal of this action.
+    #[must_use]
     pub fn goal(&self) -> &Arc<A::Goal> {
         self.live.goal()
     }
diff --git a/rclrs/src/action/action_server/executing_goal.rs b/rclrs/src/action/action_server/executing_goal.rs
index 89cd9683..7a98ed2e 100644
--- a/rclrs/src/action/action_server/executing_goal.rs
+++ b/rclrs/src/action/action_server/executing_goal.rs
@@ -5,6 +5,13 @@ use std::{
 };
 use rosidl_runtime_rs::Action;
 
+/// This represents a goal that is in the Executing state. This struct is held
+/// by an action server implementation and is used to provide feedback to the
+/// client, to listen for cancellation requests, and to transition the goal into
+/// its next state.
+///
+/// If you drop this struct without explicitly transitioning it to its next state,
+/// the goal will report itself as aborted.
 pub struct ExecutingGoal<A: Action> {
     live: Arc<LiveActionServerGoal<A>>,
 }
@@ -44,12 +51,15 @@ impl<A: Action> ExecutingGoal<A> {
 
     /// Transition the goal into the cancelling state.
     ///
-    /// This does not require an action client to request a cancellation. Instead
-    /// you may act as the action client and commanding that the goal transition
-    /// into cancelling.
+    /// This does not require an action client to request a cancellation. If no
+    /// cancellation was requested, then using this function will have your action
+    /// server act as an action client that is commanding that the goal transition
+    /// to cancel.
     ///
     /// If there are any open cancellation requests for this goal from any action
-    /// clients, they will all be notified that the cancellation is accepted.
+    /// clients, they will all be notified that the cancellation is accepted. Any
+    /// new cancellation requests that arrive for the goal after this will
+    /// automatically be accepted.
     ///
     /// For the goal to reach the cancelled state, you must follow this up with
     /// [`CancellingGoal::cancelled_with`].
diff --git a/rclrs/src/action/action_server/live_action_server_goal.rs b/rclrs/src/action/action_server/live_action_server_goal.rs
index 48fc680f..b6a9a721 100644
--- a/rclrs/src/action/action_server/live_action_server_goal.rs
+++ b/rclrs/src/action/action_server/live_action_server_goal.rs
@@ -12,7 +12,7 @@ use std::{
     sync::Arc,
     ops::Deref,
 };
-use rosidl_runtime_rs::{Action, Message, Service};
+use rosidl_runtime_rs::{Action, Message};
 
 /// This struct is the bridge to the rcl_action API for action server goals that
 /// are still active. It can be used to perform transitions while keeping data in
@@ -205,7 +205,7 @@ impl<A: Action> LiveActionServerGoal<A> {
         self.handle.provide_result(self.server.as_ref(), response_rmw)?;
 
         // Publish the state change.
-        self.server.publish_status();
+        self.server.publish_status()?;
 
         // Notify rcl that a goal has terminated and to therefore recalculate the expired goal timer.
         unsafe {
diff --git a/rclrs/src/action/action_server/requested_goal.rs b/rclrs/src/action/action_server/requested_goal.rs
index 8e7ea661..937809e8 100644
--- a/rclrs/src/action/action_server/requested_goal.rs
+++ b/rclrs/src/action/action_server/requested_goal.rs
@@ -19,13 +19,26 @@ pub struct RequestedGoal<A: Action> {
 
 impl<A: Action> RequestedGoal<A> {
     /// Get the goal of this action.
+    #[must_use]
     pub fn goal(&self) -> &Arc<A::Goal> {
         &self.goal_request
     }
 
     /// Accept the requested goal. The action client will be notified that the
     /// goal was accepted, and you will be able to begin executing.
-    pub fn accept(mut self) -> Result<AcceptedGoal<A>, RclrsError> {
+    ///
+    /// This will panic if a [`RclrsError::GoalAcceptanceError`] occurs which
+    /// most likely indicates a memory allocation failure, which the program is
+    /// not likely to recover from anyway.
+    #[must_use]
+    pub fn accept(self) -> AcceptedGoal<A> {
+        self.try_accept().unwrap()
+    }
+
+    /// An alternative to [`RequestedGoal::accept`] which does not panic in the
+    /// event of an error.
+    #[must_use]
+    pub fn try_accept(mut self) -> Result<AcceptedGoal<A>, RclrsError> {
         let handle = {
             let mut goal_info = unsafe {
                 // SAFETY: Zero-initialized rcl structs are always safe to make
@@ -75,7 +88,12 @@ impl<A: Action> RequestedGoal<A> {
         self.board.handle.goals.lock().unwrap().insert(*handle.goal_id(), Arc::clone(&handle));
 
         self.accepted = true;
-        self.send_goal_response()?;
+        if let Err(err) = self.send_goal_response() {
+            log_error!(
+                "requested_goal.try_accept",
+                "Failed to send an action goal acceptance response: {err}",
+            );
+        }
 
         let live = Arc::new(LiveActionServerGoal::new(
             handle,
diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs
index 78a68261..8b3e449b 100644
--- a/rclrs/src/error.rs
+++ b/rclrs/src/error.rs
@@ -71,6 +71,8 @@ pub enum RclrsError {
     ///
     /// We have no way of diagnosing which of these errors caused the failure, so
     /// all we can do is indicate that an error occurred with accepting the goal.
+    /// However, the implementation of rclrs automatically protects from all of
+    /// these errors except memory allocation failure.
     GoalAcceptanceError,
 }
 
diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs
index 67132624..699435fb 100644
--- a/rclrs/src/time.rs
+++ b/rclrs/src/time.rs
@@ -33,11 +33,14 @@ impl Time {
         Ok(builtin_interfaces::msg::Time { nanosec, sec })
     }
 
+    /// Convenience function for converting time into an rcl message.
     pub fn to_rcl(&self) -> Result<builtin_interfaces__msg__Time, TryFromIntError> {
         let (sec, nanosec) = self.to_sec_nanosec()?;
         Ok(builtin_interfaces__msg__Time { sec, nanosec })
     }
 
+    /// Convenience function for converting time into a (sec, nanosec) representation
+    /// which is commonly used in the ROS ecosystem.
     pub fn to_sec_nanosec(&self) -> Result<(i32, u32), TryFromIntError> {
         let sec = self.nsec / 1_000_000_000;
         let nanosec = self.nsec % 1_000_000_000;
diff --git a/rclrs/src/wait_set/rcl_primitive.rs b/rclrs/src/wait_set/rcl_primitive.rs
index 393d23b5..405df6a0 100644
--- a/rclrs/src/wait_set/rcl_primitive.rs
+++ b/rclrs/src/wait_set/rcl_primitive.rs
@@ -147,7 +147,7 @@ pub struct ActionServerReady {
     pub cancel_request: bool,
     /// True if there is a result request message ready to take, false otherwise.
     pub result_request: bool,
-    //// True if a goal has expired, false otherwise.
+    /// True if a goal has expired, false otherwise.
     pub goal_expired: bool,
 }
 

From d767c7199cbde80ddc73cbb301776a4dc163af9f Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Wed, 30 Jul 2025 18:04:43 +0800
Subject: [PATCH 13/20] Implementing goal status client

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/src/action.rs                           | 44 +++++++++-
 rclrs/src/action/action_client.rs             | 85 ++++++++++++++++---
 .../action/action_client/feedback_client.rs   |  9 +-
 rclrs/src/action/action_client/goal_client.rs | 40 +--------
 .../src/action/action_client/status_client.rs | 46 ++++++++++
 rclrs/src/action/action_server.rs             | 45 +++-------
 .../action_server_goal_handle.rs              |  8 +-
 .../action_server/live_action_server_goal.rs  | 10 +--
 rclrs/src/error.rs                            | 21 +++++
 9 files changed, 206 insertions(+), 102 deletions(-)
 create mode 100644 rclrs/src/action/action_client/status_client.rs

diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs
index 0381560a..782c2af4 100644
--- a/rclrs/src/action.rs
+++ b/rclrs/src/action.rs
@@ -9,7 +9,11 @@ pub use action_goal_receiver::*;
 pub(crate) mod action_server;
 pub use action_server::*;
 
-use crate::rcl_bindings::RCL_ACTION_UUID_SIZE;
+use crate::{
+    rcl_bindings::RCL_ACTION_UUID_SIZE,
+    vendor::builtin_interfaces::msg::Time,
+    log_error,
+};
 use std::fmt;
 
 
@@ -59,8 +63,8 @@ pub enum CancelResponse {
 
 /// Values defined by `action_msgs/msg/GoalStatus`
 #[repr(i8)]
-#[derive(Debug, Clone, Hash, PartialEq, Eq)]
-pub enum GoalStatus {
+#[derive(Debug, Clone, Hash, PartialEq, Eq, PartialOrd, Ord)]
+pub enum GoalStatusCode {
     /// The goal status has never been initialized. This likely means it has not
     /// yet been accepted.
     Unknown = 0,
@@ -79,3 +83,37 @@ pub enum GoalStatus {
     /// during execution or cancelling.
     Aborted = 6,
 }
+
+impl From<i8> for GoalStatusCode {
+    fn from(value: i8) -> Self {
+        if value <= 0 && value <= 6 {
+            unsafe {
+                // SAFETY: We have already ensured that the integer value is
+                // within the acceptable range for the enum, so transmuting is
+                // safe.
+                return std::mem::transmute(value);
+            }
+        }
+
+        log_error!(
+            "goal_status_code.from",
+            "Invalid integer value being cast to a goal status code: {value}. \
+            Values should be in the range [0, 6]. We will set this as 0 (Unknown).",
+        );
+        GoalStatusCode::Unknown
+    }
+}
+
+/// A status update for a goal. Includes the status code, the goal uuid, and the
+/// timestamp of when the status was set by the action server.
+#[derive(Debug, Clone, PartialEq, PartialOrd)]
+pub struct GoalStatus {
+    /// The status code describing what status was set by the action server.
+    pub code: GoalStatusCode,
+    /// The uuid of the goal whose status was updated.
+    pub goal_id: GoalUuid,
+    /// Time that the status was set by the action server. The time measured by
+    /// the action server might not align with the time measured by the action
+    /// client, so care should be taken when using this time value.
+    pub stamp: Time,
+}
diff --git a/rclrs/src/action/action_client.rs b/rclrs/src/action/action_client.rs
index d1500b7d..fa6f83b3 100644
--- a/rclrs/src/action/action_client.rs
+++ b/rclrs/src/action/action_client.rs
@@ -1,14 +1,14 @@
 use crate::{
     rcl_bindings::*,
-    GoalStatus, GoalUuid, ToResult, Node, NodeHandle, QoSProfile,
-    RclrsError, ENTITY_LIFECYCLE_MUTEX,
+    vendor::builtin_interfaces::msg::Time,
+    DropGuard, GoalStatus, GoalUuid, ToResult, Node, NodeHandle,
+    QoSProfile, RclrsError, TakeFailedAsNone, ENTITY_LIFECYCLE_MUTEX,
 };
 use std::{
     borrow::Borrow,
     collections::HashMap,
     ffi::CString,
-    marker::PhantomData,
-    sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard},
+    sync::{Arc, Mutex, MutexGuard},
 };
 use tokio::sync::{
     watch::Sender as WatchSender,
@@ -23,6 +23,9 @@ pub use feedback_client::*;
 mod goal_client;
 pub use goal_client::*;
 
+mod status_client;
+pub use status_client::*;
+
 /// `ActionClientOptions` are used by [`Node::create_action_client`][1] to initialize an
 /// [`ActionClient`].
 ///
@@ -141,7 +144,9 @@ impl<A: Action> ActionClientState<A> {
         let board = Arc::new(ActionClientGoalBoard {
             handle,
             node: Arc::clone(node),
-            result_clients: Default::default(),
+            feedback_senders: Default::default(),
+            status_senders: Default::default(),
+            result_senders: Default::default(),
         });
 
         Ok(Self { board })
@@ -150,7 +155,8 @@ impl<A: Action> ActionClientState<A> {
 
 struct ActionClientGoalBoard<A: Action> {
     feedback_senders: Mutex<HashMap<GoalUuid, Vec<UnboundedSender<A::Feedback>>>>,
-    result_clients: Mutex<HashMap<rmw_request_id_t, Sender<A::Result>>>,
+    status_senders: Mutex<HashMap<GoalUuid, WatchSender<GoalStatus>>>,
+    result_senders: Mutex<HashMap<rmw_request_id_t, Sender<A::Result>>>,
     handle: Arc<ActionClientHandle>,
     /// Ensure the parent node remains alive as long as the subscription is held.
     /// This implementation will change in the future.
@@ -160,7 +166,10 @@ struct ActionClientGoalBoard<A: Action> {
 
 impl<A: Action> ActionClientGoalBoard<A> {
     fn execute_feedback(&self) -> Result<(), RclrsError> {
-        let feedback_rmw = self.handle.take_feedback::<A>()?;
+        let Some(feedback_rmw) = self.handle.take_feedback::<A>().take_failed_as_none()? else {
+            return Ok(());
+        };
+
         let (goal_uuid, feedback) = A::split_feedback_message(feedback_rmw);
         let feedback: A::Feedback = Message::from_rmw_message(feedback);
         if let Some(senders) = self.feedback_senders.lock().unwrap().get(&GoalUuid(goal_uuid)) {
@@ -178,7 +187,30 @@ impl<A: Action> ActionClientGoalBoard<A> {
     }
 
     fn execute_status(&self) -> Result<(), RclrsError> {
-        todo!()
+        let Some(goal_statuses) = self.handle.take_status().take_failed_as_none()? else {
+            return Ok(());
+        };
+
+        let all_status_senders = self.status_senders.lock().unwrap();
+        for index in 0..goal_statuses.msg.status_list.size {
+            let rcl_status = unsafe {
+                &*goal_statuses.msg.status_list.data.add(index)
+            };
+
+            let stamp = &rcl_status.goal_info.stamp;
+            let goal_id = GoalUuid(rcl_status.goal_info.goal_id.uuid);
+            let status = GoalStatus {
+                goal_id,
+                code: rcl_status.status.into(),
+                stamp: Time { sec: stamp.sec, nanosec: stamp.nanosec },
+            };
+
+            if let Some(sender) = all_status_senders.get(&goal_id) {
+                sender.send_modify(|watched_status| *watched_status = status);
+            }
+        }
+
+        Ok(())
     }
 
     fn execute_goal_response(&self) -> Result<(), RclrsError> {
@@ -215,15 +247,23 @@ impl<A: Action> Drop for GoalClientLifecycle<A> {
             GoalClientKind::Feedback(goal_uuid) => {
                 let mut all_feedback_senders = self.board.feedback_senders.lock().unwrap();
 
-                let mut empty = false;
+                let mut is_empty = false;
                 if let Some(senders) = all_feedback_senders.get_mut(&goal_uuid) {
                     senders.retain(|sender| !sender.is_closed());
-                    empty = senders.is_empty();
+                    is_empty = senders.is_empty();
                 }
-                if empty {
+                if is_empty {
                     all_feedback_senders.remove(&goal_uuid);
                 }
             }
+            GoalClientKind::Status(goal_uuid) => {
+                let mut all_status_senders = self.board.status_senders.lock().unwrap();
+
+                let remove = all_status_senders.get(&goal_uuid).is_some_and(|sender| sender.is_closed());
+                if remove {
+                    all_status_senders.remove(&goal_uuid);
+                }
+            }
         }
     }
 }
@@ -254,8 +294,31 @@ impl ActionClientHandle {
             rcl_action_take_feedback(&*handle, &mut feedback_rmw as *mut _ as *mut _)
         }
         .ok()?;
+
         Ok(feedback_rmw)
     }
+
+    fn take_status(&self) -> Result<DropGuard<rcl_action_goal_status_array_t>, RclrsError> {
+        let mut goal_statuses = DropGuard::new(
+            unsafe {
+                // SAFETY: No preconditions
+                rcl_action_get_zero_initialized_goal_status_array()
+            },
+            |mut goal_statuses| unsafe {
+                // SAFETY: The goal_status array is either zero-initialized and empty or populated by
+                // `rcl_action_get_goal_status_array`. In either case, it can be safely finalized.
+                rcl_action_goal_status_array_fini(&mut goal_statuses);
+            }
+        );
+
+        unsafe {
+            let handle = self.lock();
+            rcl_action_take_status(&*handle, &mut *goal_statuses as *mut _ as *mut _)
+        }
+        .ok()?;
+
+        Ok(goal_statuses)
+    }
 }
 
 impl Drop for ActionClientHandle {
diff --git a/rclrs/src/action/action_client/feedback_client.rs b/rclrs/src/action/action_client/feedback_client.rs
index d2a69a6d..fa29d8b0 100644
--- a/rclrs/src/action/action_client/feedback_client.rs
+++ b/rclrs/src/action/action_client/feedback_client.rs
@@ -1,11 +1,6 @@
-use crate::{GoalStatus, GoalUuid};
-use super::{ActionClientGoalBoard, GoalClientLifecycle};
+use super::GoalClientLifecycle;
 use rosidl_runtime_rs::Action;
-use tokio::sync::{
-    watch::Receiver as Watcher,
-    mpsc::UnboundedReceiver,
-    oneshot::Receiver,
-};
+use tokio::sync::mpsc::UnboundedReceiver;
 use std::{
     ops::{Deref, DerefMut},
     sync::Arc,
diff --git a/rclrs/src/action/action_client/goal_client.rs b/rclrs/src/action/action_client/goal_client.rs
index 38e2bfe9..17daa897 100644
--- a/rclrs/src/action/action_client/goal_client.rs
+++ b/rclrs/src/action/action_client/goal_client.rs
@@ -1,9 +1,7 @@
-use crate::{GoalStatus, GoalUuid, FeedbackClient};
-use super::{ActionClientGoalBoard, GoalClientLifecycle};
+use crate::{FeedbackClient, StatusClient};
+use super::{GoalClientLifecycle};
 use rosidl_runtime_rs::Action;
 use tokio::sync::{
-    watch::Receiver as Watcher,
-    mpsc::UnboundedReceiver,
     oneshot::Receiver,
 };
 use std::{
@@ -32,40 +30,6 @@ pub struct GoalClient<A: Action> {
     pub result: Receiver<A::Result>
 }
 
-/// This struct allows you to monitor the status of the goal. Through the
-/// [`DerefMut`] trait you can use the [`watch::Receiver`][Watcher] API to
-/// check the latest status and monitor changes.
-pub struct StatusClient<A: Action> {
-    receiver: Watcher<GoalStatus>,
-    /// This keeps track of whether any goal client components are still being used.
-    /// This must come after the receiver in the struct to ensure cleanup is done
-    /// correctly.
-    #[allow(unused)]
-    lifecycle: Arc<GoalClientLifecycle<A>>,
-}
-
-impl<A: Action> Clone for StatusClient<A> {
-    fn clone(&self) -> Self {
-        Self {
-            receiver: self.receiver.clone(),
-            lifecycle: Arc::clone(&self.lifecycle),
-        }
-    }
-}
-
-impl<A: Action> Deref for StatusClient<A> {
-    type Target = Watcher<GoalStatus>;
-    fn deref(&self) -> &Self::Target {
-        &self.receiver
-    }
-}
-
-impl<A: Action> DerefMut for StatusClient<A> {
-    fn deref_mut(&mut self) -> &mut Self::Target {
-        &mut self.receiver
-    }
-}
-
 /// This struct allows you to receive the result of the goal. Through the [`DerefMut`]
 /// trait you can use the [`oneshot::Receiver`][Receiver] API to await the final
 /// result. Note that it can only be received once.
diff --git a/rclrs/src/action/action_client/status_client.rs b/rclrs/src/action/action_client/status_client.rs
new file mode 100644
index 00000000..163792e7
--- /dev/null
+++ b/rclrs/src/action/action_client/status_client.rs
@@ -0,0 +1,46 @@
+use crate::{GoalStatusCode, GoalUuid, FeedbackClient};
+use super::{ActionClientGoalBoard, GoalClientLifecycle};
+use rosidl_runtime_rs::Action;
+use tokio::sync::{
+    watch::Receiver as Watcher,
+    mpsc::UnboundedReceiver,
+    oneshot::Receiver,
+};
+use std::{
+    ops::{Deref, DerefMut},
+    sync::Arc,
+};
+
+/// This struct allows you to monitor the status of the goal. Through the
+/// [`DerefMut`] trait you can use the [`watch::Receiver`][Watcher] API to
+/// check the latest status and monitor changes.
+pub struct StatusClient<A: Action> {
+    receiver: Watcher<GoalStatusCode>,
+    /// This keeps track of whether any goal client components are still being used.
+    /// This must come after the receiver in the struct to ensure cleanup is done
+    /// correctly.
+    #[allow(unused)]
+    lifecycle: Arc<GoalClientLifecycle<A>>,
+}
+
+impl<A: Action> Clone for StatusClient<A> {
+    fn clone(&self) -> Self {
+        Self {
+            receiver: self.receiver.clone(),
+            lifecycle: Arc::clone(&self.lifecycle),
+        }
+    }
+}
+
+impl<A: Action> Deref for StatusClient<A> {
+    type Target = Watcher<GoalStatusCode>;
+    fn deref(&self) -> &Self::Target {
+        &self.receiver
+    }
+}
+
+impl<A: Action> DerefMut for StatusClient<A> {
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        &mut self.receiver
+    }
+}
diff --git a/rclrs/src/action/action_server.rs b/rclrs/src/action/action_server.rs
index add53ed5..d55b2fc5 100644
--- a/rclrs/src/action/action_server.rs
+++ b/rclrs/src/action/action_server.rs
@@ -2,8 +2,9 @@ use crate::{
     action::GoalUuid,
     error::ToResult,
     rcl_bindings::*,
-    ActionGoalReceiver, DropGuard, GoalStatus, Node, NodeHandle, QoSProfile, RclPrimitive, RclrsError,
-    RclPrimitiveHandle, RclPrimitiveKind, ReadyKind, Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
+    ActionGoalReceiver, DropGuard, GoalStatusCode, Node, NodeHandle, QoSProfile, RclPrimitive, RclrsError,
+    RclPrimitiveHandle, RclPrimitiveKind, ReadyKind, TakeFailedAsNone,
+    Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
 };
 use rosidl_runtime_rs::{Action, Message, RmwGoalRequest, RmwResultRequest};
 use std::{
@@ -260,7 +261,7 @@ impl<A: Action> ActionServerState<A> {
     }
 
     /// Used internally to change a receiver into an action server without the
-    /// risk of dropping buffered any goal requests or receiving goals out of
+    /// risk of dropping any buffered goal requests or receiving goals out of
     /// their original order.
     pub(super) fn drain_receiver_into_callback<Task>(
         &self,
@@ -322,17 +323,8 @@ impl<A: Action> ActionServerGoalBoard<A> {
     }
 
     fn execute_goal_request(self: &Arc<Self>) -> Result<(), RclrsError> {
-        let (request, goal_request_id) = match self.handle.take_goal_request() {
-            Ok(res) => res,
-            Err(err) => {
-                if err.is_take_failed() {
-                    // Spurious wakeup – this may happen even when a waitset indicated that this
-                    // action was ready, so it shouldn't be an error.
-                    return Ok(());
-                }
-
-                return Err(err);
-            }
+        let Some((request, goal_request_id)) = self.handle.take_goal_request().take_failed_as_none()? else {
+            return Ok(());
         };
 
         let (uuid, request) = <A as Action>::split_goal_request(request);
@@ -361,17 +353,8 @@ impl<A: Action> ActionServerGoalBoard<A> {
     }
 
     fn execute_cancel_request(&self) -> Result<(), RclrsError> {
-        let (request, request_id) = match self.handle.take_cancel_request() {
-            Ok(res) => res,
-            Err(err) => {
-                if err.is_take_failed() {
-                    // Spurious wakeup – this may happen even when a waitset indicated that this
-                    // action was ready, so it shouldn't be an error.
-                    return Ok(());
-                }
-
-                return Err(err);
-            }
+        let Some((request, request_id)) = self.handle.take_cancel_request().take_failed_as_none()? else {
+            return Ok(());
         };
 
         let response_rmw = {
@@ -436,14 +419,8 @@ impl<A: Action> ActionServerGoalBoard<A> {
     }
 
     fn execute_result_request(&self) -> Result<(), RclrsError> {
-        let (request, mut request_id) = match self.handle.take_result_request() {
-            Ok(res) => res,
-            Err(err) => {
-                if err.is_take_failed() {
-                    return Ok(());
-                }
-                return Err(err);
-            },
+        let Some((request, mut request_id)) = self.handle.take_result_request().take_failed_as_none()? else {
+            return Ok(());
         };
 
         let uuid = GoalUuid(*<A as Action>::get_result_request_uuid(&request));
@@ -453,7 +430,7 @@ impl<A: Action> ActionServerGoalBoard<A> {
             // The goal either never existed or expired, so we give back an
             // unknown response
             let result_rmw = <<A::Result as Message>::RmwMsg as Default>::default();
-            let mut response_rmw = A::create_result_response(GoalStatus::Unknown as i8, result_rmw);
+            let mut response_rmw = A::create_result_response(GoalStatusCode::Unknown as i8, result_rmw);
 
             let server = self.handle.lock();
             unsafe {
diff --git a/rclrs/src/action/action_server/action_server_goal_handle.rs b/rclrs/src/action/action_server/action_server_goal_handle.rs
index 06c93a7e..e71df5e7 100644
--- a/rclrs/src/action/action_server/action_server_goal_handle.rs
+++ b/rclrs/src/action/action_server/action_server_goal_handle.rs
@@ -3,7 +3,7 @@ use crate::{
     log_error,
     GoalUuid, ToResult, RclrsError, RclReturnCode, RclErrorMsg, ActionServerHandle,
 };
-use super::{GoalStatus};
+use super::{GoalStatusCode};
 use std::sync::{Mutex, MutexGuard};
 use rosidl_runtime_rs::{Action, RmwResultResponse};
 
@@ -40,8 +40,8 @@ impl<A: Action> ActionServerGoalHandle<A> {
     }
 
     /// Returns the goal state.
-    pub(super) fn get_status(&self) -> GoalStatus {
-        let mut state = GoalStatus::Unknown as rcl_action_goal_state_t;
+    pub(super) fn get_status(&self) -> GoalStatusCode {
+        let mut state = GoalStatusCode::Unknown as rcl_action_goal_state_t;
         {
             let rcl_handle = self.rcl_handle.lock().unwrap();
             // SAFETY: The provided goal handle is properly initialized by construction.
@@ -61,7 +61,7 @@ impl<A: Action> ActionServerGoalHandle<A> {
     /// This is used to check if we should respond as accepting a cancellation
     /// request for this goal after it is no longer live.
     pub(super) fn is_cancelled(&self) -> bool {
-        self.get_status() == GoalStatus::Cancelled
+        self.get_status() == GoalStatusCode::Cancelled
     }
 
     /// Get the unique identifier of the goal.
diff --git a/rclrs/src/action/action_server/live_action_server_goal.rs b/rclrs/src/action/action_server/live_action_server_goal.rs
index b6a9a721..74e311f5 100644
--- a/rclrs/src/action/action_server/live_action_server_goal.rs
+++ b/rclrs/src/action/action_server/live_action_server_goal.rs
@@ -5,7 +5,7 @@ use crate::{
 };
 use super::{
     ActionServerGoalHandle, ActionServerHandle, CancellationState,
-    CancellationRequest, GoalStatus, TerminalStatus,
+    CancellationRequest, GoalStatusCode, TerminalStatus,
 };
 use std::{
     borrow::Cow,
@@ -236,19 +236,19 @@ impl<A: Action> Deref for LiveActionServerGoal<A> {
 impl<A: Action> Drop for LiveActionServerGoal<A> {
     fn drop(&mut self) {
         match self.get_status() {
-            GoalStatus::Accepted => {
+            GoalStatusCode::Accepted => {
                 // Transition into executing and then into aborted to reach a
                 // terminal state.
                 self.transition_to_executing();
                 self.transition_to_aborted(&Default::default());
             }
-            GoalStatus::Cancelling | GoalStatus::Executing => {
+            GoalStatusCode::Cancelling | GoalStatusCode::Executing => {
                 self.transition_to_aborted(&Default::default());
             }
-            GoalStatus::Succeeded | GoalStatus::Cancelled | GoalStatus::Aborted => {
+            GoalStatusCode::Succeeded | GoalStatusCode::Cancelled | GoalStatusCode::Aborted => {
                 // Already in a terminal state, no need to do anything.
             }
-            GoalStatus::Unknown => {
+            GoalStatusCode::Unknown => {
                 log_error!(
                     "LiveActionServerGoal.drop",
                     "Goal status is unknown. This indicates a bug, please \
diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs
index 8b3e449b..78222bcb 100644
--- a/rclrs/src/error.rs
+++ b/rclrs/src/error.rs
@@ -596,3 +596,24 @@ impl RclrsErrorFilter for Vec<RclrsError> {
         self
     }
 }
+
+pub trait TakeFailedAsNone {
+    type T;
+    fn take_failed_as_none(self) -> Result<Option<Self::T>, RclrsError>;
+}
+
+impl<T> TakeFailedAsNone for Result<T, RclrsError> {
+    type T = T;
+    fn take_failed_as_none(self) -> Result<Option<T>, RclrsError> {
+        match self {
+            Ok(value) => Ok(Some(value)),
+            Err(err) => {
+                if err.is_take_failed() {
+                    return Ok(None);
+                }
+
+                return Err(err);
+            }
+        }
+    }
+}

From 50bb189e4b52e46913bca9cfeacfc56ed7cd21cb Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Wed, 30 Jul 2025 19:19:17 +0800
Subject: [PATCH 14/20] Implementing goal result client

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/src/action/action_client.rs             | 66 ++++++++++++++++---
 .../action_client/cancellation_client.rs      | 10 ++-
 .../action/action_client/feedback_client.rs   |  7 +-
 rclrs/src/action/action_client/goal_client.rs | 38 +----------
 .../src/action/action_client/result_client.rs | 31 +++++++++
 5 files changed, 101 insertions(+), 51 deletions(-)
 create mode 100644 rclrs/src/action/action_client/result_client.rs

diff --git a/rclrs/src/action/action_client.rs b/rclrs/src/action/action_client.rs
index fa6f83b3..12b9b41a 100644
--- a/rclrs/src/action/action_client.rs
+++ b/rclrs/src/action/action_client.rs
@@ -1,7 +1,7 @@
 use crate::{
     rcl_bindings::*,
     vendor::builtin_interfaces::msg::Time,
-    DropGuard, GoalStatus, GoalUuid, ToResult, Node, NodeHandle,
+    DropGuard, GoalStatus, GoalStatusCode, GoalUuid, ToResult, Node, NodeHandle,
     QoSProfile, RclrsError, TakeFailedAsNone, ENTITY_LIFECYCLE_MUTEX,
 };
 use std::{
@@ -15,7 +15,10 @@ use tokio::sync::{
     mpsc::UnboundedSender,
     oneshot::Sender,
 };
-use rosidl_runtime_rs::{Action, Message, RmwFeedbackMessage};
+use rosidl_runtime_rs::{Action, Message, RmwFeedbackMessage, RmwResultResponse};
+
+mod cancellation_client;
+pub use cancellation_client::*;
 
 mod feedback_client;
 pub use feedback_client::*;
@@ -26,6 +29,9 @@ pub use goal_client::*;
 mod status_client;
 pub use status_client::*;
 
+mod result_client;
+pub use result_client::*;
+
 /// `ActionClientOptions` are used by [`Node::create_action_client`][1] to initialize an
 /// [`ActionClient`].
 ///
@@ -156,7 +162,7 @@ impl<A: Action> ActionClientState<A> {
 struct ActionClientGoalBoard<A: Action> {
     feedback_senders: Mutex<HashMap<GoalUuid, Vec<UnboundedSender<A::Feedback>>>>,
     status_senders: Mutex<HashMap<GoalUuid, WatchSender<GoalStatus>>>,
-    result_senders: Mutex<HashMap<rmw_request_id_t, Sender<A::Result>>>,
+    result_senders: Mutex<HashMap<i64, Sender<(GoalStatusCode, A::Result)>>>,
     handle: Arc<ActionClientHandle>,
     /// Ensure the parent node remains alive as long as the subscription is held.
     /// This implementation will change in the future.
@@ -222,22 +228,42 @@ impl<A: Action> ActionClientGoalBoard<A> {
     }
 
     fn execute_result_response(&self) -> Result<(), RclrsError> {
-        todo!()
+        let Some((result_rmw, header)) = self.handle.take_result_response::<A>().take_failed_as_none()? else {
+            return Ok(());
+        };
+
+        let (status, result) = A::split_result_response(result_rmw);
+        let status_code = status.into();
+
+        let seq = header.sequence_number;
+        let Some(sender) = self.result_senders.lock().unwrap().remove(&seq) else {
+            // If the sender doesn't exist, most likely it means the ResultClient
+            // has been dropped.
+            return Ok(());
+        };
+
+        let result = Message::from_rmw_message(result);
+        sender.send((status_code, result));
+        Ok(())
     }
 }
 
 /// Once all of the constituent clients for the goal are dropped, this will
 /// remove the [`GoalClientSender`][super::GoalClientSender] from the goal
 /// board.
+///
+/// This also ensures that the action client remains alive for as long as any
+/// of its constituent clients are using it.
 struct GoalClientLifecycle<A: Action> {
     kind: GoalClientKind,
-    board: Arc<ActionClientGoalBoard<A>>,
+    client: Arc<ActionClient<A>>,
 }
 
 enum GoalClientKind {
+    Request(i64),
     Feedback(GoalUuid),
     Status(GoalUuid),
-    Result(rmw_request_id_t),
+    Result(i64),
     Cancellation,
 }
 
@@ -245,7 +271,7 @@ impl<A: Action> Drop for GoalClientLifecycle<A> {
     fn drop(&mut self) {
         match self.kind {
             GoalClientKind::Feedback(goal_uuid) => {
-                let mut all_feedback_senders = self.board.feedback_senders.lock().unwrap();
+                let mut all_feedback_senders = self.client.board.feedback_senders.lock().unwrap();
 
                 let mut is_empty = false;
                 if let Some(senders) = all_feedback_senders.get_mut(&goal_uuid) {
@@ -257,13 +283,17 @@ impl<A: Action> Drop for GoalClientLifecycle<A> {
                 }
             }
             GoalClientKind::Status(goal_uuid) => {
-                let mut all_status_senders = self.board.status_senders.lock().unwrap();
+                let mut all_status_senders = self.client.board.status_senders.lock().unwrap();
 
                 let remove = all_status_senders.get(&goal_uuid).is_some_and(|sender| sender.is_closed());
                 if remove {
                     all_status_senders.remove(&goal_uuid);
                 }
             }
+            GoalClientKind::Result(seq) => {
+                let mut all_result_senders = self.client.board.result_senders.lock().unwrap();
+                all_result_senders.remove(&seq);
+            }
         }
     }
 }
@@ -319,6 +349,26 @@ impl ActionClientHandle {
 
         Ok(goal_statuses)
     }
+
+    fn take_result_response<A: Action>(&self) -> Result<(RmwResultResponse<A>, rmw_request_id_t), RclrsError> {
+        let mut result_rmw = RmwResultResponse::<A>::default();
+        let mut response_header = rmw_request_id_t {
+            writer_guid: [0; 16],
+            sequence_number: 0,
+        };
+
+        unsafe {
+            let handle = self.lock();
+            rcl_action_take_result_response(
+                &*handle,
+                &mut response_header,
+                &mut result_rmw as *mut _ as *mut _,
+            )
+        }
+        .ok()?;
+
+        Ok((result_rmw, response_header))
+    }
 }
 
 impl Drop for ActionClientHandle {
diff --git a/rclrs/src/action/action_client/cancellation_client.rs b/rclrs/src/action/action_client/cancellation_client.rs
index 971292bc..6d452532 100644
--- a/rclrs/src/action/action_client/cancellation_client.rs
+++ b/rclrs/src/action/action_client/cancellation_client.rs
@@ -1,5 +1,11 @@
-
+use super::GoalClientLifecycle;
+use rosidl_runtime_rs::Action;
+use tokio::sync::mpsc::UnboundedReceiver;
+use std::{
+    ops::{Deref, DerefMut},
+    sync::Arc,
+};
 
 pub struct CancellationClient<A: Action> {
-    board: Arc<ActionClientGoalBoard<A>>,
+    board: Arc<GoalClientLifecycle<A>>,
 }
diff --git a/rclrs/src/action/action_client/feedback_client.rs b/rclrs/src/action/action_client/feedback_client.rs
index fa29d8b0..2bb256cc 100644
--- a/rclrs/src/action/action_client/feedback_client.rs
+++ b/rclrs/src/action/action_client/feedback_client.rs
@@ -1,10 +1,7 @@
 use super::GoalClientLifecycle;
 use rosidl_runtime_rs::Action;
 use tokio::sync::mpsc::UnboundedReceiver;
-use std::{
-    ops::{Deref, DerefMut},
-    sync::Arc,
-};
+use std::ops::{Deref, DerefMut};
 
 /// This struct allows you to receive feedback messages for the goal. Through the
 /// [`DerefMut`] trait you can use the [`UnboundedReceiver`] API to await new
@@ -15,7 +12,7 @@ pub struct FeedbackClient<A: Action> {
     /// This must come after the receiver in the struct to ensure cleanup is done
     /// correctly.
     #[allow(unused)]
-    lifecycle: Arc<GoalClientLifecycle<A>>,
+    lifecycle: GoalClientLifecycle<A>,
 }
 
 impl<A: Action> Deref for FeedbackClient<A> {
diff --git a/rclrs/src/action/action_client/goal_client.rs b/rclrs/src/action/action_client/goal_client.rs
index 17daa897..65042a8c 100644
--- a/rclrs/src/action/action_client/goal_client.rs
+++ b/rclrs/src/action/action_client/goal_client.rs
@@ -1,13 +1,5 @@
-use crate::{FeedbackClient, StatusClient};
-use super::{GoalClientLifecycle};
+use crate::{FeedbackClient, StatusClient, ResultClient};
 use rosidl_runtime_rs::Action;
-use tokio::sync::{
-    oneshot::Receiver,
-};
-use std::{
-    ops::{Deref, DerefMut},
-    sync::Arc,
-};
 
 /// The goal client bundles a set of receivers that will allow you to await
 /// different information from the action server, such as feedback messages,
@@ -27,31 +19,5 @@ pub struct GoalClient<A: Action> {
     /// Watch the status of the goal.
     pub status: StatusClient<A>,
     /// Get the final result of the goal.
-    pub result: Receiver<A::Result>
+    pub result: ResultClient<A>,
 }
-
-/// This struct allows you to receive the result of the goal. Through the [`DerefMut`]
-/// trait you can use the [`oneshot::Receiver`][Receiver] API to await the final
-/// result. Note that it can only be received once.
-pub struct ResultClient<A: Action> {
-    receiver: Receiver<A::Result>,
-    /// This keeps track of whether any goal client components are still being used.
-    /// This must come after the receiver in the struct to ensure cleanup is done
-    /// correctly.
-    #[allow(unused)]
-    lifecycle: Arc<GoalClientLifecycle<A>>,
-}
-
-impl<A: Action> Deref for ResultClient<A> {
-    type Target = Receiver<A::Result>;
-    fn deref(&self) -> &Self::Target {
-        &self.receiver
-    }
-}
-
-impl<A: Action> DerefMut for ResultClient<A> {
-    fn deref_mut(&mut self) -> &mut Self::Target {
-        &mut self.receiver
-    }
-}
-
diff --git a/rclrs/src/action/action_client/result_client.rs b/rclrs/src/action/action_client/result_client.rs
new file mode 100644
index 00000000..ad21483c
--- /dev/null
+++ b/rclrs/src/action/action_client/result_client.rs
@@ -0,0 +1,31 @@
+use crate::GoalStatusCode;
+use super::GoalClientLifecycle;
+use rosidl_runtime_rs::Action;
+use tokio::sync::oneshot::Receiver;
+use std::ops::{Deref, DerefMut};
+
+/// This struct allows you to receive the result of the goal. Through the [`DerefMut`]
+/// trait you can use the [`oneshot::Receiver`][Receiver] API to await the final
+/// result. Note that it can only be received once.
+pub struct ResultClient<A: Action> {
+    receiver: Receiver<(GoalStatusCode, A::Result)>,
+    /// This keeps track of whether any goal client components are still being used.
+    /// This must come after the receiver in the struct to ensure cleanup is done
+    /// correctly.
+    #[allow(unused)]
+    lifecycle: GoalClientLifecycle<A>,
+}
+
+impl<A: Action> Deref for ResultClient<A> {
+    type Target = Receiver<(GoalStatusCode, A::Result)>;
+    fn deref(&self) -> &Self::Target {
+        &self.receiver
+    }
+}
+
+impl<A: Action> DerefMut for ResultClient<A> {
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        &mut self.receiver
+    }
+}
+

From 176b5f2d37c4b5db472e8922f7fe8aa9f608076b Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Wed, 30 Jul 2025 23:38:56 +0800
Subject: [PATCH 15/20] CancellationClient API in progress

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/src/action.rs                           |  49 ++-
 rclrs/src/action/action_client.rs             | 289 ++++++++++++++++--
 .../action_client/cancellation_client.rs      |  76 ++++-
 rclrs/src/action/action_client/goal_client.rs |   9 +-
 .../action_client/requested_goal_client.rs    |  33 ++
 .../src/action/action_client/result_client.rs |   9 +
 .../src/action/action_client/status_client.rs |  10 +-
 .../action_server/cancellation_state.rs       |   6 +-
 8 files changed, 444 insertions(+), 37 deletions(-)
 create mode 100644 rclrs/src/action/action_client/requested_goal_client.rs

diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs
index 782c2af4..b236ff7b 100644
--- a/rclrs/src/action.rs
+++ b/rclrs/src/action.rs
@@ -53,17 +53,56 @@ impl Deref for GoalUuid {
 }
 
 /// The response returned by an [`ActionServer`]'s cancel callback when a goal is requested to be cancelled.
-#[derive(PartialEq, Eq)]
-pub enum CancelResponse {
+#[repr(i8)]
+#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
+pub enum CancelResponseCode {
+    /// The server will try to cancel the goal.
+    Accept = 0,
     /// The server will not try to cancel the goal.
     Reject = 1,
-    /// The server will try to cancel the goal.
-    Accept = 2,
+    /// The requested goal is unknown.
+    UnknownGoal = 2,
+    /// The goal already reached a terminal state.
+    GoalTerminated = 3,
+}
+
+impl CancelResponseCode {
+    /// Check if the cancellation was accepted.
+    pub fn is_accepted(&self) -> bool {
+        matches!(self, Self::Accept)
+    }
+}
+
+impl From<i8> for CancelResponseCode {
+    fn from(value: i8) -> Self {
+        if value <= 0 && value <= 3 {
+            unsafe {
+                // SAFETY: We have already ensured that the integer value is
+                // within the acceptable range for the enum, so transmuting is
+                // safe.
+                return std::mem::transmute(value);
+            }
+        }
+
+        log_error!(
+            "cancel_response.from",
+            "Invalid integer value being cast to a cancel response: {value}. \
+            Values should be in the range [0, 3]. We will set this as 1 (Reject).",
+        );
+        CancelResponseCode::Reject
+    }
+}
+
+#[derive(Debug, Clone, PartialEq, PartialOrd)]
+pub struct CancelResponse {
+    pub code: CancelResponseCode,
+    pub goal_id: GoalUuid,
+    pub stamp: Time,
 }
 
 /// Values defined by `action_msgs/msg/GoalStatus`
 #[repr(i8)]
-#[derive(Debug, Clone, Hash, PartialEq, Eq, PartialOrd, Ord)]
+#[derive(Debug, Clone, Copy, Hash, PartialEq, Eq, PartialOrd, Ord)]
 pub enum GoalStatusCode {
     /// The goal status has never been initialized. This likely means it has not
     /// yet been accepted.
diff --git a/rclrs/src/action/action_client.rs b/rclrs/src/action/action_client.rs
index 12b9b41a..062b47f5 100644
--- a/rclrs/src/action/action_client.rs
+++ b/rclrs/src/action/action_client.rs
@@ -1,21 +1,27 @@
 use crate::{
     rcl_bindings::*,
-    vendor::builtin_interfaces::msg::Time,
-    DropGuard, GoalStatus, GoalStatusCode, GoalUuid, ToResult, Node, NodeHandle,
-    QoSProfile, RclrsError, TakeFailedAsNone, ENTITY_LIFECYCLE_MUTEX,
+    vendor::{
+        action_msgs::{msg::GoalInfo, srv::{CancelGoal_Response, CancelGoal_Request}},
+        builtin_interfaces::msg::Time,
+        unique_identifier_msgs::msg::UUID,
+    },
+    CancelResponse, CancelResponseCode, DropGuard, GoalStatus, GoalStatusCode, GoalUuid, ToResult, Node,
+    NodeHandle, QoSProfile, RclrsError, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind,
+    ReadyKind, TakeFailedAsNone, Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
 };
 use std::{
-    borrow::Borrow,
+    any::Any,
+    borrow::{Borrow, Cow},
     collections::HashMap,
     ffi::CString,
-    sync::{Arc, Mutex, MutexGuard},
+    sync::{Arc, Mutex, MutexGuard, Weak},
 };
 use tokio::sync::{
     watch::Sender as WatchSender,
     mpsc::UnboundedSender,
-    oneshot::Sender,
+    oneshot::{channel as oneshot_channel, Sender},
 };
-use rosidl_runtime_rs::{Action, Message, RmwFeedbackMessage, RmwResultResponse};
+use rosidl_runtime_rs::{Action, Message, RmwGoalResponse, RmwFeedbackMessage, RmwResultResponse};
 
 mod cancellation_client;
 pub use cancellation_client::*;
@@ -32,6 +38,9 @@ pub use status_client::*;
 mod result_client;
 pub use result_client::*;
 
+mod requested_goal_client;
+pub use requested_goal_client::*;
+
 /// `ActionClientOptions` are used by [`Node::create_action_client`][1] to initialize an
 /// [`ActionClient`].
 ///
@@ -95,17 +104,16 @@ pub type ActionClient<A> = Arc<ActionClientState<A>>;
 /// [1]: std::sync::Weak
 pub struct ActionClientState<A: Action> {
     board: Arc<ActionClientGoalBoard<A>>,
+    #[allow(unused)]
+    lifecycle: WaitableLifecycle,
 }
 
 impl<A: Action> ActionClientState<A> {
     /// Creates a new action client.
-    pub(crate) fn new<'a>(
+    pub(crate) fn create<'a>(
         node: &Node,
         options: impl Into<ActionClientOptions<'a>>,
-    ) -> Result<Self, RclrsError>
-    where
-        A: rosidl_runtime_rs::Action,
-    {
+    ) -> Result<Arc<Self>, RclrsError> {
         let options = options.into();
         // SAFETY: Getting a zero-initialized value is always safe.
         let mut rcl_action_client = unsafe { rcl_action_get_zero_initialized_client() };
@@ -150,26 +158,50 @@ impl<A: Action> ActionClientState<A> {
         let board = Arc::new(ActionClientGoalBoard {
             handle,
             node: Arc::clone(node),
+            pending_goal_clients: Default::default(),
             feedback_senders: Default::default(),
             status_senders: Default::default(),
+            cancel_response_senders: Default::default(),
             result_senders: Default::default(),
+            client: Default::default(),
         });
 
-        Ok(Self { board })
+        let async_commands = node.commands().async_worker_commands();
+        let (waitable, lifecycle) = Waitable::new(
+            Box::new(ActionClientExecutable {
+                board: Arc::clone(&board),
+            }),
+            Some(Arc::clone(async_commands.get_guard_condition())),
+        );
+        async_commands.add_to_wait_set(waitable);
+
+        let client = Arc::new(Self { board, lifecycle });
+        *client.board.client.lock().unwrap() = Arc::downgrade(&client);
+        Ok(client)
     }
 }
 
 struct ActionClientGoalBoard<A: Action> {
+    pending_goal_clients: Mutex<HashMap<i64, PendingGoalClient<A>>>,
     feedback_senders: Mutex<HashMap<GoalUuid, Vec<UnboundedSender<A::Feedback>>>>,
     status_senders: Mutex<HashMap<GoalUuid, WatchSender<GoalStatus>>>,
+    cancel_response_senders: Mutex<HashMap<i64, CancelResponseSender>>,
     result_senders: Mutex<HashMap<i64, Sender<(GoalStatusCode, A::Result)>>>,
     handle: Arc<ActionClientHandle>,
+    client: Mutex<Weak<ActionClientState<A>>>,
     /// Ensure the parent node remains alive as long as the subscription is held.
     /// This implementation will change in the future.
     #[allow(unused)]
     node: Node,
 }
 
+enum CancelResponseSender {
+    /// Used when only a single goal is being cancelled
+    SingleGoalCancel(Sender<CancelResponse>),
+    /// Used when multiple goals are being cancelled
+    MultiGoalCancel(Sender<(CancelResponseCode, Vec<GoalUuid>)>),
+}
+
 impl<A: Action> ActionClientGoalBoard<A> {
     fn execute_feedback(&self) -> Result<(), RclrsError> {
         let Some(feedback_rmw) = self.handle.take_feedback::<A>().take_failed_as_none()? else {
@@ -220,11 +252,64 @@ impl<A: Action> ActionClientGoalBoard<A> {
     }
 
     fn execute_goal_response(&self) -> Result<(), RclrsError> {
-        todo!()
+        let Some(client) = self.client.lock().unwrap().upgrade() else {
+            // The action client has already expired which means the user is not
+            // waiting for this response any longer. We can just discard it.
+            return Ok(());
+        };
+
+        let Some((response_rmw, header)) = self.handle.take_goal_response::<A>().take_failed_as_none()? else {
+            return Ok(());
+        };
+
+        let accepted = A::get_goal_response_accepted(&response_rmw);
+        let stamp = A::get_goal_response_stamp(&response_rmw);
+
+        let Some(pending) = self.pending_goal_clients.lock().unwrap().remove(&header.sequence_number) else {
+            // This suggests that the RequestedGoalClient was dropped before the result arrived.
+            return Ok(());
+        };
+
+        if accepted {
+            let _ = pending.sender.send(
+                Some(GoalClient {
+                    feedback: pending.feedback,
+                    status: pending.status,
+                    result: self.request_result(Arc::clone(&client), pending.goal_id)?,
+                    cancellation: CancellationClient::new(client, pending.goal_id),
+                    stamp: Time { sec: stamp.0, nanosec: stamp.1 },
+                })
+            );
+        } else {
+            pending.sender.send(None);
+        }
+
+        Ok(())
     }
 
     fn execute_cancel_response(&self) -> Result<(), RclrsError> {
-        todo!()
+        let Some((result, header)) = self.handle.take_cancel_response().take_failed_as_none()? else {
+            return Ok(());
+        };
+
+        let seq = header.sequence_number;
+        let Some(sender) = self.cancel_response_senders.lock().unwrap().remove(&seq) else {
+            // If the sender doesn't exist, most likely it means the CancelResponseClient
+            // has been dropped.
+            return Ok(());
+        };
+
+        let code =
+        match sender {
+            CancelResponseSender::SingleGoalCancel(sender) => {
+                let _ = sender.send(result.return_code.into());
+            }
+            CancelResponseSender::MultiGoalCancel(sender) => {
+                result.goals_canceling
+            }
+        }
+
+        Ok(())
     }
 
     fn execute_result_response(&self) -> Result<(), RclrsError> {
@@ -242,10 +327,65 @@ impl<A: Action> ActionClientGoalBoard<A> {
             return Ok(());
         };
 
+
         let result = Message::from_rmw_message(result);
-        sender.send((status_code, result));
+        let _ = sender.send((status_code, result));
         Ok(())
     }
+
+    fn request_single_cancel(
+        &self,
+        client: ActionClient<A>,
+        goal_id: GoalUuid,
+    ) -> Result<CancelResponseClient<A>, RclrsError> {
+        let seq = self.handle.send_cancel_goal(goal_id, Time { sec: 0, nanosec: 0 })?;
+        let (sender, receiver) = oneshot_channel();
+        self.cancel_response_senders.lock().unwrap().insert(seq, CancelResponseSender::SingleGoalCancel(sender));
+        Ok(CancelResponseClient::new(
+            receiver,
+            GoalClientLifecycle {
+                kind: GoalClientKind::CancelResponse(seq),
+                client,
+            },
+        ))
+    }
+
+    fn request_result(
+        &self,
+        client: ActionClient<A>,
+        goal_id: GoalUuid,
+    ) -> Result<ResultClient<A>, RclrsError> {
+        let request_rmw = A::create_result_request(&*goal_id);
+        let mut seq: i64 = 0;
+        unsafe {
+            let handle = self.handle.lock();
+            rcl_action_send_result_request(
+                &*handle,
+                &request_rmw as *const _ as *const _,
+                &mut seq,
+            )
+        }
+        .ok()?;
+
+        let (sender, receiver) = oneshot_channel();
+        self.result_senders.lock().unwrap().insert(seq, sender);
+        let lifecycle = GoalClientLifecycle {
+            client,
+            kind: GoalClientKind::Result(seq),
+        };
+        Ok(ResultClient::new(receiver, lifecycle))
+    }
+}
+
+/// This struct represents a goal client that has not been accepted yet. We
+/// create the feedback and status channels before sending the request just in
+/// case the action server sends updates for the goal before we process that the
+/// goal was accepted. This ensures that no information can be lost by accident.
+struct PendingGoalClient<A: Action> {
+    goal_id: GoalUuid,
+    feedback: FeedbackClient<A>,
+    status: StatusClient<A>,
+    sender: Sender<Option<GoalClient<A>>>,
 }
 
 /// Once all of the constituent clients for the goal are dropped, this will
@@ -256,7 +396,7 @@ impl<A: Action> ActionClientGoalBoard<A> {
 /// of its constituent clients are using it.
 struct GoalClientLifecycle<A: Action> {
     kind: GoalClientKind,
-    client: Arc<ActionClient<A>>,
+    client: ActionClient<A>,
 }
 
 enum GoalClientKind {
@@ -264,12 +404,16 @@ enum GoalClientKind {
     Feedback(GoalUuid),
     Status(GoalUuid),
     Result(i64),
-    Cancellation,
+    CancelResponse(i64),
 }
 
 impl<A: Action> Drop for GoalClientLifecycle<A> {
     fn drop(&mut self) {
         match self.kind {
+            GoalClientKind::Request(seq) => {
+                let mut all_pending_goal_clients = self.client.board.pending_goal_clients.lock().unwrap();
+                all_pending_goal_clients.remove(&seq);
+            }
             GoalClientKind::Feedback(goal_uuid) => {
                 let mut all_feedback_senders = self.client.board.feedback_senders.lock().unwrap();
 
@@ -290,6 +434,10 @@ impl<A: Action> Drop for GoalClientLifecycle<A> {
                     all_status_senders.remove(&goal_uuid);
                 }
             }
+            GoalClientKind::CancelResponse(seq) => {
+                let mut all_cancel_response_senders = self.client.board.cancel_response_senders.lock().unwrap();
+                all_cancel_response_senders.remove(&seq);
+            }
             GoalClientKind::Result(seq) => {
                 let mut all_result_senders = self.client.board.result_senders.lock().unwrap();
                 all_result_senders.remove(&seq);
@@ -302,6 +450,42 @@ struct ActionClientExecutable<A: Action> {
     board: Arc<ActionClientGoalBoard<A>>,
 }
 
+impl<A: Action> RclPrimitive for ActionClientExecutable<A> {
+    unsafe fn execute(&mut self, ready: ReadyKind, _: &mut dyn Any) -> Result<(), RclrsError> {
+        let ready = ready.for_action_client()?;
+
+        if ready.goal_response {
+            self.board.execute_goal_response()?;
+        }
+
+        if ready.feedback {
+            self.board.execute_feedback()?;
+        }
+
+        if ready.status {
+            self.board.execute_status()?;
+        }
+
+        if ready.cancel_response {
+            self.board.execute_cancel_response()?;
+        }
+
+        if ready.result_response {
+            self.board.execute_result_response()?;
+        }
+
+        Ok(())
+    }
+
+    fn kind(&self) -> RclPrimitiveKind {
+        RclPrimitiveKind::ActionClient
+    }
+
+    fn handle(&self) -> crate::RclPrimitiveHandle {
+        RclPrimitiveHandle::ActionClient(self.board.handle.lock())
+    }
+}
+
 /// Manage the lifecycle of an `rcl_action_client_t`, including managing its dependencies
 /// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are
 /// [dropped after][1] the `rcl_action_client_t`.
@@ -317,6 +501,26 @@ impl ActionClientHandle {
         self.rcl_action_client.lock().unwrap()
     }
 
+    fn take_goal_response<A: Action>(&self) -> Result<(RmwGoalResponse<A>, rmw_request_id_t), RclrsError> {
+        let mut response_rmw = RmwGoalResponse::<A>::default();
+        let mut response_header = rmw_request_id_t {
+            writer_guid: [0; 16],
+            sequence_number: 0,
+        };
+
+        unsafe {
+            let handle = self.lock();
+            rcl_action_take_goal_response(
+                &*handle,
+                &mut response_header,
+                &mut response_rmw as *mut _ as *mut _,
+            )
+        }
+        .ok()?;
+
+        Ok((response_rmw, response_header))
+    }
+
     fn take_feedback<A: Action>(&self) -> Result<RmwFeedbackMessage<A>, RclrsError> {
         let mut feedback_rmw = RmwFeedbackMessage::<A>::default();
         unsafe {
@@ -369,6 +573,55 @@ impl ActionClientHandle {
 
         Ok((result_rmw, response_header))
     }
+
+    fn send_cancel_goal(
+        &self,
+        goal_id: GoalUuid,
+        stamp: Time,
+    ) -> Result<i64, RclrsError> {
+        let cancel_request = CancelGoal_Request {
+            goal_info: GoalInfo {
+                goal_id: UUID { uuid: *goal_id },
+                stamp,
+            }
+        };
+
+        let cancel_request_rmw = <CancelGoal_Request as Message>::into_rmw_message(Cow::Owned(cancel_request));
+        let mut seq: i64 = 0;
+
+        unsafe {
+            let handle = self.lock();
+            rcl_action_send_cancel_request(
+                &*handle,
+                &cancel_request_rmw as *const _ as *const _,
+                &mut seq,
+            )
+        }
+        .ok()?;
+
+        Ok(seq)
+    }
+
+    fn take_cancel_response(&self) -> Result<(CancelGoal_Response, rmw_request_id_t), RclrsError> {
+        let mut result_rmw = <CancelGoal_Response as Message>::RmwMsg::default();
+        let mut header = rmw_request_id_t {
+            writer_guid: [0; 16],
+            sequence_number: 0,
+        };
+
+        unsafe {
+            let handle = self.lock();
+            rcl_action_take_cancel_response(
+                &*handle,
+                &mut header,
+                &mut result_rmw as *mut _ as *mut _,
+            )
+        }
+        .ok()?;
+
+        let result = Message::from_rmw_message(result_rmw);
+        Ok((result, header))
+    }
 }
 
 impl Drop for ActionClientHandle {
diff --git a/rclrs/src/action/action_client/cancellation_client.rs b/rclrs/src/action/action_client/cancellation_client.rs
index 6d452532..f6e00dff 100644
--- a/rclrs/src/action/action_client/cancellation_client.rs
+++ b/rclrs/src/action/action_client/cancellation_client.rs
@@ -1,11 +1,81 @@
+use crate::{ActionClient, CancelResponseCode, GoalUuid, RclrsError};
 use super::GoalClientLifecycle;
+use tokio::sync::oneshot::Receiver;
 use rosidl_runtime_rs::Action;
-use tokio::sync::mpsc::UnboundedReceiver;
 use std::{
-    ops::{Deref, DerefMut},
     sync::Arc,
+    ops::{Deref, DerefMut}
 };
 
+/// This can be used to request the cancellation of a specific goal. When you
+/// put in the request you will get a one-shot receiver which will tell you
+/// whether the request was accepted or rejected.
 pub struct CancellationClient<A: Action> {
-    board: Arc<GoalClientLifecycle<A>>,
+    client: ActionClient<A>,
+    goal_id: GoalUuid,
+}
+
+impl<A: Action> Clone for CancellationClient<A> {
+    fn clone(&self) -> Self {
+        Self {
+            client: Arc::clone(&self.client),
+            goal_id: self.goal_id,
+        }
+    }
+}
+
+impl<A: Action> CancellationClient<A> {
+    /// Ask the goal to cancel. You will receiver a [`CancelResponseClient`]
+    /// which you can use to await the response from the action server.
+    ///
+    /// If an rcl error occurs, this will panic. This function is not likely to
+    /// result in any rcl errors.
+    pub fn cancel(&self) -> CancelResponseClient<A> {
+        self.try_cancel().unwrap()
+    }
+
+    /// A version of [`Self::cancel`] which does not panic when an error occurs.
+    pub fn try_cancel(&self) -> Result<CancelResponseClient<A>, RclrsError> {
+        self.client.board.request_single_cancel(Arc::clone(&self.client), self.goal_id)
+    }
+
+    pub(super) fn new(
+        client: ActionClient<A>,
+        goal_id: GoalUuid,
+    ) -> Self {
+        Self { client, goal_id }
+    }
+}
+
+/// This will allow you to receive a response to your cancellation request so you
+/// can know if the cancellation was successful or not.
+pub struct CancelResponseClient<A: Action> {
+    receiver: Receiver<CancelResponseCode>,
+    /// This keeps track of whether any goal client components are still being used.
+    /// This must come after the receiver in the struct to ensure cleanup is done
+    /// correctly.
+    #[allow(unused)]
+    lifecycle: GoalClientLifecycle<A>,
+}
+
+impl<A: Action> Deref for CancelResponseClient<A> {
+    type Target = Receiver<CancelResponseCode>;
+    fn deref(&self) -> &Self::Target {
+        &self.receiver
+    }
+}
+
+impl<A: Action> DerefMut for CancelResponseClient<A> {
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        &mut self.receiver
+    }
+}
+
+impl<A: Action> CancelResponseClient<A> {
+    pub(super) fn new(
+        receiver: Receiver<CancelResponseCode>,
+        lifecycle: GoalClientLifecycle<A>,
+    ) -> Self {
+        Self { receiver, lifecycle }
+    }
 }
diff --git a/rclrs/src/action/action_client/goal_client.rs b/rclrs/src/action/action_client/goal_client.rs
index 65042a8c..3bcfddf5 100644
--- a/rclrs/src/action/action_client/goal_client.rs
+++ b/rclrs/src/action/action_client/goal_client.rs
@@ -1,4 +1,7 @@
-use crate::{FeedbackClient, StatusClient, ResultClient};
+use crate::{
+    vendor::builtin_interfaces::msg::Time,
+    CancellationClient, FeedbackClient, StatusClient, ResultClient,
+};
 use rosidl_runtime_rs::Action;
 
 /// The goal client bundles a set of receivers that will allow you to await
@@ -20,4 +23,8 @@ pub struct GoalClient<A: Action> {
     pub status: StatusClient<A>,
     /// Get the final result of the goal.
     pub result: ResultClient<A>,
+    /// Use this if you want to request the goal to be cancelled.
+    pub cancellation: CancellationClient<A>,
+    /// The time that the goal was accepted.
+    pub stamp: Time,
 }
diff --git a/rclrs/src/action/action_client/requested_goal_client.rs b/rclrs/src/action/action_client/requested_goal_client.rs
new file mode 100644
index 00000000..0799ee2d
--- /dev/null
+++ b/rclrs/src/action/action_client/requested_goal_client.rs
@@ -0,0 +1,33 @@
+use crate::GoalClient;
+use super::GoalClientLifecycle;
+use rosidl_runtime_rs::Action;
+use tokio::sync::oneshot::Receiver;
+use std::ops::{Deref, DerefMut};
+
+/// This struct allows you to receive a [`GoalClient`] for a goal that you
+/// requested. Through the [`DerefMut`] trait you can use the [`oneshot::Receiver`][Receiver]
+/// API to await the goal client. Note that it can only be received once.
+///
+/// If the action server rejects the goal then this will yield a [`None`] instead
+/// of a [`GoalClient`].
+pub struct RequestedGoalClient<A: Action> {
+    receiver: Receiver<Option<GoalClient<A>>>,
+    /// This keeps track of whether any goal client components are still being used.
+    /// This must come after the receiver in the struct to ensure cleanup is done
+    /// correctly.
+    #[allow(unused)]
+    lifecycle: GoalClientLifecycle<A>,
+}
+
+impl<A: Action> Deref for RequestedGoalClient<A> {
+    type Target = Receiver<Option<GoalClient<A>>>;
+    fn deref(&self) -> &Self::Target {
+        &self.receiver
+    }
+}
+
+impl<A: Action> DerefMut for RequestedGoalClient<A> {
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        &mut self.receiver
+    }
+}
diff --git a/rclrs/src/action/action_client/result_client.rs b/rclrs/src/action/action_client/result_client.rs
index ad21483c..8a21330f 100644
--- a/rclrs/src/action/action_client/result_client.rs
+++ b/rclrs/src/action/action_client/result_client.rs
@@ -16,6 +16,15 @@ pub struct ResultClient<A: Action> {
     lifecycle: GoalClientLifecycle<A>,
 }
 
+impl<A: Action> ResultClient<A> {
+    pub(super) fn new(
+        receiver: Receiver<(GoalStatusCode, A::Result)>,
+        lifecycle: GoalClientLifecycle<A>,
+    ) -> Self {
+        Self { receiver, lifecycle }
+    }
+}
+
 impl<A: Action> Deref for ResultClient<A> {
     type Target = Receiver<(GoalStatusCode, A::Result)>;
     fn deref(&self) -> &Self::Target {
diff --git a/rclrs/src/action/action_client/status_client.rs b/rclrs/src/action/action_client/status_client.rs
index 163792e7..879888d7 100644
--- a/rclrs/src/action/action_client/status_client.rs
+++ b/rclrs/src/action/action_client/status_client.rs
@@ -1,11 +1,7 @@
-use crate::{GoalStatusCode, GoalUuid, FeedbackClient};
-use super::{ActionClientGoalBoard, GoalClientLifecycle};
+use crate::GoalStatusCode;
+use super::GoalClientLifecycle;
 use rosidl_runtime_rs::Action;
-use tokio::sync::{
-    watch::Receiver as Watcher,
-    mpsc::UnboundedReceiver,
-    oneshot::Receiver,
-};
+use tokio::sync::watch::Receiver as Watcher;
 use std::{
     ops::{Deref, DerefMut},
     sync::Arc,
diff --git a/rclrs/src/action/action_server/cancellation_state.rs b/rclrs/src/action/action_server/cancellation_state.rs
index 4638229c..8da24742 100644
--- a/rclrs/src/action/action_server/cancellation_state.rs
+++ b/rclrs/src/action/action_server/cancellation_state.rs
@@ -8,7 +8,7 @@ use crate::{
         unique_identifier_msgs::msg::UUID,
     },
     log_error,
-    CancelResponse, GoalUuid, ToResult, Node, RclrsErrorFilter,
+    CancelResponseCode, GoalUuid, ToResult, Node, RclrsErrorFilter,
 };
 use super::ActionServerHandle;
 use std::{
@@ -237,9 +237,9 @@ impl<A: Action> CancellationRequestInner<A> {
         let mut response = CancelGoal_Response::default();
         response.goals_canceling = self.accepted.drain(..).collect();
         if response.goals_canceling.is_empty() {
-            response.return_code = CancelResponse::Reject as i8;
+            response.return_code = CancelResponseCode::Reject as i8;
         } else {
-            response.return_code = CancelResponse::Accept as i8;
+            response.return_code = CancelResponseCode::Accept as i8;
         }
 
         let mut response_rmw = CancelGoal_Response::into_rmw_message(Cow::Owned(response)).into_owned();

From b115469f5a250543f37aee12c27e3c4fbe38b310 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Thu, 31 Jul 2025 20:03:11 +0800
Subject: [PATCH 16/20] Finish action cancellation request API

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/src/action.rs                           |  46 ++++++-
 rclrs/src/action/action_client.rs             | 116 +++++++++++++++---
 .../action_client/cancellation_client.rs      |  55 ++++++++-
 rclrs/src/action/action_server.rs             |   6 +-
 rclrs/src/error.rs                            |  11 +-
 rclrs/src/node.rs                             |   2 +-
 6 files changed, 208 insertions(+), 28 deletions(-)

diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs
index b236ff7b..5e88355d 100644
--- a/rclrs/src/action.rs
+++ b/rclrs/src/action.rs
@@ -1,4 +1,4 @@
-use std::ops::Deref;
+use std::{collections::HashMap, ops::Deref};
 
 pub(crate) mod action_client;
 pub use action_client::*;
@@ -21,6 +21,14 @@ use std::fmt;
 #[derive(Copy, Clone, Debug, Default, PartialEq, Eq, PartialOrd, Ord, Hash)]
 pub struct GoalUuid(pub [u8; RCL_ACTION_UUID_SIZE]);
 
+impl GoalUuid {
+    /// A zeroed-out goal ID has a special meaning for cancellation requests
+    /// which indicates that no specific goal is being requested.
+    fn zero() -> Self {
+        Self([0; RCL_ACTION_UUID_SIZE])
+    }
+}
+
 impl fmt::Display for GoalUuid {
     fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> {
         write!(f, "{:02x}{:02x}{:02x}{:02x}-{:02x}{:02x}-{:02x}{:02x}-{:02x}{:02x}-{:02x}{:02x}{:02x}{:02x}{:02x}{:02x}",
@@ -93,11 +101,43 @@ impl From<i8> for CancelResponseCode {
     }
 }
 
+/// This is returned by [`CancellationClient`] to inform whether a cancellation
+/// of a single goal was successful.
+///
+/// When a cancellation request might cancel multiple goals, [`MultiCancelResponse`]
+/// will be used.
 #[derive(Debug, Clone, PartialEq, PartialOrd)]
 pub struct CancelResponse {
+    /// What kind of response was given.
     pub code: CancelResponseCode,
-    pub goal_id: GoalUuid,
-    pub stamp: Time,
+    /// What time the response took effect according to the action server.
+    /// This will be default-initialized if no goal was cancelled.
+    pub stamp: Option<Time>,
+}
+
+impl CancelResponse {
+    /// Check whether the request was accepted.
+    pub fn is_accepted(&self) -> bool {
+        self.code.is_accepted()
+    }
+}
+
+/// This is returned by [`ActionClientState::cancel_all_goals`] and
+/// [`ActionClientState::cancel_goals_prior_to`].
+#[derive(Debug, Clone, PartialEq)]
+pub struct MultiCancelResponse {
+    /// What kind of response was given.
+    pub code: CancelResponseCode,
+    /// The time stamp that the response took effect for each goal that is being
+    /// cancelled. If the request was not accepted then this may be empty.
+    pub stamps: HashMap<GoalUuid, Time>,
+}
+
+impl MultiCancelResponse {
+    /// Check whether the request was accepted.
+    pub fn is_accepted(&self) -> bool {
+        self.code.is_accepted()
+    }
 }
 
 /// Values defined by `action_msgs/msg/GoalStatus`
diff --git a/rclrs/src/action/action_client.rs b/rclrs/src/action/action_client.rs
index 062b47f5..9ceeeacb 100644
--- a/rclrs/src/action/action_client.rs
+++ b/rclrs/src/action/action_client.rs
@@ -5,9 +5,11 @@ use crate::{
         builtin_interfaces::msg::Time,
         unique_identifier_msgs::msg::UUID,
     },
-    CancelResponse, CancelResponseCode, DropGuard, GoalStatus, GoalStatusCode, GoalUuid, ToResult, Node,
-    NodeHandle, QoSProfile, RclrsError, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind,
-    ReadyKind, TakeFailedAsNone, Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
+    log_warn,
+    CancelResponse, CancelResponseCode, DropGuard, GoalStatus, GoalStatusCode, GoalUuid,
+    MultiCancelResponse, Node, NodeHandle, QoSProfile, RclrsError, RclPrimitive,
+    RclPrimitiveHandle, RclPrimitiveKind, ReadyKind, TakeFailedAsNone, ToResult,
+    Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
 };
 use std::{
     any::Any,
@@ -109,6 +111,53 @@ pub struct ActionClientState<A: Action> {
 }
 
 impl<A: Action> ActionClientState<A> {
+    /// Ask the action server to cancel a single goal.
+    ///
+    /// In the unlikely event of an error at the rcl layer, this will panic.
+    /// Use [`Self::try_cancel_goal`] to handle errors without panicking.
+    pub fn cancel_goal(self: &Arc<Self>, goal_id: GoalUuid) -> CancelResponseClient<A> {
+        self.try_cancel_goal(goal_id).unwrap()
+    }
+
+    /// Try to ask the action server to cancel a single goal. If an rcl error
+    /// happens, you can handle it.
+    pub fn try_cancel_goal(self: &Arc<Self>, goal_id: GoalUuid) -> Result<CancelResponseClient<A>, RclrsError> {
+        self.board.request_single_cancel(self, goal_id)
+    }
+
+    /// Ask the action server to cancel all of its goals.
+    ///
+    /// In the unlikely event of an error at the rcl layer, this will panic.
+    /// Use [`Self::try_cancel_all_goals`] to catch errors without panicking.
+    pub fn cancel_all_goals(self: &Arc<Self>) -> MultiCancelResponseClient<A> {
+        self.try_cancel_all_goals().unwrap()
+    }
+
+    /// Try to ask the action server to cancel all of its goals. If an rcl error
+    /// happens, you can handle it.
+    pub fn try_cancel_all_goals(self: &Arc<Self>) -> Result<MultiCancelResponseClient<A>, RclrsError> {
+        self.board.request_multi_cancel(self, None)
+    }
+
+    /// Ask the action server to cancel all of its goals that started before a
+    /// specified time stamp.
+    ///
+    /// <div class="warning">Make sure the time stamp is based on the time according to the action server.</div>
+    ///
+    /// In the unlikely event of an error at the rcl layer, this will panic.
+    /// Use [`Self::try_cancel_goals_prior_to`] to catch errors without panicking.
+    pub fn cancel_goals_prior_to(self: &Arc<Self>, time: Time) -> MultiCancelResponseClient<A> {
+        self.try_cancel_goals_prior_to(time).unwrap()
+    }
+
+    /// Try to ask the action server to cancel all of its goals that started before
+    /// a specified time stamp. If an rcl error happens, you can handle it.
+    ///
+    /// <div class="warning">Make sure the time stamp is based on the time according to the action server.</div>
+    pub fn try_cancel_goals_prior_to(self: &Arc<Self>, time: Time) -> Result<MultiCancelResponseClient<A>, RclrsError> {
+        self.board.request_multi_cancel(self, Some(time))
+    }
+
     /// Creates a new action client.
     pub(crate) fn create<'a>(
         node: &Node,
@@ -199,7 +248,7 @@ enum CancelResponseSender {
     /// Used when only a single goal is being cancelled
     SingleGoalCancel(Sender<CancelResponse>),
     /// Used when multiple goals are being cancelled
-    MultiGoalCancel(Sender<(CancelResponseCode, Vec<GoalUuid>)>),
+    MultiGoalCancel(Sender<MultiCancelResponse>),
 }
 
 impl<A: Action> ActionClientGoalBoard<A> {
@@ -213,11 +262,11 @@ impl<A: Action> ActionClientGoalBoard<A> {
         if let Some(senders) = self.feedback_senders.lock().unwrap().get(&GoalUuid(goal_uuid)) {
             // Avoid making any unnecessary clones
             for sender in senders.iter().take(senders.len() - 1) {
-                sender.send(feedback.clone());
+                let _ =sender.send(feedback.clone());
             }
 
             if let Some(last_sender) = senders.last() {
-                last_sender.send(feedback);
+                let _ = last_sender.send(feedback);
             }
         }
 
@@ -281,7 +330,7 @@ impl<A: Action> ActionClientGoalBoard<A> {
                 })
             );
         } else {
-            pending.sender.send(None);
+            let _ = pending.sender.send(None);
         }
 
         Ok(())
@@ -299,13 +348,31 @@ impl<A: Action> ActionClientGoalBoard<A> {
             return Ok(());
         };
 
-        let code =
+        let code: CancelResponseCode = result.return_code.into();
         match sender {
             CancelResponseSender::SingleGoalCancel(sender) => {
-                let _ = sender.send(result.return_code.into());
+                if result.goals_canceling.len() > 1 {
+                    log_warn!(
+                        "action_client.execute_cancel_response",
+                        "Multiple goals were cancelled when only one was expected. \
+                        This may indicate a client library implementation bug.",
+                    );
+                }
+
+                // Use the first goal's info to get the time stamp. We should
+                // only be expecting one goal to be cancelled anyway.
+                let stamp = result
+                    .goals_canceling
+                    .first()
+                    .map(|g| g.stamp.clone());
+                let _ = sender.send(CancelResponse { code, stamp });
             }
             CancelResponseSender::MultiGoalCancel(sender) => {
-                result.goals_canceling
+                let mut stamps = HashMap::default();
+                for info in result.goals_canceling {
+                    stamps.insert(GoalUuid(info.goal_id.uuid), info.stamp);
+                }
+                let _ = sender.send(MultiCancelResponse { code, stamps });
             }
         }
 
@@ -335,7 +402,7 @@ impl<A: Action> ActionClientGoalBoard<A> {
 
     fn request_single_cancel(
         &self,
-        client: ActionClient<A>,
+        client: &ActionClient<A>,
         goal_id: GoalUuid,
     ) -> Result<CancelResponseClient<A>, RclrsError> {
         let seq = self.handle.send_cancel_goal(goal_id, Time { sec: 0, nanosec: 0 })?;
@@ -345,7 +412,26 @@ impl<A: Action> ActionClientGoalBoard<A> {
             receiver,
             GoalClientLifecycle {
                 kind: GoalClientKind::CancelResponse(seq),
-                client,
+                client: Arc::clone(client),
+            },
+        ))
+    }
+
+    fn request_multi_cancel(
+        &self,
+        client: &ActionClient<A>,
+        stamp: Option<Time>,
+    ) -> Result<MultiCancelResponseClient<A>, RclrsError> {
+        let goal_id = GoalUuid::zero();
+        let stamp = stamp.unwrap_or(Time { sec: 0, nanosec: 0 });
+        let seq = self.handle.send_cancel_goal(goal_id, stamp)?;
+        let (sender, receiver) = oneshot_channel();
+        self.cancel_response_senders.lock().unwrap().insert(seq, CancelResponseSender::MultiGoalCancel(sender));
+        Ok(MultiCancelResponseClient::new(
+            receiver,
+            GoalClientLifecycle {
+                kind: GoalClientKind::CancelResponse(seq),
+                client: Arc::clone(client),
             },
         ))
     }
@@ -504,7 +590,7 @@ impl ActionClientHandle {
     fn take_goal_response<A: Action>(&self) -> Result<(RmwGoalResponse<A>, rmw_request_id_t), RclrsError> {
         let mut response_rmw = RmwGoalResponse::<A>::default();
         let mut response_header = rmw_request_id_t {
-            writer_guid: [0; 16],
+            writer_guid: [0; RCL_ACTION_UUID_SIZE],
             sequence_number: 0,
         };
 
@@ -557,7 +643,7 @@ impl ActionClientHandle {
     fn take_result_response<A: Action>(&self) -> Result<(RmwResultResponse<A>, rmw_request_id_t), RclrsError> {
         let mut result_rmw = RmwResultResponse::<A>::default();
         let mut response_header = rmw_request_id_t {
-            writer_guid: [0; 16],
+            writer_guid: [0; RCL_ACTION_UUID_SIZE],
             sequence_number: 0,
         };
 
@@ -605,7 +691,7 @@ impl ActionClientHandle {
     fn take_cancel_response(&self) -> Result<(CancelGoal_Response, rmw_request_id_t), RclrsError> {
         let mut result_rmw = <CancelGoal_Response as Message>::RmwMsg::default();
         let mut header = rmw_request_id_t {
-            writer_guid: [0; 16],
+            writer_guid: [0; RCL_ACTION_UUID_SIZE],
             sequence_number: 0,
         };
 
diff --git a/rclrs/src/action/action_client/cancellation_client.rs b/rclrs/src/action/action_client/cancellation_client.rs
index f6e00dff..acdcaa8c 100644
--- a/rclrs/src/action/action_client/cancellation_client.rs
+++ b/rclrs/src/action/action_client/cancellation_client.rs
@@ -1,4 +1,4 @@
-use crate::{ActionClient, CancelResponseCode, GoalUuid, RclrsError};
+use crate::{ActionClient, CancelResponse, GoalUuid, MultiCancelResponse, RclrsError};
 use super::GoalClientLifecycle;
 use tokio::sync::oneshot::Receiver;
 use rosidl_runtime_rs::Action;
@@ -36,7 +36,7 @@ impl<A: Action> CancellationClient<A> {
 
     /// A version of [`Self::cancel`] which does not panic when an error occurs.
     pub fn try_cancel(&self) -> Result<CancelResponseClient<A>, RclrsError> {
-        self.client.board.request_single_cancel(Arc::clone(&self.client), self.goal_id)
+        self.client.board.request_single_cancel(&self.client, self.goal_id)
     }
 
     pub(super) fn new(
@@ -49,8 +49,14 @@ impl<A: Action> CancellationClient<A> {
 
 /// This will allow you to receive a response to your cancellation request so you
 /// can know if the cancellation was successful or not.
+///
+/// You can obtain one of these using [`CancellationClient`] (which is provided
+/// by [`GoalClient`][1]) or using [`ActionClientState::cancel_goal`][2].
+///
+/// [1]: crate::GoalClient
+/// [2]: crate::ActionClientState::cancel_goal
 pub struct CancelResponseClient<A: Action> {
-    receiver: Receiver<CancelResponseCode>,
+    receiver: Receiver<CancelResponse>,
     /// This keeps track of whether any goal client components are still being used.
     /// This must come after the receiver in the struct to ensure cleanup is done
     /// correctly.
@@ -59,7 +65,7 @@ pub struct CancelResponseClient<A: Action> {
 }
 
 impl<A: Action> Deref for CancelResponseClient<A> {
-    type Target = Receiver<CancelResponseCode>;
+    type Target = Receiver<CancelResponse>;
     fn deref(&self) -> &Self::Target {
         &self.receiver
     }
@@ -73,7 +79,46 @@ impl<A: Action> DerefMut for CancelResponseClient<A> {
 
 impl<A: Action> CancelResponseClient<A> {
     pub(super) fn new(
-        receiver: Receiver<CancelResponseCode>,
+        receiver: Receiver<CancelResponse>,
+        lifecycle: GoalClientLifecycle<A>,
+    ) -> Self {
+        Self { receiver, lifecycle }
+    }
+}
+
+/// This will allow you to receive a response to a cancellation request that
+/// impacts multiple goals.
+///
+/// You can obtain one of these using [`ActionClientState::cancel_all_goals`][1]
+/// or [`ActionClientState::cancel_goals_prior_to`][2].
+///
+/// [1]: crate::ActionClientState::cancel_all_goals
+/// [2]: crate::ActionClientState::cancel_goals_prior_to
+pub struct MultiCancelResponseClient<A: Action> {
+    receiver: Receiver<MultiCancelResponse>,
+    /// This keeps track of whether any goal client components are still being used.
+    /// This must come after the receiver in the struct to ensure cleanup is done
+    /// correctly.
+    #[allow(unused)]
+    lifecycle: GoalClientLifecycle<A>,
+}
+
+impl<A: Action> Deref for MultiCancelResponseClient<A> {
+    type Target = Receiver<MultiCancelResponse>;
+    fn deref(&self) -> &Self::Target {
+        &self.receiver
+    }
+}
+
+impl<A: Action> DerefMut for MultiCancelResponseClient<A> {
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        &mut self.receiver
+    }
+}
+
+impl<A: Action> MultiCancelResponseClient<A> {
+    pub(super) fn new(
+        receiver: Receiver<MultiCancelResponse>,
         lifecycle: GoalClientLifecycle<A>,
     ) -> Self {
         Self { receiver, lifecycle }
diff --git a/rclrs/src/action/action_server.rs b/rclrs/src/action/action_server.rs
index d55b2fc5..f8c933a4 100644
--- a/rclrs/src/action/action_server.rs
+++ b/rclrs/src/action/action_server.rs
@@ -583,7 +583,7 @@ impl<A: Action> ActionServerHandle<A> {
 
     fn take_goal_request(&self) -> Result<(RmwGoalRequest<A>, rmw_request_id_t), RclrsError> {
         let mut request_id = rmw_request_id_t {
-            writer_guid: [0; 16],
+            writer_guid: [0; RCL_ACTION_UUID_SIZE],
             sequence_number: 0,
         };
         let mut request_rmw = RmwGoalRequest::<A>::default();
@@ -605,7 +605,7 @@ impl<A: Action> ActionServerHandle<A> {
 
     fn take_cancel_request(&self) -> Result<(action_msgs__srv__CancelGoal_Request, rmw_request_id_t), RclrsError> {
         let mut request_id = rmw_request_id_t {
-            writer_guid: [0; 16],
+            writer_guid: [0; RCL_ACTION_UUID_SIZE],
             sequence_number: 0,
         };
         // SAFETY: No preconditions
@@ -628,7 +628,7 @@ impl<A: Action> ActionServerHandle<A> {
 
     fn take_result_request(&self) -> Result<(RmwResultRequest<A>, rmw_request_id_t), RclrsError> {
         let mut request_id = rmw_request_id_t {
-            writer_guid: [0; 16],
+            writer_guid: [0; RCL_ACTION_UUID_SIZE],
             sequence_number: 0,
         };
 
diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs
index 78222bcb..bd7a826a 100644
--- a/rclrs/src/error.rs
+++ b/rclrs/src/error.rs
@@ -96,7 +96,10 @@ impl RclrsError {
             RclrsError::RclError {
                 code: RclReturnCode::SubscriptionTakeFailed
                     | RclReturnCode::ServiceTakeFailed
-                    | RclReturnCode::ClientTakeFailed,
+                    | RclReturnCode::ClientTakeFailed
+                    | RclReturnCode::ActionServerTakeFailed
+                    | RclReturnCode::ActionClientTakeFailed
+                    | RclReturnCode::EventTakeFailed,
                 ..
             }
         )
@@ -597,8 +600,14 @@ impl RclrsErrorFilter for Vec<RclrsError> {
     }
 }
 
+/// A helper trait to handle common error handling flows
 pub trait TakeFailedAsNone {
+    /// The type you would receive when there is no error
     type T;
+
+    /// If the result has an error indicating that a take failed, convert the
+    /// output into an `Ok(None)`. Any other error returns `Err(error)`. If there
+    /// is no error, return `Ok(Some(value))`.
     fn take_failed_as_none(self) -> Result<Option<Self::T>, RclrsError>;
 }
 
diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index 6ead5da6..b623460d 100644
--- a/rclrs/src/node.rs
+++ b/rclrs/src/node.rs
@@ -360,7 +360,7 @@ impl NodeState {
         self: &Arc<Self>,
         options: impl Into<ActionClientOptions<'a>>,
     ) -> Result<ActionClient<A>, RclrsError> {
-        todo!();
+        ActionClientState::create(self, options)
     }
 
     /// Creates an [`ActionServer`].

From 5225df897eb3d2854cc293482e7f31bfbea6367e Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Thu, 31 Jul 2025 21:00:29 +0800
Subject: [PATCH 17/20] Finish API for action clients -- needs testing

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/Cargo.toml                              |   8 +-
 rclrs/src/action.rs                           |  12 ++
 rclrs/src/action/action_client.rs             | 119 +++++++++++++++++-
 .../action/action_client/feedback_client.rs   |   9 ++
 .../action_client/requested_goal_client.rs    |  18 ++-
 .../src/action/action_client/status_client.rs |  15 ++-
 6 files changed, 175 insertions(+), 6 deletions(-)

diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml
index 53a2d0f5..71627de9 100644
--- a/rclrs/Cargo.toml
+++ b/rclrs/Cargo.toml
@@ -36,9 +36,15 @@ rosidl_runtime_rs = "0.4"
 serde = { version = "1", optional = true, features = ["derive"] }
 serde-big-array = { version = "0.5.1", optional = true }
 
-# Needed to watch for the cancel signal for actions
+# Needed to watch for the cancel signal for actions. Note that this only brings
+# in the sync module of tokio, which is a fairly light weight dependency. The
+# channels in this module work with any async runtime, so this does not lock us
+# into the tokio runtime.
 tokio = { version = "1", features = ["sync"] }
 
+# Needed by action clients to generate UUID values for their goals
+uuid = { version = "1", features = ["v4"] }
+
 [dev-dependencies]
 # Needed for e.g. writing yaml files in tests
 tempfile = "3.3.0"
diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs
index 5e88355d..8cc09c05 100644
--- a/rclrs/src/action.rs
+++ b/rclrs/src/action.rs
@@ -60,6 +60,18 @@ impl Deref for GoalUuid {
     }
 }
 
+impl From<[u8; RCL_ACTION_UUID_SIZE]> for GoalUuid {
+    fn from(value: [u8; RCL_ACTION_UUID_SIZE]) -> Self {
+        Self(value)
+    }
+}
+
+impl From<&[u8; RCL_ACTION_UUID_SIZE]> for GoalUuid {
+    fn from(value: &[u8; RCL_ACTION_UUID_SIZE]) -> Self {
+        Self(*value)
+    }
+}
+
 /// The response returned by an [`ActionServer`]'s cancel callback when a goal is requested to be cancelled.
 #[repr(i8)]
 #[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
diff --git a/rclrs/src/action/action_client.rs b/rclrs/src/action/action_client.rs
index 9ceeeacb..8b815fe0 100644
--- a/rclrs/src/action/action_client.rs
+++ b/rclrs/src/action/action_client.rs
@@ -20,7 +20,7 @@ use std::{
 };
 use tokio::sync::{
     watch::Sender as WatchSender,
-    mpsc::UnboundedSender,
+    mpsc::{unbounded_channel, UnboundedSender},
     oneshot::{channel as oneshot_channel, Sender},
 };
 use rosidl_runtime_rs::{Action, Message, RmwGoalResponse, RmwFeedbackMessage, RmwResultResponse};
@@ -111,6 +111,40 @@ pub struct ActionClientState<A: Action> {
 }
 
 impl<A: Action> ActionClientState<A> {
+    /// Request the action server to execute a goal. You will receive a
+    /// [`RequestedGoalClient`] which you can use to wait for the reply from the
+    /// action server that indicates whether the goal was accepted.
+    ///
+    /// If the goal is accepted, the [`RequestedGoalClient`] will yield a
+    /// [`GoalClient`]. If the goal was rejected by the action server then it will
+    /// yield a [`None`]. The easiest way to get the response is to use `.await`
+    /// in an async function.
+    ///
+    /// In the unlikely event of an error at the rcl layer, this will panic.
+    /// Use [`Self::try_request_goal`] to handle errors without panicking.
+    pub fn request_goal(self: &Arc<Self>, goal: A::Goal) -> RequestedGoalClient<A> {
+        self.try_request_goal(goal).unwrap()
+    }
+
+    /// A version of [`Self::request_goal`] which allows you to handle any errors
+    /// that may happen at the rcl layer.
+    pub fn try_request_goal(
+        self: &Arc<Self>,
+        goal: A::Goal,
+    ) -> Result<RequestedGoalClient<A>, RclrsError> {
+        self.board.request_goal(self, goal)
+    }
+
+    /// Get a client to receive feedback for a specific goal.
+    pub fn receive_feedback(self: &Arc<Self>, goal_id: GoalUuid) -> FeedbackClient<A> {
+        self.board.create_feedback_client(self, goal_id)
+    }
+
+    /// Get a client to watch the status of a specific goal.
+    pub fn watch_status(self: &Arc<Self>, goal_id: GoalUuid) -> StatusClient<A> {
+        self.board.create_status_client(self, goal_id)
+    }
+
     /// Ask the action server to cancel a single goal.
     ///
     /// In the unlikely event of an error at the rcl layer, this will panic.
@@ -400,6 +434,89 @@ impl<A: Action> ActionClientGoalBoard<A> {
         Ok(())
     }
 
+    fn request_goal(
+        &self,
+        client: &ActionClient<A>,
+        goal: A::Goal,
+    ) -> Result<RequestedGoalClient<A>, RclrsError> {
+        let goal_id: GoalUuid = uuid::Uuid::new_v4().as_bytes().into();
+        let goal_rmw = <A::Goal as Message>::into_rmw_message(Cow::Owned(goal)).into_owned();
+        let request = A::create_goal_request(&*goal_id, goal_rmw);
+
+        let mut seq: i64 = 0;
+        unsafe {
+            let handle = self.handle.lock();
+            rcl_action_send_goal_request(
+                &*handle,
+                &request as *const _ as *const _,
+                &mut seq,
+            )
+        }
+        .ok()?;
+
+        let (sender, receiver) = oneshot_channel();
+        let feedback = self.create_feedback_client(client, goal_id);
+        let status = self.create_status_client(client, goal_id);
+
+        self.pending_goal_clients.lock().unwrap().insert(
+            seq,
+            PendingGoalClient { goal_id, feedback, status, sender },
+        );
+
+        Ok(RequestedGoalClient::new(
+            goal_id,
+            receiver,
+            GoalClientLifecycle {
+                kind: GoalClientKind::Request(seq),
+                client: Arc::clone(client),
+            },
+        ))
+    }
+
+    fn create_feedback_client(
+        &self,
+        client: &ActionClient<A>,
+        goal_id: GoalUuid,
+    ) -> FeedbackClient<A> {
+        let (sender, receiver) = unbounded_channel();
+        self.feedback_senders.lock().unwrap().entry(goal_id).or_default().push(sender);
+        FeedbackClient::new(
+            receiver,
+            GoalClientLifecycle {
+                kind: GoalClientKind::Feedback(goal_id),
+                client: Arc::clone(client),
+            },
+        )
+    }
+
+    fn create_status_client(
+        &self,
+        client: &ActionClient<A>,
+        goal_id: GoalUuid,
+    ) -> StatusClient<A> {
+        let receiver = self
+            .status_senders
+            .lock()
+            .unwrap()
+            .entry(goal_id)
+            .or_insert_with(||
+                WatchSender::new(GoalStatus {
+                    code: GoalStatusCode::Unknown,
+                    goal_id,
+                    stamp: Time { sec: 0, nanosec: 0 },
+                })
+            )
+            .subscribe();
+
+        StatusClient::new(
+            receiver,
+            Arc::new(GoalClientLifecycle {
+                kind: GoalClientKind::Status(goal_id),
+                client: Arc::clone(client),
+            }),
+        )
+    }
+
     fn request_single_cancel(
         &self,
         client: &ActionClient<A>,
diff --git a/rclrs/src/action/action_client/feedback_client.rs b/rclrs/src/action/action_client/feedback_client.rs
index 2bb256cc..f69d57c3 100644
--- a/rclrs/src/action/action_client/feedback_client.rs
+++ b/rclrs/src/action/action_client/feedback_client.rs
@@ -27,3 +27,12 @@ impl<A: Action> DerefMut for FeedbackClient<A> {
         &mut self.receiver
     }
 }
+
+impl<A: Action> FeedbackClient<A> {
+    pub(super) fn new(
+        receiver: UnboundedReceiver<A::Feedback>,
+        lifecycle: GoalClientLifecycle<A>,
+    ) -> Self {
+        Self { receiver, lifecycle }
+    }
+}
diff --git a/rclrs/src/action/action_client/requested_goal_client.rs b/rclrs/src/action/action_client/requested_goal_client.rs
index 0799ee2d..12a26651 100644
--- a/rclrs/src/action/action_client/requested_goal_client.rs
+++ b/rclrs/src/action/action_client/requested_goal_client.rs
@@ -1,4 +1,4 @@
-use crate::GoalClient;
+use crate::{GoalClient, GoalUuid};
 use super::GoalClientLifecycle;
 use rosidl_runtime_rs::Action;
 use tokio::sync::oneshot::Receiver;
@@ -11,6 +11,7 @@ use std::ops::{Deref, DerefMut};
 /// If the action server rejects the goal then this will yield a [`None`] instead
 /// of a [`GoalClient`].
 pub struct RequestedGoalClient<A: Action> {
+    goal_id: GoalUuid,
     receiver: Receiver<Option<GoalClient<A>>>,
     /// This keeps track of whether any goal client components are still being used.
     /// This must come after the receiver in the struct to ensure cleanup is done
@@ -19,6 +20,21 @@ pub struct RequestedGoalClient<A: Action> {
     lifecycle: GoalClientLifecycle<A>,
 }
 
+impl<A: Action> RequestedGoalClient<A> {
+    /// Get the unique ID for the goal that has been requested.
+    pub fn goal_id(&self) -> &GoalUuid {
+        &self.goal_id
+    }
+
+    pub(super) fn new(
+        goal_id: GoalUuid,
+        receiver: Receiver<Option<GoalClient<A>>>,
+        lifecycle: GoalClientLifecycle<A>,
+    ) -> Self {
+        Self { goal_id, receiver, lifecycle }
+    }
+}
+
 impl<A: Action> Deref for RequestedGoalClient<A> {
     type Target = Receiver<Option<GoalClient<A>>>;
     fn deref(&self) -> &Self::Target {
diff --git a/rclrs/src/action/action_client/status_client.rs b/rclrs/src/action/action_client/status_client.rs
index 879888d7..b271b440 100644
--- a/rclrs/src/action/action_client/status_client.rs
+++ b/rclrs/src/action/action_client/status_client.rs
@@ -1,4 +1,4 @@
-use crate::GoalStatusCode;
+use crate::GoalStatus;
 use super::GoalClientLifecycle;
 use rosidl_runtime_rs::Action;
 use tokio::sync::watch::Receiver as Watcher;
@@ -11,7 +11,7 @@ use std::{
 /// [`DerefMut`] trait you can use the [`watch::Receiver`][Watcher] API to
 /// check the latest status and monitor changes.
 pub struct StatusClient<A: Action> {
-    receiver: Watcher<GoalStatusCode>,
+    receiver: Watcher<GoalStatus>,
     /// This keeps track of whether any goal client components are still being used.
     /// This must come after the receiver in the struct to ensure cleanup is done
     /// correctly.
@@ -29,7 +29,7 @@ impl<A: Action> Clone for StatusClient<A> {
 }
 
 impl<A: Action> Deref for StatusClient<A> {
-    type Target = Watcher<GoalStatusCode>;
+    type Target = Watcher<GoalStatus>;
     fn deref(&self) -> &Self::Target {
         &self.receiver
     }
@@ -40,3 +40,12 @@ impl<A: Action> DerefMut for StatusClient<A> {
         &mut self.receiver
     }
 }
+
+impl<A: Action> StatusClient<A> {
+    pub(super) fn new(
+        receiver: Watcher<GoalStatus>,
+        lifecycle: Arc<GoalClientLifecycle<A>>,
+    ) -> Self {
+        Self { receiver, lifecycle }
+    }
+}

From a9bd493da1a5cf208feeb40203b6c67a205b5bb7 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Tue, 5 Aug 2025 23:11:39 +0800
Subject: [PATCH 18/20] Link against rcl_action

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/build.rs | 1 +
 1 file changed, 1 insertion(+)

diff --git a/rclrs/build.rs b/rclrs/build.rs
index 04927ff0..f5bc733e 100644
--- a/rclrs/build.rs
+++ b/rclrs/build.rs
@@ -116,6 +116,7 @@ fn main() {
     }
 
     println!("cargo:rustc-link-lib=dylib=rcl");
+    println!("cargo:rustc-link-lib=dylib=rcl_action");
     println!("cargo:rustc-link-lib=dylib=rcl_yaml_param_parser");
     println!("cargo:rustc-link-lib=dylib=rcutils");
     println!("cargo:rustc-link-lib=dylib=rmw");

From d2019926f6088c1853f8961e3022582bb05561f9 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Sun, 10 Aug 2025 23:37:49 +0800
Subject: [PATCH 19/20] Add initial tests and improve ergonomics

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 rclrs/Cargo.toml                              |   9 ++
 rclrs/src/action.rs                           | 151 +++++++++++++++++-
 rclrs/src/action/action_client.rs             | 106 ++++++++----
 .../action/action_client/feedback_client.rs   |  12 +-
 rclrs/src/action/action_client/goal_client.rs |  82 +++++++++-
 .../action_client/requested_goal_client.rs    |  25 ++-
 .../src/action/action_client/result_client.rs |  40 ++++-
 .../src/action/action_client/status_client.rs |  71 ++++++--
 rclrs/src/action/action_server.rs             |  17 +-
 .../src/action/action_server/accepted_goal.rs |  54 ++++++-
 .../action_server_goal_handle.rs              |   2 +-
 .../action_server/cancellation_state.rs       |  14 ++
 .../action/action_server/cancelling_goal.rs   |  24 ++-
 .../action/action_server/executing_goal.rs    |  54 ++++++-
 .../action_server/feedback_publisher.rs       |  22 +++
 .../action_server/live_action_server_goal.rs  |  54 +++++--
 16 files changed, 636 insertions(+), 101 deletions(-)
 create mode 100644 rclrs/src/action/action_server/feedback_publisher.rs

diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml
index 71627de9..4c2ce4f5 100644
--- a/rclrs/Cargo.toml
+++ b/rclrs/Cargo.toml
@@ -17,12 +17,18 @@ path = "src/lib.rs"
 # Needed for dynamically finding type support libraries
 ament_rs = { version = "0.2", optional = true }
 
+# Needed to create the GoalClientStream
+async-stream = "*"
+
 # Needed for uploading documentation to docs.rs
 cfg-if = "1.0.0"
 
 # Needed for clients
 futures = "0.3"
 
+# Needed for racing futures
+futures-lite = { version = "2.6", features = ["std", "race"] }
+
 # Needed for the runtime-agnostic timeout feature
 async-std = "1.13"
 
@@ -42,6 +48,9 @@ serde-big-array = { version = "0.5.1", optional = true }
 # into the tokio runtime.
 tokio = { version = "1", features = ["sync"] }
 
+# Needed to combine concurrent streams for easier ergonomics in action clients
+tokio-stream = "0.1"
+
 # Needed by action clients to generate UUID values for their goals
 uuid = { version = "1", features = ["v4"] }
 
diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs
index 8cc09c05..fd826433 100644
--- a/rclrs/src/action.rs
+++ b/rclrs/src/action.rs
@@ -10,8 +10,9 @@ pub(crate) mod action_server;
 pub use action_server::*;
 
 use crate::{
-    rcl_bindings::RCL_ACTION_UUID_SIZE,
+    rcl_bindings::*,
     vendor::builtin_interfaces::msg::Time,
+    DropGuard,
     log_error,
 };
 use std::fmt;
@@ -95,7 +96,7 @@ impl CancelResponseCode {
 
 impl From<i8> for CancelResponseCode {
     fn from(value: i8) -> Self {
-        if value <= 0 && value <= 3 {
+        if 0 <= value && value <= 3 {
             unsafe {
                 // SAFETY: We have already ensured that the integer value is
                 // within the acceptable range for the enum, so transmuting is
@@ -177,7 +178,7 @@ pub enum GoalStatusCode {
 
 impl From<i8> for GoalStatusCode {
     fn from(value: i8) -> Self {
-        if value <= 0 && value <= 6 {
+        if 0 <= value && value <= 6 {
             unsafe {
                 // SAFETY: We have already ensured that the integer value is
                 // within the acceptable range for the enum, so transmuting is
@@ -208,3 +209,147 @@ pub struct GoalStatus {
     /// client, so care should be taken when using this time value.
     pub stamp: Time,
 }
+
+fn empty_goal_status_array() -> DropGuard<rcl_action_goal_status_array_t> {
+    DropGuard::new(
+        unsafe {
+            // SAFETY: No preconditions
+            let mut array = rcl_action_get_zero_initialized_goal_status_array();
+            array.allocator = rcutils_get_default_allocator();
+            array
+        },
+        |mut goal_statuses| unsafe {
+            // SAFETY: The goal_status array is either zero-initialized and empty or populated by
+            // `rcl_action_get_goal_status_array`. In either case, it can be safely finalized.
+            rcl_action_goal_status_array_fini(&mut goal_statuses);
+        }
+    )
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::*;
+    use example_interfaces::action::{Fibonacci, Fibonacci_Goal, Fibonacci_Result, Fibonacci_Feedback};
+    use tokio::sync::mpsc::unbounded_channel;
+    use futures::StreamExt;
+    use std::time::Duration;
+
+    #[test]
+    fn test_action_success() {
+        let mut executor = Context::default().create_basic_executor();
+
+        let node = executor.create_node(&format!("test_action_success_{}", line!())).unwrap();
+        let action_name = format!("test_action_success_{}_action", line!());
+        let _action_server = node.create_action_server(
+            &action_name,
+            fibonacci_action,
+        ).unwrap();
+
+        let client = node.create_action_client::<Fibonacci>(&action_name).unwrap();
+
+        let order_10_sequence = [1, 1, 2, 3, 5, 8, 13, 21, 34, 55];
+
+        let request = client.request_goal(Fibonacci_Goal {
+            order: 10,
+        });
+
+        let promise = executor.commands().run(async move {
+            let mut goal_client_stream = request.await.unwrap().stream();
+            let mut expected_feedback_len = 0;
+            while let Some(event) = goal_client_stream.next().await {
+                match event {
+                    GoalEvent::Feedback(feedback) => {
+                        expected_feedback_len += 1;
+                        assert_eq!(feedback.sequence.len(), expected_feedback_len);
+                    }
+                    GoalEvent::Status(s) => {
+                        assert!(
+                            matches!(s.code, GoalStatusCode::Unknown | GoalStatusCode::Executing | GoalStatusCode::Succeeded),
+                            "Actual code: {:?}",
+                            s.code,
+                        );
+                    }
+                    GoalEvent::Result((status, result)) => {
+                        assert_eq!(status, GoalStatusCode::Succeeded);
+                        assert_eq!(result.sequence, order_10_sequence);
+                        return;
+                    }
+                }
+            }
+        });
+
+        executor.spin(SpinOptions::default().until_promise_resolved(promise));
+
+        let request = client.request_goal(Fibonacci_Goal {
+            order: 10,
+        });
+
+        let promise = executor.commands().run(async move {
+            let (status, result) = request.await.unwrap().result.await;
+            assert_eq!(status, GoalStatusCode::Succeeded);
+            assert_eq!(result.sequence, order_10_sequence);
+        });
+
+        executor.spin(SpinOptions::default().until_promise_resolved(promise));
+    }
+
+    async fn fibonacci_action(handle: RequestedGoal<Fibonacci>) -> TerminatedGoal {
+        let goal_order = handle.goal().order;
+        if goal_order < 0 {
+            return handle.reject();
+        }
+
+        let mut result = Fibonacci_Result::default();
+
+        let executing = match handle.accept().begin() {
+            BeginAcceptedGoal::Execute(executing) => executing,
+            BeginAcceptedGoal::Cancel(cancelling) => {
+                return cancelling.cancelled_with(result);
+            }
+        };
+
+        let (sender, mut receiver) = unbounded_channel();
+        std::thread::spawn(move || {
+
+            let mut previous = 0;
+            let mut current = 1;
+
+            for _ in 0..goal_order {
+                if let Err(_) = sender.send(current) {
+                    // The action has been cancelled early, so just drop this thread.
+                    return;
+                }
+
+                let next = previous + current;
+                previous = current;
+                current = next;
+                std::thread::sleep(Duration::from_micros(10));
+            }
+        });
+
+        let mut sequence = Vec::new();
+        loop {
+            match executing.unless_cancel_requested(receiver.recv()).await {
+                Ok(Some(next)) => {
+                    // We have a new item in the sequence
+                    sequence.push(next);
+                    executing.publish_feedback(
+                        Fibonacci_Feedback {
+                            sequence: sequence.clone(),
+                        }
+                    );
+                }
+                Ok(None) => {
+                    // The sequence has finished
+                    result.sequence = sequence;
+                    return executing.succeeded_with(result);
+                }
+                Err(_) => {
+                    // The action has been cancelled
+                    result.sequence = sequence;
+                    return executing.begin_cancelling().cancelled_with(result);
+                }
+            }
+        }
+    }
+}
diff --git a/rclrs/src/action/action_client.rs b/rclrs/src/action/action_client.rs
index 8b815fe0..72fb54dd 100644
--- a/rclrs/src/action/action_client.rs
+++ b/rclrs/src/action/action_client.rs
@@ -11,6 +11,7 @@ use crate::{
     RclPrimitiveHandle, RclPrimitiveKind, ReadyKind, TakeFailedAsNone, ToResult,
     Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
 };
+use super::empty_goal_status_array;
 use std::{
     any::Any,
     borrow::{Borrow, Cow},
@@ -140,11 +141,33 @@ impl<A: Action> ActionClientState<A> {
         self.board.create_feedback_client(self, goal_id)
     }
 
-    /// Get a client to watch the status of a specific goal.
-    pub fn watch_status(self: &Arc<Self>, goal_id: GoalUuid) -> StatusClient<A> {
+    /// Get a client to receive all status updates for a specific goal. If you
+    /// only care about the latest status update, consider using [`Self::watch_status`]
+    /// instead.
+    pub fn receive_status(self: &Arc<Self>, goal_id: GoalUuid) -> StatusClient<A> {
         self.board.create_status_client(self, goal_id)
     }
 
+    /// Get a client to watch the status of a specific goal.
+    pub fn watch_status(self: &Arc<Self>, goal_id: GoalUuid) -> StatusWatcher<A> {
+        self.board.create_status_watcher(self, goal_id)
+    }
+
+    /// Ask to receive the result of a goal. You can `.await` the [`ResultClient`]
+    /// to get the result asynchronously.
+    ///
+    /// In the unlikely event of an error at the rcl layer, this will panic.
+    /// Use [`Self::try_receive_result`] to catch errors without panicking.
+    pub fn receive_result(self: &Arc<Self>, goal_id: GoalUuid) -> ResultClient<A> {
+        self.try_receive_result(goal_id).unwrap()
+    }
+
+    /// Try to receive the result of a goal. If an rcl error happens, you can
+    /// handle it.
+    pub fn try_receive_result(self: &Arc<Self>, goal_id: GoalUuid) -> Result<ResultClient<A>, RclrsError> {
+        self.board.request_result(Arc::clone(self), goal_id)
+    }
+
     /// Ask the action server to cancel a single goal.
     ///
     /// In the unlikely event of an error at the rcl layer, this will panic.
@@ -244,6 +267,7 @@ impl<A: Action> ActionClientState<A> {
             pending_goal_clients: Default::default(),
             feedback_senders: Default::default(),
             status_senders: Default::default(),
+            status_posters: Default::default(),
             cancel_response_senders: Default::default(),
             result_senders: Default::default(),
             client: Default::default(),
@@ -267,7 +291,8 @@ impl<A: Action> ActionClientState<A> {
 struct ActionClientGoalBoard<A: Action> {
     pending_goal_clients: Mutex<HashMap<i64, PendingGoalClient<A>>>,
     feedback_senders: Mutex<HashMap<GoalUuid, Vec<UnboundedSender<A::Feedback>>>>,
-    status_senders: Mutex<HashMap<GoalUuid, WatchSender<GoalStatus>>>,
+    status_senders: Mutex<HashMap<GoalUuid, Vec<UnboundedSender<GoalStatus>>>>,
+    status_posters: Mutex<HashMap<GoalUuid, WatchSender<GoalStatus>>>,
     cancel_response_senders: Mutex<HashMap<i64, CancelResponseSender>>,
     result_senders: Mutex<HashMap<i64, Sender<(GoalStatusCode, A::Result)>>>,
     handle: Arc<ActionClientHandle>,
@@ -313,6 +338,7 @@ impl<A: Action> ActionClientGoalBoard<A> {
         };
 
         let all_status_senders = self.status_senders.lock().unwrap();
+        let all_status_watchers = self.status_posters.lock().unwrap();
         for index in 0..goal_statuses.msg.status_list.size {
             let rcl_status = unsafe {
                 &*goal_statuses.msg.status_list.data.add(index)
@@ -326,8 +352,14 @@ impl<A: Action> ActionClientGoalBoard<A> {
                 stamp: Time { sec: stamp.sec, nanosec: stamp.nanosec },
             };
 
-            if let Some(sender) = all_status_senders.get(&goal_id) {
-                sender.send_modify(|watched_status| *watched_status = status);
+            if let Some(senders) = all_status_senders.get(&goal_id) {
+                for sender in senders {
+                    let _ = sender.send(status.clone());
+                }
+            }
+
+            if let Some(watcher) = all_status_watchers.get(&goal_id) {
+                watcher.send_modify(|watched_status| *watched_status = status);
             }
         }
 
@@ -456,7 +488,7 @@ impl<A: Action> ActionClientGoalBoard<A> {
 
         let (sender, receiver) = oneshot_channel();
         let feedback = self.create_feedback_client(client, goal_id);
-        let status = self.create_status_client(client, goal_id);
+        let status = self.create_status_watcher(client, goal_id);
 
         self.pending_goal_clients.lock().unwrap().insert(
             seq,
@@ -482,6 +514,7 @@ impl<A: Action> ActionClientGoalBoard<A> {
         self.feedback_senders.lock().unwrap().entry(goal_id).or_default().push(sender);
         FeedbackClient::new(
             receiver,
+            goal_id,
             GoalClientLifecycle {
                 kind: GoalClientKind::Feedback(goal_id),
                 client: Arc::clone(client),
@@ -494,8 +527,25 @@ impl<A: Action> ActionClientGoalBoard<A> {
         client: &ActionClient<A>,
         goal_id: GoalUuid,
     ) -> StatusClient<A> {
-        let receiver = self
-            .status_senders
+        let (sender, receiver) = unbounded_channel();
+        self.status_senders.lock().unwrap().entry(goal_id).or_default().push(sender);
+        StatusClient::new(
+            receiver,
+            goal_id,
+            GoalClientLifecycle {
+                kind: GoalClientKind::Status(goal_id),
+                client: Arc::clone(client),
+            },
+        )
+    }
+
+    fn create_status_watcher(
+        &self,
+        client: &ActionClient<A>,
+        goal_id: GoalUuid,
+    ) -> StatusWatcher<A> {
+        let watcher = self
+            .status_posters
             .lock()
             .unwrap()
             .entry(goal_id)
@@ -508,8 +558,8 @@ impl<A: Action> ActionClientGoalBoard<A> {
             )
             .subscribe();
 
-        StatusClient::new(
-            receiver,
+        StatusWatcher::new(
+            watcher,
             Arc::new(GoalClientLifecycle {
                 kind: GoalClientKind::Status(goal_id),
                 client: Arc::clone(client),
@@ -587,7 +637,7 @@ impl<A: Action> ActionClientGoalBoard<A> {
 struct PendingGoalClient<A: Action> {
     goal_id: GoalUuid,
     feedback: FeedbackClient<A>,
-    status: StatusClient<A>,
+    status: StatusWatcher<A>,
     sender: Sender<Option<GoalClient<A>>>,
 }
 
@@ -630,11 +680,24 @@ impl<A: Action> Drop for GoalClientLifecycle<A> {
                 }
             }
             GoalClientKind::Status(goal_uuid) => {
-                let mut all_status_senders = self.client.board.status_senders.lock().unwrap();
+                {
+                    let mut all_status_senders = self.client.board.status_senders.lock().unwrap();
+                    let mut is_empty = false;
+                    if let Some(senders) = all_status_senders.get_mut(&goal_uuid) {
+                        senders.retain(|sender| !sender.is_closed());
+                        is_empty = senders.is_empty();
+                    }
+                    if is_empty {
+                        all_status_senders.remove(&goal_uuid);
+                    }
+                }
 
-                let remove = all_status_senders.get(&goal_uuid).is_some_and(|sender| sender.is_closed());
-                if remove {
-                    all_status_senders.remove(&goal_uuid);
+                {
+                    let mut all_status_posters = self.client.board.status_posters.lock().unwrap();
+                    let remove = all_status_posters.get(&goal_uuid).is_some_and(|sender| sender.is_closed());
+                    if remove {
+                        all_status_posters.remove(&goal_uuid);
+                    }
                 }
             }
             GoalClientKind::CancelResponse(seq) => {
@@ -736,18 +799,7 @@ impl ActionClientHandle {
     }
 
     fn take_status(&self) -> Result<DropGuard<rcl_action_goal_status_array_t>, RclrsError> {
-        let mut goal_statuses = DropGuard::new(
-            unsafe {
-                // SAFETY: No preconditions
-                rcl_action_get_zero_initialized_goal_status_array()
-            },
-            |mut goal_statuses| unsafe {
-                // SAFETY: The goal_status array is either zero-initialized and empty or populated by
-                // `rcl_action_get_goal_status_array`. In either case, it can be safely finalized.
-                rcl_action_goal_status_array_fini(&mut goal_statuses);
-            }
-        );
-
+        let mut goal_statuses = empty_goal_status_array();
         unsafe {
             let handle = self.lock();
             rcl_action_take_status(&*handle, &mut *goal_statuses as *mut _ as *mut _)
diff --git a/rclrs/src/action/action_client/feedback_client.rs b/rclrs/src/action/action_client/feedback_client.rs
index f69d57c3..ff8468ac 100644
--- a/rclrs/src/action/action_client/feedback_client.rs
+++ b/rclrs/src/action/action_client/feedback_client.rs
@@ -1,4 +1,4 @@
-use super::GoalClientLifecycle;
+use super::{GoalClientLifecycle, GoalUuid};
 use rosidl_runtime_rs::Action;
 use tokio::sync::mpsc::UnboundedReceiver;
 use std::ops::{Deref, DerefMut};
@@ -8,6 +8,7 @@ use std::ops::{Deref, DerefMut};
 /// feedback messages.
 pub struct FeedbackClient<A: Action> {
     receiver: UnboundedReceiver<A::Feedback>,
+    goal_id: GoalUuid,
     /// This keeps track of whether any goal client components are still being used.
     /// This must come after the receiver in the struct to ensure cleanup is done
     /// correctly.
@@ -15,6 +16,12 @@ pub struct FeedbackClient<A: Action> {
     lifecycle: GoalClientLifecycle<A>,
 }
 
+impl<A: Action> Clone for FeedbackClient<A> {
+    fn clone(&self) -> Self {
+        self.lifecycle.client.receive_feedback(self.goal_id)
+    }
+}
+
 impl<A: Action> Deref for FeedbackClient<A> {
     type Target = UnboundedReceiver<A::Feedback>;
     fn deref(&self) -> &Self::Target {
@@ -31,8 +38,9 @@ impl<A: Action> DerefMut for FeedbackClient<A> {
 impl<A: Action> FeedbackClient<A> {
     pub(super) fn new(
         receiver: UnboundedReceiver<A::Feedback>,
+        goal_id: GoalUuid,
         lifecycle: GoalClientLifecycle<A>,
     ) -> Self {
-        Self { receiver, lifecycle }
+        Self { receiver, goal_id, lifecycle }
     }
 }
diff --git a/rclrs/src/action/action_client/goal_client.rs b/rclrs/src/action/action_client/goal_client.rs
index 3bcfddf5..3fe7fdf9 100644
--- a/rclrs/src/action/action_client/goal_client.rs
+++ b/rclrs/src/action/action_client/goal_client.rs
@@ -1,8 +1,13 @@
 use crate::{
     vendor::builtin_interfaces::msg::Time,
-    CancellationClient, FeedbackClient, StatusClient, ResultClient,
+    CancellationClient, FeedbackClient, GoalStatus, GoalStatusCode, StatusWatcher, ResultClient,
 };
 use rosidl_runtime_rs::Action;
+use tokio_stream::{StreamMap, Stream};
+use std::{
+    pin::Pin,
+    task::{Context, Poll},
+};
 
 /// The goal client bundles a set of receivers that will allow you to await
 /// different information from the action server, such as feedback messages,
@@ -20,7 +25,7 @@ pub struct GoalClient<A: Action> {
     /// Receive feedback messages for the goal.
     pub feedback: FeedbackClient<A>,
     /// Watch the status of the goal.
-    pub status: StatusClient<A>,
+    pub status: StatusWatcher<A>,
     /// Get the final result of the goal.
     pub result: ResultClient<A>,
     /// Use this if you want to request the goal to be cancelled.
@@ -28,3 +33,76 @@ pub struct GoalClient<A: Action> {
     /// The time that the goal was accepted.
     pub stamp: Time,
 }
+
+impl<A: Action> Clone for GoalClient<A> {
+    fn clone(&self) -> Self {
+        Self {
+            feedback: self.feedback.clone(),
+            status: self.status.clone(),
+            result: self.result.clone(),
+            cancellation: self.cancellation.clone(),
+            stamp: self.stamp.clone(),
+        }
+    }
+}
+
+impl<A: Action> GoalClient<A> {
+    /// Create a concurrent stream that will emit events related to this goal.
+    pub fn stream(self) -> GoalClientStream<A> {
+        let Self { mut feedback, mut status, result, .. } = self;
+
+        let rx_feedback = Box::pin(async_stream::stream! {
+            while let Some(msg) = feedback.recv().await {
+                yield GoalEvent::Feedback(msg);
+            }
+        }) as Pin<Box<dyn Stream<Item = GoalEvent<A>> + Send>>;
+
+        let rx_status = Box::pin(async_stream::stream! {
+            let initial_value = (*status.borrow_and_update()).clone();
+            yield GoalEvent::Status(initial_value);
+
+            while let Ok(_) = status.changed().await {
+                let value = (*status.borrow_and_update()).clone();
+                yield GoalEvent::Status(value);
+            }
+        }) as Pin<Box<dyn Stream<Item = GoalEvent<A>> + Send>>;
+
+        let rx_result = Box::pin(async_stream::stream! {
+            yield GoalEvent::Result(result.await);
+        }) as Pin<Box<dyn Stream<Item = GoalEvent<A>> + Send>>;
+
+        let mut stream_map: StreamMap<i32, Pin<Box<dyn Stream<Item = GoalEvent<A>> + Send>>> = StreamMap::new();
+        stream_map.insert(0, rx_feedback);
+        stream_map.insert(1, rx_status);
+        stream_map.insert(2, rx_result);
+        GoalClientStream { stream_map }
+    }
+}
+
+/// Use this to
+pub struct GoalClientStream<A: Action> {
+    stream_map: StreamMap<i32, Pin<Box<dyn Stream<Item = GoalEvent<A>> + Send>>>,
+}
+
+impl<A: Action> Stream for GoalClientStream<A> {
+    type Item = GoalEvent<A>;
+
+    fn poll_next(self: Pin<&mut Self>, cx: &mut Context) -> Poll<Option<Self::Item>> {
+        Stream::poll_next(Pin::new(&mut self.get_mut().stream_map), cx)
+            .map(|r| r.map(|(_, event)| event))
+    }
+
+    fn size_hint(&self) -> (usize, Option<usize>) {
+        self.stream_map.size_hint()
+    }
+}
+
+/// Any of the possible update events that can happen for a goal.
+pub enum GoalEvent<A: Action> {
+    /// A feedback message was received
+    Feedback(A::Feedback),
+    /// A status update was received
+    Status(GoalStatus),
+    /// The result of the goal was received
+    Result((GoalStatusCode, A::Result)),
+}
diff --git a/rclrs/src/action/action_client/requested_goal_client.rs b/rclrs/src/action/action_client/requested_goal_client.rs
index 12a26651..33495f3b 100644
--- a/rclrs/src/action/action_client/requested_goal_client.rs
+++ b/rclrs/src/action/action_client/requested_goal_client.rs
@@ -2,11 +2,18 @@ use crate::{GoalClient, GoalUuid};
 use super::GoalClientLifecycle;
 use rosidl_runtime_rs::Action;
 use tokio::sync::oneshot::Receiver;
-use std::ops::{Deref, DerefMut};
+use std::{
+    future::Future,
+    ops::{Deref, DerefMut},
+    pin::Pin,
+    task::{Context, Poll},
+};
 
 /// This struct allows you to receive a [`GoalClient`] for a goal that you
-/// requested. Through the [`DerefMut`] trait you can use the [`oneshot::Receiver`][Receiver]
-/// API to await the goal client. Note that it can only be received once.
+/// requested. Call `.await` on this to obtain the response to the goal request.
+/// Note that the [`GoalClient`] can only be received once.
+///
+/// Through the [`DerefMut`] trait you can use the [`oneshot::Receiver`][Receiver] API.
 ///
 /// If the action server rejects the goal then this will yield a [`None`] instead
 /// of a [`GoalClient`].
@@ -47,3 +54,15 @@ impl<A: Action> DerefMut for RequestedGoalClient<A> {
         &mut self.receiver
     }
 }
+
+impl<A: Action> Future for RequestedGoalClient<A> {
+    type Output = Option<GoalClient<A>>;
+
+    fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
+        Future::poll(Pin::new(&mut self.get_mut().receiver), cx)
+            // SAFETY: The Receiver returns an Err if the sender is dropped, but
+            // the RegisteredGoalClient makes sure that the sender is alive in
+            // the ActionClient, so we can always safely unwrap this.
+            .map(|result| result.unwrap())
+    }
+}
diff --git a/rclrs/src/action/action_client/result_client.rs b/rclrs/src/action/action_client/result_client.rs
index 8a21330f..8b9aef42 100644
--- a/rclrs/src/action/action_client/result_client.rs
+++ b/rclrs/src/action/action_client/result_client.rs
@@ -2,18 +2,34 @@ use crate::GoalStatusCode;
 use super::GoalClientLifecycle;
 use rosidl_runtime_rs::Action;
 use tokio::sync::oneshot::Receiver;
-use std::ops::{Deref, DerefMut};
+use std::{
+    future::Future,
+    ops::{Deref, DerefMut},
+    pin::Pin,
+    sync::Arc,
+    task::{Context, Poll},
+};
+use futures::{future::Shared, FutureExt};
 
 /// This struct allows you to receive the result of the goal. Through the [`DerefMut`]
 /// trait you can use the [`oneshot::Receiver`][Receiver] API to await the final
 /// result. Note that it can only be received once.
 pub struct ResultClient<A: Action> {
-    receiver: Receiver<(GoalStatusCode, A::Result)>,
+    receiver: Shared<Receiver<(GoalStatusCode, A::Result)>>,
     /// This keeps track of whether any goal client components are still being used.
     /// This must come after the receiver in the struct to ensure cleanup is done
     /// correctly.
     #[allow(unused)]
-    lifecycle: GoalClientLifecycle<A>,
+    lifecycle: Arc<GoalClientLifecycle<A>>,
+}
+
+impl<A: Action> Clone for ResultClient<A> {
+    fn clone(&self) -> Self {
+        Self {
+            receiver: self.receiver.clone(),
+            lifecycle: Arc::clone(&self.lifecycle),
+        }
+    }
 }
 
 impl<A: Action> ResultClient<A> {
@@ -21,12 +37,15 @@ impl<A: Action> ResultClient<A> {
         receiver: Receiver<(GoalStatusCode, A::Result)>,
         lifecycle: GoalClientLifecycle<A>,
     ) -> Self {
-        Self { receiver, lifecycle }
+        Self {
+            receiver: receiver.shared(),
+            lifecycle: Arc::new(lifecycle),
+        }
     }
 }
 
 impl<A: Action> Deref for ResultClient<A> {
-    type Target = Receiver<(GoalStatusCode, A::Result)>;
+    type Target = Shared<Receiver<(GoalStatusCode, A::Result)>>;
     fn deref(&self) -> &Self::Target {
         &self.receiver
     }
@@ -38,3 +57,14 @@ impl<A: Action> DerefMut for ResultClient<A> {
     }
 }
 
+impl<A: Action> Future for ResultClient<A> {
+    type Output = (GoalStatusCode, A::Result);
+
+    fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
+        Future::poll(Pin::new(&mut self.get_mut().receiver), cx)
+            // SAFETY: The Receiver returns an Err if the sender is dropped, but
+            // the RegisteredGoalClient makes sure that the sender is alive in
+            // the ActionClient, so we can always safely unwrap this.
+            .map(|result| result.unwrap())
+    }
+}
diff --git a/rclrs/src/action/action_client/status_client.rs b/rclrs/src/action/action_client/status_client.rs
index b271b440..d8edb1f3 100644
--- a/rclrs/src/action/action_client/status_client.rs
+++ b/rclrs/src/action/action_client/status_client.rs
@@ -1,17 +1,62 @@
-use crate::GoalStatus;
+use crate::{GoalStatus, GoalUuid};
 use super::GoalClientLifecycle;
 use rosidl_runtime_rs::Action;
-use tokio::sync::watch::Receiver as Watcher;
+use tokio::sync::{watch::Receiver as Watcher, mpsc::UnboundedReceiver};
 use std::{
     ops::{Deref, DerefMut},
     sync::Arc,
 };
 
+/// This struct allows you to receive every status update experienced by a goal.
+///
+/// If you only care about checking the latest status, you can call [`Self::watch`]
+/// to use a [`StatusWatcher`] instead.
+pub struct StatusClient<A: Action> {
+    receiver: UnboundedReceiver<GoalStatus>,
+    goal_id: GoalUuid,
+    #[allow(unused)]
+    lifecycle: GoalClientLifecycle<A>,
+}
+
+impl<A: Action> Clone for StatusClient<A> {
+    fn clone(&self) -> Self {
+        self.lifecycle.client.receive_status(self.goal_id)
+    }
+}
+
+impl<A: Action> Deref for StatusClient<A> {
+    type Target = UnboundedReceiver<GoalStatus>;
+    fn deref(&self) -> &Self::Target {
+        &self.receiver
+    }
+}
+
+impl<A: Action> DerefMut for StatusClient<A> {
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        &mut self.receiver
+    }
+}
+
+impl<A: Action> StatusClient<A> {
+    /// Watch the latest value of the goal status.
+    pub fn watch(&self) -> StatusWatcher<A> {
+        self.lifecycle.client.watch_status(self.goal_id)
+    }
+
+    pub(super) fn new(
+        receiver: UnboundedReceiver<GoalStatus>,
+        goal_id: GoalUuid,
+        lifecycle: GoalClientLifecycle<A>,
+    ) -> Self {
+        Self { receiver, goal_id, lifecycle }
+    }
+}
+
 /// This struct allows you to monitor the status of the goal. Through the
 /// [`DerefMut`] trait you can use the [`watch::Receiver`][Watcher] API to
 /// check the latest status and monitor changes.
-pub struct StatusClient<A: Action> {
-    receiver: Watcher<GoalStatus>,
+pub struct StatusWatcher<A: Action> {
+    watcher: Watcher<GoalStatus>,
     /// This keeps track of whether any goal client components are still being used.
     /// This must come after the receiver in the struct to ensure cleanup is done
     /// correctly.
@@ -19,33 +64,33 @@ pub struct StatusClient<A: Action> {
     lifecycle: Arc<GoalClientLifecycle<A>>,
 }
 
-impl<A: Action> Clone for StatusClient<A> {
+impl<A: Action> Clone for StatusWatcher<A> {
     fn clone(&self) -> Self {
         Self {
-            receiver: self.receiver.clone(),
+            watcher: self.watcher.clone(),
             lifecycle: Arc::clone(&self.lifecycle),
         }
     }
 }
 
-impl<A: Action> Deref for StatusClient<A> {
+impl<A: Action> Deref for StatusWatcher<A> {
     type Target = Watcher<GoalStatus>;
     fn deref(&self) -> &Self::Target {
-        &self.receiver
+        &self.watcher
     }
 }
 
-impl<A: Action> DerefMut for StatusClient<A> {
+impl<A: Action> DerefMut for StatusWatcher<A> {
     fn deref_mut(&mut self) -> &mut Self::Target {
-        &mut self.receiver
+        &mut self.watcher
     }
 }
 
-impl<A: Action> StatusClient<A> {
+impl<A: Action> StatusWatcher<A> {
     pub(super) fn new(
-        receiver: Watcher<GoalStatus>,
+        watcher: Watcher<GoalStatus>,
         lifecycle: Arc<GoalClientLifecycle<A>>,
     ) -> Self {
-        Self { receiver, lifecycle }
+        Self { watcher, lifecycle }
     }
 }
diff --git a/rclrs/src/action/action_server.rs b/rclrs/src/action/action_server.rs
index f8c933a4..e6b7153c 100644
--- a/rclrs/src/action/action_server.rs
+++ b/rclrs/src/action/action_server.rs
@@ -6,6 +6,7 @@ use crate::{
     RclPrimitiveHandle, RclPrimitiveKind, ReadyKind, TakeFailedAsNone,
     Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
 };
+use super::empty_goal_status_array;
 use rosidl_runtime_rs::{Action, Message, RmwGoalRequest, RmwResultRequest};
 use std::{
     any::Any,
@@ -33,6 +34,9 @@ use cancelling_goal::*;
 mod executing_goal;
 pub use executing_goal::*;
 
+mod feedback_publisher;
+pub use feedback_publisher::*;
+
 mod live_action_server_goal;
 use live_action_server_goal::*;
 
@@ -550,18 +554,7 @@ impl<A: Action> ActionServerHandle<A> {
     }
 
     pub(super) fn publish_status(&self) -> Result<(), RclrsError> {
-        let mut goal_statuses = DropGuard::new(
-            unsafe {
-                // SAFETY: No preconditions
-                rcl_action_get_zero_initialized_goal_status_array()
-            },
-            |mut goal_statuses| unsafe {
-                // SAFETY: The goal_status array is either zero-initialized and empty or populated by
-                // `rcl_action_get_goal_status_array`. In either case, it can be safely finalized.
-                rcl_action_goal_status_array_fini(&mut goal_statuses);
-            },
-        );
-
+        let mut goal_statuses = empty_goal_status_array();
         let rcl_handle = self.lock();
         unsafe {
             // SAFETY: The action server is locked through the handle and goal_statuses is
diff --git a/rclrs/src/action/action_server/accepted_goal.rs b/rclrs/src/action/action_server/accepted_goal.rs
index e40e2596..3f8eeccb 100644
--- a/rclrs/src/action/action_server/accepted_goal.rs
+++ b/rclrs/src/action/action_server/accepted_goal.rs
@@ -1,4 +1,4 @@
-use super::{CancellingGoal, ExecutingGoal, LiveActionServerGoal};
+use super::{CancellingGoal, ExecutingGoal, FeedbackPublisher, LiveActionServerGoal};
 use std::{
     future::Future,
     sync::Arc
@@ -33,10 +33,13 @@ impl<A: Action> AcceptedGoal<A> {
     /// Process a [`Future`] until it is finished or until a cancellation request
     /// is received.
     ///
-    /// If the [`Future`] finishes, its output will be provided in [`Ok`]. If a
-    /// cancellation request is received before the [`Future`] is finished, you
-    /// will receive an [`Err`] with the current state of the [`Future`], which
-    /// you can continue processing later if you choose.
+    /// If the [`Future`] finishes, its output will be provided in [`Ok`].
+    ///
+    /// If a cancellation request is received before the [`Future`] is finished,
+    /// you will receive an [`Err`] with the current state of the [`Future`],
+    /// which you can continue processing later if you choose. If you would rather
+    /// discard the [`Future`] when a cancellation request is received, you can
+    /// call [`Self::unless_cancel_requested`] instead.
     ///
     /// After the cancellation request is received, you will still need to trigger
     /// [`Self::begin_cancelling`] or [`Self::reject_cancellation`] to respond to
@@ -48,6 +51,27 @@ impl<A: Action> AcceptedGoal<A> {
         self.live.cancellation().until_cancel_requested(f).await
     }
 
+    /// Process a [`Future`] until it is finished unless a cancellation request
+    /// is received.
+    ///
+    /// If the [`Future`] finishes, its output will be provided in [`Ok`].
+    ///
+    /// If a cancellation request is received before the [`Future`] is finished,
+    /// the [`Future`] will be discarded. This allows non-[`Unpin`] futures to
+    /// be passed to this method. If your future implements [`Unpin`] and you want
+    /// the option to keep processing it after the cancellation request is received,
+    /// then you can call [`Self::until_cancel_requested`] instead.
+    ///
+    /// After the cancellation request is received, you will still need to trigger
+    /// [`Self::begin_cancelling`] or [`Self::reject_cancellation`] to respond to
+    /// the request. Otherwise the cancellation request will not receive a response
+    /// until the goal reaches a terminal state.
+    //
+    // TODO(@mxgrey): Add a doctest and example for this.
+    pub async fn unless_cancel_requested<F: Future>(&self, f: F) -> Result<F::Output, ()> {
+        self.live.cancellation().unless_cancel_requested(f).await
+    }
+
     /// Transition the goal into the cancelling state.
     ///
     /// This does not require an action client to request a cancellation. If no
@@ -82,17 +106,31 @@ impl<A: Action> AcceptedGoal<A> {
     #[must_use]
     pub fn begin(self) -> BeginAcceptedGoal<A> {
         if self.live.cancel_requested() {
-            BeginAcceptedGoal::Execute(self.execute())
-        } else {
             BeginAcceptedGoal::Cancel(self.begin_cancelling())
+        } else {
+            BeginAcceptedGoal::Execute(self.execute())
         }
     }
 
     /// Publish feedback for action clients to read.
-    pub fn publish_feedback(&self, feedback: &A::Feedback) {
+    ///
+    /// If you need to publish feedback from a separate thread or async task
+    /// which does not have direct access to the goal's state machine, you can
+    /// use [`Self::feedback_publisher`] to get a handle that you can pass along.
+    pub fn publish_feedback(&self, feedback: A::Feedback) {
         self.live.publish_feedback(feedback);
     }
 
+    /// Get a handle specifically for publishing feedback for this goal. This
+    /// publisher can be used separately from the overall state machine of the
+    /// goal, but it will stop working once the goal reaches a terminal state.
+    ///
+    /// If you just need to publish a one-off feedback message, you can use
+    /// [`Self::publish_feedback`].
+    pub fn feedback_publisher(&self) -> FeedbackPublisher<A> {
+        FeedbackPublisher::new(Arc::clone(&self.live))
+    }
+
     pub(super) fn new(live: Arc<LiveActionServerGoal<A>>) -> Self {
         Self { live }
     }
diff --git a/rclrs/src/action/action_server/action_server_goal_handle.rs b/rclrs/src/action/action_server/action_server_goal_handle.rs
index e71df5e7..3351c170 100644
--- a/rclrs/src/action/action_server/action_server_goal_handle.rs
+++ b/rclrs/src/action/action_server/action_server_goal_handle.rs
@@ -43,7 +43,7 @@ impl<A: Action> ActionServerGoalHandle<A> {
     pub(super) fn get_status(&self) -> GoalStatusCode {
         let mut state = GoalStatusCode::Unknown as rcl_action_goal_state_t;
         {
-            let rcl_handle = self.rcl_handle.lock().unwrap();
+            let rcl_handle = self.lock();
             // SAFETY: The provided goal handle is properly initialized by construction.
             let r = unsafe { rcl_action_goal_handle_get_status(&*rcl_handle, &mut state).ok() };
             if let Err(err) = r {
diff --git a/rclrs/src/action/action_server/cancellation_state.rs b/rclrs/src/action/action_server/cancellation_state.rs
index 8da24742..bde70aa1 100644
--- a/rclrs/src/action/action_server/cancellation_state.rs
+++ b/rclrs/src/action/action_server/cancellation_state.rs
@@ -18,6 +18,7 @@ use std::{
     future::Future,
 };
 use futures::{future::{select, Either}, pin_mut};
+use futures_lite::future::race;
 use rosidl_runtime_rs::{Action, Message};
 use tokio::sync::watch::{Sender, Receiver, channel as watch_channel};
 
@@ -44,6 +45,19 @@ impl<A: Action> CancellationState<A> {
         }
     }
 
+    pub(super) fn unless_cancel_requested<F: Future>(&self, f: F) -> impl Future<Output = Result<F::Output, ()>> {
+        let mut watcher = self.receiver.clone();
+        race(
+            async move {
+                Ok(f.await)
+            },
+            async move {
+                let _ = watcher.wait_for(|request_received| *request_received).await;
+                Err(())
+            }
+        )
+    }
+
     /// Check if a cancellation is currently being requested.
     pub(super) fn cancel_requested(&self) -> bool {
         *self.receiver.borrow()
diff --git a/rclrs/src/action/action_server/cancelling_goal.rs b/rclrs/src/action/action_server/cancelling_goal.rs
index 24d19370..351f0559 100644
--- a/rclrs/src/action/action_server/cancelling_goal.rs
+++ b/rclrs/src/action/action_server/cancelling_goal.rs
@@ -1,4 +1,4 @@
-use super::{LiveActionServerGoal, TerminatedGoal};
+use super::{LiveActionServerGoal, FeedbackPublisher, TerminatedGoal};
 use std::sync::Arc;
 use rosidl_runtime_rs::Action;
 
@@ -25,7 +25,7 @@ impl<A: Action> CancellingGoal<A> {
     ///
     /// "Cancelled" is a terminal state, so the state of the goal can no longer
     /// be changed after this. Publish all relevant feedback before calling this.
-    pub fn cancelled_with(self, result: &A::Result) -> TerminatedGoal {
+    pub fn cancelled_with(self, result: A::Result) -> TerminatedGoal {
         self.live.transition_to_cancelled(result);
         TerminatedGoal { uuid: *self.live.goal_id() }
     }
@@ -34,7 +34,7 @@ impl<A: Action> CancellingGoal<A> {
     ///
     /// "Succeeded" is a terminal state, so the state of the goal can no longer
     /// be changed after this. Publish all relevant feedback before calling this.
-    pub fn succeeded_with(self, result: &A::Result) -> TerminatedGoal {
+    pub fn succeeded_with(self, result: A::Result) -> TerminatedGoal {
         self.live.transition_to_succeed(result);
         TerminatedGoal { uuid: *self.live.goal_id() }
     }
@@ -43,16 +43,30 @@ impl<A: Action> CancellingGoal<A> {
     ///
     /// "Aborted" is a terminal state, so the state of the goal can no longer
     /// be changed after this. Publish all relevant feedback before calling this.
-    pub fn aborted_with(self, result: &A::Result) -> TerminatedGoal {
+    pub fn aborted_with(self, result: A::Result) -> TerminatedGoal {
         self.live.transition_to_aborted(result);
         TerminatedGoal { uuid: *self.live.goal_id() }
     }
 
     /// Publish feedback for action clients to read.
-    pub fn publish_feedback(&self, feedback: &A::Feedback) {
+    ///
+    /// If you need to publish feedback from a separate thread or async task
+    /// which does not have direct access to the goal's state machine, you can
+    /// use [`Self::feedback_publisher`] to get a handle that you can pass along.
+    pub fn publish_feedback(&self, feedback: A::Feedback) {
         self.live.publish_feedback(feedback);
     }
 
+    /// Get a handle specifically for publishing feedback for this goal. This
+    /// publisher can be used separately from the overall state machine of the
+    /// goal, but it will stop working once the goal reaches a terminal state.
+    ///
+    /// If you just need to publish a one-off feedback message, you can use
+    /// [`Self::publish_feedback`].
+    pub fn feedback_publisher(&self) -> FeedbackPublisher<A> {
+        FeedbackPublisher::new(Arc::clone(&self.live))
+    }
+
     pub(super) fn new(live: Arc<LiveActionServerGoal<A>>) -> Self {
         live.transition_to_cancelling();
         Self { live }
diff --git a/rclrs/src/action/action_server/executing_goal.rs b/rclrs/src/action/action_server/executing_goal.rs
index 7a98ed2e..179111bf 100644
--- a/rclrs/src/action/action_server/executing_goal.rs
+++ b/rclrs/src/action/action_server/executing_goal.rs
@@ -1,4 +1,4 @@
-use super::{CancellingGoal, LiveActionServerGoal, TerminatedGoal};
+use super::{CancellingGoal, FeedbackPublisher, LiveActionServerGoal, TerminatedGoal};
 use std::{
     future::Future,
     sync::Arc,
@@ -26,7 +26,7 @@ impl<A: Action> ExecutingGoal<A> {
     ///
     /// "Succeeded" is a terminal state, so the state of the goal can no longer
     /// be changed after this. Publish all relevant feedback before calling this.
-    pub fn succeeded_with(self, result: &A::Result) -> TerminatedGoal {
+    pub fn succeeded_with(self, result: A::Result) -> TerminatedGoal {
         self.live.transition_to_succeed(result);
         TerminatedGoal { uuid: *self.live.goal_id() }
     }
@@ -34,10 +34,13 @@ impl<A: Action> ExecutingGoal<A> {
     /// Process a [`Future`] until it is finished or until a cancellation request
     /// is received.
     ///
-    /// If the [`Future`] finishes, its output will be provided in [`Ok`]. If a
-    /// cancellation request is received before the [`Future`] is finished, you
-    /// will receive an [`Err`] with the current state of the [`Future`], which
-    /// you can continue processing later if you choose.
+    /// If the [`Future`] finishes, its output will be provided in [`Ok`].
+    ///
+    /// If a cancellation request is received before the [`Future`] is finished,
+    /// you will receive an [`Err`] with the current state of the [`Future`],
+    /// which you can continue processing later if you choose. If you would rather
+    /// discard the [`Future`] when a cancellation request is received, you can
+    /// call [`Self::unless_cancel_requested`] instead.
     ///
     /// After the cancellation request is received, you will still need to trigger
     /// [`Self::begin_cancelling`] or [`Self::reject_cancellation`] to respond to
@@ -49,6 +52,27 @@ impl<A: Action> ExecutingGoal<A> {
         self.live.cancellation().until_cancel_requested(f).await
     }
 
+    /// Process a [`Future`] until it is finished unless a cancellation request
+    /// is received.
+    ///
+    /// If the [`Future`] finishes, its output will be provided in [`Ok`].
+    ///
+    /// If a cancellation request is received before the [`Future`] is finished,
+    /// the [`Future`] will be discarded. This allows non-[`Unpin`] futures to
+    /// be passed to this method. If your future implements [`Unpin`] and you want
+    /// the option to keep processing it after the cancellation request is received,
+    /// then you can call [`Self::until_cancel_requested`] instead.
+    ///
+    /// After the cancellation request is received, you will still need to trigger
+    /// [`Self::begin_cancelling`] or [`Self::reject_cancellation`] to respond to
+    /// the request. Otherwise the cancellation request will not receive a response
+    /// until the goal reaches a terminal state.
+    //
+    // TODO(@mxgrey): Add a doctest and example for this.
+    pub async fn unless_cancel_requested<F: Future>(&self, f: F) -> Result<F::Output, ()> {
+        self.live.cancellation().unless_cancel_requested(f).await
+    }
+
     /// Transition the goal into the cancelling state.
     ///
     /// This does not require an action client to request a cancellation. If no
@@ -78,16 +102,30 @@ impl<A: Action> ExecutingGoal<A> {
     ///
     /// "Aborted" is a terminal state, so the state of the goal can no longer
     /// be changed after this. Publish all relevant feedback before calling this.
-    pub fn aborted_with(self, result: &A::Result) -> TerminatedGoal {
+    pub fn aborted_with(self, result: A::Result) -> TerminatedGoal {
         self.live.transition_to_aborted(result);
         TerminatedGoal { uuid: *self.live.goal_id() }
     }
 
     /// Publish feedback for action clients to read.
-    pub fn publish_feedback(&self, feedback: &A::Feedback) {
+    ///
+    /// If you need to publish feedback from a separate thread or async task
+    /// which does not have direct access to the goal's state machine, you can
+    /// use [`Self::feedback_publisher`] to get a handle that you can pass along.
+    pub fn publish_feedback(&self, feedback: A::Feedback) {
         self.live.publish_feedback(feedback);
     }
 
+    /// Get a handle specifically for publishing feedback for this goal. This
+    /// publisher can be used separately from the overall state machine of the
+    /// goal, but it will stop working once the goal reaches a terminal state.
+    ///
+    /// If you just need to publish a one-off feedback message, you can use
+    /// [`Self::publish_feedback`].
+    pub fn feedback_publisher(&self) -> FeedbackPublisher<A> {
+        FeedbackPublisher::new(Arc::clone(&self.live))
+    }
+
     pub(super) fn new(live: Arc<LiveActionServerGoal<A>>) -> Self {
         live.transition_to_executing();
         Self { live }
diff --git a/rclrs/src/action/action_server/feedback_publisher.rs b/rclrs/src/action/action_server/feedback_publisher.rs
new file mode 100644
index 00000000..ca714d17
--- /dev/null
+++ b/rclrs/src/action/action_server/feedback_publisher.rs
@@ -0,0 +1,22 @@
+use super::LiveActionServerGoal;
+use std::sync::Arc;
+use rosidl_runtime_rs::Action;
+
+/// Use this to send feedback from an action server. This struct can be cloned
+/// and sent to other threads without the main goal handle. It can continue to
+/// send feedback until the goal has reached a terminal state.
+#[derive(Clone)]
+pub struct FeedbackPublisher<A: Action> {
+    live: Arc<LiveActionServerGoal<A>>,
+}
+
+impl<A: Action> FeedbackPublisher<A> {
+    /// Publish a feedback message, unless the goal has reached a terminal state.
+    pub fn publish(&self, feedback: A::Feedback) -> Result<(), A::Feedback> {
+        self.live.safe_publish_feedback(feedback)
+    }
+
+    pub(super) fn new(live: Arc<LiveActionServerGoal<A>>) -> Self {
+        Self { live }
+    }
+}
diff --git a/rclrs/src/action/action_server/live_action_server_goal.rs b/rclrs/src/action/action_server/live_action_server_goal.rs
index 74e311f5..32222b44 100644
--- a/rclrs/src/action/action_server/live_action_server_goal.rs
+++ b/rclrs/src/action/action_server/live_action_server_goal.rs
@@ -91,7 +91,7 @@ impl<A: Action> LiveActionServerGoal<A> {
     /// so no more methods may be called on a goal handle after this is called.
     ///
     /// Returns an error if the goal is in any state other than executing.
-    pub(super) fn transition_to_aborted(&self, result: &A::Result) {
+    pub(super) fn transition_to_aborted(&self, result: A::Result) {
         let r = self
             .update_state(rcl_action_goal_event_t::GOAL_EVENT_ABORT)
             .and_then(|_| self.terminate_goal(TerminalStatus::Aborted, result));
@@ -111,7 +111,7 @@ impl<A: Action> LiveActionServerGoal<A> {
     /// terminal state, so no more methods may be called on a goal handle after this is called.
     ///
     /// Returns an error if the goal is in any state other than executing.
-    pub(super) fn transition_to_succeed(&self, result: &A::Result) {
+    pub(super) fn transition_to_succeed(&self, result: A::Result) {
         let r = self
             .update_state(rcl_action_goal_event_t::GOAL_EVENT_SUCCEED)
             .and_then(|_| self.terminate_goal(TerminalStatus::Succeeded, result));
@@ -131,7 +131,7 @@ impl<A: Action> LiveActionServerGoal<A> {
     /// terminal state, so no more methods may be called on a goal handle after this is called.
     ///
     /// Returns an error if the goal is in any state other than executing or pending.
-    pub(super) fn transition_to_cancelled(&self, result: &A::Result) {
+    pub(super) fn transition_to_cancelled(&self, result: A::Result) {
         let r = self
             .update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCELED)
             .and_then(|_| self.terminate_goal(TerminalStatus::Cancelled, result));
@@ -167,11 +167,10 @@ impl<A: Action> LiveActionServerGoal<A> {
 
     /// Send an update about the goal's progress.
     ///
-    /// This may only be called when the goal is executing.
-    ///
-    /// Returns an error if the goal is in any state other than executing.
-    pub(super) fn publish_feedback(&self, feedback: &A::Feedback) {
-        let feedback_rmw = <<A as Action>::Feedback as Message>::into_rmw_message(Cow::Borrowed(feedback));
+    /// This should only be called in between a goal being accepted and terminated,
+    /// but that is not enforced in this method.
+    pub(super) fn publish_feedback(&self, feedback: A::Feedback) {
+        let feedback_rmw = <<A as Action>::Feedback as Message>::into_rmw_message(Cow::Owned(feedback));
         let mut feedback_msg = <A as Action>::create_feedback_message(&*self.goal_id(), feedback_rmw.into_owned());
         let r = unsafe {
             // SAFETY: The action server is locked through the handle, meaning that no other
@@ -195,12 +194,43 @@ impl<A: Action> LiveActionServerGoal<A> {
         }
     }
 
+    /// Atomically check the state of the goal and send feedback if the state
+    /// has not terminated.
+    ///
+    /// This is used by [`crate::FeedbackSender`] to ensure that it does not
+    /// send feedback in an unacceptable state, even if the state of the goal
+    /// may change while it tries to send the feedback.
+    pub(super) fn safe_publish_feedback(&self, feedback: A::Feedback) -> Result<(), A::Feedback> {
+        let goal_handle = self.handle.lock();
+
+        let mut state = GoalStatusCode::Unknown as rcl_action_goal_state_t;
+        let r = unsafe {
+            // SAFETY: The goal handle is properly initialized and its mutex is locked
+            rcl_action_goal_handle_get_status(&*goal_handle, &mut state).ok()
+        };
+        if let Err(err) = r {
+            log_error!(
+                "LiveActionServerGoal.safe_publish_feedback",
+                "Unexpected error while getting status: {err}",
+            );
+            return Err(feedback);
+        }
+
+        // The goal's rcl_handle mutex is locked and will prevent the goal status from
+        // being changed until this function is finished.
+        //
+        // The publish_feedback method does not need to lock the goal's rcl_handle
+        // so there is no double-lock to worry about.
+        self.publish_feedback(feedback);
+        Ok(())
+    }
+
     fn terminate_goal(
         &self,
         status: TerminalStatus,
-        result: &A::Result,
+        result: A::Result,
     ) -> Result<(), RclrsError> {
-        let result_rmw = <A::Result as Message>::into_rmw_message(Cow::Borrowed(result)).into_owned();
+        let result_rmw = <A::Result as Message>::into_rmw_message(Cow::Owned(result)).into_owned();
         let response_rmw = <A as Action>::create_result_response(status as i8, result_rmw);
         self.handle.provide_result(self.server.as_ref(), response_rmw)?;
 
@@ -240,10 +270,10 @@ impl<A: Action> Drop for LiveActionServerGoal<A> {
                 // Transition into executing and then into aborted to reach a
                 // terminal state.
                 self.transition_to_executing();
-                self.transition_to_aborted(&Default::default());
+                self.transition_to_aborted(Default::default());
             }
             GoalStatusCode::Cancelling | GoalStatusCode::Executing => {
-                self.transition_to_aborted(&Default::default());
+                self.transition_to_aborted(Default::default());
             }
             GoalStatusCode::Succeeded | GoalStatusCode::Cancelled | GoalStatusCode::Aborted => {
                 // Already in a terminal state, no need to do anything.

From 7451d202329129d3f9481343799bbbb49db6f2c4 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Mon, 11 Aug 2025 00:57:53 +0800
Subject: [PATCH 20/20] Add co-author credit

Co-authored-by: Nathan Wiebe Neufeldt <wn.nathan@gmail.com>
Signed-off-by: Michael X. Grey <greyxmike@gmail.com>