diff --git a/examples/minimal_action_client/Cargo.lock b/examples/minimal_action_client/Cargo.lock new file mode 100644 index 00000000..22842582 --- /dev/null +++ b/examples/minimal_action_client/Cargo.lock @@ -0,0 +1,611 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "action_msgs" +version = "2.0.2" +dependencies = [ + "builtin_interfaces", + "rosidl_runtime_rs", + "service_msgs", + "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 = "2.0.2" +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.12.0" +dependencies = [ + "action_msgs", + "builtin_interfaces", + "rosidl_runtime_rs", + "service_msgs", + "unique_identifier_msgs", +] + +[[package]] +name = "examples_rclrs_minimal_action_client" +version = "0.4.1" +dependencies = [ + "anyhow", + "backtrace", + "example_interfaces", + "rclrs", + "rosidl_runtime_rs", +] + +[[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.33" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9dee91521343f4c5c6a63edd65e54f31f5c92fe8978c40a4282f8372194c6a7d" +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 = "service_msgs" +version = "2.0.2" +dependencies = [ + "builtin_interfaces", + "rosidl_runtime_rs", +] + +[[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 = "unique_identifier_msgs" +version = "2.5.0" +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 = "2.0.2" + +[[patch.unused]] +name = "rosgraph_msgs" +version = "2.0.2" + +[[patch.unused]] +name = "test_msgs" +version = "2.0.2" diff --git a/examples/minimal_action_client/Cargo.toml b/examples/minimal_action_client/Cargo.toml new file mode 100644 index 00000000..79e2bf9d --- /dev/null +++ b/examples/minimal_action_client/Cargo.toml @@ -0,0 +1,19 @@ +[package] +name = "examples_rclrs_minimal_action_client" +version = "0.4.1" +# This project is not military-sponsored, Jacob's employment contract just requires him to use this email address +authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] +edition = "2021" + +[[bin]] +name = "minimal_action_client" +path = "src/minimal_action_client.rs" + +[dependencies] +anyhow = {version = "1", features = ["backtrace"]} +example_interfaces = "*" +rclrs = "0.4" +rosidl_runtime_rs = "0.4" + +# This specific version is compatible with Rust 1.75 +backtrace = "=0.3.74" diff --git a/examples/minimal_action_client/package.xml b/examples/minimal_action_client/package.xml new file mode 100644 index 00000000..a6def0d6 --- /dev/null +++ b/examples/minimal_action_client/package.xml @@ -0,0 +1,26 @@ + + + + examples_rclrs_minimal_action_client + 0.4.1 + Minimal action client examples for rclrs. + Esteve Fernandez + Nikolai Morin + + Jacob Hassold + Apache License 2.0 + + rclrs + rosidl_runtime_rs + example_interfaces + + rclrs + rosidl_runtime_rs + example_interfaces + + + ament_cargo + + diff --git a/examples/minimal_action_client/src/minimal_action_client.rs b/examples/minimal_action_client/src/minimal_action_client.rs new file mode 100644 index 00000000..0b271701 --- /dev/null +++ b/examples/minimal_action_client/src/minimal_action_client.rs @@ -0,0 +1,16 @@ +use anyhow::{Error, Result}; +use rclrs::*; + +fn main() -> Result<(), Error> { + let mut executor = Context::default_from_env()?.create_basic_executor(); + + let node = executor.create_node("minimal_action_client")?; + + let _client = + node.create_action_client::("fibonacci")?; + + executor + .spin(SpinOptions::default()) + .first_error() + .map_err(|err| err.into()) +} diff --git a/examples/minimal_action_server/Cargo.lock b/examples/minimal_action_server/Cargo.lock new file mode 100644 index 00000000..29c495c1 --- /dev/null +++ b/examples/minimal_action_server/Cargo.lock @@ -0,0 +1,611 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "action_msgs" +version = "2.0.2" +dependencies = [ + "builtin_interfaces", + "rosidl_runtime_rs", + "service_msgs", + "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 = "2.0.2" +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.12.0" +dependencies = [ + "action_msgs", + "builtin_interfaces", + "rosidl_runtime_rs", + "service_msgs", + "unique_identifier_msgs", +] + +[[package]] +name = "examples_rclrs_minimal_action_server" +version = "0.4.1" +dependencies = [ + "anyhow", + "backtrace", + "example_interfaces", + "rclrs", + "rosidl_runtime_rs", +] + +[[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.33" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9dee91521343f4c5c6a63edd65e54f31f5c92fe8978c40a4282f8372194c6a7d" +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 = "service_msgs" +version = "2.0.2" +dependencies = [ + "builtin_interfaces", + "rosidl_runtime_rs", +] + +[[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 = "unique_identifier_msgs" +version = "2.5.0" +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 = "2.0.2" + +[[patch.unused]] +name = "rosgraph_msgs" +version = "2.0.2" + +[[patch.unused]] +name = "test_msgs" +version = "2.0.2" diff --git a/examples/minimal_action_server/Cargo.toml b/examples/minimal_action_server/Cargo.toml new file mode 100644 index 00000000..587607af --- /dev/null +++ b/examples/minimal_action_server/Cargo.toml @@ -0,0 +1,19 @@ +[package] +name = "examples_rclrs_minimal_action_server" +version = "0.4.1" +# This project is not military-sponsored, Jacob's employment contract just requires him to use this email address +authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] +edition = "2021" + +[[bin]] +name = "minimal_action_server" +path = "src/minimal_action_server.rs" + +[dependencies] +anyhow = {version = "1", features = ["backtrace"]} +example_interfaces = "*" +rclrs = "0.4" +rosidl_runtime_rs = "0.4" + +# This specific version is compatible with Rust 1.75 +backtrace = "=0.3.74" diff --git a/examples/minimal_action_server/package.xml b/examples/minimal_action_server/package.xml new file mode 100644 index 00000000..5e0a18b0 --- /dev/null +++ b/examples/minimal_action_server/package.xml @@ -0,0 +1,26 @@ + + + + examples_rclrs_minimal_action_server + 0.4.1 + Minimal action server examples for rclrs. + Esteve Fernandez + Nikolai Morin + + Jacob Hassold + Apache License 2.0 + + rclrs + rosidl_runtime_rs + example_interfaces + + rclrs + rosidl_runtime_rs + example_interfaces + + + ament_cargo + + diff --git a/examples/minimal_action_server/src/minimal_action_server.rs b/examples/minimal_action_server/src/minimal_action_server.rs new file mode 100644 index 00000000..3af90c3a --- /dev/null +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -0,0 +1,81 @@ +use anyhow::{Error, Result}; +use rclrs::*; +use std::{sync::Arc, thread}; + +type Fibonacci = example_interfaces::action::Fibonacci; +type GoalHandleFibonacci = rclrs::ServerGoalHandle; + +fn handle_goal( + _uuid: rclrs::GoalUuid, + goal: example_interfaces::action::Fibonacci_Goal, +) -> rclrs::GoalResponse { + println!("Received goal request with order {}", goal.order); + if goal.order > 9000 { + rclrs::GoalResponse::Reject + } else { + rclrs::GoalResponse::AcceptAndExecute + } +} + +fn handle_cancel(_goal_handle: Arc) -> rclrs::CancelResponse { + println!("Got request to cancel goal"); + rclrs::CancelResponse::Accept +} + +fn execute(goal_handle: Arc) { + println!("Executing goal"); + let mut feedback = example_interfaces::action::Fibonacci_Feedback { + sequence: [0, 1].to_vec(), + }; + + for i in 1..goal_handle.goal().order { + if goal_handle.is_canceling() { + let result = example_interfaces::action::Fibonacci_Result { + sequence: Vec::new(), + }; + + goal_handle.canceled(&result); + println!("Goal canceled"); + return; + } + + // Update sequence sequence + feedback + .sequence + .push(feedback.sequence[i as usize] + feedback.sequence[(i - 1) as usize]); + // Publish feedback + goal_handle.publish_feedback(&feedback); + println!("Publishing feedback"); + thread::sleep(std::time::Duration::from_millis(100)); + } + + let result = example_interfaces::action::Fibonacci_Result { + sequence: feedback.sequence, + }; + goal_handle.succeed(&result); + println!("Goal succeeded"); +} + +fn handle_accepted(goal_handle: Arc) { + thread::spawn(move || { + execute(goal_handle); + }); +} + +fn main() -> Result<(), Error> { + let mut executor = Context::default_from_env()?.create_basic_executor(); + + let node = executor.create_node("minimal_action_server")?; + + let _action_server = node.create_action_server::( + "fibonacci", + handle_goal, + handle_cancel, + handle_accepted, + ); + + executor + .spin(SpinOptions::default()) + .first_error() + .map_err(|err| err.into()) +} diff --git a/rclrs/Cargo.lock b/rclrs/Cargo.lock index 5fc5e7bd..dd90d950 100644 --- a/rclrs/Cargo.lock +++ b/rclrs/Cargo.lock @@ -4,10 +4,11 @@ version = 3 [[package]] name = "action_msgs" -version = "1.2.1" +version = "2.0.2" dependencies = [ "builtin_interfaces", "rosidl_runtime_rs", + "service_msgs", "unique_identifier_msgs", ] @@ -94,7 +95,7 @@ checksum = "1b8e56985ec62d17e9c1001dc89c88ecd7dc08e47eba5ec7c29c7b5eeecde967" [[package]] name = "builtin_interfaces" -version = "1.2.1" +version = "2.0.2" dependencies = [ "rosidl_runtime_rs", ] @@ -515,6 +516,14 @@ dependencies = [ "syn", ] +[[package]] +name = "service_msgs" +version = "2.0.2" +dependencies = [ + "builtin_interfaces", + "rosidl_runtime_rs", +] + [[package]] name = "shlex" version = "1.3.0" @@ -556,11 +565,12 @@ dependencies = [ [[package]] name = "test_msgs" -version = "1.2.1" +version = "2.0.2" dependencies = [ "action_msgs", "builtin_interfaces", "rosidl_runtime_rs", + "service_msgs", "unique_identifier_msgs", ] @@ -594,7 +604,7 @@ checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512" [[package]] name = "unique_identifier_msgs" -version = "2.2.1" +version = "2.5.0" dependencies = [ "rosidl_runtime_rs", ] @@ -775,8 +785,8 @@ dependencies = [ [[patch.unused]] name = "rcl_interfaces" -version = "1.2.1" +version = "2.0.2" [[patch.unused]] name = "rosgraph_msgs" -version = "1.2.1" +version = "2.0.2" 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"); diff --git a/rclrs/package.xml b/rclrs/package.xml index 4c3754f4..d8b0d30a 100644 --- a/rclrs/package.xml +++ b/rclrs/package.xml @@ -16,10 +16,11 @@ libclang-dev rosidl_runtime_rs rcl + rcl_action builtin_interfaces rcl_interfaces rosgraph_msgs - + test_msgs 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]: +pub struct ActionClientHandle { + rcl_action_client: Mutex, + node_handle: Arc, + pub(crate) in_use_by_wait_set: Arc, +} + +impl ActionClientHandle { + pub(crate) fn lock(&self) -> MutexGuard { + 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`] 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 = Arc>; + +/// 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 +where + ActionT: rosidl_runtime_rs::Action, +{ + _marker: PhantomData ActionT>, + pub(crate) handle: Arc, + 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 ActionClientState +where + T: rosidl_runtime_rs::Action, +{ + /// Creates a new action client. + pub(crate) fn new<'a>( + node: &Node, + options: impl Into>, + ) -> Result + 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 ActionClientBase for ActionClientState +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 + ?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]: +pub struct ActionServerHandle { + rcl_action_server: Mutex, + node_handle: Arc, + pub(crate) in_use_by_wait_set: Arc, +} + +impl ActionServerHandle { + pub(crate) fn lock(&self) -> MutexGuard { + 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`] 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, mode: ReadyMode) -> Result<(), RclrsError>; +} + +pub(crate) enum ReadyMode { + GoalRequest, + CancelRequest, + ResultRequest, + GoalExpired, +} + +pub type GoalCallback = dyn Fn(GoalUuid, ::Goal) -> GoalResponse + 'static + Send + Sync; +pub type CancelCallback = dyn Fn(Arc>) -> CancelResponse + 'static + Send + Sync; +pub type AcceptedCallback = dyn Fn(Arc>) + '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 = Arc>; + +/// 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 +where + ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, +{ + pub(crate) handle: Arc, + num_entities: WaitableNumEntities, + goal_callback: Box>, + cancel_callback: Box>, + accepted_callback: Box>, + // 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>>>, + goal_results: Mutex::Response as Message>::RmwMsg>>, + result_requests: Mutex>>, + /// 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 ActionServerState +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>, + goal_callback: impl Fn(GoalUuid, T::Goal) -> GoalResponse + 'static + Send + Sync, + cancel_callback: impl Fn(Arc>) -> CancelResponse + 'static + Send + Sync, + accepted_callback: impl Fn(Arc>) + 'static + Send + Sync, + ) -> Result + 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<(<::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 = <<::SendGoalService as Service>::Request as Message>::RmwMsg; + let mut request_rmw = RmwRequest::::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 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 = ::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) -> 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(*::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::::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<(<::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 = <<::GetResultService as Service>::Request as Message>::RmwMsg; + let mut request_rmw = RmwRequest::::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 as *mut _, + ) + } + .ok()?; + + Ok((request_rmw, request_id)) + } + + fn send_result_response( + &self, + mut request_id: rmw_request_id_t, + response_rmw: &mut <<::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(*::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 = ::RmwMsg::default(); + let mut response_rmw = ::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: ::RmwMsg) -> Result<(), RclrsError> { + let response_rmw = ::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: &::Feedback) -> Result<(), RclrsError> { + let feedback_rmw = <::Feedback as Message>::into_rmw_message(std::borrow::Cow::Borrowed(feedback)); + let mut feedback_msg = ::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: <<::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 ActionServerBase for ActionServerState +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, 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 + ?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 +where + ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, +{ + rcl_handle: Mutex<*mut rcl_action_goal_handle_t>, + action_server: Weak>, + goal_request: Arc, + 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 Send for ServerGoalHandle where ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl {} +unsafe impl Sync for ServerGoalHandle where ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl {} + +impl ServerGoalHandle +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>, + goal_request: Arc, + 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 { + 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 = ::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 = ::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 = ::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 { + 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 { + 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 Drop for ServerGoalHandle +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 = ::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/clock.rs b/rclrs/src/clock.rs index f7c085e1..ae7fb058 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -88,6 +88,11 @@ impl Clock { self.kind } + /// Returns the clock's `rcl_clock_t`. + pub(crate) fn rcl_clock(&self) -> Arc> { + Arc::clone(&self.rcl_clock) + } + /// Returns the current clock's timestamp. pub fn now(&self) -> Time { let mut clock = self.rcl_clock.lock().unwrap(); 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 { + value: ManuallyDrop, + drop_fn: F, +} + +impl DropGuard { + /// 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 Deref for DropGuard { + type Target = T; + + fn deref(&self) -> &T { + &*self.value + } +} + +impl DerefMut for DropGuard { + fn deref_mut(&mut self) -> &mut T { + &mut *self.value + } +} + +impl Drop for DropGuard { + 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 527a4d3a..c5372c29 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -201,6 +201,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, @@ -249,6 +269,15 @@ impl TryFrom 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 => { @@ -330,6 +359,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/executor.rs b/rclrs/src/executor.rs index f38eec5b..186e17e3 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -76,6 +76,14 @@ impl Executor { for ready_service in ready_entities.services { ready_service.execute()?; } + + for (ready_action_client, mode) in ready_entities.action_clients { + ready_action_client.execute(mode)?; + } + + for (ready_action_server, mode) in ready_entities.action_servers { + ready_action_server.execute(mode)?; + } } // Clear out any nodes that have been dropped. diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 73d47819..02cea577 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -5,10 +5,12 @@ //! //! [1]: https://github.com/ros2-rust/ros2_rust/blob/main/README.md +mod action; mod arguments; mod client; mod clock; mod context; +mod drop_guard; mod error; mod executor; mod logging; @@ -31,10 +33,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 b2b734de..c7ac5507 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -19,11 +19,14 @@ use std::{ use rosidl_runtime_rs::Message; use crate::{ - rcl_bindings::*, Client, ClientBase, ClientOptions, ClientState, Clock, ContextHandle, + rcl_bindings::*, ActionClient, ActionClientBase, ActionClientOptions, ActionClientState, + ActionServer, ActionServerBase, ActionServerOptions, ActionServerState, CancelResponse, Client, + ClientBase, ClientOptions, ClientState, Clock, ContextHandle, GoalResponse, GoalUuid, GuardCondition, LogParams, Logger, ParameterBuilder, ParameterInterface, ParameterVariant, - Parameters, Publisher, PublisherOptions, PublisherState, RclrsError, Service, ServiceBase, - ServiceOptions, ServiceState, Subscription, SubscriptionBase, SubscriptionCallback, - SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams, ENTITY_LIFECYCLE_MUTEX, + Parameters, Publisher, PublisherOptions, PublisherState, RclrsError, ServerGoalHandle, Service, + ServiceBase, ServiceOptions, ServiceState, Subscription, SubscriptionBase, + SubscriptionCallback, SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams, + ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -87,6 +90,8 @@ pub struct NodeState { pub(crate) guard_conditions_mtx: Mutex>>, pub(crate) services_mtx: Mutex>>, pub(crate) subscriptions_mtx: Mutex>>, + pub(crate) action_servers_mtx: Mutex>>, + pub(crate) action_clients_mtx: Mutex>>, time_source: TimeSource, parameter: ParameterInterface, logger: Logger, @@ -281,10 +286,63 @@ impl NodeState { T: rosidl_runtime_rs::Service, { let client = Arc::new(ClientState::::new(self, options)?); - { self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); + self.clients_mtx + .lock() + .unwrap() + .push(Arc::downgrade(&client) as Weak); Ok(client) } + /// 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, + options: impl Into>, + ) -> Result, RclrsError> + where + T: rosidl_runtime_rs::Action, + { + let action_client = Arc::new(ActionClientState::::new(self, options)?); + self.action_clients_mtx + .lock() + .unwrap() + .push(Arc::downgrade(&action_client) as Weak); + 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, + options: impl Into>, + handle_goal: GoalCallback, + handle_cancel: CancelCallback, + handle_accepted: AcceptedCallback, + ) -> Result, RclrsError> + where + ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, + GoalCallback: Fn(GoalUuid, ::Goal) -> GoalResponse + 'static + Send + Sync, + CancelCallback: Fn(Arc>) -> CancelResponse + 'static + Send + Sync, + AcceptedCallback: Fn(Arc>) + 'static + Send + Sync, + { + let action_server = Arc::new(ActionServerState::::new( + self, + options, + handle_goal, + handle_cancel, + handle_accepted, + )?); + self.action_servers_mtx + .lock() + .unwrap() + .push(Arc::downgrade(&action_server) as Weak); + Ok(action_server) + } + /// Creates a [`GuardCondition`][1] with no callback. /// /// A weak pointer to the `GuardCondition` is stored within this node. @@ -298,7 +356,9 @@ impl NodeState { Arc::clone(&self.handle.context_handle), None, )); - { self.guard_conditions_mtx.lock().unwrap() } + self.guard_conditions_mtx + .lock() + .unwrap() .push(Arc::downgrade(&guard_condition) as Weak); guard_condition } @@ -319,7 +379,9 @@ impl NodeState { Arc::clone(&self.handle.context_handle), Some(Box::new(callback) as Box), )); - { self.guard_conditions_mtx.lock().unwrap() } + self.guard_conditions_mtx + .lock() + .unwrap() .push(Arc::downgrade(&guard_condition) as Weak); guard_condition } @@ -419,7 +481,9 @@ impl NodeState { F: Fn(&rmw_request_id_t, T::Request) -> T::Response + 'static + Send, { let service = Arc::new(ServiceState::::new(self, options, callback)?); - { self.services_mtx.lock().unwrap() } + self.services_mtx + .lock() + .unwrap() .push(Arc::downgrade(&service) as Weak); Ok(service) } @@ -474,7 +538,8 @@ impl NodeState { T: Message, { let subscription = Arc::new(SubscriptionState::::new(self, options, callback)?); - { self.subscriptions_mtx.lock() } + self.subscriptions_mtx + .lock() .unwrap() .push(Arc::downgrade(&subscription) as Weak); Ok(subscription) @@ -482,28 +547,54 @@ impl NodeState { /// Returns the subscriptions that have not been dropped yet. pub(crate) fn live_subscriptions(&self) -> Vec> { - { self.subscriptions_mtx.lock().unwrap() } + self.subscriptions_mtx + .lock() + .unwrap() .iter() .filter_map(Weak::upgrade) .collect() } pub(crate) fn live_clients(&self) -> Vec> { - { self.clients_mtx.lock().unwrap() } + self.clients_mtx + .lock() + .unwrap() .iter() .filter_map(Weak::upgrade) .collect() } pub(crate) fn live_guard_conditions(&self) -> Vec> { - { self.guard_conditions_mtx.lock().unwrap() } + self.guard_conditions_mtx + .lock() + .unwrap() .iter() .filter_map(Weak::upgrade) .collect() } pub(crate) fn live_services(&self) -> Vec> { - { self.services_mtx.lock().unwrap() } + self.services_mtx + .lock() + .unwrap() + .iter() + .filter_map(Weak::upgrade) + .collect() + } + + pub(crate) fn live_action_clients(&self) -> Vec> { + self.action_clients_mtx + .lock() + .unwrap() + .iter() + .filter_map(Weak::upgrade) + .collect() + } + + pub(crate) fn live_action_servers(&self) -> Vec> { + self.action_servers_mtx + .lock() + .unwrap() .iter() .filter_map(Weak::upgrade) .collect() diff --git a/rclrs/src/node/node_options.rs b/rclrs/src/node/node_options.rs index 4744a481..d7abf7ed 100644 --- a/rclrs/src/node/node_options.rs +++ b/rclrs/src/node/node_options.rs @@ -361,6 +361,8 @@ impl<'a> NodeOptions<'a> { guard_conditions_mtx: Mutex::default(), services_mtx: Mutex::default(), subscriptions_mtx: Mutex::default(), + action_clients_mtx: Mutex::default(), + action_servers_mtx: Mutex::default(), time_source: TimeSource::builder(self.clock_type) .clock_qos(self.clock_qos) .build(), 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 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 90f43400..497c1e12 100644 --- a/rclrs/src/rcl_bindings.rs +++ b/rclrs/src/rcl_bindings.rs @@ -138,6 +138,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; @@ -146,6 +147,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 #include +#include #include #include #include @@ -8,3 +9,4 @@ #include const size_t rmw_gid_storage_size_constant = RMW_GID_STORAGE_SIZE; +const size_t rcl_action_uuid_size_constant = UUID_SIZE; diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index c0e0c659..63715e00 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -18,9 +18,13 @@ use std::{sync::Arc, time::Duration, vec::Vec}; use crate::{ + action::{ + client::ReadyMode as ActionClientReadyMode, server::ReadyMode as ActionServerReadyMode, + }, error::{to_rclrs_result, RclReturnCode, RclrsError, ToResult}, rcl_bindings::*, - ClientBase, Context, ContextHandle, Node, ServiceBase, SubscriptionBase, + ActionClientBase, ActionServerBase, ClientBase, Context, ContextHandle, Node, ServiceBase, + SubscriptionBase, }; mod exclusivity_guard; @@ -50,6 +54,8 @@ pub struct WaitSet { // The guard conditions that are currently registered in the wait set. guard_conditions: Vec>>, services: Vec>>, + action_clients: Vec>>, + action_servers: Vec>>, handle: WaitSetHandle, } @@ -63,6 +69,10 @@ pub struct ReadyEntities { pub guard_conditions: Vec>, /// A list of services that have potentially received requests. pub services: Vec>, + /// A list of action clients and the ways in which they are ready. + pub action_clients: Vec<(Arc, ActionClientReadyMode)>, + /// A list of action servers and the ways in which they are ready. + pub action_servers: Vec<(Arc, ActionServerReadyMode)>, } impl Drop for rcl_wait_set_t { @@ -123,6 +133,8 @@ impl WaitSet { guard_conditions: Vec::new(), clients: Vec::new(), services: Vec::new(), + action_clients: Vec::new(), + action_servers: Vec::new(), handle: WaitSetHandle { rcl_wait_set, context_handle: Arc::clone(&context.handle), @@ -138,16 +150,41 @@ impl WaitSet { let live_clients = node.live_clients(); let live_guard_conditions = node.live_guard_conditions(); let live_services = node.live_services(); + let live_action_clients = node.live_action_clients(); + let live_action_servers = node.live_action_servers(); let ctx = Context { handle: Arc::clone(&node.handle.context_handle), }; + + let mut num_subscriptions = live_subscriptions.len(); + let mut num_guard_conditions = live_guard_conditions.len(); + let mut num_timers = 0; + let mut num_clients = live_clients.len(); + let mut num_services = live_services.len(); + let mut num_events = 0; + + let action_client_entities = live_action_clients + .iter() + .map(|client| client.num_entities()); + let action_server_entities = live_action_servers + .iter() + .map(|server| server.num_entities()); + for num_entities in action_client_entities.chain(action_server_entities) { + num_subscriptions += num_entities.num_subscriptions; + num_timers += num_entities.num_timers; + num_guard_conditions += num_entities.num_guard_conditions; + num_clients += num_entities.num_clients; + num_services += num_entities.num_services; + num_events += num_entities.num_events; + } + let mut wait_set = WaitSet::new( - live_subscriptions.len(), - live_guard_conditions.len(), - 0, - live_clients.len(), - live_services.len(), - 0, + num_subscriptions, + num_guard_conditions, + num_timers, + num_clients, + num_services, + num_events, &ctx, )?; @@ -166,6 +203,15 @@ impl WaitSet { for live_service in &live_services { wait_set.add_service(live_service.clone())?; } + + for live_action_client in &live_action_clients { + wait_set.add_action_client(live_action_client.clone())?; + } + + for live_action_server in &live_action_servers { + wait_set.add_action_server(live_action_server.clone())?; + } + Ok(wait_set) } @@ -178,6 +224,8 @@ impl WaitSet { self.guard_conditions.clear(); self.clients.clear(); self.services.clear(); + self.action_clients.clear(); + self.action_servers.clear(); // This cannot fail – the rcl_wait_set_clear function only checks that the input handle is // valid, which it always is in our case. Hence, only debug_assert instead of returning // Result. @@ -311,6 +359,73 @@ impl WaitSet { Ok(()) } + /// Adds an action client to the wait set. + /// + /// # Errors + /// - If the action client was already added to this wait set or another one, + /// [`AlreadyAddedToWaitSet`][1] will be returned + /// - If the number of entities in the wait set would be larger than the + /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned + /// + /// [1]: crate::RclrsError + /// [2]: crate::RclReturnCode + pub fn add_action_client( + &mut self, + action_client: Arc, + ) -> Result<(), RclrsError> { + let exclusive_client = ExclusivityGuard::new( + Arc::clone(&action_client), + Arc::clone(&action_client.handle().in_use_by_wait_set), + )?; + unsafe { + // SAFETY: I'm not sure if it's required, but the action client pointer will remain + // valid for as long as the wait set exists, because it's stored in self.action_clients. + // Passing in a null pointer for the third and fourth arguments is explicitly allowed. + rcl_action_wait_set_add_action_client( + &mut self.handle.rcl_wait_set, + &*action_client.handle().lock(), + core::ptr::null_mut(), + core::ptr::null_mut(), + ) + } + .ok()?; + self.action_clients.push(exclusive_client); + Ok(()) + } + + /// Adds an action server to the wait set. + /// + /// # Errors + /// - If the action server was already added to this wait set or another one, + /// [`AlreadyAddedToWaitSet`][1] will be returned + /// - If the number of entities in the wait set would be larger than the + /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned + /// + /// [1]: crate::RclrsError + /// [2]: crate::RclReturnCode + pub fn add_action_server( + &mut self, + action_server: Arc, + ) -> Result<(), RclrsError> { + let exclusive_server = ExclusivityGuard::new( + Arc::clone(&action_server), + Arc::clone(&action_server.handle().in_use_by_wait_set), + )?; + unsafe { + // SAFETY: I'm not sure if it's required, but the action server pointer will remain + // valid for as long as the wait set exists, because it's stored in self.action_servers. + // Passing in a null pointer for the third argument is explicitly allowed. + rcl_action_wait_set_add_action_server( + &mut self.handle.rcl_wait_set, + &*action_server.handle().lock(), + core::ptr::null_mut(), + ) + } + .ok()?; + self.action_servers.push(exclusive_server); + Ok(()) + } + /// Blocks until the wait set is ready, or until the timeout has been exceeded. /// /// If the timeout is `None` then this function will block indefinitely until @@ -347,8 +462,9 @@ impl WaitSet { }; // SAFETY: The comments in rcl mention "This function cannot operate on the same wait set // in multiple threads, and the wait sets may not share content." - // We cannot currently guarantee that the wait sets may not share content, but it is - // mentioned in the doc comment for `add_subscription`. + // By taking exclusive ownership of `self`, we can guarantee that the wait set is not in + // use from another thread. We guarantee that waits sets may not share content using + // `ExclusivityGuard`s on each entity added. // Also, the rcl_wait_set is obviously valid. match unsafe { rcl_wait(&mut self.handle.rcl_wait_set, timeout_ns) }.ok() { Ok(_) => (), @@ -365,6 +481,8 @@ impl WaitSet { clients: Vec::new(), guard_conditions: Vec::new(), services: Vec::new(), + action_clients: Vec::new(), + action_servers: Vec::new(), }; for (i, subscription) in self.subscriptions.iter().enumerate() { // SAFETY: The `subscriptions` entry is an array of pointers, and this dereferencing is @@ -409,10 +527,117 @@ impl WaitSet { ready_entities.services.push(Arc::clone(&service.waitable)); } } + + for action_client in &self.action_clients { + let mut is_feedback_ready = false; + let mut is_status_ready = false; + let mut is_goal_response_ready = false; + let mut is_cancel_response_ready = false; + let mut is_result_response_ready = false; + // SAFETY: The wait set is exclusively owned by this function, which guarantees thread + // safety. + unsafe { + rcl_action_client_wait_set_get_entities_ready( + &self.handle.rcl_wait_set, + &*action_client.waitable.handle().lock(), + &mut is_feedback_ready, + &mut is_status_ready, + &mut is_goal_response_ready, + &mut is_cancel_response_ready, + &mut is_result_response_ready, + ) + .ok()?; + } + if is_feedback_ready { + ready_entities.action_clients.push(( + Arc::clone(&action_client.waitable), + ActionClientReadyMode::Feedback, + )); + } + if is_status_ready { + ready_entities.action_clients.push(( + Arc::clone(&action_client.waitable), + ActionClientReadyMode::Status, + )); + } + if is_goal_response_ready { + ready_entities.action_clients.push(( + Arc::clone(&action_client.waitable), + ActionClientReadyMode::GoalResponse, + )); + } + if is_cancel_response_ready { + ready_entities.action_clients.push(( + Arc::clone(&action_client.waitable), + ActionClientReadyMode::CancelResponse, + )); + } + if is_result_response_ready { + ready_entities.action_clients.push(( + Arc::clone(&action_client.waitable), + ActionClientReadyMode::ResultResponse, + )); + } + } + + for action_server in &self.action_servers { + let mut is_goal_request_ready = false; + let mut is_cancel_request_ready = false; + let mut is_result_request_ready = false; + let mut is_goal_expired = false; + // SAFETY: The wait set is exclusively owned by this function, which guarantees thread + // safety. + unsafe { + rcl_action_server_wait_set_get_entities_ready( + &self.handle.rcl_wait_set, + &*action_server.waitable.handle().lock(), + &mut is_goal_request_ready, + &mut is_cancel_request_ready, + &mut is_result_request_ready, + &mut is_goal_expired, + ) + .ok()?; + } + if is_goal_request_ready { + ready_entities.action_servers.push(( + Arc::clone(&action_server.waitable), + ActionServerReadyMode::GoalRequest, + )); + } + if is_cancel_request_ready { + ready_entities.action_servers.push(( + Arc::clone(&action_server.waitable), + ActionServerReadyMode::CancelRequest, + )); + } + if is_result_request_ready { + ready_entities.action_servers.push(( + Arc::clone(&action_server.waitable), + ActionServerReadyMode::ResultRequest, + )); + } + if is_goal_expired { + ready_entities.action_servers.push(( + Arc::clone(&action_server.waitable), + ActionServerReadyMode::GoalExpired, + )); + } + } + Ok(ready_entities) } } +#[derive(Default)] +pub struct WaitableNumEntities { + pub(crate) num_subscriptions: usize, + pub(crate) num_guard_conditions: usize, + pub(crate) num_timers: usize, + pub(crate) num_clients: usize, + pub(crate) num_services: usize, + pub(crate) num_events: usize, +} + #[cfg(test)] mod tests { use super::*;