From 05464a8cd238207c10a57d236c15d496683f1473 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 28 Nov 2022 16:39:43 +0100 Subject: [PATCH 01/52] Added skeleton examples for action client and server --- .../minimal_action_client_server/Cargo.lock | 600 ++++++++++++++++++ .../minimal_action_client_server/Cargo.toml | 26 + .../minimal_action_client_server/package.xml | 26 + .../src/minimal_action_client.rs | 13 + .../src/minimal_action_server.rs | 13 + rclrs/Cargo.lock | 22 +- 6 files changed, 694 insertions(+), 6 deletions(-) create mode 100644 examples/minimal_action_client_server/Cargo.lock create mode 100644 examples/minimal_action_client_server/Cargo.toml create mode 100644 examples/minimal_action_client_server/package.xml create mode 100644 examples/minimal_action_client_server/src/minimal_action_client.rs create mode 100644 examples/minimal_action_client_server/src/minimal_action_server.rs diff --git a/examples/minimal_action_client_server/Cargo.lock b/examples/minimal_action_client_server/Cargo.lock new file mode 100644 index 000000000..effdc762b --- /dev/null +++ b/examples/minimal_action_client_server/Cargo.lock @@ -0,0 +1,600 @@ +# 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 = "examples_rclrs_minimal_action_client_server" +version = "0.4.1" +dependencies = [ + "action_msgs", + "anyhow", + "backtrace", + "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_server/Cargo.toml b/examples/minimal_action_client_server/Cargo.toml new file mode 100644 index 000000000..e810016ea --- /dev/null +++ b/examples/minimal_action_client_server/Cargo.toml @@ -0,0 +1,26 @@ +[package] +name = "examples_rclrs_minimal_action_client_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_client" +path = "src/minimal_action_client.rs" + +[[bin]] +name = "minimal_action_server" +path = "src/minimal_action_server.rs" + +[dependencies] +action_msgs = "*" +anyhow = {version = "1", features = ["backtrace"]} +rclrs = "0.4" +rosidl_runtime_rs = "0.4" + +# This specific version is compatible with Rust 1.75 +backtrace = "=0.3.74" + +[package.metadata.ros] +install_to_share = ["launch"] diff --git a/examples/minimal_action_client_server/package.xml b/examples/minimal_action_client_server/package.xml new file mode 100644 index 000000000..177aee989 --- /dev/null +++ b/examples/minimal_action_client_server/package.xml @@ -0,0 +1,26 @@ + + + + examples_rclrs_minimal_action_client_server + 0.3.1 + Package containing an example of actions in rclrs. + Esteve Fernandez + Nikolai Morin + + Jacob Hassold + Apache License 2.0 + + rclrs + rosidl_runtime_rs + action_msgs + + rclrs + rosidl_runtime_rs + action_msgs + + + ament_cargo + + diff --git a/examples/minimal_action_client_server/src/minimal_action_client.rs b/examples/minimal_action_client_server/src/minimal_action_client.rs new file mode 100644 index 000000000..fe0e07a90 --- /dev/null +++ b/examples/minimal_action_client_server/src/minimal_action_client.rs @@ -0,0 +1,13 @@ +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")?; + + executor + .spin(SpinOptions::default()) + .first_error() + .map_err(|err| err.into()) +} diff --git a/examples/minimal_action_client_server/src/minimal_action_server.rs b/examples/minimal_action_client_server/src/minimal_action_server.rs new file mode 100644 index 000000000..e94ae166d --- /dev/null +++ b/examples/minimal_action_client_server/src/minimal_action_server.rs @@ -0,0 +1,13 @@ +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_server")?; + + executor + .spin(SpinOptions::default()) + .first_error() + .map_err(|err| err.into()) +} diff --git a/rclrs/Cargo.lock b/rclrs/Cargo.lock index 5fc5e7bdc..dd90d950e 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" From 8137733040303805ef9fd4616f2e8fd30cf048c9 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 28 Nov 2022 17:18:07 +0100 Subject: [PATCH 02/52] Added action template --- examples/minimal_action_client_server/Cargo.toml | 3 --- 1 file changed, 3 deletions(-) diff --git a/examples/minimal_action_client_server/Cargo.toml b/examples/minimal_action_client_server/Cargo.toml index e810016ea..f096c9cb0 100644 --- a/examples/minimal_action_client_server/Cargo.toml +++ b/examples/minimal_action_client_server/Cargo.toml @@ -21,6 +21,3 @@ rosidl_runtime_rs = "0.4" # This specific version is compatible with Rust 1.75 backtrace = "=0.3.74" - -[package.metadata.ros] -install_to_share = ["launch"] From fb90bec3995164f18eef584f8624558426beacbc Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 28 Nov 2022 18:21:03 +0100 Subject: [PATCH 03/52] Added basic create_action_client function --- .../minimal_action_client_server/Cargo.lock | 13 ++++++++- .../minimal_action_client_server/Cargo.toml | 2 +- .../minimal_action_client_server/package.xml | 4 +-- .../src/minimal_action_client.rs | 4 ++- rclrs/src/action.rs | 27 ++++++++++++++++++ rclrs/src/lib.rs | 2 ++ rclrs/src/node.rs | 28 +++++++++++++++---- 7 files changed, 70 insertions(+), 10 deletions(-) create mode 100644 rclrs/src/action.rs diff --git a/examples/minimal_action_client_server/Cargo.lock b/examples/minimal_action_client_server/Cargo.lock index effdc762b..90dfa60d8 100644 --- a/examples/minimal_action_client_server/Cargo.lock +++ b/examples/minimal_action_client_server/Cargo.lock @@ -131,13 +131,24 @@ 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_server" version = "0.4.1" dependencies = [ - "action_msgs", "anyhow", "backtrace", + "example_interfaces", "rclrs", "rosidl_runtime_rs", ] diff --git a/examples/minimal_action_client_server/Cargo.toml b/examples/minimal_action_client_server/Cargo.toml index f096c9cb0..7a37d7e31 100644 --- a/examples/minimal_action_client_server/Cargo.toml +++ b/examples/minimal_action_client_server/Cargo.toml @@ -14,8 +14,8 @@ name = "minimal_action_server" path = "src/minimal_action_server.rs" [dependencies] -action_msgs = "*" anyhow = {version = "1", features = ["backtrace"]} +example_interfaces = "*" rclrs = "0.4" rosidl_runtime_rs = "0.4" diff --git a/examples/minimal_action_client_server/package.xml b/examples/minimal_action_client_server/package.xml index 177aee989..b410cc76e 100644 --- a/examples/minimal_action_client_server/package.xml +++ b/examples/minimal_action_client_server/package.xml @@ -14,11 +14,11 @@ rclrs rosidl_runtime_rs - action_msgs + example_interfaces rclrs rosidl_runtime_rs - action_msgs + example_interfaces ament_cargo diff --git a/examples/minimal_action_client_server/src/minimal_action_client.rs b/examples/minimal_action_client_server/src/minimal_action_client.rs index fe0e07a90..9d32cef1a 100644 --- a/examples/minimal_action_client_server/src/minimal_action_client.rs +++ b/examples/minimal_action_client_server/src/minimal_action_client.rs @@ -4,7 +4,9 @@ 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 mut node = executor.create_node("minimal_action_client")?; + + let _client = node.create_action_client::("fibonacci")?; executor .spin(SpinOptions::default()) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs new file mode 100644 index 000000000..445b31ca1 --- /dev/null +++ b/rclrs/src/action.rs @@ -0,0 +1,27 @@ +use crate::{rcl_bindings::*, Node, RclrsError}; + +use std::marker::PhantomData; + +pub struct ActionClient +where + T: rosidl_runtime_rs::Action, +{ + _marker: PhantomData, +} + +impl ActionClient +where + T: rosidl_runtime_rs::Action, +{ + /// Creates a new action client. + pub(crate) fn new(node: &Node, topic: &str) -> Result + // This uses pub(crate) visibility to avoid instantiating this struct outside + // [`Node::create_client`], see the struct's documentation for the rationale + where + T: rosidl_runtime_rs::Action, + { + Ok(Self { + _marker: Default::default(), + }) + } +} diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 73d478191..fb93d0ece 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -5,6 +5,7 @@ //! //! [1]: https://github.com/ros2-rust/ros2_rust/blob/main/README.md +mod action; mod arguments; mod client; mod clock; @@ -31,6 +32,7 @@ mod rcl_bindings; #[cfg(feature = "dyn_msg")] pub mod dynamic_message; +pub use action::*; pub use arguments::*; pub use client::*; pub use clock::*; diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index b2b734dee..48b2befe9 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -19,11 +19,12 @@ use std::{ use rosidl_runtime_rs::Message; use crate::{ - rcl_bindings::*, Client, ClientBase, ClientOptions, ClientState, Clock, ContextHandle, - 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, + rcl_bindings::*, ActionClient, Client, ClientBase, ClientOptions, ClientState, Clock, + ContextHandle, 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, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -285,6 +286,23 @@ impl NodeState { Ok(client) } + /// Creates a [`Client`][1]. + /// + /// [1]: crate::ActionClient + // TODO: make action client's lifetime depend on node's lifetime + pub fn create_action_client( + self: &Arc, + topic: &str, + ) -> Result>, RclrsError> + where + T: rosidl_runtime_rs::Action, + { + let client = Arc::new(ActionClient::::new(self, topic)?); + // self.clients + // .push(Arc::downgrade(&client) as Weak); + Ok(client) + } + /// Creates a [`GuardCondition`][1] with no callback. /// /// A weak pointer to the `GuardCondition` is stored within this node. From b03c7a1a342a552adb13c1636ac5fb209b4f9daa Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 29 Nov 2022 17:22:33 +0100 Subject: [PATCH 04/52] Fix linter --- .../minimal_action_client_server/src/minimal_action_client.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/minimal_action_client_server/src/minimal_action_client.rs b/examples/minimal_action_client_server/src/minimal_action_client.rs index 9d32cef1a..731763aa1 100644 --- a/examples/minimal_action_client_server/src/minimal_action_client.rs +++ b/examples/minimal_action_client_server/src/minimal_action_client.rs @@ -6,7 +6,8 @@ fn main() -> Result<(), Error> { let mut node = executor.create_node("minimal_action_client")?; - let _client = node.create_action_client::("fibonacci")?; + let _client = + node.create_action_client::("fibonacci")?; executor .spin(SpinOptions::default()) From b1957d439cb04dbaede17f7d97f6ac5514110889 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 17 Jul 2023 10:08:00 +0200 Subject: [PATCH 05/52] Split action client and server examples --- .../Cargo.lock | 2 +- .../Cargo.toml | 6 +- .../package.xml | 6 +- .../src/minimal_action_client.rs | 2 +- examples/minimal_action_server/Cargo.lock | 611 ++++++++++++++++++ examples/minimal_action_server/Cargo.toml | 19 + examples/minimal_action_server/package.xml | 26 + .../src/minimal_action_server.rs | 0 8 files changed, 662 insertions(+), 10 deletions(-) rename examples/{minimal_action_client_server => minimal_action_client}/Cargo.lock (99%) rename examples/{minimal_action_client_server => minimal_action_client}/Cargo.toml (81%) rename examples/{minimal_action_client_server => minimal_action_client}/package.xml (84%) rename examples/{minimal_action_client_server => minimal_action_client}/src/minimal_action_client.rs (84%) create mode 100644 examples/minimal_action_server/Cargo.lock create mode 100644 examples/minimal_action_server/Cargo.toml create mode 100644 examples/minimal_action_server/package.xml rename examples/{minimal_action_client_server => minimal_action_server}/src/minimal_action_server.rs (100%) diff --git a/examples/minimal_action_client_server/Cargo.lock b/examples/minimal_action_client/Cargo.lock similarity index 99% rename from examples/minimal_action_client_server/Cargo.lock rename to examples/minimal_action_client/Cargo.lock index 90dfa60d8..228425824 100644 --- a/examples/minimal_action_client_server/Cargo.lock +++ b/examples/minimal_action_client/Cargo.lock @@ -143,7 +143,7 @@ dependencies = [ ] [[package]] -name = "examples_rclrs_minimal_action_client_server" +name = "examples_rclrs_minimal_action_client" version = "0.4.1" dependencies = [ "anyhow", diff --git a/examples/minimal_action_client_server/Cargo.toml b/examples/minimal_action_client/Cargo.toml similarity index 81% rename from examples/minimal_action_client_server/Cargo.toml rename to examples/minimal_action_client/Cargo.toml index 7a37d7e31..79e2bf9de 100644 --- a/examples/minimal_action_client_server/Cargo.toml +++ b/examples/minimal_action_client/Cargo.toml @@ -1,5 +1,5 @@ [package] -name = "examples_rclrs_minimal_action_client_server" +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 "] @@ -9,10 +9,6 @@ edition = "2021" name = "minimal_action_client" path = "src/minimal_action_client.rs" -[[bin]] -name = "minimal_action_server" -path = "src/minimal_action_server.rs" - [dependencies] anyhow = {version = "1", features = ["backtrace"]} example_interfaces = "*" diff --git a/examples/minimal_action_client_server/package.xml b/examples/minimal_action_client/package.xml similarity index 84% rename from examples/minimal_action_client_server/package.xml rename to examples/minimal_action_client/package.xml index b410cc76e..a6def0d69 100644 --- a/examples/minimal_action_client_server/package.xml +++ b/examples/minimal_action_client/package.xml @@ -3,9 +3,9 @@ href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> - examples_rclrs_minimal_action_client_server - 0.3.1 - Package containing an example of actions in rclrs. + examples_rclrs_minimal_action_client + 0.4.1 + Minimal action client examples for rclrs. Esteve Fernandez Nikolai Morin diff --git a/examples/minimal_action_client_server/src/minimal_action_client.rs b/examples/minimal_action_client/src/minimal_action_client.rs similarity index 84% rename from examples/minimal_action_client_server/src/minimal_action_client.rs rename to examples/minimal_action_client/src/minimal_action_client.rs index 731763aa1..0b2717018 100644 --- a/examples/minimal_action_client_server/src/minimal_action_client.rs +++ b/examples/minimal_action_client/src/minimal_action_client.rs @@ -4,7 +4,7 @@ use rclrs::*; fn main() -> Result<(), Error> { let mut executor = Context::default_from_env()?.create_basic_executor(); - let mut node = executor.create_node("minimal_action_client")?; + let node = executor.create_node("minimal_action_client")?; let _client = node.create_action_client::("fibonacci")?; diff --git a/examples/minimal_action_server/Cargo.lock b/examples/minimal_action_server/Cargo.lock new file mode 100644 index 000000000..29c495c1b --- /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 000000000..587607afb --- /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 000000000..5e0a18b09 --- /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_client_server/src/minimal_action_server.rs b/examples/minimal_action_server/src/minimal_action_server.rs similarity index 100% rename from examples/minimal_action_client_server/src/minimal_action_server.rs rename to examples/minimal_action_server/src/minimal_action_server.rs From 28ece7a1cea38606e1b05db51514351c47b9c21f Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 17 Jul 2023 13:55:08 +0200 Subject: [PATCH 06/52] checkin --- .../src/minimal_action_server.rs | 42 ++++++++++- rclrs/src/action.rs | 71 ++++++++++++++++++- rclrs/src/lib.rs | 2 + rclrs/src/node.rs | 41 ++++++++--- rclrs/src/rcl_bindings.rs | 2 + rclrs/src/rcl_wrapper.h | 3 + 6 files changed, 149 insertions(+), 12 deletions(-) diff --git a/examples/minimal_action_server/src/minimal_action_server.rs b/examples/minimal_action_server/src/minimal_action_server.rs index e94ae166d..04022f147 100644 --- a/examples/minimal_action_server/src/minimal_action_server.rs +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -1,10 +1,50 @@ use anyhow::{Error, Result}; use rclrs::*; +use std::sync::Arc; +use std::thread; + +type Fibonacci = example_interfaces::action::Fibonacci; +type GoalHandleFibonacci = rclrs::ServerGoalHandle; + +fn handle_goal( + _uuid: &rclrs::GoalUUID, + goal: Arc, +) -> 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"); + thread::sleep(std::time::Duration::from_millis(100)); +} + +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 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()) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index 445b31ca1..906f90fdf 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -1,7 +1,27 @@ use crate::{rcl_bindings::*, Node, RclrsError}; +use std::sync::Arc; + +// 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_goal_handle_t {} + +unsafe impl Sync for rcl_action_goal_handle_t {} use std::marker::PhantomData; +pub type GoalUUID = [u8; RCL_ACTION_UUID_SIZE]; + +pub enum GoalResponse { + Reject = 1, + AcceptAndExecute = 2, + AcceptAndDefer = 3, +} + +pub enum CancelResponse { + Reject = 1, + Accept = 2, +} + pub struct ActionClient where T: rosidl_runtime_rs::Action, @@ -15,8 +35,6 @@ where { /// Creates a new action client. pub(crate) fn new(node: &Node, topic: &str) -> Result - // This uses pub(crate) visibility to avoid instantiating this struct outside - // [`Node::create_client`], see the struct's documentation for the rationale where T: rosidl_runtime_rs::Action, { @@ -25,3 +43,52 @@ where }) } } + +pub struct ActionServer +where + T: rosidl_runtime_rs::Action, +{ + _marker: PhantomData, +} + +impl ActionServer +where + T: rosidl_runtime_rs::Action, +{ + /// Creates a new action server. + pub(crate) fn new(node: &Node, topic: &str) -> Result + where + T: rosidl_runtime_rs::Action, + { + Ok(Self { + _marker: Default::default(), + }) + } +} + +pub struct ServerGoalHandle +where + T: rosidl_runtime_rs::Action, +{ + rcl_handle: Arc, + _marker: PhantomData, +} + +impl ServerGoalHandle +where + T: rosidl_runtime_rs::Action, +{ + pub(crate) fn new(rcl_handle: Arc) {} + + pub(crate) fn is_canceling() -> bool { + false + } + + pub(crate) fn is_active() -> bool { + false + } + + pub(crate) fn is_executing() -> bool { + false + } +} diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index fb93d0ece..2cf2c4a49 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -44,6 +44,8 @@ pub use node::*; pub use parameter::*; pub use publisher::*; pub use qos::*; +use rcl_bindings::rcl_action_goal_handle_t; +use rcl_bindings::rcl_context_is_valid; pub use rcl_bindings::rmw_request_id_t; pub use service::*; pub use subscription::*; diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 48b2befe9..ca1c2f9d2 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -19,12 +19,12 @@ use std::{ use rosidl_runtime_rs::Message; use crate::{ - rcl_bindings::*, ActionClient, Client, ClientBase, ClientOptions, ClientState, Clock, - ContextHandle, 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, + rcl_bindings::*, ActionClient, ActionServer, CancelResponse, Client, ClientBase, ClientOptions, + ClientState, Clock, ContextHandle, GoalResponse, GoalUUID, GuardCondition, LogParams, Logger, + ParameterBuilder, ParameterInterface, ParameterVariant, 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 @@ -286,7 +286,7 @@ impl NodeState { Ok(client) } - /// Creates a [`Client`][1]. + /// Creates an [`ActionClient`][1]. /// /// [1]: crate::ActionClient // TODO: make action client's lifetime depend on node's lifetime @@ -297,10 +297,33 @@ impl NodeState { where T: rosidl_runtime_rs::Action, { - let client = Arc::new(ActionClient::::new(self, topic)?); + let action_client = Arc::new(ActionClient::::new(self, topic)?); // self.clients // .push(Arc::downgrade(&client) as Weak); - Ok(client) + 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( + self: &Arc, + topic: &str, + handle_goal: fn( + &crate::action::GoalUUID, + Arc<::RmwMsg>, + ) -> GoalResponse, + handle_cancel: fn(Arc>) -> CancelResponse, + handle_accepted: fn(Arc>), + ) -> Result>, RclrsError> + where + T: rosidl_runtime_rs::Action, + { + let action_server = Arc::new(ActionServer::::new(self, topic)?); + // self.servers + // .push(Arc::downgrade(&server) as Weak); + Ok(action_server) } /// Creates a [`GuardCondition`][1] with no callback. diff --git a/rclrs/src/rcl_bindings.rs b/rclrs/src/rcl_bindings.rs index 90f434009..a9f250503 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 = 24; 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 ececf491f..e563020a7 100644 --- a/rclrs/src/rcl_wrapper.h +++ b/rclrs/src/rcl_wrapper.h @@ -1,5 +1,7 @@ #include #include +#include +#include #include #include #include @@ -8,3 +10,4 @@ #include const size_t rmw_gid_storage_size_constant = RMW_GID_STORAGE_SIZE; +const size_t rcl_action_uuid_size_constant = UUID_SIZE; \ No newline at end of file From 4d27f28235d1032f37f3161fdb3b2e935d0edcb1 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 17 Jul 2023 15:05:07 +0200 Subject: [PATCH 07/52] checkin --- .../src/minimal_action_server.rs | 31 ++++++++++++++++++- rclrs/src/action.rs | 6 ++-- 2 files changed, 33 insertions(+), 4 deletions(-) diff --git a/examples/minimal_action_server/src/minimal_action_server.rs b/examples/minimal_action_server/src/minimal_action_server.rs index 04022f147..a15825152 100644 --- a/examples/minimal_action_server/src/minimal_action_server.rs +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -25,7 +25,36 @@ fn handle_cancel(_goal_handle: Arc) -> rclrs::CancelRespons fn execute(goal_handle: Arc) { println!("Executing goal"); - thread::sleep(std::time::Duration::from_millis(100)); + let feedback = example_interfaces::action::Fibonacci_Feedback { + sequence: Vec::new(), + }; + feedback.sequence.push(0); + feedback.sequence.push(1); + let result = example_interfaces::action::Fibonacci_Result { + sequence: Vec::new(), + }; + + let mut i = 1; + while i < goal_handle.goal().unwrap().order && rclrs::ok() { + if goal_handle.is_canceling() { + result.sequence = feedback.sequence.clone(); + goal_handle.canceled(&result); + println!("Goal canceled"); + return; + } + // Update 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)); + } + // Check if goal is done + if rclrs::ok() { + result.sequence = feedback.sequence.clone(); + goal_handle.succeed(&result); + println!("Goal succeeded"); + } } fn handle_accepted(goal_handle: Arc) { diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index 906f90fdf..f63d50a72 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -80,15 +80,15 @@ where { pub(crate) fn new(rcl_handle: Arc) {} - pub(crate) fn is_canceling() -> bool { + pub(crate) fn is_canceling(&self) -> bool { false } - pub(crate) fn is_active() -> bool { + pub(crate) fn is_active(&self) -> bool { false } - pub(crate) fn is_executing() -> bool { + pub(crate) fn is_executing(&self) -> bool { false } } From e84ae6add697d58e51302971503ec92e4d8c742c Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 10 Aug 2023 11:52:39 +0200 Subject: [PATCH 08/52] checkin --- rclrs/src/lib.rs | 2 -- 1 file changed, 2 deletions(-) diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 2cf2c4a49..fb93d0ece 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -44,8 +44,6 @@ pub use node::*; pub use parameter::*; pub use publisher::*; pub use qos::*; -use rcl_bindings::rcl_action_goal_handle_t; -use rcl_bindings::rcl_context_is_valid; pub use rcl_bindings::rmw_request_id_t; pub use service::*; pub use subscription::*; From eb4026bc64f4b6930b430a22606c8780fd068a59 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 10 Aug 2023 12:29:15 +0200 Subject: [PATCH 09/52] fix visibility --- rclrs/src/action.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index f63d50a72..bff40543f 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -78,17 +78,17 @@ impl ServerGoalHandle where T: rosidl_runtime_rs::Action, { - pub(crate) fn new(rcl_handle: Arc) {} + pub fn new(rcl_handle: Arc) {} - pub(crate) fn is_canceling(&self) -> bool { + pub fn is_canceling(&self) -> bool { false } - pub(crate) fn is_active(&self) -> bool { + pub fn is_active(&self) -> bool { false } - pub(crate) fn is_executing(&self) -> bool { + pub fn is_executing(&self) -> bool { false } } From b308baca6f9f9a41f76f0b99c276dc18eb6f43fe Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 8 Nov 2023 10:10:13 +0100 Subject: [PATCH 10/52] Instantiate a new ActionClient --- rclrs/src/action.rs | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index bff40543f..272d7861c 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -78,7 +78,12 @@ impl ServerGoalHandle where T: rosidl_runtime_rs::Action, { - pub fn new(rcl_handle: Arc) {} + pub fn new(rcl_handle: Arc) -> Self { + Self { + rcl_handle, + _marker: Default::default(), + } + } pub fn is_canceling(&self) -> bool { false From e5dd17d6981563d1eacb5db63c4434d84dc6c396 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 17 Jul 2023 13:55:08 +0200 Subject: [PATCH 11/52] checkin --- rclrs/src/lib.rs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index fb93d0ece..ed39694be 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -44,6 +44,8 @@ pub use node::*; pub use parameter::*; pub use publisher::*; pub use qos::*; +use rcl_bindings::rcl_context_is_valid; +use rcl_bindings::rcl_action_goal_handle_t; pub use rcl_bindings::rmw_request_id_t; pub use service::*; pub use subscription::*; From 6d4163655f9fb2e9dea5e7c30a86b57a98e6e83c Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 17 Jul 2023 13:55:08 +0200 Subject: [PATCH 12/52] checkin --- .../minimal_action_server/src/minimal_action_server.rs | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/examples/minimal_action_server/src/minimal_action_server.rs b/examples/minimal_action_server/src/minimal_action_server.rs index a15825152..6daa5abc8 100644 --- a/examples/minimal_action_server/src/minimal_action_server.rs +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -75,6 +75,13 @@ fn main() -> Result<(), Error> { handle_accepted, ); + let _action_server = node.create_action_server::( + "fibonacci", + handle_goal, + handle_cancel, + handle_accepted, + ); + executor .spin(SpinOptions::default()) .first_error() From 8abbe131ab59ebd4a616fb7b2c0adc6c184ce59e Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 17 Nov 2023 15:34:07 +0100 Subject: [PATCH 13/52] checkin --- .../minimal_action_server/src/minimal_action_server.rs | 7 ------- 1 file changed, 7 deletions(-) diff --git a/examples/minimal_action_server/src/minimal_action_server.rs b/examples/minimal_action_server/src/minimal_action_server.rs index 6daa5abc8..a15825152 100644 --- a/examples/minimal_action_server/src/minimal_action_server.rs +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -75,13 +75,6 @@ fn main() -> Result<(), Error> { handle_accepted, ); - let _action_server = node.create_action_server::( - "fibonacci", - handle_goal, - handle_cancel, - handle_accepted, - ); - executor .spin(SpinOptions::default()) .first_error() From 5cb9610908c10a0e576c18340215095824a3988e Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 17 Nov 2023 17:20:05 +0100 Subject: [PATCH 14/52] checkin --- .../src/minimal_action_server.rs | 35 ++++++++++--------- rclrs/src/action.rs | 12 ++++++- 2 files changed, 29 insertions(+), 18 deletions(-) diff --git a/examples/minimal_action_server/src/minimal_action_server.rs b/examples/minimal_action_server/src/minimal_action_server.rs index a15825152..94c119c4f 100644 --- a/examples/minimal_action_server/src/minimal_action_server.rs +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -26,35 +26,36 @@ fn handle_cancel(_goal_handle: Arc) -> rclrs::CancelRespons fn execute(goal_handle: Arc) { println!("Executing goal"); let feedback = example_interfaces::action::Fibonacci_Feedback { - sequence: Vec::new(), - }; - feedback.sequence.push(0); - feedback.sequence.push(1); - let result = example_interfaces::action::Fibonacci_Result { - sequence: Vec::new(), + sequence: [0, 1].to_vec(), }; - let mut i = 1; - while i < goal_handle.goal().unwrap().order && rclrs::ok() { + for i in 1..goal_handle.goal_request.order { if goal_handle.is_canceling() { - result.sequence = feedback.sequence.clone(); + let result = example_interfaces::action::Fibonacci_Result { + sequence: Vec::new(), + }; + goal_handle.canceled(&result); println!("Goal canceled"); return; } - // Update sequence - feedback.sequence.push(feedback.sequence[i as usize] + feedback.sequence[(i - 1) as usize]); + + // 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)); } - // Check if goal is done - if rclrs::ok() { - result.sequence = feedback.sequence.clone(); - goal_handle.succeed(&result); - println!("Goal succeeded"); - } + + let result = example_interfaces::action::Fibonacci_Result { + sequence: Vec::new(), + }; + result.sequence = feedback.sequence.clone(); + goal_handle.succeed(&result); + println!("Goal succeeded"); } fn handle_accepted(goal_handle: Arc) { diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index 272d7861c..32da33f16 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -71,6 +71,7 @@ where T: rosidl_runtime_rs::Action, { rcl_handle: Arc, + goal_request: Arc, _marker: PhantomData, } @@ -78,9 +79,10 @@ impl ServerGoalHandle where T: rosidl_runtime_rs::Action, { - pub fn new(rcl_handle: Arc) -> Self { + pub fn new(rcl_handle: Arc, goal_request: Arc) -> Self { Self { rcl_handle, + goal_request: Arc::clone(&goal_request), _marker: Default::default(), } } @@ -96,4 +98,12 @@ where pub fn is_executing(&self) -> bool { false } + + pub fn succeed(&self, result: &T::Result) -> Result<(), RclrsError> { + Ok(()) + } + + pub fn canceled(&self, result: &T::Result) -> Result<(), RclrsError> { + Ok(()) + } } From 9d06c464ce67f728ed2328863117e30b415c531b Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 6 Jun 2024 00:01:48 -0400 Subject: [PATCH 15/52] Fix rclrs to compile after rebase Update uses of a `Mutex` with a `NodeHandle`, as was done elsewhere in the crate. Also, correct the documented UUID size to 16 rather than 24. The minimal_action_client compiles, but the minimal_action_server does not yet, complaining about expecting the `rmw` versions of messages rather than the idiomatic ones. --- rclrs/src/action.rs | 2 +- rclrs/src/lib.rs | 3 +-- rclrs/src/rcl_bindings.rs | 2 +- 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index 32da33f16..67b83f940 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -79,7 +79,7 @@ impl ServerGoalHandle where T: rosidl_runtime_rs::Action, { - pub fn new(rcl_handle: Arc, goal_request: Arc) -> Self { + pub fn new(rcl_handle: Arc, goal_request: Arc) -> Self { Self { rcl_handle, goal_request: Arc::clone(&goal_request), diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index ed39694be..e211c4d71 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -44,9 +44,8 @@ pub use node::*; pub use parameter::*; pub use publisher::*; pub use qos::*; -use rcl_bindings::rcl_context_is_valid; -use rcl_bindings::rcl_action_goal_handle_t; pub use rcl_bindings::rmw_request_id_t; +use rcl_bindings::{rcl_action_goal_handle_t, rcl_context_is_valid}; pub use service::*; pub use subscription::*; pub use time::*; diff --git a/rclrs/src/rcl_bindings.rs b/rclrs/src/rcl_bindings.rs index a9f250503..497c1e12c 100644 --- a/rclrs/src/rcl_bindings.rs +++ b/rclrs/src/rcl_bindings.rs @@ -138,7 +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 = 24; + pub const RCL_ACTION_UUID_SIZE: usize = 16; extern "C" { pub fn rcl_context_is_valid(context: *const rcl_context_t) -> bool; From d3222f879ddfe33cd622fae66bd6005cbb4db067 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 6 Jun 2024 15:33:38 -0400 Subject: [PATCH 16/52] Sketch out action server construction and destruction This follows generally the same pattern as the service server. It required adding a typesupport function to the Action trait and pulling in some more rcl_action bindings. --- rclrs/package.xml | 3 +- rclrs/src/action.rs | 93 ++++++++++++++++++++++++++++++++++++++++- rclrs/src/rcl_wrapper.h | 5 +-- 3 files changed, 95 insertions(+), 6 deletions(-) diff --git a/rclrs/package.xml b/rclrs/package.xml index 4c3754f48..d8b0d30ae 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 index 67b83f940..189ebc636 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -1,5 +1,8 @@ -use crate::{rcl_bindings::*, Node, RclrsError}; -use std::sync::Arc; +use crate::{error::ToResult, rcl_bindings::*, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX}; +use std::{ + 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. @@ -22,6 +25,46 @@ pub enum CancelResponse { Accept = 2, } +/// 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: Node, + 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; + // /// Tries to take a new request and run the callback with it. + // fn execute(&self) -> Result<(), RclrsError>; +} + pub struct ActionClient where T: rosidl_runtime_rs::Action, @@ -49,6 +92,10 @@ where T: rosidl_runtime_rs::Action, { _marker: PhantomData, + pub(crate) handle: Arc, + // goal_callback: (), + // cancel_callback: (), + // accepted_callback: (), } impl ActionServer @@ -60,8 +107,50 @@ where where T: rosidl_runtime_rs::Action, { + // SAFETY: Getting a zero-initialized value is always safe. + let mut rcl_action_server = unsafe { rcl_action_get_zero_initialized_server() }; + let type_support = ::get_type_support() + as *const rosidl_action_type_support_t; + let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { + err, + s: topic.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 _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + // SAFETY: + // * The rcl_action_server is zero-initialized as mandated by this function. + // * The rcl_node is kept alive by the NodeHandle it is a dependency of the action server. + // * The topic 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. + rcl_action_server_init( + &mut rcl_action_server, + &mut *rcl_node, + todo!(), + type_support, + topic_c_string.as_ptr(), + &action_server_options as *const _, + ) + .ok()?; + } + } + + let handle = Arc::new(ActionServerHandle { + rcl_action_server: Mutex::new(rcl_action_server), + node: node.clone(), + in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + }); + Ok(Self { _marker: Default::default(), + handle, }) } } diff --git a/rclrs/src/rcl_wrapper.h b/rclrs/src/rcl_wrapper.h index e563020a7..998f4022a 100644 --- a/rclrs/src/rcl_wrapper.h +++ b/rclrs/src/rcl_wrapper.h @@ -1,7 +1,6 @@ #include #include -#include -#include +#include #include #include #include @@ -10,4 +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; \ No newline at end of file +const size_t rcl_action_uuid_size_constant = UUID_SIZE; From e7476f9e2be7e64744caa7d8e018b3fcc94e12e4 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 6 Jun 2024 16:15:13 -0400 Subject: [PATCH 17/52] Sketch out action client as well --- rclrs/src/action.rs | 117 ++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 113 insertions(+), 4 deletions(-) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index 189ebc636..5f73f5180 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -25,6 +25,44 @@ pub enum CancelResponse { Accept = 2, } +// 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 {} + +// 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_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: Node, + 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); + } + } +} + /// 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`. @@ -55,6 +93,16 @@ impl Drop for ActionServerHandle { } } +/// 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; + // /// Tries to take a new request and run the callback with it. + // fn execute(&self) -> Result<(), RclrsError>; +} + /// Trait to be implemented by concrete ActionServer structs. /// /// See [`ActionServer`] for an example @@ -69,7 +117,8 @@ pub struct ActionClient where T: rosidl_runtime_rs::Action, { - _marker: PhantomData, + _marker: PhantomData T>, + pub(crate) handle: Arc, } impl ActionClient @@ -81,17 +130,68 @@ where where T: rosidl_runtime_rs::Action, { + // SAFETY: Getting a zero-initialized value is always safe. + let mut rcl_action_client = unsafe { rcl_action_get_zero_initialized_client() }; + let type_support = ::get_type_support() + as *const rosidl_action_type_support_t; + let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { + err, + s: topic.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_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 client. + // * The topic 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, + topic_c_string.as_ptr(), + &action_client_options, + ) + .ok()?; + } + } + + let handle = Arc::new(ActionClientHandle { + rcl_action_client: Mutex::new(rcl_action_client), + node: node.clone(), + in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + }); + Ok(Self { _marker: Default::default(), + handle, }) } } +impl ActionClientBase for ActionClient +where + T: rosidl_runtime_rs::Action, +{ + fn handle(&self) -> &ActionClientHandle { + &self.handle + } +} + pub struct ActionServer where T: rosidl_runtime_rs::Action, { - _marker: PhantomData, + _marker: PhantomData T>, pub(crate) handle: Arc, // goal_callback: (), // cancel_callback: (), @@ -133,10 +233,10 @@ where rcl_action_server_init( &mut rcl_action_server, &mut *rcl_node, - todo!(), + todo!("pass in a rcl_clock_t"), type_support, topic_c_string.as_ptr(), - &action_server_options as *const _, + &action_server_options, ) .ok()?; } @@ -155,6 +255,15 @@ where } } +impl ActionServerBase for ActionServer +where + T: rosidl_runtime_rs::Action, +{ + fn handle(&self) -> &ActionServerHandle { + &self.handle + } +} + pub struct ServerGoalHandle where T: rosidl_runtime_rs::Action, From 788dc52c672a0e663e99d8cbb6d958a78c893a9f Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 6 Jun 2024 16:37:04 -0400 Subject: [PATCH 18/52] Pass rcl_clock_t from Node to ActionServer This is needed to initialize the rcl_action_server_t. In rclcpp, the clock ends up stored in the Server itself (by way of ServerBase and ServerBaseImpl); we'll see if that ends up being necessary here. --- rclrs/src/action.rs | 8 +++++--- rclrs/src/clock.rs | 5 +++++ rclrs/src/node.rs | 2 +- 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index 5f73f5180..a86605a09 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -1,4 +1,4 @@ -use crate::{error::ToResult, rcl_bindings::*, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX}; +use crate::{error::ToResult, rcl_bindings::*, Clock, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX}; use std::{ ffi::CString, sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, @@ -203,7 +203,7 @@ where T: rosidl_runtime_rs::Action, { /// Creates a new action server. - pub(crate) fn new(node: &Node, topic: &str) -> Result + pub(crate) fn new(node: &Node, clock: Clock, topic: &str) -> Result where T: rosidl_runtime_rs::Action, { @@ -221,6 +221,8 @@ where { let mut rcl_node = node.handle.rcl_node.lock().unwrap(); + let rcl_clock = clock.rcl_clock(); + let mut rcl_clock = rcl_clock.lock().unwrap(); let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); unsafe { // SAFETY: @@ -233,7 +235,7 @@ where rcl_action_server_init( &mut rcl_action_server, &mut *rcl_node, - todo!("pass in a rcl_clock_t"), + &mut *rcl_clock, type_support, topic_c_string.as_ptr(), &action_server_options, diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs index f7c085e14..ae7fb0582 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/node.rs b/rclrs/src/node.rs index ca1c2f9d2..c4bae368e 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -320,7 +320,7 @@ impl NodeState { where T: rosidl_runtime_rs::Action, { - let action_server = Arc::new(ActionServer::::new(self, topic)?); + let action_server = Arc::new(ActionServer::::new(self, self.get_clock(), topic)?); // self.servers // .push(Arc::downgrade(&server) as Weak); Ok(action_server) From 2e0a1b889d3b67abbaa847447cc4db7434b4f04d Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 6 Jun 2024 16:57:29 -0400 Subject: [PATCH 19/52] Split action servers and clients into separate modules --- rclrs/src/action.rs | 256 ++----------------------------------- rclrs/src/action/client.rs | 124 ++++++++++++++++++ rclrs/src/action/server.rs | 130 +++++++++++++++++++ 3 files changed, 262 insertions(+), 248 deletions(-) create mode 100644 rclrs/src/action/client.rs create mode 100644 rclrs/src/action/server.rs diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index a86605a09..714ad41e7 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -1,8 +1,11 @@ -use crate::{error::ToResult, rcl_bindings::*, Clock, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX}; -use std::{ - ffi::CString, - sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, -}; +mod client; +mod server; + +use crate::{rcl_bindings::*, RclrsError}; +use std::{marker::PhantomData, sync::Arc}; + +pub use client::{ActionClient, ActionClientBase}; +pub use server::{ActionServer, ActionServerBase}; // 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. @@ -10,8 +13,6 @@ unsafe impl Send for rcl_action_goal_handle_t {} unsafe impl Sync for rcl_action_goal_handle_t {} -use std::marker::PhantomData; - pub type GoalUUID = [u8; RCL_ACTION_UUID_SIZE]; pub enum GoalResponse { @@ -25,247 +26,6 @@ pub enum CancelResponse { Accept = 2, } -// 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 {} - -// 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_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: Node, - 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); - } - } -} - -/// 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: Node, - 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 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; - // /// Tries to take a new request and run the callback with it. - // fn execute(&self) -> Result<(), RclrsError>; -} - -/// 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; - // /// Tries to take a new request and run the callback with it. - // fn execute(&self) -> Result<(), RclrsError>; -} - -pub struct ActionClient -where - T: rosidl_runtime_rs::Action, -{ - _marker: PhantomData T>, - pub(crate) handle: Arc, -} - -impl ActionClient -where - T: rosidl_runtime_rs::Action, -{ - /// Creates a new action client. - pub(crate) fn new(node: &Node, topic: &str) -> Result - where - T: rosidl_runtime_rs::Action, - { - // SAFETY: Getting a zero-initialized value is always safe. - let mut rcl_action_client = unsafe { rcl_action_get_zero_initialized_client() }; - let type_support = ::get_type_support() - as *const rosidl_action_type_support_t; - let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { - err, - s: topic.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_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 client. - // * The topic 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, - topic_c_string.as_ptr(), - &action_client_options, - ) - .ok()?; - } - } - - let handle = Arc::new(ActionClientHandle { - rcl_action_client: Mutex::new(rcl_action_client), - node: node.clone(), - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), - }); - - Ok(Self { - _marker: Default::default(), - handle, - }) - } -} - -impl ActionClientBase for ActionClient -where - T: rosidl_runtime_rs::Action, -{ - fn handle(&self) -> &ActionClientHandle { - &self.handle - } -} - -pub struct ActionServer -where - T: rosidl_runtime_rs::Action, -{ - _marker: PhantomData T>, - pub(crate) handle: Arc, - // goal_callback: (), - // cancel_callback: (), - // accepted_callback: (), -} - -impl ActionServer -where - T: rosidl_runtime_rs::Action, -{ - /// Creates a new action server. - pub(crate) fn new(node: &Node, clock: Clock, topic: &str) -> Result - where - T: rosidl_runtime_rs::Action, - { - // SAFETY: Getting a zero-initialized value is always safe. - let mut rcl_action_server = unsafe { rcl_action_get_zero_initialized_server() }; - let type_support = ::get_type_support() - as *const rosidl_action_type_support_t; - let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { - err, - s: topic.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 rcl_clock = clock.rcl_clock(); - let mut rcl_clock = rcl_clock.lock().unwrap(); - let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); - unsafe { - // SAFETY: - // * The rcl_action_server is zero-initialized as mandated by this function. - // * The rcl_node is kept alive by the NodeHandle it is a dependency of the action server. - // * The topic 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. - rcl_action_server_init( - &mut rcl_action_server, - &mut *rcl_node, - &mut *rcl_clock, - type_support, - topic_c_string.as_ptr(), - &action_server_options, - ) - .ok()?; - } - } - - let handle = Arc::new(ActionServerHandle { - rcl_action_server: Mutex::new(rcl_action_server), - node: node.clone(), - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), - }); - - Ok(Self { - _marker: Default::default(), - handle, - }) - } -} - -impl ActionServerBase for ActionServer -where - T: rosidl_runtime_rs::Action, -{ - fn handle(&self) -> &ActionServerHandle { - &self.handle - } -} - pub struct ServerGoalHandle where T: rosidl_runtime_rs::Action, diff --git a/rclrs/src/action/client.rs b/rclrs/src/action/client.rs new file mode 100644 index 000000000..9e8b4d5a9 --- /dev/null +++ b/rclrs/src/action/client.rs @@ -0,0 +1,124 @@ +use crate::{error::ToResult, rcl_bindings::*, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX}; +use std::{ + 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: Node, + 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; + // /// Tries to take a new request and run the callback with it. + // fn execute(&self) -> Result<(), RclrsError>; +} + +pub struct ActionClient +where + T: rosidl_runtime_rs::Action, +{ + _marker: PhantomData T>, + pub(crate) handle: Arc, +} + +impl ActionClient +where + T: rosidl_runtime_rs::Action, +{ + /// Creates a new action client. + pub(crate) fn new(node: &Node, topic: &str) -> Result + where + T: rosidl_runtime_rs::Action, + { + // SAFETY: Getting a zero-initialized value is always safe. + let mut rcl_action_client = unsafe { rcl_action_get_zero_initialized_client() }; + let type_support = ::get_type_support() + as *const rosidl_action_type_support_t; + let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { + err, + s: topic.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 Node because it is a dependency of the action client. + // * The topic 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, + topic_c_string.as_ptr(), + &action_client_options, + ) + .ok()?; + } + } + + let handle = Arc::new(ActionClientHandle { + rcl_action_client: Mutex::new(rcl_action_client), + node: node.clone(), + in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + }); + + Ok(Self { + _marker: Default::default(), + handle, + }) + } +} + +impl ActionClientBase for ActionClient +where + T: rosidl_runtime_rs::Action, +{ + fn handle(&self) -> &ActionClientHandle { + &self.handle + } +} diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs new file mode 100644 index 000000000..b92f8805e --- /dev/null +++ b/rclrs/src/action/server.rs @@ -0,0 +1,130 @@ +use crate::{error::ToResult, rcl_bindings::*, Clock, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX}; +use std::{ + 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_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: Node, + 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; + // /// Tries to take a new request and run the callback with it. + // fn execute(&self) -> Result<(), RclrsError>; +} + +pub struct ActionServer +where + T: rosidl_runtime_rs::Action, +{ + _marker: PhantomData T>, + pub(crate) handle: Arc, + // goal_callback: (), + // cancel_callback: (), + // accepted_callback: (), +} + +impl ActionServer +where + T: rosidl_runtime_rs::Action, +{ + /// Creates a new action server. + pub(crate) fn new(node: &Node, clock: Clock, topic: &str) -> Result + where + T: rosidl_runtime_rs::Action, + { + // SAFETY: Getting a zero-initialized value is always safe. + let mut rcl_action_server = unsafe { rcl_action_get_zero_initialized_server() }; + let type_support = ::get_type_support() + as *const rosidl_action_type_support_t; + let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { + err, + s: topic.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 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 Node because it is a dependency of the action server. + // * The topic 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, + topic_c_string.as_ptr(), + &action_server_options, + ) + .ok()?; + } + } + + let handle = Arc::new(ActionServerHandle { + rcl_action_server: Mutex::new(rcl_action_server), + node: node.clone(), + in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + }); + + Ok(Self { + _marker: Default::default(), + handle, + }) + } +} + +impl ActionServerBase for ActionServer +where + T: rosidl_runtime_rs::Action, +{ + fn handle(&self) -> &ActionServerHandle { + &self.handle + } +} From e4f572c3d9efb7dddf463269febd18d25e49d68a Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 6 Jun 2024 17:24:47 -0400 Subject: [PATCH 20/52] Move ServerGoalHandle to separate module --- rclrs/src/action.rs | 53 ++------------------------ rclrs/src/action/server_goal_handle.rs | 53 ++++++++++++++++++++++++++ 2 files changed, 56 insertions(+), 50 deletions(-) create mode 100644 rclrs/src/action/server_goal_handle.rs diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index 714ad41e7..b4fbe6516 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -1,17 +1,12 @@ mod client; mod server; +mod server_goal_handle; -use crate::{rcl_bindings::*, RclrsError}; -use std::{marker::PhantomData, sync::Arc}; +use crate::rcl_bindings::RCL_ACTION_UUID_SIZE; pub use client::{ActionClient, ActionClientBase}; pub use server::{ActionServer, ActionServerBase}; - -// 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_goal_handle_t {} - -unsafe impl Sync for rcl_action_goal_handle_t {} +pub use server_goal_handle::ServerGoalHandle; pub type GoalUUID = [u8; RCL_ACTION_UUID_SIZE]; @@ -25,45 +20,3 @@ pub enum CancelResponse { Reject = 1, Accept = 2, } - -pub struct ServerGoalHandle -where - T: rosidl_runtime_rs::Action, -{ - rcl_handle: Arc, - goal_request: Arc, - _marker: PhantomData, -} - -impl ServerGoalHandle -where - T: rosidl_runtime_rs::Action, -{ - pub fn new(rcl_handle: Arc, goal_request: Arc) -> Self { - Self { - rcl_handle, - goal_request: Arc::clone(&goal_request), - _marker: Default::default(), - } - } - - pub fn is_canceling(&self) -> bool { - false - } - - pub fn is_active(&self) -> bool { - false - } - - pub fn is_executing(&self) -> bool { - false - } - - pub fn succeed(&self, result: &T::Result) -> Result<(), RclrsError> { - Ok(()) - } - - pub fn canceled(&self, result: &T::Result) -> Result<(), RclrsError> { - Ok(()) - } -} diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs new file mode 100644 index 000000000..af075dbea --- /dev/null +++ b/rclrs/src/action/server_goal_handle.rs @@ -0,0 +1,53 @@ +use crate::{rcl_bindings::*, RclrsError}; +use std::{ + marker::PhantomData, + sync::{Arc, Mutex}, +}; + +// 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_goal_handle_t {} + +unsafe impl Sync for rcl_action_goal_handle_t {} + +pub struct ServerGoalHandle +where + T: rosidl_runtime_rs::Action, +{ + rcl_handle: Arc>, + goal_request: Arc, + _marker: PhantomData, +} + +impl ServerGoalHandle +where + T: rosidl_runtime_rs::Action, +{ + pub fn new(rcl_handle: Arc>, goal_request: Arc) -> Self { + Self { + rcl_handle, + goal_request: Arc::clone(&goal_request), + _marker: Default::default(), + } + } + + pub fn is_canceling(&self) -> bool { + false + } + + pub fn is_active(&self) -> bool { + false + } + + pub fn is_executing(&self) -> bool { + false + } + + pub fn succeed(&self, result: &T::Result) -> Result<(), RclrsError> { + Ok(()) + } + + pub fn canceled(&self, result: &T::Result) -> Result<(), RclrsError> { + Ok(()) + } +} From d7ece4bfc1dd28dcc324dea24e5e234a444a83ef Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 6 Jun 2024 18:49:19 -0400 Subject: [PATCH 21/52] Begin implementing ActionServerGoalHandle functions So far, none of the implemented functionality actually depends on the action type. It could be separated out into a concrete type like is done in rclcpp, in case that reduces the cost of monomorphization. --- rclrs/src/action/server_goal_handle.rs | 101 +++++++++++++++++++++++-- 1 file changed, 95 insertions(+), 6 deletions(-) diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs index af075dbea..cbe766ac9 100644 --- a/rclrs/src/action/server_goal_handle.rs +++ b/rclrs/src/action/server_goal_handle.rs @@ -1,4 +1,4 @@ -use crate::{rcl_bindings::*, RclrsError}; +use crate::{rcl_bindings::*, RclrsError, ToResult}; use std::{ marker::PhantomData, sync::{Arc, Mutex}, @@ -10,13 +10,25 @@ unsafe impl Send for rcl_action_goal_handle_t {} unsafe impl Sync for rcl_action_goal_handle_t {} +// 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, +} + pub struct ServerGoalHandle where T: rosidl_runtime_rs::Action, { rcl_handle: Arc>, goal_request: Arc, - _marker: PhantomData, } impl ServerGoalHandle @@ -27,27 +39,104 @@ where Self { rcl_handle, goal_request: Arc::clone(&goal_request), - _marker: Default::default(), } } + /// 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 { - false + 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 { - false + 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 { - false + 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(&mut *rcl_handle, event).ok() } + } + + pub fn abort(&self, result: &T::Result) -> Result<(), RclrsError> { + self.update_state(rcl_action_goal_event_t::GOAL_EVENT_ABORT)?; + + // TODO: Invoke on_terminal_state callback + Ok(()) } pub fn succeed(&self, result: &T::Result) -> Result<(), RclrsError> { + self.update_state(rcl_action_goal_event_t::GOAL_EVENT_SUCCEED)?; + + // TODO: Invoke on_terminal_state callback Ok(()) } pub fn canceled(&self, result: &T::Result) -> Result<(), RclrsError> { + self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCELED)?; + + // TODO: Invoke on_terminal_state callback + Ok(()) + } + + pub fn execute(&self, result: &T::Result) -> Result<(), RclrsError> { + self.update_state(rcl_action_goal_event_t::GOAL_EVENT_EXECUTE)?; + + // TODO: Invoke on_executing callback 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) + } + } +} + +impl Drop for ServerGoalHandle +where + T: rosidl_runtime_rs::Action, +{ + /// Cancel the goal if its handle is dropped without reaching a terminal state. + fn drop(&mut self) { + if self.try_canceling() == Ok(true) { + // TODO: Invoke on_terminal_state callback + } + } } From 33a90d51e5aa1c143f67dd978de54c413998d5ae Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 8 Jun 2024 15:32:05 -0400 Subject: [PATCH 22/52] Make GoalUuid into a newtype --- rclrs/src/action.rs | 27 +++++++++++++++++++++++++- rclrs/src/action/server_goal_handle.rs | 14 +++++++++++-- rclrs/src/node.rs | 4 ++-- 3 files changed, 40 insertions(+), 5 deletions(-) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index b4fbe6516..77b3bc3b9 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -3,12 +3,37 @@ mod server; mod server_goal_handle; use crate::rcl_bindings::RCL_ACTION_UUID_SIZE; +use std::fmt; pub use client::{ActionClient, ActionClientBase}; pub use server::{ActionServer, ActionServerBase}; pub use server_goal_handle::ServerGoalHandle; -pub type GoalUUID = [u8; RCL_ACTION_UUID_SIZE]; +#[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], + ) + } +} pub enum GoalResponse { Reject = 1, diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs index cbe766ac9..155babbf2 100644 --- a/rclrs/src/action/server_goal_handle.rs +++ b/rclrs/src/action/server_goal_handle.rs @@ -1,4 +1,4 @@ -use crate::{rcl_bindings::*, RclrsError, ToResult}; +use crate::{rcl_bindings::*, GoalUuid, RclrsError, ToResult}; use std::{ marker::PhantomData, sync::{Arc, Mutex}, @@ -29,16 +29,22 @@ where { rcl_handle: Arc>, goal_request: Arc, + uuid: GoalUuid, } impl ServerGoalHandle where T: rosidl_runtime_rs::Action, { - pub fn new(rcl_handle: Arc>, goal_request: Arc) -> Self { + pub(crate) fn new( + rcl_handle: Arc>, + goal_request: Arc, + uuid: GoalUuid, + ) -> Self { Self { rcl_handle, goal_request: Arc::clone(&goal_request), + uuid, } } @@ -127,6 +133,10 @@ where Ok(false) } } + + pub fn goal_id(&self) -> GoalUuid { + self.uuid + } } impl Drop for ServerGoalHandle diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index c4bae368e..668fd5c70 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -20,7 +20,7 @@ use rosidl_runtime_rs::Message; use crate::{ rcl_bindings::*, ActionClient, ActionServer, CancelResponse, Client, ClientBase, ClientOptions, - ClientState, Clock, ContextHandle, GoalResponse, GoalUUID, GuardCondition, LogParams, Logger, + ClientState, Clock, ContextHandle, GoalResponse, GoalUuid, GuardCondition, LogParams, Logger, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, PublisherOptions, PublisherState, RclrsError, ServerGoalHandle, Service, ServiceBase, ServiceOptions, ServiceState, Subscription, SubscriptionBase, SubscriptionCallback, @@ -311,7 +311,7 @@ impl NodeState { self: &Arc, topic: &str, handle_goal: fn( - &crate::action::GoalUUID, + &crate::action::GoalUuid, Arc<::RmwMsg>, ) -> GoalResponse, handle_cancel: fn(Arc>) -> CancelResponse, From 47cda208cd3bf8a664f5820913d39d0920a63f04 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 8 Jun 2024 15:51:21 -0400 Subject: [PATCH 23/52] Document the ServerGoalHandle struct --- rclrs/src/action/server_goal_handle.rs | 60 ++++++++++++++++++++------ 1 file changed, 48 insertions(+), 12 deletions(-) diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs index 155babbf2..d06837072 100644 --- a/rclrs/src/action/server_goal_handle.rs +++ b/rclrs/src/action/server_goal_handle.rs @@ -23,22 +23,28 @@ enum GoalStatus { Aborted = 6, } -pub struct ServerGoalHandle +/// 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 - T: rosidl_runtime_rs::Action, + ActionT: rosidl_runtime_rs::Action, { rcl_handle: Arc>, - goal_request: Arc, + goal_request: Arc, uuid: GoalUuid, } -impl ServerGoalHandle +impl ServerGoalHandle where - T: rosidl_runtime_rs::Action, + ActionT: rosidl_runtime_rs::Action, { pub(crate) fn new( rcl_handle: Arc>, - goal_request: Arc, + goal_request: Arc, uuid: GoalUuid, ) -> Self { Self { @@ -86,28 +92,52 @@ where unsafe { rcl_action_update_goal_state(&mut *rcl_handle, event).ok() } } - pub fn abort(&self, result: &T::Result) -> Result<(), RclrsError> { + /// 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)?; // TODO: Invoke on_terminal_state callback Ok(()) } - pub fn succeed(&self, result: &T::Result) -> Result<(), RclrsError> { + /// 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)?; // TODO: Invoke on_terminal_state callback Ok(()) } - pub fn canceled(&self, result: &T::Result) -> Result<(), RclrsError> { + /// 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)?; // TODO: Invoke on_terminal_state callback Ok(()) } - pub fn execute(&self, result: &T::Result) -> Result<(), RclrsError> { + /// 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: &ActionT::Result) -> Result<(), RclrsError> { self.update_state(rcl_action_goal_event_t::GOAL_EVENT_EXECUTE)?; // TODO: Invoke on_executing callback @@ -134,14 +164,20 @@ where } } + /// 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) + } } -impl Drop for ServerGoalHandle +impl Drop for ServerGoalHandle where - T: rosidl_runtime_rs::Action, + ActionT: rosidl_runtime_rs::Action, { /// Cancel the goal if its handle is dropped without reaching a terminal state. fn drop(&mut self) { From 83d24708cd14edc96d68cc866ccf7678ce77b5a7 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 8 Jun 2024 16:18:52 -0400 Subject: [PATCH 24/52] Add documentation and clean up --- rclrs/src/action.rs | 8 ++++++++ rclrs/src/action/client.rs | 3 +-- rclrs/src/action/server.rs | 3 +-- rclrs/src/action/server_goal_handle.rs | 15 +++++++++++---- rclrs/src/lib.rs | 1 - 5 files changed, 21 insertions(+), 9 deletions(-) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index 77b3bc3b9..6fe231eed 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -9,6 +9,7 @@ pub use client::{ActionClient, ActionClientBase}; pub use server::{ActionServer, ActionServerBase}; 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]); @@ -35,13 +36,20 @@ impl fmt::Display for GoalUuid { } } +/// The response returned by an [`ActionServer`]'s goal callback when a goal request is received. 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. 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 index 9e8b4d5a9..c78d4f925 100644 --- a/rclrs/src/action/client.rs +++ b/rclrs/src/action/client.rs @@ -68,8 +68,7 @@ where { // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_action_client = unsafe { rcl_action_get_zero_initialized_client() }; - let type_support = ::get_type_support() - as *const rosidl_action_type_support_t; + let type_support = T::get_type_support() as *const rosidl_action_type_support_t; let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { err, s: topic.into(), diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index b92f8805e..e56c15d1b 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -71,8 +71,7 @@ where { // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_action_server = unsafe { rcl_action_get_zero_initialized_server() }; - let type_support = ::get_type_support() - as *const rosidl_action_type_support_t; + let type_support = T::get_type_support() as *const rosidl_action_type_support_t; let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { err, s: topic.into(), diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs index d06837072..5c4a84fa9 100644 --- a/rclrs/src/action/server_goal_handle.rs +++ b/rclrs/src/action/server_goal_handle.rs @@ -1,8 +1,5 @@ use crate::{rcl_bindings::*, GoalUuid, RclrsError, ToResult}; -use std::{ - marker::PhantomData, - sync::{Arc, Mutex}, -}; +use std::sync::{Arc, Mutex}; // 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. @@ -173,6 +170,16 @@ where 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: Arc) -> Result<(), RclrsError> { + // TODO: Invoke public_feedback callback + todo!() + } } impl Drop for ServerGoalHandle diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index e211c4d71..fb93d0ece 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -45,7 +45,6 @@ pub use parameter::*; pub use publisher::*; pub use qos::*; pub use rcl_bindings::rmw_request_id_t; -use rcl_bindings::{rcl_action_goal_handle_t, rcl_context_is_valid}; pub use service::*; pub use subscription::*; pub use time::*; From ae62fb1c58aa38dfd3b7a2e64f7a9005ec73c3a3 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Tue, 11 Jun 2024 14:50:31 -0400 Subject: [PATCH 25/52] Take goal, cancel, and accepted callbacks in ActionServer I'm not sure that this is actually the signature that we'll want, but we'll start from there. --- rclrs/src/action.rs | 2 +- rclrs/src/action/server.rs | 31 +++++++++++++++++++++---------- rclrs/src/node.rs | 35 +++++++++++++++++++++-------------- 3 files changed, 43 insertions(+), 25 deletions(-) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index 6fe231eed..c2ef0e7ad 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -1,5 +1,5 @@ mod client; -mod server; +pub(crate) mod server; mod server_goal_handle; use crate::rcl_bindings::RCL_ACTION_UUID_SIZE; diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index e56c15d1b..9fcef626e 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -1,7 +1,6 @@ -use crate::{error::ToResult, rcl_bindings::*, Clock, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX}; +use crate::{action::{GoalResponse, GoalUuid, CancelResponse, ServerGoalHandle}, error::ToResult, rcl_bindings::*, Clock, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX}; use std::{ ffi::CString, - marker::PhantomData, sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, }; @@ -49,15 +48,18 @@ pub trait ActionServerBase: Send + Sync { // fn execute(&self) -> Result<(), RclrsError>; } -pub struct ActionServer +pub type GoalCallback = dyn Fn(GoalUuid, ::Goal) -> GoalResponse + 'static + Send + Sync; +pub type CancelCallback = dyn Fn(ServerGoalHandle) -> CancelResponse + 'static + Send + Sync; +pub type AcceptedCallback = dyn Fn(ServerGoalHandle) + 'static + Send + Sync; + +pub struct ActionServer where - T: rosidl_runtime_rs::Action, + ActionT: rosidl_runtime_rs::Action, { - _marker: PhantomData T>, pub(crate) handle: Arc, - // goal_callback: (), - // cancel_callback: (), - // accepted_callback: (), + goal_callback: Box>, + cancel_callback: Box>, + accepted_callback: Box>, } impl ActionServer @@ -65,7 +67,14 @@ where T: rosidl_runtime_rs::Action, { /// Creates a new action server. - pub(crate) fn new(node: &Node, clock: Clock, topic: &str) -> Result + pub(crate) fn new( + node: &Node, + clock: Clock, + topic: &str, + goal_callback: impl Fn(GoalUuid, T::Goal) -> GoalResponse + 'static + Send + Sync, + cancel_callback: impl Fn(ServerGoalHandle) -> CancelResponse + 'static + Send + Sync, + accepted_callback: impl Fn(ServerGoalHandle) + 'static + Send + Sync, + ) -> Result where T: rosidl_runtime_rs::Action, { @@ -113,8 +122,10 @@ where }); Ok(Self { - _marker: Default::default(), handle, + goal_callback: Box::new(goal_callback), + cancel_callback: Box::new(cancel_callback), + accepted_callback: Box::new(accepted_callback), }) } } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 668fd5c70..3701a84c0 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -298,8 +298,8 @@ impl NodeState { T: rosidl_runtime_rs::Action, { let action_client = Arc::new(ActionClient::::new(self, topic)?); - // self.clients - // .push(Arc::downgrade(&client) as Weak); + // self.action_clients + // .push(Arc::downgrade(&action_client) as Weak); Ok(action_client) } @@ -307,22 +307,29 @@ impl NodeState { /// /// [1]: crate::ActionServer // TODO: make action server's lifetime depend on node's lifetime - pub fn create_action_server( + pub fn create_action_server( self: &Arc, topic: &str, - handle_goal: fn( - &crate::action::GoalUuid, - Arc<::RmwMsg>, - ) -> GoalResponse, - handle_cancel: fn(Arc>) -> CancelResponse, - handle_accepted: fn(Arc>), - ) -> Result>, RclrsError> + handle_goal: GoalCallback, + handle_cancel: CancelCallback, + handle_accepted: AcceptedCallback, + ) -> Result>, RclrsError> where - T: rosidl_runtime_rs::Action, + ActionT: rosidl_runtime_rs::Action, + GoalCallback: Fn(GoalUuid, ::Goal) -> GoalResponse + 'static + Send + Sync, + CancelCallback: Fn(ServerGoalHandle) -> CancelResponse + 'static + Send + Sync, + AcceptedCallback: Fn(ServerGoalHandle) + 'static + Send + Sync, { - let action_server = Arc::new(ActionServer::::new(self, self.get_clock(), topic)?); - // self.servers - // .push(Arc::downgrade(&server) as Weak); + let action_server = Arc::new(ActionServer::::new( + self, + self.get_clock(), + topic, + handle_goal, + handle_cancel, + handle_accepted, + )?); + // self.action_servers + // .push(Arc::downgrade(&action_server) as Weak); Ok(action_server) } From f197f24604b8264d87e5880a43cdba2caba19f08 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 6 Jul 2024 16:41:35 -0400 Subject: [PATCH 26/52] Store action clients and servers in the Node --- rclrs/src/node.rs | 30 ++++++++++++++++-------------- rclrs/src/node/node_options.rs | 2 ++ 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 3701a84c0..3402d33ab 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -19,7 +19,7 @@ use std::{ use rosidl_runtime_rs::Message; use crate::{ - rcl_bindings::*, ActionClient, ActionServer, CancelResponse, Client, ClientBase, ClientOptions, + rcl_bindings::*, ActionClient, ActionClientBase, ActionServer, ActionServerBase, CancelResponse, Client, ClientBase, ClientOptions, ClientState, Clock, ContextHandle, GoalResponse, GoalUuid, GuardCondition, LogParams, Logger, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, PublisherOptions, PublisherState, RclrsError, ServerGoalHandle, Service, ServiceBase, @@ -88,6 +88,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, @@ -282,7 +284,7 @@ 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) } @@ -298,8 +300,8 @@ impl NodeState { T: rosidl_runtime_rs::Action, { let action_client = Arc::new(ActionClient::::new(self, topic)?); - // self.action_clients - // .push(Arc::downgrade(&action_client) as Weak); + self.action_clients_mtx.lock().unwrap() + .push(Arc::downgrade(&action_client) as Weak); Ok(action_client) } @@ -328,8 +330,8 @@ impl NodeState { handle_cancel, handle_accepted, )?); - // self.action_servers - // .push(Arc::downgrade(&action_server) as Weak); + self.action_servers_mtx.lock().unwrap() + .push(Arc::downgrade(&action_server) as Weak); Ok(action_server) } @@ -346,7 +348,7 @@ 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 } @@ -367,7 +369,7 @@ 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 } @@ -467,7 +469,7 @@ 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) } @@ -522,7 +524,7 @@ 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) @@ -530,28 +532,28 @@ 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() diff --git a/rclrs/src/node/node_options.rs b/rclrs/src/node/node_options.rs index 4744a4812..d7abf7ed8 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(), From 98eef1911685082e7386d291513faebef61b9802 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sun, 7 Jul 2024 12:07:39 -0400 Subject: [PATCH 27/52] Add action client/server entities to wait set They still aren't handled in the .wait() method, though. --- rclrs/src/action/client.rs | 31 +++++++-- rclrs/src/action/server.rs | 28 +++++++- rclrs/src/node.rs | 75 +++++++++++++++++----- rclrs/src/wait.rs | 128 +++++++++++++++++++++++++++++++++++-- 4 files changed, 233 insertions(+), 29 deletions(-) diff --git a/rclrs/src/action/client.rs b/rclrs/src/action/client.rs index c78d4f925..9e20fd710 100644 --- a/rclrs/src/action/client.rs +++ b/rclrs/src/action/client.rs @@ -1,4 +1,7 @@ -use crate::{error::ToResult, rcl_bindings::*, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX}; +use crate::{ + error::ToResult, rcl_bindings::*, wait::WaitableNumEntities, Node, RclrsError, + ENTITY_LIFECYCLE_MUTEX, +}; use std::{ ffi::CString, marker::PhantomData, @@ -45,16 +48,18 @@ impl Drop for ActionClientHandle { pub trait ActionClientBase: Send + Sync { /// Internal function to get a reference to the `rcl` handle. fn handle(&self) -> &ActionClientHandle; + fn num_entities(&self) -> &WaitableNumEntities; // /// Tries to take a new request and run the callback with it. // fn execute(&self) -> Result<(), RclrsError>; } -pub struct ActionClient +pub struct ActionClient where - T: rosidl_runtime_rs::Action, + ActionT: rosidl_runtime_rs::Action, { - _marker: PhantomData T>, + _marker: PhantomData ActionT>, pub(crate) handle: Arc, + num_entities: WaitableNumEntities, } impl ActionClient @@ -106,9 +111,23 @@ where in_use_by_wait_set: Arc::new(AtomicBool::new(false)), }); + let mut num_entities = WaitableNumEntities::default(); + unsafe { + rcl_action_client_wait_set_get_num_entities( + &*handle.lock(), + &mut num_entities.num_subscriptions, + &mut num_entities.num_guard_conditions, + &mut num_entities.num_timers, + &mut num_entities.num_clients, + &mut num_entities.num_services, + ) + .ok()?; + } + Ok(Self { _marker: Default::default(), handle, + num_entities, }) } } @@ -120,4 +139,8 @@ where fn handle(&self) -> &ActionClientHandle { &self.handle } + + fn num_entities(&self) -> &WaitableNumEntities { + &self.num_entities + } } diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 9fcef626e..abbed6022 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -1,4 +1,10 @@ -use crate::{action::{GoalResponse, GoalUuid, CancelResponse, ServerGoalHandle}, error::ToResult, rcl_bindings::*, Clock, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX}; +use crate::{ + action::{CancelResponse, GoalResponse, GoalUuid, ServerGoalHandle}, + error::ToResult, + rcl_bindings::*, + wait::WaitableNumEntities, + Clock, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX, +}; use std::{ ffi::CString, sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, @@ -44,6 +50,7 @@ impl Drop for ActionServerHandle { pub trait ActionServerBase: Send + Sync { /// Internal function to get a reference to the `rcl` handle. fn handle(&self) -> &ActionServerHandle; + fn num_entities(&self) -> &WaitableNumEntities; // /// Tries to take a new request and run the callback with it. // fn execute(&self) -> Result<(), RclrsError>; } @@ -57,6 +64,7 @@ where ActionT: rosidl_runtime_rs::Action, { pub(crate) handle: Arc, + num_entities: WaitableNumEntities, goal_callback: Box>, cancel_callback: Box>, accepted_callback: Box>, @@ -121,8 +129,22 @@ where 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), @@ -137,4 +159,8 @@ where fn handle(&self) -> &ActionServerHandle { &self.handle } + + fn num_entities(&self) -> &WaitableNumEntities { + &self.num_entities + } } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 3402d33ab..b00280cf5 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -19,12 +19,13 @@ use std::{ use rosidl_runtime_rs::Message; use crate::{ - rcl_bindings::*, ActionClient, ActionClientBase, ActionServer, ActionServerBase, CancelResponse, Client, ClientBase, ClientOptions, - ClientState, Clock, ContextHandle, GoalResponse, GoalUuid, GuardCondition, LogParams, Logger, - ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, - PublisherOptions, PublisherState, RclrsError, ServerGoalHandle, Service, ServiceBase, - ServiceOptions, ServiceState, Subscription, SubscriptionBase, SubscriptionCallback, - SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, ActionClient, ActionClientBase, ActionServer, ActionServerBase, + CancelResponse, Client, ClientBase, ClientOptions, ClientState, Clock, ContextHandle, + GoalResponse, GoalUuid, GuardCondition, LogParams, Logger, ParameterBuilder, + ParameterInterface, ParameterVariant, 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 @@ -284,7 +285,10 @@ 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) } @@ -300,7 +304,9 @@ impl NodeState { T: rosidl_runtime_rs::Action, { let action_client = Arc::new(ActionClient::::new(self, topic)?); - self.action_clients_mtx.lock().unwrap() + self.action_clients_mtx + .lock() + .unwrap() .push(Arc::downgrade(&action_client) as Weak); Ok(action_client) } @@ -330,7 +336,9 @@ impl NodeState { handle_cancel, handle_accepted, )?); - self.action_servers_mtx.lock().unwrap() + self.action_servers_mtx + .lock() + .unwrap() .push(Arc::downgrade(&action_server) as Weak); Ok(action_server) } @@ -348,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 } @@ -369,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 } @@ -469,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) } @@ -524,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) @@ -532,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/wait.rs b/rclrs/src/wait.rs index c0e0c659d..1da55f411 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -20,7 +20,8 @@ use std::{sync::Arc, time::Duration, vec::Vec}; use crate::{ 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 +51,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, } @@ -123,6 +126,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 +143,37 @@ 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 +192,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 +213,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 +348,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 @@ -413,6 +517,16 @@ impl WaitSet { } } +#[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::*; From f6d11a76365ba5a06c9b20f60f839f21b1db5766 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sun, 7 Jul 2024 13:38:25 -0400 Subject: [PATCH 28/52] Handle action server/client readiness in WaitSet and executor Currently, action servers and clients that are ready in multiple ways are returned to the executor as a list of pairs, with one readiness mode per entry. This could alternatively be encoded as a bitfield or similar struct, with any given client/server only occurring once in the list. However, to ensure that the executor has control over execution order, we would need to expose individual `execute_readiness_mode()` methods from the client and server, rather than a unified `execute(Mode)` method. That's fine too, but something to keep in mind. --- rclrs/src/action.rs | 2 +- rclrs/src/action/client.rs | 17 +++++- rclrs/src/action/server.rs | 16 ++++- rclrs/src/executor.rs | 8 +++ rclrs/src/wait.rs | 119 +++++++++++++++++++++++++++++++++++-- 5 files changed, 153 insertions(+), 9 deletions(-) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index c2ef0e7ad..e84944c49 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -1,4 +1,4 @@ -mod client; +pub(crate) mod client; pub(crate) mod server; mod server_goal_handle; diff --git a/rclrs/src/action/client.rs b/rclrs/src/action/client.rs index 9e20fd710..808c10bf4 100644 --- a/rclrs/src/action/client.rs +++ b/rclrs/src/action/client.rs @@ -48,9 +48,18 @@ impl Drop for ActionClientHandle { 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 take a new request and run the callback with it. - // fn execute(&self) -> Result<(), RclrsError>; + /// 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, } pub struct ActionClient @@ -143,4 +152,8 @@ where fn num_entities(&self) -> &WaitableNumEntities { &self.num_entities } + + fn execute(&self, mode: ReadyMode) -> Result<(), RclrsError> { + todo!() + } } diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index abbed6022..a6d95cc61 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -50,9 +50,17 @@ impl Drop for ActionServerHandle { 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 take a new request and run the callback with it. - // fn execute(&self) -> Result<(), RclrsError>; + /// Tries to run the callback for the given readiness mode. + fn execute(&self, mode: ReadyMode) -> Result<(), RclrsError>; +} + +pub(crate) enum ReadyMode { + GoalRequest, + CancelRequest, + ResultRequest, + GoalExpired, } pub type GoalCallback = dyn Fn(GoalUuid, ::Goal) -> GoalResponse + 'static + Send + Sync; @@ -163,4 +171,8 @@ where fn num_entities(&self) -> &WaitableNumEntities { &self.num_entities } + + fn execute(&self, mode: ReadyMode) -> Result<(), RclrsError> { + todo!() + } } diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index f38eec5bf..186e17e3e 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/wait.rs b/rclrs/src/wait.rs index 1da55f411..63715e00d 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -18,6 +18,9 @@ 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::*, ActionClientBase, ActionServerBase, ClientBase, Context, ContextHandle, Node, ServiceBase, @@ -66,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 { @@ -156,8 +163,12 @@ impl WaitSet { 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()); + 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; @@ -451,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(_) => (), @@ -469,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 @@ -513,6 +527,103 @@ 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) } } From f48a590a3d37234d2df724880daa9d5afa981388 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 13 Jul 2024 16:51:55 -0400 Subject: [PATCH 29/52] Add rcl_action error codes There's one error code, ActionNameInvalid, that conflicts with the EventInvalid code. We should consult with upstream about this, as it causes issues for representing error codes as enum values. These two error codes are probably never returned by the same function, but it would be better to keep them unique. --- rclrs/src/error.rs | 38 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs index 527a4d3a6..c5372c297 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)." } From fb8e703a5b8707af218655b3eb3d7306492c39c9 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 13 Jul 2024 19:16:01 -0400 Subject: [PATCH 30/52] [WIP] Start defining server/client execute functions This is just the basic layout. I'm trying to avoid defining the `take_*` functions that rclcpp has to link taking messages to executing on them. I'm not sure if that's actually worthwhile yet. This area should also be revisited once it's functional to see whether portions can be moved into the non-polymorphic subfunctions. Doing so could reduce compile times by avoiding excessive monomorphization. --- rclrs/src/action/client.rs | 28 +++++++++++++++++++- rclrs/src/action/server.rs | 53 ++++++++++++++++++++++++++++++++++++-- 2 files changed, 78 insertions(+), 3 deletions(-) diff --git a/rclrs/src/action/client.rs b/rclrs/src/action/client.rs index 808c10bf4..38d9fb303 100644 --- a/rclrs/src/action/client.rs +++ b/rclrs/src/action/client.rs @@ -139,6 +139,26 @@ where num_entities, }) } + + 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 ActionClient @@ -154,6 +174,12 @@ where } fn execute(&self, mode: ReadyMode) -> Result<(), RclrsError> { - todo!() + 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(), + } } } diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index a6d95cc61..29198abb3 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -1,6 +1,6 @@ use crate::{ action::{CancelResponse, GoalResponse, GoalUuid, ServerGoalHandle}, - error::ToResult, + error::{RclReturnCode, ToResult}, rcl_bindings::*, wait::WaitableNumEntities, Clock, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX, @@ -158,6 +158,50 @@ where accepted_callback: Box::new(accepted_callback), }) } + + fn execute_goal_request(&self) -> Result<(), RclrsError> { + // Take pending goal request + let (request_id, request) = { + let mut request_id = rmw_request_id_t { + writer_guid: [0; 16], + sequence_number: 0, + }; + type RmwMsg = + <::Goal as rosidl_runtime_rs::Message>::RmwMsg; + let mut request_rmw = RmwMsg::::default(); + let handle = &*self.handle.lock(); + let take_result = unsafe { + // SAFETY: The three pointers are valid/initialized + rcl_action_take_goal_request( + handle, + &mut request_id, + &mut request_rmw as *mut RmwMsg as *mut _, + ) + }; + match take_result.try_into().unwrap() { + RclReturnCode::Ok => (), + // Spurious wakeup – this may happen even when a waitset indicated that this + // service was ready, so it shouldn't be an error. + RclReturnCode::ServiceTakeFailed => return Ok(()), + _ => return take_result.ok(), + } + let request = todo!("Convert request_rmw to expected type"); + (request_id, request) + }; + todo!() + } + + fn execute_cancel_request(&self) -> Result<(), RclrsError> { + todo!() + } + + fn execute_result_request(&self) -> Result<(), RclrsError> { + todo!() + } + + fn execute_goal_expired(&self) -> Result<(), RclrsError> { + todo!() + } } impl ActionServerBase for ActionServer @@ -173,6 +217,11 @@ where } fn execute(&self, mode: ReadyMode) -> Result<(), RclrsError> { - todo!() + 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(), + } } } From 3083cc4c5398282c4763d39ca82d5600f9ccf2ee Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 20 Jul 2024 18:16:08 -0400 Subject: [PATCH 31/52] Use rcl-allocated goal handle pointer in ServerGoalHandle The `rcl_action_accept_new_goal()` function returns a pre-allocated `rcl_action_goal_handle_t` pointer, which is also stored within the action server proper. This means we cannot store a non-pointer version of this in the `ServerGoalHandle`. Instead, we'll keep a mutex-guarded mutable pointer. The `Arc` is unnecessary since this pointer is never shared with anyone. Also, we need to clean up the goal handle by calling `rcl_action_goal_handle_fini()` when the `ServerGoalHandle` is dropped. --- rclrs/src/action/server_goal_handle.rs | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs index 5c4a84fa9..a6eeca6c9 100644 --- a/rclrs/src/action/server_goal_handle.rs +++ b/rclrs/src/action/server_goal_handle.rs @@ -30,7 +30,7 @@ pub struct ServerGoalHandle where ActionT: rosidl_runtime_rs::Action, { - rcl_handle: Arc>, + rcl_handle: Mutex<*mut rcl_action_goal_handle_t>, goal_request: Arc, uuid: GoalUuid, } @@ -40,12 +40,12 @@ where ActionT: rosidl_runtime_rs::Action, { pub(crate) fn new( - rcl_handle: Arc>, + rcl_handle: *mut rcl_action_goal_handle_t, goal_request: Arc, uuid: GoalUuid, ) -> Self { Self { - rcl_handle, + rcl_handle: Mutex::new(rcl_handle), goal_request: Arc::clone(&goal_request), uuid, } @@ -57,7 +57,7 @@ where { 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()? } + 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. @@ -74,7 +74,7 @@ where 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) } + unsafe { rcl_action_goal_handle_is_active(*rcl_handle) } } /// Returns whether the goal is executing. @@ -86,7 +86,7 @@ where 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(&mut *rcl_handle, event).ok() } + unsafe { rcl_action_update_goal_state(*rcl_handle, event).ok() } } /// Indicate that the goal could not be reached and has been aborted. @@ -147,7 +147,7 @@ where // 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) }; + 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)?; } @@ -191,5 +191,11 @@ where if self.try_canceling() == Ok(true) { // TODO: Invoke on_terminal_state callback } + { + 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); } + } } } From 908859fa91992db087027a08d8d9f400fb5d9227 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 20 Jul 2024 18:23:50 -0400 Subject: [PATCH 32/52] Split execute_goal_request() out into three functions The logic in `execute_goal_request()` was starting to get unwieldy, so I split it into three functions. The idea is that the first, `take_goal_request()`, should handle everything up until we call the user's callback. Meanwhile, `send_goal_response()` handles everything after the user callback, sending the response and, if accepted, setting everything up for the action server. `execute_goal_request()` is the overall function coordinating all of this. It passes request data from the `take*` function to the user callback and passes the returned response into the `send*` function. In addition to splitting the logic into digestible pieces, this means we could also easily make the goal-request callback asynchronous by delaying the `send*` function until the user has given their asynchronous response. --- rclrs/src/action.rs | 1 + rclrs/src/action/server.rs | 126 ++++++++++++++++++++++++++++++------- 2 files changed, 104 insertions(+), 23 deletions(-) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index e84944c49..7c6b23539 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -37,6 +37,7 @@ impl fmt::Display for GoalUuid { } /// The response returned by an [`ActionServer`]'s goal callback when a goal request is received. +#[derive(PartialEq, Eq)] pub enum GoalResponse { /// The goal is rejected and will not be executed. Reject = 1, diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 29198abb3..c606031ec 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -5,6 +5,7 @@ use crate::{ wait::WaitableNumEntities, Clock, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; +use rosidl_runtime_rs::{Action, Message, Service}; use std::{ ffi::CString, sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, @@ -159,36 +160,115 @@ where }) } - fn execute_goal_request(&self) -> Result<(), RclrsError> { - // Take pending goal request - let (request_id, request) = { - let mut request_id = rmw_request_id_t { - writer_guid: [0; 16], - sequence_number: 0, + 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 three pointers are valid/initialized + 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, response: GoalResponse, request: &<::Request as Message>::RmwMsg, mut request_id: rmw_request_id_t) -> Result<(), RclrsError> { + let accepted = response != GoalResponse::Reject; + + // SAFETY: No preconditions + let mut goal_info = unsafe { rcl_action_get_zero_initialized_goal_info() }; + // Populate the goal UUID; the other fields will be populated by rcl_action later on. + // TODO(nwn): Check this claim. + rosidl_runtime_rs::ExtractUuid::extract_uuid(request, &mut goal_info.goal_id.uuid); + + let goal_handle = if accepted { + 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, + ) }; - type RmwMsg = - <::Goal as rosidl_runtime_rs::Message>::RmwMsg; - let mut request_rmw = RmwMsg::::default(); + if goal_handle_ptr.is_null() { + // Other than rcl_get_error_string(), there's no indication what happened. + panic!("Failed to accept goal"); + } else { + Some(ServerGoalHandle::::new(goal_handle_ptr, todo!(""), GoalUuid(goal_info.goal_id.uuid))) + } + } else { + None + }; + + { + type RmwResponse = <<::SendGoalService as Service>::Response as Message>::RmwMsg; + let mut response_rmw = RmwResponse::::default(); + // TODO(nwn): Set the `accepted` field through a trait, similarly to how we extracted the UUID. + // response_rmw.accepted = accepted; let handle = &*self.handle.lock(); - let take_result = unsafe { - // SAFETY: The three pointers are valid/initialized - rcl_action_take_goal_request( + 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, `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 request_rmw as *mut RmwMsg as *mut _, + &mut response_rmw as *mut RmwResponse as *mut _, ) - }; - match take_result.try_into().unwrap() { - RclReturnCode::Ok => (), + }.ok()?; // TODO(nwn): Suppress RclReturnCode::Timeout? + } + + if let Some(goal_handle) = goal_handle { + // Goal was accepted + + // TODO: Add a UUID->goal_handle entry to a server goal map. + + // TODO: If accept_and_execute, update goal state + + // TODO: Call publish_status() + + // TODO: Call the goal_accepted callback + } + + Ok(()) + } + + fn execute_goal_request(&self) -> Result<(), RclrsError> { + let (request, request_id) = match self.take_goal_request() { + Ok(res) => res, + Err(RclrsError::RclError { code: RclReturnCode::ServiceTakeFailed, .. }) => { // Spurious wakeup – this may happen even when a waitset indicated that this - // service was ready, so it shouldn't be an error. - RclReturnCode::ServiceTakeFailed => return Ok(()), - _ => return take_result.ok(), - } - let request = todo!("Convert request_rmw to expected type"); - (request_id, request) + // action was ready, so it shouldn't be an error. + return Ok(()); + }, + Err(err) => return Err(err), }; - todo!() + + let response: GoalResponse = + { + let mut uuid = GoalUuid::default(); + rosidl_runtime_rs::ExtractUuid::extract_uuid(&request, &mut uuid.0); + + todo!("Optionally convert request to an idiomatic type for the user's callback."); + todo!("Call self.goal_callback(uuid, request)"); + }; + + self.send_goal_response(response, &request, request_id)?; + + Ok(()) } fn execute_cancel_request(&self) -> Result<(), RclrsError> { From 2b5a6b5bc4d27c575aa4e6c693f11a6480cd1d28 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 25 Jul 2024 15:41:17 -0400 Subject: [PATCH 33/52] Partial implementation of ActionServer::publish_status() --- rclrs/src/action/server.rs | 44 +++++++++++++++++++++++++- rclrs/src/action/server_goal_handle.rs | 2 +- 2 files changed, 44 insertions(+), 2 deletions(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index c606031ec..8f76eb7c8 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -236,7 +236,9 @@ where // TODO: Add a UUID->goal_handle entry to a server goal map. - // TODO: If accept_and_execute, update goal state + if response == GoalResponse::AcceptAndExecute { + goal_handle.execute()?; + } // TODO: Call publish_status() @@ -282,6 +284,46 @@ where fn execute_goal_expired(&self) -> Result<(), RclrsError> { todo!() } + + fn publish_status(&self) -> Result<(), RclrsError> { + // We need to hold the lock across this entire method because + // rcl_action_server_get_goal_handles() returns an internal pointer to the + // goal data. + let handle = &*self.handle.lock(); + + let mut goal_handles = std::ptr::null_mut::<*mut rcl_action_goal_handle_t>(); + let mut num_goal_handles = 0; + unsafe { + // SAFETY: The action server is locked for this entire function, ensuring that the + // goal_handles array remains valid, unless rcl_shutdown is called. However, that is + // outside our control. + rcl_action_server_get_goal_handles(handle, &mut goal_handles, &mut num_goal_handles) + }.ok()?; + + let mut goal_statuses = unsafe { + // SAFETY: No preconditions + rcl_action_get_zero_initialized_goal_status_array() + }; + unsafe { + // SAFETY: The action server is locked through the handle and goal_statuses is + // zero-initialized. + rcl_action_get_goal_status_array(handle, &mut goal_statuses) + }.ok()?; + // TODO(nwn): Ensure that rcl_action_goal_status_array_fini() is always called on exit. + + let goal_status_slice = unsafe { + // SAFETY: rcl_action_get_goal_status_array initializes goal_statues.msg.status_list as + // an array of goal statuses with the indicated size. The memory backing this array is + // not modified by anything else during the lifetime of the slice. + std::slice::from_raw_parts(goal_statuses.msg.status_list.data, goal_statuses.msg.status_list.size) + }; + for goal_status in goal_status_slice { + // Copy into the correct message type to pass to rcl_action_publish_status(). + } + // Call rcl_action_publish_status(). + + todo!() + } } impl ActionServerBase for ActionServer diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs index a6eeca6c9..ceccbe610 100644 --- a/rclrs/src/action/server_goal_handle.rs +++ b/rclrs/src/action/server_goal_handle.rs @@ -134,7 +134,7 @@ where /// 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: &ActionT::Result) -> Result<(), RclrsError> { + pub fn execute(&self) -> Result<(), RclrsError> { self.update_state(rcl_action_goal_event_t::GOAL_EVENT_EXECUTE)?; // TODO: Invoke on_executing callback From 2c8cd972af21370a0e916d54b9eabaef2f2de0dc Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Tue, 6 Aug 2024 21:54:53 -0400 Subject: [PATCH 34/52] Add DropGuard convenience wrapper This provides a convenient RAII-style way to ensure that a function is always called when a specific value gets dropped. --- rclrs/src/drop_guard.rs | 48 +++++++++++++++++++++++++++++++++++++++++ rclrs/src/lib.rs | 2 ++ 2 files changed, 50 insertions(+) create mode 100644 rclrs/src/drop_guard.rs diff --git a/rclrs/src/drop_guard.rs b/rclrs/src/drop_guard.rs new file mode 100644 index 000000000..f4e47b2df --- /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/lib.rs b/rclrs/src/lib.rs index fb93d0ece..02cea577f 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -10,6 +10,7 @@ mod arguments; mod client; mod clock; mod context; +mod drop_guard; mod error; mod executor; mod logging; @@ -37,6 +38,7 @@ 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::*; From 5cce85f3d90d818f94e796b2dcd7a6a353d6a54f Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Tue, 6 Aug 2024 22:43:14 -0400 Subject: [PATCH 35/52] Complete implementation of ActionServer::publish_status() This skips some of the steps that rclcpp performs, as they appear to be unnecessary. Testing should reveal whether that's true or not. --- rclrs/src/action/server.rs | 45 ++++++++++++-------------------------- 1 file changed, 14 insertions(+), 31 deletions(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 8f76eb7c8..68efaca30 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -3,7 +3,7 @@ use crate::{ error::{RclReturnCode, ToResult}, rcl_bindings::*, wait::WaitableNumEntities, - Clock, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX, + Clock, DropGuard, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; use rosidl_runtime_rs::{Action, Message, Service}; use std::{ @@ -286,43 +286,26 @@ where } fn publish_status(&self) -> Result<(), RclrsError> { - // We need to hold the lock across this entire method because - // rcl_action_server_get_goal_handles() returns an internal pointer to the - // goal data. - let handle = &*self.handle.lock(); - - let mut goal_handles = std::ptr::null_mut::<*mut rcl_action_goal_handle_t>(); - let mut num_goal_handles = 0; - unsafe { - // SAFETY: The action server is locked for this entire function, ensuring that the - // goal_handles array remains valid, unless rcl_shutdown is called. However, that is - // outside our control. - rcl_action_server_get_goal_handles(handle, &mut goal_handles, &mut num_goal_handles) - }.ok()?; - - let mut goal_statuses = unsafe { + let mut goal_statuses = DropGuard::new(unsafe { // SAFETY: No preconditions rcl_action_get_zero_initialized_goal_status_array() - }; + }, |mut goal_status| 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_status); + }); + unsafe { // SAFETY: The action server is locked through the handle and goal_statuses is // zero-initialized. - rcl_action_get_goal_status_array(handle, &mut goal_statuses) + rcl_action_get_goal_status_array(&*self.handle.lock(), &mut *goal_statuses) }.ok()?; - // TODO(nwn): Ensure that rcl_action_goal_status_array_fini() is always called on exit. - - let goal_status_slice = unsafe { - // SAFETY: rcl_action_get_goal_status_array initializes goal_statues.msg.status_list as - // an array of goal statuses with the indicated size. The memory backing this array is - // not modified by anything else during the lifetime of the slice. - std::slice::from_raw_parts(goal_statuses.msg.status_list.data, goal_statuses.msg.status_list.size) - }; - for goal_status in goal_status_slice { - // Copy into the correct message type to pass to rcl_action_publish_status(). - } - // Call rcl_action_publish_status(). - todo!() + 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() } } From 555b267cf15a83ad8d52fe779a7caa81d65c8721 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Wed, 7 Aug 2024 18:14:14 -0400 Subject: [PATCH 36/52] Move goal acceptance logic back into execute_goal_request() This trims the send_goal_response() function down to only be a wrapper around the rcl_action equivalent. In addition to improving logical separation, this also simplifies control flow when handling rejection. --- rclrs/src/action/server.rs | 175 +++++++++++++++++++++---------------- 1 file changed, 99 insertions(+), 76 deletions(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 68efaca30..9db9df862 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -175,100 +175,115 @@ where &mut request_id, &mut request_rmw as *mut RmwRequest as *mut _, ) - }.ok()?; + } + .ok()?; Ok((request_rmw, request_id)) } - fn send_goal_response(&self, response: GoalResponse, request: &<::Request as Message>::RmwMsg, mut request_id: rmw_request_id_t) -> Result<(), RclrsError> { - let accepted = response != GoalResponse::Reject; + fn send_goal_response( + &self, + mut request_id: rmw_request_id_t, + accepted: bool, + ) -> Result<(), RclrsError> { + type RmwResponse = <<::SendGoalService as Service>::Response as Message>::RmwMsg; + let mut response_rmw = RmwResponse::::default(); + // TODO(nwn): Set the `accepted` field through a trait, similarly to how we extracted the UUID. + // response_rmw.accepted = accepted; + 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 RmwResponse 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) -> 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 mut uuid = GoalUuid::default(); + rosidl_runtime_rs::ExtractUuid::extract_uuid(&request, &mut uuid.0); + + 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); + } // SAFETY: No preconditions let mut goal_info = unsafe { rcl_action_get_zero_initialized_goal_info() }; // Populate the goal UUID; the other fields will be populated by rcl_action later on. // TODO(nwn): Check this claim. - rosidl_runtime_rs::ExtractUuid::extract_uuid(request, &mut goal_info.goal_id.uuid); + goal_info.goal_id.uuid = uuid.0; - let goal_handle = if accepted { + let goal_handle = { 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, - ) + 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 { - Some(ServerGoalHandle::::new(goal_handle_ptr, todo!(""), GoalUuid(goal_info.goal_id.uuid))) + ServerGoalHandle::::new( + goal_handle_ptr, + todo!(""), + GoalUuid(goal_info.goal_id.uuid), + ) } - } else { - None }; - { - type RmwResponse = <<::SendGoalService as Service>::Response as Message>::RmwMsg; - let mut response_rmw = RmwResponse::::default(); - // TODO(nwn): Set the `accepted` field through a trait, similarly to how we extracted the UUID. - // response_rmw.accepted = accepted; - let handle = &*self.handle.lock(); - 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, `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 RmwResponse as *mut _, - ) - }.ok()?; // TODO(nwn): Suppress RclReturnCode::Timeout? - } - - if let Some(goal_handle) = goal_handle { - // Goal was accepted - - // TODO: Add a UUID->goal_handle entry to a server goal map. + self.send_goal_response(request_id, true)?; - if response == GoalResponse::AcceptAndExecute { - goal_handle.execute()?; - } - - // TODO: Call publish_status() + // TODO: Add a UUID->goal_handle entry to a server goal map. - // TODO: Call the goal_accepted callback + if response == GoalResponse::AcceptAndExecute { + goal_handle.execute()?; } - Ok(()) - } + self.publish_status()?; - fn execute_goal_request(&self) -> Result<(), RclrsError> { - let (request, request_id) = match self.take_goal_request() { - Ok(res) => res, - Err(RclrsError::RclError { code: RclReturnCode::ServiceTakeFailed, .. }) => { - // Spurious wakeup – this may happen even when a waitset indicated that this - // action was ready, so it shouldn't be an error. - return Ok(()); - }, - Err(err) => return Err(err), - }; - - let response: GoalResponse = - { - let mut uuid = GoalUuid::default(); - rosidl_runtime_rs::ExtractUuid::extract_uuid(&request, &mut uuid.0); - - todo!("Optionally convert request to an idiomatic type for the user's callback."); - todo!("Call self.goal_callback(uuid, request)"); - }; - - self.send_goal_response(response, &request, request_id)?; + // TODO: Call the user's goal_accepted callback. + todo!("Call self.accepted_callback(goal_handle)"); Ok(()) } @@ -286,26 +301,34 @@ where } 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_status| 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_status); - }); + 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()?; + } + .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() + rcl_action_publish_status( + &*self.handle.lock(), + &goal_statuses.msg as *const _ as *const std::ffi::c_void, + ) + } + .ok() } } From 6da2e698d46705cec6218dc4631173d1f1f66a51 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Wed, 7 Aug 2024 18:45:30 -0400 Subject: [PATCH 37/52] Integrate RMW message methods into ActionImpl Rather than having a bunch of standalone traits implementing various message functions like `ExtractUuid` and `SetAccepted`, with the trait bounds on each associated type in `ActionImpl`, we'll instead add these functions directly to the `ActionImpl` trait. This is simpler on both the rosidl_runtime_rs and the rclrs side. --- rclrs/src/action/server.rs | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 9db9df862..630c90cba 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -5,7 +5,7 @@ use crate::{ wait::WaitableNumEntities, Clock, DropGuard, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; -use rosidl_runtime_rs::{Action, Message, Service}; +use rosidl_runtime_rs::{Action, ActionImpl, Message, Service}; use std::{ ffi::CString, sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, @@ -165,7 +165,7 @@ where writer_guid: [0; 16], sequence_number: 0, }; - type RmwRequest = <<::SendGoalService as Service>::Request as Message>::RmwMsg; + type RmwRequest = <<::SendGoalService as Service>::Request as Message>::RmwMsg; let mut request_rmw = RmwRequest::::default(); let handle = &*self.handle.lock(); unsafe { @@ -186,10 +186,9 @@ where mut request_id: rmw_request_id_t, accepted: bool, ) -> Result<(), RclrsError> { - type RmwResponse = <<::SendGoalService as Service>::Response as Message>::RmwMsg; + type RmwResponse = <<::SendGoalService as Service>::Response as Message>::RmwMsg; let mut response_rmw = RmwResponse::::default(); - // TODO(nwn): Set the `accepted` field through a trait, similarly to how we extracted the UUID. - // response_rmw.accepted = accepted; + ::set_goal_response_accepted(&mut response_rmw, accepted); let handle = &*self.handle.lock(); let result = unsafe { // SAFETY: The action server handle is locked and so synchronized with other @@ -232,8 +231,7 @@ where Err(err) => return Err(err), }; - let mut uuid = GoalUuid::default(); - rosidl_runtime_rs::ExtractUuid::extract_uuid(&request, &mut uuid.0); + let uuid = GoalUuid(::get_goal_request_uuid(&request)); let response: GoalResponse = { todo!("Optionally convert request to an idiomatic type for the user's callback."); From 31e38ce5ef19053f6b606e6cc1515513dd6f6d86 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Wed, 7 Aug 2024 19:55:11 -0400 Subject: [PATCH 38/52] Add UUID->GoalHandle hash-map to action server This requires a mutex to enable simultaneous goal acceptance in a multithreaded executor. We also make ServerGoalHandles implement Send and Sync, since they will be accessed by multiple threads during callbacks. Due to containing a raw pointer field, the type doesn't automatically implement Send/Sync; however, we guarantee that the uses of this pointer is safe and synchronized, allowing us to safely implement the traits. --- rclrs/src/action/server.rs | 25 ++++++++++++++----------- rclrs/src/action/server_goal_handle.rs | 14 ++++++++------ 2 files changed, 22 insertions(+), 17 deletions(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 630c90cba..9489ad0dc 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -7,6 +7,7 @@ use crate::{ }; use rosidl_runtime_rs::{Action, ActionImpl, Message, Service}; use std::{ + collections::HashMap, ffi::CString, sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, }; @@ -77,6 +78,7 @@ where goal_callback: Box>, cancel_callback: Box>, accepted_callback: Box>, + goal_handles: Mutex>>>, } impl ActionServer @@ -157,6 +159,7 @@ where goal_callback: Box::new(goal_callback), cancel_callback: Box::new(cancel_callback), accepted_callback: Box::new(accepted_callback), + goal_handles: Mutex::new(HashMap::new()), }) } @@ -243,13 +246,13 @@ where return self.send_goal_response(request_id, false); } - // SAFETY: No preconditions - let mut goal_info = unsafe { rcl_action_get_zero_initialized_goal_info() }; - // Populate the goal UUID; the other fields will be populated by rcl_action later on. - // TODO(nwn): Check this claim. - goal_info.goal_id.uuid = uuid.0; - 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 @@ -262,17 +265,17 @@ where // Other than rcl_get_error_string(), there's no indication what happened. panic!("Failed to accept goal"); } else { - ServerGoalHandle::::new( + Arc::new(ServerGoalHandle::::new( goal_handle_ptr, - todo!(""), - GoalUuid(goal_info.goal_id.uuid), - ) + todo!("Create an Arc holding the goal message"), + uuid, + )) } }; self.send_goal_response(request_id, true)?; - // TODO: Add a UUID->goal_handle entry to a server goal map. + self.goal_handles.lock().unwrap().insert(uuid, Arc::clone(&goal_handle)); if response == GoalResponse::AcceptAndExecute { goal_handle.execute()?; diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs index ceccbe610..a91627906 100644 --- a/rclrs/src/action/server_goal_handle.rs +++ b/rclrs/src/action/server_goal_handle.rs @@ -1,12 +1,6 @@ use crate::{rcl_bindings::*, GoalUuid, RclrsError, ToResult}; use std::sync::{Arc, Mutex}; -// 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_goal_handle_t {} - -unsafe impl Sync for rcl_action_goal_handle_t {} - // Values defined by `action_msgs/msg/GoalStatus` #[repr(i8)] #[derive(Debug, Clone, Hash, PartialEq, Eq)] @@ -35,6 +29,14 @@ where 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 {} +unsafe impl Sync for ServerGoalHandle where ActionT: rosidl_runtime_rs::Action {} + impl ServerGoalHandle where ActionT: rosidl_runtime_rs::Action, From 8d820f9456dfb0145310f18d40df9f315e67d638 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 9 Aug 2024 17:11:02 -0400 Subject: [PATCH 39/52] Add rosidl_runtime_rs::ActionImpl::create_feedback_message() Adds a trait method to create a feedback message given the goal ID and the user-facing feedback message type. Depending on how many times we do this, it may end up valuable to define a GoalUuid type in rosidl_runtime_rs itself. We wouldn't be able to utilize the `RCL_ACTION_UUID_SIZE` constant imported from `rcl_action`, but this is pretty much guaranteed to be 16 forever. Defining this method signature also required inverting the super-trait relationship between Action and ActionImpl. Now ActionImpl is the sub-trait as this gives it access to all of Action's associated types. Action doesn't need to care about anything from ActionImpl (hopefully). --- rclrs/src/action/server.rs | 6 +++--- rclrs/src/node.rs | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 9489ad0dc..8c9656ee2 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -83,7 +83,7 @@ where impl ActionServer where - T: rosidl_runtime_rs::Action, + T: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, { /// Creates a new action server. pub(crate) fn new( @@ -95,7 +95,7 @@ where accepted_callback: impl Fn(ServerGoalHandle) + 'static + Send + Sync, ) -> Result where - T: rosidl_runtime_rs::Action, + T: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, { // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_action_server = unsafe { rcl_action_get_zero_initialized_server() }; @@ -335,7 +335,7 @@ where impl ActionServerBase for ActionServer where - T: rosidl_runtime_rs::Action, + T: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, { fn handle(&self) -> &ActionServerHandle { &self.handle diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index b00280cf5..a42f9bf05 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -323,7 +323,7 @@ impl NodeState { handle_accepted: AcceptedCallback, ) -> Result>, RclrsError> where - ActionT: rosidl_runtime_rs::Action, + ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, GoalCallback: Fn(GoalUuid, ::Goal) -> GoalResponse + 'static + Send + Sync, CancelCallback: Fn(ServerGoalHandle) -> CancelResponse + 'static + Send + Sync, AcceptedCallback: Fn(ServerGoalHandle) + 'static + Send + Sync, From b238d67c00211d195603e98dff77117daa847a9e Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 9 Aug 2024 17:15:32 -0400 Subject: [PATCH 40/52] Add ActionServer::publish_feedback() method This still needs to be hooked up to the ActionServerGoalHandle, though. --- rclrs/src/action/server.rs | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 8c9656ee2..7db6f9f2b 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -331,6 +331,22 @@ where } .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); + 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() + } } impl ActionServerBase for ActionServer From de5b094f02385a63ee6e51e22ec9300ec4cc2690 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 9 Aug 2024 17:55:57 -0400 Subject: [PATCH 41/52] Implement goal expiration in action server --- rclrs/src/action/server.rs | 27 ++++++++++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 7db6f9f2b..aec5c3b1d 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -298,7 +298,32 @@ where } fn execute_goal_expired(&self) -> Result<(), RclrsError> { - todo!() + // 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_handles.lock().unwrap().remove(&uuid); + } else { + break + } + } + + Ok(()) } fn publish_status(&self) -> Result<(), RclrsError> { From 2fd278fe6212d94bd177f069421c4eb4d28e9650 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 9 Aug 2024 20:37:22 -0400 Subject: [PATCH 42/52] Implement goal cancel requests --- rclrs/src/action.rs | 1 + rclrs/src/action/server.rs | 158 ++++++++++++++++++++++++- rclrs/src/action/server_goal_handle.rs | 9 ++ 3 files changed, 165 insertions(+), 3 deletions(-) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index 7c6b23539..ad91e4f35 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -48,6 +48,7 @@ pub enum GoalResponse { } /// 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, diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index aec5c3b1d..83987f4d5 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -275,7 +275,10 @@ where self.send_goal_response(request_id, true)?; - self.goal_handles.lock().unwrap().insert(uuid, Arc::clone(&goal_handle)); + self.goal_handles + .lock() + .unwrap() + .insert(uuid, Arc::clone(&goal_handle)); if response == GoalResponse::AcceptAndExecute { goal_handle.execute()?; @@ -289,8 +292,157 @@ where 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> { - todo!() + 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 execute_result_request(&self) -> Result<(), RclrsError> { @@ -319,7 +471,7 @@ where let uuid = GoalUuid(expired_goal.goal_id.uuid); self.goal_handles.lock().unwrap().remove(&uuid); } else { - break + break; } } diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs index a91627906..43d59b606 100644 --- a/rclrs/src/action/server_goal_handle.rs +++ b/rclrs/src/action/server_goal_handle.rs @@ -91,6 +91,15 @@ where 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, From cbf4ff69c84fdfc8acdf9d1ecbe33e41cb79c53b Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 16 Aug 2024 18:05:35 -0400 Subject: [PATCH 43/52] Implement action result requests in the action server --- rclrs/src/action/server.rs | 110 ++++++++++++++++++++++++++++++++++++- 1 file changed, 107 insertions(+), 3 deletions(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 83987f4d5..910176cba 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -71,14 +71,18 @@ pub type AcceptedCallback = dyn Fn(ServerGoalHandle) + 'static pub struct ActionServer where - ActionT: rosidl_runtime_rs::Action, + 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>>, } impl ActionServer @@ -160,6 +164,8 @@ where 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()), }) } @@ -172,7 +178,9 @@ where let mut request_rmw = RmwRequest::::default(); let handle = &*self.handle.lock(); unsafe { - // SAFETY: The three pointers are valid/initialized + // 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, @@ -445,8 +453,104 @@ where 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> { - todo!() + 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 { + type RmwResponse = <<::GetResultService as rosidl_runtime_rs::Service>::Response as Message>::RmwMsg; + let mut response_rmw = RmwResponse::::default(); + // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_UNKNOWN in the rcl + // bindings. + ::set_result_response_status(&mut response_rmw, 0); + self.send_result_response(request_id, &mut response_rmw)?; + } + + Ok(()) } fn execute_goal_expired(&self) -> Result<(), RclrsError> { From 24cc6116d1728d064b4347828c7f7db8f1d8f172 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 23 Aug 2024 16:31:22 -0400 Subject: [PATCH 44/52] Fix formatting in example --- examples/minimal_action_server/src/minimal_action_server.rs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/examples/minimal_action_server/src/minimal_action_server.rs b/examples/minimal_action_server/src/minimal_action_server.rs index 94c119c4f..781544cec 100644 --- a/examples/minimal_action_server/src/minimal_action_server.rs +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -1,7 +1,6 @@ use anyhow::{Error, Result}; use rclrs::*; -use std::sync::Arc; -use std::thread; +use std::{env, sync::Arc, thread}; type Fibonacci = example_interfaces::action::Fibonacci; type GoalHandleFibonacci = rclrs::ServerGoalHandle; From 04b8c58394811f03f859530dab0face503be9e9b Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 23 Aug 2024 17:29:10 -0400 Subject: [PATCH 45/52] Hook up ServerGoalHandle callbacks into the ActionServer These aren't actually implemented as callbacks like in rclcpp, but rather just regular method calls. This required making some of the ActionServer and ActionServerBase methods use an Arc receiver, but the ActionServer is only ever created within an Arc, so this is fine. --- rclrs/src/action/server.rs | 9 ++++---- rclrs/src/action/server_goal_handle.rs | 29 +++++++++++++++++--------- 2 files changed, 24 insertions(+), 14 deletions(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 910176cba..b9f78d7c6 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -55,7 +55,7 @@ pub trait ActionServerBase: Send + Sync { /// 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, mode: ReadyMode) -> Result<(), RclrsError>; + fn execute(self: Arc, mode: ReadyMode) -> Result<(), RclrsError>; } pub(crate) enum ReadyMode { @@ -228,7 +228,7 @@ where } } - fn execute_goal_request(&self) -> Result<(), RclrsError> { + fn execute_goal_request(self: Arc) -> Result<(), RclrsError> { let (request, request_id) = match self.take_goal_request() { Ok(res) => res, Err(RclrsError::RclError { @@ -275,6 +275,7 @@ where } else { Arc::new(ServerGoalHandle::::new( goal_handle_ptr, + Arc::downgrade(&self), todo!("Create an Arc holding the goal message"), uuid, )) @@ -582,7 +583,7 @@ where Ok(()) } - fn publish_status(&self) -> Result<(), RclrsError> { + pub(crate) fn publish_status(&self) -> Result<(), RclrsError> { let mut goal_statuses = DropGuard::new( unsafe { // SAFETY: No preconditions @@ -642,7 +643,7 @@ where &self.num_entities } - fn execute(&self, mode: ReadyMode) -> Result<(), RclrsError> { + fn execute(self: Arc, mode: ReadyMode) -> Result<(), RclrsError> { match mode { ReadyMode::GoalRequest => self.execute_goal_request(), ReadyMode::CancelRequest => self.execute_cancel_request(), diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs index 43d59b606..ab93a5c7d 100644 --- a/rclrs/src/action/server_goal_handle.rs +++ b/rclrs/src/action/server_goal_handle.rs @@ -1,5 +1,5 @@ -use crate::{rcl_bindings::*, GoalUuid, RclrsError, ToResult}; -use std::sync::{Arc, Mutex}; +use crate::{action::ActionServer, rcl_bindings::*, GoalUuid, RclrsError, ToResult}; +use std::sync::{Arc, Mutex, Weak}; // Values defined by `action_msgs/msg/GoalStatus` #[repr(i8)] @@ -22,9 +22,10 @@ enum GoalStatus { /// passed to the user in the associated `handle_accepted` callback. pub struct ServerGoalHandle where - ActionT: rosidl_runtime_rs::Action, + 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, } @@ -34,20 +35,22 @@ where // 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 {} -unsafe impl Sync for ServerGoalHandle where ActionT: rosidl_runtime_rs::Action {} +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, + 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, } @@ -148,7 +151,10 @@ where pub fn execute(&self) -> Result<(), RclrsError> { self.update_state(rcl_action_goal_event_t::GOAL_EVENT_EXECUTE)?; - // TODO: Invoke on_executing callback + // Publish the state change. + if let Some(action_server) = self.action_server.upgrade() { + action_server.publish_status()?; + } Ok(()) } @@ -188,14 +194,17 @@ where /// /// Returns an error if the goal is in any state other than executing. pub fn publish_feedback(&self, feedback: Arc) -> Result<(), RclrsError> { - // TODO: Invoke public_feedback callback - todo!() + // 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, + 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) { From 1d6cc38561e21acc5fc4b56b67b6f5834430b859 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 23 Aug 2024 18:35:04 -0400 Subject: [PATCH 46/52] Switch to create_result_response() in rclrs --- rclrs/src/action/server.rs | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index b9f78d7c6..b0e2c1c99 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -543,11 +543,10 @@ where self.result_requests.lock().unwrap().entry(uuid).or_insert(vec![]).push(request_id); } } else { - type RmwResponse = <<::GetResultService as rosidl_runtime_rs::Service>::Response as Message>::RmwMsg; - let mut response_rmw = RmwResponse::::default(); // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_UNKNOWN in the rcl // bindings. - ::set_result_response_status(&mut response_rmw, 0); + let null_response = ::RmwMsg::default(); + let mut response_rmw = ::create_result_response(0, null_response); self.send_result_response(request_id, &mut response_rmw)?; } From 94b517c00b1b26803d8823a9bf35ee9fcef264db Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 23 Aug 2024 19:23:50 -0400 Subject: [PATCH 47/52] Hook up goal termination methods for goal handles These now call into the action server to send a response to prior (queued) and future goal result requests. There are still some synchronization tweaks needed for this to be correct. --- rclrs/src/action/server.rs | 56 ++++++++++++++++++++++++++ rclrs/src/action/server_goal_handle.rs | 28 +++++++++++-- 2 files changed, 80 insertions(+), 4 deletions(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index b0e2c1c99..2e5c51405 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -573,6 +573,8 @@ where if num_expired > 0 { // Clean up the expired goal. let uuid = GoalUuid(expired_goal.goal_id.uuid); + self.goal_results.lock().unwrap().remove(&uuid); + self.result_requests.lock().unwrap().remove(&uuid); self.goal_handles.lock().unwrap().remove(&uuid); } else { break; @@ -582,6 +584,29 @@ where Ok(()) } + // TODO(nwn): Replace `status` with a "properly typed" action_msgs::msg::GoalStatus enum. + pub(crate) fn terminate_goal(&self, goal_id: &GoalUuid, status: i8, result: ::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 { @@ -628,6 +653,37 @@ where } .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 ActionServer diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs index ab93a5c7d..3ee47d086 100644 --- a/rclrs/src/action/server_goal_handle.rs +++ b/rclrs/src/action/server_goal_handle.rs @@ -112,7 +112,12 @@ where pub fn abort(&self, result: &ActionT::Result) -> Result<(), RclrsError> { self.update_state(rcl_action_goal_event_t::GOAL_EVENT_ABORT)?; - // TODO: Invoke on_terminal_state callback + 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(()) } @@ -125,7 +130,12 @@ where pub fn succeed(&self, result: &ActionT::Result) -> Result<(), RclrsError> { self.update_state(rcl_action_goal_event_t::GOAL_EVENT_SUCCEED)?; - // TODO: Invoke on_terminal_state callback + 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(()) } @@ -138,7 +148,12 @@ where pub fn canceled(&self, result: &ActionT::Result) -> Result<(), RclrsError> { self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCELED)?; - // TODO: Invoke on_terminal_state callback + 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(()) } @@ -209,7 +224,12 @@ where /// Cancel the goal if its handle is dropped without reaching a terminal state. fn drop(&mut self) { if self.try_canceling() == Ok(true) { - // TODO: Invoke on_terminal_state callback + 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(); From 53ebbbebbd849c6a6d102cc1479e5900b61a29c9 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 28 Sep 2024 15:02:03 -0400 Subject: [PATCH 48/52] Implement client-side trait methods for action messages This adds methods to ActionImpl for creating and accessing action-specific message types. These are needed by the rclrs ActionClient to generically read and write RMW messages. Due to issues with qualified paths in certain places (https://github.com/rust-lang/rust/issues/86935), the generator now refers directly to its service and message types rather than going through associated types of the various traits. This also makes the generated code a little easier to read, with the trait method signatures from rosidl_runtime_rs still enforcing type-safety. --- rclrs/src/action/server.rs | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 2e5c51405..4028c30ec 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -197,9 +197,7 @@ where mut request_id: rmw_request_id_t, accepted: bool, ) -> Result<(), RclrsError> { - type RmwResponse = <<::SendGoalService as Service>::Response as Message>::RmwMsg; - let mut response_rmw = RmwResponse::::default(); - ::set_goal_response_accepted(&mut response_rmw, accepted); + 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 @@ -210,7 +208,7 @@ where rcl_action_send_goal_response( handle, &mut request_id, - &mut response_rmw as *mut RmwResponse as *mut _, + &mut response_rmw as *mut _ as *mut _, ) } .ok(); @@ -242,7 +240,7 @@ where Err(err) => return Err(err), }; - let uuid = GoalUuid(::get_goal_request_uuid(&request)); + let uuid = GoalUuid(*::get_goal_request_uuid(&request)); let response: GoalResponse = { todo!("Optionally convert request to an idiomatic type for the user's callback."); @@ -522,7 +520,7 @@ where Err(err) => return Err(err), }; - let uuid = GoalUuid(::get_result_request_uuid(&request)); + let uuid = GoalUuid(*::get_result_request_uuid(&request)); let goal_exists = unsafe { // SAFETY: No preconditions @@ -640,7 +638,7 @@ where 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); + 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 From 3e8d325c60810c35d62852da6b1c0a5347833082 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 7 Jun 2025 16:41:11 -0400 Subject: [PATCH 49/52] Fix compilation after rebase Just the bare minimum to get things working after the rebase. Lots of clean-up still to do. --- .../src/minimal_action_server.rs | 15 +++++++-------- rclrs/src/action/server.rs | 8 ++++---- rclrs/src/action/server_goal_handle.rs | 4 ++-- rclrs/src/node.rs | 4 ++-- 4 files changed, 15 insertions(+), 16 deletions(-) diff --git a/examples/minimal_action_server/src/minimal_action_server.rs b/examples/minimal_action_server/src/minimal_action_server.rs index 781544cec..3af90c3a4 100644 --- a/examples/minimal_action_server/src/minimal_action_server.rs +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -1,13 +1,13 @@ use anyhow::{Error, Result}; use rclrs::*; -use std::{env, sync::Arc, thread}; +use std::{sync::Arc, thread}; type Fibonacci = example_interfaces::action::Fibonacci; type GoalHandleFibonacci = rclrs::ServerGoalHandle; fn handle_goal( - _uuid: &rclrs::GoalUUID, - goal: Arc, + _uuid: rclrs::GoalUuid, + goal: example_interfaces::action::Fibonacci_Goal, ) -> rclrs::GoalResponse { println!("Received goal request with order {}", goal.order); if goal.order > 9000 { @@ -24,11 +24,11 @@ fn handle_cancel(_goal_handle: Arc) -> rclrs::CancelRespons fn execute(goal_handle: Arc) { println!("Executing goal"); - let feedback = example_interfaces::action::Fibonacci_Feedback { + let mut feedback = example_interfaces::action::Fibonacci_Feedback { sequence: [0, 1].to_vec(), }; - for i in 1..goal_handle.goal_request.order { + for i in 1..goal_handle.goal().order { if goal_handle.is_canceling() { let result = example_interfaces::action::Fibonacci_Result { sequence: Vec::new(), @@ -50,9 +50,8 @@ fn execute(goal_handle: Arc) { } let result = example_interfaces::action::Fibonacci_Result { - sequence: Vec::new(), + sequence: feedback.sequence, }; - result.sequence = feedback.sequence.clone(); goal_handle.succeed(&result); println!("Goal succeeded"); } @@ -68,7 +67,7 @@ fn main() -> Result<(), Error> { let node = executor.create_node("minimal_action_server")?; - let _action_server = node.create_action_server::( + let _action_server = node.create_action_server::( "fibonacci", handle_goal, handle_cancel, diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 4028c30ec..ee1204c2b 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -66,8 +66,8 @@ pub(crate) enum ReadyMode { } pub type GoalCallback = dyn Fn(GoalUuid, ::Goal) -> GoalResponse + 'static + Send + Sync; -pub type CancelCallback = dyn Fn(ServerGoalHandle) -> CancelResponse + 'static + Send + Sync; -pub type AcceptedCallback = dyn Fn(ServerGoalHandle) + 'static + Send + Sync; +pub type CancelCallback = dyn Fn(Arc>) -> CancelResponse + 'static + Send + Sync; +pub type AcceptedCallback = dyn Fn(Arc>) + 'static + Send + Sync; pub struct ActionServer where @@ -95,8 +95,8 @@ where clock: Clock, topic: &str, goal_callback: impl Fn(GoalUuid, T::Goal) -> GoalResponse + 'static + Send + Sync, - cancel_callback: impl Fn(ServerGoalHandle) -> CancelResponse + 'static + Send + Sync, - accepted_callback: impl Fn(ServerGoalHandle) + '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, diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs index 3ee47d086..640e0e005 100644 --- a/rclrs/src/action/server_goal_handle.rs +++ b/rclrs/src/action/server_goal_handle.rs @@ -208,10 +208,10 @@ where /// 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: Arc) -> Result<(), RclrsError> { + 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)?; + action_server.publish_feedback(&self.uuid, feedback)?; } Ok(()) } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index a42f9bf05..80af394de 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -325,8 +325,8 @@ impl NodeState { where ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, GoalCallback: Fn(GoalUuid, ::Goal) -> GoalResponse + 'static + Send + Sync, - CancelCallback: Fn(ServerGoalHandle) -> CancelResponse + 'static + Send + Sync, - AcceptedCallback: Fn(ServerGoalHandle) + 'static + Send + Sync, + CancelCallback: Fn(Arc>) -> CancelResponse + 'static + Send + Sync, + AcceptedCallback: Fn(Arc>) + 'static + Send + Sync, { let action_server = Arc::new(ActionServer::::new( self, From fb06971b2625d517f83dd41b4e6b62d59ca6f370 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 7 Jun 2025 18:11:10 -0400 Subject: [PATCH 50/52] Fix linker error When compiling the examples that use the action client/server, you got "undefined reference" errors for any `rcl_action_*` functions during the linking phase. Telling rustc to link against the rcl_action shared library fixes this. --- rclrs/build.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/rclrs/build.rs b/rclrs/build.rs index 04927ff09..f5bc733ef 100644 --- a/rclrs/build.rs +++ b/rclrs/build.rs @@ -116,6 +116,7 @@ fn main() { } println!("cargo:rustc-link-lib=dylib=rcl"); + println!("cargo:rustc-link-lib=dylib=rcl_action"); println!("cargo:rustc-link-lib=dylib=rcl_yaml_param_parser"); println!("cargo:rustc-link-lib=dylib=rcutils"); println!("cargo:rustc-link-lib=dylib=rmw"); From dbdebd10f0cce025c2c4d3a8d66497f036dc120b Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 7 Jun 2025 19:26:29 -0400 Subject: [PATCH 51/52] Switch to shared state for action client/server This matches the pattern used by the other endpoint structs. --- rclrs/src/action.rs | 4 +-- rclrs/src/action/client.rs | 42 +++++++++++++++++----- rclrs/src/action/server.rs | 49 +++++++++++++++++++++----- rclrs/src/action/server_goal_handle.rs | 6 ++-- rclrs/src/node.rs | 23 ++++++------ 5 files changed, 90 insertions(+), 34 deletions(-) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index ad91e4f35..63f3e04e8 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -5,8 +5,8 @@ mod server_goal_handle; use crate::rcl_bindings::RCL_ACTION_UUID_SIZE; use std::fmt; -pub use client::{ActionClient, ActionClientBase}; -pub use server::{ActionServer, ActionServerBase}; +pub use client::{ActionClient, ActionClientBase, ActionClientState}; +pub use server::{ActionServer, ActionServerBase, ActionServerState}; pub use server_goal_handle::ServerGoalHandle; /// A unique identifier for a goal request. diff --git a/rclrs/src/action/client.rs b/rclrs/src/action/client.rs index 38d9fb303..34c52485f 100644 --- a/rclrs/src/action/client.rs +++ b/rclrs/src/action/client.rs @@ -1,5 +1,5 @@ use crate::{ - error::ToResult, rcl_bindings::*, wait::WaitableNumEntities, Node, RclrsError, + error::ToResult, rcl_bindings::*, wait::WaitableNumEntities, Node, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; use std::{ @@ -19,7 +19,7 @@ unsafe impl Send for rcl_action_client_t {} /// [1]: pub struct ActionClientHandle { rcl_action_client: Mutex, - node: Node, + node_handle: Arc, pub(crate) in_use_by_wait_set: Arc, } @@ -32,7 +32,7 @@ impl ActionClientHandle { 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 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. @@ -62,16 +62,41 @@ pub(crate) enum ReadyMode { ResultResponse, } -pub struct ActionClient +/// +/// 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 ActionClient +impl ActionClientState where T: rosidl_runtime_rs::Action, { @@ -97,7 +122,7 @@ where // SAFETY: // * The rcl_action_client was zero-initialized as expected by this function. - // * The rcl_node is kept alive by the Node because it is a dependency of the action client. + // * The rcl_node is kept alive by the NodeHandle because it is a dependency of the action client. // * The topic 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 @@ -116,7 +141,7 @@ where let handle = Arc::new(ActionClientHandle { rcl_action_client: Mutex::new(rcl_action_client), - node: node.clone(), + node_handle: Arc::clone(&node.handle), in_use_by_wait_set: Arc::new(AtomicBool::new(false)), }); @@ -137,6 +162,7 @@ where _marker: Default::default(), handle, num_entities, + node: Arc::clone(node), }) } @@ -161,7 +187,7 @@ where } } -impl ActionClientBase for ActionClient +impl ActionClientBase for ActionClientState where T: rosidl_runtime_rs::Action, { diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index ee1204c2b..787900034 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -3,7 +3,7 @@ use crate::{ error::{RclReturnCode, ToResult}, rcl_bindings::*, wait::WaitableNumEntities, - Clock, DropGuard, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX, + Clock, DropGuard, Node, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; use rosidl_runtime_rs::{Action, ActionImpl, Message, Service}; use std::{ @@ -23,7 +23,7 @@ unsafe impl Send for rcl_action_server_t {} /// [1]: pub struct ActionServerHandle { rcl_action_server: Mutex, - node: Node, + node_handle: Arc, pub(crate) in_use_by_wait_set: Arc, } @@ -36,7 +36,7 @@ impl ActionServerHandle { 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 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. @@ -69,7 +69,33 @@ pub type GoalCallback = dyn Fn(GoalUuid, = dyn Fn(Arc>) -> CancelResponse + 'static + Send + Sync; pub type AcceptedCallback = dyn Fn(Arc>) + 'static + Send + Sync; -pub struct ActionServer +/// 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, { @@ -83,16 +109,19 @@ where 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 ActionServer +impl ActionServerState where T: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, { /// Creates a new action server. pub(crate) fn new( node: &Node, - clock: Clock, topic: &str, goal_callback: impl Fn(GoalUuid, T::Goal) -> GoalResponse + 'static + Send + Sync, cancel_callback: impl Fn(Arc>) -> CancelResponse + 'static + Send + Sync, @@ -114,13 +143,14 @@ where { 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 Node because it is a dependency of the action server. + // * The rcl_node is kept alive by the NodeHandle because it is a dependency of the action server. // * The topic 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 @@ -140,7 +170,7 @@ where let handle = Arc::new(ActionServerHandle { rcl_action_server: Mutex::new(rcl_action_server), - node: node.clone(), + node_handle: Arc::clone(&node.handle), in_use_by_wait_set: Arc::new(AtomicBool::new(false)), }); @@ -166,6 +196,7 @@ where goal_handles: Mutex::new(HashMap::new()), goal_results: Mutex::new(HashMap::new()), result_requests: Mutex::new(HashMap::new()), + node: node.clone(), }) } @@ -684,7 +715,7 @@ where } } -impl ActionServerBase for ActionServer +impl ActionServerBase for ActionServerState where T: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, { diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs index 640e0e005..6910f8d5e 100644 --- a/rclrs/src/action/server_goal_handle.rs +++ b/rclrs/src/action/server_goal_handle.rs @@ -1,4 +1,4 @@ -use crate::{action::ActionServer, rcl_bindings::*, GoalUuid, RclrsError, ToResult}; +use crate::{action::ActionServerState, rcl_bindings::*, GoalUuid, RclrsError, ToResult}; use std::sync::{Arc, Mutex, Weak}; // Values defined by `action_msgs/msg/GoalStatus` @@ -25,7 +25,7 @@ where ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, { rcl_handle: Mutex<*mut rcl_action_goal_handle_t>, - action_server: Weak>, + action_server: Weak>, goal_request: Arc, uuid: GoalUuid, } @@ -44,7 +44,7 @@ where { pub(crate) fn new( rcl_handle: *mut rcl_action_goal_handle_t, - action_server: Weak>, + action_server: Weak>, goal_request: Arc, uuid: GoalUuid, ) -> Self { diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 80af394de..d33266d9c 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -19,13 +19,13 @@ use std::{ use rosidl_runtime_rs::Message; use crate::{ - rcl_bindings::*, ActionClient, ActionClientBase, ActionServer, ActionServerBase, - CancelResponse, Client, ClientBase, ClientOptions, ClientState, Clock, ContextHandle, - GoalResponse, GoalUuid, GuardCondition, LogParams, Logger, ParameterBuilder, - ParameterInterface, ParameterVariant, Parameters, Publisher, PublisherOptions, PublisherState, - RclrsError, ServerGoalHandle, Service, ServiceBase, ServiceOptions, ServiceState, Subscription, - SubscriptionBase, SubscriptionCallback, SubscriptionOptions, SubscriptionState, TimeSource, - ToLogParams, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, ActionClient, ActionClientBase, ActionClientState, ActionServer, + ActionServerBase, ActionServerState, CancelResponse, Client, ClientBase, ClientOptions, + ClientState, Clock, ContextHandle, GoalResponse, GoalUuid, GuardCondition, LogParams, Logger, + ParameterBuilder, ParameterInterface, ParameterVariant, 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 @@ -299,11 +299,11 @@ impl NodeState { pub fn create_action_client( self: &Arc, topic: &str, - ) -> Result>, RclrsError> + ) -> Result, RclrsError> where T: rosidl_runtime_rs::Action, { - let action_client = Arc::new(ActionClient::::new(self, topic)?); + let action_client = Arc::new(ActionClientState::::new(self, topic)?); self.action_clients_mtx .lock() .unwrap() @@ -321,16 +321,15 @@ impl NodeState { handle_goal: GoalCallback, handle_cancel: CancelCallback, handle_accepted: AcceptedCallback, - ) -> Result>, RclrsError> + ) -> 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(ActionServer::::new( + let action_server = Arc::new(ActionServerState::::new( self, - self.get_clock(), topic, handle_goal, handle_cancel, From 8973176fc61f03edae3aa2f733596d800ddf3c5d Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 7 Jun 2025 20:27:40 -0400 Subject: [PATCH 52/52] Use option struct for create_action_{client,server} arguments This works similarly to the option struct used for the other endpoint types, except that it cannot use the PrimitiveOptions stuff since actions have multiple different QoS settings. --- rclrs/src/action.rs | 4 +-- rclrs/src/action/client.rs | 65 ++++++++++++++++++++++++++++++++------ rclrs/src/action/server.rs | 63 ++++++++++++++++++++++++++++++------ rclrs/src/node.rs | 27 ++++++++-------- rclrs/src/qos.rs | 19 +++++++++++ 5 files changed, 145 insertions(+), 33 deletions(-) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index 63f3e04e8..4432beac8 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -5,8 +5,8 @@ mod server_goal_handle; use crate::rcl_bindings::RCL_ACTION_UUID_SIZE; use std::fmt; -pub use client::{ActionClient, ActionClientBase, ActionClientState}; -pub use server::{ActionServer, ActionServerBase, ActionServerState}; +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. diff --git a/rclrs/src/action/client.rs b/rclrs/src/action/client.rs index 34c52485f..556fd1a02 100644 --- a/rclrs/src/action/client.rs +++ b/rclrs/src/action/client.rs @@ -1,8 +1,9 @@ use crate::{ - error::ToResult, rcl_bindings::*, wait::WaitableNumEntities, Node, NodeHandle, RclrsError, - ENTITY_LIFECYCLE_MUTEX, + 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}, @@ -101,17 +102,22 @@ where T: rosidl_runtime_rs::Action, { /// Creates a new action client. - pub(crate) fn new(node: &Node, topic: &str) -> Result + 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 topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { - err, - s: topic.into(), - })?; + 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() }; @@ -123,7 +129,7 @@ where // 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 topic name and the options are copied by this function, so they can be dropped + // * 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. @@ -132,7 +138,7 @@ where &mut rcl_action_client, &mut *rcl_node, type_support, - topic_c_string.as_ptr(), + action_name_c_string.as_ptr(), &action_client_options, ) .ok()?; @@ -209,3 +215,44 @@ where } } } + +/// `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 index 787900034..949af77f4 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -3,10 +3,11 @@ use crate::{ error::{RclReturnCode, ToResult}, rcl_bindings::*, wait::WaitableNumEntities, - Clock, DropGuard, Node, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + 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}, @@ -120,9 +121,9 @@ where T: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, { /// Creates a new action server. - pub(crate) fn new( + pub(crate) fn new<'a>( node: &Node, - topic: &str, + 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, @@ -130,13 +131,15 @@ where 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 topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { - err, - s: topic.into(), - })?; + 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() }; @@ -151,7 +154,7 @@ where // 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 topic name and the options are copied by this function, so they can be dropped + // * 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. @@ -161,7 +164,7 @@ where &mut *rcl_node, &mut *rcl_clock, type_support, - topic_c_string.as_ptr(), + action_name_c_string.as_ptr(), &action_server_options, ) .ok()?; @@ -736,3 +739,45 @@ where } } } + +/// `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/node.rs b/rclrs/src/node.rs index d33266d9c..c7ac5507d 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -19,13 +19,14 @@ use std::{ use rosidl_runtime_rs::Message; use crate::{ - rcl_bindings::*, ActionClient, ActionClientBase, ActionClientState, ActionServer, - ActionServerBase, ActionServerState, CancelResponse, Client, ClientBase, ClientOptions, - ClientState, Clock, ContextHandle, GoalResponse, GoalUuid, GuardCondition, LogParams, Logger, - ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, - PublisherOptions, PublisherState, RclrsError, ServerGoalHandle, Service, ServiceBase, - ServiceOptions, ServiceState, Subscription, SubscriptionBase, SubscriptionCallback, - SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams, ENTITY_LIFECYCLE_MUTEX, + 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, 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 @@ -296,14 +297,14 @@ impl NodeState { /// /// [1]: crate::ActionClient // TODO: make action client's lifetime depend on node's lifetime - pub fn create_action_client( + pub fn create_action_client<'a, T>( self: &Arc, - topic: &str, + options: impl Into>, ) -> Result, RclrsError> where T: rosidl_runtime_rs::Action, { - let action_client = Arc::new(ActionClientState::::new(self, topic)?); + let action_client = Arc::new(ActionClientState::::new(self, options)?); self.action_clients_mtx .lock() .unwrap() @@ -315,9 +316,9 @@ impl NodeState { /// /// [1]: crate::ActionServer // TODO: make action server's lifetime depend on node's lifetime - pub fn create_action_server( + pub fn create_action_server<'a, ActionT, GoalCallback, CancelCallback, AcceptedCallback>( self: &Arc, - topic: &str, + options: impl Into>, handle_goal: GoalCallback, handle_cancel: CancelCallback, handle_accepted: AcceptedCallback, @@ -330,7 +331,7 @@ impl NodeState { { let action_server = Arc::new(ActionServerState::::new( self, - topic, + options, handle_goal, handle_cancel, handle_accepted, diff --git a/rclrs/src/qos.rs b/rclrs/src/qos.rs index dcde59023..dcaa0b30f 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, +};