Started adding support for arrays and nested messages. More derives!

Serde derives should maybe be a feature that can be toggled.

Cargo.toml

@@ -11,10 +11,14 @@ edition = "2018"
 headers = ["rcl/headers", "msg_gen/headers"]
 
 [dependencies]
+serde = { version = "1.0", features = ["derive"] }
 common = { path = "common", version = "0.0.1" }
 rcl = { path = "rcl", version = "0.0.1" }
 msg_gen = { path = "msg_gen", version = "0.0.1" }
 
+[dev-dependencies]
+serde_json = "1.0"
+
 [build-dependencies]
 common = { path = "common", version = "0.0.1" }
 msg_gen = { path = "msg_gen", version = "0.0.1" }

examples/publish_complex_msgs.rs

@@ -0,0 +1,50 @@
+use r2r::*;
+use builtin_interfaces::msg::Duration;
+use trajectory_msgs::msg::*;
+
+fn main() -> Result<(), ()> {
+    let mut ctx = rcl_create_context()?;
+    let mut node = rcl_create_node(&mut ctx, "testnode", "")?;
+    let publisher = rcl_create_publisher::<JointTrajectoryPoint>(&mut node, "/hej")?;
+
+    let mut c = 0;
+    let cb = move |x:std_msgs::msg::String| {
+        println!("at count {} got: {}", c, x.data);
+        c = c + 1;
+        let positions: Vec<f64> = vec!(94.2 * c as f64);
+        let to_send = JointTrajectoryPoint {
+            positions,
+            time_from_start : Duration { sec: c, nanosec: 0 },
+            ..Default::default()
+        };
+        publish(&publisher, &to_send).unwrap();
+    };
+
+    let cb2 = move |x:JointTrajectoryPoint| {
+        let serialized = serde_json::to_string(&x).unwrap();
+        println!("JTP serialized as: {}", serialized);
+    };    
+
+    let sub1 = rcl_create_subscription(&mut node, "/hopp", Box::new(cb))?;
+    let sub2 = rcl_create_subscription(&mut node, "/hej", Box::new(cb2))?;
+
+    // TODO: group subscriptions in a "node" struct
+    let mut subst: Vec<Box<Sub>> = vec![Box::new(sub1), Box::new(sub2)];
+
+    // 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(())
+
+}
+

examples/subscriber_with_thread.rs

@@ -5,16 +5,17 @@ use r2r::*;
 
 fn main() -> Result<(), ()> {
     let mut ctx = rcl_create_context()?;
-    let mut node = rcl_create_node("qqq", "", &mut ctx)?;
+    let mut node = rcl_create_node(&mut ctx, "qqq", "")?;
 
     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>();
+    let (tx, rx) = mpsc::channel::<String>();
     thread::spawn(move|| {
         loop {
             let msg = rx.recv().unwrap();
-            println!("other thread received: {}", msg.data);
+            let deserialized: std_msgs::msg::String = serde_json::from_str(&msg).unwrap();
+            println!("received: {}, deserialized ros msg = {:?}", msg, deserialized);
         }
     });
 
@@ -22,7 +23,8 @@ fn main() -> Result<(), ()> {
     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 serialized = serde_json::to_string(&x).unwrap();
+        tx.send(serialized).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 };

msg_gen/Cargo.toml

@@ -16,3 +16,4 @@ rcl = { path = "../rcl", version = "0.0.1" }
 bindgen = "0.50.0"
 rcl = { path = "../rcl", version = "0.0.1" }
 common = { path = "../common", version = "0.0.1" }
+heck = "0.3.1"

msg_gen/build.rs

@@ -6,6 +6,8 @@ use std::path::PathBuf;
 use common::*;
 
 fn main() {
+    println!("cargo:rerun-if-changed=../");
+
     let msgs = read_file("../msgs.txt").unwrap();
     let msg_list = parse_msgs(&msgs);
     let msg_map = as_map(&msg_list);
@@ -24,12 +26,17 @@ fn main() {
 
     for msg in msg_list {
 
+        // I *think* these are always just lowercase
         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));
+        // filename is certainly CamelCase -> snake_case. convert
-        includes.push_str(&format!("#include <{}/{}/{}__rosidl_typesupport_introspection_c.h>\n", &module, &prefix, &name));
+        use heck::SnakeCase;
+        let include_filename = msg.name.to_snake_case();
+
+        includes.push_str(&format!("#include <{}/{}/{}.h>\n", &module, &prefix, &include_filename));
+        includes.push_str(&format!("#include <{}/{}/{}__rosidl_typesupport_introspection_c.h>\n", &module, &prefix, &include_filename));
 
         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);
@@ -52,6 +59,7 @@ fn main() {
         // blacklist types that are handled by rcl bindings
         .blacklist_type("rosidl_message_type_support_t")
         .blacklist_type("rosidl_generator_c__String")
+        .blacklist_type("rosidl_generator_c__double__Sequence") // etc...
         .default_enum_style(bindgen::EnumVariation::Rust { non_exhaustive: false } );
 
     let ament_prefix_var_name = "AMENT_PREFIX_PATH";

msg_gen/src/lib.rs

@@ -26,7 +26,7 @@ use std::ffi::CStr;
 //  float64[] accelerations
 //  float64[] effort
 //  builtin_interfaces/Duration time_from_start
-//  martin@martin-XPS-15-9550 ~ $ ros2 msg show builtin_interfaces/Duration
+//  martin@martin-XPS-15-9550 ~ $ ros2 msg show builtin_interfaces/msg/Duration
 //  int32 sec
 //  uint32 nanosec
 //  
@@ -57,14 +57,19 @@ fn field_type(t: u8) -> String {
     // 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_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_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_MESSAGE as u8) { "message".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 = 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 members = (*ptr).data as *const rosidl_typesupport_introspection_c__MessageMembers;
@@ -84,7 +89,21 @@ pub fn generate_rust_msg(module: &str, prefix: &str, name: &str) -> String {
             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);
+            let rust_field_type = if rust_field_type == "message" {
+                // perform a hack!
+                let ts = (*member).members_;
+                let members = (*ts).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] );
+                format!("{module}::{prefix}::{msgname}", module = module, prefix=prefix, msgname = name)
+            } else { rust_field_type };
+            let s = if is_array {
+                format!("pub {}: Vec<{}>,\n",field_name, rust_field_type)
+            } else {
+                format!("pub {}: {},\n",field_name, rust_field_type)
+            };
             fields.push_str(&s);
         }
 
@@ -102,8 +121,21 @@ pub fn generate_rust_msg(module: &str, prefix: &str, name: &str) -> String {
 
             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 if rust_field_type == "message" {
+                // perform a hack!
+                let ts = (*member).members_;
+                let members = (*ts).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] );
+                from_native.push_str(&format!("{field_name}: {module}::{prefix}::{msgname}::from_native(&msg.{field_name}),", 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));
+                if is_array {
+                    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},\n", field_name = field_name));
+                }
             }
         }
         from_native.push_str("      }\n    }\n");
@@ -121,8 +153,14 @@ pub fn generate_rust_msg(module: &str, prefix: &str, name: &str) -> String {
             // 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 if rust_field_type == "message" {
+                copy_to_native.push_str(&format!("self.{field_name}.copy_to_native(&mut msg.{field_name});", field_name = field_name));
             } else {
-                copy_to_native.push_str(&format!("msg.{field_name} = self.{field_name};\n", field_name = field_name));
+                if is_array {
+                    copy_to_native.push_str(&format!("msg.{field_name}.update(&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");
@@ -143,7 +181,7 @@ pub fn generate_rust_msg(module: &str, prefix: &str, name: &str) -> String {
         }}\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)]
+                          #[derive(Clone,Debug,Default,PartialEq,Serialize,Deserialize)]
                           pub struct {msgname} {{\n
                               {fields}
                           }}\n

msgs.txt

@@ -1,3 +1,5 @@
 std_msgs/msg/Bool
 std_msgs/msg/String
 std_msgs/msg/Int32
+builtin_interfaces/msg/Duration
+trajectory_msgs/msg/JointTrajectoryPoint

rcl/build.rs

@@ -4,6 +4,8 @@ use std::env;
 use std::path::PathBuf;
 
 fn main() {
+    println!("cargo:rerun-if-changed=../");
+
     let headers_enabled = env::var_os("CARGO_FEATURE_HEADERS").is_some();
     let mut builder = bindgen::Builder::default()
         .header("src/rcl_wrapper.h")

rcl/src/lib.rs

@@ -55,3 +55,31 @@ impl Default for rmw_qos_profile_t {
     }
 }
 
+
+// conversions from/to vectors
+// macro:ify this increase maintainability
+
+impl rosidl_generator_c__double__Sequence {
+    pub fn update(&mut self, values: &[f64]) {
+        // crash here?
+        unsafe { rosidl_generator_c__float64__Sequence__fini(self as *mut _); }
+        unsafe { rosidl_generator_c__float64__Sequence__init(self as *mut _, values.len()); }
+        unsafe { std::ptr::copy(values.as_ptr(), self.data, values.len()); }
+    }
+
+    pub fn to_vec(&self) -> Vec<f64> {
+        let mut dst = Vec::with_capacity(self.size);
+        unsafe { dst.set_len(self.size); }
+        unsafe { std::ptr::copy(self.data, dst.as_mut_ptr(), self.size); }
+        dst
+    }
+
+    // dont think we need fini? surely messages call fini on all their fields...?
+    //
+    // extern "C" {
+    //     pub fn rosidl_generator_c__float64__Sequence__fini(
+    //         sequence: *mut rosidl_generator_c__double__Sequence,
+    //     );
+    // }
+}
+

rcl/src/rcl_wrapper.h

@@ -6,6 +6,9 @@
 #include <rosidl_generator_c/string.h>
 #include <rosidl_generator_c/string_functions.h>
 
+#include <rosidl_generator_c/primitives_sequence.h>
+#include <rosidl_generator_c/primitives_sequence_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

@@ -5,6 +5,7 @@ use rcl::*;
 
 use std::ffi::CString;
 use std::ops::{Deref, DerefMut};
+use serde::{Serialize, Deserialize};
 
 pub trait WrappedTypesupport {
     type CStruct;
@@ -132,9 +133,9 @@ pub fn rcl_create_context() -> Result<rcl_context_t, ()> {
 }
 
 pub fn rcl_create_node(
+    ctx: &mut rcl_context_t,
     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();
@@ -290,6 +291,10 @@ pub fn rcl_take_subst(
         }
     }
 
+    unsafe {
+        rcl_wait_set_fini(&mut ws);
+    }
+
     Ok(())
 }
 
@@ -363,4 +368,30 @@ mod tests {
         assert_eq!(msg,msg2);
     }
 
+    #[test]
+    fn test_float_sequence() -> () {
+        use trajectory_msgs::msg::*;
+        let native = WrappedNativeMsg::<JointTrajectoryPoint>::new();
+        let mut msg = JointTrajectoryPoint::from_native(&native);
+        msg.positions.push(39.0);
+        msg.positions.push(34.0);
+        let new_native = WrappedNativeMsg::<JointTrajectoryPoint>::from(&msg);
+        let new_msg = JointTrajectoryPoint::from_native(&new_native);
+        println!("{:#?}", new_msg);
+        assert_eq!(msg,new_msg);
+    }
+
+    #[test]
+    fn test_deault() -> () {
+        use trajectory_msgs::msg::*;
+        let mut msg: JointTrajectoryPoint = Default::default();
+        msg.positions.push(39.0);
+        msg.positions.push(34.0);
+        let mut new_native = WrappedNativeMsg::<JointTrajectoryPoint>::from(&msg);
+        unsafe { *((*new_native).positions.data) = 88.9 };
+        let new_msg = JointTrajectoryPoint::from_native(&new_native);
+        println!("{:#?}", new_msg);
+        assert_ne!(msg,new_msg);
+    }
+
 }