From 8502a3a7d432632673a8cb6e9a59cc745124f324 Mon Sep 17 00:00:00 2001
From: Martin Dahl <martin.dahl@gmail.com>
Date: Tue, 20 Aug 2019 13:16:24 +0200
Subject: [PATCH] Arrays of Strings

---
 msg_gen/build.rs   |  1 +
 msg_gen/src/lib.rs | 39 ++++++++-------------------------------
 rcl/src/lib.rs     | 29 +++++++++++++++++++++++++++++
 3 files changed, 38 insertions(+), 31 deletions(-)

diff --git a/msg_gen/build.rs b/msg_gen/build.rs
index 6052e97..faef4f2 100644
--- a/msg_gen/build.rs
+++ b/msg_gen/build.rs
@@ -59,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__String__Sequence")
         .blacklist_type("rosidl_generator_c__double__Sequence") // etc...
         .default_enum_style(bindgen::EnumVariation::Rust { non_exhaustive: false } );
 
diff --git a/msg_gen/src/lib.rs b/msg_gen/src/lib.rs
index 36b50bd..4b60dcc 100644
--- a/msg_gen/src/lib.rs
+++ b/msg_gen/src/lib.rs
@@ -14,25 +14,6 @@ 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/msg/Duration
-//  int32 sec
-//  uint32 nanosec
-//  
-//  
-
-
 fn field_type(t: u8) -> String {
 
     // rosidl_typesupport_introspection_c__ROS_TYPE_FLOAT = 1,
@@ -119,7 +100,9 @@ pub fn generate_rust_msg(module: &str, prefix: &str, name: &str) -> String {
 
             let rust_field_type = field_type(type_id);
 
-            if rust_field_type == "std::string::String" {
+            if is_array {
+                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));
             } else if rust_field_type == "message" {
                 // perform a hack!
@@ -131,11 +114,7 @@ pub fn generate_rust_msg(module: &str, prefix: &str, name: &str) -> String {
                 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 {
-                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(&format!("{field_name}: msg.{field_name},\n", field_name = field_name));
             }
         }
         from_native.push_str("      }\n    }\n");
@@ -151,16 +130,14 @@ pub fn generate_rust_msg(module: &str, prefix: &str, name: &str) -> String {
             let rust_field_type = field_type(type_id);
 
             // handle other special cases...
-            if rust_field_type == "std::string::String" {
+            if is_array {
+                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));
             } 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 {
-                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(&format!("msg.{field_name} = self.{field_name};\n", field_name = field_name));
             }
         }
         copy_to_native.push_str("}\n");
diff --git a/rcl/src/lib.rs b/rcl/src/lib.rs
index 58fdfae..42dfd86 100644
--- a/rcl/src/lib.rs
+++ b/rcl/src/lib.rs
@@ -59,6 +59,35 @@ impl Default for rmw_qos_profile_t {
 // conversions from/to vectors
 // macro:ify this increase maintainability
 
+impl rosidl_generator_c__String__Sequence {
+    pub fn update(&mut self, values: &[String]) {
+        unsafe { rosidl_generator_c__String__Sequence__fini(self as *mut _); }
+        unsafe { rosidl_generator_c__String__Sequence__init(self as *mut _, values.len()); }
+        let strs = unsafe { std::slice::from_raw_parts_mut(self.data, values.len()) };
+        for (target, source) in strs.iter_mut().zip(values) {
+            target.assign(&source);
+        }
+    }
+
+    pub fn to_vec(&self) -> Vec<String> {
+        let mut dst = Vec::with_capacity(self.size);
+        let strs = unsafe { std::slice::from_raw_parts(self.data, self.size) };
+        for s in strs {
+            dst.push(s.to_str().to_owned());
+        }
+        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,
+    //     );
+    // }
+}
+
+
 impl rosidl_generator_c__double__Sequence {
     pub fn update(&mut self, values: &[f64]) {
         // crash here?