From b79146c0aaf8bc92f23f46ebbde599a6808ff76e Mon Sep 17 00:00:00 2001 From: Martin Dahl Date: Fri, 30 Aug 2019 10:43:21 +0200 Subject: [PATCH] Cleanup untyped part 2 --- build.rs | 8 +- examples/publish_complex_msgs.rs | 18 +- examples/rostopic_echo.rs | 21 +- examples/subscriber.rs | 14 +- examples/subscriber_with_thread.rs | 7 +- msg_gen/src/lib.rs | 335 ++++++++++++++++------------- src/lib.rs | 84 ++++---- tests/threads.rs | 10 +- 8 files changed, 266 insertions(+), 231 deletions(-) diff --git a/build.rs b/build.rs index f13edc6..337e1d5 100644 --- a/build.rs +++ b/build.rs @@ -39,14 +39,14 @@ fn main() { codegen.push_str("}\n"); } - let codegen_typehacks = generate_untyped_helpers(&msgs_list); + let untyped_helper = generate_untyped_helper(&msgs_list); let out_path = PathBuf::from(env::var("OUT_DIR").unwrap()); let msgs_fn = out_path.join("generated_msgs.rs"); - let hacks_fn = out_path.join("generated_typehacks.rs"); + let untyped_fn = out_path.join("generated_untyped_helper.rs"); let mut f = File::create(msgs_fn).unwrap(); write!(f, "{}", codegen).unwrap(); - let mut f = File::create(hacks_fn).unwrap(); - write!(f, "{}", codegen_typehacks).unwrap(); + let mut f = File::create(untyped_fn).unwrap(); + write!(f, "{}", untyped_helper).unwrap(); } diff --git a/examples/publish_complex_msgs.rs b/examples/publish_complex_msgs.rs index 0801aa5..49cb6d1 100644 --- a/examples/publish_complex_msgs.rs +++ b/examples/publish_complex_msgs.rs @@ -1,18 +1,18 @@ -use r2r::*; -use builtin_interfaces::msg::Duration; -use trajectory_msgs::msg::*; -use std_msgs::msg::Int32; +use r2r; +use r2r::builtin_interfaces::msg::Duration; +use r2r::trajectory_msgs::msg::*; +use r2r::std_msgs::msg::Int32; use failure::Error; fn main() -> Result<(), Error> { - let ctx = Context::create()?; - let mut node = Node::create(ctx, "testnode", "")?; + let ctx = r2r::Context::create()?; + let mut node = r2r::Node::create(ctx, "testnode", "")?; let publisher = node.create_publisher::("/hej")?; let publisher2 = node.create_publisher::("/native_count")?; let mut c = 0; let mut positions: Vec = Vec::new(); - let cb = move |x:std_msgs::msg::String| { + let cb = move |x:r2r::std_msgs::msg::String| { println!("at count {} got: {}", c, x.data); c = c + 1; positions.push(c as f64); @@ -21,7 +21,7 @@ fn main() -> Result<(), Error> { time_from_start : Duration { sec: c, nanosec: 0 }, ..Default::default() }; - let mut native = WrappedNativeMsg::::new(); + let mut native = r2r::WrappedNativeMsg::::new(); native.data = c; publisher.publish(&to_send).unwrap(); @@ -33,7 +33,7 @@ fn main() -> Result<(), Error> { println!("JTP serialized as: {}", serialized); }; - let cb3 = move |raw_c:&WrappedNativeMsg| { + let cb3 = move |raw_c:&r2r::WrappedNativeMsg| { println!("Raw c data: {:?}", raw_c.positions); }; diff --git a/examples/rostopic_echo.rs b/examples/rostopic_echo.rs index 43941b8..c0cf3c2 100644 --- a/examples/rostopic_echo.rs +++ b/examples/rostopic_echo.rs @@ -1,12 +1,12 @@ -use r2r::*; +use r2r; use std::thread; use std::env; use std::collections::HashMap; use failure::Error; fn main() -> Result<(), Error> { - let ctx = Context::create()?; - let mut node = Node::create(ctx, "echo", "")?; + let ctx = r2r::Context::create()?; + let mut node = r2r::Node::create(ctx, "echo", "")?; let args: Vec = env::args().collect(); let topic = args.get(1).expect("provide a topic!"); @@ -36,10 +36,17 @@ fn main() -> Result<(), Error> { let echo = &format!("{}_echo", topic); let echo_pub = node.create_publisher_untyped(echo, type_name)?; - let cb = move |msg: serde_json::Value | { - let s = serde_json::to_string_pretty(&msg).unwrap(); - println!("{}\n---\n", &s); - echo_pub.publish(msg).unwrap(); + let cb = move |msg: r2r::Result | { + match msg { + Ok(msg) => { + let s = serde_json::to_string_pretty(&msg).unwrap(); + println!("{}\n---\n", &s); + echo_pub.publish(msg).unwrap(); + } + Err(err) => { + println!("Could not parse msg. {}", err); + } + } }; let _subref = node.subscribe_untyped(topic, type_name, Box::new(cb))?; diff --git a/examples/subscriber.rs b/examples/subscriber.rs index 3ae97ef..d21410a 100644 --- a/examples/subscriber.rs +++ b/examples/subscriber.rs @@ -1,24 +1,24 @@ -use r2r::*; +use r2r; use std::sync::mpsc; use std::thread; use failure::Error; fn main() -> Result<(), Error> { - let ctx = Context::create()?; + let ctx = r2r::Context::create()?; let th = { - let mut node = Node::create(ctx, "testnode", "")?; + let mut node = r2r::Node::create(ctx, "testnode", "")?; let (tx, rx) = mpsc::channel::(); let p = node - .create_publisher::("/hej") + .create_publisher::("/hej") .unwrap(); let th = thread::spawn(move || loop { println!("thread looping"); let des = if let Ok(msg) = rx.recv() { - let deserialized: std_msgs::msg::String = serde_json::from_str(&msg).unwrap(); + let deserialized: r2r::std_msgs::msg::String = serde_json::from_str(&msg).unwrap(); println!( "received: {}, deserialized ros msg = {:#?}", msg, deserialized @@ -34,12 +34,12 @@ fn main() -> Result<(), Error> { }); let tx1 = tx.clone(); - let cb = move |x: std_msgs::msg::String| { + let cb = move |x: r2r::std_msgs::msg::String| { let serialized = serde_json::to_string(&x).unwrap(); tx1.send(serialized).unwrap(); // pass msg on to other thread for printing }; - let cb2 = move |x: &WrappedNativeMsg| { + let cb2 = move |x: &r2r::WrappedNativeMsg| { // use native data! let s = x.data.to_str(); println!("native ros msg: {}", s); diff --git a/examples/subscriber_with_thread.rs b/examples/subscriber_with_thread.rs index b03294e..151e2f5 100644 --- a/examples/subscriber_with_thread.rs +++ b/examples/subscriber_with_thread.rs @@ -2,11 +2,12 @@ use std::sync::mpsc; use std::thread; use failure::Error; -use r2r::*; +use r2r; +use r2r::std_msgs; fn main() -> Result<(), Error> { - let ctx = Context::create()?; - let mut node = Node::create(ctx, "testnode", "")?; + let ctx = r2r::Context::create()?; + let mut node = r2r::Node::create(ctx, "testnode", "")?; let publisher = node.create_publisher::("/hej")?; let pubint = node.create_publisher::("/count")?; diff --git a/msg_gen/src/lib.rs b/msg_gen/src/lib.rs index d6bed8c..d566734 100644 --- a/msg_gen/src/lib.rs +++ b/msg_gen/src/lib.rs @@ -9,55 +9,103 @@ include!(concat!(env!("OUT_DIR"), "/introspection_functions.rs")); #[macro_use] extern crate lazy_static; -use std::collections::HashMap; use rcl::*; +use std::collections::HashMap; use std::ffi::CStr; fn field_type(t: u8) -> String { // lovely... // move to common - if t == (rosidl_typesupport_introspection_c__ROS_TYPE_STRING as u8) { "std::string::String".to_owned() } - else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_BOOLEAN as u8) { "bool".to_owned() } - else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_CHAR as u8) { "i8".to_owned() } - else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_WCHAR as u8) { "u16".to_owned() } - else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_OCTET as u8) { "u8".to_owned() } - else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_UINT8 as u8) { "u8".to_owned() } - else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_INT8 as u8) { "i8".to_owned() } - else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_UINT16 as u8) { "u16".to_owned() } - else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_INT16 as u8) { "i16".to_owned() } - else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_UINT32 as u8) { "u32".to_owned() } - else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_INT32 as u8) { "i32".to_owned() } - else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_UINT64 as u8) { "u64".to_owned() } - else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_INT64 as u8) { "i64".to_owned() } - else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_FLOAT as u8) { "f32".to_owned() } - 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) { "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 { panic!("ros native type not implemented: {}", t); } + if t == (rosidl_typesupport_introspection_c__ROS_TYPE_STRING as u8) { + "std::string::String".to_owned() + } else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_BOOLEAN as u8) { + "bool".to_owned() + } else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_CHAR as u8) { + "i8".to_owned() + } else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_WCHAR as u8) { + "u16".to_owned() + } else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_OCTET as u8) { + "u8".to_owned() + } else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_UINT8 as u8) { + "u8".to_owned() + } else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_INT8 as u8) { + "i8".to_owned() + } else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_UINT16 as u8) { + "u16".to_owned() + } else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_INT16 as u8) { + "i16".to_owned() + } else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_UINT32 as u8) { + "u32".to_owned() + } else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_INT32 as u8) { + "i32".to_owned() + } else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_UINT64 as u8) { + "u64".to_owned() + } else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_INT64 as u8) { + "i64".to_owned() + } else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_FLOAT as u8) { + "f32".to_owned() + } 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) { + "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 { + panic!("ros native type not implemented: {}", t); + } } -unsafe fn introspection<'a>(ptr: *const rosidl_message_type_support_t) -> (String, String, String, String, &'a [rosidl_typesupport_introspection_c__MessageMember]) { +unsafe fn introspection<'a>( + ptr: *const rosidl_message_type_support_t, +) -> ( + String, + String, + String, + String, + &'a [rosidl_typesupport_introspection_c__MessageMember], +) { let members = (*ptr).data as *const rosidl_typesupport_introspection_c__MessageMembers; - let namespace = CStr::from_ptr((*members).message_namespace_).to_str().unwrap(); + let namespace = CStr::from_ptr((*members).message_namespace_) + .to_str() + .unwrap(); let name = CStr::from_ptr((*members).message_name_).to_str().unwrap(); let nn: Vec<&str> = namespace.split("__").into_iter().take(2).collect(); - let (module, prefix) = ( nn[0], nn[1] ); - let c_struct = format!("{module}__{prefix}__{msgname}", module = module, prefix=prefix, msgname = name); - let memberslice = std::slice::from_raw_parts((*members).members_, (*members).member_count_ as usize); - (module.to_owned(), prefix.to_owned(), name.to_owned(), c_struct, memberslice) + let (module, prefix) = (nn[0], nn[1]); + let c_struct = format!( + "{module}__{prefix}__{msgname}", + module = module, + prefix = prefix, + msgname = name + ); + let memberslice = + std::slice::from_raw_parts((*members).members_, (*members).member_count_ as usize); + ( + module.to_owned(), + prefix.to_owned(), + name.to_owned(), + c_struct, + memberslice, + ) } fn field_name(field_name: &str) -> String { // check for reserved words - if field_name == "type" { "type_".into() } else { field_name.to_owned() } + if field_name == "type" { + "type_".into() + } else { + field_name.to_owned() + } } // TODO: this is a terrible hack :) 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_)); + let ptr = INTROSPECTION_FNS + .get(key.as_str()) + .expect(&format!("code generation error: {}", name_)); let ptr = *ptr as *const i32 as *const rosidl_message_type_support_t; unsafe { let (module, prefix, name, c_struct, members) = introspection(ptr); @@ -72,25 +120,35 @@ 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) - } else { rust_field_type }; + format!( + "{module}::{prefix}::{msgname}", + module = module, + prefix = prefix, + msgname = name + ) + } else { + rust_field_type + }; let s = if member.is_array_ { // if member.array_size_ > 0 { - // fixed size array - // format!("pub {}: [{};{}usize],\n",field_name, rust_field_type, array_size) - // actually lets use a vector anyway because its more convenient with traits. assert on the fixed size instead! + // fixed size array + // format!("pub {}: [{};{}usize],\n",field_name, rust_field_type, array_size) + // actually lets use a vector anyway because its more convenient with traits. assert on the fixed size instead! //} else { - // vector type - format!("pub {}: Vec<{}>,\n",field_name, rust_field_type) - //} + // vector type + format!("pub {}: Vec<{}>,\n", field_name, rust_field_type) + //} } else { - format!("pub {}: {},\n",field_name, rust_field_type) + format!("pub {}: {},\n", field_name, rust_field_type) }; fields.push_str(&s); } let mut from_native = String::new(); - from_native.push_str(&format!("fn from_native(msg: &Self::CStruct) -> {} {{\n", name)); + from_native.push_str(&format!( + "fn from_native(msg: &Self::CStruct) -> {} {{\n", + name + )); from_native.push_str(&format!(" {} {{\n", name)); for member in members { @@ -101,25 +159,40 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { if rust_field_type == "message" { let (module, prefix, name, _, _) = introspection(member.members_); from_native.push_str(&format!("{field_name} : {{\n", field_name = field_name)); - from_native.push_str(&format!("let mut temp = Vec::with_capacity(msg.{field_name}.size);\n",field_name = field_name)); + from_native.push_str(&format!( + "let mut temp = Vec::with_capacity(msg.{field_name}.size);\n", + field_name = field_name + )); from_native.push_str(&format!("let slice = unsafe {{ std::slice::from_raw_parts(msg.{field_name}.data, msg.{field_name}.size)}};\n",field_name = field_name)); from_native.push_str(&format!("for s in slice {{ temp.push({module}::{prefix}::{msgname}::from_native(s)); }}\n", module = module, prefix=prefix, msgname = name)); from_native.push_str("temp },\n"); } else { if member.array_size_ > 0 { // fixed size array, copy elements (happens to be the same now that we are using vectors...) - from_native.push_str(&format!("{field_name}: msg.{field_name}.to_vec(),\n", field_name = field_name)); + from_native.push_str(&format!( + "{field_name}: msg.{field_name}.to_vec(),\n", + field_name = field_name + )); } else { - from_native.push_str(&format!("{field_name}: msg.{field_name}.to_vec(),\n", field_name = field_name)); + from_native.push_str(&format!( + "{field_name}: msg.{field_name}.to_vec(),\n", + field_name = field_name + )); } } } else if rust_field_type == "std::string::String" { - from_native.push_str(&format!("{field_name}: msg.{field_name}.to_str().to_owned(),\n", field_name = field_name)); + from_native.push_str(&format!( + "{field_name}: msg.{field_name}.to_str().to_owned(),\n", + field_name = field_name + )); } 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)); } else { - from_native.push_str(&format!("{field_name}: msg.{field_name},\n", field_name = field_name)); + from_native.push_str(&format!( + "{field_name}: msg.{field_name},\n", + field_name = field_name + )); } } from_native.push_str(" }\n }\n"); @@ -134,7 +207,11 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { if member.is_array_ { if rust_field_type == "message" { let (_, _, _, c_struct, _) = introspection(member.members_); - copy_to_native.push_str(&format!("unsafe {{ {c_struct}__Sequence__fini(&mut msg.{field_name}) }};\n", c_struct = c_struct, field_name = field_name)); + copy_to_native.push_str(&format!( + "unsafe {{ {c_struct}__Sequence__fini(&mut msg.{field_name}) }};\n", + c_struct = c_struct, + field_name = field_name + )); copy_to_native.push_str(&format!("unsafe {{ {c_struct}__Sequence__init(&mut msg.{field_name}, self.{field_name}.len()) }};\n", c_struct = c_struct, field_name = field_name)); copy_to_native.push_str(&format!("let slice = unsafe {{ std::slice::from_raw_parts_mut(msg.{field_name}.data, msg.{field_name}.size)}};\n",field_name = field_name)); copy_to_native.push_str(&format!("for (t,s) in slice.iter_mut().zip(&self.{field_name}) {{ s.copy_to_native(t);}}\n", field_name=field_name)); @@ -148,20 +225,33 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { // extra assertion copy_to_native.push_str(&format!("assert!(self.{field_name}.len() <= {array_size}, \"Field {{}} is upper bounded by {{}}!\", \"{field_name}\", {array_size});\n", field_name = field_name, array_size = member.array_size_)); } - copy_to_native.push_str(&format!("msg.{field_name}.update(&self.{field_name});\n", field_name = field_name)); + copy_to_native.push_str(&format!( + "msg.{field_name}.update(&self.{field_name});\n", + field_name = field_name + )); } } } else if rust_field_type == "std::string::String" { - copy_to_native.push_str(&format!("msg.{field_name}.assign(&self.{field_name});\n", field_name = field_name)); + copy_to_native.push_str(&format!( + "msg.{field_name}.assign(&self.{field_name});\n", + field_name = field_name + )); } else if rust_field_type == "message" { - copy_to_native.push_str(&format!("self.{field_name}.copy_to_native(&mut msg.{field_name});\n", field_name = field_name)); + copy_to_native.push_str(&format!( + "self.{field_name}.copy_to_native(&mut msg.{field_name});\n", + field_name = field_name + )); } else { - copy_to_native.push_str(&format!("msg.{field_name} = self.{field_name};\n", field_name = field_name)); + copy_to_native.push_str(&format!( + "msg.{field_name} = self.{field_name};\n", + field_name = field_name + )); } } copy_to_native.push_str("}\n"); - let typesupport = format!("impl WrappedTypesupport for {msgname} {{ \n + let typesupport = format!( + "impl WrappedTypesupport for {msgname} {{ \n type CStruct = {c_struct}; \n\n fn get_ts() -> &'static rosidl_message_type_support_t {{ \n unsafe {{ &*rosidl_typesupport_c__get_message_type_support_handle__{c_struct}() }} @@ -174,124 +264,63 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { }}\n {from_native}\n\n {copy_to_native}\n\n - }}\n", msgname = name, c_struct = &c_struct, from_native=from_native, copy_to_native=copy_to_native); + }}\n", + msgname = name, + c_struct = &c_struct, + from_native = from_native, + copy_to_native = copy_to_native + ); - let module_str = format!(" + let module_str = format!( + " #[derive(Clone,Debug,Default,PartialEq,Serialize,Deserialize)] pub struct {msgname} {{\n {fields} }}\n {typesupport}\n\n - ", msgname = name, fields = fields, typesupport = typesupport); + ", + msgname = name, + fields = fields, + typesupport = typesupport + ); module_str } - } - -// this is even worse, it was added as an afterthought when I wanted to implement rostopic echo -pub fn generate_untyped_helpers(msgs: &Vec) -> String { - let mut ts_helper = format!("fn untyped_ts_helper(typename: &str) -> Result<&'static rosidl_message_type_support_t> {{"); - for msg in msgs { - ts_helper.push_str(&generate_untyped_ts_helper(&msg.module, &msg.prefix, &msg.name)); +pub fn generate_untyped_helper(msgs: &Vec) -> String { + let open = String::from( + " +impl WrappedNativeMsgUntyped { + fn new_from(typename: &str) -> Result { +", + ); + let close = String::from( + " + else + { + return Err(Error::InvalidMessageType{ msgtype: typename.into() }) + } } - ts_helper.push_str(&format!("return Err(Error::InvalidMessageType{{ msgtype: typename.into() }})\n}}")); +} +", + ); - let mut ds_helper = format!("fn untyped_deserialize_helper(typename: &str) -> Result serde_json::Value> {{"); + let mut lines = String::new(); for msg in msgs { - ds_helper.push_str(&generate_untyped_deserialize_helper(&msg.module, &msg.prefix, &msg.name)); + let typename = format!("{}/{}/{}", msg.module, msg.prefix, msg.name); + let rustname = format!("{}::{}::{}", msg.module, msg.prefix, msg.name); + + lines.push_str(&format!( + " + if typename == \"{typename}\" {{ + return Ok(WrappedNativeMsgUntyped::new::<{rustname}>()); + }} +", + typename = typename, + rustname = rustname + )); } - ds_helper.push_str(&format!("return Err(Error::InvalidMessageType{{ msgtype: typename.into() }})\n}}")); - let mut se_helper = format!(" -fn untyped_serialize_helper(typename: &str) -> Result std::result::Result<*mut std::os::raw::c_void, serde_json::error::Error>> {{"); - for msg in msgs { - se_helper.push_str(&generate_untyped_serialize_helper(&msg.module, &msg.prefix, &msg.name)); - } - se_helper.push_str(&format!("return Err(Error::InvalidMessageType{{ msgtype: typename.into() }})\n}}")); - - let mut alloc_helper = format!("fn untyped_alloc_helper(typename: &str) -> Result<*mut std::os::raw::c_void> {{"); - for msg in msgs { - alloc_helper.push_str(&generate_untyped_alloc_helper(&msg.module, &msg.prefix, &msg.name)); - } - alloc_helper.push_str(&format!("return Err(Error::InvalidMessageType{{ msgtype: typename.into() }})\n}}")); - - let mut dealloc_helper = format!("fn untyped_dealloc_helper(typename: &str) -> Result {{"); - for msg in msgs { - dealloc_helper.push_str(&generate_untyped_dealloc_helper(&msg.module, &msg.prefix, &msg.name)); - } - dealloc_helper.push_str(&format!("return Err(Error::InvalidMessageType{{ msgtype: typename.into() }})\n}}")); - - format!("{} \n\n {} \n\n {} \n\n {} \n\n {} \n\n", ts_helper, ds_helper, se_helper, alloc_helper, dealloc_helper) -} - - -pub fn generate_untyped_ts_helper(module_: &str, prefix_: &str, name_: &str) -> String { - let typename = format!("{}/{}/{}", module_, prefix_, name_); - let rustname = format!("{}::{}::{}", module_, prefix_, name_); - - format!(" - if typename == \"{typename}\" {{ - return Ok({rustname}::get_ts()); - }} -", typename = typename, rustname = rustname) -} - -pub fn generate_untyped_deserialize_helper(module_: &str, prefix_: &str, name_: &str) -> String { - let typename = format!("{}/{}/{}", module_, prefix_, name_); - let rustname = format!("{}::{}::{}", module_, prefix_, name_); - - format!(" - if typename == \"{typename}\" {{ - let x = | native: *const std::os::raw::c_void | {{ - let ptr = native as *const <{rustname} as WrappedTypesupport>::CStruct; - let msg = unsafe {{ {rustname}::from_native(&*ptr) }}; - serde_json::to_value(&msg).unwrap() // should never crash, we serialize from a known struct - }}; - return Ok(x); - }}", typename = typename, rustname = rustname) -} - -pub fn generate_untyped_serialize_helper(module_: &str, prefix_: &str, name_: &str) -> String { - let typename = format!("{}/{}/{}", module_, prefix_, name_); - let rustname = format!("{}::{}::{}", module_, prefix_, name_); - - format!(" - if typename == \"{typename}\" {{ - let x = | json: serde_json::Value | {{ - serde_json::from_value(json).map(|msg: {rustname}| {{ - let native = {rustname}::create_msg(); - unsafe {{ msg.copy_to_native(&mut *native); }} - native as *mut std::os::raw::c_void }}) - }}; - return Ok(x); - }}", typename = typename, rustname = rustname) -} - - -pub fn generate_untyped_alloc_helper(module_: &str, prefix_: &str, name_: &str) -> String { - let typename = format!("{}/{}/{}", module_, prefix_, name_); - let rustname = format!("{}::{}::{}", module_, prefix_, name_); - - format!(" - if typename == \"{typename}\" {{ - return Ok({rustname}::create_msg() as *mut std::os::raw::c_void); - }} -", typename = typename, rustname = rustname) -} - - -pub fn generate_untyped_dealloc_helper(module_: &str, prefix_: &str, name_: &str) -> String { - let typename = format!("{}/{}/{}", module_, prefix_, name_); - let rustname = format!("{}::{}::{}", module_, prefix_, name_); - - format!(" - if typename == \"{typename}\" {{ - let y = | native: *mut std::os::raw::c_void | {{ - let native_msg = native as *mut <{rustname} as WrappedTypesupport>::CStruct; - {rustname}::destroy_msg(native_msg); - }}; - return Ok(y); - }}", typename = typename, rustname = rustname) + format!("{}{}{}", open, lines, close) } diff --git a/src/lib.rs b/src/lib.rs index 65f6a80..7647447 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,5 +1,5 @@ include!(concat!(env!("OUT_DIR"), "/generated_msgs.rs")); -include!(concat!(env!("OUT_DIR"), "/generated_typehacks.rs")); +include!(concat!(env!("OUT_DIR"), "/generated_untyped_helper.rs")); #[macro_use] extern crate failure_derive; use serde::{Deserialize, Serialize}; @@ -15,9 +15,9 @@ use rcl::*; mod error; use error::*; -type Result = std::result::Result; +pub type Result = std::result::Result; -pub trait WrappedTypesupport { +pub trait WrappedTypesupport: Serialize + serde::de::DeserializeOwned { type CStruct; fn get_ts() -> &'static rosidl_message_type_support_t; @@ -47,16 +47,7 @@ pub struct WrappedNativeMsgUntyped { } impl WrappedNativeMsgUntyped { - fn new_from(typename: &str) -> Result { - if typename == "std_msgs/msg/String" { - Ok(WrappedNativeMsgUntyped::new::()) - } else - { return Err(Error::InvalidMessageType{ msgtype: typename.into() }) } - } -} - -impl WrappedNativeMsgUntyped { - fn new() -> Self where T: WrappedTypesupport + Serialize + serde::de::DeserializeOwned { + fn new() -> Self where T: WrappedTypesupport { let destroy = | native: *mut std::os::raw::c_void | { let native_msg = native as *mut T::CStruct; T::destroy_msg(native_msg); @@ -87,10 +78,18 @@ impl WrappedNativeMsgUntyped { json.map_err(|serde_err|Error::SerdeError { err: serde_err.to_string() }) } - fn from_json(&self, json: serde_json::Value) -> Result<()> { + fn from_json(&mut self, json: serde_json::Value) -> Result<()> { (self.msg_from_json)(self.msg, json). map_err(|serde_err|Error::SerdeError { err: serde_err.to_string() }) } + + pub fn void_ptr(&self) -> *const std::os::raw::c_void { + self.msg as *const _ as *const std::os::raw::c_void + } + + pub fn void_ptr_mut(&mut self) -> *mut std::os::raw::c_void { + self.msg as *mut _ as *mut std::os::raw::c_void + } } impl Drop for WrappedNativeMsgUntyped { @@ -180,10 +179,8 @@ where struct WrappedSubUntyped { rcl_handle: rcl_subscription_t, - callback: Box ()>, - serialize: Box serde_json::Value>, - dealloc: Box ()>, - rcl_msg: *mut std::os::raw::c_void, + rcl_msg: WrappedNativeMsgUntyped, + callback: Box) -> ()>, } impl Sub for WrappedSub @@ -243,19 +240,18 @@ impl Sub for WrappedSubUntyped } fn rcl_msg(&mut self) -> *mut std::os::raw::c_void { - self.rcl_msg + self.rcl_msg.void_ptr_mut() } fn run_cb(&mut self) -> () { - let string = (self.serialize)(self.rcl_msg); - (self.callback)(string); + let json = self.rcl_msg.to_json(); + (self.callback)(json); } fn destroy(&mut self, node: &mut rcl_node_t) { unsafe { rcl_subscription_fini(&mut self.rcl_handle, node); } - (self.dealloc)(self.rcl_msg); // manually delete message } } @@ -454,19 +450,15 @@ impl Node { &mut self, topic: &str, topic_type: &str, - callback: Box ()>, + callback: Box) -> ()>, ) -> Result<&rcl_subscription_t> { - let ts = untyped_ts_helper(topic_type)?; - let de = untyped_deserialize_helper(topic_type)?; - let subscription_handle = self.create_subscription_helper(topic, ts)?; - let dealloc = untyped_dealloc_helper(topic_type)?; + let msg = WrappedNativeMsgUntyped::new_from(topic_type)?; + let subscription_handle = self.create_subscription_helper(topic, msg.ts)?; let ws = WrappedSubUntyped { rcl_handle: subscription_handle, - rcl_msg: untyped_alloc_helper(topic_type)?, + rcl_msg: msg, callback: callback, - serialize: Box::new(de), - dealloc: Box::new(dealloc), }; self.subs.push(Box::new(ws)); Ok(self.subs.last().unwrap().handle()) // hmm... @@ -505,7 +497,7 @@ impl Node { } pub fn create_publisher_untyped(&mut self, topic: &str, topic_type: &str) -> Result { - let ts = untyped_ts_helper(topic_type)?; + let dummy = WrappedNativeMsgUntyped::new_from(topic_type)?; // TODO, get ts without allocating msg let mut publisher_handle = unsafe { rcl_get_zero_initialized_publisher() }; let topic_c_string = CString::new(topic).map_err(|_|Error::RCL_RET_INVALID_ARGUMENT)?; @@ -515,7 +507,7 @@ impl Node { rcl_publisher_init( &mut publisher_handle, self.node_handle.as_mut(), - ts, + dummy.ts, topic_c_string.as_ptr(), &publisher_options, ) @@ -707,22 +699,17 @@ impl PublisherUntyped { // upgrade to actual ref. if still alive let publisher = self.handle.upgrade().ok_or(Error::RCL_RET_PUBLISHER_INVALID)?; - // figure out which serializer to use, publish, then destroy - let se = untyped_serialize_helper(&self.type_)?; - let dealloc = untyped_dealloc_helper(&self.type_)?; + let mut native_msg = WrappedNativeMsgUntyped::new_from(&self.type_)?; + native_msg.from_json(msg)?; - // copy rust msg to native and publish it - let native_ptr = se(msg).map_err(|e| Error::SerdeError { err: e.to_string() })?; let result = unsafe { rcl_publish( publisher.as_ref(), - native_ptr, + native_msg.void_ptr(), std::ptr::null_mut(), ) }; - dealloc(native_ptr); - if result == RCL_RET_OK as i32 { Ok(()) } else { @@ -883,9 +870,20 @@ mod tests { } #[test] - fn refactor_untyped() { - let ptr = native as *const ::CStruct; - let msg = unsafe { builtin_interfaces::msg::Duration::from_native(&*ptr) }; + fn test_untyped_json() -> () { + use trajectory_msgs::msg::*; + let mut msg: JointTrajectoryPoint = Default::default(); + msg.positions.push(39.0); + msg.positions.push(34.0); + let json = serde_json::to_value(msg.clone()).unwrap(); + + let mut native = WrappedNativeMsgUntyped::new_from("trajectory_msgs/msg/JointTrajectoryPoint").unwrap(); + native.from_json(json.clone()).unwrap(); + let json2 = native.to_json().unwrap(); + assert_eq!(json, json2); + + let msg2: JointTrajectoryPoint = serde_json::from_value(json2).unwrap(); + assert_eq!(msg, msg2); } } diff --git a/tests/threads.rs b/tests/threads.rs index 6ab5e43..5bd655e 100644 --- a/tests/threads.rs +++ b/tests/threads.rs @@ -2,13 +2,13 @@ use std::thread; use std::time::Duration; use failure::Error; -use r2r::*; +use r2r; #[test] // Let's create and drop a lot of node and publishers for a while to see that we can cope. fn doesnt_crash() -> Result<(), Error> { // a global shared context. - let ctx = Context::create()?; + let ctx = r2r::Context::create()?; for c in 0..10 { let mut ths = Vec::new(); @@ -16,14 +16,14 @@ fn doesnt_crash() -> Result<(), Error> { // create concurrent nodes that max out the cpu let ctx = ctx.clone(); ths.push(thread::spawn(move || { - let mut node = Node::create(ctx, &format!("testnode{}", i), "").unwrap(); + let mut node = r2r::Node::create(ctx, &format!("testnode{}", i), "").unwrap(); // each with 10 publishers for _j in 0..10 { let p = node - .create_publisher::(&format!("/r2r{}", i)) + .create_publisher::(&format!("/r2r{}", i)) .unwrap(); - let to_send = std_msgs::msg::String { + let to_send = r2r::std_msgs::msg::String { data: format!("[node{}]: {}", i, c), };