Initial experiments with bindings and msg generation.

.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

Cargo.toml

@@ -0,0 +1,22 @@
+
+[package]
+name = "r2r"
+version = "0.0.1"
+authors = ["Martin Dahl <martin.dahl@gmail.com>"]
+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]

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

common/Cargo.toml

@@ -0,0 +1,9 @@
+
+[package]
+name = "common"
+version = "0.0.1"
+authors = ["Martin Dahl <martin.dahl@gmail.com>"]
+description = "Minimal ros2 bindings."
+license = "Apache-2.0/MIT"
+edition = "2018"
+

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<RosMsg> {
+    let v: Vec<Vec<&str>> = 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<String> {
+    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");
+        
+
+    }    
+        
+
+}
+

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::<std_msgs::msg::String>(&mut node, "/hej")?;
+    let pubint = rcl_create_publisher::<std_msgs::msg::Int32>(&mut node, "/count")?;
+
+    let (tx, rx) = mpsc::channel::<std_msgs::msg::String>();
+    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<Box<Sub>> = 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(())
+
+}
+

msg_gen/Cargo.toml

@@ -0,0 +1,18 @@
+[package]
+name = "msg_gen"
+version = "0.0.1"
+authors = ["Martin Dahl <martin.dahl@gmail.com>"]
+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" }

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);
+}

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");
+    }
+}
+

msgs.txt

@@ -0,0 +1,3 @@
+std_msgs/msg/Bool
+std_msgs/msg/String
+std_msgs/msg/Int32

rcl/Cargo.toml

@@ -0,0 +1,14 @@
+[package]
+name = "rcl"
+version = "0.0.1"
+authors = ["Martin Dahl <martin.dahl@gmail.com>"]
+edition = "2018"
+
+[features]
+headers = []
+
+[dependencies]
+libc = "0.2.0"
+
+[build-dependencies]
+bindgen = "0.50.0"

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!");
+    }
+}

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 },
+        }
+    }
+}
+

rcl/src/rcl_wrapper.h

@@ -0,0 +1,12 @@
+// ros client library
+#include <rcl/rcl.h>
+#include <rcutils/error_handling.h>
+
+// low level msg type handling
+#include <rosidl_generator_c/string.h>
+#include <rosidl_generator_c/string_functions.h>
+
+#include <rosidl_generator_c/message_type_support_struct.h>
+#include <rosidl_typesupport_introspection_c/message_introspection.h>
+#include <rosidl_typesupport_introspection_c/field_types.h>
+

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<T>
+where
+    T: WrappedTypesupport,
+{
+    pub msg: *mut T::CStruct,
+}
+
+impl<T> WrappedNativeMsg<T>
+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<T> Drop for WrappedNativeMsg<T>
+where
+    T: WrappedTypesupport,
+{
+    fn drop(&mut self) {
+        T::destroy_msg(self.msg);
+    }
+}
+
+impl<T> Deref for WrappedNativeMsg<T>
+where
+    T: WrappedTypesupport,
+{
+    type Target = T::CStruct;
+
+    fn deref(&self) -> &Self::Target {
+        unsafe { &(*self.msg) }
+    }
+}
+
+impl<T> DerefMut for WrappedNativeMsg<T>
+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<T>
+where
+    T: WrappedTypesupport,
+{
+    pub rcl_handle: rcl_subscription_t,
+    pub callback: Box<dyn FnMut(T) -> ()>,
+    pub rcl_msg: WrappedNativeMsg<T>,
+}
+
+impl<T> Sub for WrappedSubT<T>
+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<rcl_context_t, ()> {
+    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<rcl_node_t, ()> {
+    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<T>(node: &mut rcl_node_t, topic: &str) -> Result<rcl_publisher_t, ()>
+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<T>(publisher: &rcl_publisher_t, msg: &T) -> Result<(), ()>
+where
+    T: WrappedTypesupport,
+{
+    // copy rust msg to native and publish it
+    let native_msg: WrappedNativeMsg<T> = WrappedNativeMsg::<T>::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<T>(
+    node: &mut rcl_node_t,
+    topic: &str,
+    callback: Box<dyn FnMut(T) -> ()>,
+) -> Result<WrappedSubT<T>, ()>
+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::<T>::new(),
+            callback: callback,
+        };
+
+        Ok(wrapped_sub)
+    } else {
+        eprintln!("{}", result);
+        Err(())
+    }
+}
+
+pub fn rcl_take_subst(
+    ctx: &mut rcl_context_t,
+    subs: &mut Vec<Box<dyn Sub>>,
+    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::<std_msgs::msg::String>::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::<std_msgs::msg::String>::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::<std_msgs::msg::String>::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::<std_msgs::msg::Bool>::from(&msg);
+        let msg2 = std_msgs::msg::Bool::from_native(&msg_native);
+        assert_eq!(msg,msg2);
+    }
+
+}