Parse connection headers and String messages

Author: jonbinney

Cargo.toml

@@ -12,6 +12,3 @@ git = "https://github.com/rust-lang/regex"
 [dependencies.log]
 git = "https://github.com/rust-lang/log"
 
-[dependencies.rustc-serialize]
-git = "https://github.com/rust-lang/rustc-serialize"
-

src/bin/tcpros_subscriber.rs

@@ -3,6 +3,46 @@ extern crate ros_rust;
 use std::mem;
 use std::os;
 use std::old_io::TcpStream;
+use std::old_io::MemReader;
+use std::collections::HashMap;
+
+use ros_rust::msg::std_msgs;
+
+/// Read a TCPROS connection header into a HashMap of key and value strings
+fn read_connection_header<R: Reader>(stream: &mut R) -> Result<HashMap<String, String>, String> {
+    let header_length = match stream.read_le_u32() {
+        Ok(header_length) => header_length,
+        Err(_) => return Err("Failed to read connection header length".to_string()),
+    };
+
+    // Read the connection header fields
+    let mut header_bytes_read = 0u32;
+    let mut fields: HashMap<String, String> = HashMap::new();
+    while header_bytes_read < header_length {
+        // Read the length of this field
+        let field_length = match stream.read_le_u32() {
+            Ok(field_length) => field_length,
+            Err(_) => return Err("Failed to read connection header length".to_string()),
+        };
+        header_bytes_read += 4;
+
+        // Read the field itself
+        let field_bytes = match stream.read_exact(field_length as usize) {
+            Ok(field_bytes) => field_bytes,
+            Err(_) => return Err("Failed to read connection header field".to_string()),
+        };
+
+        let field_string = match String::from_utf8(field_bytes) {
+            Ok(s) => s,
+            Err(_) => return Err("Failed to interpret connection header field as string".to_string()),
+        };
+
+        println!("{:?}", field_string);
+
+        header_bytes_read += field_length;
+    }
+    Ok(fields)
+}
 
 #[allow(dead_code)]
 fn main() {
@@ -47,10 +87,16 @@ fn main() {
         Err(_) => panic!("Unable to send data to server".to_string()),
     };
 
+    // Read the connection header that the server sends
+    let connection_header_fields = match read_connection_header(&mut stream) {
+        Ok(fields) => fields,
+        Err(err) => panic!(err),
+    };
+
     println!("Reading data");
     loop {
-        match stream.read_byte() {
-            Ok(b) => println!("{}", b.to_string()),
+        match std_msgs::String::from_stream(&mut stream) {
+            Ok(s) => println!("{}", s.data),
             Err(_) => panic!("Read failed!"),
         }
     }

src/lib.rs

@@ -9,7 +9,8 @@ extern crate regex;
 #[macro_use]
 extern crate log;
 
-pub mod xml;
+pub mod msg;
 pub mod http;
+pub mod xml;
 pub mod xmlrpc;
 

src/msg/mod.rs

@@ -0,0 +1 @@
+pub mod std_msgs;

src/msg/std_msgs/mod.rs

@@ -0,0 +1,3 @@
+mod string;
+
+pub use msg::std_msgs::string::String;

src/msg/std_msgs/string.rs

@@ -0,0 +1,29 @@
+use std::string::String as std_String;
+use std::old_io::Reader;
+
+pub struct String {
+    pub data: std_String,
+}
+
+impl String {
+    pub fn from_stream<R: Reader>(stream : &mut R) -> Result<String, std_String> {
+        let string_size = match stream.read_le_u32() {
+            Ok(string_size) => string_size,
+            Err(_) => return Err("Failed to read string length".to_string()),
+        };
+
+        // Read the string data
+        let string_data_bytes = match stream.read_exact(string_size as usize) {
+            Ok(string_data_bytes) => string_data_bytes,
+            Err(_) => return Err("Failed to read string data field".to_string()),
+        };
+
+        let data = match std_String::from_utf8(string_data_bytes) {
+            Ok(data) => data,
+            Err(_) => return Err("Failed to interpret string data as utf8 string".to_string()),
+        };
+
+        Ok(String {data: data})
+    }
+}
+