From f6ced1ee04cc727694fc6f846093cf5e78a4b420 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 28 Nov 2022 16:39:43 +0100 Subject: [PATCH 01/61] Added skeleton examples for action client and server --- .../minimal_action_client_server/Cargo.toml | 29 +++++++++++++++++++ .../minimal_action_client_server/package.xml | 26 +++++++++++++++++ .../src/minimal_action_client.rs | 11 +++++++ .../src/minimal_action_server.rs | 11 +++++++ 4 files changed, 77 insertions(+) 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.toml b/examples/minimal_action_client_server/Cargo.toml new file mode 100644 index 000000000..bbcb3d324 --- /dev/null +++ b/examples/minimal_action_client_server/Cargo.toml @@ -0,0 +1,29 @@ +[package] +name = "examples_rclrs_minimal_action_client_server" +version = "0.3.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] +anyhow = {version = "1", features = ["backtrace"]} + +[dependencies.rclrs] +version = "0.3" + +[dependencies.rosidl_runtime_rs] +version = "0.3" + +[dependencies.action_msgs] +version = "*" + +[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..d1d32f3b1 --- /dev/null +++ b/examples/minimal_action_client_server/src/minimal_action_client.rs @@ -0,0 +1,11 @@ +use std::env; + +use anyhow::{Error, Result}; + +fn main() -> Result<(), Error> { + let context = rclrs::Context::new(env::args())?; + + let _node = rclrs::create_node(&context, "minimal_action_client")?; + + Ok(()) +} 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..4a1995d9a --- /dev/null +++ b/examples/minimal_action_client_server/src/minimal_action_server.rs @@ -0,0 +1,11 @@ +use std::env; + +use anyhow::{Error, Result}; + +fn main() -> Result<(), Error> { + let context = rclrs::Context::new(env::args())?; + + let mut node = rclrs::create_node(&context, "minimal_action_server")?; + + rclrs::spin(&node).map_err(|err| err.into()) +} From f27904186bc11c72e103b8714b1115b6a24bf282 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 28 Nov 2022 17:18:07 +0100 Subject: [PATCH 02/61] Added action template --- examples/minimal_action_client_server/Cargo.toml | 3 --- .../src/minimal_action_server.rs | 2 +- .../rosidl_generator_rs_generate_interfaces.cmake | 11 ++++++----- rosidl_generator_rs/resource/action.rs.em | 9 +++++++++ 4 files changed, 16 insertions(+), 9 deletions(-) create mode 100644 rosidl_generator_rs/resource/action.rs.em diff --git a/examples/minimal_action_client_server/Cargo.toml b/examples/minimal_action_client_server/Cargo.toml index bbcb3d324..4f51278fc 100644 --- a/examples/minimal_action_client_server/Cargo.toml +++ b/examples/minimal_action_client_server/Cargo.toml @@ -24,6 +24,3 @@ version = "0.3" [dependencies.action_msgs] version = "*" - -[package.metadata.ros] -install_to_share = ["launch"] diff --git a/examples/minimal_action_client_server/src/minimal_action_server.rs b/examples/minimal_action_client_server/src/minimal_action_server.rs index 4a1995d9a..bf05587b6 100644 --- a/examples/minimal_action_client_server/src/minimal_action_server.rs +++ b/examples/minimal_action_client_server/src/minimal_action_server.rs @@ -5,7 +5,7 @@ use anyhow::{Error, Result}; fn main() -> Result<(), Error> { let context = rclrs::Context::new(env::args())?; - let mut node = rclrs::create_node(&context, "minimal_action_server")?; + let node = rclrs::create_node(&context, "minimal_action_server")?; rclrs::spin(&node).map_err(|err| err.into()) } diff --git a/rosidl_generator_rs/cmake/rosidl_generator_rs_generate_interfaces.cmake b/rosidl_generator_rs/cmake/rosidl_generator_rs_generate_interfaces.cmake index 9cfdfa579..1dc47514f 100644 --- a/rosidl_generator_rs/cmake/rosidl_generator_rs_generate_interfaces.cmake +++ b/rosidl_generator_rs/cmake/rosidl_generator_rs_generate_interfaces.cmake @@ -37,13 +37,13 @@ foreach(_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES}) if(_parent_folder STREQUAL "msg") set(_has_msg TRUE) - set(_idl_file_without_actions ${_idl_file_without_actions} ${_idl_file}) + set(_idl_files ${_idl_files} ${_idl_file}) elseif(_parent_folder STREQUAL "srv") set(_has_srv TRUE) - set(_idl_file_without_actions ${_idl_file_without_actions} ${_idl_file}) + set(_idl_files ${_idl_files} ${_idl_file}) elseif(_parent_folder STREQUAL "action") set(_has_action TRUE) - message(WARNING "Rust actions generation is not implemented") + set(_idl_files ${_idl_files} ${_idl_file}) else() message(FATAL_ERROR "Interface file with unknown parent folder: ${_idl_file}") endif() @@ -81,12 +81,13 @@ endforeach() set(target_dependencies "${rosidl_generator_rs_BIN}" ${rosidl_generator_rs_GENERATOR_FILES} + "${rosidl_generator_rs_TEMPLATE_DIR}/action.rs.em" "${rosidl_generator_rs_TEMPLATE_DIR}/msg_idiomatic.rs.em" "${rosidl_generator_rs_TEMPLATE_DIR}/msg_rmw.rs.em" "${rosidl_generator_rs_TEMPLATE_DIR}/msg.rs.em" "${rosidl_generator_rs_TEMPLATE_DIR}/srv.rs.em" ${rosidl_generate_interfaces_ABS_IDL_FILES} - ${_idl_file_without_actions} + ${_idl_files} ${_dependency_files}) foreach(dep ${target_dependencies}) if(NOT EXISTS "${dep}") @@ -99,7 +100,7 @@ rosidl_write_generator_arguments( "${generator_arguments_file}" PACKAGE_NAME "${PROJECT_NAME}" IDL_TUPLES "${rosidl_generate_interfaces_IDL_TUPLES}" - ROS_INTERFACE_FILES "${_idl_file_without_actions}" + ROS_INTERFACE_FILES "${_idl_files}" ROS_INTERFACE_DEPENDENCIES "${_dependencies}" OUTPUT_DIR "${_output_path}" TEMPLATE_DIR "${rosidl_generator_rs_TEMPLATE_DIR}" diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em new file mode 100644 index 000000000..bb74ad772 --- /dev/null +++ b/rosidl_generator_rs/resource/action.rs.em @@ -0,0 +1,9 @@ +@{ +TEMPLATE( + 'msg_idiomatic.rs.em', + package_name=package_name, interface_path=interface_path, + msg_specs=msg_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ From 483164abf1fcb8772757cf3a37955356b25be178 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 28 Nov 2022 17:47:22 +0100 Subject: [PATCH 03/61] Added action generation --- rosidl_generator_rs/resource/action.rs.em | 30 ++++++++++++++++++- .../rosidl_generator_rs/__init__.py | 27 +++++++++++++++++ 2 files changed, 56 insertions(+), 1 deletion(-) diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em index bb74ad772..ff76ff092 100644 --- a/rosidl_generator_rs/resource/action.rs.em +++ b/rosidl_generator_rs/resource/action.rs.em @@ -1,8 +1,36 @@ +@{ +action_msg_specs = [] + +for subfolder, action in action_specs: + action_msg_specs.append((subfolder, action.goal)) + action_msg_specs.append((subfolder, action.result)) + action_msg_specs.append((subfolder, action.feedback)) + action_msg_specs.append((subfolder, action.feedback_message)) + +action_srv_specs = [] + +for subfolder, action in action_specs: + action_srv_specs.append((subfolder, action.send_goal_service)) + action_srv_specs.append((subfolder, action.get_result_service)) +}@ + +pub mod rmw { +@{ +TEMPLATE( + 'msg_rmw.rs.em', + package_name=package_name, interface_path=interface_path, + msg_specs=action_msg_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ +} // mod rmw + @{ TEMPLATE( 'msg_idiomatic.rs.em', package_name=package_name, interface_path=interface_path, - msg_specs=msg_specs, + msg_specs=action_msg_specs, get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) diff --git a/rosidl_generator_rs/rosidl_generator_rs/__init__.py b/rosidl_generator_rs/rosidl_generator_rs/__init__.py index 502d1d34d..b7850a6d8 100644 --- a/rosidl_generator_rs/rosidl_generator_rs/__init__.py +++ b/rosidl_generator_rs/rosidl_generator_rs/__init__.py @@ -23,6 +23,11 @@ import rosidl_pycommon from rosidl_parser.definition import AbstractGenericString +from rosidl_parser.definition import AbstractNestedType +from rosidl_parser.definition import AbstractSequence +from rosidl_parser.definition import AbstractString +from rosidl_parser.definition import AbstractWString +from rosidl_parser.definition import Action from rosidl_parser.definition import Array from rosidl_parser.definition import BasicType from rosidl_parser.definition import BoundedSequence @@ -86,6 +91,10 @@ def generate_rs(generator_arguments_file, typesupport_impls): os.path.join(template_dir, 'srv.rs.em'): ['rust/src/%s.rs'], } + mapping_actions = { + os.path.join(template_dir, 'action.rs.em'): ['rust/src/%s.rs'], + } + # Ensure the required templates exist for template_file in mapping_msgs.keys(): assert os.path.exists(template_file), \ @@ -93,6 +102,9 @@ def generate_rs(generator_arguments_file, typesupport_impls): for template_file in mapping_srvs.keys(): assert os.path.exists(template_file), \ 'Services template file %s not found' % template_file + for template_file in mapping_actions.keys(): + assert os.path.exists(template_file), \ + 'Actions template file %s not found' % template_file data = { 'pre_field_serde': pre_field_serde, @@ -107,6 +119,7 @@ def generate_rs(generator_arguments_file, typesupport_impls): convert_lower_case_underscore_to_camel_case, 'msg_specs': [], 'srv_specs': [], + 'action_specs': [], 'package_name': args['package_name'], 'typesupport_impls': typesupport_impls, 'interface_path': idl_rel_path, @@ -121,6 +134,9 @@ def generate_rs(generator_arguments_file, typesupport_impls): for service in idl_content.get_elements_of_type(Service): data['srv_specs'].append(('srv', service)) + for action in idl_content.get_elements_of_type(Action): + data['action_specs'].append(('action', action)) + if data['msg_specs']: for template_file, generated_filenames in mapping_msgs.items(): for generated_filename in generated_filenames: @@ -143,6 +159,17 @@ def generate_rs(generator_arguments_file, typesupport_impls): generated_file, minimum_timestamp=latest_target_timestamp) + if data['action_specs']: + for template_file, generated_filenames in mapping_actions.items(): + for generated_filename in generated_filenames: + generated_file = os.path.join(args['output_dir'], + generated_filename % 'action') + rosidl_pycommon.expand_template( + os.path.join(template_dir, template_file), + data.copy(), + generated_file, + minimum_timestamp=latest_target_timestamp) + rosidl_pycommon.expand_template( os.path.join(template_dir, 'lib.rs.em'), data.copy(), From f555bbf4b277458b90b47d9b16b2d335befc41ec Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 28 Nov 2022 18:21:03 +0100 Subject: [PATCH 04/61] Added basic create_action_client function --- .../minimal_action_client_server/Cargo.toml | 2 +- .../minimal_action_client_server/package.xml | 4 +-- .../src/minimal_action_client.rs | 4 ++- rclrs/src/action.rs | 28 +++++++++++++++++++ rclrs/src/lib.rs | 2 ++ rclrs/src/node.rs | 28 ++++++++++++++++--- rosidl_generator_rs/resource/action.rs.em | 17 +++++++++++ rosidl_generator_rs/resource/lib.rs.em | 4 +++ rosidl_runtime_rs/src/lib.rs | 2 +- rosidl_runtime_rs/src/traits.rs | 14 ++++++++++ 10 files changed, 96 insertions(+), 9 deletions(-) create mode 100644 rclrs/src/action.rs diff --git a/examples/minimal_action_client_server/Cargo.toml b/examples/minimal_action_client_server/Cargo.toml index 4f51278fc..5224a60c7 100644 --- a/examples/minimal_action_client_server/Cargo.toml +++ b/examples/minimal_action_client_server/Cargo.toml @@ -22,5 +22,5 @@ version = "0.3" [dependencies.rosidl_runtime_rs] version = "0.3" -[dependencies.action_msgs] +[dependencies.example_interfaces] version = "*" 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 d1d32f3b1..5e2ac1d26 100644 --- a/examples/minimal_action_client_server/src/minimal_action_client.rs +++ b/examples/minimal_action_client_server/src/minimal_action_client.rs @@ -5,7 +5,9 @@ use anyhow::{Error, Result}; fn main() -> Result<(), Error> { let context = rclrs::Context::new(env::args())?; - let _node = rclrs::create_node(&context, "minimal_action_client")?; + let mut node = rclrs::create_node(&context, "minimal_client")?; + + let _client = node.create_action_client::("fibonacci")?; Ok(()) } diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs new file mode 100644 index 000000000..0132fc242 --- /dev/null +++ b/rclrs/src/action.rs @@ -0,0 +1,28 @@ +use crate::{rcl_bindings::*, RclrsError}; +use std::sync::{Arc, Mutex, MutexGuard}; + +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(rcl_node_mtx: Arc>, 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 4924b36cb..c3d39e2b1 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; @@ -32,6 +33,7 @@ pub mod dynamic_message; use std::{sync::Arc, time::Duration}; +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 97684d6bc..311fb4598 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -13,10 +13,10 @@ use rosidl_runtime_rs::Message; pub use self::{builder::*, graph::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, - ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, - RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, - TimeSource, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, ActionClient, Client, ClientBase, Clock, Context, ContextHandle, + GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, + QoSProfile, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, + SubscriptionCallback, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -210,6 +210,26 @@ impl Node { 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( + &mut self, + topic: &str, + ) -> Result>, RclrsError> + where + T: rosidl_runtime_rs::Action, + { + let client = Arc::new(ActionClient::::new( + Arc::clone(&self.rcl_node_mtx), + 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. diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em index ff76ff092..40c5acd59 100644 --- a/rosidl_generator_rs/resource/action.rs.em +++ b/rosidl_generator_rs/resource/action.rs.em @@ -35,3 +35,20 @@ TEMPLATE( get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) }@ + +@[for subfolder, action_spec in action_specs] + +@{ +type_name = action_spec.namespaced_type.name +}@ + + // Corresponds to @(package_name)__@(subfolder)__@(type_name) + pub struct @(type_name); + + impl rosidl_runtime_rs::Action for @(type_name) { + type Goal = crate::@(subfolder)::rmw::@(type_name)_Goal; + type Result = crate::@(subfolder)::rmw::@(type_name)_Result; + type Feedback = crate::@(subfolder)::rmw::@(type_name)_Feedback; + } + +@[end for] diff --git a/rosidl_generator_rs/resource/lib.rs.em b/rosidl_generator_rs/resource/lib.rs.em index 51e4a5ba4..79a0e1def 100644 --- a/rosidl_generator_rs/resource/lib.rs.em +++ b/rosidl_generator_rs/resource/lib.rs.em @@ -7,3 +7,7 @@ pub mod msg; @[if len(srv_specs) > 0]@ pub mod srv; @[end if]@ + +@[if len(action_specs) > 0]@ +pub mod action; +@[end if]@ diff --git a/rosidl_runtime_rs/src/lib.rs b/rosidl_runtime_rs/src/lib.rs index 93f844192..01ad9f464 100644 --- a/rosidl_runtime_rs/src/lib.rs +++ b/rosidl_runtime_rs/src/lib.rs @@ -9,4 +9,4 @@ mod string; pub use string::{BoundedString, BoundedWString, String, StringExceedsBoundsError, WString}; mod traits; -pub use traits::{Message, RmwMessage, SequenceAlloc, Service}; +pub use traits::{Action, Message, RmwMessage, SequenceAlloc, Service}; diff --git a/rosidl_runtime_rs/src/traits.rs b/rosidl_runtime_rs/src/traits.rs index 15f206108..dd29fb23e 100644 --- a/rosidl_runtime_rs/src/traits.rs +++ b/rosidl_runtime_rs/src/traits.rs @@ -159,3 +159,17 @@ pub trait Service: 'static { /// Get a pointer to the correct `rosidl_service_type_support_t` structure. fn get_type_support() -> *const std::os::raw::c_void; } + +/// Trait for actions. +/// +/// User code never needs to call this trait's method, much less implement this trait. +pub trait Action: 'static { + /// The goal message associated with this service. + type Goal: Message; + + /// The result message associated with this service. + type Result: Message; + + /// The feedback message associated with this service. + type Feedback: Message; +} From a89c196443929a9256f4e3315cdd2682eedf98c9 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 29 Nov 2022 17:22:33 +0100 Subject: [PATCH 05/61] 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 5e2ac1d26..cb8df7ea5 100644 --- a/examples/minimal_action_client_server/src/minimal_action_client.rs +++ b/examples/minimal_action_client_server/src/minimal_action_client.rs @@ -7,7 +7,8 @@ fn main() -> Result<(), Error> { let mut node = rclrs::create_node(&context, "minimal_client")?; - let _client = node.create_action_client::("fibonacci")?; + let _client = + node.create_action_client::("fibonacci")?; Ok(()) } From 203b252d11936bf34c695019afd8f85be49ce25a Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 17 Jul 2023 10:08:00 +0200 Subject: [PATCH 06/61] Split action client and server examples --- .../Cargo.toml | 6 +---- .../package.xml | 4 +-- .../src/minimal_action_client.rs | 0 examples/minimal_action_server/Cargo.toml | 22 ++++++++++++++++ examples/minimal_action_server/package.xml | 26 +++++++++++++++++++ .../src/minimal_action_server.rs | 0 6 files changed, 51 insertions(+), 7 deletions(-) rename examples/{minimal_action_client_server => minimal_action_client}/Cargo.toml (81%) rename examples/{minimal_action_client_server => minimal_action_client}/package.xml (86%) rename examples/{minimal_action_client_server => minimal_action_client}/src/minimal_action_client.rs (100%) 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.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 5224a60c7..b1de4903b 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.3.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"]} diff --git a/examples/minimal_action_client_server/package.xml b/examples/minimal_action_client/package.xml similarity index 86% rename from examples/minimal_action_client_server/package.xml rename to examples/minimal_action_client/package.xml index b410cc76e..a2576efb1 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 + examples_rclrs_minimal_action_client 0.3.1 - Package containing an example of actions in rclrs. + 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 100% rename from examples/minimal_action_client_server/src/minimal_action_client.rs rename to examples/minimal_action_client/src/minimal_action_client.rs diff --git a/examples/minimal_action_server/Cargo.toml b/examples/minimal_action_server/Cargo.toml new file mode 100644 index 000000000..1599d15ad --- /dev/null +++ b/examples/minimal_action_server/Cargo.toml @@ -0,0 +1,22 @@ +[package] +name = "examples_rclrs_minimal_action_server" +version = "0.3.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"]} + +[dependencies.rclrs] +version = "0.3" + +[dependencies.rosidl_runtime_rs] +version = "0.3" + +[dependencies.example_interfaces] +version = "*" diff --git a/examples/minimal_action_server/package.xml b/examples/minimal_action_server/package.xml new file mode 100644 index 000000000..74ae8e40e --- /dev/null +++ b/examples/minimal_action_server/package.xml @@ -0,0 +1,26 @@ + + + + examples_rclrs_minimal_action_server + 0.3.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 0b0bf4d110841369a3dbfb9d263e82758df81d93 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 17 Jul 2023 13:55:08 +0200 Subject: [PATCH 07/61] checkin --- .../src/minimal_action_server.rs | 42 ++++++++++- rclrs/src/action.rs | 70 ++++++++++++++++++- rclrs/src/lib.rs | 2 + rclrs/src/node.rs | 38 ++++++++-- rclrs/src/rcl_bindings.rs | 2 + rclrs/src/rcl_wrapper.h | 3 + 6 files changed, 147 insertions(+), 10 deletions(-) diff --git a/examples/minimal_action_server/src/minimal_action_server.rs b/examples/minimal_action_server/src/minimal_action_server.rs index bf05587b6..8e64e09f2 100644 --- a/examples/minimal_action_server/src/minimal_action_server.rs +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -1,11 +1,51 @@ use std::env; +use std::sync::Arc; +use std::thread; use anyhow::{Error, Result}; +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 context = rclrs::Context::new(env::args())?; - let node = rclrs::create_node(&context, "minimal_action_server")?; + let mut node = rclrs::create_node(&context, "minimal_action_server")?; + + let _action_server = node.create_action_server::( + "fibonacci", + handle_goal, + handle_cancel, + handle_accepted, + ); rclrs::spin(&node).map_err(|err| err.into()) } diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index 0132fc242..f9096374a 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -1,8 +1,27 @@ use crate::{rcl_bindings::*, RclrsError}; use std::sync::{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_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, @@ -16,8 +35,6 @@ where { /// Creates a new action client. pub(crate) fn new(rcl_node_mtx: Arc>, 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, { @@ -26,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(rcl_node_mtx: Arc>, 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 c3d39e2b1..d16137953 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::*; diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 311fb4598..d230df7eb 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -13,10 +13,11 @@ use rosidl_runtime_rs::Message; pub use self::{builder::*, graph::*}; use crate::{ - rcl_bindings::*, ActionClient, Client, ClientBase, Clock, Context, ContextHandle, - GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, - QoSProfile, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, - SubscriptionCallback, TimeSource, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, ActionClient, ActionServer, CancelResponse, Client, ClientBase, Clock, + Context, ContextHandle, GoalResponse, GoalUUID, GuardCondition, ParameterInterface, + ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, ServerGoalHandle, Service, + ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, TimeSource, + ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -210,7 +211,7 @@ impl Node { Ok(client) } - /// Creates a [`Client`][1]. + /// Creates an [`ActionClient`][1]. /// /// [1]: crate::ActionClient // TODO: make action client's lifetime depend on node's lifetime @@ -221,13 +222,36 @@ impl Node { where T: rosidl_runtime_rs::Action, { - let client = Arc::new(ActionClient::::new( + let action_client = Arc::new(ActionClient::::new( Arc::clone(&self.rcl_node_mtx), 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( + &mut self, + topic: &str, + handle_goal: fn(&crate::action::GoalUUID, Arc) -> 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( + Arc::clone(&self.rcl_node_mtx), + 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 94491bc91..e90cf4556 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,5 +147,6 @@ 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 fe97cb4e5..14bd42189 100644 --- a/rclrs/src/rcl_wrapper.h +++ b/rclrs/src/rcl_wrapper.h @@ -1,5 +1,7 @@ #include #include +#include +#include #include #include #include @@ -7,3 +9,4 @@ #include const size_t rmw_gid_storage_size_constant = RMW_GID_STORAGE_SIZE; +const size_t rcl_action_uuid_size_constant = UUID_SIZE; \ No newline at end of file From 1cc545e90b2bfda2c3460dad1969bbb84007400e Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 17 Jul 2023 15:05:07 +0200 Subject: [PATCH 08/61] 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 8e64e09f2..9ae1f8cc3 100644 --- a/examples/minimal_action_server/src/minimal_action_server.rs +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -26,7 +26,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 f9096374a..919e0484d 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 9fe31695b786f7b750f329bf4ecf258aea60cd56 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 10 Aug 2023 11:52:39 +0200 Subject: [PATCH 09/61] checkin --- examples/minimal_action_client/src/minimal_action_client.rs | 2 +- examples/minimal_action_server/src/minimal_action_server.rs | 2 +- rclrs/src/lib.rs | 2 -- rclrs/src/node.rs | 2 +- 4 files changed, 3 insertions(+), 5 deletions(-) diff --git a/examples/minimal_action_client/src/minimal_action_client.rs b/examples/minimal_action_client/src/minimal_action_client.rs index cb8df7ea5..e67c46656 100644 --- a/examples/minimal_action_client/src/minimal_action_client.rs +++ b/examples/minimal_action_client/src/minimal_action_client.rs @@ -5,7 +5,7 @@ use anyhow::{Error, Result}; fn main() -> Result<(), Error> { let context = rclrs::Context::new(env::args())?; - let mut node = rclrs::create_node(&context, "minimal_client")?; + let node = rclrs::create_node(&context, "minimal_client")?; let _client = node.create_action_client::("fibonacci")?; diff --git a/examples/minimal_action_server/src/minimal_action_server.rs b/examples/minimal_action_server/src/minimal_action_server.rs index 9ae1f8cc3..253393882 100644 --- a/examples/minimal_action_server/src/minimal_action_server.rs +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -76,5 +76,5 @@ fn main() -> Result<(), Error> { handle_accepted, ); - rclrs::spin(&node).map_err(|err| err.into()) + rclrs::spin(node).map_err(|err| err.into()) } diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index d16137953..c3d39e2b1 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_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::*; diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index d230df7eb..96417edfa 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -216,7 +216,7 @@ impl Node { /// [1]: crate::ActionClient // TODO: make action client's lifetime depend on node's lifetime pub fn create_action_client( - &mut self, + &self, topic: &str, ) -> Result>, RclrsError> where From e685f83e121ea81afed1858fbbfc13f2140c1742 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 10 Aug 2023 12:29:15 +0200 Subject: [PATCH 10/61] 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 919e0484d..c8a75c59c 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 1b5dd0c702f664af7f3fcfcb2f06b6f2787252b2 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 8 Nov 2023 10:10:13 +0100 Subject: [PATCH 11/61] 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 c8a75c59c..4fb7c3d0f 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 eaa47f15ddb051d5d252a9d315094af786a2a58a Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 28 Nov 2022 17:47:22 +0100 Subject: [PATCH 12/61] dded action generation --- rosidl_generator_rs/resource/action.rs.em | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em index 40c5acd59..115186f7d 100644 --- a/rosidl_generator_rs/resource/action.rs.em +++ b/rosidl_generator_rs/resource/action.rs.em @@ -26,6 +26,17 @@ TEMPLATE( }@ } // mod rmw +@{ +TEMPLATE( + 'msg_idiomatic.rs.em', + package_name=package_name, interface_path=interface_path, + msg_specs=action_msg_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ +} // mod rmw + @{ TEMPLATE( 'msg_idiomatic.rs.em', From 3740a27cf72009de64250824dc763f0648b06e7c Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 17 Jul 2023 13:55:08 +0200 Subject: [PATCH 13/61] checkin --- rclrs/src/lib.rs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index c3d39e2b1..d16137953 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 3e4e718ad1cf1431d8dfff84399104c77400e1ae Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 17 Jul 2023 13:55:08 +0200 Subject: [PATCH 14/61] checkin --- .../minimal_action_server/src/minimal_action_server.rs | 7 +++++++ rclrs/src/node.rs | 8 ++++---- 2 files changed, 11 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 253393882..0c6ba4046 100644 --- a/examples/minimal_action_server/src/minimal_action_server.rs +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -76,5 +76,12 @@ fn main() -> Result<(), Error> { handle_accepted, ); + let _action_server = node.create_action_server::( + "fibonacci", + handle_goal, + handle_cancel, + handle_accepted, + ); + rclrs::spin(node).map_err(|err| err.into()) } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 96417edfa..762f959f2 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -14,10 +14,10 @@ use rosidl_runtime_rs::Message; pub use self::{builder::*, graph::*}; use crate::{ rcl_bindings::*, ActionClient, ActionServer, CancelResponse, Client, ClientBase, Clock, - Context, ContextHandle, GoalResponse, GoalUUID, GuardCondition, ParameterInterface, - ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, ServerGoalHandle, Service, - ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, TimeSource, - ENTITY_LIFECYCLE_MUTEX, + Context, ContextHandle, GoalResponse, GoalUUID, GuardCondition, ParameterBuilder, + ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, + ServerGoalHandle, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, + TimeSource, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread From 747f05ddf8f53698d9af547d5091381590e2505c Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 17 Nov 2023 15:34:07 +0100 Subject: [PATCH 15/61] 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 0c6ba4046..253393882 100644 --- a/examples/minimal_action_server/src/minimal_action_server.rs +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -76,12 +76,5 @@ fn main() -> Result<(), Error> { handle_accepted, ); - let _action_server = node.create_action_server::( - "fibonacci", - handle_goal, - handle_cancel, - handle_accepted, - ); - rclrs::spin(node).map_err(|err| err.into()) } From e1c837cd4ac882161542f867ac0bbcd211151f52 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 17 Nov 2023 16:22:37 +0100 Subject: [PATCH 16/61] checkin --- examples/minimal_action_client/Cargo.toml | 4 ++-- examples/minimal_action_server/Cargo.toml | 4 ++-- rosidl_generator_rs/resource/action.rs.em | 2 ++ 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/examples/minimal_action_client/Cargo.toml b/examples/minimal_action_client/Cargo.toml index b1de4903b..96275ba4e 100644 --- a/examples/minimal_action_client/Cargo.toml +++ b/examples/minimal_action_client/Cargo.toml @@ -13,10 +13,10 @@ path = "src/minimal_action_client.rs" anyhow = {version = "1", features = ["backtrace"]} [dependencies.rclrs] -version = "0.3" +version = "0.4" [dependencies.rosidl_runtime_rs] -version = "0.3" +version = "0.4" [dependencies.example_interfaces] version = "*" diff --git a/examples/minimal_action_server/Cargo.toml b/examples/minimal_action_server/Cargo.toml index 1599d15ad..3ec27526c 100644 --- a/examples/minimal_action_server/Cargo.toml +++ b/examples/minimal_action_server/Cargo.toml @@ -13,10 +13,10 @@ path = "src/minimal_action_server.rs" anyhow = {version = "1", features = ["backtrace"]} [dependencies.rclrs] -version = "0.3" +version = "0.4" [dependencies.rosidl_runtime_rs] -version = "0.3" +version = "0.4" [dependencies.example_interfaces] version = "*" diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em index 115186f7d..6b55f294f 100644 --- a/rosidl_generator_rs/resource/action.rs.em +++ b/rosidl_generator_rs/resource/action.rs.em @@ -21,6 +21,7 @@ TEMPLATE( package_name=package_name, interface_path=interface_path, msg_specs=action_msg_specs, get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) }@ @@ -32,6 +33,7 @@ TEMPLATE( package_name=package_name, interface_path=interface_path, msg_specs=action_msg_specs, get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) }@ From af46ec4238ba575fdafe25b1667f575e951ac393 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 17 Nov 2023 17:20:05 +0100 Subject: [PATCH 17/61] 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 253393882..a964b53bb 100644 --- a/examples/minimal_action_server/src/minimal_action_server.rs +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -27,35 +27,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 4fb7c3d0f..46e895788 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 3e6fd1b24e4fe4523f16efaf31852a217ded4e28 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 17 Jan 2024 15:58:56 +0100 Subject: [PATCH 18/61] Fix missing exported pre_field_serde field --- rosidl_generator_rs/resource/action.rs.em | 1 + 1 file changed, 1 insertion(+) diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em index 6b55f294f..f9adb0b6c 100644 --- a/rosidl_generator_rs/resource/action.rs.em +++ b/rosidl_generator_rs/resource/action.rs.em @@ -45,6 +45,7 @@ TEMPLATE( package_name=package_name, interface_path=interface_path, msg_specs=action_msg_specs, get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) }@ From 55edf8814f4bcf8a80cd856a146e2f320b8485d2 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 17 Jan 2024 16:16:26 +0100 Subject: [PATCH 19/61] Removed extra code --- rosidl_generator_rs/resource/action.rs.em | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em index f9adb0b6c..b37919dc3 100644 --- a/rosidl_generator_rs/resource/action.rs.em +++ b/rosidl_generator_rs/resource/action.rs.em @@ -27,18 +27,6 @@ TEMPLATE( }@ } // mod rmw -@{ -TEMPLATE( - 'msg_idiomatic.rs.em', - package_name=package_name, interface_path=interface_path, - msg_specs=action_msg_specs, - get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, - pre_field_serde=pre_field_serde, - get_idiomatic_rs_type=get_idiomatic_rs_type, - constant_value_to_rs=constant_value_to_rs) -}@ -} // mod rmw - @{ TEMPLATE( 'msg_idiomatic.rs.em', From dccc667c4a89efc5c66c78771d461f662aa5797d Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 6 Jun 2024 00:01:48 -0400 Subject: [PATCH 20/61] 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 | 8 ++++---- rclrs/src/lib.rs | 3 +-- rclrs/src/node.rs | 15 +++------------ rclrs/src/rcl_bindings.rs | 2 +- 4 files changed, 9 insertions(+), 19 deletions(-) diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index 46e895788..2936dd5e1 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -1,4 +1,4 @@ -use crate::{rcl_bindings::*, RclrsError}; +use crate::{rcl_bindings::*, NodeHandle, RclrsError}; use std::sync::{Arc, Mutex, MutexGuard}; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -34,7 +34,7 @@ where T: rosidl_runtime_rs::Action, { /// Creates a new action client. - pub(crate) fn new(rcl_node_mtx: Arc>, topic: &str) -> Result + pub(crate) fn new(node_handle: Arc, topic: &str) -> Result where T: rosidl_runtime_rs::Action, { @@ -56,7 +56,7 @@ where T: rosidl_runtime_rs::Action, { /// Creates a new action server. - pub(crate) fn new(rcl_node_mtx: Arc>, topic: &str) -> Result + pub(crate) fn new(node_handle: Arc, topic: &str) -> Result where T: rosidl_runtime_rs::Action, { @@ -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 d16137953..d8adf6500 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/node.rs b/rclrs/src/node.rs index 762f959f2..b3a094654 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -215,17 +215,11 @@ impl Node { /// /// [1]: crate::ActionClient // TODO: make action client's lifetime depend on node's lifetime - pub fn create_action_client( - &self, - topic: &str, - ) -> Result>, RclrsError> + pub fn create_action_client(&self, topic: &str) -> Result>, RclrsError> where T: rosidl_runtime_rs::Action, { - let action_client = Arc::new(ActionClient::::new( - Arc::clone(&self.rcl_node_mtx), - topic, - )?); + let action_client = Arc::new(ActionClient::::new(Arc::clone(&self.handle), topic)?); // self.clients // .push(Arc::downgrade(&client) as Weak); Ok(action_client) @@ -245,10 +239,7 @@ impl Node { where T: rosidl_runtime_rs::Action, { - let action_server = Arc::new(ActionServer::::new( - Arc::clone(&self.rcl_node_mtx), - topic, - )?); + let action_server = Arc::new(ActionServer::::new(Arc::clone(&self.handle), topic)?); // self.servers // .push(Arc::downgrade(&server) as Weak); Ok(action_server) diff --git a/rclrs/src/rcl_bindings.rs b/rclrs/src/rcl_bindings.rs index e90cf4556..4eec1acb9 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 573754d3ae43e10c689d6f94189429555440821c Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 6 Jun 2024 15:33:38 -0400 Subject: [PATCH 21/61] 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 +- rosidl_generator_rs/resource/action.rs.em | 5 ++ rosidl_runtime_rs/src/traits.rs | 3 + 5 files changed, 103 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 2936dd5e1..5aa50e10d 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -1,5 +1,8 @@ -use crate::{rcl_bindings::*, NodeHandle, RclrsError}; -use std::sync::{Arc, Mutex, MutexGuard}; +use crate::{error::ToResult, rcl_bindings::*, NodeHandle, 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_handle: Arc, + pub(crate) in_use_by_wait_set: Arc, +} + +impl ActionServerHandle { + pub(crate) fn lock(&self) -> MutexGuard { + self.rcl_action_server.lock().unwrap() + } +} + +impl Drop for ActionServerHandle { + fn drop(&mut self) { + let rcl_action_server = self.rcl_action_server.get_mut().unwrap(); + let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The entity lifecycle mutex is locked to protect against the risk of + // global variables in the rmw implementation being unsafely modified during cleanup. + unsafe { + rcl_action_server_fini(rcl_action_server, &mut *rcl_node); + } + } +} + +/// Trait to be implemented by concrete ActionServer structs. +/// +/// See [`ActionServer`] for an example +pub trait ActionServerBase: Send + Sync { + /// Internal function to get a reference to the `rcl` handle. + fn handle(&self) -> &ActionServerHandle; + // /// 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_handle, + 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 14bd42189..33aae1a81 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 @@ -9,4 +8,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; diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em index b37919dc3..b4abf3a02 100644 --- a/rosidl_generator_rs/resource/action.rs.em +++ b/rosidl_generator_rs/resource/action.rs.em @@ -51,6 +51,11 @@ type_name = action_spec.namespaced_type.name type Goal = crate::@(subfolder)::rmw::@(type_name)_Goal; type Result = crate::@(subfolder)::rmw::@(type_name)_Result; type Feedback = crate::@(subfolder)::rmw::@(type_name)_Feedback; + + fn get_type_support() -> *const std::os::raw::c_void { + // SAFETY: No preconditions for this function. + unsafe { rosidl_typesupport_c__get_action_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } + } } @[end for] diff --git a/rosidl_runtime_rs/src/traits.rs b/rosidl_runtime_rs/src/traits.rs index dd29fb23e..a8ae0586a 100644 --- a/rosidl_runtime_rs/src/traits.rs +++ b/rosidl_runtime_rs/src/traits.rs @@ -172,4 +172,7 @@ pub trait Action: 'static { /// The feedback message associated with this service. type Feedback: Message; + + /// Get a pointer to the correct `rosidl_action_type_support_t` structure. + fn get_type_support() -> *const std::os::raw::c_void; } From 3733e5ec9a3fa47c10bbb5695ee707296e67f399 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 6 Jun 2024 16:15:13 -0400 Subject: [PATCH 22/61] 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 5aa50e10d..48ed859fa 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_handle: Arc, + pub(crate) in_use_by_wait_set: Arc, +} + +impl ActionClientHandle { + pub(crate) fn lock(&self) -> MutexGuard { + self.rcl_action_client.lock().unwrap() + } +} + +impl Drop for ActionClientHandle { + fn drop(&mut self) { + let rcl_action_client = self.rcl_action_client.get_mut().unwrap(); + let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The entity lifecycle mutex is locked to protect against the risk of + // global variables in the rmw implementation being unsafely modified during cleanup. + unsafe { + rcl_action_client_fini(rcl_action_client, &mut *rcl_node); + } + } +} + /// 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_handle, + 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 ac4cc64cc5907abb3aa047d1f498a6b96a01d6eb Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 6 Jun 2024 16:37:04 -0400 Subject: [PATCH 23/61] 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 48ed859fa..93ee5247f 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -1,4 +1,4 @@ -use crate::{error::ToResult, rcl_bindings::*, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX}; +use crate::{error::ToResult, rcl_bindings::*, Clock, NodeHandle, 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_handle: Arc, topic: &str) -> Result + pub(crate) fn new(node_handle: Arc, 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 b3a094654..c616d19ba 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -239,7 +239,7 @@ impl Node { where T: rosidl_runtime_rs::Action, { - let action_server = Arc::new(ActionServer::::new(Arc::clone(&self.handle), topic)?); + let action_server = Arc::new(ActionServer::::new(Arc::clone(&self.handle), self.get_clock(), topic)?); // self.servers // .push(Arc::downgrade(&server) as Weak); Ok(action_server) From 018ef668e14e1a7401dd7a1700aa9cf84473bd39 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 6 Jun 2024 16:57:29 -0400 Subject: [PATCH 24/61] Split action servers and clients into separate modules --- rclrs/src/action.rs | 256 ++----------------------------------- rclrs/src/action/client.rs | 124 ++++++++++++++++++ rclrs/src/action/server.rs | 136 ++++++++++++++++++++ rclrs/src/node.rs | 6 +- 4 files changed, 273 insertions(+), 249 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 93ee5247f..714ad41e7 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -1,8 +1,11 @@ -use crate::{error::ToResult, rcl_bindings::*, Clock, NodeHandle, 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_handle: Arc, - pub(crate) in_use_by_wait_set: Arc, -} - -impl ActionClientHandle { - pub(crate) fn lock(&self) -> MutexGuard { - self.rcl_action_client.lock().unwrap() - } -} - -impl Drop for ActionClientHandle { - fn drop(&mut self) { - let rcl_action_client = self.rcl_action_client.get_mut().unwrap(); - let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); - let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); - // SAFETY: The entity lifecycle mutex is locked to protect against the risk of - // global variables in the rmw implementation being unsafely modified during cleanup. - unsafe { - rcl_action_client_fini(rcl_action_client, &mut *rcl_node); - } - } -} - -/// Manage the lifecycle of an `rcl_action_server_t`, including managing its dependencies -/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are -/// [dropped after][1] the `rcl_action_server_t`. -/// -/// [1]: -pub struct ActionServerHandle { - rcl_action_server: Mutex, - node_handle: Arc, - pub(crate) in_use_by_wait_set: Arc, -} - -impl ActionServerHandle { - pub(crate) fn lock(&self) -> MutexGuard { - self.rcl_action_server.lock().unwrap() - } -} - -impl Drop for ActionServerHandle { - fn drop(&mut self) { - let rcl_action_server = self.rcl_action_server.get_mut().unwrap(); - let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); - let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); - // SAFETY: The entity lifecycle mutex is locked to protect against the risk of - // global variables in the rmw implementation being unsafely modified during cleanup. - unsafe { - rcl_action_server_fini(rcl_action_server, &mut *rcl_node); - } - } -} - -/// Trait to be implemented by concrete 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_handle: Arc, 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_handle, - 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_handle: Arc, 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_handle, - 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..0ea72b8fd --- /dev/null +++ b/rclrs/src/action/client.rs @@ -0,0 +1,124 @@ +use crate::{error::ToResult, rcl_bindings::*, NodeHandle, 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_handle: Arc, + pub(crate) in_use_by_wait_set: Arc, +} + +impl ActionClientHandle { + pub(crate) fn lock(&self) -> MutexGuard { + self.rcl_action_client.lock().unwrap() + } +} + +impl Drop for ActionClientHandle { + fn drop(&mut self) { + let rcl_action_client = self.rcl_action_client.get_mut().unwrap(); + let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The entity lifecycle mutex is locked to protect against the risk of + // global variables in the rmw implementation being unsafely modified during cleanup. + unsafe { + rcl_action_client_fini(rcl_action_client, &mut *rcl_node); + } + } +} + +/// Trait to be implemented by concrete ActionClient structs. +/// +/// See [`ActionClient`] for an example +pub trait ActionClientBase: Send + Sync { + /// Internal function to get a reference to the `rcl` handle. + fn handle(&self) -> &ActionClientHandle; + // /// 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_handle: Arc, 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 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 + // 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_handle, + 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..6ffd51e5a --- /dev/null +++ b/rclrs/src/action/server.rs @@ -0,0 +1,136 @@ +use crate::{ + error::ToResult, rcl_bindings::*, Clock, NodeHandle, 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_handle: Arc, + pub(crate) in_use_by_wait_set: Arc, +} + +impl ActionServerHandle { + pub(crate) fn lock(&self) -> MutexGuard { + self.rcl_action_server.lock().unwrap() + } +} + +impl Drop for ActionServerHandle { + fn drop(&mut self) { + let rcl_action_server = self.rcl_action_server.get_mut().unwrap(); + let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The entity lifecycle mutex is locked to protect against the risk of + // global variables in the rmw implementation being unsafely modified during cleanup. + unsafe { + rcl_action_server_fini(rcl_action_server, &mut *rcl_node); + } + } +} + +/// Trait to be implemented by concrete ActionServer structs. +/// +/// See [`ActionServer`] for an example +pub trait ActionServerBase: Send + Sync { + /// Internal function to get a reference to the `rcl` handle. + fn handle(&self) -> &ActionServerHandle; + // /// 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_handle: Arc, + 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 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 + // 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_handle, + 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 + } +} diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index c616d19ba..e8ec1d79f 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -239,7 +239,11 @@ impl Node { where T: rosidl_runtime_rs::Action, { - let action_server = Arc::new(ActionServer::::new(Arc::clone(&self.handle), self.get_clock(), topic)?); + let action_server = Arc::new(ActionServer::::new( + Arc::clone(&self.handle), + self.get_clock(), + topic, + )?); // self.servers // .push(Arc::downgrade(&server) as Weak); Ok(action_server) From 279bd8a65c49441190d62ac8f6c0d5605064dda2 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 6 Jun 2024 17:24:47 -0400 Subject: [PATCH 25/61] 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 8b76a0ee9563451e5fb270ba567c2c25c9f21675 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 6 Jun 2024 18:49:19 -0400 Subject: [PATCH 26/61] 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 a45b8ba4583da0abd60466fe88c1a28e3e490c97 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 8 Jun 2024 15:32:05 -0400 Subject: [PATCH 27/61] 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 e8ec1d79f..bd1da4e37 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -14,7 +14,7 @@ use rosidl_runtime_rs::Message; pub use self::{builder::*, graph::*}; use crate::{ rcl_bindings::*, ActionClient, ActionServer, CancelResponse, Client, ClientBase, Clock, - Context, ContextHandle, GoalResponse, GoalUUID, GuardCondition, ParameterBuilder, + Context, ContextHandle, GoalResponse, GoalUuid, GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, ServerGoalHandle, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, TimeSource, ENTITY_LIFECYCLE_MUTEX, @@ -232,7 +232,7 @@ impl Node { pub fn create_action_server( &mut self, topic: &str, - handle_goal: fn(&crate::action::GoalUUID, Arc) -> GoalResponse, + handle_goal: fn(&crate::action::GoalUuid, Arc) -> GoalResponse, handle_cancel: fn(Arc>) -> CancelResponse, handle_accepted: fn(Arc>), ) -> Result>, RclrsError> From 03b9b743edd70db06f764bc3963b001a7920d4ea Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 8 Jun 2024 15:51:21 -0400 Subject: [PATCH 28/61] 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 045a66dc26d85cc28452c1529e0f457ab5593de7 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 8 Jun 2024 16:18:52 -0400 Subject: [PATCH 29/61] 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 0ea72b8fd..7f0d226ba 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 6ffd51e5a..778188293 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -77,8 +77,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 d8adf6500..c3d39e2b1 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 38c5b959440ded177f07f9ca9210bd13d7c272b6 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Tue, 11 Jun 2024 12:21:17 -0400 Subject: [PATCH 30/61] Fix action typesupport function --- rosidl_generator_rs/resource/action.rs.em | 29 +++++++++++++---------- 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em index b4abf3a02..8927355ac 100644 --- a/rosidl_generator_rs/resource/action.rs.em +++ b/rosidl_generator_rs/resource/action.rs.em @@ -44,18 +44,23 @@ TEMPLATE( type_name = action_spec.namespaced_type.name }@ - // Corresponds to @(package_name)__@(subfolder)__@(type_name) - pub struct @(type_name); - - impl rosidl_runtime_rs::Action for @(type_name) { - type Goal = crate::@(subfolder)::rmw::@(type_name)_Goal; - type Result = crate::@(subfolder)::rmw::@(type_name)_Result; - type Feedback = crate::@(subfolder)::rmw::@(type_name)_Feedback; - - fn get_type_support() -> *const std::os::raw::c_void { - // SAFETY: No preconditions for this function. - unsafe { rosidl_typesupport_c__get_action_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } - } +#[link(name = "@(package_name)__rosidl_typesupport_c")] +extern "C" { + fn rosidl_typesupport_c__get_action_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::os::raw::c_void; +} + +// Corresponds to @(package_name)__@(subfolder)__@(type_name) +pub struct @(type_name); + +impl rosidl_runtime_rs::Action for @(type_name) { + type Goal = crate::@(subfolder)::rmw::@(type_name)_Goal; + type Result = crate::@(subfolder)::rmw::@(type_name)_Result; + type Feedback = crate::@(subfolder)::rmw::@(type_name)_Feedback; + + fn get_type_support() -> *const std::os::raw::c_void { + // SAFETY: No preconditions for this function. + unsafe { rosidl_typesupport_c__get_action_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } } +} @[end for] From a323bd00beb7cf3da17e729d06db6174b17f4fa4 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Tue, 11 Jun 2024 14:50:31 -0400 Subject: [PATCH 31/61] 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 | 25 ++++++++++++++++--------- rclrs/src/node.rs | 28 +++++++++++++++++----------- 3 files changed, 34 insertions(+), 21 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 778188293..64a31b8ed 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -1,9 +1,8 @@ use crate::{ - error::ToResult, rcl_bindings::*, Clock, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + action::{GoalResponse, GoalUuid, CancelResponse, ServerGoalHandle}, error::ToResult, rcl_bindings::*, Clock, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; use std::{ ffi::CString, - marker::PhantomData, sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, }; @@ -51,15 +50,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 @@ -71,6 +73,9 @@ where node_handle: Arc, 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, @@ -119,8 +124,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 bd1da4e37..6179a3a3c 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -220,8 +220,8 @@ impl Node { T: rosidl_runtime_rs::Action, { let action_client = Arc::new(ActionClient::::new(Arc::clone(&self.handle), topic)?); - // self.clients - // .push(Arc::downgrade(&client) as Weak); + // self.action_clients + // .push(Arc::downgrade(&action_client) as Weak); Ok(action_client) } @@ -229,23 +229,29 @@ impl Node { /// /// [1]: crate::ActionServer // TODO: make action server's lifetime depend on node's lifetime - pub fn create_action_server( + pub fn create_action_server( &mut self, topic: &str, - handle_goal: fn(&crate::action::GoalUuid, Arc) -> 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( + let action_server = Arc::new(ActionServer::::new( Arc::clone(&self.handle), self.get_clock(), topic, + handle_goal, + handle_cancel, + handle_accepted, )?); - // self.servers - // .push(Arc::downgrade(&server) as Weak); + // self.action_servers + // .push(Arc::downgrade(&action_server) as Weak); Ok(action_server) } From 03194194101db5b9ad414af0d1db7dc1a01c5ecd Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 6 Jul 2024 16:41:35 -0400 Subject: [PATCH 32/61] Store action clients and servers in the Node --- rclrs/src/node.rs | 30 ++++++++++++++++-------------- rclrs/src/node/builder.rs | 2 ++ 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 6179a3a3c..4d9f5a8a5 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -13,7 +13,7 @@ use rosidl_runtime_rs::Message; pub use self::{builder::*, graph::*}; use crate::{ - rcl_bindings::*, ActionClient, ActionServer, CancelResponse, Client, ClientBase, Clock, + rcl_bindings::*, ActionClient, ActionClientBase, ActionServer, ActionServerBase, CancelResponse, Client, ClientBase, Clock, Context, ContextHandle, GoalResponse, GoalUuid, GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, ServerGoalHandle, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, @@ -64,6 +64,8 @@ pub struct Node { 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, pub(crate) handle: Arc, @@ -207,7 +209,7 @@ impl Node { T: rosidl_runtime_rs::Service, { let client = Arc::new(Client::::new(Arc::clone(&self.handle), topic)?); - { self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); + self.clients_mtx.lock().unwrap().push(Arc::downgrade(&client) as Weak); Ok(client) } @@ -220,8 +222,8 @@ impl Node { T: rosidl_runtime_rs::Action, { let action_client = Arc::new(ActionClient::::new(Arc::clone(&self.handle), 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) } @@ -250,8 +252,8 @@ impl Node { 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) } @@ -269,7 +271,7 @@ impl Node { 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 } @@ -291,7 +293,7 @@ impl Node { 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 } @@ -330,7 +332,7 @@ impl Node { topic, callback, )?); - { self.services_mtx.lock().unwrap() } + self.services_mtx.lock().unwrap() .push(Arc::downgrade(&service) as Weak); Ok(service) } @@ -354,7 +356,7 @@ impl Node { qos, callback, )?); - { self.subscriptions_mtx.lock() } + self.subscriptions_mtx.lock() .unwrap() .push(Arc::downgrade(&subscription) as Weak); Ok(subscription) @@ -362,28 +364,28 @@ impl Node { /// 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/builder.rs b/rclrs/src/node/builder.rs index 011d5d2f3..d1a9a698b 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -314,6 +314,8 @@ impl NodeBuilder { guard_conditions_mtx: Mutex::new(vec![]), services_mtx: Mutex::new(vec![]), subscriptions_mtx: Mutex::new(vec![]), + action_servers_mtx: Mutex::new(vec![]), + action_clients_mtx: Mutex::new(vec![]), time_source: TimeSource::builder(self.clock_type) .clock_qos(self.clock_qos) .build(), From b6c8b476e4c8ddae07fa917e96fd48d6eb89927c Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sun, 7 Jul 2024 12:07:39 -0400 Subject: [PATCH 33/61] 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 | 26 +++++++- rclrs/src/node.rs | 72 ++++++++++++++++----- rclrs/src/wait.rs | 128 +++++++++++++++++++++++++++++++++++-- 4 files changed, 229 insertions(+), 28 deletions(-) diff --git a/rclrs/src/action/client.rs b/rclrs/src/action/client.rs index 7f0d226ba..deef8a16b 100644 --- a/rclrs/src/action/client.rs +++ b/rclrs/src/action/client.rs @@ -1,4 +1,7 @@ -use crate::{error::ToResult, rcl_bindings::*, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX}; +use crate::{ + error::ToResult, rcl_bindings::*, wait::WaitableNumEntities, NodeHandle, 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 64a31b8ed..b9d0b2c6b 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -1,5 +1,9 @@ use crate::{ - action::{GoalResponse, GoalUuid, CancelResponse, ServerGoalHandle}, error::ToResult, rcl_bindings::*, Clock, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + action::{CancelResponse, GoalResponse, GoalUuid, ServerGoalHandle}, + error::ToResult, + rcl_bindings::*, + wait::WaitableNumEntities, + Clock, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; use std::{ ffi::CString, @@ -46,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>; } @@ -59,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>, @@ -123,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), @@ -139,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 4d9f5a8a5..4d593b530 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -13,11 +13,11 @@ use rosidl_runtime_rs::Message; pub use self::{builder::*, graph::*}; use crate::{ - rcl_bindings::*, ActionClient, ActionClientBase, ActionServer, ActionServerBase, CancelResponse, Client, ClientBase, Clock, - Context, ContextHandle, GoalResponse, GoalUuid, GuardCondition, ParameterBuilder, - ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, - ServerGoalHandle, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, - TimeSource, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, ActionClient, ActionClientBase, ActionServer, ActionServerBase, + CancelResponse, Client, ClientBase, Clock, Context, ContextHandle, GoalResponse, GoalUuid, + GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, + QoSProfile, RclrsError, ServerGoalHandle, Service, ServiceBase, Subscription, SubscriptionBase, + SubscriptionCallback, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -209,7 +209,10 @@ impl Node { T: rosidl_runtime_rs::Service, { let client = Arc::new(Client::::new(Arc::clone(&self.handle), topic)?); - self.clients_mtx.lock().unwrap().push(Arc::downgrade(&client) as Weak); + self.clients_mtx + .lock() + .unwrap() + .push(Arc::downgrade(&client) as Weak); Ok(client) } @@ -222,7 +225,9 @@ impl Node { T: rosidl_runtime_rs::Action, { let action_client = Arc::new(ActionClient::::new(Arc::clone(&self.handle), topic)?); - self.action_clients_mtx.lock().unwrap() + self.action_clients_mtx + .lock() + .unwrap() .push(Arc::downgrade(&action_client) as Weak); Ok(action_client) } @@ -252,7 +257,9 @@ impl Node { 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) } @@ -271,7 +278,9 @@ impl Node { 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 } @@ -293,7 +302,9 @@ impl Node { 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 } @@ -332,7 +343,9 @@ impl Node { topic, callback, )?); - self.services_mtx.lock().unwrap() + self.services_mtx + .lock() + .unwrap() .push(Arc::downgrade(&service) as Weak); Ok(service) } @@ -356,7 +369,8 @@ impl Node { qos, callback, )?); - self.subscriptions_mtx.lock() + self.subscriptions_mtx + .lock() .unwrap() .push(Arc::downgrade(&subscription) as Weak); Ok(subscription) @@ -364,28 +378,54 @@ impl Node { /// 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 2ef99c026..68d6d962b 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 2fbd496af7427c77fa895f6bb1ddd6c94aa3b4e5 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sun, 7 Jul 2024 13:38:25 -0400 Subject: [PATCH 34/61] 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 | 21 +++++-- rclrs/src/wait.rs | 119 +++++++++++++++++++++++++++++++++++-- 5 files changed, 162 insertions(+), 13 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 deef8a16b..98d7ca37b 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 b9d0b2c6b..0a5a81e09 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 37c43a68e..7bd96c588 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -25,13 +25,15 @@ impl SingleThreadedExecutor { /// Add a node to the executor. pub fn add_node(&self, node: &Arc) -> Result<(), RclrsError> { - { self.nodes_mtx.lock().unwrap() }.push(Arc::downgrade(node)); + self.nodes_mtx.lock().unwrap().push(Arc::downgrade(node)); Ok(()) } /// Remove a node from the executor. pub fn remove_node(&self, node: Arc) -> Result<(), RclrsError> { - { self.nodes_mtx.lock().unwrap() } + self.nodes_mtx + .lock() + .unwrap() .retain(|n| !n.upgrade().map(|n| Arc::ptr_eq(&n, &node)).unwrap_or(false)); Ok(()) } @@ -40,7 +42,10 @@ impl SingleThreadedExecutor { /// /// This function additionally checks that the context is still valid. pub fn spin_once(&self, timeout: Option) -> Result<(), RclrsError> { - for node in { self.nodes_mtx.lock().unwrap() } + for node in self + .nodes_mtx + .lock() + .unwrap() .iter() .filter_map(Weak::upgrade) .filter(|node| unsafe { @@ -61,6 +66,14 @@ impl SingleThreadedExecutor { 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)?; + } } Ok(()) @@ -68,7 +81,7 @@ impl SingleThreadedExecutor { /// Convenience function for calling [`SingleThreadedExecutor::spin_once`] in a loop. pub fn spin(&self) -> Result<(), RclrsError> { - while !{ self.nodes_mtx.lock().unwrap() }.is_empty() { + while !self.nodes_mtx.lock().unwrap().is_empty() { match self.spin_once(None) { Ok(_) | Err(RclrsError::RclError { diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 68d6d962b..68c3570fe 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 d5fc2f35d86958ef8afeca93eb87c69ca3dbe565 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 13 Jul 2024 16:51:55 -0400 Subject: [PATCH 35/61] 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 3eba2549f..67018ae9a 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -174,6 +174,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, @@ -222,6 +242,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 => { @@ -303,6 +332,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 e72d1c915a35555b8a9664383cefd102b59bfb75 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 13 Jul 2024 19:11:56 -0400 Subject: [PATCH 36/61] Add ActionImpl trait with internal messages and services This is incomplete, since the service types aren't yet being generated. --- rosidl_generator_rs/resource/action.rs.em | 24 ++++++++++++++++--- rosidl_runtime_rs/src/traits.rs | 28 +++++++++++++++++++---- 2 files changed, 45 insertions(+), 7 deletions(-) diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em index 8927355ac..d8dc56ec2 100644 --- a/rosidl_generator_rs/resource/action.rs.em +++ b/rosidl_generator_rs/resource/action.rs.em @@ -1,4 +1,13 @@ @{ +from rosidl_parser.definition import ( + ACTION_FEEDBACK_MESSAGE_SUFFIX, + ACTION_FEEDBACK_SUFFIX, + ACTION_GOAL_SERVICE_SUFFIX, + ACTION_GOAL_SUFFIX, + ACTION_RESULT_SERVICE_SUFFIX, + ACTION_RESULT_SUFFIX, +) + action_msg_specs = [] for subfolder, action in action_specs: @@ -53,9 +62,9 @@ extern "C" { pub struct @(type_name); impl rosidl_runtime_rs::Action for @(type_name) { - type Goal = crate::@(subfolder)::rmw::@(type_name)_Goal; - type Result = crate::@(subfolder)::rmw::@(type_name)_Result; - type Feedback = crate::@(subfolder)::rmw::@(type_name)_Feedback; + type Goal = crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SUFFIX); + type Result = crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SUFFIX); + type Feedback = crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_SUFFIX); fn get_type_support() -> *const std::os::raw::c_void { // SAFETY: No preconditions for this function. @@ -63,4 +72,13 @@ impl rosidl_runtime_rs::Action for @(type_name) { } } +impl rosidl_runtime_rs::ActionImpl for @(type_name) { + type GoalStatusMessage = action_msgs::msg::rmw::GoalStatusArray; + type FeedbackMessage = crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_MESSAGE_SUFFIX); + + type SendGoalService = crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX); + type CancelGoalService = action_msgs::srv::rmw::CancelGoal; + type GetResultService = crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX); +} + @[end for] diff --git a/rosidl_runtime_rs/src/traits.rs b/rosidl_runtime_rs/src/traits.rs index a8ae0586a..6bd4da1f4 100644 --- a/rosidl_runtime_rs/src/traits.rs +++ b/rosidl_runtime_rs/src/traits.rs @@ -163,16 +163,36 @@ pub trait Service: 'static { /// Trait for actions. /// /// User code never needs to call this trait's method, much less implement this trait. -pub trait Action: 'static { - /// The goal message associated with this service. +pub trait Action: 'static + ActionImpl { + /// The goal message associated with this action. type Goal: Message; - /// The result message associated with this service. + /// The result message associated with this action. type Result: Message; - /// The feedback message associated with this service. + /// The feedback message associated with this action. type Feedback: Message; /// Get a pointer to the correct `rosidl_action_type_support_t` structure. fn get_type_support() -> *const std::os::raw::c_void; } + +/// Trait for action implementation details. +/// +/// User code never needs to implement this trait, nor use its associated types. +pub trait ActionImpl: 'static { + /// The goal_status message associated with this action. + type GoalStatusMessage: Message; + + /// The feedback message associated with this action. + type FeedbackMessage: Message; + + /// The send_goal service associated with this action. + type SendGoalService: Service; + + /// The cancel_goal service associated with this action. + type CancelGoalService: Service; + + /// The get_result service associated with this action. + type GetResultService: Service; +} From 02dcc7a7486645f4fab11e45b8a725247ab39fd1 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 13 Jul 2024 19:16:01 -0400 Subject: [PATCH 37/61] [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 98d7ca37b..0c8f62ddf 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 0a5a81e09..17169ce57 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, NodeHandle, 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 edebb769603f6ca8fed4fc712ced74e4daa7f0f6 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 20 Jul 2024 11:35:26 -0400 Subject: [PATCH 38/61] Split srv.rs.em into idiomatic and rmw template files This results in the exact same file being produced for services, except for some whitespace changes. However, it enables actions to invoke the respective service template for its generation, similar to how the it works for services and their underlying messages. --- rosidl_generator_rs/resource/srv.rs.em | 69 ++----------------- .../resource/srv_idiomatic.rs.em | 44 ++++++++++++ rosidl_generator_rs/resource/srv_rmw.rs.em | 44 ++++++++++++ 3 files changed, 92 insertions(+), 65 deletions(-) create mode 100644 rosidl_generator_rs/resource/srv_idiomatic.rs.em create mode 100644 rosidl_generator_rs/resource/srv_rmw.rs.em diff --git a/rosidl_generator_rs/resource/srv.rs.em b/rosidl_generator_rs/resource/srv.rs.em index 369696ff7..dd99e8e76 100644 --- a/rosidl_generator_rs/resource/srv.rs.em +++ b/rosidl_generator_rs/resource/srv.rs.em @@ -1,84 +1,23 @@ -@{ -req_res_specs = [] - -for subfolder, service in srv_specs: - req_res_specs.append((subfolder, service.request_message)) - req_res_specs.append((subfolder, service.response_message)) -}@ - @{ TEMPLATE( - 'msg_idiomatic.rs.em', + 'srv_idiomatic.rs.em', package_name=package_name, interface_path=interface_path, - msg_specs=req_res_specs, + srv_specs=srv_specs, get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, pre_field_serde=pre_field_serde, get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) -}@ - -@[for subfolder, srv_spec in srv_specs] - -@{ -type_name = srv_spec.namespaced_type.name -}@ - -#[link(name = "@(package_name)__rosidl_typesupport_c")] -extern "C" { - fn rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::os::raw::c_void; } -// Corresponds to @(package_name)__@(subfolder)__@(type_name) -pub struct @(type_name); - -impl rosidl_runtime_rs::Service for @(type_name) { - type Request = crate::@(subfolder)::@(type_name)_Request; - type Response = crate::@(subfolder)::@(type_name)_Response; - - fn get_type_support() -> *const std::os::raw::c_void { - // SAFETY: No preconditions for this function. - unsafe { rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } - } -} - -@[end for] - pub mod rmw { @{ TEMPLATE( - 'msg_rmw.rs.em', + 'srv_rmw.rs.em', package_name=package_name, interface_path=interface_path, - msg_specs=req_res_specs, + srv_specs=srv_specs, get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, pre_field_serde=pre_field_serde, get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) }@ - -@[for subfolder, srv_spec in srv_specs] - -@{ -type_name = srv_spec.namespaced_type.name -}@ - - #[link(name = "@(package_name)__rosidl_typesupport_c")] - extern "C" { - fn rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::os::raw::c_void; - } - - // Corresponds to @(package_name)__@(subfolder)__@(type_name) - pub struct @(type_name); - - impl rosidl_runtime_rs::Service for @(type_name) { - type Request = crate::@(subfolder)::rmw::@(type_name)_Request; - type Response = crate::@(subfolder)::rmw::@(type_name)_Response; - - fn get_type_support() -> *const std::os::raw::c_void { - // SAFETY: No preconditions for this function. - unsafe { rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } - } - } - -@[end for] - } // mod rmw diff --git a/rosidl_generator_rs/resource/srv_idiomatic.rs.em b/rosidl_generator_rs/resource/srv_idiomatic.rs.em new file mode 100644 index 000000000..cf202ca01 --- /dev/null +++ b/rosidl_generator_rs/resource/srv_idiomatic.rs.em @@ -0,0 +1,44 @@ +@{ +req_res_specs = [] + +for subfolder, service in srv_specs: + req_res_specs.append((subfolder, service.request_message)) + req_res_specs.append((subfolder, service.response_message)) +}@ + +@{ +TEMPLATE( + 'msg_idiomatic.rs.em', + package_name=package_name, interface_path=interface_path, + msg_specs=req_res_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ + +@[for subfolder, srv_spec in srv_specs] + +@{ +type_name = srv_spec.namespaced_type.name +}@ + +#[link(name = "@(package_name)__rosidl_typesupport_c")] +extern "C" { + fn rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::os::raw::c_void; +} + +// Corresponds to @(package_name)__@(subfolder)__@(type_name) +pub struct @(type_name); + +impl rosidl_runtime_rs::Service for @(type_name) { + type Request = crate::@(subfolder)::@(type_name)_Request; + type Response = crate::@(subfolder)::@(type_name)_Response; + + fn get_type_support() -> *const std::os::raw::c_void { + // SAFETY: No preconditions for this function. + unsafe { rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } + } +} + +@[end for] diff --git a/rosidl_generator_rs/resource/srv_rmw.rs.em b/rosidl_generator_rs/resource/srv_rmw.rs.em new file mode 100644 index 000000000..c7c04c2ad --- /dev/null +++ b/rosidl_generator_rs/resource/srv_rmw.rs.em @@ -0,0 +1,44 @@ +@{ +req_res_specs = [] + +for subfolder, service in srv_specs: + req_res_specs.append((subfolder, service.request_message)) + req_res_specs.append((subfolder, service.response_message)) +}@ + +@{ +TEMPLATE( + 'msg_rmw.rs.em', + package_name=package_name, interface_path=interface_path, + msg_specs=req_res_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ + +@[for subfolder, srv_spec in srv_specs] + +@{ +type_name = srv_spec.namespaced_type.name +}@ + + #[link(name = "@(package_name)__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::os::raw::c_void; + } + + // Corresponds to @(package_name)__@(subfolder)__@(type_name) + pub struct @(type_name); + + impl rosidl_runtime_rs::Service for @(type_name) { + type Request = crate::@(subfolder)::rmw::@(type_name)_Request; + type Response = crate::@(subfolder)::rmw::@(type_name)_Response; + + fn get_type_support() -> *const std::os::raw::c_void { + // SAFETY: No preconditions for this function. + unsafe { rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } + } + } + +@[end for] From c9a28f75fd0668e17f3f55209f35af6ec9cc8e36 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 20 Jul 2024 12:12:43 -0400 Subject: [PATCH 39/61] Generate underlying service definitions for actions Not tested --- rosidl_generator_rs/resource/action.rs.em | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em index d8dc56ec2..26182249f 100644 --- a/rosidl_generator_rs/resource/action.rs.em +++ b/rosidl_generator_rs/resource/action.rs.em @@ -33,6 +33,15 @@ TEMPLATE( pre_field_serde=pre_field_serde, get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) + +TEMPLATE( + 'srv_rmw.rs.em', + package_name=package_name, interface_path=interface_path, + srv_specs=action_srv_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) }@ } // mod rmw @@ -47,6 +56,17 @@ TEMPLATE( constant_value_to_rs=constant_value_to_rs) }@ +@{ +TEMPLATE( + 'srv_idiomatic.rs.em', + package_name=package_name, interface_path=interface_path, + srv_specs=action_srv_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ + @[for subfolder, action_spec in action_specs] @{ From dc8781ce6af751c61e11448e3e0196a63574e087 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 20 Jul 2024 17:54:54 -0400 Subject: [PATCH 40/61] Add runtime trait to get the UUID from a goal request C++ uses duck typing for this, knowing that for any `Action`, the type `Action::Impl::SendGoalService::Request` will always have a `goal_id` field of type `unique_identifier_msgs::msg::UUID` without having to prove this to the compiler. Rust's generics are more strict, requiring that this be proven using type bounds. The `Request` type is also action-specific as it contains a `goal` field containing the `Goal` message type of the action. We therefore cannot enforce that all `Request`s are a specific type in `rclrs`. This seems most easily represented using associated type bounds on the `SendGoalService` associated type within `ActionImpl`. To avoid introducing to `rosidl_runtime_rs` a circular dependency on message packages like `unique_identifier_msgs`, the `ExtractUuid` trait only operates on a byte array rather than a more nicely typed `UUID` message type. I'll likely revisit this as we introduce more similar bounds on the generated types. --- rosidl_runtime_rs/src/lib.rs | 2 +- rosidl_runtime_rs/src/traits.rs | 10 +++++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/rosidl_runtime_rs/src/lib.rs b/rosidl_runtime_rs/src/lib.rs index 01ad9f464..7e06b7369 100644 --- a/rosidl_runtime_rs/src/lib.rs +++ b/rosidl_runtime_rs/src/lib.rs @@ -9,4 +9,4 @@ mod string; pub use string::{BoundedString, BoundedWString, String, StringExceedsBoundsError, WString}; mod traits; -pub use traits::{Action, Message, RmwMessage, SequenceAlloc, Service}; +pub use traits::{Action, ActionImpl, ExtractUuid, Message, RmwMessage, SequenceAlloc, Service}; diff --git a/rosidl_runtime_rs/src/traits.rs b/rosidl_runtime_rs/src/traits.rs index 6bd4da1f4..adb65e626 100644 --- a/rosidl_runtime_rs/src/traits.rs +++ b/rosidl_runtime_rs/src/traits.rs @@ -188,7 +188,7 @@ pub trait ActionImpl: 'static { type FeedbackMessage: Message; /// The send_goal service associated with this action. - type SendGoalService: Service; + type SendGoalService: Service>; /// The cancel_goal service associated with this action. type CancelGoalService: Service; @@ -196,3 +196,11 @@ pub trait ActionImpl: 'static { /// The get_result service associated with this action. type GetResultService: Service; } + +/// Trait for types containing a special UUID field. +/// +/// User code never needs to implement this trait, nor call its method. +pub trait ExtractUuid: 'static { + /// Copies the UUID field from `self` into the provided buffer. + fn extract_uuid(&self, bytes: &mut [u8; 16]); +} From 9d6a8b83cddbe7e6d782a44da8ada2ad78e11b88 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 20 Jul 2024 18:16:08 -0400 Subject: [PATCH 41/61] 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 54250724d7205be023f39b6f29aeac90cfbf5cd5 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 20 Jul 2024 18:23:50 -0400 Subject: [PATCH 42/61] 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 17169ce57..c747252d0 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -5,6 +5,7 @@ use crate::{ wait::WaitableNumEntities, Clock, NodeHandle, 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 9595b6e88c2c7c5638650175c05e3ca0ad72771a Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 25 Jul 2024 15:41:17 -0400 Subject: [PATCH 43/61] 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 c747252d0..3a479b41f 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 5496b108ed8357acd620710dfed1b6d566b912ea Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Tue, 6 Aug 2024 21:54:53 -0400 Subject: [PATCH 44/61] 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 c3d39e2b1..6b689f498 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 node; @@ -38,6 +39,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 node::*; From 46e709332c8af0bab26937480ee30c530056d159 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Tue, 6 Aug 2024 22:43:14 -0400 Subject: [PATCH 45/61] 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 3a479b41f..0ede383d4 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, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + Clock, DropGuard, NodeHandle, 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 b15c0c5b3629f285d27aec70d840b69f29a6bd8e Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Wed, 7 Aug 2024 18:14:14 -0400 Subject: [PATCH 46/61] 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 0ede383d4..1416d84ff 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 f8fedbc253b0f9e001b672d27f238a1e418db782 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Wed, 7 Aug 2024 18:45:30 -0400 Subject: [PATCH 47/61] 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 +++++------- rosidl_runtime_rs/src/lib.rs | 2 +- rosidl_runtime_rs/src/traits.rs | 14 ++++++-------- 3 files changed, 12 insertions(+), 16 deletions(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 1416d84ff..594a0e3f3 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -5,7 +5,7 @@ use crate::{ wait::WaitableNumEntities, Clock, DropGuard, NodeHandle, 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."); diff --git a/rosidl_runtime_rs/src/lib.rs b/rosidl_runtime_rs/src/lib.rs index 7e06b7369..7c5bad461 100644 --- a/rosidl_runtime_rs/src/lib.rs +++ b/rosidl_runtime_rs/src/lib.rs @@ -9,4 +9,4 @@ mod string; pub use string::{BoundedString, BoundedWString, String, StringExceedsBoundsError, WString}; mod traits; -pub use traits::{Action, ActionImpl, ExtractUuid, Message, RmwMessage, SequenceAlloc, Service}; +pub use traits::{Action, ActionImpl, Message, RmwMessage, SequenceAlloc, Service}; diff --git a/rosidl_runtime_rs/src/traits.rs b/rosidl_runtime_rs/src/traits.rs index adb65e626..f40c42f7c 100644 --- a/rosidl_runtime_rs/src/traits.rs +++ b/rosidl_runtime_rs/src/traits.rs @@ -188,19 +188,17 @@ pub trait ActionImpl: 'static { type FeedbackMessage: Message; /// The send_goal service associated with this action. - type SendGoalService: Service>; + type SendGoalService: Service; /// The cancel_goal service associated with this action. type CancelGoalService: Service; /// The get_result service associated with this action. type GetResultService: Service; -} -/// Trait for types containing a special UUID field. -/// -/// User code never needs to implement this trait, nor call its method. -pub trait ExtractUuid: 'static { - /// Copies the UUID field from `self` into the provided buffer. - fn extract_uuid(&self, bytes: &mut [u8; 16]); + /// Get the UUID of a goal request. + fn get_goal_request_uuid(request: &<::Request as Message>::RmwMsg) -> [u8; 16]; + + /// Sets the `accepted` field of a goal response. + fn set_goal_response_accepted(response: &mut <::Response as Message>::RmwMsg, accepted: bool); } From 241f64283fb60e2a302bed557f027e63727a0a38 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Wed, 7 Aug 2024 19:55:11 -0400 Subject: [PATCH 48/61] 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 594a0e3f3..6d95c21ab 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 1e75ce8adf72bcafad78f8d8a93a4cb2d2fa499f Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 9 Aug 2024 17:11:02 -0400 Subject: [PATCH 49/61] 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 +- rosidl_runtime_rs/src/traits.rs | 7 +++++-- 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 6d95c21ab..197ee6741 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 4d593b530..a77342c65 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -244,7 +244,7 @@ impl Node { 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, diff --git a/rosidl_runtime_rs/src/traits.rs b/rosidl_runtime_rs/src/traits.rs index f40c42f7c..7377dbba9 100644 --- a/rosidl_runtime_rs/src/traits.rs +++ b/rosidl_runtime_rs/src/traits.rs @@ -163,7 +163,7 @@ pub trait Service: 'static { /// Trait for actions. /// /// User code never needs to call this trait's method, much less implement this trait. -pub trait Action: 'static + ActionImpl { +pub trait Action: 'static { /// The goal message associated with this action. type Goal: Message; @@ -180,7 +180,7 @@ pub trait Action: 'static + ActionImpl { /// Trait for action implementation details. /// /// User code never needs to implement this trait, nor use its associated types. -pub trait ActionImpl: 'static { +pub trait ActionImpl: 'static + Action { /// The goal_status message associated with this action. type GoalStatusMessage: Message; @@ -201,4 +201,7 @@ pub trait ActionImpl: 'static { /// Sets the `accepted` field of a goal response. fn set_goal_response_accepted(response: &mut <::Response as Message>::RmwMsg, accepted: bool); + + /// Create a feedback message with the given goal ID and contents. + fn create_feedback_message(goal_id: &[u8; 16], feedback: &<::Feedback as Message>::RmwMsg) -> ::RmwMsg; } From ea04dcbd90cdc569b34373c6d5cead04a207dfb8 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 9 Aug 2024 17:15:32 -0400 Subject: [PATCH 50/61] 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 197ee6741..d86edc074 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 b06c14cf8c86973ea2b22899cd47697074fa0a3b Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 9 Aug 2024 17:55:57 -0400 Subject: [PATCH 51/61] 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 d86edc074..ece4515b5 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 249d5ec9ee9c47bfb10917b5eb2b80d9f085c4dc Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 9 Aug 2024 20:37:22 -0400 Subject: [PATCH 52/61] 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 ece4515b5..e52ddabac 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 213f8911e6b9bcab50c448f03855088e4ababa20 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 16 Aug 2024 18:04:03 -0400 Subject: [PATCH 53/61] Add GetResultService methods to ActionImpl --- rosidl_runtime_rs/src/traits.rs | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/rosidl_runtime_rs/src/traits.rs b/rosidl_runtime_rs/src/traits.rs index 7377dbba9..344a7fa5e 100644 --- a/rosidl_runtime_rs/src/traits.rs +++ b/rosidl_runtime_rs/src/traits.rs @@ -204,4 +204,10 @@ pub trait ActionImpl: 'static + Action { /// Create a feedback message with the given goal ID and contents. fn create_feedback_message(goal_id: &[u8; 16], feedback: &<::Feedback as Message>::RmwMsg) -> ::RmwMsg; + + /// Get the UUID of a result request. + fn get_result_request_uuid(request: &<::Request as Message>::RmwMsg) -> [u8; 16]; + + /// Sets the `status` field of a result response. + fn set_result_response_status(response: &mut <::Response as Message>::RmwMsg, status: i8); } From a7c45fcfdae38f59af69cc42880cc8f27a0e7260 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 16 Aug 2024 18:05:35 -0400 Subject: [PATCH 54/61] 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 e52ddabac..95aaab1d0 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 a33ec4869c0db55032bbeea7ec61867ad8661d5f Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 16 Aug 2024 19:15:36 -0400 Subject: [PATCH 55/61] Implement ActionImpl trait methods in generator These still don't build without errors, but it's close. --- rosidl_generator_rs/resource/action.rs.em | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em index 26182249f..130d88732 100644 --- a/rosidl_generator_rs/resource/action.rs.em +++ b/rosidl_generator_rs/resource/action.rs.em @@ -99,6 +99,29 @@ impl rosidl_runtime_rs::ActionImpl for @(type_name) { type SendGoalService = crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX); type CancelGoalService = action_msgs::srv::rmw::CancelGoal; type GetResultService = crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX); + + fn get_goal_request_uuid(request: &<::Request as rosidl_runtime_rs::Message>::RmwMsg) -> [u8; 16] { + request.goal_id.uuid + } + + fn set_goal_response_accepted(response: &mut <::Response as rosidl_runtime_rs::Message>::RmwMsg, accepted: bool) { + response.accepted = accepted; + } + + fn create_feedback_message(goal_id: &[u8; 16], feedback: &<::Feedback as rosidl_runtime_rs::Message>::RmwMsg) -> ::RmwMsg { + let mut message = ::RmwMsg::default(); + message.goal_id.uuid = *goal_id; + message.feedback = feedback.clone(); + message + } + + fn get_result_request_uuid(request: &<::Request as rosidl_runtime_rs::Message>::RmwMsg) -> [u8; 16] { + request.goal_id.uuid + } + + fn set_result_response_status(response: &mut <::Response as rosidl_runtime_rs::Message>::RmwMsg, status: i8) { + response.status = status; + } } @[end for] From e98882f447e29e1060ecc94a794873db719297cf Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 23 Aug 2024 16:31:22 -0400 Subject: [PATCH 56/61] Fix formatting in example --- examples/minimal_action_server/src/minimal_action_server.rs | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/examples/minimal_action_server/src/minimal_action_server.rs b/examples/minimal_action_server/src/minimal_action_server.rs index a964b53bb..a87eb1f9f 100644 --- a/examples/minimal_action_server/src/minimal_action_server.rs +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -1,6 +1,4 @@ -use std::env; -use std::sync::Arc; -use std::thread; +use std::{env, sync::Arc, thread}; use anyhow::{Error, Result}; From 4ff9fd483f5501caa253ae4356aa80c79cdf4b30 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 23 Aug 2024 17:29:10 -0400 Subject: [PATCH 57/61] 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 95aaab1d0..4e77fb2df 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 d8a0a1d2269fa8b7f8b1543892d7eae4c81f46e2 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 23 Aug 2024 18:32:15 -0400 Subject: [PATCH 58/61] Replace set_result_response_status with create_result_response rclrs needs to be able to generically construct result responses, including the user-defined result field. --- rosidl_generator_rs/resource/action.rs.em | 7 +++++-- rosidl_runtime_rs/src/traits.rs | 4 ++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em index 130d88732..3257d44b5 100644 --- a/rosidl_generator_rs/resource/action.rs.em +++ b/rosidl_generator_rs/resource/action.rs.em @@ -119,8 +119,11 @@ impl rosidl_runtime_rs::ActionImpl for @(type_name) { request.goal_id.uuid } - fn set_result_response_status(response: &mut <::Response as rosidl_runtime_rs::Message>::RmwMsg, status: i8) { - response.status = status; + fn create_result_response(status: i8, result: <::Result as Message>::RmwMsg) -> <::Response as Message>::RmwMsg { + <::Response as rosidl_runtime_rs::Message>::RmwMsg { + status, + result, + } } } diff --git a/rosidl_runtime_rs/src/traits.rs b/rosidl_runtime_rs/src/traits.rs index 344a7fa5e..4ac60e515 100644 --- a/rosidl_runtime_rs/src/traits.rs +++ b/rosidl_runtime_rs/src/traits.rs @@ -208,6 +208,6 @@ pub trait ActionImpl: 'static + Action { /// Get the UUID of a result request. fn get_result_request_uuid(request: &<::Request as Message>::RmwMsg) -> [u8; 16]; - /// Sets the `status` field of a result response. - fn set_result_response_status(response: &mut <::Response as Message>::RmwMsg, status: i8); + /// Create a result response message with the given status and contents. + fn create_result_response(status: i8, result: <::Result as Message>::RmwMsg) -> <::Response as Message>::RmwMsg; } From 4d597c8492182fd4ff34e36ff1d6e569e85d289b Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 23 Aug 2024 18:35:04 -0400 Subject: [PATCH 59/61] 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 4e77fb2df..3a5b8a3d8 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 be5b7e1df07c98363a5589aef505afdbb5ce0d3f Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 23 Aug 2024 19:23:50 -0400 Subject: [PATCH 60/61] 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 3a5b8a3d8..dfb4e9659 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 75fd2759ecfb63a80c641a2117a9a90cc5d4a3a8 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 28 Sep 2024 15:02:03 -0400 Subject: [PATCH 61/61] 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 ++-- rosidl_generator_rs/resource/action.rs.em | 67 +++++++++++++++++++---- rosidl_runtime_rs/src/traits.rs | 34 ++++++++++-- 3 files changed, 90 insertions(+), 23 deletions(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index dfb4e9659..0b0357224 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 diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em index 3257d44b5..4c72b90b8 100644 --- a/rosidl_generator_rs/resource/action.rs.em +++ b/rosidl_generator_rs/resource/action.rs.em @@ -6,6 +6,8 @@ from rosidl_parser.definition import ( ACTION_GOAL_SUFFIX, ACTION_RESULT_SERVICE_SUFFIX, ACTION_RESULT_SUFFIX, + SERVICE_REQUEST_MESSAGE_SUFFIX, + SERVICE_RESPONSE_MESSAGE_SUFFIX, ) action_msg_specs = [] @@ -100,31 +102,74 @@ impl rosidl_runtime_rs::ActionImpl for @(type_name) { type CancelGoalService = action_msgs::srv::rmw::CancelGoal; type GetResultService = crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX); - fn get_goal_request_uuid(request: &<::Request as rosidl_runtime_rs::Message>::RmwMsg) -> [u8; 16] { - request.goal_id.uuid + fn create_goal_request(goal_id: &[u8; 16], goal: crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SUFFIX)) -> crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX) { + crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX) { + goal_id: unique_identifier_msgs::msg::rmw::UUID { uuid: *goal_id }, + goal, + } + } + + fn get_goal_request_uuid(request: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX)) -> &[u8; 16] { + &request.goal_id.uuid + } + + fn create_goal_response(accepted: bool, stamp: (i32, u32)) -> crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX) { + crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX) { + accepted, + stamp: builtin_interfaces::msg::rmw::Time { + sec: stamp.0, + nanosec: stamp.1, + }, + } + } + + fn get_goal_response_accepted(response: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX)) -> bool { + response.accepted } - fn set_goal_response_accepted(response: &mut <::Response as rosidl_runtime_rs::Message>::RmwMsg, accepted: bool) { - response.accepted = accepted; + fn get_goal_response_stamp(response: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX)) -> (i32, u32) { + (response.stamp.sec, response.stamp.nanosec) } - fn create_feedback_message(goal_id: &[u8; 16], feedback: &<::Feedback as rosidl_runtime_rs::Message>::RmwMsg) -> ::RmwMsg { - let mut message = ::RmwMsg::default(); + fn create_feedback_message(goal_id: &[u8; 16], feedback: crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_SUFFIX)) -> crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_MESSAGE_SUFFIX) { + let mut message = crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_MESSAGE_SUFFIX)::default(); message.goal_id.uuid = *goal_id; - message.feedback = feedback.clone(); + message.feedback = feedback; message } - fn get_result_request_uuid(request: &<::Request as rosidl_runtime_rs::Message>::RmwMsg) -> [u8; 16] { - request.goal_id.uuid + fn get_feedback_message_uuid(feedback: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_MESSAGE_SUFFIX)) -> &[u8; 16] { + &feedback.goal_id.uuid + } + + fn get_feedback_message_feedback(feedback: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_MESSAGE_SUFFIX)) -> &crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_SUFFIX) { + &feedback.feedback } - fn create_result_response(status: i8, result: <::Result as Message>::RmwMsg) -> <::Response as Message>::RmwMsg { - <::Response as rosidl_runtime_rs::Message>::RmwMsg { + fn create_result_request(goal_id: &[u8; 16]) -> crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX) { + crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX) { + goal_id: unique_identifier_msgs::msg::rmw::UUID { uuid: *goal_id }, + } + } + + fn get_result_request_uuid(request: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX)) -> &[u8; 16] { + &request.goal_id.uuid + } + + fn create_result_response(status: i8, result: crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SUFFIX)) -> crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX) { + crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX) { status, result, } } + + fn get_result_response_result(response: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX)) -> &crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SUFFIX) { + &response.result + } + + fn get_result_response_status(response: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX)) -> i8 { + response.status + } } @[end for] diff --git a/rosidl_runtime_rs/src/traits.rs b/rosidl_runtime_rs/src/traits.rs index 4ac60e515..d0402dd5f 100644 --- a/rosidl_runtime_rs/src/traits.rs +++ b/rosidl_runtime_rs/src/traits.rs @@ -196,18 +196,42 @@ pub trait ActionImpl: 'static + Action { /// The get_result service associated with this action. type GetResultService: Service; + /// Create a goal request message with the given UUID and goal. + fn create_goal_request(goal_id: &[u8; 16], goal: <::Goal as Message>::RmwMsg) -> <::Request as Message>::RmwMsg; + /// Get the UUID of a goal request. - fn get_goal_request_uuid(request: &<::Request as Message>::RmwMsg) -> [u8; 16]; + fn get_goal_request_uuid(request: &<::Request as Message>::RmwMsg) -> &[u8; 16]; + + /// Create a goal response message with the given acceptance and timestamp. + fn create_goal_response(accepted: bool, stamp: (i32, u32)) -> <::Response as Message>::RmwMsg; - /// Sets the `accepted` field of a goal response. - fn set_goal_response_accepted(response: &mut <::Response as Message>::RmwMsg, accepted: bool); + /// Get the `accepted` field of a goal response. + fn get_goal_response_accepted(response: &<::Response as Message>::RmwMsg) -> bool; + + /// Get the `stamp` field of a goal response. + fn get_goal_response_stamp(response: &<::Response as Message>::RmwMsg) -> (i32, u32); /// Create a feedback message with the given goal ID and contents. - fn create_feedback_message(goal_id: &[u8; 16], feedback: &<::Feedback as Message>::RmwMsg) -> ::RmwMsg; + fn create_feedback_message(goal_id: &[u8; 16], feedback: <::Feedback as Message>::RmwMsg) -> ::RmwMsg; + + /// Get the UUID of a feedback message. + fn get_feedback_message_uuid(feedback: &::RmwMsg) -> &[u8; 16]; + + /// Get the feedback of a feedback message. + fn get_feedback_message_feedback(feedback: &::RmwMsg) -> &<::Feedback as Message>::RmwMsg; + + /// Create a result request message with the given goal ID. + fn create_result_request(goal_id: &[u8; 16]) -> <::Request as Message>::RmwMsg; /// Get the UUID of a result request. - fn get_result_request_uuid(request: &<::Request as Message>::RmwMsg) -> [u8; 16]; + fn get_result_request_uuid(request: &<::Request as Message>::RmwMsg) -> &[u8; 16]; /// Create a result response message with the given status and contents. fn create_result_response(status: i8, result: <::Result as Message>::RmwMsg) -> <::Response as Message>::RmwMsg; + + /// Get the result of a result response. + fn get_result_response_result(response: &<::Response as Message>::RmwMsg) -> &<::Result as Message>::RmwMsg; + + /// Get the status of a result response. + fn get_result_response_status(response: &<::Response as Message>::RmwMsg) -> i8; }