From cd94bc5ba574589495db22b55ef7475c6ce9acfb Mon Sep 17 00:00:00 2001 From: Martin Dahl Date: Tue, 7 Apr 2020 20:25:00 +0200 Subject: [PATCH] =?UTF-8?q?Service=20servers!=20=F0=9F=96=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 1 + build.rs | 3 + examples/service.rs | 21 +++++++ msg_gen/build.rs | 1 + msg_gen/src/lib.rs | 18 ++++++ src/lib.rs | 139 +++++++++++++++++++++++++++++++++++++++++++- 6 files changed, 180 insertions(+), 3 deletions(-) create mode 100644 examples/service.rs diff --git a/README.md b/README.md index 29c6528..daf7b09 100644 --- a/README.md +++ b/README.md @@ -23,6 +23,7 @@ What works? - Up to date with ROS2 ~Dashing~ Eloquent - Building Rust types - Publish/subscribe +- Services (servers, not clients yet) TODO -------------------- diff --git a/build.rs b/build.rs index 9bd9f1c..0c2d33d 100644 --- a/build.rs +++ b/build.rs @@ -37,6 +37,9 @@ fn main() { codegen.push_str("#[allow(non_snake_case)]\n"); codegen.push_str(&format!(" pub mod {} {{\n", msg)); codegen.push_str(" use super::super::super::*;\n"); + + codegen.push_str(&generate_rust_service(module, prefix, msg)); + for s in &["Request", "Response"] { let msgname = format!("{}_{}", msg, s); codegen.push_str(&generate_rust_msg(module, prefix, &msgname)); diff --git a/examples/service.rs b/examples/service.rs new file mode 100644 index 0000000..8d990a4 --- /dev/null +++ b/examples/service.rs @@ -0,0 +1,21 @@ +use r2r; +use failure::Error; + +use r2r::example_interfaces::srv::AddTwoInts; + +fn handle_service(request: AddTwoInts::Request) -> AddTwoInts::Response { + println!("request: {} + {}", request.a, request.b); + AddTwoInts::Response { + sum: request.a + request.b + } +} + +fn main() -> Result<(), Error> { + let ctx = r2r::Context::create()?; + let mut node = r2r::Node::create(ctx, "testnode", "")?; + node.create_service::("/add_two_ints", Box::new(handle_service))?; + + loop { + node.spin_once(std::time::Duration::from_millis(100)); + } +} diff --git a/msg_gen/build.rs b/msg_gen/build.rs index 3cbb247..84017a0 100644 --- a/msg_gen/build.rs +++ b/msg_gen/build.rs @@ -77,6 +77,7 @@ fn main() { .derive_copy(false) // blacklist types that are handled by rcl bindings .blacklist_type("rosidl_message_type_support_t") + .blacklist_type("rosidl_service_type_support_t") .blacklist_type("rosidl_generator_c__String") .blacklist_type("rosidl_generator_c__String__Sequence") .blacklist_type("rosidl_generator_c__U16String") diff --git a/msg_gen/src/lib.rs b/msg_gen/src/lib.rs index 4c3140f..488e291 100644 --- a/msg_gen/src/lib.rs +++ b/msg_gen/src/lib.rs @@ -102,6 +102,24 @@ fn field_name(field_name: &str) -> String { } } +pub fn generate_rust_service(module_: &str, prefix_: &str, name_: &str) -> String { + format!( + " + pub struct Service(); + impl WrappedServiceTypeSupport for Service {{ + type Request = Request; + type Response = Response; + fn get_ts() -> &'static rosidl_service_type_support_t {{ + unsafe {{ + &*rosidl_typesupport_c__get_service_type_support_handle__{}__{}__{}() + }} + }} + }} + + ", module_, prefix_, name_) +} + + // TODO: this is a terrible hack :) pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { let key = format!("{}__{}__{}", module_, prefix_, name_); diff --git a/src/lib.rs b/src/lib.rs index 0dee56b..0bd079b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -17,7 +17,7 @@ use error::*; pub type Result = std::result::Result; -pub trait WrappedTypesupport: Serialize + serde::de::DeserializeOwned { +pub trait WrappedTypesupport: Serialize + serde::de::DeserializeOwned + Default { type CStruct; fn get_ts() -> &'static rosidl_message_type_support_t; @@ -27,6 +27,14 @@ pub trait WrappedTypesupport: Serialize + serde::de::DeserializeOwned { fn copy_to_native(&self, msg: &mut Self::CStruct); } +pub trait WrappedServiceTypeSupport { + type Request: WrappedTypesupport; + type Response: WrappedTypesupport; + + fn get_ts() -> &'static rosidl_service_type_support_t; +} + + #[derive(Debug)] pub struct WrappedNativeMsg where @@ -255,6 +263,64 @@ impl Sub for WrappedSubUntyped } } + +// services +struct WrappedService +where + T: WrappedServiceTypeSupport, +{ + rcl_handle: rcl_service_t, + rcl_request: rmw_request_id_t, + callback: Box T::Response>, + rcl_request_msg: WrappedNativeMsg, +} + +pub trait Service { + fn handle(&self) -> &rcl_service_t; + fn run_cb(&mut self) -> (); + fn rcl_request_id(&mut self) -> *mut rmw_request_id_t; + fn rcl_request_msg(&mut self) -> *mut std::os::raw::c_void; + fn destroy(&mut self, node: &mut rcl_node_t) -> (); +} + +impl Service for WrappedService +where + T: WrappedServiceTypeSupport, +{ + fn handle(&self) -> &rcl_service_t { + &self.rcl_handle + } + + fn rcl_request_msg(&mut self) -> *mut std::os::raw::c_void { + self.rcl_request_msg.void_ptr_mut() + } + + fn rcl_request_id(&mut self) -> *mut rmw_request_id_t { + &mut self.rcl_request + } + + fn run_cb(&mut self) -> () { + // copy native msg to rust type and run callback + let request = T::Request::from_native(&self.rcl_request_msg); + let response = (self.callback)(request); + let mut native_response = WrappedNativeMsg::::from(&response); + let res = unsafe { + rcl_send_response(&self.rcl_handle, &mut self.rcl_request, native_response.void_ptr_mut()) + }; + + // TODO + if res != RCL_RET_OK as i32 { + panic!("service error {}", res); + } + } + + fn destroy(&mut self, node: &mut rcl_node_t) { + unsafe { + rcl_service_fini(&mut self.rcl_handle, node); + } + } +} + // The publish function is thread safe. ROS2 docs state: // ============= // @@ -347,6 +413,8 @@ pub struct Node { node_handle: Box, // the node owns the subscribers subs: Vec>, + // servies, + services: Vec>, // and the publishers, whom we allow to be shared.. hmm. pubs: Vec>, } @@ -379,6 +447,7 @@ impl Node { context: ctx, node_handle: node_handle, subs: Vec::new(), + services: Vec::new(), pubs: Vec::new(), }) } else { @@ -464,11 +533,54 @@ impl Node { Ok(self.subs.last().unwrap().handle()) // hmm... } + pub fn create_service_helper(&mut self, service_name: &str, + service_ts: *const rosidl_service_type_support_t) + -> Result { + let mut service_handle = unsafe { rcl_get_zero_initialized_service() }; + let service_name_c_string = CString::new(service_name) + .map_err(|_|Error::RCL_RET_INVALID_ARGUMENT)?; + + let result = unsafe { + let service_options = rcl_service_get_default_options(); + rcl_service_init(&mut service_handle, self.node_handle.as_mut(), + service_ts, service_name_c_string.as_ptr(), &service_options) + }; + if result == RCL_RET_OK as i32 { + Ok(service_handle) + } else { + Err(Error::from_rcl_error(result)) + } + } + + pub fn create_service( + &mut self, + service_name: &str, + callback: Box T::Response>, + ) -> Result<&rcl_service_t> + where + T: WrappedServiceTypeSupport, + { + let service_handle = self.create_service_helper(service_name, T::get_ts())?; + let ws = WrappedService:: { + rcl_handle: service_handle, + rcl_request: rmw_request_id_t { + writer_guid: [0; 16usize], + sequence_number: 0, + }, + rcl_request_msg: WrappedNativeMsg::::new(), + callback: callback, + }; + + self.services.push(Box::new(ws)); + Ok(self.services.last().unwrap().handle()) // hmm... + } + pub fn create_publisher_helper(&mut self, topic: &str, typesupport: *const rosidl_message_type_support_t) -> Result { let mut publisher_handle = unsafe { rcl_get_zero_initialized_publisher() }; - let topic_c_string = CString::new(topic).map_err(|_|Error::RCL_RET_INVALID_ARGUMENT)?; + let topic_c_string = CString::new(topic) + .map_err(|_|Error::RCL_RET_INVALID_ARGUMENT)?; let result = unsafe { let mut publisher_options = rcl_publisher_get_default_options(); @@ -534,7 +646,7 @@ impl Node { 0, 0, 0, - 0, + self.services.len(), 0, ctx.as_mut(), rcutils_get_default_allocator(), @@ -551,6 +663,12 @@ impl Node { } } + for s in self.services.iter() { + unsafe { + rcl_wait_set_add_service(&mut ws, s.handle(), std::ptr::null_mut()); + } + } + unsafe { rcl_wait(&mut ws, timeout); } @@ -570,6 +688,21 @@ impl Node { } } + + let ws_services = + unsafe { std::slice::from_raw_parts(ws.services, ws.size_of_services) }; + assert_eq!(ws_services.len(), self.services.len()); + for (s, ws_s) in self.services.iter_mut().zip(ws_services) { + if ws_s != &std::ptr::null() { + let ret = unsafe { + rcl_take_request(s.handle(), s.rcl_request_id(), s.rcl_request_msg()) + }; + if ret == RCL_RET_OK as i32 { + s.run_cb(); + } + } + } + unsafe { rcl_wait_set_fini(&mut ws); }