diff --git a/Cargo.toml b/Cargo.toml index ec80080..14d6258 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -15,6 +15,8 @@ lazy_static = "1.4.0" common = { path = "common", version = "0.0.3" } rcl = { path = "rcl", version = "0.0.3" } msg_gen = { path = "msg_gen", version = "0.0.3" } +actions = { path = "actions", version = "0.0.1" } +uuid = { version = "0.8", features = ["serde", "v4"] } [dev-dependencies] serde_json = "1.0.62" diff --git a/actions/Cargo.toml b/actions/Cargo.toml new file mode 100644 index 0000000..45c8142 --- /dev/null +++ b/actions/Cargo.toml @@ -0,0 +1,16 @@ +[package] +name = "actions" +version = "0.0.1" +authors = ["Martin Dahl "] +edition = "2018" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +rcl = { path = "../rcl", version = "0.0.3" } +msg_gen = { path = "../msg_gen", version = "0.0.3" } + +[build-dependencies] +bindgen = "0.57.0" +itertools = "0.10.0" +common = { path = "../common", version = "0.0.3" } diff --git a/actions/build.rs b/actions/build.rs new file mode 100644 index 0000000..43c7bca --- /dev/null +++ b/actions/build.rs @@ -0,0 +1,82 @@ +use bindgen; +use itertools::Itertools; +use std::env; +use std::path::{Path, PathBuf}; + +fn main() { + let mut builder = bindgen::Builder::default() + .header("src/action_wrapper.h") + .derive_copy(false) + .size_t_is_usize(true) + .default_enum_style(bindgen::EnumVariation::Rust { + non_exhaustive: false, + }); + + if let Some(cmake_includes) = env::var("CMAKE_INCLUDE_DIRS").ok() { + // we are running from cmake, do special thing. + let mut includes = cmake_includes.split(":").collect::>(); + includes.sort(); + includes.dedup(); + + for x in &includes { + let clang_arg = format!("-I{}", x); + println!("adding clang arg: {}", clang_arg); + builder = builder.clang_arg(clang_arg); + } + + env::var("CMAKE_LIBRARIES") + .unwrap_or(String::new()) + .split(":") + .into_iter() + .filter(|s| s.contains(".so") || s.contains(".dylib")) + .flat_map(|l| Path::new(l).parent().and_then(|p| p.to_str())) + .unique() + .for_each(|pp| { + println!("cargo:rustc-link-search=native={}", pp) + // we could potentially do the below instead of hardcoding which libs we rely on. + // let filename = path.file_stem().and_then(|f| f.to_str()).unwrap(); + // let without_lib = filename.strip_prefix("lib").unwrap(); + // println!("cargo:rustc-link-lib=dylib={}", without_lib); + }); + } else { + let ament_prefix_var_name = "AMENT_PREFIX_PATH"; + let ament_prefix_var = env::var(ament_prefix_var_name).expect("Source your ROS!"); + + for ament_prefix_path in ament_prefix_var.split(":") { + builder = builder.clang_arg(format!("-I{}/include", ament_prefix_path)); + println!( + "added include search dir: {}", + format!("-I{}/include", ament_prefix_path) + ); + println!("cargo:rustc-link-search=native={}/lib", ament_prefix_path); + } + } + + println!("cargo:rustc-link-lib=dylib=rcl_action"); + + let bindings = builder + .no_debug("_OSUnaligned.*") + // whitelist a few specific things that we need. + .whitelist_recursively(false) + .whitelist_type("rcl_action_client_t").opaque_type("rcl_action_client_t") + .whitelist_type("rcl_action_server_t").opaque_type("rcl_action_server_t") + .whitelist_type("rcl_action_goal_info_t").opaque_type("rcl_action_goal_info_t") + .whitelist_type("rcl_action_goal_handle_t").opaque_type("rcl_action_goal_handle_t") + .whitelist_type("rcl_action_cancel_request_t").opaque_type("rcl_action_cancel_request_t") + .whitelist_type("rcl_action_cancel_response_t").opaque_type("rcl_action_cancel_response_t") + .whitelist_type("rcl_action_goal_info_t").opaque_type("rcl_action_goal_info_t") + .whitelist_type("rcl_action_goal_event_t").opaque_type("rcl_action_goal_event_t") + .whitelist_type("rcl_action_goal_state_t").opaque_type("rcl_action_goal_state_t") + .whitelist_type("rcl_action_goal_status_array_t").opaque_type("rcl_action_goal_status_array_t") + .whitelist_function("rcl_action_.*") + // .whitelist_type("rosidl_action_type_support_t").opaque_type("rosidl_action_type_support_t") + .whitelist_type("rcl_action_client_options_t") + .whitelist_type("rcl_action_server_options_t") + .generate() + .expect("Unable to generate bindings"); + + let out_path = PathBuf::from(env::var("OUT_DIR").unwrap()); + bindings + .write_to_file(out_path.join("action_bindings.rs")) + .expect("Couldn't write bindings!"); +} diff --git a/actions/src/action_wrapper.h b/actions/src/action_wrapper.h new file mode 100644 index 0000000..91d050f --- /dev/null +++ b/actions/src/action_wrapper.h @@ -0,0 +1 @@ +#include diff --git a/actions/src/lib.rs b/actions/src/lib.rs new file mode 100644 index 0000000..08a012c --- /dev/null +++ b/actions/src/lib.rs @@ -0,0 +1,9 @@ +#![allow(non_upper_case_globals)] +#![allow(non_camel_case_types)] +#![allow(non_snake_case)] +#![allow(improper_ctypes)] +#![allow(dead_code)] +include!(concat!(env!("OUT_DIR"), "/action_bindings.rs")); + +use rcl::*; +use msg_gen::*; diff --git a/build.rs b/build.rs index 3f6864c..43901f7 100644 --- a/build.rs +++ b/build.rs @@ -57,6 +57,27 @@ fn main() { codegen.push_str(&msg_gen::generate_rust_msg(module, prefix, &msgname)); println!("cargo:rustc-cfg=r2r__{}__{}__{}", module, prefix, msg); } + + // "internal" services that implements the action type + for srv in &["SendGoal", "GetResult"] { + codegen.push_str("#[allow(non_snake_case)]\n"); + codegen.push_str(&format!(" pub mod {} {{\n", srv)); + codegen.push_str(" use super::super::super::super::*;\n"); + + let srvname = format!("{}_{}", msg, srv); + codegen.push_str(&msg_gen::generate_rust_service(module, prefix, &srvname)); + + for s in &["Request", "Response"] { + let msgname = format!("{}_{}_{}", msg, srv, s); + codegen.push_str(&msg_gen::generate_rust_msg(module, prefix, &msgname)); + } + codegen.push_str(" }\n"); + } + + // also "internal" feedback message type that wraps the feedback type with a uuid + let feedback_msgname = format!("{}_FeedbackMessage", msg); + codegen.push_str(&msg_gen::generate_rust_msg(module, prefix, &feedback_msgname)); + codegen.push_str(" }\n"); } } else if prefix == &"srv" { diff --git a/examples/action_client.rs b/examples/action_client.rs new file mode 100644 index 0000000..4356c5a --- /dev/null +++ b/examples/action_client.rs @@ -0,0 +1,48 @@ +use r2r; +use r2r::example_interfaces::action::Fibonacci; +use std::cell::RefCell; +use std::rc::Rc; + +fn main() -> Result<(), Box> { + let ctx = r2r::Context::create()?; + let mut node = r2r::Node::create(ctx, "testnode", "")?; + let client = node.create_action_client::("/fibonacci")?; + + println!("waiting for action service..."); + while !node.action_server_available(&client)? { + std::thread::sleep(std::time::Duration::from_millis(500)); + } + println!("action service available."); + + let goal = Fibonacci::Goal { order: 10 }; + let goal_accepted = Rc::new(RefCell::new(None)); + let cb_ga = goal_accepted.clone(); + let cb = Box::new(move |r: Fibonacci::SendGoal::Response| { + println!("got response {:?}", r); + *cb_ga.borrow_mut() = Some(r.accepted); + }); + + let feedback_cb = Box::new(move |fb: Fibonacci::Feedback| { + println!("got feedback {:?}", fb); + }); + + let result_cb = Box::new(move |r: Fibonacci::Result| { + println!("final result {:?}", r); + }); + + println!("sending goal: {:?}", goal); + client.send_goal_request(goal, cb, feedback_cb, result_cb)?; + + let mut c = 0; + loop { + node.spin_once(std::time::Duration::from_millis(1000)); + std::thread::sleep(std::time::Duration::from_millis(1000)); + c += 1; + if c > 100 { + println!("shutdown"); + break; + } + } + + Ok(()) +} diff --git a/msg_gen/build.rs b/msg_gen/build.rs index afd8f93..a5694c5 100644 --- a/msg_gen/build.rs +++ b/msg_gen/build.rs @@ -82,11 +82,21 @@ fn main() { introspecion_map.push_str(&format!("m.insert(\"{}\", {});\n", key, val)); } } else if msg.prefix == "action" { - for s in &["Goal", "Result", "Feedback"] { + for s in &["Goal", "Result", "Feedback", "FeedbackMessage"] { let key = &format!("{}__{}__{}_{}", &msg.module, &msg.prefix, &msg.name, s); let val = &format!("unsafe {{ rosidl_typesupport_introspection_c__get_message_type_support_handle__{}__{}__{}_{}() }} as *const i32 as usize", &msg.module, &msg.prefix, &msg.name, s); introspecion_map.push_str(&format!("m.insert(\"{}\", {});\n", key, val)); } + // "internal" services + for srv in &["SendGoal", "GetResult"] { + // TODO: refactor this is copy paste from services... + for s in &["Request", "Response"] { + let msgname = format!("{}_{}_{}", msg.name, srv, s); + let key = &format!("{}__{}__{}", &msg.module, &msg.prefix, msgname); + let val = &format!("unsafe {{ rosidl_typesupport_introspection_c__get_message_type_support_handle__{}__{}__{}() }} as *const i32 as usize", &msg.module, &msg.prefix, msgname); + introspecion_map.push_str(&format!("m.insert(\"{}\", {});\n", key, val)); + } + } } else { let key = &format!("{}__{}__{}", &msg.module, &msg.prefix, &msg.name); let val = &format!("unsafe {{ rosidl_typesupport_introspection_c__get_message_type_support_handle__{}__{}__{}() }} as *const i32 as usize", &msg.module, &msg.prefix, &msg.name); diff --git a/msg_gen/src/lib.rs b/msg_gen/src/lib.rs index 6c969a5..276f8f2 100644 --- a/msg_gen/src/lib.rs +++ b/msg_gen/src/lib.rs @@ -50,9 +50,9 @@ fn field_type(t: u8) -> String { } else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_DOUBLE as u8) { "f64".to_owned() } else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_LONG_DOUBLE as u8) { + // f128 does not exist in rust "u128".to_owned() } - // f128 does not exist in rust else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_MESSAGE as u8) { "message".to_owned() } else { @@ -129,15 +129,42 @@ pub fn generate_rust_action(module_: &str, prefix_: &str, name_: &str) -> String type Goal = Goal; type Result = Result; type Feedback = Feedback; + + // internal structs + type FeedbackMessage = FeedbackMessage; + type SendGoal = SendGoal::Service; + type GetResult = GetResult::Service; + fn get_ts() -> &'static rosidl_action_type_support_t {{ unsafe {{ - &*rosidl_typesupport_c__get_action_type_support_handle__{}__{}__{}() + &*rosidl_typesupport_c__get_action_type_support_handle__{module}__{prefix}__{name}() }} }} + + fn make_goal_request_msg(goal_id: unique_identifier_msgs::msg::UUID, goal: Goal) -> SendGoal::Request {{ + SendGoal::Request {{ + goal_id, + goal + }} + }} + + fn make_result_request_msg(goal_id: unique_identifier_msgs::msg::UUID) -> GetResult::Request {{ + GetResult::Request {{ + goal_id, + }} + }} + + fn destructure_feedback_msg(msg: FeedbackMessage) -> (unique_identifier_msgs::msg::UUID, Feedback) {{ + (msg.goal_id, msg.feedback) + }} + + fn destructure_result_response_msg(msg: GetResult::Response) -> (i8, Result) {{ + (msg.status, msg.result) + }} }} ", - module_, prefix_, name_ + module = module_, prefix = prefix_, name = name_ ) } @@ -146,7 +173,7 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { let key = format!("{}__{}__{}", module_, prefix_, name_); let ptr = INTROSPECTION_FNS .get(key.as_str()) - .expect(&format!("code generation error: {}", name_)); + .expect(&format!("code generation error: {}", key)); let ptr = *ptr as *const i32 as *const rosidl_message_type_support_t; unsafe { let (module, prefix, mut name, c_struct, members) = introspection(ptr); @@ -159,14 +186,15 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { // we only want to keep the last part. // same for actions with _Goal, _Result, _Feedback // TODO: refactor... - let mut nn = name.splitn(2, "_"); - let _mod_name = nn - .next() - .expect(&format!("malformed service name {}", name)); - let msg_name = nn - .next() - .expect(&format!("malformed service name {}", name)); - name = msg_name.to_owned(); + // handle special case of ActionName_ServiceName_Response + let nn = name.splitn(3, "_").collect::>(); + if let [ _mod_name, _srv_name, msg_name ] = &nn[..] { + name = msg_name.to_string(); + } else if let [ _mod_name, msg_name ] = &nn[..] { + name = msg_name.to_string(); + } else { + panic!("malformed service name {}", name); + } } let mut fields = String::new(); @@ -176,12 +204,32 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { let rust_field_type = field_type(member.type_id_); let rust_field_type = if rust_field_type == "message" { let (module, prefix, name, _, _) = introspection(member.members_); - format!( - "{module}::{prefix}::{msgname}", - module = module, - prefix = prefix, - msgname = name - ) + // hack here to rustify nested action type names + if prefix == "action" { + if let Some((n1,n2)) = name.rsplit_once("_") { + format!("{module}::{prefix}::{srvname}::{msgname}", + module = module, + prefix = prefix, + srvname = n1, + msgname = n2 + ) + } + else { + format!( + "{module}::{prefix}::{msgname}", + module = module, + prefix = prefix, + msgname = name + ) + } + } else { + format!( + "{module}::{prefix}::{msgname}", + module = module, + prefix = prefix, + msgname = name + ) + } } else { rust_field_type }; @@ -266,7 +314,16 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { )); } else if rust_field_type == "message" { let (module, prefix, name, _, _) = introspection(member.members_); - from_native.push_str(&format!("{field_name}: {module}::{prefix}::{msgname}::from_native(&msg.{field_name}),\n", field_name = field_name, module = module, prefix=prefix, msgname = name)); + // same hack as above to rustify message type names + if prefix == "action" { + if let Some((n1,n2)) = name.rsplit_once("_") { + from_native.push_str(&format!("{field_name}: {module}::{prefix}::{srvname}::{msgname}::from_native(&msg.{field_name}),\n", field_name = field_name, module = module, prefix=prefix, srvname = n1, msgname = n2)); + } else { + panic!("ooops at from_native"); + } + } else { + from_native.push_str(&format!("{field_name}: {module}::{prefix}::{msgname}::from_native(&msg.{field_name}),\n", field_name = field_name, module = module, prefix=prefix, msgname = name)); + } } else { from_native.push_str(&format!( "{field_name}: msg.{field_name},\n", diff --git a/src/lib.rs b/src/lib.rs index e1d13f5..7988873 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -11,9 +11,11 @@ use std::marker::PhantomData; use std::mem::MaybeUninit; use std::ops::{Deref, DerefMut}; use std::time::Duration; +use std::fmt::Debug; use msg_gen::*; use rcl::*; +use actions::*; mod error; use error::*; @@ -23,7 +25,7 @@ pub use utils::*; pub type Result = std::result::Result; -pub trait WrappedTypesupport: Serialize + serde::de::DeserializeOwned + Default { +pub trait WrappedTypesupport: Serialize + serde::de::DeserializeOwned + Default + Debug { type CStruct; fn get_ts() -> &'static rosidl_message_type_support_t; @@ -45,7 +47,20 @@ pub trait WrappedActionTypeSupport { type Result: WrappedTypesupport; type Feedback: WrappedTypesupport; + // internal... + type FeedbackMessage: WrappedTypesupport; + type SendGoal: WrappedServiceTypeSupport; + type GetResult: WrappedServiceTypeSupport; + fn get_ts() -> &'static rosidl_action_type_support_t; + + fn make_goal_request_msg(goal_id: unique_identifier_msgs::msg::UUID, goal: Self::Goal) -> + <::SendGoal as WrappedServiceTypeSupport>::Request; + fn make_result_request_msg(goal_id: unique_identifier_msgs::msg::UUID) -> + <::GetResult as WrappedServiceTypeSupport>::Request; + fn destructure_feedback_msg(msg: Self::FeedbackMessage) -> (unique_identifier_msgs::msg::UUID, Self::Feedback); + fn destructure_result_response_msg(msg: <::GetResult as WrappedServiceTypeSupport>::Response) -> + (i8, Self::Result); } #[derive(Debug)] @@ -411,6 +426,206 @@ where } } + +// action clients +#[derive(Debug, Copy, Clone, PartialEq)] +enum GoalStatus { + Unknown, + Accepted, + Executing, + Canceling, + Succeeded, + Canceled, + Aborted, +} + +impl GoalStatus { + #[allow(dead_code)] + pub fn to_rcl(&self) -> i8 { + match self { + GoalStatus::Unknown => 0, + GoalStatus::Accepted => 1, + GoalStatus::Executing => 2, + GoalStatus::Canceling => 3, + GoalStatus::Succeeded => 4, + GoalStatus::Canceled => 5, + GoalStatus::Aborted => 6, + } + } + + pub fn from_rcl(s: i8) -> Self { + match s { + 0 => GoalStatus::Unknown, + 1 => GoalStatus::Accepted, + 2 => GoalStatus::Executing, + 3 => GoalStatus::Canceling, + 4 => GoalStatus::Succeeded, + 5 => GoalStatus::Canceled, + 6 => GoalStatus::Aborted, + _ => panic!("unknown action status: {}", s), + } + } +} + +struct WrappedActionClient +where + T: WrappedActionTypeSupport, +{ + rcl_handle: rcl_action_client_t, + goal_request_callbacks: Vec<(i64, Box::SendGoal as WrappedServiceTypeSupport>::Response)>)>, + feedback_callbacks: Vec<(uuid::Uuid, Box ()>)>, + goal_statuses: Vec<(uuid::Uuid, GoalStatus)>, + result_request_callbacks: Vec<(i64, Box::GetResult as WrappedServiceTypeSupport>::Response)>)>, + result_callbacks: Vec<(uuid::Uuid, Box ()>)>, +} + +pub trait ActionClient_ { + fn handle(&self) -> &rcl_action_client_t; + fn destroy(&mut self, node: &mut rcl_node_t) -> (); + + fn handle_goal_response(&mut self) -> (); + fn handle_feedback_msg(&mut self) -> (); + fn handle_result_response(&mut self) -> (); + + fn run_result_request(&mut self, uuid: &uuid::Uuid) -> (); +} + + +use std::convert::TryInto; +fn vec_to_uuid_bytes(v: Vec) -> [T; 16] { + v.try_into() + .unwrap_or_else(|v: Vec| panic!("Expected a Vec of length {} but it was {}", 16, v.len())) +} + + +impl WrappedActionClient +where + T: WrappedActionTypeSupport, +{ + pub fn send_result_request(&mut self, uuid: &uuid::Uuid, cb: Box ()>) -> Result<()> { + let uuid_msg = unique_identifier_msgs::msg::UUID { uuid: uuid.as_bytes().to_vec() }; + let request_msg = T::make_result_request_msg(uuid_msg); + let native_msg = WrappedNativeMsg::<<::GetResult as + WrappedServiceTypeSupport>::Request>::from(&request_msg); + let mut seq_no = 0i64; + let result = + unsafe { rcl_action_send_result_request(&self.rcl_handle, native_msg.void_ptr(), &mut seq_no) }; + + if result == RCL_RET_OK as i32 { + self.result_request_callbacks.push((seq_no, Box::new(move |r: <::GetResult as + WrappedServiceTypeSupport>::Response| { + let (status, result) = T::destructure_result_response_msg(r); + let status = GoalStatus::from_rcl(status); + println!("status: {:?}, result: {:?}", status, result); + (cb)(result); + }))); + Ok(()) + } else { + eprintln!("coult not send request {}", result); + Err(Error::from_rcl_error(result)) + } + } +} + +impl ActionClient_ for WrappedActionClient +where + T: WrappedActionTypeSupport, +{ + fn handle(&self) -> &rcl_action_client_t { + &self.rcl_handle + } + + fn handle_goal_response(&mut self) -> () { + let mut request_id = MaybeUninit::::uninit(); + let mut response_msg = WrappedNativeMsg::<<::SendGoal as WrappedServiceTypeSupport>::Response>::new(); + + let ret = unsafe { + rcl_action_take_goal_response(&self.rcl_handle, request_id.as_mut_ptr(), response_msg.void_ptr_mut()) + }; + if ret == RCL_RET_OK as i32 { + let request_id = unsafe { request_id.assume_init() }; + if let Some(idx) = self.goal_request_callbacks.iter().position(|(id, _)| id == &request_id.sequence_number) { + let (_, cb_to_run) = self.goal_request_callbacks.swap_remove(idx); + let response = <::SendGoal as WrappedServiceTypeSupport>::Response::from_native(&response_msg); + (cb_to_run)(response); + } else { + // I don't think this should be able to occur? Let's panic so we + // find out... + let we_have: String = self + .goal_request_callbacks + .iter() + .map(|(id, _)| id.to_string()) + .collect::>() + .join(","); + eprintln!( + "no such req id: {}, we have [{}], ignoring", + request_id.sequence_number, we_have + ); + } + } + } + + fn handle_feedback_msg(&mut self) -> () { + let mut feedback_msg = WrappedNativeMsg::::new(); + let ret = unsafe { + rcl_action_take_feedback(&self.rcl_handle, feedback_msg.void_ptr_mut()) + }; + if ret == RCL_RET_OK as i32 { + let msg = T::FeedbackMessage::from_native(&feedback_msg); + let (uuid, feedback) = T::destructure_feedback_msg(msg); + let msg_uuid = uuid::Uuid::from_bytes(vec_to_uuid_bytes(uuid.uuid)); + if let Some((_,cb)) = self.feedback_callbacks.iter_mut().find(|(uuid, _)| uuid == &msg_uuid) { + (cb)(feedback); + } + } + } + + fn handle_result_response(&mut self) -> () { + let mut request_id = MaybeUninit::::uninit(); + let mut response_msg = WrappedNativeMsg::<<::GetResult as WrappedServiceTypeSupport>::Response>::new(); + + let ret = unsafe { + rcl_action_take_result_response(&self.rcl_handle, request_id.as_mut_ptr(), response_msg.void_ptr_mut()) + }; + + if ret == RCL_RET_OK as i32 { + let request_id = unsafe { request_id.assume_init() }; + if let Some(idx) = self.result_request_callbacks.iter().position(|(id, _)| id == &request_id.sequence_number) { + let (_, cb_to_run) = self.result_request_callbacks.swap_remove(idx); + let response = <::GetResult as WrappedServiceTypeSupport>::Response::from_native(&response_msg); + (cb_to_run)(response); + } else { + // I don't think this should be able to occur? Let's panic so we + // find out... + let we_have: String = self + .result_request_callbacks + .iter() + .map(|(id, _)| id.to_string()) + .collect::>() + .join(","); + eprintln!( + "no such req id: {}, we have [{}], ignoring", + request_id.sequence_number, we_have + ); + } + } + } + + fn run_result_request(&mut self, uuid: &uuid::Uuid) -> () { + if let Some(idx) = self.result_callbacks.iter().position(|(cb_uuid, _)| cb_uuid == uuid) { + let (_, result_cb) = self.result_callbacks.swap_remove(idx); + println!("asking for final result for {}", uuid); + self.send_result_request(uuid, result_cb).unwrap(); // TODO error handling. + } + } + + fn destroy(&mut self, node: &mut rcl_node_t) { + unsafe { + rcl_action_client_fini(&mut self.rcl_handle, node); + } + } +} + // The publish function is thread safe. ROS2 docs state: // ============= // @@ -465,6 +680,15 @@ where client_: Weak>>, } +unsafe impl Send for ActionClient where T: WrappedActionTypeSupport {} + +pub struct ActionClient +where + T: WrappedActionTypeSupport, +{ + client_: Weak>>, +} + #[derive(Debug, Clone)] pub struct Context { context_handle: Arc>, @@ -637,6 +861,8 @@ pub struct Node { services: Vec>, // clients with hack to avoid locking just to wait..., clients: Vec<(rcl_client_t, Arc>)>, + // action clients + action_clients: Vec<(rcl_action_client_t, Arc>)>, // timers, timers: Vec, // and the publishers, whom we allow to be shared.. hmm. @@ -767,6 +993,7 @@ impl Node { subs: Vec::new(), services: Vec::new(), clients: Vec::new(), + action_clients: Vec::new(), timers: Vec::new(), pubs: Vec::new(), }; @@ -981,6 +1208,76 @@ impl Node { } } + pub fn create_action_client_helper( + &mut self, + action_name: &str, + action_ts: *const rosidl_action_type_support_t, + ) -> Result { + let mut client_handle = unsafe { rcl_action_get_zero_initialized_client() }; + let action_name_c_string = + CString::new(action_name).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?; + + let result = unsafe { + let client_options = rcl_action_client_get_default_options(); + rcl_action_client_init( + &mut client_handle, + self.node_handle.as_mut(), + action_ts, + action_name_c_string.as_ptr(), + &client_options, + ) + }; + if result == RCL_RET_OK as i32 { + Ok(client_handle) + } else { + Err(Error::from_rcl_error(result)) + } + } + + pub fn create_action_client(&mut self, action_name: &str) -> Result> + where + T: WrappedActionTypeSupport, + { + let client_handle = self.create_action_client_helper(action_name, T::get_ts())?; + let cloned_handle = unsafe { core::ptr::read(&client_handle) }; + let wa = WrappedActionClient:: { + rcl_handle: cloned_handle, + goal_request_callbacks: Vec::new(), + feedback_callbacks: Vec::new(), + goal_statuses: Vec::new(), + result_request_callbacks: Vec::new(), + result_callbacks: Vec::new(), + }; + + let arc = Arc::new(Mutex::new(wa)); + let client_ = Arc::downgrade(&arc); + self.action_clients.push((client_handle, arc)); + let c = ActionClient { client_ }; + Ok(c) + } + + pub fn action_server_available( + &self, + client: &ActionClient, + ) -> Result { + let client = client + .client_ + .upgrade() + .ok_or(Error::RCL_RET_CLIENT_INVALID)?; + let client = client.lock().unwrap(); + let mut avail = false; + let result = unsafe { + rcl_action_server_is_available(self.node_handle.as_ref(), client.handle(), &mut avail) + }; + + if result == RCL_RET_OK as i32 { + Ok(avail) + } else { + eprintln!("coult not send request {}", result); + Err(Error::from_rcl_error(result)) + } + } + pub fn create_publisher_helper( &mut self, topic: &str, @@ -1036,6 +1333,19 @@ impl Node { Ok(p) } + fn get_num_waits(rcl_handle: &rcl_action_client_t, num_subs: &mut usize, num_gc: &mut usize , num_timers: &mut usize, + num_clients: &mut usize , num_services: &mut usize) -> Result<()> { + unsafe { + let result = rcl_action_client_wait_set_get_num_entities(rcl_handle, num_subs, num_gc, + num_timers, num_clients, num_services); + if result == RCL_RET_OK as i32 { + Ok(()) + } else { + Err(Error::from_rcl_error(result)) + } + } + } + pub fn spin_once(&mut self, timeout: Duration) { let timeout = timeout.as_nanos() as i64; let mut ws = unsafe { rcl_get_zero_initialized_wait_set() }; @@ -1046,16 +1356,39 @@ impl Node { // #[doc = "* For example, calling rcl_wait() in two threads on two different wait sets"] // #[doc = "* that both contain a single, shared guard condition is undefined behavior."] + // count action client wait set needs + let mut total_action_subs = 0; + let mut total_action_clients = 0; + for (ach, _) in &self.action_clients { + let mut num_subs = 0; + let mut num_gc = 0; + let mut num_timers = 0; + let mut num_clients = 0; + let mut num_services = 0; + + Self::get_num_waits(ach, &mut num_subs, &mut num_gc, &mut num_timers, &mut num_clients, &mut num_services) + .expect("could not get action client wait sets"); + // sanity check + assert_eq!(num_subs, 2); + assert_eq!(num_clients, 3); + assert_eq!(num_gc, 0); + assert_eq!(num_timers, 0); + assert_eq!(num_services, 0); + + total_action_subs += num_subs; + total_action_clients += num_clients; + } + { let mut ctx = self.context.context_handle.lock().unwrap(); unsafe { rcl_wait_set_init( &mut ws, - self.subs.len(), + self.subs.len() + total_action_subs, 0, self.timers.len(), - self.clients.len(), + self.clients.len() + total_action_clients, self.services.len(), 0, ctx.as_mut(), @@ -1091,6 +1424,16 @@ impl Node { } } + // code below assumes that actions are added last... perhaps a bad assumption. + for (ac, _) in self.action_clients.iter() { + unsafe { + rcl_action_wait_set_add_action_client(&mut ws, + ac, + std::ptr::null_mut(), + std::ptr::null_mut()); + } + } + let ret = unsafe { rcl_wait(&mut ws, timeout) }; if ret == RCL_RET_TIMEOUT as i32 { @@ -1101,8 +1444,7 @@ impl Node { } let ws_subs = - unsafe { std::slice::from_raw_parts(ws.subscriptions, ws.size_of_subscriptions) }; - assert_eq!(ws_subs.len(), self.subs.len()); + unsafe { std::slice::from_raw_parts(ws.subscriptions, self.subs.len()) }; let mut msg_info = rmw_message_info_t::default(); // we dont care for now for (s, ws_s) in self.subs.iter_mut().zip(ws_subs) { if ws_s != &std::ptr::null() { @@ -1139,8 +1481,7 @@ impl Node { } } - let ws_clients = unsafe { std::slice::from_raw_parts(ws.clients, ws.size_of_clients) }; - assert_eq!(ws_clients.len(), self.clients.len()); + let ws_clients = unsafe { std::slice::from_raw_parts(ws.clients, self.clients.len()) }; for ((_, s), ws_s) in self.clients.iter_mut().zip(ws_clients) { if ws_s != &std::ptr::null() { let mut s = s.lock().unwrap(); @@ -1166,6 +1507,64 @@ impl Node { } } + for (ac, s) 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; + + let ret = unsafe { + rcl_action_client_wait_set_get_entities_ready(&ws, ac, + &mut is_feedback_ready, + &mut is_status_ready, + &mut is_goal_response_ready, + &mut is_cancel_response_ready, + &mut is_result_response_ready) + }; + + if ret != RCL_RET_OK as i32 { + continue; + } + + if is_goal_response_ready { + let mut acs = s.lock().unwrap(); + acs.handle_goal_response(); + } + + if is_result_response_ready { + let mut acs = s.lock().unwrap(); + acs.handle_result_response(); + } + + if is_feedback_ready { + let mut acs = s.lock().unwrap(); + acs.handle_feedback_msg(); + } + + if is_status_ready { + let mut status_array = WrappedNativeMsg::::new(); + let ret = unsafe { + rcl_action_take_status(ac, status_array.void_ptr_mut()) + }; + if ret == RCL_RET_OK as i32 { + let arr = action_msgs::msg::GoalStatusArray::from_native(&status_array); + // TODO: actually use this information. + for a in &arr.status_list { + let uuid = uuid::Uuid::from_bytes(vec_to_uuid_bytes(a.goal_info.goal_id.uuid.clone())); + let status = GoalStatus::from_rcl(a.status); + println!("goal status for {}: {:?}", uuid, status); + + if status == GoalStatus::Succeeded { + // query for the result. + let mut acs = s.lock().unwrap(); + acs.run_result_request(&uuid); + } + } + } + } + } + unsafe { rcl_wait_set_fini(&mut ws); } @@ -1400,6 +1799,46 @@ where } } +impl ActionClient +where + T: WrappedActionTypeSupport, +{ + pub fn send_goal_request(&self, goal: T::Goal, + cb: Box::SendGoal as WrappedServiceTypeSupport>::Response) -> ()>, + feedback_cb: Box ()>, + result_cb: Box ()>) -> Result<()> + where + T: WrappedActionTypeSupport, + { + // upgrade to actual ref. if still alive + let client = self + .client_ + .upgrade() + .ok_or(Error::RCL_RET_CLIENT_INVALID)?; + let mut client = client.lock().unwrap(); + // copy rust msg to native and publish it + let uuid = uuid::Uuid::new_v4(); + let uuid_msg = unique_identifier_msgs::msg::UUID { uuid: uuid.as_bytes().to_vec() }; + println!("UUID: {:?}", uuid); + client.feedback_callbacks.push((uuid, feedback_cb)); + client.result_callbacks.push((uuid, result_cb)); + let request_msg = T::make_goal_request_msg(uuid_msg, goal); + let native_msg = WrappedNativeMsg::<<::SendGoal as + WrappedServiceTypeSupport>::Request>::from(&request_msg); + let mut seq_no = 0i64; + let result = + unsafe { rcl_action_send_goal_request(&client.rcl_handle, native_msg.void_ptr(), &mut seq_no) }; + + if result == RCL_RET_OK as i32 { + client.goal_request_callbacks.push((seq_no, cb)); + Ok(()) + } else { + eprintln!("coult not send request {}", result); + Err(Error::from_rcl_error(result)) + } + } +} + impl PublisherUntyped { pub fn publish(&self, msg: serde_json::Value) -> Result<()> { // upgrade to actual ref. if still alive @@ -1761,5 +2200,10 @@ mod tests { let fb2 = Fibonacci::Feedback::from_native(&fbn); println!("feedback2 {:?}", fb2); assert_eq!(fb, fb2); + + let fb = WrappedNativeMsg::::new(); + let fb1 = Fibonacci::Feedback::default(); + let fb2 = Fibonacci::Feedback::from_native(&fb); + assert_eq!(fb1, fb2); } }