From 2ebeaf86829c9bcd56888a85bdee1ad07aca823c Mon Sep 17 00:00:00 2001 From: Martin Dahl Date: Mon, 19 Aug 2019 20:39:08 +0200 Subject: [PATCH] Initial experiments with bindings and msg generation. --- .gitignore | 11 + Cargo.toml | 22 ++ build.rs | 40 ++++ common/Cargo.toml | 9 + common/src/lib.rs | 74 ++++++ examples/subscriber_with_thread.rs | 53 +++++ msg_gen/Cargo.toml | 18 ++ msg_gen/build.rs | 76 ++++++ msg_gen/src/lib.rs | 168 +++++++++++++ msgs.txt | 3 + rcl/Cargo.toml | 14 ++ rcl/build.rs | 38 +++ rcl/src/lib.rs | 57 +++++ rcl/src/rcl_wrapper.h | 12 + src/lib.rs | 366 +++++++++++++++++++++++++++++ 15 files changed, 961 insertions(+) create mode 100644 .gitignore create mode 100644 Cargo.toml create mode 100644 build.rs create mode 100644 common/Cargo.toml create mode 100644 common/src/lib.rs create mode 100644 examples/subscriber_with_thread.rs create mode 100644 msg_gen/Cargo.toml create mode 100644 msg_gen/build.rs create mode 100644 msg_gen/src/lib.rs create mode 100644 msgs.txt create mode 100644 rcl/Cargo.toml create mode 100644 rcl/build.rs create mode 100644 rcl/src/lib.rs create mode 100644 rcl/src/rcl_wrapper.h create mode 100644 src/lib.rs diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..96b8a87 --- /dev/null +++ b/.gitignore @@ -0,0 +1,11 @@ +/target/ + +Cargo.lock + +# code generation +msg_gen/src/introspection_functions.rs +msg_gen/src/msg_bindings.rs +msg_gen/src/msg_includes.h +rcl/src/rcl_bindings.rs +rcl/src/rcl_wrapper.h +src/generated_msgs.rs diff --git a/Cargo.toml b/Cargo.toml new file mode 100644 index 0000000..0a09d4d --- /dev/null +++ b/Cargo.toml @@ -0,0 +1,22 @@ + +[package] +name = "r2r" +version = "0.0.1" +authors = ["Martin Dahl "] +description = "Minimal ros2 bindings." +license = "Apache-2.0/MIT" +edition = "2018" + +[features] +headers = ["rcl/headers", "msg_gen/headers"] + +[dependencies] +common = { path = "common", version = "0.0.1" } +rcl = { path = "rcl", version = "0.0.1" } +msg_gen = { path = "msg_gen", version = "0.0.1" } + +[build-dependencies] +common = { path = "common", version = "0.0.1" } +msg_gen = { path = "msg_gen", version = "0.0.1" } + +[workspace] diff --git a/build.rs b/build.rs new file mode 100644 index 0000000..8609b8a --- /dev/null +++ b/build.rs @@ -0,0 +1,40 @@ +use msg_gen::*; +use common::*; + +fn main() { + let msgs = read_file("./msgs.txt").unwrap(); + let msgs = parse_msgs(&msgs); + let msgs = as_map(&msgs); + + let mut codegen = String::new(); + + for (module, prefixes) in &msgs { + println!("cargo:rustc-link-lib=dylib={}__rosidl_typesupport_c", module); + println!("cargo:rustc-link-lib=dylib={}__rosidl_typesupport_introspection_c", module); + println!("cargo:rustc-link-lib=dylib={}__rosidl_generator_c", module); + + codegen.push_str(&format!("pub mod {} {{\n", module)); + + for (prefix, msgs) in prefixes { + codegen.push_str(&format!(" pub mod {} {{\n", prefix)); + codegen.push_str(" use super::super::*;\n"); + + for msg in msgs { + let module_lc = String::from(*module).to_lowercase(); + let prefix_lc = String::from(*prefix).to_lowercase(); + let name_lc = String::from(*msg).to_lowercase(); + codegen.push_str(&generate_rust_msg(&module_lc, &prefix_lc, &name_lc)); + } + + codegen.push_str(" }\n"); + + } + + codegen.push_str("}\n"); + } + + use std::io::Write; + use std::fs::File; + let mut f = File::create("src/generated_msgs.rs").unwrap(); + write!(f, "{}", codegen).unwrap(); +} diff --git a/common/Cargo.toml b/common/Cargo.toml new file mode 100644 index 0000000..b5426a8 --- /dev/null +++ b/common/Cargo.toml @@ -0,0 +1,9 @@ + +[package] +name = "common" +version = "0.0.1" +authors = ["Martin Dahl "] +description = "Minimal ros2 bindings." +license = "Apache-2.0/MIT" +edition = "2018" + diff --git a/common/src/lib.rs b/common/src/lib.rs new file mode 100644 index 0000000..a06f357 --- /dev/null +++ b/common/src/lib.rs @@ -0,0 +1,74 @@ +use std::collections::HashMap; + +#[derive(Debug)] +pub struct RosMsg { + pub module: String, // e.g. std_msgs + pub prefix: String, // e.g. "msg" or "srv" + pub name: String, // e.g. "String" +} + +pub fn parse_msgs(msgs: &str) -> Vec { + let v: Vec> = msgs.lines().map(|l| l.split("/").into_iter().take(3).collect()).collect(); + v.iter().filter(|v|v.len() == 3). + map(|v| RosMsg { module: v[0].into(), prefix: v[1].into(), name: v[2].into()}).collect() + +} + +pub fn as_map(included_msgs: &[RosMsg]) -> HashMap<&str, HashMap<&str, Vec<&str>>> { + let mut msgs = HashMap::new(); + for msg in included_msgs { + msgs.entry(msg.module.as_str()).or_insert(HashMap::new()).entry(msg.prefix.as_str()). + or_insert(Vec::new()).push(msg.name.as_str()); + } + msgs +} + +use std::io::{self, Read}; +use std::fs::File; + +pub fn read_file(filename: &str) -> io::Result { + let mut file = File::open(filename)?; + let mut s = String::new(); + file.read_to_string(&mut s)?; + Ok(s) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_parse_msgs() -> () { + let msgs = " +std_msgs/msg/Bool +x/y +std_msgs/msg/String +"; + let parsed = parse_msgs(msgs); + assert_eq!(parsed[0].module, "std_msgs"); + assert_eq!(parsed[0].prefix, "msg"); + assert_eq!(parsed[0].name, "Bool"); + assert_eq!(parsed[1].module, "std_msgs"); + assert_eq!(parsed[1].prefix, "msg"); + assert_eq!(parsed[1].name, "String"); + } + + #[test] + fn test_as_map() -> () { + let msgs = " +std_msgs/msg/Bool +x/y +std_msgs/msg/String +"; + let parsed = parse_msgs(msgs); + let map = as_map(&parsed); + + assert_eq!(map.get("std_msgs").unwrap().get("msg").unwrap()[0], "Bool"); + assert_eq!(map.get("std_msgs").unwrap().get("msg").unwrap()[1], "String"); + + + } + + +} + diff --git a/examples/subscriber_with_thread.rs b/examples/subscriber_with_thread.rs new file mode 100644 index 0000000..1164db1 --- /dev/null +++ b/examples/subscriber_with_thread.rs @@ -0,0 +1,53 @@ +use std::thread; +use std::sync::mpsc; + +use r2r::*; + +fn main() -> Result<(), ()> { + let mut ctx = rcl_create_context()?; + let mut node = rcl_create_node("qqq", "", &mut ctx)?; + + let publisher = rcl_create_publisher::(&mut node, "/hej")?; + let pubint = rcl_create_publisher::(&mut node, "/count")?; + + let (tx, rx) = mpsc::channel::(); + thread::spawn(move|| { + loop { + let msg = rx.recv().unwrap(); + println!("other thread received: {}", msg.data); + } + }); + + let mut c = 0; + let cb = move |x:std_msgs::msg::String| { + let to_send = format!("at count {} got: {}", c, x.data); + c = c + 1; + tx.send(x.clone()).unwrap(); // pass msg on to other thread for printing + let to_send = std_msgs::msg::String { data: to_send }; + publish(&publisher, &to_send).unwrap(); + let to_send = std_msgs::msg::Int32 { data: c }; + publish(&pubint, &to_send).unwrap(); + }; + + let ws2 = rcl_create_subscription(&mut node, "/hopp", Box::new(cb))?; + + // TODO: group subscriptions in a "node" struct + let mut subst: Vec> = vec![Box::new(ws2)]; + + // run for 10 seconds + let mut count = 0; + while count < 100 { + let timeout = 100 * 1000 * 1000; // 0.1 sec + rcl_take_subst(&mut ctx, &mut subst, timeout)?; + count = count + 1; + } + + + // TODO: crashes here. maybe because pub and sub are not cleaned up + rcl_destroy_node(&mut node); + rcl_destroy_ctx(&mut ctx); + + Ok(()) + +} + diff --git a/msg_gen/Cargo.toml b/msg_gen/Cargo.toml new file mode 100644 index 0000000..e790a4a --- /dev/null +++ b/msg_gen/Cargo.toml @@ -0,0 +1,18 @@ +[package] +name = "msg_gen" +version = "0.0.1" +authors = ["Martin Dahl "] +edition = "2018" + +[features] +headers = [] + +[dependencies] +lazy_static = "1.3.0" +libc = "0.2.0" +rcl = { path = "../rcl", version = "0.0.1" } + +[build-dependencies] +bindgen = "0.50.0" +rcl = { path = "../rcl", version = "0.0.1" } +common = { path = "../common", version = "0.0.1" } diff --git a/msg_gen/build.rs b/msg_gen/build.rs new file mode 100644 index 0000000..c7b0705 --- /dev/null +++ b/msg_gen/build.rs @@ -0,0 +1,76 @@ +extern crate bindgen; + +use std::env; +use std::path::PathBuf; + +use common::*; + +fn main() { + let msgs = read_file("../msgs.txt").unwrap(); + let msg_list = parse_msgs(&msgs); + let msg_map = as_map(&msg_list); + + for module in msg_map.keys() { + println!("cargo:rustc-link-lib=dylib={}__rosidl_typesupport_c", module); + println!("cargo:rustc-link-lib=dylib={}__rosidl_typesupport_introspection_c", module); + println!("cargo:rustc-link-lib=dylib={}__rosidl_generator_c", module); + } + + let mut includes = String::new(); + let mut introspecion_map = String::from("\ + lazy_static! { \n + static ref INTROSPECTION_FNS: HashMap<&'static str, usize> = {\n + let mut m = HashMap::new();\n"); + + for msg in msg_list { + + let module = msg.module.to_lowercase(); + let prefix = msg.prefix.to_lowercase(); + let name = msg.name.to_lowercase(); + + includes.push_str(&format!("#include <{}/{}/{}.h>\n", &module, &prefix, &name)); + includes.push_str(&format!("#include <{}/{}/{}__rosidl_typesupport_introspection_c.h>\n", &module, &prefix, &name)); + + let key = &format!("{}__{}__{}", module, prefix, 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); + introspecion_map.push_str(&format!("m.insert(\"{}\", {});\n",key,val)); + } + introspecion_map.push_str("m \n }; }\n\n"); + + use std::io::Write; + use std::fs::File; + let mut f = File::create("src/msg_includes.h").unwrap(); + write!(f, "{}", includes).unwrap(); + + let mut f = File::create("src/introspection_functions.rs").unwrap(); + write!(f, "{}", introspecion_map).unwrap(); + + let headers_enabled = env::var_os("CARGO_FEATURE_HEADERS").is_some(); + let mut builder = bindgen::Builder::default() + .header("src/msg_includes.h") + .derive_copy(false) + // blacklist types that are handled by rcl bindings + .blacklist_type("rosidl_message_type_support_t") + .blacklist_type("rosidl_generator_c__String") + .default_enum_style(bindgen::EnumVariation::Rust { non_exhaustive: false } ); + + 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!("cargo:rustc-link-search=native={}/lib", ament_prefix_path); + } + + // bindgen takes time so we dont want to do it always... must be a better way + if headers_enabled { + let bindings = builder.generate().expect("Unable to generate bindings"); + + let out_path = PathBuf::from("."); + bindings + .write_to_file(out_path.join("src/msg_bindings.rs")) + .expect("Couldn't write bindings!"); + } + +// assert!(false); +} diff --git a/msg_gen/src/lib.rs b/msg_gen/src/lib.rs new file mode 100644 index 0000000..6ad5b11 --- /dev/null +++ b/msg_gen/src/lib.rs @@ -0,0 +1,168 @@ +#![allow(non_upper_case_globals)] +#![allow(non_camel_case_types)] +#![allow(non_snake_case)] +#![allow(improper_ctypes)] +#![allow(dead_code)] +include!("./msg_bindings.rs"); +include!("./introspection_functions.rs"); + +#[macro_use] +extern crate lazy_static; + +use std::collections::HashMap; +use rcl::*; + +use std::ffi::CStr; + +// Try to make these work, containing a nested msg with arrays. +// +// martin@martin-XPS-15-9550 ~ $ ros2 msg show trajectory_msgs/msg/JointTrajectoryPoint +// # Each trajectory point specifies either positions[, velocities[, accelerations]] +// # or positions[, effort] for the trajectory to be executed. +// # All specified values are in the same order as the joint names in JointTrajectory.msg. +// +// float64[] positions +// float64[] velocities +// float64[] accelerations +// float64[] effort +// builtin_interfaces/Duration time_from_start +// martin@martin-XPS-15-9550 ~ $ ros2 msg show builtin_interfaces/Duration +// int32 sec +// uint32 nanosec +// +// + + +fn field_type(t: u8) -> String { + + // rosidl_typesupport_introspection_c__ROS_TYPE_FLOAT = 1, + // rosidl_typesupport_introspection_c__ROS_TYPE_DOUBLE = 2, + // rosidl_typesupport_introspection_c__ROS_TYPE_LONG_DOUBLE = 3, + // rosidl_typesupport_introspection_c__ROS_TYPE_CHAR = 4, + // rosidl_typesupport_introspection_c__ROS_TYPE_WCHAR = 5, + // rosidl_typesupport_introspection_c__ROS_TYPE_BOOLEAN = 6, + // rosidl_typesupport_introspection_c__ROS_TYPE_OCTET = 7, + // rosidl_typesupport_introspection_c__ROS_TYPE_UINT8 = 8, + // rosidl_typesupport_introspection_c__ROS_TYPE_INT8 = 9, + // rosidl_typesupport_introspection_c__ROS_TYPE_UINT16 = 10, + // rosidl_typesupport_introspection_c__ROS_TYPE_INT16 = 11, + // rosidl_typesupport_introspection_c__ROS_TYPE_UINT32 = 12, + // rosidl_typesupport_introspection_c__ROS_TYPE_INT32 = 13, + // rosidl_typesupport_introspection_c__ROS_TYPE_UINT64 = 14, + // rosidl_typesupport_introspection_c__ROS_TYPE_INT64 = 15, + // rosidl_typesupport_introspection_c__ROS_TYPE_STRING = 16, + // rosidl_typesupport_introspection_c__ROS_TYPE_WSTRING = 17, + // rosidl_typesupport_introspection_c__ROS_TYPE_MESSAGE = 18, + + // todo: add these as needed... + 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_INT32 as u8) { "i32".to_owned() } + else { panic!("ros native type not implemented: {}", t); } +} + +// 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("code generation error"); + let ptr = *ptr as *const i32 as *const rosidl_message_type_support_t; + unsafe { + let members = (*ptr).data as *const rosidl_typesupport_introspection_c__MessageMembers; + 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); + + let mut fields = String::new(); + + for member in memberslice { + let field_name = CStr::from_ptr((*member).name_).to_str().unwrap(); + let type_id = (*member).type_id_; + let is_array = (*member).is_array_; // TODO: use + let rust_field_type = field_type(type_id); + let s = 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!(" {} {{\n", name)); + + for member in memberslice { + let field_name = CStr::from_ptr((*member).name_).to_str().unwrap(); + let type_id = (*member).type_id_; + let is_array = (*member).is_array_; // TODO: use + + let rust_field_type = field_type(type_id); + + 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)); + } else { + from_native.push_str(&format!("{field_name}: msg.{field_name},\n", field_name = field_name)); + } + } + from_native.push_str(" }\n }\n"); + + let mut copy_to_native = String::new(); + copy_to_native.push_str("fn copy_to_native(&self, msg: &mut Self::CStruct) {"); + + for member in memberslice { + let field_name = CStr::from_ptr((*member).name_).to_str().unwrap(); + let type_id = (*member).type_id_; + let is_array = (*member).is_array_; // TODO: use + + let rust_field_type = field_type(type_id); + + // handle other special cases... + 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)); + } else { + 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 + 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}() }} + }}\n + fn create_msg() -> *mut {c_struct} {{\n + unsafe {{ {c_struct}__create() }}\n + }}\n + fn destroy_msg(msg: *mut {c_struct}) -> () {{\n + unsafe {{ {c_struct}__destroy(msg) }};\n + }}\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); + + let module_str = format!(" + #[derive(Clone,Debug,PartialEq)] + pub struct {msgname} {{\n + {fields} + }}\n + {typesupport}\n\n + ", msgname = name, fields = fields, typesupport = typesupport); + + module_str + } + +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_fn_ptrs() -> () { + + assert_eq!(generate_rust_msg("std_msgs", "msg", "string"), "hej"); + } +} + diff --git a/msgs.txt b/msgs.txt new file mode 100644 index 0000000..ce94c1f --- /dev/null +++ b/msgs.txt @@ -0,0 +1,3 @@ +std_msgs/msg/Bool +std_msgs/msg/String +std_msgs/msg/Int32 diff --git a/rcl/Cargo.toml b/rcl/Cargo.toml new file mode 100644 index 0000000..ed39261 --- /dev/null +++ b/rcl/Cargo.toml @@ -0,0 +1,14 @@ +[package] +name = "rcl" +version = "0.0.1" +authors = ["Martin Dahl "] +edition = "2018" + +[features] +headers = [] + +[dependencies] +libc = "0.2.0" + +[build-dependencies] +bindgen = "0.50.0" diff --git a/rcl/build.rs b/rcl/build.rs new file mode 100644 index 0000000..d1ff069 --- /dev/null +++ b/rcl/build.rs @@ -0,0 +1,38 @@ +extern crate bindgen; + +use std::env; +use std::path::PathBuf; + +fn main() { + let headers_enabled = env::var_os("CARGO_FEATURE_HEADERS").is_some(); + let mut builder = bindgen::Builder::default() + .header("src/rcl_wrapper.h") + .derive_copy(false) + .default_enum_style(bindgen::EnumVariation::Rust { non_exhaustive: false } ); + + 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!("cargo:rustc-link-search=native={}/lib", ament_prefix_path); + } + + println!("cargo:rustc-link-lib=dylib=rcl"); + println!("cargo:rustc-link-lib=dylib=rcl_logging_noop"); + println!("cargo:rustc-link-lib=dylib=rcutils"); + println!("cargo:rustc-link-lib=dylib=rmw"); + println!("cargo:rustc-link-lib=dylib=rmw_implementation"); + println!("cargo:rustc-link-lib=dylib=rosidl_typesupport_c"); + println!("cargo:rustc-link-lib=dylib=rosidl_generator_c"); + + // bindgen takes time so we dont want to do it always... must be a better way + if headers_enabled { + let bindings = builder.generate().expect("Unable to generate bindings"); + + let out_path = PathBuf::from("."); + bindings + .write_to_file(out_path.join("src/rcl_bindings.rs")) + .expect("Couldn't write bindings!"); + } +} diff --git a/rcl/src/lib.rs b/rcl/src/lib.rs new file mode 100644 index 0000000..ec9c594 --- /dev/null +++ b/rcl/src/lib.rs @@ -0,0 +1,57 @@ +#![allow(non_upper_case_globals)] +#![allow(non_camel_case_types)] +#![allow(non_snake_case)] +#![allow(improper_ctypes)] +#![allow(dead_code)] +pub mod rcl_bindings; +pub use rcl_bindings::*; + +use std::ffi::CStr; +use std::ffi::CString; + + +// special treatment to convert to/from rust strings. +// ros strings are owned by ros, assignment is a copy +impl rosidl_generator_c__String { + pub fn to_str(&self) -> &str { + let s = unsafe { CStr::from_ptr(self.data as *mut i8) }; + s.to_str().unwrap_or("") + } + + pub fn assign(&mut self, other: &str) -> () { + let q = CString::new(other).unwrap(); + let to_send_ptr = q.as_ptr() as *const i8; + unsafe { + rosidl_generator_c__String__assign(self as *mut _, to_send_ptr); + } + } +} + +impl Default for rmw_message_info_t { + fn default() -> Self { + rmw_message_info_t { + publisher_gid: rmw_gid_t { + implementation_identifier: std::ptr::null(), + data: [0; 24], + }, + from_intra_process: false, + } + } +} + +impl Default for rmw_qos_profile_t { + fn default() -> Self { + rmw_qos_profile_t { + history: rmw_qos_history_policy_t::RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT, + depth: 10, + reliability: rmw_qos_reliability_policy_t::RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT, + durability: rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT, + avoid_ros_namespace_conventions: false, + deadline: rmw_time_t { sec: 0, nsec: 0 }, + lifespan: rmw_time_t { sec: 0, nsec: 0 }, + liveliness: rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + liveliness_lease_duration: rmw_time_t { sec: 0, nsec: 0 }, + } + } +} + diff --git a/rcl/src/rcl_wrapper.h b/rcl/src/rcl_wrapper.h new file mode 100644 index 0000000..30478ca --- /dev/null +++ b/rcl/src/rcl_wrapper.h @@ -0,0 +1,12 @@ +// ros client library +#include +#include + +// low level msg type handling +#include +#include + +#include +#include +#include + diff --git a/src/lib.rs b/src/lib.rs new file mode 100644 index 0000000..c4df9c3 --- /dev/null +++ b/src/lib.rs @@ -0,0 +1,366 @@ +include!("./generated_msgs.rs"); + +use msg_gen::*; +use rcl::*; + +use std::ffi::CString; +use std::ops::{Deref, DerefMut}; + +pub trait WrappedTypesupport { + type CStruct; + + fn get_ts() -> &'static rosidl_message_type_support_t; + fn create_msg() -> *mut Self::CStruct; + fn destroy_msg(msg: *mut Self::CStruct); + fn from_native(msg: &Self::CStruct) -> Self; + fn copy_to_native(&self, msg: &mut Self::CStruct); +} + +#[derive(Debug)] +pub struct WrappedNativeMsg +where + T: WrappedTypesupport, +{ + pub msg: *mut T::CStruct, +} + +impl WrappedNativeMsg +where + T: WrappedTypesupport, +{ + pub fn new() -> Self { + WrappedNativeMsg { + msg: T::create_msg(), + } + } + + pub fn from(msg: &T) -> Self { + let mut native_msg = Self::new(); + msg.copy_to_native(&mut native_msg); + native_msg + } + + 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 WrappedNativeMsg +where + T: WrappedTypesupport, +{ + fn drop(&mut self) { + T::destroy_msg(self.msg); + } +} + +impl Deref for WrappedNativeMsg +where + T: WrappedTypesupport, +{ + type Target = T::CStruct; + + fn deref(&self) -> &Self::Target { + unsafe { &(*self.msg) } + } +} + +impl DerefMut for WrappedNativeMsg +where + T: WrappedTypesupport, +{ + fn deref_mut(&mut self) -> &mut Self::Target { + unsafe { &mut (*self.msg) } + } +} + +pub trait Sub { + fn handle(&self) -> &rcl_subscription_t; + fn run_cb(&mut self) -> (); + fn rcl_msg(&mut self) -> *mut std::os::raw::c_void; +} + +pub struct WrappedSubT +where + T: WrappedTypesupport, +{ + pub rcl_handle: rcl_subscription_t, + pub callback: Box ()>, + pub rcl_msg: WrappedNativeMsg, +} + +impl Sub for WrappedSubT +where + T: WrappedTypesupport, +{ + fn handle(&self) -> &rcl_subscription_t { + &self.rcl_handle + } + + fn rcl_msg(&mut self) -> *mut std::os::raw::c_void { + self.rcl_msg.void_ptr_mut() + } + + fn run_cb(&mut self) -> () { + // copy native msg to rust type and run callback + let msg = T::from_native(&self.rcl_msg); + (self.callback)(msg); + } +} + +pub fn rcl_create_context() -> Result { + let mut ctx = unsafe { rcl_get_zero_initialized_context() }; + let isok = unsafe { + let allocator = rcutils_get_default_allocator(); + let mut init_options = rcl_get_zero_initialized_init_options(); + rcl_init_options_init(&mut init_options, allocator); + rcl_init(0, std::ptr::null(), &init_options, &mut ctx); + let isok = rcl_context_is_valid(&mut ctx); + rcl_init_options_fini(&mut init_options as *mut _); + isok + }; + + if isok { + Ok(ctx) + } else { + Err(()) + } +} + +pub fn rcl_create_node( + name: &str, + namespace: &str, + ctx: &mut rcl_context_t, +) -> Result { + let c_node_name = CString::new(name).unwrap(); + let c_node_ns = CString::new(namespace).unwrap(); + let mut node_handle = unsafe { rcl_get_zero_initialized_node() }; + let nr = unsafe { + let node_options = rcl_node_get_default_options(); + rcl_node_init( + &mut node_handle as *mut _, + c_node_name.as_ptr(), + c_node_ns.as_ptr(), + ctx, + &node_options as *const _, + ) + }; + if nr == RCL_RET_OK as i32 { + Ok(node_handle) + } else { + eprintln!("{}", nr); + Err(()) + } +} + +pub fn rcl_destroy_node(node: &mut rcl_node_t) { + unsafe { + rcl_node_fini(node); + } +} + +pub fn rcl_destroy_ctx(ctx: &mut rcl_context_t) { + unsafe { + rcl_shutdown(ctx); + } +} + +pub fn rcl_create_publisher(node: &mut rcl_node_t, topic: &str) -> Result +where + T: WrappedTypesupport, +{ + let mut publisher_handle = unsafe { rcl_get_zero_initialized_publisher() }; + let topic_c_string = CString::new(topic).unwrap(); + + let result = unsafe { + let mut publisher_options = rcl_publisher_get_default_options(); + publisher_options.qos = rmw_qos_profile_t::default(); + rcl_publisher_init( + &mut publisher_handle, + node, + T::get_ts(), + topic_c_string.as_ptr(), + &publisher_options, + ) + }; + if result == RCL_RET_OK as i32 { + Ok(publisher_handle) + } else { + eprintln!("{}", result); + Err(()) + } +} + +pub fn publish(publisher: &rcl_publisher_t, msg: &T) -> Result<(), ()> +where + T: WrappedTypesupport, +{ + // copy rust msg to native and publish it + let native_msg: WrappedNativeMsg = WrappedNativeMsg::::from(msg); + let result = unsafe { rcl_publish(publisher, native_msg.void_ptr(), std::ptr::null_mut()) }; + + if result == RCL_RET_OK as i32 { + Ok(()) + } else { + eprintln!("{}", result); + Err(()) + } +} + +pub fn rcl_create_subscription( + node: &mut rcl_node_t, + topic: &str, + callback: Box ()>, +) -> Result, ()> +where + T: WrappedTypesupport, +{ + let mut subscription_handle = unsafe { rcl_get_zero_initialized_subscription() }; + let topic_c_string = CString::new(topic).unwrap(); + + let result = unsafe { + let mut subscription_options = rcl_subscription_get_default_options(); + subscription_options.qos = rmw_qos_profile_t::default(); + rcl_subscription_init( + &mut subscription_handle, + node, + T::get_ts(), + topic_c_string.as_ptr(), + &subscription_options, + ) + }; + if result == RCL_RET_OK as i32 { + let wrapped_sub = WrappedSubT { + rcl_handle: subscription_handle, + rcl_msg: WrappedNativeMsg::::new(), + callback: callback, + }; + + Ok(wrapped_sub) + } else { + eprintln!("{}", result); + Err(()) + } +} + +pub fn rcl_take_subst( + ctx: &mut rcl_context_t, + subs: &mut Vec>, + timeout: i64, +) -> Result<(), ()> { + let mut ws = unsafe { rcl_get_zero_initialized_wait_set() }; + + unsafe { + rcl_wait_set_init( + &mut ws, + subs.len(), + 0, + 0, + 0, + 0, + 0, + ctx, + rcutils_get_default_allocator(), + ); + rcl_wait_set_clear(&mut ws); + } + + for s in subs.iter() { + unsafe { + rcl_wait_set_add_subscription(&mut ws, s.handle(), std::ptr::null_mut()); + } + } + + unsafe { + rcl_wait(&mut ws, timeout); + } + + for s in subs { + let mut msg_info = rmw_message_info_t::default(); + + let ret = unsafe { rcl_take(s.handle(), s.rcl_msg(), &mut msg_info, std::ptr::null_mut()) }; + + // fresh message, run cb + if ret == RCL_RET_OK as i32 { + s.run_cb(); + } + } + + Ok(()) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_ros_str() -> () { + let hej = "hej hopp"; + let mut msg = WrappedNativeMsg::::new(); + msg.data.assign(hej); + assert_eq!(msg.data.to_str(), hej); + } + + #[test] + fn test_copy_fields() -> () { + let msg_orig = std_msgs::msg::String { data: "hej".into() }; + let rosmsg = WrappedNativeMsg::::from(&msg_orig); + let msg2 = std_msgs::msg::String::from_native(&rosmsg); + assert_eq!(msg_orig, msg2); + } + + #[test] + fn test_introspection_string() -> () { + unsafe { + use std::ffi::CStr; + + let x = rosidl_typesupport_introspection_c__get_message_type_support_handle__std_msgs__msg__String(); + let members = (*x).data as *const rosidl_typesupport_introspection_c__MessageMembers; + println!("{:#?}", *members); + + assert_eq!((*members).member_count_, 1); + + let s = CStr::from_ptr((*members).message_namespace_) + .to_str() + .unwrap(); + assert_eq!(s, "std_msgs__msg"); + + let s = CStr::from_ptr((*members).message_name_).to_str().unwrap(); + assert_eq!(s, "String"); + + let member = (*members).members_; + println!("member: {:#?}", *member); + let field_name = CStr::from_ptr((*member).name_).to_str().unwrap(); + let type_id = (*member).type_id_; + let is_array = (*member).is_array_; + assert_eq!(field_name, "data"); + assert_eq!( + type_id, + rosidl_typesupport_introspection_c__ROS_TYPE_STRING as u8 + ); + assert_eq!(is_array, false); + } + } + + #[test] + fn test_generation_string_use() -> () { + let msg = std_msgs::msg::String { data: "hej".into() }; + let msg2 = msg.clone(); + let msg_native = WrappedNativeMsg::::from(&msg2); + let msg2 = std_msgs::msg::String::from_native(&msg_native); + assert_eq!(msg, msg2) + } + + #[test] + fn test_generation_bool_use() -> () { + let msg = std_msgs::msg::Bool { data: true }; + let msg_native = WrappedNativeMsg::::from(&msg); + let msg2 = std_msgs::msg::Bool::from_native(&msg_native); + assert_eq!(msg,msg2); + } + +}