mirror
/
r2r
mirror of https://github.com/sequenceplanner/r2r.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Cleanup untyped part 2

This commit is contained in:
Martin Dahl 2019-08-30 10:43:21 +02:00
parent 3d2ca44626
commit b79146c0aa
8 changed files with 266 additions and 231 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
build.rs
View File

@ -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();
}

18
examples/publish_complex_msgs.rs
View File

@ -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::<JointTrajectoryPoint>("/hej")?;
let publisher2 = node.create_publisher::<Int32>("/native_count")?;
let mut c = 0;
let mut positions: Vec<f64> = 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::<Int32>::new();
let mut native = r2r::WrappedNativeMsg::<Int32>::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<JointTrajectoryPoint>| {
let cb3 = move |raw_c:&r2r::WrappedNativeMsg<JointTrajectoryPoint>| {
println!("Raw c data: {:?}", raw_c.positions);
};

21
examples/rostopic_echo.rs
View File

@ -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<String> = 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<serde_json::Value> | {
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))?;

14
examples/subscriber.rs
View File

@ -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::<String>();
let p = node
.create_publisher::<std_msgs::msg::String>("/hej")
.create_publisher::<r2r::std_msgs::msg::String>("/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<std_msgs::msg::String>| {
let cb2 = move |x: &r2r::WrappedNativeMsg<r2r::std_msgs::msg::String>| {
// use native data!
let s = x.data.to_str();
println!("native ros msg: {}", s);

7
examples/subscriber_with_thread.rs
View File

@ -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::<std_msgs::msg::String>("/hej")?;
let pubint = node.create_publisher::<std_msgs::msg::Int32>("/count")?;

335
msg_gen/src/lib.rs
View File

@ -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<common::RosMsg>) -> 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<common::RosMsg>) -> String {
let open = String::from(
"
impl WrappedNativeMsgUntyped {
fn new_from(typename: &str) -> Result<Self> {
",
);
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<fn(native: *const std::os::raw::c_void) -> 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<fn(json: serde_json::Value) -> 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<fn(*mut std::os::raw::c_void)> {{");
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)
}

84
src/lib.rs
View File

@ -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<T> = std::result::Result<T, Error>;
pub type Result<T> = std::result::Result<T, Error>;
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<Self> {
if typename == "std_msgs/msg/String" {
Ok(WrappedNativeMsgUntyped::new::<std_msgs::msg::String>())
} else
{ return Err(Error::InvalidMessageType{ msgtype: typename.into() }) }
}
}
impl WrappedNativeMsgUntyped {
fn new<T>() -> Self where T: WrappedTypesupport + Serialize + serde::de::DeserializeOwned {
fn new<T>() -> 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<dyn FnMut(serde_json::Value) -> ()>,
serialize: Box<dyn FnMut(*const std::os::raw::c_void) -> serde_json::Value>,
dealloc: Box<dyn FnMut(*mut std::os::raw::c_void) -> ()>,
rcl_msg: *mut std::os::raw::c_void,
rcl_msg: WrappedNativeMsgUntyped,
callback: Box<dyn FnMut(Result<serde_json::Value>) -> ()>,
}
impl<T> Sub for WrappedSub<T>
@ -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<dyn FnMut(serde_json::Value) -> ()>,
callback: Box<dyn FnMut(Result<serde_json::Value>) -> ()>,
) -> 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<PublisherUntyped> {
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 <builtin_interfaces::msg::Duration as WrappedTypesupport>::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);
}
}

10
tests/threads.rs
View File

@ -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::<std_msgs::msg::String>(&format!("/r2r{}", i))
.create_publisher::<r2r::std_msgs::msg::String>(&format!("/r2r{}", i))
.unwrap();
let to_send = std_msgs::msg::String {
let to_send = r2r::std_msgs::msg::String {
data: format!("[node{}]: {}", i, c),
};