diff --git a/examples/minimal_action_client/Cargo.toml b/examples/minimal_action_client/Cargo.toml new file mode 100644 index 000000000..96275ba4e --- /dev/null +++ b/examples/minimal_action_client/Cargo.toml @@ -0,0 +1,22 @@ +[package] +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 "] +edition = "2021" + +[[bin]] +name = "minimal_action_client" +path = "src/minimal_action_client.rs" + +[dependencies] +anyhow = {version = "1", features = ["backtrace"]} + +[dependencies.rclrs] +version = "0.4" + +[dependencies.rosidl_runtime_rs] +version = "0.4" + +[dependencies.example_interfaces] +version = "*" diff --git a/examples/minimal_action_client/package.xml b/examples/minimal_action_client/package.xml new file mode 100644 index 000000000..a2576efb1 --- /dev/null +++ b/examples/minimal_action_client/package.xml @@ -0,0 +1,26 @@ + + + + examples_rclrs_minimal_action_client + 0.3.1 + Minimal action client examples for rclrs. + Esteve Fernandez + Nikolai Morin + + Jacob Hassold + Apache License 2.0 + + rclrs + rosidl_runtime_rs + example_interfaces + + rclrs + rosidl_runtime_rs + example_interfaces + + + ament_cargo + + diff --git a/examples/minimal_action_client/src/minimal_action_client.rs b/examples/minimal_action_client/src/minimal_action_client.rs new file mode 100644 index 000000000..e67c46656 --- /dev/null +++ b/examples/minimal_action_client/src/minimal_action_client.rs @@ -0,0 +1,14 @@ +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_client")?; + + let _client = + node.create_action_client::("fibonacci")?; + + Ok(()) +} diff --git a/examples/minimal_action_server/Cargo.toml b/examples/minimal_action_server/Cargo.toml new file mode 100644 index 000000000..3ec27526c --- /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.4" + +[dependencies.rosidl_runtime_rs] +version = "0.4" + +[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_server/src/minimal_action_server.rs b/examples/minimal_action_server/src/minimal_action_server.rs new file mode 100644 index 000000000..a964b53bb --- /dev/null +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -0,0 +1,81 @@ +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"); + let feedback = example_interfaces::action::Fibonacci_Feedback { + sequence: [0, 1].to_vec(), + }; + + for i in 1..goal_handle.goal_request.order { + if goal_handle.is_canceling() { + let result = example_interfaces::action::Fibonacci_Result { + sequence: Vec::new(), + }; + + goal_handle.canceled(&result); + println!("Goal canceled"); + return; + } + + // Update sequence sequence + feedback + .sequence + .push(feedback.sequence[i as usize] + feedback.sequence[(i - 1) as usize]); + // Publish feedback + goal_handle.publish_feedback(&feedback); + println!("Publishing feedback"); + thread::sleep(std::time::Duration::from_millis(100)); + } + + let result = example_interfaces::action::Fibonacci_Result { + sequence: Vec::new(), + }; + result.sequence = feedback.sequence.clone(); + goal_handle.succeed(&result); + println!("Goal succeeded"); +} + +fn handle_accepted(goal_handle: Arc) { + thread::spawn(move || { + execute(goal_handle); + }); +} + +fn main() -> Result<(), Error> { + let context = rclrs::Context::new(env::args())?; + + 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 new file mode 100644 index 000000000..46e895788 --- /dev/null +++ b/rclrs/src/action.rs @@ -0,0 +1,109 @@ +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, +{ + _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 + where + T: rosidl_runtime_rs::Action, + { + Ok(Self { + _marker: Default::default(), + }) + } +} + +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, + 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/lib.rs b/rclrs/src/lib.rs index 325cd5b9a..67cc0c6b9 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; @@ -30,6 +31,7 @@ pub mod dynamic_message; use std::sync::Arc; use std::time::Duration; +pub use action::*; pub use arguments::*; pub use client::*; pub use clock::*; @@ -40,6 +42,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 f1792c1e6..5b3ca6f01 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -13,9 +13,10 @@ pub use self::builder::*; pub use self::graph::*; use crate::rcl_bindings::*; use crate::{ - Client, ClientBase, Clock, Context, GuardCondition, ParameterBuilder, ParameterInterface, - ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service, ServiceBase, - Subscription, SubscriptionBase, SubscriptionCallback, TimeSource, ToResult, + ActionClient, ActionServer, CancelResponse, Client, ClientBase, Clock, Context, GoalResponse, + GoalUUID, GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, + Publisher, QoSProfile, RclrsError, ServerGoalHandle, Service, ServiceBase, Subscription, + SubscriptionBase, SubscriptionCallback, TimeSource, ToResult, }; impl Drop for rcl_node_t { @@ -195,6 +196,49 @@ impl Node { Ok(client) } + /// Creates an [`ActionClient`][1]. + /// + /// [1]: crate::ActionClient + // TODO: make action client's lifetime depend on node's lifetime + pub fn create_action_client( + &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, + )?); + // self.clients + // .push(Arc::downgrade(&client) as Weak); + Ok(action_client) + } + + /// Creates an [`ActionServer`][1]. + /// + /// [1]: crate::ActionServer + // TODO: make action server's lifetime depend on node's lifetime + pub fn create_action_server( + &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. /// /// A weak pointer to the `GuardCondition` is stored within this node. 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 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 fd48a5545..6dd727132 100644 --- a/rosidl_generator_rs/cmake/rosidl_generator_rs_generate_interfaces.cmake +++ b/rosidl_generator_rs/cmake/rosidl_generator_rs_generate_interfaces.cmake @@ -53,13 +53,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() @@ -107,12 +107,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}") @@ -125,7 +126,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..b37919dc3 --- /dev/null +++ b/rosidl_generator_rs/resource/action.rs.em @@ -0,0 +1,56 @@ +@{ +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, + 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', + 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) +}@ + +@[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_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(), 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 d468a42d5..fee02fb5b 100644 --- a/rosidl_runtime_rs/src/traits.rs +++ b/rosidl_runtime_rs/src/traits.rs @@ -160,3 +160,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; +}