From 3776ed1ad52ce8d58c2660de7df2902655289e63 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 23 Aug 2017 02:36:31 +0200 Subject: [PATCH 01/50] Reformat code --- src/api/master.rs | 55 +- src/api/naming/mapper.rs | 36 +- src/api/naming/mod.rs | 184 ++++--- src/api/naming/path.rs | 128 +++-- src/api/resolve.rs | 53 +- src/api/ros.rs | 75 +-- src/api/slave.rs | 104 ++-- src/api/slavehandler.rs | 180 ++++--- src/api/value.rs | 2 +- src/build_tools/genmsg.rs | 61 ++- src/build_tools/helpers.rs | 252 ++++++---- src/build_tools/msg.rs | 885 +++++++++++++++++++-------------- src/msg.rs | 4 +- src/rosxmlrpc/client.rs | 10 +- src/rosxmlrpc/serde/decoder.rs | 297 ++++++----- src/rosxmlrpc/serde/encoder.rs | 576 +++++++++++---------- src/rosxmlrpc/serde/value.rs | 526 ++++++++++++-------- src/rosxmlrpc/server.rs | 26 +- src/tcpros/client.rs | 83 ++-- src/tcpros/header.rs | 303 +++++++++-- src/tcpros/publisher.rs | 62 ++- src/tcpros/service.rs | 117 +++-- src/tcpros/subscriber.rs | 74 +-- src/tcpros/util/streamfork.rs | 6 +- 24 files changed, 2497 insertions(+), 1602 deletions(-) diff --git a/src/api/master.rs b/src/api/master.rs index e06fc75..9bdc279 100644 --- a/src/api/master.rs +++ b/src/api/master.rs @@ -49,40 +49,58 @@ impl Master { let values = match data { rosxmlrpc::XmlRpcValue::Array(values) => values, _ => { - bail!(ErrorKind::XmlRpc(ReErrorKind::Serde(SeErrorKind::MismatchedDataFormat( - "while handling request".into())))) + bail!(ErrorKind::XmlRpc( + ReErrorKind::Serde(SeErrorKind::MismatchedDataFormat( + "while handling request".into(), + )), + )) } }; if values.len() != 3 { - bail!(ErrorKind::XmlRpc(ReErrorKind::Serde(SeErrorKind::MismatchedDataFormat("while \ + bail!(ErrorKind::XmlRpc( + ReErrorKind::Serde(SeErrorKind::MismatchedDataFormat( + "while \ handling \ request" - .into())))) + .into(), + )), + )) } let mut values = values.into_iter(); let code = match values.next() { Some(rosxmlrpc::XmlRpcValue::Int(v)) => v, _ => { - bail!(ErrorKind::XmlRpc(ReErrorKind::Serde(SeErrorKind::MismatchedDataFormat( - "while handling request".into())))) + bail!(ErrorKind::XmlRpc( + ReErrorKind::Serde(SeErrorKind::MismatchedDataFormat( + "while handling request".into(), + )), + )) } }; let message = match values.next() { Some(rosxmlrpc::XmlRpcValue::String(v)) => v, _ => { - bail!(ErrorKind::XmlRpc(ReErrorKind::Serde(SeErrorKind::MismatchedDataFormat( - "while handling request".into())))) + bail!(ErrorKind::XmlRpc( + ReErrorKind::Serde(SeErrorKind::MismatchedDataFormat( + "while handling request".into(), + )), + )) } }; let value = match values.next() { Some(v) => v, _ => { - bail!(ErrorKind::XmlRpc(ReErrorKind::Serde(SeErrorKind::MismatchedDataFormat( - "while handling request".into())))) + bail!(ErrorKind::XmlRpc( + ReErrorKind::Serde(SeErrorKind::MismatchedDataFormat( + "while handling request".into(), + )), + )) } }; if code != 1 { - bail!(ErrorKind::XmlRpc(ReErrorKind::Serde(SeErrorKind::Msg(message)))); + bail!(ErrorKind::XmlRpc( + ReErrorKind::Serde(SeErrorKind::Msg(message)), + )); } Ok(value) } @@ -185,10 +203,9 @@ impl Master { fn to_api_error(v: (bool, String)) -> Error { use super::error::api::ErrorKind as ApiErrorKind; match v.0 { - false => ErrorKind::Api(ApiErrorKind::SystemFail(v.1)), - true => ErrorKind::Api(ApiErrorKind::BadData(v.1)), - } - .into() + false => ErrorKind::Api(ApiErrorKind::SystemFail(v.1)), + true => ErrorKind::Api(ApiErrorKind::BadData(v.1)), + }.into() } #[derive(Debug)] @@ -201,20 +218,22 @@ impl Decodable for ResponseData { let message = d.read_struct_field("status_message", 1, |d| d.read_str())?; match code { 0 | -1 => Ok(ResponseData(Err((code != 0, message)))), - 1 => Ok(ResponseData(Ok(d.read_struct_field("data", 2, |d| T::decode(d))?))), + 1 => Ok(ResponseData( + Ok(d.read_struct_field("data", 2, |d| T::decode(d))?), + )), _ => Err(d.error("Invalid response code returned by ROS")), } }) } } -#[derive(Debug,RustcDecodable)] +#[derive(Debug, RustcDecodable)] pub struct TopicData { pub name: String, pub connections: Vec, } -#[derive(Debug,RustcDecodable)] +#[derive(Debug, RustcDecodable)] pub struct SystemState { pub publishers: Vec, pub subscribers: Vec, diff --git a/src/api/naming/mapper.rs b/src/api/naming/mapper.rs index f890cb0..924ad64 100644 --- a/src/api/naming/mapper.rs +++ b/src/api/naming/mapper.rs @@ -52,10 +52,14 @@ mod tests { let src2 = "/foo/ter".parse::().expect(FAILED_TO_MAP); let dst = "/d/e/f".parse::().expect(FAILED_TO_MAP); mapper.add(src2.get(), dst); - assert_eq!("/a/b/c", - format!("{}", mapper.translate(src1.get()).expect(FAILED_TO_MAP))); - assert_eq!("/d/e/f", - format!("{}", mapper.translate(src2.get()).expect(FAILED_TO_MAP))); + assert_eq!( + "/a/b/c", + format!("{}", mapper.translate(src1.get()).expect(FAILED_TO_MAP)) + ); + assert_eq!( + "/d/e/f", + format!("{}", mapper.translate(src2.get()).expect(FAILED_TO_MAP)) + ); } #[test] @@ -67,10 +71,14 @@ mod tests { let src2 = "/foo/ter".parse::().expect(FAILED_TO_MAP); let dst = "/".parse::().expect(FAILED_TO_MAP); mapper.add(src2.get(), dst); - assert_eq!("/a/b/c", - format!("{}", mapper.translate(src1.get()).expect(FAILED_TO_MAP))); - assert_eq!("", - format!("{}", mapper.translate(src2.get()).expect(FAILED_TO_MAP))); + assert_eq!( + "/a/b/c", + format!("{}", mapper.translate(src1.get()).expect(FAILED_TO_MAP)) + ); + assert_eq!( + "", + format!("{}", mapper.translate(src2.get()).expect(FAILED_TO_MAP)) + ); } #[test] @@ -92,11 +100,15 @@ mod tests { let src = "/foo/bar".parse::().expect(FAILED_TO_MAP); let dst1 = "/a/b/c".parse::().expect(FAILED_TO_MAP); mapper.add(src.get(), dst1); - assert_eq!("/a/b/c", - format!("{}", mapper.translate(src.get()).expect(FAILED_TO_MAP))); + assert_eq!( + "/a/b/c", + format!("{}", mapper.translate(src.get()).expect(FAILED_TO_MAP)) + ); let dst2 = "/d/e/f".parse::().expect(FAILED_TO_MAP); mapper.add(src.get(), dst2); - assert_eq!("/d/e/f", - format!("{}", mapper.translate(src.get()).expect(FAILED_TO_MAP))); + assert_eq!( + "/d/e/f", + format!("{}", mapper.translate(src.get()).expect(FAILED_TO_MAP)) + ); } } diff --git a/src/api/naming/mod.rs b/src/api/naming/mod.rs index b609634..c5613a9 100644 --- a/src/api/naming/mod.rs +++ b/src/api/naming/mod.rs @@ -17,10 +17,10 @@ impl Resolver { let path = name.parse::()?; let namespace = path.parent()?.take(); Ok(Resolver { - path: path, - namespace: namespace, - mapper: Mapper::new(), - }) + path: path, + namespace: namespace, + mapper: Mapper::new(), + }) } pub fn map(&mut self, source: &str, destination: &str) -> Result<(), Error> { @@ -36,10 +36,10 @@ impl Resolver { return name.parse(); } Ok(if first_char == b'~' { - self.path.slice() + (String::from("/") + &name[1..]).parse::()? - } else { - self.namespace.slice() + (String::from("/") + name).parse::()? - }) + self.path.slice() + (String::from("/") + &name[1..]).parse::()? + } else { + self.namespace.slice() + (String::from("/") + name).parse::()? + }) } pub fn translate(&self, name: &str) -> Result { @@ -83,68 +83,106 @@ mod tests { #[test] fn resolves_absolute_names() { let r = Resolver::new("/some/long/path").expect(FAILED_TO_RESOLVE); - assert_eq!(vec![String::from("foo")], - r.resolve("/foo").expect(FAILED_TO_RESOLVE).get()); - assert_eq!(vec![String::from("foo"), String::from("bar")], - r.resolve("/foo/bar").expect(FAILED_TO_RESOLVE).get()); - assert_eq!(vec![String::from("f1_aA"), - String::from("Ba02"), - String::from("Xx")], - r.resolve("/f1_aA/Ba02/Xx").expect(FAILED_TO_RESOLVE).get()); + assert_eq!( + vec![String::from("foo")], + r.resolve("/foo").expect(FAILED_TO_RESOLVE).get() + ); + assert_eq!( + vec![String::from("foo"), String::from("bar")], + r.resolve("/foo/bar").expect(FAILED_TO_RESOLVE).get() + ); + assert_eq!( + vec![ + String::from("f1_aA"), + String::from("Ba02"), + String::from("Xx"), + ], + r.resolve("/f1_aA/Ba02/Xx").expect(FAILED_TO_RESOLVE).get() + ); } #[test] fn resolves_relative_names() { let r = Resolver::new("/some/long/path").expect(FAILED_TO_RESOLVE); - assert_eq!(vec![String::from("some"), - String::from("long"), - String::from("foo")], - r.resolve("foo").expect(FAILED_TO_RESOLVE).get()); - assert_eq!(vec![String::from("some"), - String::from("long"), - String::from("foo"), - String::from("bar")], - r.resolve("foo/bar").expect(FAILED_TO_RESOLVE).get()); - assert_eq!(vec![String::from("some"), - String::from("long"), - String::from("f1_aA"), - String::from("Ba02"), - String::from("Xx")], - r.resolve("f1_aA/Ba02/Xx").expect(FAILED_TO_RESOLVE).get()); + assert_eq!( + vec![ + String::from("some"), + String::from("long"), + String::from("foo"), + ], + r.resolve("foo").expect(FAILED_TO_RESOLVE).get() + ); + assert_eq!( + vec![ + String::from("some"), + String::from("long"), + String::from("foo"), + String::from("bar"), + ], + r.resolve("foo/bar").expect(FAILED_TO_RESOLVE).get() + ); + assert_eq!( + vec![ + String::from("some"), + String::from("long"), + String::from("f1_aA"), + String::from("Ba02"), + String::from("Xx"), + ], + r.resolve("f1_aA/Ba02/Xx").expect(FAILED_TO_RESOLVE).get() + ); } #[test] fn resolves_private_names() { let r = Resolver::new("/some/long/path").expect(FAILED_TO_RESOLVE); - assert_eq!(vec![String::from("some"), - String::from("long"), - String::from("path"), - String::from("foo")], - r.resolve("~foo").expect(FAILED_TO_RESOLVE).get()); - assert_eq!(vec![String::from("some"), - String::from("long"), - String::from("path"), - String::from("foo"), - String::from("bar")], - r.resolve("~foo/bar").expect(FAILED_TO_RESOLVE).get()); - assert_eq!(vec![String::from("some"), - String::from("long"), - String::from("path"), - String::from("f1_aA"), - String::from("Ba02"), - String::from("Xx")], - r.resolve("~f1_aA/Ba02/Xx").expect(FAILED_TO_RESOLVE).get()); + assert_eq!( + vec![ + String::from("some"), + String::from("long"), + String::from("path"), + String::from("foo"), + ], + r.resolve("~foo").expect(FAILED_TO_RESOLVE).get() + ); + assert_eq!( + vec![ + String::from("some"), + String::from("long"), + String::from("path"), + String::from("foo"), + String::from("bar"), + ], + r.resolve("~foo/bar").expect(FAILED_TO_RESOLVE).get() + ); + assert_eq!( + vec![ + String::from("some"), + String::from("long"), + String::from("path"), + String::from("f1_aA"), + String::from("Ba02"), + String::from("Xx"), + ], + r.resolve("~f1_aA/Ba02/Xx").expect(FAILED_TO_RESOLVE).get() + ); } #[test] fn translates_strings() { let r = Resolver::new("/some/long/path").expect(FAILED_TO_RESOLVE); - assert_eq!(String::from("/f1_aA/Ba02/Xx"), - r.translate("/f1_aA/Ba02/Xx").expect(FAILED_TO_RESOLVE)); - assert_eq!(String::from("/some/long/f1_aA/Ba02/Xx"), - r.translate("f1_aA/Ba02/Xx").expect(FAILED_TO_RESOLVE)); - assert_eq!(String::from("/some/long/path/f1_aA/Ba02/Xx"), - r.translate("~f1_aA/Ba02/Xx").expect(FAILED_TO_RESOLVE)); + assert_eq!( + String::from("/f1_aA/Ba02/Xx"), + r.translate("/f1_aA/Ba02/Xx").expect(FAILED_TO_RESOLVE) + ); + assert_eq!( + String::from("/some/long/f1_aA/Ba02/Xx"), + r.translate("f1_aA/Ba02/Xx").expect(FAILED_TO_RESOLVE) + ); + assert_eq!( + String::from("/some/long/path/f1_aA/Ba02/Xx"), + r.translate("~f1_aA/Ba02/Xx").expect(FAILED_TO_RESOLVE) + ); } #[test] @@ -155,17 +193,29 @@ mod tests { r.map("/z", "/f").expect(FAILED_TO_RESOLVE); r.map("/a1", "g").expect(FAILED_TO_RESOLVE); r.map("a2", "~g").expect(FAILED_TO_RESOLVE); - assert_eq!(String::from("/d"), - r.translate("/some/long/a").expect(FAILED_TO_RESOLVE)); - assert_eq!(String::from("/e"), - r.translate("path/x").expect(FAILED_TO_RESOLVE)); - assert_eq!(String::from("/f"), - r.translate("/z").expect(FAILED_TO_RESOLVE)); - assert_eq!(String::from("/some/long/g"), - r.translate("/a1").expect(FAILED_TO_RESOLVE)); - assert_eq!(String::from("/some/long/path/g"), - r.translate("/some/long/a2").expect(FAILED_TO_RESOLVE)); - assert_eq!(String::from("/some/long/other"), - r.translate("other").expect(FAILED_TO_RESOLVE)); + assert_eq!( + String::from("/d"), + r.translate("/some/long/a").expect(FAILED_TO_RESOLVE) + ); + assert_eq!( + String::from("/e"), + r.translate("path/x").expect(FAILED_TO_RESOLVE) + ); + assert_eq!( + String::from("/f"), + r.translate("/z").expect(FAILED_TO_RESOLVE) + ); + assert_eq!( + String::from("/some/long/g"), + r.translate("/a1").expect(FAILED_TO_RESOLVE) + ); + assert_eq!( + String::from("/some/long/path/g"), + r.translate("/some/long/a2").expect(FAILED_TO_RESOLVE) + ); + assert_eq!( + String::from("/some/long/other"), + r.translate("other").expect(FAILED_TO_RESOLVE) + ); } } diff --git a/src/api/naming/path.rs b/src/api/naming/path.rs index 2661758..03dd489 100644 --- a/src/api/naming/path.rs +++ b/src/api/naming/path.rs @@ -20,7 +20,7 @@ pub trait Path { } } -#[derive(Clone,Debug)] +#[derive(Clone, Debug)] pub struct Slice<'a> { chain: &'a [String], } @@ -52,7 +52,7 @@ impl<'a, T: Path> Add for Slice<'a> { } } -#[derive(Clone,Debug)] +#[derive(Clone, Debug)] pub struct Buffer { chain: Vec, } @@ -154,65 +154,97 @@ mod tests { #[test] fn names_are_parsed() { - assert_eq!(vec![String::from("foo")], - "/foo".parse::().expect(FAILED_TO_HANDLE).get()); - assert_eq!(vec![String::from("foo"), String::from("bar")], - "/foo/bar".parse::().expect(FAILED_TO_HANDLE).get()); - assert_eq!(vec![String::from("f1_aA"), - String::from("Ba02"), - String::from("Xx")], - "/f1_aA/Ba02/Xx" - .parse::() - .expect(FAILED_TO_HANDLE) - .get()); + assert_eq!( + vec![String::from("foo")], + "/foo".parse::().expect(FAILED_TO_HANDLE).get() + ); + assert_eq!( + vec![String::from("foo"), String::from("bar")], + "/foo/bar".parse::().expect(FAILED_TO_HANDLE).get() + ); + assert_eq!( + vec![ + String::from("f1_aA"), + String::from("Ba02"), + String::from("Xx"), + ], + "/f1_aA/Ba02/Xx" + .parse::() + .expect(FAILED_TO_HANDLE) + .get() + ); } #[test] fn is_formatted() { - assert_eq!("/foo", - format!("{}", "/foo".parse::().expect(FAILED_TO_HANDLE))); - assert_eq!("/foo/bar", - format!("{}", "/foo/bar".parse::().expect(FAILED_TO_HANDLE))); - assert_eq!("/f1_aA/Ba02/Xx", - format!("{}", - "/f1_aA/Ba02/Xx".parse::().expect(FAILED_TO_HANDLE))); + assert_eq!( + "/foo", + format!("{}", "/foo".parse::().expect(FAILED_TO_HANDLE)) + ); + assert_eq!( + "/foo/bar", + format!("{}", "/foo/bar".parse::().expect(FAILED_TO_HANDLE)) + ); + assert_eq!( + "/f1_aA/Ba02/Xx", + format!( + "{}", + "/f1_aA/Ba02/Xx".parse::().expect(FAILED_TO_HANDLE) + ) + ); } #[test] fn parents_are_handled() { - assert_eq!("", - format!("{}", - "/foo" - .parse::() - .expect(FAILED_TO_HANDLE) - .parent() - .expect(FAILED_TO_HANDLE))); - assert_eq!("/foo", - format!("{}", - "/foo/bar" - .parse::() - .expect(FAILED_TO_HANDLE) - .parent() - .expect(FAILED_TO_HANDLE))); - assert_eq!("/f1_aA/Ba02", - format!("{}", - "/f1_aA/Ba02/Xx" - .parse::() - .expect(FAILED_TO_HANDLE) - .parent() - .expect(FAILED_TO_HANDLE))); - assert!("/" + assert_eq!( + "", + format!( + "{}", + "/foo" .parse::() .expect(FAILED_TO_HANDLE) .parent() - .is_err()); - assert!("/foo" + .expect(FAILED_TO_HANDLE) + ) + ); + assert_eq!( + "/foo", + format!( + "{}", + "/foo/bar" .parse::() .expect(FAILED_TO_HANDLE) .parent() .expect(FAILED_TO_HANDLE) + ) + ); + assert_eq!( + "/f1_aA/Ba02", + format!( + "{}", + "/f1_aA/Ba02/Xx" + .parse::() + .expect(FAILED_TO_HANDLE) .parent() - .is_err()); + .expect(FAILED_TO_HANDLE) + ) + ); + assert!( + "/" + .parse::() + .expect(FAILED_TO_HANDLE) + .parent() + .is_err() + ); + assert!( + "/foo" + .parse::() + .expect(FAILED_TO_HANDLE) + .parent() + .expect(FAILED_TO_HANDLE) + .parent() + .is_err() + ); } #[test] @@ -221,8 +253,10 @@ mod tests { let bar = "/B4r/x".parse::().expect(FAILED_TO_HANDLE); let baz = "/bA_z".parse::().expect(FAILED_TO_HANDLE); assert_eq!("/foo/B4r/x", format!("{}", foo.slice() + bar.slice())); - assert_eq!("/B4r/x/foo/foo", - format!("{}", bar.slice() + foo.slice() + foo.slice())); + assert_eq!( + "/B4r/x/foo/foo", + format!("{}", bar.slice() + foo.slice() + foo.slice()) + ); assert_eq!("/foo/B4r/x/bA_z", format!("{}", foo + bar + baz)); } } diff --git a/src/api/resolve.rs b/src/api/resolve.rs index 76f8fdd..cb78ea6 100644 --- a/src/api/resolve.rs +++ b/src/api/resolve.rs @@ -42,8 +42,11 @@ pub fn mappings() -> Vec<(String, String)> { .filter(|v| v.len() == 2) .map(|v| v.into_iter().map(fix_prefix)) .map(|mut v| { - (v.next().expect(UNEXPECTED_EMPTY_ARRAY), v.next().expect(UNEXPECTED_EMPTY_ARRAY)) - }) + ( + v.next().expect(UNEXPECTED_EMPTY_ARRAY), + v.next().expect(UNEXPECTED_EMPTY_ARRAY), + ) + }) .collect() } @@ -56,18 +59,18 @@ fn fix_prefix(v: String) -> String { } fn find_with_prefix(prefix: &str) -> Option { - args() - .skip(1) - .find(|v| v.starts_with(prefix)) - .map(|v| String::from(v.trim_left_matches(prefix))) + args().skip(1).find(|v| v.starts_with(prefix)).map(|v| { + String::from(v.trim_left_matches(prefix)) + }) } #[cfg(not(test))] fn system_hostname() -> String { use nix::unistd::gethostname; let mut hostname = [0u8; 256]; - gethostname(&mut hostname) - .expect("Hostname is either unavailable or too long to fit into buffer"); + gethostname(&mut hostname).expect( + "Hostname is either unavailable or too long to fit into buffer", + ); let hostname = hostname .into_iter() .take_while(|&v| *v != 0u8) @@ -133,20 +136,26 @@ mod tests { let testcase = TESTCASE.lock().expect(FAILED_TO_LOCK); set_args(&vec![]); assert_eq!(Vec::<(String, String)>::new(), mappings()); - set_args(&vec!["a:=x", - "b=e", - "/c:=d", - "e:=/f_g", - "__name:=something", - "a:=b:=c", - "_oo_e:=/ab_c", - "/x_y:=_i"]); - assert_eq!(vec![(String::from("a"), String::from("x")), - (String::from("/c"), String::from("d")), - (String::from("e"), String::from("/f_g")), - (String::from("~oo_e"), String::from("/ab_c")), - (String::from("/x_y"), String::from("~i"))], - mappings()); + set_args(&vec![ + "a:=x", + "b=e", + "/c:=d", + "e:=/f_g", + "__name:=something", + "a:=b:=c", + "_oo_e:=/ab_c", + "/x_y:=_i", + ]); + assert_eq!( + vec![ + (String::from("a"), String::from("x")), + (String::from("/c"), String::from("d")), + (String::from("e"), String::from("/f_g")), + (String::from("~oo_e"), String::from("/ab_c")), + (String::from("/x_y"), String::from("~i")), + ], + mappings() + ); } #[test] diff --git a/src/api/ros.rs b/src/api/ros.rs index c2609ab..948d478 100644 --- a/src/api/ros.rs +++ b/src/api/ros.rs @@ -34,7 +34,9 @@ impl Ros { let namespace = namespace.trim_right_matches("/"); if name.contains("/") { - bail!(ErrorKind::Naming(naming::error::ErrorKind::IllegalCharacter(name.into()))); + bail!(ErrorKind::Naming( + naming::error::ErrorKind::IllegalCharacter(name.into()), + )); } let name = format!("{}/{}", namespace, name); @@ -43,12 +45,12 @@ impl Ros { let slave = Slave::new(&master_uri, &hostname, 0, &name)?; let master = Master::new(&master_uri, &name, &slave.uri()); Ok(Ros { - master: master, - slave: slave, - hostname: String::from(hostname), - resolver: resolver, - name: name, - }) + master: master, + slave: slave, + hostname: String::from(hostname), + resolver: resolver, + name: name, + }) } fn map(&mut self, source: &str, destination: &str) -> Result<()> { @@ -60,15 +62,12 @@ impl Ros { } pub fn param<'a, 'b>(&'a self, name: &'b str) -> Option> { - self.resolver - .translate(name) - .ok() - .map(|v| { - Parameter { - master: &self.master, - name: v, - } - }) + self.resolver.translate(name).ok().map(|v| { + Parameter { + master: &self.master, + name: v, + } + }) } pub fn parameters(&self) -> MasterResult> { @@ -90,12 +89,16 @@ impl Ros { } pub fn service(&mut self, service: &str, handler: F) -> Result<()> - where T: ServicePair, - F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static + where + T: ServicePair, + F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static, { let name = self.resolver.translate(service)?; - let api = self.slave - .add_service::(&self.hostname, &name, handler)?; + let api = self.slave.add_service::( + &self.hostname, + &name, + handler, + )?; if let Err(err) = self.master.register_service(&name, &api) { self.slave.remove_service(&name); @@ -108,20 +111,25 @@ impl Ros { } pub fn subscribe(&mut self, topic: &str, callback: F) -> Result<()> - where T: Message, - F: Fn(T) -> () + Send + 'static + where + T: Message, + F: Fn(T) -> () + Send + 'static, { let name = self.resolver.translate(topic)?; self.slave.add_subscription::(&name, callback)?; match self.master.register_subscriber(&name, &T::msg_type()) { Ok(publishers) => { - if let Err(err) = - self.slave - .add_publishers_to_subscription(&name, publishers.into_iter()) { - error!("Failed to subscribe to all publishers of topic '{}': {}", - name, - err); + if let Err(err) = self.slave.add_publishers_to_subscription( + &name, + publishers.into_iter(), + ) + { + error!( + "Failed to subscribe to all publishers of topic '{}': {}", + name, + err + ); } Ok(()) } @@ -134,16 +142,19 @@ impl Ros { } pub fn publish(&mut self, topic: &str) -> Result> - where T: Message + where + T: Message, { let name = self.resolver.translate(topic)?; let stream = self.slave.add_publication::(&self.hostname, &name)?; match self.master.register_publisher(&name, &T::msg_type()) { Ok(_) => Ok(stream), Err(error) => { - error!("Failed to register publisher for topic '{}': {}", - name, - error); + error!( + "Failed to register publisher for topic '{}': {}", + name, + error + ); self.slave.remove_publication(&name); self.master.unregister_publisher(&name)?; Err(error.into()) diff --git a/src/api/slave.rs b/src/api/slave.rs index d2eda02..d2d771a 100644 --- a/src/api/slave.rs +++ b/src/api/slave.rs @@ -36,43 +36,50 @@ impl Slave { } }); Ok(Slave { - name: String::from(name), - uri: uri, - publications: pubs, - subscriptions: subs, - services: services, - }) + name: String::from(name), + uri: uri, + publications: pubs, + subscriptions: subs, + services: services, + }) } pub fn uri(&self) -> &str { return &self.uri; } - pub fn add_publishers_to_subscription(&mut self, - topic: &str, - publishers: T) - -> SerdeResult<()> - where T: Iterator + pub fn add_publishers_to_subscription( + &mut self, + topic: &str, + publishers: T, + ) -> SerdeResult<()> + where + T: Iterator, { - add_publishers_to_subscription(&mut self.subscriptions.lock().expect(FAILED_TO_LOCK), - &self.name, - topic, - publishers) + add_publishers_to_subscription( + &mut self.subscriptions.lock().expect(FAILED_TO_LOCK), + &self.name, + topic, + publishers, + ) } - pub fn add_service(&mut self, - hostname: &str, - service: &str, - handler: F) - -> SerdeResult - where T: ServicePair, - F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static + pub fn add_service( + &mut self, + hostname: &str, + service: &str, + handler: F, + ) -> SerdeResult + where + T: ServicePair, + F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static, { use std::collections::hash_map::Entry; - match self.services - .lock() - .expect(FAILED_TO_LOCK) - .entry(String::from(service)) { + match self.services.lock().expect(FAILED_TO_LOCK).entry( + String::from( + service, + ), + ) { Entry::Occupied(..) => { error!("Duplicate initiation of service '{}' attempted", service); Err(ErrorKind::Duplicate("service".into()).into()) @@ -90,17 +97,18 @@ impl Slave { self.services.lock().expect(FAILED_TO_LOCK).remove(service); } - pub fn add_publication(&mut self, - hostname: &str, - topic: &str) - -> error::tcpros::Result> - where T: Message + pub fn add_publication( + &mut self, + hostname: &str, + topic: &str, + ) -> error::tcpros::Result> + where + T: Message, { use std::collections::hash_map::Entry; - match self.publications - .lock() - .expect(FAILED_TO_LOCK) - .entry(String::from(topic)) { + match self.publications.lock().expect(FAILED_TO_LOCK).entry( + String::from(topic), + ) { Entry::Occupied(publisher_entry) => publisher_entry.get().stream(), Entry::Vacant(entry) => { let publisher = Publisher::new::(format!("{}:0", hostname).as_str(), topic)?; @@ -110,21 +118,20 @@ impl Slave { } pub fn remove_publication(&mut self, topic: &str) { - self.publications - .lock() - .expect(FAILED_TO_LOCK) - .remove(topic); + self.publications.lock().expect(FAILED_TO_LOCK).remove( + topic, + ); } pub fn add_subscription(&mut self, topic: &str, callback: F) -> Result<()> - where T: Message, - F: Fn(T) -> () + Send + 'static + where + T: Message, + F: Fn(T) -> () + Send + 'static, { use std::collections::hash_map::Entry; - match self.subscriptions - .lock() - .expect(FAILED_TO_LOCK) - .entry(String::from(topic)) { + match self.subscriptions.lock().expect(FAILED_TO_LOCK).entry( + String::from(topic), + ) { Entry::Occupied(..) => { error!("Duplicate subscription to topic '{}' attempted", topic); Err(ErrorKind::Duplicate("subscription".into()).into()) @@ -138,10 +145,9 @@ impl Slave { } pub fn remove_subscription(&mut self, topic: &str) { - self.subscriptions - .lock() - .expect(FAILED_TO_LOCK) - .remove(topic); + self.subscriptions.lock().expect(FAILED_TO_LOCK).remove( + topic, + ); } } diff --git a/src/api/slavehandler.rs b/src/api/slavehandler.rs index b34c67a..09df35e 100644 --- a/src/api/slavehandler.rs +++ b/src/api/slavehandler.rs @@ -20,10 +20,11 @@ pub struct SlaveHandler { } impl XmlRpcServer for SlaveHandler { - fn handle(&self, - method_name: &str, - mut req: ParameterIterator) - -> error::rosxmlrpc::serde::Result { + fn handle( + &self, + method_name: &str, + mut req: ParameterIterator, + ) -> error::rosxmlrpc::serde::Result { info!("Slave API method called: {}", method_name); self.handle_call(method_name, &mut req) } @@ -41,11 +42,12 @@ macro_rules! pop{ } impl SlaveHandler { - pub fn new(master_uri: &str, - hostname: &str, - name: &str, - shutdown_signal: Sender<()>) - -> SlaveHandler { + pub fn new( + master_uri: &str, + hostname: &str, + name: &str, + shutdown_signal: Sender<()>, + ) -> SlaveHandler { SlaveHandler { subscriptions: Arc::new(Mutex::new(HashMap::new())), publications: Arc::new(Mutex::new(HashMap::new())), @@ -57,10 +59,11 @@ impl SlaveHandler { } } - fn handle_call(&self, - method_name: &str, - req: &mut ParameterIterator) - -> error::rosxmlrpc::serde::Result { + fn handle_call( + &self, + method_name: &str, + req: &mut ParameterIterator, + ) -> error::rosxmlrpc::serde::Result { match method_name { "getBusStats" => encode_response(self.get_bus_stats(req), "Bus stats"), "getBusInfo" => encode_response(self.get_bus_info(req), "Bus stats"), @@ -122,9 +125,9 @@ impl SlaveHandler { fn shutdown(&self, req: &mut ParameterIterator) -> HandleResult { let caller_id = pop!(req; String); - let message = pop::(req) - .unwrap_or(Err("".into())) - .unwrap_or("".into()); + let message = pop::(req).unwrap_or(Err("".into())).unwrap_or( + "".into(), + ); if caller_id == "" { return Ok(Err("Empty strings given".into())); } @@ -140,17 +143,19 @@ impl SlaveHandler { if caller_id == "" { return Ok(Err("Empty strings given".into())); } - Ok(Ok(self.publications - .lock() - .expect(FAILED_TO_LOCK) - .values() - .map(|ref v| { - return Topic { - name: v.topic.clone(), - datatype: v.msg_type.clone(), - }; - }) - .collect())) + Ok(Ok( + self.publications + .lock() + .expect(FAILED_TO_LOCK) + .values() + .map(|ref v| { + return Topic { + name: v.topic.clone(), + datatype: v.msg_type.clone(), + }; + }) + .collect(), + )) } fn get_subscriptions(&self, req: &mut ParameterIterator) -> HandleResult> { @@ -158,17 +163,19 @@ impl SlaveHandler { if caller_id == "" { return Ok(Err("Empty strings given".into())); } - Ok(Ok(self.subscriptions - .lock() - .expect(FAILED_TO_LOCK) - .values() - .map(|ref v| { - return Topic { - name: v.topic.clone(), - datatype: v.msg_type.clone(), - }; - }) - .collect())) + Ok(Ok( + self.subscriptions + .lock() + .expect(FAILED_TO_LOCK) + .values() + .map(|ref v| { + return Topic { + name: v.topic.clone(), + datatype: v.msg_type.clone(), + }; + }) + .collect(), + )) } fn publisher_update(&self, req: &mut ParameterIterator) -> HandleResult { @@ -178,11 +185,12 @@ impl SlaveHandler { if caller_id == "" || topic == "" || publishers.iter().any(|ref x| x.as_str() == "") { return Ok(Err("Empty strings given".into())); } - add_publishers_to_subscription(&mut self.subscriptions.lock().expect(FAILED_TO_LOCK), - &self.name, - &topic, - publishers.into_iter()) - .and(Ok(Ok(0))) + add_publishers_to_subscription( + &mut self.subscriptions.lock().expect(FAILED_TO_LOCK), + &self.name, + &topic, + publishers.into_iter(), + ).and(Ok(Ok(0))) } fn get_master_uri(&self, req: &mut ParameterIterator) -> HandleResult<&str> { @@ -212,9 +220,11 @@ impl SlaveHandler { let protocols = match protocols { XmlRpcValue::Array(protocols) => protocols, _ => { - return Ok(Err("Protocols need to be provided as [ [String, \ + return Ok(Err( + "Protocols need to be provided as [ [String, \ XmlRpcLegalValue] ]" - .into())); + .into(), + )); } }; let mut has_tcpros = false; @@ -233,12 +243,14 @@ impl SlaveHandler { } } -pub fn add_publishers_to_subscription(subscriptions: &mut HashMap, - name: &str, - topic: &str, - publishers: T) - -> Result<()> - where T: Iterator +pub fn add_publishers_to_subscription( + subscriptions: &mut HashMap, + name: &str, + topic: &str, + publishers: T, +) -> Result<()> +where + T: Iterator, { if let Some(mut subscription) = subscriptions.get_mut(topic) { for publisher in publishers { @@ -255,62 +267,66 @@ pub fn add_publishers_to_subscription(subscriptions: &mut HashMap(response: HandleResult, - message: &str) - -> error::rosxmlrpc::serde::Result { +fn encode_response( + response: HandleResult, + message: &str, +) -> error::rosxmlrpc::serde::Result { use std::error::Error; let mut res = Answer::new(); match response { - Ok(value) => { - match value { - // Success - Ok(value) => res.add(&(1i32, message, value)), - // Bad request provided - Err(err) => res.add(&(-1i32, err, 0)), - } + Ok(value) => { + match value { + // Success + Ok(value) => res.add(&(1i32, message, value)), + // Bad request provided + Err(err) => res.add(&(-1i32, err, 0)), } - // System failure while handling request - Err(err) => res.add(&(0i32, err.description(), 0)), } - .map(|_| res) + // System failure while handling request + Err(err) => res.add(&(0i32, err.description(), 0)), + }.map(|_| res) } fn pop(req: &mut ParameterIterator) -> HandleResult { Ok(Ok(match req.next() { - Some(v) => v, - None => return Ok(Err("Missing parameter".into())), - } - .read::() - .map_err(|v| error::rosxmlrpc::Error::from(v))?)) + Some(v) => v, + None => return Ok(Err("Missing parameter".into())), + }.read::() + .map_err(|v| error::rosxmlrpc::Error::from(v))?)) } -fn connect_to_publisher(subscriber: &mut Subscriber, - caller_id: &str, - publisher: &str, - topic: &str) - -> Result<()> { +fn connect_to_publisher( + subscriber: &mut Subscriber, + caller_id: &str, + publisher: &str, + topic: &str, +) -> Result<()> { let (protocol, hostname, port) = request_topic(publisher, caller_id, topic)?; if protocol != "TCPROS" { - bail!("Publisher responded with a non-TCPROS protocol: {}", - protocol) + bail!( + "Publisher responded with a non-TCPROS protocol: {}", + protocol + ) } subscriber .connect_to((hostname.as_str(), port as u16)) .map_err(|err| ErrorKind::Io(err).into()) } -fn request_topic(publisher_uri: &str, - caller_id: &str, - topic: &str) - -> error::rosxmlrpc::Result<(String, String, i32)> { +fn request_topic( + publisher_uri: &str, + caller_id: &str, + topic: &str, +) -> error::rosxmlrpc::Result<(String, String, i32)> { let mut request = rosxmlrpc::client::Request::new("requestTopic"); request.add(&caller_id)?; request.add(&topic)?; request.add(&[["TCPROS"]])?; let client = rosxmlrpc::Client::new(publisher_uri); - let protocols = client - .request::<(i32, String, (String, String, i32))>(request)?; + let protocols = client.request::<(i32, String, (String, String, i32))>( + request, + )?; Ok(protocols.2) } diff --git a/src/api/value.rs b/src/api/value.rs index e7126eb..42f4380 100644 --- a/src/api/value.rs +++ b/src/api/value.rs @@ -1,4 +1,4 @@ -#[derive(Debug,RustcDecodable,RustcEncodable)] +#[derive(Debug, RustcDecodable, RustcEncodable)] pub struct Topic { pub name: String, pub datatype: String, diff --git a/src/build_tools/genmsg.rs b/src/build_tools/genmsg.rs index a45627b..a69f0fe 100644 --- a/src/build_tools/genmsg.rs +++ b/src/build_tools/genmsg.rs @@ -16,10 +16,9 @@ pub fn depend_on_messages(folders: &[&str], messages: &[&str]) -> Result .messages .iter() .map(|(&(ref pack, ref _name), ref _value)| pack.clone()) - .chain(message_map - .services - .iter() - .map(|&(ref pack, ref _name)| pack.clone())) + .chain(message_map.services.iter().map(|&(ref pack, ref _name)| { + pack.clone() + })) .collect::>(); for package in packages { output.push(format!(" pub mod {} {{", package)); @@ -31,16 +30,18 @@ pub fn depend_on_messages(folders: &[&str], messages: &[&str]) -> Result .collect::>(); for name in &names { let key = (package.clone(), name.clone()); - let message = message_map - .messages - .get(&key) - .expect("Internal implementation contains mismatch in map keys"); - let hash = hashes - .get(&key) - .expect("Internal implementation contains mismatch in map keys"); + let message = message_map.messages.get(&key).expect( + "Internal implementation contains mismatch in map keys", + ); + let hash = hashes.get(&key).expect( + "Internal implementation contains mismatch in map keys", + ); let definition = helpers::generate_message_definition(&message_map.messages, &message)?; output.push(message.struct_string()); - output.push(format!(" impl ::rosrust::Message for {} {{", message.name)); + output.push(format!( + " impl ::rosrust::Message for {} {{", + message.name + )); output.push(create_function("msg_definition", &definition)); output.push(create_function("md5sum", &hash)); output.push(create_function("msg_type", &message.get_type())); @@ -66,16 +67,21 @@ pub fn depend_on_messages(folders: &[&str], messages: &[&str]) -> Result .map(|&(ref _pack, ref name)| name.clone()) .collect::>(); for name in &names { - let hash = hashes - .get(&(package.clone(), name.clone())) - .expect("Internal implementation contains mismatch in map keys"); - output.push(" #[allow(dead_code,non_camel_case_types,non_snake_case)]".into()); + let hash = hashes.get(&(package.clone(), name.clone())).expect( + "Internal implementation contains mismatch in map keys", + ); + output.push( + " #[allow(dead_code,non_camel_case_types,non_snake_case)]".into(), + ); output.push(" #[derive(Serialize,Deserialize,Debug)]".into()); output.push(format!(" pub struct {} {{}}", name)); output.push(format!(" impl ::rosrust::Message for {} {{", name)); output.push(create_function("msg_definition", "")); output.push(create_function("md5sum", &hash)); - output.push(create_function("msg_type", &format!("{}/{}", package, name))); + output.push(create_function( + "msg_type", + &format!("{}/{}", package, name), + )); output.push(" }".into()); output.push(format!(" impl ::rosrust::Service for {} {{", name)); @@ -90,12 +96,14 @@ pub fn depend_on_messages(folders: &[&str], messages: &[&str]) -> Result } fn create_function(name: &str, value: &str) -> String { - format!(r#" + format!( + r#" fn {}() -> ::std::string::String {{ {:?}.into() }}"#, - name, - value) + name, + value + ) } fn string_into_pair<'a>(input: &'a str) -> Result<(&'a str, &'a str)> { @@ -107,8 +115,10 @@ fn string_into_pair<'a>(input: &'a str) -> Result<(&'a str, &'a str)> { let name = match parts.next() { Some(v) => v, None => { - bail!("Package string needs to be in package/name format: {}", - input) + bail!( + "Package string needs to be in package/name format: {}", + input + ) } }; Ok((package, name)) @@ -129,9 +139,10 @@ mod tests { #[test] fn depend_on_messages_printout() { - let data = depend_on_messages(&[&FILEPATH], - &["geometry_msgs/PoseStamped", "sensor_msgs/Imu"]) - .unwrap(); + let data = depend_on_messages( + &[&FILEPATH], + &["geometry_msgs/PoseStamped", "sensor_msgs/Imu"], + ).unwrap(); println!("{}", data); // TODO: actually test this output data } diff --git a/src/build_tools/helpers.rs b/src/build_tools/helpers.rs index 2ce582e..e439e77 100644 --- a/src/build_tools/helpers.rs +++ b/src/build_tools/helpers.rs @@ -35,8 +35,10 @@ pub fn calculate_md5(message_map: &MessageMap) -> Result bail!("Message map does not contain all needed elements"), }; println!("|{}|{}|", req, res); - hashes.insert((pack.clone(), name.clone()), - calculate_md5_from_representation(&format!("{}{}", req, res))); + hashes.insert( + (pack.clone(), name.clone()), + calculate_md5_from_representation(&format!("{}{}", req, res)), + ); } if hashes.len() < message_map.messages.len() + message_map.services.len() { bail!("Message map does not contain all needed elements"); @@ -52,9 +54,10 @@ fn calculate_md5_from_representation(v: &str) -> String { hasher.result_str() } -pub fn generate_message_definition(message_map: &HashMap<(String, String), Msg>, - message: &Msg) - -> Result { +pub fn generate_message_definition( + message_map: &HashMap<(String, String), Msg>, + message: &Msg, +) -> Result { let mut handled_messages = HashSet::<(String, String)>::new(); let mut result = message.source.clone(); let mut pending = message @@ -120,9 +123,9 @@ pub fn get_message_map(folders: &[&str], messages: &[(&str, &str)]) -> Result Result Result v, @@ -170,9 +175,11 @@ fn get_message(folders: &[&str], package: &str, name: &str) -> Result Result { let fields = match_lines(source)?; Ok(Msg { - package: package.to_owned(), - name: name.to_owned(), - fields: fields, - source: source.trim().into(), - }) + package: package.to_owned(), + name: name.to_owned(), + fields: fields, + source: source.trim().into(), + }) } pub fn new_string(&self) -> String { @@ -28,18 +28,22 @@ impl Msg { for field in self.fields.iter() { match field.case { FieldCase::Unit => { - lines.push(format!(" {}: {},", - field.name, - field.datatype.rust_newtype())); + lines.push(format!( + " {}: {},", + field.name, + field.datatype.rust_newtype() + )); } FieldCase::Vector => { lines.push(format!(" {}: Vec::new(),", field.name)); } FieldCase::Array(l) => { let newtype = field.datatype.rust_newtype(); - lines.push(format!(" {}: [{}],", - field.name, - itertools::repeat_n(newtype.as_str(), l).join(", "))); + lines.push(format!( + " {}: [{}],", + field.name, + itertools::repeat_n(newtype.as_str(), l).join(", ") + )); } FieldCase::Const(..) => {} } @@ -57,20 +61,17 @@ impl Msg { self.fields .iter() .filter_map(|field| match field.datatype { - DataType::LocalStruct(ref name) => { - Some((self.package.clone(), name.clone())) - } - DataType::RemoteStruct(ref pkg, ref name) => { - Some((pkg.clone(), name.clone())) - } - _ => None, - }) + DataType::LocalStruct(ref name) => Some((self.package.clone(), name.clone())), + DataType::RemoteStruct(ref pkg, ref name) => Some((pkg.clone(), name.clone())), + _ => None, + }) .collect() } - pub fn calculate_md5(&self, - hashes: &HashMap<(String, String), String>) - -> ::std::result::Result { + pub fn calculate_md5( + &self, + hashes: &HashMap<(String, String), String>, + ) -> ::std::result::Result { use crypto::md5::Md5; use crypto::digest::Digest; let mut hasher = Md5::new(); @@ -78,9 +79,10 @@ impl Msg { Ok(hasher.result_str()) } - pub fn get_md5_representation(&self, - hashes: &HashMap<(String, String), String>) - -> ::std::result::Result { + pub fn get_md5_representation( + &self, + hashes: &HashMap<(String, String), String>, + ) -> ::std::result::Result { let constants = self.fields .iter() .filter(|ref v| v.is_constant()) @@ -105,7 +107,9 @@ impl Msg { output.push(format!(" pub mod {} {{", self.name)); for field in &self.fields { if let Some(s) = field.to_const_string() { - output.push(" #[allow(dead_code,non_upper_case_globals)]".into()); + output.push( + " #[allow(dead_code,non_upper_case_globals)]".into(), + ); output.push(format!(" pub {}", s)); } } @@ -115,7 +119,9 @@ impl Msg { pub fn struct_string(&self) -> String { let mut output = Vec::::new(); - output.push(" #[allow(dead_code,non_camel_case_types,non_snake_case)]".into()); + output.push( + " #[allow(dead_code,non_camel_case_types,non_snake_case)]".into(), + ); output.push(" #[derive(Serialize,Deserialize,Debug)]".into()); output.push(format!(" pub struct {} {{", self.name)); for field in &self.fields { @@ -145,9 +151,9 @@ fn match_field(data: &str) -> Option { None => return None, }; Some(FieldLine { - field_type: captures.get(1).unwrap().as_str().into(), - field_name: captures.get(2).unwrap().as_str().into(), - }) + field_type: captures.get(1).unwrap().as_str().into(), + field_name: captures.get(2).unwrap().as_str().into(), + }) } fn match_vector_field(data: &str) -> Option { @@ -162,9 +168,9 @@ fn match_vector_field(data: &str) -> Option { None => return None, }; Some(FieldLine { - field_type: captures.get(1).unwrap().as_str().into(), - field_name: captures.get(2).unwrap().as_str().into(), - }) + field_type: captures.get(1).unwrap().as_str().into(), + field_name: captures.get(2).unwrap().as_str().into(), + }) } fn match_array_field(data: &str) -> Option<(FieldLine, usize)> { @@ -178,11 +184,13 @@ fn match_array_field(data: &str) -> Option<(FieldLine, usize)> { Some(v) => v, None => return None, }; - Some((FieldLine { - field_type: captures.get(1).unwrap().as_str().into(), - field_name: captures.get(3).unwrap().as_str().into(), - }, - captures.get(2).unwrap().as_str().parse().unwrap())) + Some(( + FieldLine { + field_type: captures.get(1).unwrap().as_str().into(), + field_name: captures.get(3).unwrap().as_str().into(), + }, + captures.get(2).unwrap().as_str().parse().unwrap(), + )) } fn match_const_string(data: &str) -> Option<(FieldLine, String)> { @@ -196,11 +204,13 @@ fn match_const_string(data: &str) -> Option<(FieldLine, String)> { Some(v) => v, None => return None, }; - Some((FieldLine { - field_type: captures.get(1).unwrap().as_str().into(), - field_name: captures.get(2).unwrap().as_str().into(), - }, - captures.get(3).unwrap().as_str().into())) + Some(( + FieldLine { + field_type: captures.get(1).unwrap().as_str().into(), + field_name: captures.get(2).unwrap().as_str().into(), + }, + captures.get(3).unwrap().as_str().into(), + )) } fn match_const_numeric(data: &str) -> Option<(FieldLine, String)> { @@ -214,16 +224,22 @@ fn match_const_numeric(data: &str) -> Option<(FieldLine, String)> { Some(v) => v, None => return None, }; - Some((FieldLine { - field_type: captures.get(1).unwrap().as_str().into(), - field_name: captures.get(2).unwrap().as_str().into(), - }, - captures.get(3).unwrap().as_str().into())) + Some(( + FieldLine { + field_type: captures.get(1).unwrap().as_str().into(), + field_name: captures.get(2).unwrap().as_str().into(), + }, + captures.get(3).unwrap().as_str().into(), + )) } fn match_line(data: &str) -> Option> { if let Some((info, data)) = match_const_string(data.trim()) { - return Some(FieldInfo::new(&info.field_type, &info.field_name, FieldCase::Const(data))); + return Some(FieldInfo::new( + &info.field_type, + &info.field_name, + FieldCase::Const(data), + )); } let data = match strip_useless(data) { Ok(v) => v, @@ -234,29 +250,49 @@ fn match_line(data: &str) -> Option> { return None; } if let Some(info) = match_field(data) { - return Some(FieldInfo::new(&info.field_type, &info.field_name, FieldCase::Unit)); + return Some(FieldInfo::new( + &info.field_type, + &info.field_name, + FieldCase::Unit, + )); } if let Some(info) = match_vector_field(data) { - return Some(FieldInfo::new(&info.field_type, &info.field_name, FieldCase::Vector)); + return Some(FieldInfo::new( + &info.field_type, + &info.field_name, + FieldCase::Vector, + )); } if let Some((info, count)) = match_array_field(data) { - return Some(FieldInfo::new(&info.field_type, &info.field_name, FieldCase::Array(count))); + return Some(FieldInfo::new( + &info.field_type, + &info.field_name, + FieldCase::Array(count), + )); } if let Some((info, data)) = match_const_numeric(data) { - return Some(FieldInfo::new(&info.field_type, &info.field_name, FieldCase::Const(data))); + return Some(FieldInfo::new( + &info.field_type, + &info.field_name, + FieldCase::Const(data), + )); } Some(Err(format!("Unsupported content of line: {}", data).into())) } #[inline] fn strip_useless<'a>(data: &'a str) -> Result<&'a str> { - Ok(data.splitn(2, '#') - .next() - .ok_or_else(|| { - format!("Somehow splitting a line resulted in 0 parts?! Happened here: {}", - data) - })? - .trim()) + Ok( + data.splitn(2, '#') + .next() + .ok_or_else(|| { + format!( + "Somehow splitting a line resulted in 0 parts?! Happened here: {}", + data + ) + })? + .trim(), + ) } #[inline] @@ -267,13 +303,13 @@ fn match_lines(data: &str) -> Result> { .chain_err(|| "Failed to parse line in data string") } -#[derive(Debug,PartialEq)] +#[derive(Debug, PartialEq)] struct FieldLine { field_type: String, field_name: String, } -#[derive(Debug,PartialEq)] +#[derive(Debug, PartialEq)] enum FieldCase { Unit, Vector, @@ -281,7 +317,7 @@ enum FieldCase { Const(String), } -#[derive(Debug,PartialEq)] +#[derive(Debug, PartialEq)] struct FieldInfo { datatype: DataType, name: String, @@ -296,17 +332,18 @@ impl FieldInfo { } } - fn md5_string(&self, - package: &str, - hashes: &HashMap<(String, String), String>) - -> ::std::result::Result { + fn md5_string( + &self, + package: &str, + hashes: &HashMap<(String, String), String>, + ) -> ::std::result::Result { let datatype = self.datatype.md5_string(package, hashes)?; Ok(match self.case { - FieldCase::Unit => format!("{} {}", datatype, self.name), - FieldCase::Vector => format!("{}[] {}", datatype, self.name), - FieldCase::Array(l) => format!("{}[{}] {}", datatype, l, self.name), - FieldCase::Const(ref v) => format!("{} {}={}", datatype, self.name, v), - }) + FieldCase::Unit => format!("{} {}", datatype, self.name), + FieldCase::Vector => format!("{}[] {}", datatype, self.name), + FieldCase::Array(l) => format!("{}[{}] {}", datatype, l, self.name), + FieldCase::Const(ref v) => format!("{} {}={}", datatype, self.name, v), + }) } fn to_string(&self) -> Option { @@ -325,34 +362,37 @@ impl FieldInfo { _ => return None, }; Some(match self.datatype { - DataType::Bool => format!("const {}: bool = {:?};", self.name, value != "0"), - DataType::String => format!("static {}: &'static str = {:?};", self.name, value), - DataType::Time => return None, - DataType::Duration => return None, - DataType::LocalStruct(..) => return None, - DataType::RemoteStruct(..) => return None, - _ => { - let datatype = self.datatype.rust_type(); - format!("const {}: {} = {} as {};", - self.name, - datatype, - value, - datatype) - } - }) + DataType::Bool => format!("const {}: bool = {:?};", self.name, value != "0"), + DataType::String => format!("static {}: &'static str = {:?};", self.name, value), + DataType::Time => return None, + DataType::Duration => return None, + DataType::LocalStruct(..) => return None, + DataType::RemoteStruct(..) => return None, + _ => { + let datatype = self.datatype.rust_type(); + format!( + "const {}: {} = {} as {};", + self.name, + datatype, + value, + datatype + ) + } + }) } fn new(datatype: &str, name: &str, case: FieldCase) -> Result { Ok(FieldInfo { - datatype: parse_datatype(&datatype) - .ok_or_else(|| format!("Unsupported datatype: {}", datatype))?, - name: name.to_owned(), - case: case, - }) + datatype: parse_datatype(&datatype).ok_or_else(|| { + format!("Unsupported datatype: {}", datatype) + })?, + name: name.to_owned(), + case: case, + }) } } -#[derive(Debug,PartialEq)] +#[derive(Debug, PartialEq)] enum DataType { Bool, I8, @@ -411,36 +451,38 @@ impl DataType { } } - fn md5_string(&self, - package: &str, - hashes: &HashMap<(String, String), String>) - -> ::std::result::Result { - Ok(match *self { - DataType::Bool => "bool", - DataType::I8 => "int8", - DataType::I16 => "int16", - DataType::I32 => "int32", - DataType::I64 => "int64", - DataType::U8 => "uint8", - DataType::U16 => "uint16", - DataType::U32 => "uint32", - DataType::U64 => "uint64", - DataType::F32 => "float32", - DataType::F64 => "float64", - DataType::String => "string", - DataType::Time => "time", - DataType::Duration => "duration", - DataType::LocalStruct(ref name) => { - hashes - .get(&(package.to_owned(), name.clone())) - .ok_or(())? - .as_str() - } - DataType::RemoteStruct(ref pkg, ref name) => { - hashes.get(&(pkg.clone(), name.clone())).ok_or(())?.as_str() - } - } - .into()) + fn md5_string( + &self, + package: &str, + hashes: &HashMap<(String, String), String>, + ) -> ::std::result::Result { + Ok( + match *self { + DataType::Bool => "bool", + DataType::I8 => "int8", + DataType::I16 => "int16", + DataType::I32 => "int32", + DataType::I64 => "int64", + DataType::U8 => "uint8", + DataType::U16 => "uint16", + DataType::U32 => "uint32", + DataType::U64 => "uint64", + DataType::F32 => "float32", + DataType::F64 => "float64", + DataType::String => "string", + DataType::Time => "time", + DataType::Duration => "duration", + DataType::LocalStruct(ref name) => { + hashes + .get(&(package.to_owned(), name.clone())) + .ok_or(())? + .as_str() + } + DataType::RemoteStruct(ref pkg, ref name) => { + hashes.get(&(pkg.clone(), name.clone())).ok_or(())?.as_str() + } + }.into(), + ) } } @@ -467,7 +509,10 @@ fn parse_datatype(datatype: &str) -> Option { return None; } match parts.len() { - 2 => Some(DataType::RemoteStruct(parts[0].to_owned(), parts[1].to_owned())), + 2 => Some(DataType::RemoteStruct( + parts[0].to_owned(), + parts[1].to_owned(), + )), 1 => Some(DataType::LocalStruct(parts[0].to_owned())), _ => None, } @@ -485,24 +530,36 @@ mod tests { let mut hashes = HashMap::new(); hashes.insert(("p1".into(), "xx".into()), "ABCD".into()); hashes.insert(("p2".into(), "xx".into()), "EFGH".into()); - assert_eq!(DataType::I64.md5_string("", &hashes).unwrap(), - "int64".to_owned()); - assert_eq!(DataType::F32.md5_string("", &hashes).unwrap(), - "float32".to_owned()); - assert_eq!(DataType::String.md5_string("", &hashes).unwrap(), - "string".to_owned()); - assert_eq!(DataType::LocalStruct("xx".into()) - .md5_string("p1", &hashes) - .unwrap(), - "ABCD".to_owned()); - assert_eq!(DataType::LocalStruct("xx".into()) - .md5_string("p2", &hashes) - .unwrap(), - "EFGH".to_owned()); - assert_eq!(DataType::RemoteStruct("p1".into(), "xx".into()) - .md5_string("p2", &hashes) - .unwrap(), - "ABCD".to_owned()); + assert_eq!( + DataType::I64.md5_string("", &hashes).unwrap(), + "int64".to_owned() + ); + assert_eq!( + DataType::F32.md5_string("", &hashes).unwrap(), + "float32".to_owned() + ); + assert_eq!( + DataType::String.md5_string("", &hashes).unwrap(), + "string".to_owned() + ); + assert_eq!( + DataType::LocalStruct("xx".into()) + .md5_string("p1", &hashes) + .unwrap(), + "ABCD".to_owned() + ); + assert_eq!( + DataType::LocalStruct("xx".into()) + .md5_string("p2", &hashes) + .unwrap(), + "EFGH".to_owned() + ); + assert_eq!( + DataType::RemoteStruct("p1".into(), "xx".into()) + .md5_string("p2", &hashes) + .unwrap(), + "ABCD".to_owned() + ); } #[test] @@ -510,114 +567,155 @@ mod tests { let mut hashes = HashMap::new(); hashes.insert(("p1".into(), "xx".into()), "ABCD".into()); hashes.insert(("p2".into(), "xx".into()), "EFGH".into()); - assert_eq!(FieldInfo::new("int64", "abc", FieldCase::Unit) - .unwrap() - .md5_string("", &hashes) - .unwrap(), - "int64 abc".to_owned()); - assert_eq!(FieldInfo::new("float32", "abc", FieldCase::Array(3)) - .unwrap() - .md5_string("", &hashes) - .unwrap(), - "float32[3] abc".to_owned()); - assert_eq!(FieldInfo::new("string", "abc", FieldCase::Const("something".into())) - .unwrap() - .md5_string("", &hashes) - .unwrap(), - "string abc=something".to_owned()); - assert_eq!(FieldInfo::new("xx", "abc", FieldCase::Vector) - .unwrap() - .md5_string("p1", &hashes) - .unwrap(), - "ABCD[] abc".to_owned()); - assert_eq!(FieldInfo::new("p2/xx", "abc", FieldCase::Unit) - .unwrap() - .md5_string("p1", &hashes) - .unwrap(), - "EFGH abc".to_owned()); + assert_eq!( + FieldInfo::new("int64", "abc", FieldCase::Unit) + .unwrap() + .md5_string("", &hashes) + .unwrap(), + "int64 abc".to_owned() + ); + assert_eq!( + FieldInfo::new("float32", "abc", FieldCase::Array(3)) + .unwrap() + .md5_string("", &hashes) + .unwrap(), + "float32[3] abc".to_owned() + ); + assert_eq!( + FieldInfo::new("string", "abc", FieldCase::Const("something".into())) + .unwrap() + .md5_string("", &hashes) + .unwrap(), + "string abc=something".to_owned() + ); + assert_eq!( + FieldInfo::new("xx", "abc", FieldCase::Vector) + .unwrap() + .md5_string("p1", &hashes) + .unwrap(), + "ABCD[] abc".to_owned() + ); + assert_eq!( + FieldInfo::new("p2/xx", "abc", FieldCase::Unit) + .unwrap() + .md5_string("p1", &hashes) + .unwrap(), + "EFGH abc".to_owned() + ); } #[test] fn message_md5_string_correct() { - assert_eq!(Msg::new("std_msgs", "String", "string data") - .unwrap() - .calculate_md5(&HashMap::new()) - .unwrap(), - "992ce8a1687cec8c8bd883ec73ca41d1".to_owned()); - assert_eq!(Msg::new("geometry_msgs", - "Point", - include_str!("msg_examples/geometry_msgs/msg/Point.msg")) - .unwrap() - .calculate_md5(&HashMap::new()) - .unwrap(), - "4a842b65f413084dc2b10fb484ea7f17".to_owned()); - assert_eq!(Msg::new("geometry_msgs", - "Quaternion", - include_str!("msg_examples/geometry_msgs/msg/Quaternion.msg")) - .unwrap() - .calculate_md5(&HashMap::new()) - .unwrap(), - "a779879fadf0160734f906b8c19c7004".to_owned()); + assert_eq!( + Msg::new("std_msgs", "String", "string data") + .unwrap() + .calculate_md5(&HashMap::new()) + .unwrap(), + "992ce8a1687cec8c8bd883ec73ca41d1".to_owned() + ); + assert_eq!( + Msg::new( + "geometry_msgs", + "Point", + include_str!("msg_examples/geometry_msgs/msg/Point.msg"), + ).unwrap() + .calculate_md5(&HashMap::new()) + .unwrap(), + "4a842b65f413084dc2b10fb484ea7f17".to_owned() + ); + assert_eq!( + Msg::new( + "geometry_msgs", + "Quaternion", + include_str!("msg_examples/geometry_msgs/msg/Quaternion.msg"), + ).unwrap() + .calculate_md5(&HashMap::new()) + .unwrap(), + "a779879fadf0160734f906b8c19c7004".to_owned() + ); let mut hashes = HashMap::new(); - hashes.insert(("geometry_msgs".into(), "Point".into()), - "4a842b65f413084dc2b10fb484ea7f17".into()); - hashes.insert(("geometry_msgs".into(), "Quaternion".into()), - "a779879fadf0160734f906b8c19c7004".into()); - assert_eq!(Msg::new("geometry_msgs", - "Pose", - include_str!("msg_examples/geometry_msgs/msg/Pose.msg")) - .unwrap() - .calculate_md5(&hashes) - .unwrap(), - "e45d45a5a1ce597b249e23fb30fc871f".to_owned()); + hashes.insert( + ("geometry_msgs".into(), "Point".into()), + "4a842b65f413084dc2b10fb484ea7f17".into(), + ); + hashes.insert( + ("geometry_msgs".into(), "Quaternion".into()), + "a779879fadf0160734f906b8c19c7004".into(), + ); + assert_eq!( + Msg::new( + "geometry_msgs", + "Pose", + include_str!("msg_examples/geometry_msgs/msg/Pose.msg"), + ).unwrap() + .calculate_md5(&hashes) + .unwrap(), + "e45d45a5a1ce597b249e23fb30fc871f".to_owned() + ); } #[test] fn match_field_matches_legal_field() { - assert_eq!(FieldLine { - field_type: "geom_msgs/Twist".into(), - field_name: "myname".into(), - }, - match_field("geom_msgs/Twist myname").unwrap()); + assert_eq!( + FieldLine { + field_type: "geom_msgs/Twist".into(), + field_name: "myname".into(), + }, + match_field("geom_msgs/Twist myname").unwrap() + ); } #[test] fn match_vector_field_matches_legal_field() { - assert_eq!(FieldLine { - field_type: "geom_msgs/Twist".into(), - field_name: "myname".into(), - }, - match_vector_field("geom_msgs/Twist [ ] myname").unwrap()); + assert_eq!( + FieldLine { + field_type: "geom_msgs/Twist".into(), + field_name: "myname".into(), + }, + match_vector_field("geom_msgs/Twist [ ] myname").unwrap() + ); } #[test] fn match_array_field_matches_legal_field() { - assert_eq!((FieldLine { - field_type: "geom_msgs/Twist".into(), - field_name: "myname".into(), - }, - 127), - match_array_field("geom_msgs/Twist [ 127 ] myname").unwrap()); + assert_eq!( + ( + FieldLine { + field_type: "geom_msgs/Twist".into(), + field_name: "myname".into(), + }, + 127, + ), + match_array_field("geom_msgs/Twist [ 127 ] myname").unwrap() + ); } #[test] fn match_const_string_matches_legal_field() { - assert_eq!((FieldLine { - field_type: "string".into(), - field_name: "myname".into(), - }, - "this is # data".into()), - match_const_string("string myname = this is # data").unwrap()); + assert_eq!( + ( + FieldLine { + field_type: "string".into(), + field_name: "myname".into(), + }, + "this is # data".into(), + ), + match_const_string("string myname = this is # data").unwrap() + ); } #[test] fn match_const_numeric_matches_legal_field() { - assert_eq!((FieldLine { - field_type: "mytype".into(), - field_name: "myname".into(), - }, - "-444".into()), - match_const_numeric("mytype myname = -444").unwrap()); + assert_eq!( + ( + FieldLine { + field_type: "mytype".into(), + field_name: "myname".into(), + }, + "-444".into(), + ), + match_const_numeric("mytype myname = -444").unwrap() + ); } #[test] @@ -626,80 +724,101 @@ mod tests { assert!(match_line("# YOLO ! ").is_none()); assert!(match_line(" ").is_none()); - assert_eq!(FieldInfo { - datatype: DataType::RemoteStruct("geom_msgs".into(), "Twist".into()), - name: "myname".into(), - case: FieldCase::Unit, - }, - match_line(" geom_msgs/Twist myname # this clearly should succeed") - .unwrap() - .unwrap()); - - assert_eq!(FieldInfo { - datatype: DataType::RemoteStruct("geom_msgs".into(), "Twist".into()), - name: "myname".into(), - case: FieldCase::Vector, - }, - match_line(" geom_msgs/Twist [ ] myname # ...") - .unwrap() - .unwrap()); - - assert_eq!(FieldInfo { - datatype: DataType::U8, - name: "myname".into(), - case: FieldCase::Array(127), - }, - match_line(" char [ 127 ] myname# comment") - .unwrap() - .unwrap()); - assert_eq!(FieldInfo { - datatype: DataType::String, - name: "myname".into(), - case: FieldCase::Const("this is # data".into()), - }, - match_line(" string myname = this is # data ") - .unwrap() - .unwrap()); - assert_eq!(FieldInfo { - datatype: DataType::RemoteStruct("geom_msgs".into(), "Twist".into()), - name: "myname".into(), - case: FieldCase::Const("-444".into()), - }, - match_line(" geom_msgs/Twist myname = -444 # data ") - .unwrap() - .unwrap()); + assert_eq!( + FieldInfo { + datatype: DataType::RemoteStruct("geom_msgs".into(), "Twist".into()), + name: "myname".into(), + case: FieldCase::Unit, + }, + match_line( + " geom_msgs/Twist myname # this clearly should succeed", + ).unwrap() + .unwrap() + ); + + assert_eq!( + FieldInfo { + datatype: DataType::RemoteStruct("geom_msgs".into(), "Twist".into()), + name: "myname".into(), + case: FieldCase::Vector, + }, + match_line(" geom_msgs/Twist [ ] myname # ...") + .unwrap() + .unwrap() + ); + + assert_eq!( + FieldInfo { + datatype: DataType::U8, + name: "myname".into(), + case: FieldCase::Array(127), + }, + match_line(" char [ 127 ] myname# comment") + .unwrap() + .unwrap() + ); + assert_eq!( + FieldInfo { + datatype: DataType::String, + name: "myname".into(), + case: FieldCase::Const("this is # data".into()), + }, + match_line(" string myname = this is # data ") + .unwrap() + .unwrap() + ); + assert_eq!( + FieldInfo { + datatype: DataType::RemoteStruct("geom_msgs".into(), "Twist".into()), + name: "myname".into(), + case: FieldCase::Const("-444".into()), + }, + match_line(" geom_msgs/Twist myname = -444 # data ") + .unwrap() + .unwrap() + ); } #[test] fn match_lines_parses_real_messages() { - let data = match_lines(include_str!("msg_examples/geometry_msgs/msg/TwistWithCovariance.\ - msg")) - .unwrap(); - assert_eq!(vec![FieldInfo { - datatype: DataType::LocalStruct("Twist".into()), - name: "twist".into(), - case: FieldCase::Unit, - }, - FieldInfo { - datatype: DataType::F64, - name: "covariance".into(), - case: FieldCase::Array(36), - }], - data); - - let data = match_lines(include_str!("msg_examples/geometry_msgs/msg/PoseStamped.msg")) - .unwrap(); - assert_eq!(vec![FieldInfo { - datatype: DataType::RemoteStruct("std_msgs".into(), "Header".into()), - name: "header".into(), - case: FieldCase::Unit, - }, - FieldInfo { - datatype: DataType::LocalStruct("Pose".into()), - name: "pose".into(), - case: FieldCase::Unit, - }], - data); + let data = match_lines(include_str!( + "msg_examples/geometry_msgs/msg/TwistWithCovariance.\ + msg" + )).unwrap(); + assert_eq!( + vec![ + FieldInfo { + datatype: DataType::LocalStruct("Twist".into()), + name: "twist".into(), + case: FieldCase::Unit, + }, + FieldInfo { + datatype: DataType::F64, + name: "covariance".into(), + case: FieldCase::Array(36), + }, + ], + data + ); + + let data = match_lines(include_str!( + "msg_examples/geometry_msgs/msg/PoseStamped.msg" + )).unwrap(); + assert_eq!( + vec![ + FieldInfo { + datatype: DataType::RemoteStruct("std_msgs".into(), "Header".into()), + name: "header".into(), + case: FieldCase::Unit, + }, + FieldInfo { + datatype: DataType::LocalStruct("Pose".into()), + name: "pose".into(), + case: FieldCase::Unit, + }, + ], + data + ); } fn get_dependency_set(message: &Msg) -> HashSet<(String, String)> { @@ -708,98 +827,118 @@ mod tests { #[test] fn msg_constructor_parses_real_message() { - let data = Msg::new("geometry_msgs", - "TwistWithCovariance", - include_str!("msg_examples/geometry_msgs/msg/TwistWithCovariance.msg")) - .unwrap(); + let data = Msg::new( + "geometry_msgs", + "TwistWithCovariance", + include_str!("msg_examples/geometry_msgs/msg/TwistWithCovariance.msg"), + ).unwrap(); assert_eq!(data.package, "geometry_msgs"); assert_eq!(data.name, "TwistWithCovariance"); - assert_eq!(data.fields, - vec![FieldInfo { - datatype: DataType::LocalStruct("Twist".into()), - name: "twist".into(), - case: FieldCase::Unit, - }, - FieldInfo { - datatype: DataType::F64, - name: "covariance".into(), - case: FieldCase::Array(36), - }]); + assert_eq!( + data.fields, + vec![ + FieldInfo { + datatype: DataType::LocalStruct("Twist".into()), + name: "twist".into(), + case: FieldCase::Unit, + }, + FieldInfo { + datatype: DataType::F64, + name: "covariance".into(), + case: FieldCase::Array(36), + }, + ] + ); let dependencies = get_dependency_set(&data); assert_eq!(dependencies.len(), 1); - assert!(dependencies.contains(&("geometry_msgs".into(), "Twist".into()))); - - let data = Msg::new("geometry_msgs", - "PoseStamped", - include_str!("msg_examples/geometry_msgs/msg/PoseStamped.msg")) - .unwrap(); + assert!(dependencies.contains( + &("geometry_msgs".into(), "Twist".into()), + )); + + let data = Msg::new( + "geometry_msgs", + "PoseStamped", + include_str!("msg_examples/geometry_msgs/msg/PoseStamped.msg"), + ).unwrap(); assert_eq!(data.package, "geometry_msgs"); assert_eq!(data.name, "PoseStamped"); - assert_eq!(data.fields, - vec![FieldInfo { - datatype: DataType::RemoteStruct("std_msgs".into(), "Header".into()), - name: "header".into(), - case: FieldCase::Unit, - }, - FieldInfo { - datatype: DataType::LocalStruct("Pose".into()), - name: "pose".into(), - case: FieldCase::Unit, - }]); + assert_eq!( + data.fields, + vec![ + FieldInfo { + datatype: DataType::RemoteStruct("std_msgs".into(), "Header".into()), + name: "header".into(), + case: FieldCase::Unit, + }, + FieldInfo { + datatype: DataType::LocalStruct("Pose".into()), + name: "pose".into(), + case: FieldCase::Unit, + }, + ] + ); let dependencies = get_dependency_set(&data); assert_eq!(dependencies.len(), 2); - assert!(dependencies.contains(&("geometry_msgs".into(), "Pose".into()))); + assert!(dependencies.contains( + &("geometry_msgs".into(), "Pose".into()), + )); assert!(dependencies.contains(&("std_msgs".into(), "Header".into()))); - let data = Msg::new("sensor_msgs", - "Imu", - include_str!("msg_examples/sensor_msgs/msg/Imu.msg")) - .unwrap(); + let data = Msg::new( + "sensor_msgs", + "Imu", + include_str!("msg_examples/sensor_msgs/msg/Imu.msg"), + ).unwrap(); assert_eq!(data.package, "sensor_msgs"); assert_eq!(data.name, "Imu"); - assert_eq!(data.fields, - vec![FieldInfo { - datatype: DataType::RemoteStruct("std_msgs".into(), "Header".into()), - name: "header".into(), - case: FieldCase::Unit, - }, - FieldInfo { - datatype: DataType::RemoteStruct("geometry_msgs".into(), - "Quaternion".into()), - name: "orientation".into(), - case: FieldCase::Unit, - }, - FieldInfo { - datatype: DataType::F64, - name: "orientation_covariance".into(), - case: FieldCase::Array(9), - }, - FieldInfo { - datatype: DataType::RemoteStruct("geometry_msgs".into(), - "Vector3".into()), - name: "angular_velocity".into(), - case: FieldCase::Unit, - }, - FieldInfo { - datatype: DataType::F64, - name: "angular_velocity_covariance".into(), - case: FieldCase::Array(9), - }, - FieldInfo { - datatype: DataType::RemoteStruct("geometry_msgs".into(), - "Vector3".into()), - name: "linear_acceleration".into(), - case: FieldCase::Unit, - }, - FieldInfo { - datatype: DataType::F64, - name: "linear_acceleration_covariance".into(), - case: FieldCase::Array(9), - }]); + assert_eq!( + data.fields, + vec![ + FieldInfo { + datatype: DataType::RemoteStruct("std_msgs".into(), "Header".into()), + name: "header".into(), + case: FieldCase::Unit, + }, + FieldInfo { + datatype: DataType::RemoteStruct("geometry_msgs".into(), "Quaternion".into()), + name: "orientation".into(), + case: FieldCase::Unit, + }, + FieldInfo { + datatype: DataType::F64, + name: "orientation_covariance".into(), + case: FieldCase::Array(9), + }, + FieldInfo { + datatype: DataType::RemoteStruct("geometry_msgs".into(), "Vector3".into()), + name: "angular_velocity".into(), + case: FieldCase::Unit, + }, + FieldInfo { + datatype: DataType::F64, + name: "angular_velocity_covariance".into(), + case: FieldCase::Array(9), + }, + FieldInfo { + datatype: DataType::RemoteStruct("geometry_msgs".into(), "Vector3".into()), + name: "linear_acceleration".into(), + case: FieldCase::Unit, + }, + FieldInfo { + datatype: DataType::F64, + name: "linear_acceleration_covariance".into(), + case: FieldCase::Array(9), + }, + ] + ); let dependencies = get_dependency_set(&data); assert_eq!(dependencies.len(), 3); - assert!(dependencies.contains(&("geometry_msgs".into(), "Vector3".into()))); - assert!(dependencies.contains(&("geometry_msgs".into(), "Quaternion".into()))); + assert!(dependencies.contains( + &("geometry_msgs".into(), "Vector3".into()), + )); + assert!(dependencies.contains( + &("geometry_msgs".into(), "Quaternion".into()), + )); assert!(dependencies.contains(&("std_msgs".into(), "Header".into()))); } } diff --git a/src/msg.rs b/src/msg.rs index 34fad64..35aa6ed 100644 --- a/src/msg.rs +++ b/src/msg.rs @@ -2,7 +2,7 @@ use std::ops; const BILLION: i64 = 1000000000; -#[derive(Serialize,Deserialize,Debug)] +#[derive(Serialize, Deserialize, Debug)] pub struct Time { pub sec: i32, pub nsec: i32, @@ -25,7 +25,7 @@ impl Time { } } -#[derive(Serialize,Deserialize,Debug)] +#[derive(Serialize, Deserialize, Debug)] pub struct Duration { pub sec: i32, pub nsec: i32, diff --git a/src/rosxmlrpc/client.rs b/src/rosxmlrpc/client.rs index 194577c..8ef08af 100644 --- a/src/rosxmlrpc/client.rs +++ b/src/rosxmlrpc/client.rs @@ -27,7 +27,9 @@ impl Client { match res.pop() { Some(v) => Ok(v.value()), None => { - bail!(ErrorKind::Serde(error::serde::ErrorKind::Decoding("request tree".into()))) + bail!(ErrorKind::Serde( + error::serde::ErrorKind::Decoding("request tree".into()), + )) } } } @@ -42,7 +44,11 @@ impl Client { let mut res = serde::Decoder::new_response(res)?; let mut value = match res.pop() { Some(v) => v, - None => bail!(ErrorKind::Serde(error::serde::ErrorKind::Decoding("request".into()))), + None => { + bail!(ErrorKind::Serde( + error::serde::ErrorKind::Decoding("request".into()), + )) + } }; T::decode(&mut value).map_err(|v| v.into()) } diff --git a/src/rosxmlrpc/serde/decoder.rs b/src/rosxmlrpc/serde/decoder.rs index f8b7a6b..f9616e7 100644 --- a/src/rosxmlrpc/serde/decoder.rs +++ b/src/rosxmlrpc/serde/decoder.rs @@ -21,13 +21,20 @@ impl Decoder { pub fn new_request(body: T) -> Result<(String, Vec)> { value::XmlRpcRequest::new(body) .chain_err(|| ErrorKind::XmlRpcReading("request".into())) - .map(|value| (value.method, value.parameters.into_iter().map(Decoder::new).collect())) + .map(|value| { + ( + value.method, + value.parameters.into_iter().map(Decoder::new).collect(), + ) + }) } pub fn new_response(body: T) -> Result> { value::XmlRpcResponse::new(body) .chain_err(|| ErrorKind::XmlRpcReading("response".into())) - .map(|value| value.parameters.into_iter().map(Decoder::new).collect()) + .map(|value| { + value.parameters.into_iter().map(Decoder::new).collect() + }) } pub fn value(self) -> value::XmlRpcValue { @@ -35,16 +42,21 @@ impl Decoder { } fn read_tuple_helper(&mut self, l: usize, f: F) -> Result - where F: FnOnce(&mut Self) -> Result + where + F: FnOnce(&mut Self) -> Result, { if let Some((value::XmlRpcValue::Array(_), len)) = self.chain.next() { if l == len { f(self) } else { - Err(ErrorKind::MismatchedDataFormat(format!("an array of length {}", len)).into()) + Err( + ErrorKind::MismatchedDataFormat(format!("an array of length {}", len)).into(), + ) } } else { - Err(ErrorKind::MismatchedDataFormat("an array field".into()).into()) + Err( + ErrorKind::MismatchedDataFormat("an array field".into()).into(), + ) } } } @@ -99,7 +111,9 @@ impl rustc_serialize::Decoder for Decoder { if let Some((value::XmlRpcValue::Int(v), _)) = self.chain.next() { Ok(v) } else { - Err(ErrorKind::MismatchedDataFormat("an integer (i32) field".into()).into()) + Err( + ErrorKind::MismatchedDataFormat("an integer (i32) field".into()).into(), + ) } } @@ -115,7 +129,9 @@ impl rustc_serialize::Decoder for Decoder { if let Some((value::XmlRpcValue::Bool(v), _)) = self.chain.next() { Ok(v) } else { - Err(ErrorKind::MismatchedDataFormat("a boolean field".into()).into()) + Err( + ErrorKind::MismatchedDataFormat("a boolean field".into()).into(), + ) } } @@ -123,7 +139,9 @@ impl rustc_serialize::Decoder for Decoder { if let Some((value::XmlRpcValue::Double(v), _)) = self.chain.next() { Ok(v) } else { - Err(ErrorKind::MismatchedDataFormat("a double (f64) field".into()).into()) + Err( + ErrorKind::MismatchedDataFormat("a double (f64) field".into()).into(), + ) } } @@ -139,115 +157,143 @@ impl rustc_serialize::Decoder for Decoder { if let Some((value::XmlRpcValue::String(v), _)) = self.chain.next() { Ok(v) } else { - Err(ErrorKind::MismatchedDataFormat("a string field".into()).into()) + Err( + ErrorKind::MismatchedDataFormat("a string field".into()).into(), + ) } } fn read_enum(&mut self, _: &str, _: F) -> Result - where F: FnOnce(&mut Self) -> Result + where + F: FnOnce(&mut Self) -> Result, { bail!(ErrorKind::UnsupportedDataType("enum".into())) } fn read_enum_variant(&mut self, _: &[&str], _: F) -> Result - where F: FnMut(&mut Self, usize) -> Result + where + F: FnMut(&mut Self, usize) -> Result, { bail!(ErrorKind::UnsupportedDataType("enum variant".into())) } fn read_enum_variant_arg(&mut self, _: usize, _: F) -> Result - where F: FnOnce(&mut Self) -> Result + where + F: FnOnce(&mut Self) -> Result, { - bail!(ErrorKind::UnsupportedDataType("enum variant argument".into())) + bail!(ErrorKind::UnsupportedDataType( + "enum variant argument".into(), + )) } fn read_enum_struct_variant(&mut self, _: &[&str], _: F) -> Result - where F: FnMut(&mut Self, usize) -> Result + where + F: FnMut(&mut Self, usize) -> Result, { bail!(ErrorKind::UnsupportedDataType("enum struct variant".into())) } fn read_enum_struct_variant_field(&mut self, _: &str, _: usize, _: F) -> Result - where F: FnOnce(&mut Self) -> Result + where + F: FnOnce(&mut Self) -> Result, { - bail!(ErrorKind::UnsupportedDataType("enum struct variant field".into())) + bail!(ErrorKind::UnsupportedDataType( + "enum struct variant field".into(), + )) } fn read_struct(&mut self, name: &str, size: usize, f: F) -> Result - where F: FnOnce(&mut Self) -> Result + where + F: FnOnce(&mut Self) -> Result, { - self.read_tuple_helper(size, f) - .chain_err(|| ErrorKind::Decoding(format!("struct {}", name))) + self.read_tuple_helper(size, f).chain_err(|| { + ErrorKind::Decoding(format!("struct {}", name)) + }) } fn read_struct_field(&mut self, name: &str, _: usize, f: F) -> Result - where F: FnOnce(&mut Self) -> Result + where + F: FnOnce(&mut Self) -> Result, { f(self).chain_err(|| ErrorKind::Decoding(format!("field {}", name))) } fn read_tuple(&mut self, size: usize, f: F) -> Result - where F: FnOnce(&mut Self) -> Result + where + F: FnOnce(&mut Self) -> Result, { - self.read_tuple_helper(size, f) - .chain_err(|| ErrorKind::Decoding("tuple".into())) + self.read_tuple_helper(size, f).chain_err(|| { + ErrorKind::Decoding("tuple".into()) + }) } fn read_tuple_arg(&mut self, n: usize, f: F) -> Result - where F: FnOnce(&mut Self) -> Result + where + F: FnOnce(&mut Self) -> Result, { f(self).chain_err(|| ErrorKind::Decoding(format!("field number {}", n))) } fn read_tuple_struct(&mut self, name: &str, size: usize, f: F) -> Result - where F: FnOnce(&mut Self) -> Result + where + F: FnOnce(&mut Self) -> Result, { - self.read_tuple_helper(size, f) - .chain_err(|| ErrorKind::Decoding(format!("tuple struct {}", name))) + self.read_tuple_helper(size, f).chain_err(|| { + ErrorKind::Decoding(format!("tuple struct {}", name)) + }) } fn read_tuple_struct_arg(&mut self, n: usize, f: F) -> Result - where F: FnOnce(&mut Self) -> Result + where + F: FnOnce(&mut Self) -> Result, { f(self).chain_err(|| ErrorKind::Decoding(format!("field number {}", n))) } fn read_option(&mut self, _: F) -> Result - where F: FnMut(&mut Self, bool) -> Result + where + F: FnMut(&mut Self, bool) -> Result, { bail!(ErrorKind::UnsupportedDataType("option".into())) } fn read_seq(&mut self, f: F) -> Result - where F: FnOnce(&mut Self, usize) -> Result + where + F: FnOnce(&mut Self, usize) -> Result, { if let Some((value::XmlRpcValue::Array(_), len)) = self.chain.next() { f(self, len).chain_err(|| ErrorKind::Decoding("array".into())) } else { - Err(ErrorKind::MismatchedDataFormat("an array field".into()).into()) + Err( + ErrorKind::MismatchedDataFormat("an array field".into()).into(), + ) } } fn read_seq_elt(&mut self, n: usize, f: F) -> Result - where F: FnOnce(&mut Self) -> Result + where + F: FnOnce(&mut Self) -> Result, { f(self).chain_err(|| ErrorKind::Decoding(format!("element number {}", n))) } fn read_map(&mut self, _: F) -> Result - where F: FnOnce(&mut Self, usize) -> Result + where + F: FnOnce(&mut Self, usize) -> Result, { bail!(ErrorKind::UnsupportedDataType("map".into())) } fn read_map_elt_key(&mut self, _: usize, _: F) -> Result - where F: FnOnce(&mut Self) -> Result + where + F: FnOnce(&mut Self) -> Result, { bail!(ErrorKind::UnsupportedDataType("map element key".into())) } fn read_map_elt_val(&mut self, _: usize, _: F) -> Result - where F: FnOnce(&mut Self) -> Result + where + F: FnOnce(&mut Self) -> Result, { bail!(ErrorKind::UnsupportedDataType("map element value".into())) } @@ -268,63 +314,73 @@ mod tests { #[test] fn reads_string() { - assert_eq!("First test", String::decode( - &mut Decoder::new(XmlRpcValue::String(String::from("First test")))) - .expect(FAILED_TO_DECODE)); + assert_eq!( + "First test", + String::decode(&mut Decoder::new( + XmlRpcValue::String(String::from("First test")), + )).expect(FAILED_TO_DECODE) + ); } #[test] fn reads_int() { - assert_eq!(41, - i32::decode(&mut Decoder::new(XmlRpcValue::Int(41))).expect(FAILED_TO_DECODE)); + assert_eq!( + 41, + i32::decode(&mut Decoder::new(XmlRpcValue::Int(41))).expect(FAILED_TO_DECODE) + ); } #[test] fn reads_float() { - assert_eq!(32.5, - f64::decode(&mut Decoder::new(XmlRpcValue::Double(32.5))) - .expect(FAILED_TO_DECODE)); + assert_eq!( + 32.5, + f64::decode(&mut Decoder::new(XmlRpcValue::Double(32.5))).expect(FAILED_TO_DECODE) + ); } #[test] fn reads_bool() { - assert_eq!(true, - bool::decode(&mut Decoder::new(XmlRpcValue::Bool(true))) - .expect(FAILED_TO_DECODE)); - assert_eq!(false, - bool::decode(&mut Decoder::new(XmlRpcValue::Bool(false))) - .expect(FAILED_TO_DECODE)); + assert_eq!( + true, + bool::decode(&mut Decoder::new(XmlRpcValue::Bool(true))).expect(FAILED_TO_DECODE) + ); + assert_eq!( + false, + bool::decode(&mut Decoder::new(XmlRpcValue::Bool(false))).expect(FAILED_TO_DECODE) + ); } #[test] fn reads_array() { - assert_eq!(vec![1, 2, 3, 4, 5], - Vec::::decode(&mut Decoder::new(XmlRpcValue::Array(vec![ - XmlRpcValue::Int(1), - XmlRpcValue::Int(2), - XmlRpcValue::Int(3), - XmlRpcValue::Int(4), - XmlRpcValue::Int(5), - ]))) - .expect(FAILED_TO_DECODE)); - } - - #[derive(Debug,PartialEq,RustcDecodable)] + assert_eq!( + vec![1, 2, 3, 4, 5], + Vec::::decode(&mut Decoder::new(XmlRpcValue::Array(vec![ + XmlRpcValue::Int(1), + XmlRpcValue::Int(2), + XmlRpcValue::Int(3), + XmlRpcValue::Int(4), + XmlRpcValue::Int(5), + ]))).expect(FAILED_TO_DECODE) + ); + } + + #[derive(Debug, PartialEq, RustcDecodable)] struct ExampleTuple(i32, f64, String, bool); #[test] fn reads_tuple() { - assert_eq!(ExampleTuple(5, 0.5, String::from("abc"), false), - ExampleTuple::decode(&mut Decoder::new(XmlRpcValue::Array(vec![ - XmlRpcValue::Int(5), - XmlRpcValue::Double(0.5), - XmlRpcValue::String(String::from("abc")), - XmlRpcValue::Bool(false), - ]))) - .expect(FAILED_TO_DECODE)); + assert_eq!( + ExampleTuple(5, 0.5, String::from("abc"), false), + ExampleTuple::decode(&mut Decoder::new(XmlRpcValue::Array(vec![ + XmlRpcValue::Int(5), + XmlRpcValue::Double(0.5), + XmlRpcValue::String(String::from("abc")), + XmlRpcValue::Bool(false), + ]))).expect(FAILED_TO_DECODE) + ); } - #[derive(Debug,PartialEq,RustcDecodable)] + #[derive(Debug, PartialEq, RustcDecodable)] struct ExampleStruct { a: i32, b: ExampleTuple, @@ -332,30 +388,31 @@ mod tests { #[test] fn reads_struct() { - assert_eq!(ExampleStruct { - a: 11, - b: ExampleTuple(5, 0.5, String::from("abc"), false), - }, - ExampleStruct::decode(&mut Decoder::new(XmlRpcValue::Array(vec![ - XmlRpcValue::Int(11), - XmlRpcValue::Array(vec![ - XmlRpcValue::Int(5), - XmlRpcValue::Double(0.5), - XmlRpcValue::String(String::from("abc")), - XmlRpcValue::Bool(false), - ]), - ]))) - .expect(FAILED_TO_DECODE)); - } - - #[derive(Debug,PartialEq,RustcDecodable)] + assert_eq!( + ExampleStruct { + a: 11, + b: ExampleTuple(5, 0.5, String::from("abc"), false), + }, + ExampleStruct::decode(&mut Decoder::new(XmlRpcValue::Array(vec![ + XmlRpcValue::Int(11), + XmlRpcValue::Array(vec![ + XmlRpcValue::Int(5), + XmlRpcValue::Double(0.5), + XmlRpcValue::String(String::from("abc")), + XmlRpcValue::Bool(false), + ]), + ]))).expect(FAILED_TO_DECODE) + ); + } + + #[derive(Debug, PartialEq, RustcDecodable)] struct ExampleRequestStruct { a: i32, b: bool, c: ExampleRequestStructChild, } - #[derive(Debug,PartialEq,RustcDecodable)] + #[derive(Debug, PartialEq, RustcDecodable)] struct ExampleRequestStructChild { a: String, b: f64, @@ -387,15 +444,17 @@ mod tests { assert_eq!("mytype.mymethod", method); assert_eq!(2, parameters.len()); assert_eq!(33, i32::decode(&mut parameters[0]).expect(FAILED_TO_DECODE)); - assert_eq!(ExampleRequestStruct { - a: 41, - b: true, - c: ExampleRequestStructChild { - a: String::from("Hello"), - b: 0.5, - }, - }, - ExampleRequestStruct::decode(&mut parameters[1]).expect(FAILED_TO_DECODE)); + assert_eq!( + ExampleRequestStruct { + a: 41, + b: true, + c: ExampleRequestStructChild { + a: String::from("Hello"), + b: 0.5, + }, + }, + ExampleRequestStruct::decode(&mut parameters[1]).expect(FAILED_TO_DECODE) + ); } #[test] @@ -422,15 +481,17 @@ mod tests { let mut parameters = Decoder::new_response(&mut cursor).expect(FAILED_TO_DECODE); assert_eq!(2, parameters.len()); assert_eq!(33, i32::decode(&mut parameters[0]).expect(FAILED_TO_DECODE)); - assert_eq!(ExampleRequestStruct { - a: 41, - b: true, - c: ExampleRequestStructChild { - a: String::from("Hello"), - b: 0.5, - }, - }, - ExampleRequestStruct::decode(&mut parameters[1]).expect(FAILED_TO_DECODE)); + assert_eq!( + ExampleRequestStruct { + a: 41, + b: true, + c: ExampleRequestStructChild { + a: String::from("Hello"), + b: 0.5, + }, + }, + ExampleRequestStruct::decode(&mut parameters[1]).expect(FAILED_TO_DECODE) + ); } #[test] @@ -458,14 +519,20 @@ mod tests { let (method, mut parameters) = Decoder::new_request(&mut cursor).expect(FAILED_TO_DECODE); assert_eq!("mytype.mymethod", method); assert_eq!(2, parameters.len()); - assert_eq!(XmlRpcValue::Array(vec![XmlRpcValue::Int(41), - XmlRpcValue::Bool(true), - XmlRpcValue::Array(vec![ - XmlRpcValue::String(String::from("Hello")), - XmlRpcValue::Double(0.5), - ])]), - parameters.pop().expect(FAILED_TO_DECODE).value()); - assert_eq!(XmlRpcValue::Int(33), - parameters.pop().expect(FAILED_TO_DECODE).value()); + assert_eq!( + XmlRpcValue::Array(vec![ + XmlRpcValue::Int(41), + XmlRpcValue::Bool(true), + XmlRpcValue::Array(vec![ + XmlRpcValue::String(String::from("Hello")), + XmlRpcValue::Double(0.5), + ]), + ]), + parameters.pop().expect(FAILED_TO_DECODE).value() + ); + assert_eq!( + XmlRpcValue::Int(33), + parameters.pop().expect(FAILED_TO_DECODE).value() + ); } } diff --git a/src/rosxmlrpc/serde/encoder.rs b/src/rosxmlrpc/serde/encoder.rs index 5430e5a..820253f 100644 --- a/src/rosxmlrpc/serde/encoder.rs +++ b/src/rosxmlrpc/serde/encoder.rs @@ -21,42 +21,48 @@ impl Encoder { retval } - fn form_subtree(mut data: &mut std::vec::IntoIter<(XmlRpcValue, usize)>) - -> Option { + fn form_subtree( + mut data: &mut std::vec::IntoIter<(XmlRpcValue, usize)>, + ) -> Option { match data.next() { Some((node, count)) => { Some(match node { - XmlRpcValue::Array(_) => { - XmlRpcValue::Array((0..count) - .into_iter() - .filter_map(|_| { - Encoder::form_subtree(&mut data) - }) - .collect()) - } - _ => node, - }) + XmlRpcValue::Array(_) => { + XmlRpcValue::Array( + (0..count) + .into_iter() + .filter_map(|_| Encoder::form_subtree(&mut data)) + .collect(), + ) + } + _ => node, + }) } None => None, } } - pub fn write_request(self, - method: &str, - mut body: &mut T) - -> std::io::Result<()> { - write!(&mut body, - "{}", - value::XmlRpcRequest { - method: String::from(method), - parameters: self.form_tree(), - }) + pub fn write_request( + self, + method: &str, + mut body: &mut T, + ) -> std::io::Result<()> { + write!( + &mut body, + "{}", + value::XmlRpcRequest { + method: String::from(method), + parameters: self.form_tree(), + } + ) } pub fn write_response(self, mut body: &mut T) -> std::io::Result<()> { - write!(&mut body, - "{}", - value::XmlRpcResponse { parameters: self.form_tree() }) + write!( + &mut body, + "{}", + value::XmlRpcResponse { parameters: self.form_tree() } + ) } } @@ -130,76 +136,100 @@ impl rustc_serialize::Encoder for Encoder { } fn emit_enum(&mut self, _: &str, _: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { bail!(ErrorKind::UnsupportedDataType("enum".into())) } fn emit_enum_variant(&mut self, _: &str, _: usize, _: usize, _: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { bail!(ErrorKind::UnsupportedDataType("enum variant".into())) } fn emit_enum_variant_arg(&mut self, _: usize, _: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { - bail!(ErrorKind::UnsupportedDataType("enum variant argument".into())) + bail!(ErrorKind::UnsupportedDataType( + "enum variant argument".into(), + )) } fn emit_enum_struct_variant(&mut self, _: &str, _: usize, _: usize, _: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { bail!(ErrorKind::UnsupportedDataType("enum struct variant".into())) } fn emit_enum_struct_variant_field(&mut self, _: &str, _: usize, _: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { - bail!(ErrorKind::UnsupportedDataType("enum struct variant field".into())) + bail!(ErrorKind::UnsupportedDataType( + "enum struct variant field".into(), + )) } fn emit_struct(&mut self, name: &str, l: usize, f: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { self.data.push((XmlRpcValue::Array(vec![]), l)); - f(self).chain_err(|| ErrorKind::UnsupportedDataType(format!("struct {}", name))) + f(self).chain_err(|| { + ErrorKind::UnsupportedDataType(format!("struct {}", name)) + }) } fn emit_struct_field(&mut self, name: &str, _: usize, f: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { f(self).chain_err(|| ErrorKind::UnsupportedDataType(format!("field {}", name))) } fn emit_tuple(&mut self, l: usize, f: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { self.data.push((XmlRpcValue::Array(vec![]), l)); f(self).chain_err(|| ErrorKind::UnsupportedDataType("tuple".into())) } fn emit_tuple_arg(&mut self, n: usize, f: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { - f(self).chain_err(|| ErrorKind::UnsupportedDataType(format!("field number {}", n))) + f(self).chain_err(|| { + ErrorKind::UnsupportedDataType(format!("field number {}", n)) + }) } fn emit_tuple_struct(&mut self, name: &str, l: usize, f: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { self.data.push((XmlRpcValue::Array(vec![]), l)); - f(self).chain_err(|| ErrorKind::UnsupportedDataType(format!("tuple struct {}", name))) + f(self).chain_err(|| { + ErrorKind::UnsupportedDataType(format!("tuple struct {}", name)) + }) } fn emit_tuple_struct_arg(&mut self, n: usize, f: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { - f(self).chain_err(|| ErrorKind::UnsupportedDataType(format!("field number {}", n))) + f(self).chain_err(|| { + ErrorKind::UnsupportedDataType(format!("field number {}", n)) + }) } fn emit_option(&mut self, _: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { bail!(ErrorKind::UnsupportedDataType("option".into())) } @@ -209,38 +239,46 @@ impl rustc_serialize::Encoder for Encoder { } fn emit_option_some(&mut self, _: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { bail!(ErrorKind::UnsupportedDataType("some".into())) } fn emit_seq(&mut self, l: usize, f: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { self.data.push((XmlRpcValue::Array(vec![]), l)); f(self).chain_err(|| ErrorKind::UnsupportedDataType("array".into())) } fn emit_seq_elt(&mut self, n: usize, f: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { - f(self).chain_err(|| ErrorKind::UnsupportedDataType(format!("element number {}", n))) + f(self).chain_err(|| { + ErrorKind::UnsupportedDataType(format!("element number {}", n)) + }) } fn emit_map(&mut self, _: usize, _: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { bail!(ErrorKind::UnsupportedDataType("map".into())) } fn emit_map_elt_key(&mut self, _: usize, _: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { bail!(ErrorKind::UnsupportedDataType("map element key".into())) } fn emit_map_elt_val(&mut self, _: usize, _: F) -> EncoderResult - where F: FnOnce(&mut Self) -> EncoderResult + where + F: FnOnce(&mut Self) -> EncoderResult, { bail!(ErrorKind::UnsupportedDataType("map element value".into())) } @@ -258,74 +296,86 @@ mod tests { fn writes_response() { let mut data = vec![]; let mut encoder = Encoder::new(); - String::from("Hello") - .encode(&mut encoder) - .expect(FAILED_TO_ENCODE); + String::from("Hello").encode(&mut encoder).expect( + FAILED_TO_ENCODE, + ); encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!(concat!(r#""#, - r#""#, - r#""#, - r#""#, - r#"Hello"#, - r#""#, - r#""#, - r#""#), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE)); + assert_eq!( + concat!( + r#""#, + r#""#, + r#""#, + r#""#, + r#"Hello"#, + r#""#, + r#""#, + r#""# + ), + std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) + ); } #[test] fn writes_request() { let mut data = vec![]; let mut encoder = Encoder::new(); - String::from("Hello") - .encode(&mut encoder) - .expect(FAILED_TO_ENCODE); - encoder - .write_request("something", &mut data) - .expect(FAILED_TO_ENCODE); - assert_eq!(concat!(r#""#, - r#""#, - r#""#, - r#"something"#, - r#""#, - r#""#, - r#""#, - r#"Hello"#, - r#""#, - r#""#, - r#""#), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE)); + String::from("Hello").encode(&mut encoder).expect( + FAILED_TO_ENCODE, + ); + encoder.write_request("something", &mut data).expect( + FAILED_TO_ENCODE, + ); + assert_eq!( + concat!( + r#""#, + r#""#, + r#""#, + r#"something"#, + r#""#, + r#""#, + r#""#, + r#"Hello"#, + r#""#, + r#""#, + r#""# + ), + std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) + ); } #[test] fn writes_string() { let mut data = vec![]; let mut encoder = Encoder::new(); - String::from("Hello") - .encode(&mut encoder) - .expect(FAILED_TO_ENCODE); - String::from("There") - .encode(&mut encoder) - .expect(FAILED_TO_ENCODE); - String::from("Friend") - .encode(&mut encoder) - .expect(FAILED_TO_ENCODE); + String::from("Hello").encode(&mut encoder).expect( + FAILED_TO_ENCODE, + ); + String::from("There").encode(&mut encoder).expect( + FAILED_TO_ENCODE, + ); + String::from("Friend").encode(&mut encoder).expect( + FAILED_TO_ENCODE, + ); encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!(concat!(r#""#, - r#""#, - r#""#, - r#""#, - r#"Hello"#, - r#""#, - r#""#, - r#"There"#, - r#""#, - r#""#, - r#"Friend"#, - r#""#, - r#""#, - r#""#), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE)); + assert_eq!( + concat!( + r#""#, + r#""#, + r#""#, + r#""#, + r#"Hello"#, + r#""#, + r#""#, + r#"There"#, + r#""#, + r#""#, + r#"Friend"#, + r#""#, + r#""#, + r#""# + ), + std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) + ); } #[test] @@ -336,21 +386,25 @@ mod tests { 27i32.encode(&mut encoder).expect(FAILED_TO_ENCODE); 12i32.encode(&mut encoder).expect(FAILED_TO_ENCODE); encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!(concat!(r#""#, - r#""#, - r#""#, - r#""#, - r#"43"#, - r#""#, - r#""#, - r#"27"#, - r#""#, - r#""#, - r#"12"#, - r#""#, - r#""#, - r#""#), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE)); + assert_eq!( + concat!( + r#""#, + r#""#, + r#""#, + r#""#, + r#"43"#, + r#""#, + r#""#, + r#"27"#, + r#""#, + r#""#, + r#"12"#, + r#""#, + r#""#, + r#""# + ), + std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) + ); } #[test] @@ -361,21 +415,25 @@ mod tests { 11.25f64.encode(&mut encoder).expect(FAILED_TO_ENCODE); 77.125f64.encode(&mut encoder).expect(FAILED_TO_ENCODE); encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!(concat!(r#""#, - r#""#, - r#""#, - r#""#, - r#"33.5"#, - r#""#, - r#""#, - r#"11.25"#, - r#""#, - r#""#, - r#"77.125"#, - r#""#, - r#""#, - r#""#), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE)); + assert_eq!( + concat!( + r#""#, + r#""#, + r#""#, + r#""#, + r#"33.5"#, + r#""#, + r#""#, + r#"11.25"#, + r#""#, + r#""#, + r#"77.125"#, + r#""#, + r#""#, + r#""# + ), + std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) + ); } #[test] @@ -385,46 +443,54 @@ mod tests { true.encode(&mut encoder).expect(FAILED_TO_ENCODE); false.encode(&mut encoder).expect(FAILED_TO_ENCODE); encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!(concat!(r#""#, - r#""#, - r#""#, - r#""#, - r#"1"#, - r#""#, - r#""#, - r#"0"#, - r#""#, - r#""#, - r#""#), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE)); + assert_eq!( + concat!( + r#""#, + r#""#, + r#""#, + r#""#, + r#"1"#, + r#""#, + r#""#, + r#"0"#, + r#""#, + r#""#, + r#""# + ), + std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) + ); } #[test] fn writes_array() { let mut data = vec![]; let mut encoder = Encoder::new(); - vec![1i32, 2, 3, 4, 5] - .encode(&mut encoder) - .expect(FAILED_TO_ENCODE); + vec![1i32, 2, 3, 4, 5].encode(&mut encoder).expect( + FAILED_TO_ENCODE, + ); encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!(concat!(r#""#, - r#""#, - r#""#, - r#""#, - r#""#, - r#"1"#, - r#"2"#, - r#"3"#, - r#"4"#, - r#"5"#, - r#""#, - r#""#, - r#""#, - r#""#), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE)); - } - - #[derive(Debug,PartialEq,RustcEncodable)] + assert_eq!( + concat!( + r#""#, + r#""#, + r#""#, + r#""#, + r#""#, + r#"1"#, + r#"2"#, + r#"3"#, + r#"4"#, + r#"5"#, + r#""#, + r#""#, + r#""#, + r#""# + ), + std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) + ); + } + + #[derive(Debug, PartialEq, RustcEncodable)] struct ExampleTuple(i32, f64, String, bool); #[test] @@ -435,30 +501,34 @@ mod tests { .encode(&mut encoder) .expect(FAILED_TO_ENCODE); encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!(concat!(r#""#, - r#""#, - r#""#, - r#""#, - r#""#, - r#"5"#, - r#"0.5"#, - r#"abc"#, - r#"0"#, - r#""#, - r#""#, - r#""#, - r#""#), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE)); - } - - #[derive(Debug,PartialEq,RustcEncodable)] + assert_eq!( + concat!( + r#""#, + r#""#, + r#""#, + r#""#, + r#""#, + r#"5"#, + r#"0.5"#, + r#"abc"#, + r#"0"#, + r#""#, + r#""#, + r#""#, + r#""# + ), + std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) + ); + } + + #[derive(Debug, PartialEq, RustcEncodable)] struct ExampleRequestStruct { a: i32, b: bool, c: ExampleRequestStructChild, } - #[derive(Debug,PartialEq,RustcEncodable)] + #[derive(Debug, PartialEq, RustcEncodable)] struct ExampleRequestStructChild { a: String, b: f64, @@ -469,32 +539,35 @@ mod tests { let mut data = vec![]; let mut encoder = Encoder::new(); ExampleRequestStruct { - a: 41, - b: true, - c: ExampleRequestStructChild { - a: String::from("Hello"), - b: 0.5, - }, - } - .encode(&mut encoder) + a: 41, + b: true, + c: ExampleRequestStructChild { + a: String::from("Hello"), + b: 0.5, + }, + }.encode(&mut encoder) .expect(FAILED_TO_ENCODE); encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!(concat!(r#""#, - r#""#, - r#""#, - r#""#, - r#""#, - r#"41"#, - r#"1"#, - r#""#, - r#"Hello"#, - r#"0.5"#, - r#""#, - r#""#, - r#""#, - r#""#, - r#""#), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE)); + assert_eq!( + concat!( + r#""#, + r#""#, + r#""#, + r#""#, + r#""#, + r#"41"#, + r#"1"#, + r#""#, + r#"Hello"#, + r#"0.5"#, + r#""#, + r#""#, + r#""#, + r#""#, + r#""# + ), + std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) + ); } #[test] @@ -505,49 +578,52 @@ mod tests { .encode(&mut encoder) .expect(FAILED_TO_ENCODE); 27i32.encode(&mut encoder).expect(FAILED_TO_ENCODE); - String::from("Hello") - .encode(&mut encoder) - .expect(FAILED_TO_ENCODE); + String::from("Hello").encode(&mut encoder).expect( + FAILED_TO_ENCODE, + ); ExampleRequestStruct { - a: 41, - b: true, - c: ExampleRequestStructChild { - a: String::from("Hello"), - b: 0.5, - }, - } - .encode(&mut encoder) + a: 41, + b: true, + c: ExampleRequestStructChild { + a: String::from("Hello"), + b: 0.5, + }, + }.encode(&mut encoder) .expect(FAILED_TO_ENCODE); encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!(concat!(r#""#, - r#""#, - r#""#, - r#""#, - r#""#, - r#"5"#, - r#"0.5"#, - r#"abc"#, - r#"0"#, - r#""#, - r#""#, - r#""#, - r#"27"#, - r#""#, - r#""#, - r#"Hello"#, - r#""#, - r#""#, - r#""#, - r#"41"#, - r#"1"#, - r#""#, - r#"Hello"#, - r#"0.5"#, - r#""#, - r#""#, - r#""#, - r#""#, - r#""#), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE)); + assert_eq!( + concat!( + r#""#, + r#""#, + r#""#, + r#""#, + r#""#, + r#"5"#, + r#"0.5"#, + r#"abc"#, + r#"0"#, + r#""#, + r#""#, + r#""#, + r#"27"#, + r#""#, + r#""#, + r#"Hello"#, + r#""#, + r#""#, + r#""#, + r#"41"#, + r#"1"#, + r#""#, + r#"Hello"#, + r#"0.5"#, + r#""#, + r#""#, + r#""#, + r#""#, + r#""# + ), + std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) + ); } } diff --git a/src/rosxmlrpc/serde/value.rs b/src/rosxmlrpc/serde/value.rs index 51886ec..c16dc15 100644 --- a/src/rosxmlrpc/serde/value.rs +++ b/src/rosxmlrpc/serde/value.rs @@ -2,7 +2,7 @@ use std; use xml; use self::error::{ErrorKind, Result, ResultExt}; -#[derive(Clone,Debug,PartialEq)] +#[derive(Clone, Debug, PartialEq)] pub enum XmlRpcValue { Int(i32), Bool(bool), @@ -28,9 +28,11 @@ impl std::fmt::Display for XmlRpcValue { match *self { XmlRpcValue::Int(v) => write!(f, "{}", v), XmlRpcValue::Bool(v) => { - write!(f, - "{}", - if v { 1 } else { 0 }) + write!( + f, + "{}", + if v { 1 } else { 0 } + ) } XmlRpcValue::String(ref v) => write!(f, "{}", v), XmlRpcValue::Double(v) => write!(f, "{}", v), @@ -56,9 +58,11 @@ impl std::fmt::Display for XmlRpcValue { impl std::fmt::Display for XmlRpcRequest { fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result { - write!(f, - "{}", - self.method)?; + write!( + f, + "{}", + self.method + )?; for parameter in &self.parameters { write!(f, "{}", parameter)?; } @@ -78,8 +82,9 @@ impl std::fmt::Display for XmlRpcResponse { impl XmlRpcRequest { pub fn new(body: T) -> Result { - let tree = Tree::new(body) - .chain_err(|| "Failed to transform XML data into tree")?; + let tree = Tree::new(body).chain_err( + || "Failed to transform XML data into tree", + )?; let (key, mut children) = match tree { Tree::Node(key, children) => (key, children), Tree::Leaf(_) => { @@ -92,14 +97,14 @@ impl XmlRpcRequest { // We checked the array length, so it's safe to pop let parameters_tree = children.pop().expect(UNEXPECTED_EMPTY_ARRAY); let method = children.pop().expect(UNEXPECTED_EMPTY_ARRAY); - match method - .peel_layer("methodName") - .chain_err(|| "Bad XML-RPC Request method name value")? { + match method.peel_layer("methodName").chain_err( + || "Bad XML-RPC Request method name value", + )? { Tree::Leaf(method_name) => { Ok(XmlRpcRequest { - method: method_name, - parameters: extract_parameters(parameters_tree)?, - }) + method: method_name, + parameters: extract_parameters(parameters_tree)?, + }) } Tree::Node(_, _) => { bail!("Node 'methodName' contain a string representing the method name"); @@ -111,9 +116,9 @@ impl XmlRpcRequest { impl XmlRpcResponse { pub fn new(body: T) -> Result { extract_parameters(Tree::new(body) - .chain_err(|| "Failed to transform XML data into tree")? - .peel_layer("methodResponse")?) - .map(|parameters| XmlRpcResponse { parameters: parameters }) + .chain_err(|| "Failed to transform XML data into tree")? + .peel_layer("methodResponse")?) + .map(|parameters| XmlRpcResponse { parameters: parameters }) } } @@ -121,10 +126,10 @@ fn extract_parameters(parameters: Tree) -> Result> { if let Tree::Node(key, parameters) = parameters { if key == "params" { return parameters - .into_iter() - .map(XmlRpcValue::from_parameter) - .collect::>() - .chain_err(|| "Failed to parse parameters"); + .into_iter() + .map(XmlRpcValue::from_parameter) + .collect::>() + .chain_err(|| "Failed to parse parameters"); } } bail!("Parameters need to be contained within a node called params") @@ -132,16 +137,19 @@ fn extract_parameters(parameters: Tree) -> Result> { impl XmlRpcValue { pub fn new(body: T) -> Result { - XmlRpcValue::from_tree(Tree::new(body) - .chain_err(|| "Couldn't generate XML tree to form value")?) + XmlRpcValue::from_tree(Tree::new(body).chain_err( + || "Couldn't generate XML tree to form value", + )?) } fn read_member(tree: Tree) -> Result<(String, XmlRpcValue)> { let (key, mut children) = match tree { Tree::Node(key, children) => (key, children), Tree::Leaf(_) => { - bail!("Structure member node should contain a node called 'member' with two \ - children") + bail!( + "Structure member node should contain a node called 'member' with two \ + children" + ) } }; if key != "member" || children.len() != 2 { @@ -150,25 +158,26 @@ impl XmlRpcValue { // We tested the vector length already, so it's safe to pop let value = children.pop().expect(UNEXPECTED_EMPTY_ARRAY); let name_node = children.pop().expect(UNEXPECTED_EMPTY_ARRAY); - let name = - match name_node - .peel_layer("name") - .chain_err(|| "First struct member field should be a node called 'name'",)? { - Tree::Leaf(name) => name, - Tree::Node(_, _) => { - bail!("Struct member's name node should just contain the member's name"); - } - }; + let name = match name_node.peel_layer("name").chain_err( + || "First struct member field should be a node called 'name'", + )? { + Tree::Leaf(name) => name, + Tree::Node(_, _) => { + bail!("Struct member's name node should just contain the member's name"); + } + }; XmlRpcValue::from_tree(value) - .chain_err(|| format!("Failed to parse subtree of struct field {}", name)) + .chain_err(|| { + format!("Failed to parse subtree of struct field {}", name) + }) .map(|v| (name, v)) } fn from_parameter(tree: Tree) -> Result { - XmlRpcValue::from_tree(tree.peel_layer("param") - .chain_err(|| "Parameters should be contained within node 'param'")?) - .chain_err(|| "Failed to parse XML RPC parameter") + XmlRpcValue::from_tree(tree.peel_layer("param").chain_err( + || "Parameters should be contained within node 'param'", + )?).chain_err(|| "Failed to parse XML RPC parameter") } fn from_tree(tree: Tree) -> Result { @@ -177,7 +186,8 @@ impl XmlRpcValue { Tree::Leaf(_) => bail!("Value node should contain one node representing its data type"), }; if key == "struct" { - return Ok(XmlRpcValue::Struct(values.into_iter() + return Ok(XmlRpcValue::Struct(values + .into_iter() .map(XmlRpcValue::read_member) .collect::>>() .chain_err(|| "Couldn't parse struct")?)); @@ -188,17 +198,19 @@ impl XmlRpcValue { if key == "array" { return if let Some(Tree::Node(key, children)) = values.pop() { if key != "data" { - bail!("Node 'array' must contain 'data' node, but '{}' detected", - key); + bail!( + "Node 'array' must contain 'data' node, but '{}' detected", + key + ); } Ok(XmlRpcValue::Array(children - .into_iter() - .map(XmlRpcValue::from_tree) - .collect::>() - .chain_err(|| "Failed to parse array's children")?)) + .into_iter() + .map(XmlRpcValue::from_tree) + .collect::>() + .chain_err(|| "Failed to parse array's children")?)) } else { - bail!("Node 'array' must contain 'data' node with child values"); - }; + bail!("Node 'array' must contain 'data' node with child values"); + }; } let value = match values.pop().unwrap_or(Tree::Leaf(String::from(""))) { Tree::Leaf(value) => value, @@ -206,28 +218,22 @@ impl XmlRpcValue { }; match key.as_str() { "i4" | "int" => { - Ok(XmlRpcValue::Int(value - .parse() - .chain_err(|| { - format!("Failed to parse integer (i32) {}", - value) - })?)) + Ok(XmlRpcValue::Int(value.parse().chain_err(|| { + format!("Failed to parse integer (i32) {}", value) + })?)) } "boolean" => { - Ok(XmlRpcValue::Bool(value - .parse::() - .chain_err(|| { - format!("Expected 0 or 1 for boolean, got {}", value) - })? != 0)) + Ok(XmlRpcValue::Bool( + value.parse::().chain_err(|| { + format!("Expected 0 or 1 for boolean, got {}", value) + })? != 0, + )) } "string" => Ok(XmlRpcValue::String(value)), "double" => { - Ok(XmlRpcValue::Double(value - .parse() - .chain_err(|| { - format!("Failed to parse double (f64) {}", - value) - })?)) + Ok(XmlRpcValue::Double(value.parse().chain_err(|| { + format!("Failed to parse double (f64) {}", value) + })?)) } _ => bail!("Unsupported data type '{}'", key), } @@ -241,8 +247,10 @@ enum Tree { impl Tree { fn new(body: T) -> Result { - parse_tree(&mut xml::EventReader::new(body))? - .ok_or("XML data started with a closing tag".into()) + parse_tree(&mut xml::EventReader::new(body))?.ok_or( + "XML data started with a closing tag" + .into(), + ) } fn peel_layer(self, name: &str) -> Result { @@ -262,14 +270,15 @@ enum Node { } fn parse_tree(reader: &mut xml::EventReader) -> Result> { - match next_node(reader) - .chain_err(|| "Unexpected end of XML data")? { + match next_node(reader).chain_err(|| "Unexpected end of XML data")? { Node::Close(..) => Ok(None), Node::Data(value) => Ok(Some(Tree::Leaf(value))), Node::Open(name) => { let mut children = Vec::::new(); - while let Some(node) = parse_tree(reader) - .chain_err(|| ErrorKind::TreeParsing(name.clone()))? { + while let Some(node) = parse_tree(reader).chain_err( + || ErrorKind::TreeParsing(name.clone()), + )? + { children.push(node); } Ok(Some(Tree::Node(name, children))) @@ -367,13 +376,17 @@ mod tests { "#; let mut cursor = std::io::Cursor::new(data.as_bytes()); let value = XmlRpcValue::new(&mut cursor).expect(BAD_DATA); - assert_eq!(XmlRpcValue::Array(vec![XmlRpcValue::Int(41), - XmlRpcValue::Bool(true), - XmlRpcValue::Array(vec![ - XmlRpcValue::String(String::from("Hello")), - XmlRpcValue::Double(0.5), - ])]), - value); + assert_eq!( + XmlRpcValue::Array(vec![ + XmlRpcValue::Int(41), + XmlRpcValue::Bool(true), + XmlRpcValue::Array(vec![ + XmlRpcValue::String(String::from("Hello")), + XmlRpcValue::Double(0.5), + ]), + ]), + value + ); } #[test] @@ -404,14 +417,23 @@ mod tests { "#; let mut cursor = std::io::Cursor::new(data.as_bytes()); let value = XmlRpcValue::new(&mut cursor).expect(BAD_DATA); - assert_eq!(XmlRpcValue::Struct(vec![(String::from("a"), XmlRpcValue::Int(41)), - (String::from("b"), XmlRpcValue::Bool(true)), - (String::from("c"), - XmlRpcValue::Struct(vec![ - (String::from("xxx"), XmlRpcValue::String(String::from("Hello"))), - (String::from("yyy"), XmlRpcValue::Double(0.5)), - ]))]), - value); + assert_eq!( + XmlRpcValue::Struct(vec![ + (String::from("a"), XmlRpcValue::Int(41)), + (String::from("b"), XmlRpcValue::Bool(true)), + ( + String::from("c"), + XmlRpcValue::Struct(vec![ + ( + String::from("xxx"), + XmlRpcValue::String(String::from("Hello")) + ), + (String::from("yyy"), XmlRpcValue::Double(0.5)), + ]) + ), + ]), + value + ); } #[test] @@ -438,14 +460,20 @@ mod tests { let mut cursor = std::io::Cursor::new(data.as_bytes()); let value = XmlRpcRequest::new(&mut cursor).expect(BAD_DATA); assert_eq!("mytype.mymethod", value.method); - assert_eq!(vec![XmlRpcValue::Int(33), - XmlRpcValue::Array(vec![XmlRpcValue::Int(41), - XmlRpcValue::Bool(true), - XmlRpcValue::Array(vec![ - XmlRpcValue::String(String::from("Hello")), - XmlRpcValue::Double(0.5), - ])])], - value.parameters); + assert_eq!( + vec![ + XmlRpcValue::Int(33), + XmlRpcValue::Array(vec![ + XmlRpcValue::Int(41), + XmlRpcValue::Bool(true), + XmlRpcValue::Array(vec![ + XmlRpcValue::String(String::from("Hello")), + XmlRpcValue::Double(0.5), + ]), + ]), + ], + value.parameters + ); } #[test] @@ -470,164 +498,220 @@ mod tests { "#; let mut cursor = std::io::Cursor::new(data.as_bytes()); let value = XmlRpcResponse::new(&mut cursor).expect(BAD_DATA); - assert_eq!(vec![XmlRpcValue::Int(33), - XmlRpcValue::Array(vec![XmlRpcValue::Int(41), - XmlRpcValue::Bool(true), - XmlRpcValue::Array(vec![ - XmlRpcValue::String(String::from("Hello")), - XmlRpcValue::Double(0.5), - ])])], - value.parameters); + assert_eq!( + vec![ + XmlRpcValue::Int(33), + XmlRpcValue::Array(vec![ + XmlRpcValue::Int(41), + XmlRpcValue::Bool(true), + XmlRpcValue::Array(vec![ + XmlRpcValue::String(String::from("Hello")), + XmlRpcValue::Double(0.5), + ]), + ]), + ], + value.parameters + ); } #[test] fn writes_string() { - assert_eq!(r#"First test"#, - format!("{}", XmlRpcValue::String(String::from("First test")))); - assert_eq!(r#""#, - format!("{}", XmlRpcValue::String(String::from("")))); + assert_eq!( + r#"First test"#, + format!("{}", XmlRpcValue::String(String::from("First test"))) + ); + assert_eq!( + r#""#, + format!("{}", XmlRpcValue::String(String::from(""))) + ); } #[test] fn writes_int() { - assert_eq!(r#"41"#, - format!("{}", XmlRpcValue::Int(41))); + assert_eq!( + r#"41"#, + format!("{}", XmlRpcValue::Int(41)) + ); } #[test] fn writes_float() { - assert_eq!(r#"33.25"#, - format!("{}", XmlRpcValue::Double(33.25))); + assert_eq!( + r#"33.25"#, + format!("{}", XmlRpcValue::Double(33.25)) + ); } #[test] fn writes_bool() { - assert_eq!(r#"1"#, - format!("{}", XmlRpcValue::Bool(true))); - assert_eq!(r#"0"#, - format!("{}", XmlRpcValue::Bool(false))); + assert_eq!( + r#"1"#, + format!("{}", XmlRpcValue::Bool(true)) + ); + assert_eq!( + r#"0"#, + format!("{}", XmlRpcValue::Bool(false)) + ); } #[test] fn writes_array() { - assert_eq!(concat!(r#""#, - r#"41"#, - r#"1"#, - r#""#, - r#"Hello"#, - r#"0.5"#, - r#""#, - r#""#), - format!("{}", - XmlRpcValue::Array(vec![XmlRpcValue::Int(41), - XmlRpcValue::Bool(true), - XmlRpcValue::Array(vec![ - XmlRpcValue::String(String::from("Hello")), - XmlRpcValue::Double(0.5), - ])]))); + assert_eq!( + concat!( + r#""#, + r#"41"#, + r#"1"#, + r#""#, + r#"Hello"#, + r#"0.5"#, + r#""#, + r#""# + ), + format!( + "{}", + XmlRpcValue::Array(vec![ + XmlRpcValue::Int(41), + XmlRpcValue::Bool(true), + XmlRpcValue::Array(vec![ + XmlRpcValue::String(String::from("Hello")), + XmlRpcValue::Double(0.5), + ]), + ]) + ) + ); } #[test] fn writes_struct() { - assert_eq!(concat!(r#""#, - r#""#, - r#"a"#, - r#"41"#, - r#""#, - r#""#, - r#"b"#, - r#"1"#, - r#""#, - r#""#, - r#"c"#, - r#""#, - r#""#, - r#"xxx"#, - r#"Hello"#, - r#""#, - r#""#, - r#"yyy"#, - r#"0.5"#, - r#""#, - r#""#, - r#""#, - r#""#), - format!("{}", - XmlRpcValue::Struct(vec![(String::from("a"), XmlRpcValue::Int(41)), - (String::from("b"), - XmlRpcValue::Bool(true)), - (String::from("c"), - XmlRpcValue::Struct(vec![ - (String::from("xxx"), XmlRpcValue::String(String::from("Hello"))), - (String::from("yyy"), XmlRpcValue::Double(0.5)), - ]))]))); + assert_eq!( + concat!( + r#""#, + r#""#, + r#"a"#, + r#"41"#, + r#""#, + r#""#, + r#"b"#, + r#"1"#, + r#""#, + r#""#, + r#"c"#, + r#""#, + r#""#, + r#"xxx"#, + r#"Hello"#, + r#""#, + r#""#, + r#"yyy"#, + r#"0.5"#, + r#""#, + r#""#, + r#""#, + r#""# + ), + format!( + "{}", + XmlRpcValue::Struct(vec![ + (String::from("a"), XmlRpcValue::Int(41)), + (String::from("b"), XmlRpcValue::Bool(true)), + ( + String::from("c"), + XmlRpcValue::Struct(vec![ + ( + String::from("xxx"), + XmlRpcValue::String(String::from("Hello")) + ), + (String::from("yyy"), XmlRpcValue::Double(0.5)), + ]) + ), + ]) + ) + ); } #[test] fn writes_request() { - assert_eq!(concat!(r#""#, - r#""#, - r#"mytype.mymethod"#, - r#""#, - r#""#, - r#"33"#, - r#""#, - r#""#, - r#""#, - r#"41"#, - r#"1"#, - r#""#, - r#"Hello"#, - r#"0.5"#, - r#""#, - r#""#, - r#""#, - r#""#, - r#""#), - format!("{}", - XmlRpcRequest { - method: String::from("mytype.mymethod"), - parameters: vec![XmlRpcValue::Int(33), - XmlRpcValue::Array(vec![XmlRpcValue::Int(41), - XmlRpcValue::Bool(true), - XmlRpcValue::Array(vec![ - XmlRpcValue::String( - String::from("Hello")), - XmlRpcValue::Double(0.5), - ])])], - })); + assert_eq!( + concat!( + r#""#, + r#""#, + r#"mytype.mymethod"#, + r#""#, + r#""#, + r#"33"#, + r#""#, + r#""#, + r#""#, + r#"41"#, + r#"1"#, + r#""#, + r#"Hello"#, + r#"0.5"#, + r#""#, + r#""#, + r#""#, + r#""#, + r#""# + ), + format!( + "{}", + XmlRpcRequest { + method: String::from("mytype.mymethod"), + parameters: vec![ + XmlRpcValue::Int(33), + XmlRpcValue::Array(vec![ + XmlRpcValue::Int(41), + XmlRpcValue::Bool(true), + XmlRpcValue::Array(vec![ + XmlRpcValue::String(String::from("Hello")), + XmlRpcValue::Double(0.5), + ]), + ]), + ], + } + ) + ); } #[test] fn writes_response() { - assert_eq!(concat!(r#""#, - r#""#, - r#""#, - r#""#, - r#"33"#, - r#""#, - r#""#, - r#""#, - r#"41"#, - r#"1"#, - r#""#, - r#"Hello"#, - r#"0.5"#, - r#""#, - r#""#, - r#""#, - r#""#, - r#""#), - format!("{}", - XmlRpcResponse { - parameters: vec![XmlRpcValue::Int(33), - XmlRpcValue::Array(vec![XmlRpcValue::Int(41), - XmlRpcValue::Bool(true), - XmlRpcValue::Array(vec![ - XmlRpcValue::String( - String::from("Hello")), - XmlRpcValue::Double(0.5), - ])])], - })); + assert_eq!( + concat!( + r#""#, + r#""#, + r#""#, + r#""#, + r#"33"#, + r#""#, + r#""#, + r#""#, + r#"41"#, + r#"1"#, + r#""#, + r#"Hello"#, + r#"0.5"#, + r#""#, + r#""#, + r#""#, + r#""#, + r#""# + ), + format!( + "{}", + XmlRpcResponse { + parameters: vec![ + XmlRpcValue::Int(33), + XmlRpcValue::Array(vec![ + XmlRpcValue::Int(41), + XmlRpcValue::Bool(true), + XmlRpcValue::Array(vec![ + XmlRpcValue::String(String::from("Hello")), + XmlRpcValue::Double(0.5), + ]), + ]), + ], + } + ) + ); } } diff --git a/src/rosxmlrpc/server.rs b/src/rosxmlrpc/server.rs index c3f29ff..713ab9b 100644 --- a/src/rosxmlrpc/server.rs +++ b/src/rosxmlrpc/server.rs @@ -12,15 +12,19 @@ pub struct Server { impl Server { pub fn new(hostname: &str, port: u16, responder: T) -> Result - where T: 'static + XmlRpcServer + Send + Sync + where + T: 'static + XmlRpcServer + Send + Sync, { - let listener = hyper::Server::http((hostname, port))? - .handle(XmlRpcHandler::new(responder))?; + let listener = hyper::Server::http((hostname, port))?.handle( + XmlRpcHandler::new( + responder, + ), + )?; let uri = format!("http://{}:{}/", hostname, listener.socket.port()); Ok(Server { - listener: listener, - uri: uri, - }) + listener: listener, + uri: uri, + }) } pub fn shutdown(&mut self) -> hyper::Result<()> { @@ -28,8 +32,10 @@ impl Server { } } -pub type ParameterIterator = std::iter::Map, - fn(serde::decoder::Decoder) -> Parameter>; +pub type ParameterIterator = std::iter::Map< + std::vec::IntoIter, + fn(serde::decoder::Decoder) -> Parameter, +>; pub trait XmlRpcServer { fn handle(&self, method_name: &str, params: ParameterIterator) -> error::serde::Result; @@ -47,8 +53,8 @@ impl XmlRpcHandler { fn process(&self, req: Request, res: Response) -> Result<()> { let (method_name, parameters) = serde::Decoder::new_request(req)?; res.send(&self.handler - .handle(&method_name, parameters.into_iter().map(Parameter::new))? - .write_response()?)?; + .handle(&method_name, parameters.into_iter().map(Parameter::new))? + .write_response()?)?; Ok(()) } } diff --git a/src/tcpros/client.rs b/src/tcpros/client.rs index ae0ae91..69195bd 100644 --- a/src/tcpros/client.rs +++ b/src/tcpros/client.rs @@ -14,15 +14,16 @@ pub struct ClientResponse { impl ClientResponse { pub fn read(self) -> Result> { - self.handle - .join() - .unwrap_or(Err(ErrorKind::ServiceResponseUnknown.into())) + self.handle.join().unwrap_or(Err( + ErrorKind::ServiceResponseUnknown.into(), + )) } } impl ClientResponse { pub fn callback(self, callback: F) - where F: FnOnce(Result>) + Send + 'static + where + F: FnOnce(Result>) + Send + 'static, { thread::spawn(move || callback(self.read())); } @@ -43,42 +44,42 @@ impl Client { pub fn new(caller_id: &str, uri: &str, service: &str) -> Client { Client { info: std::sync::Arc::new(ClientInfo { - caller_id: String::from(caller_id), - uri: String::from(uri), - service: String::from(service), - }), + caller_id: String::from(caller_id), + uri: String::from(uri), + service: String::from(service), + }), phantom: std::marker::PhantomData, } } pub fn req(&self, args: &T::Request) -> Result> { - Self::request_body(args, - &self.info.uri, - &self.info.caller_id, - &self.info.service) + Self::request_body( + args, + &self.info.uri, + &self.info.caller_id, + &self.info.service, + ) } pub fn req_async(&self, args: T::Request) -> ClientResponse { let info = self.info.clone(); ClientResponse { handle: thread::spawn(move || { - Self::request_body(&args, - &info.uri, - &info.caller_id, - &info.service) - }), + Self::request_body(&args, &info.uri, &info.caller_id, &info.service) + }), } } - fn request_body(args: &T::Request, - uri: &str, - caller_id: &str, - service: &str) - -> Result> { + fn request_body( + args: &T::Request, + uri: &str, + caller_id: &str, + service: &str, + ) -> Result> { let connection = TcpStream::connect(uri.trim_left_matches("rosrpc://")); - let mut stream = - connection - .chain_err(|| ErrorKind::ServiceConnectionFail(service.into(), uri.into()))?; + let mut stream = connection.chain_err(|| { + ErrorKind::ServiceConnectionFail(service.into(), uri.into()) + })?; // Service request starts by exchanging connection headers exchange_headers::(&mut stream, caller_id, service)?; @@ -87,15 +88,16 @@ impl Client { to_writer(&mut stream, &args)?; // Service responds with a boolean byte, signalling success - let success = read_verification_byte(&mut stream) - .chain_err(|| ErrorKind::ServiceResponseInterruption)?; + let success = read_verification_byte(&mut stream).chain_err(|| { + ErrorKind::ServiceResponseInterruption + })?; Ok(if success { - // Decode response as response type upon success - Ok(from_reader(&mut stream)?) - } else { - // Decode response as string upon failure - Err(from_reader(&mut stream)?) - }) + // Decode response as response type upon success + Ok(from_reader(&mut stream)?) + } else { + // Decode response as string upon failure + Err(from_reader(&mut stream)?) + }) } } @@ -105,8 +107,9 @@ fn read_verification_byte(reader: &mut R) -> std::io::Result(mut stream: &mut U, caller_id: &str, service: &str) -> Result<()> - where T: ServicePair, - U: std::io::Write +where + T: ServicePair, + U: std::io::Write, { let mut fields = HashMap::::new(); fields.insert(String::from("callerid"), String::from(caller_id)); @@ -118,8 +121,9 @@ fn write_request(mut stream: &mut U, caller_id: &str, service: &str) -> Re } fn read_response(mut stream: &mut U) -> Result<()> - where T: ServicePair, - U: std::io::Read +where + T: ServicePair, + U: std::io::Read, { let fields = decode(&mut stream)?; if fields.get("callerid").is_none() { @@ -129,8 +133,9 @@ fn read_response(mut stream: &mut U) -> Result<()> } fn exchange_headers(mut stream: &mut U, caller_id: &str, service: &str) -> Result<()> - where T: ServicePair, - U: std::io::Write + std::io::Read +where + T: ServicePair, + U: std::io::Write + std::io::Read, { write_request::(stream, caller_id, service)?; read_response::(stream) diff --git a/src/tcpros/header.rs b/src/tcpros/header.rs index 1cf6315..1955383 100644 --- a/src/tcpros/header.rs +++ b/src/tcpros/header.rs @@ -6,23 +6,29 @@ pub fn decode(data: &mut R) -> Result, from_reader(data) } -pub fn encode(writer: &mut W, - data: &HashMap) - -> Result<(), Error> { +pub fn encode( + writer: &mut W, + data: &HashMap, +) -> Result<(), Error> { to_writer(writer, data) } -pub fn match_field(fields: &HashMap, - field: &str, - expected: &str) - -> Result<(), super::error::Error> { +pub fn match_field( + fields: &HashMap, + field: &str, + expected: &str, +) -> Result<(), super::error::Error> { use super::error::ErrorKind; let actual = match fields.get(field) { Some(actual) => actual, None => bail!(ErrorKind::HeaderMissingField(field.into())), }; if actual != &expected { - bail!(ErrorKind::HeaderMismatch(field.into(), expected.into(), actual.clone())); + bail!(ErrorKind::HeaderMismatch( + field.into(), + expected.into(), + actual.clone(), + )); } Ok(()) } @@ -51,8 +57,10 @@ mod tests { let mut data = HashMap::::new(); data.insert(String::from("abc"), String::from("123")); encode(&mut cursor, &data).expect(FAILED_TO_ENCODE); - assert_eq!(vec![11, 0, 0, 0, 7, 0, 0, 0, 97, 98, 99, 61, 49, 50, 51], - cursor.into_inner()); + assert_eq!( + vec![11, 0, 0, 0, 7, 0, 0, 0, 97, 98, 99, 61, 49, 50, 51], + cursor.into_inner() + ); } @@ -64,10 +72,62 @@ mod tests { data.insert(String::from("AAA"), String::from("B0")); encode(&mut cursor, &data).expect(FAILED_TO_ENCODE); let data = cursor.into_inner(); - assert!(vec![21, 0, 0, 0, 7, 0, 0, 0, 97, 98, 99, 61, 49, 50, 51, 6, 0, 0, 0, 65, - 65, 65, 61, 66, 48] == data || - vec![21, 0, 0, 0, 6, 0, 0, 0, 65, 65, 65, 61, 66, 48, 7, 0, 0, 0, 97, 98, 99, - 61, 49, 50, 51] == data); + assert!( + vec![ + 21, + 0, + 0, + 0, + 7, + 0, + 0, + 0, + 97, + 98, + 99, + 61, + 49, + 50, + 51, + 6, + 0, + 0, + 0, + 65, + 65, + 65, + 61, + 66, + 48, + ] == data || + vec![ + 21, + 0, + 0, + 0, + 6, + 0, + 0, + 0, + 65, + 65, + 65, + 61, + 66, + 48, + 7, + 0, + 0, + 0, + 97, + 98, + 99, + 61, + 49, + 50, + 51, + ] == data + ); } #[test] @@ -87,30 +147,203 @@ mod tests { #[test] fn reads_typical_header() { - let input = vec![0xb0, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x6d, 0x65, 0x73, 0x73, - 0x61, 0x67, 0x65, 0x5f, 0x64, 0x65, 0x66, 0x69, 0x6e, 0x69, 0x74, 0x69, - 0x6f, 0x6e, 0x3d, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x20, 0x64, 0x61, - 0x74, 0x61, 0x0a, 0x0a, 0x25, 0x00, 0x00, 0x00, 0x63, 0x61, 0x6c, 0x6c, - 0x65, 0x72, 0x69, 0x64, 0x3d, 0x2f, 0x72, 0x6f, 0x73, 0x74, 0x6f, 0x70, - 0x69, 0x63, 0x5f, 0x34, 0x37, 0x36, 0x37, 0x5f, 0x31, 0x33, 0x31, 0x36, - 0x39, 0x31, 0x32, 0x37, 0x34, 0x31, 0x35, 0x35, 0x37, 0x0a, 0x00, 0x00, - 0x00, 0x6c, 0x61, 0x74, 0x63, 0x68, 0x69, 0x6e, 0x67, 0x3d, 0x31, 0x27, - 0x00, 0x00, 0x00, 0x6d, 0x64, 0x35, 0x73, 0x75, 0x6d, 0x3d, 0x39, 0x39, - 0x32, 0x63, 0x65, 0x38, 0x61, 0x31, 0x36, 0x38, 0x37, 0x63, 0x65, 0x63, - 0x38, 0x63, 0x38, 0x62, 0x64, 0x38, 0x38, 0x33, 0x65, 0x63, 0x37, 0x33, - 0x63, 0x61, 0x34, 0x31, 0x64, 0x31, 0x0e, 0x00, 0x00, 0x00, 0x74, 0x6f, - 0x70, 0x69, 0x63, 0x3d, 0x2f, 0x63, 0x68, 0x61, 0x74, 0x74, 0x65, 0x72, - 0x14, 0x00, 0x00, 0x00, 0x74, 0x79, 0x70, 0x65, 0x3d, 0x73, 0x74, 0x64, - 0x5f, 0x6d, 0x73, 0x67, 0x73, 0x2f, 0x53, 0x74, 0x72, 0x69, 0x6e, 0x67]; + let input = vec![ + 0xb0, + 0x00, + 0x00, + 0x00, + 0x20, + 0x00, + 0x00, + 0x00, + 0x6d, + 0x65, + 0x73, + 0x73, + 0x61, + 0x67, + 0x65, + 0x5f, + 0x64, + 0x65, + 0x66, + 0x69, + 0x6e, + 0x69, + 0x74, + 0x69, + 0x6f, + 0x6e, + 0x3d, + 0x73, + 0x74, + 0x72, + 0x69, + 0x6e, + 0x67, + 0x20, + 0x64, + 0x61, + 0x74, + 0x61, + 0x0a, + 0x0a, + 0x25, + 0x00, + 0x00, + 0x00, + 0x63, + 0x61, + 0x6c, + 0x6c, + 0x65, + 0x72, + 0x69, + 0x64, + 0x3d, + 0x2f, + 0x72, + 0x6f, + 0x73, + 0x74, + 0x6f, + 0x70, + 0x69, + 0x63, + 0x5f, + 0x34, + 0x37, + 0x36, + 0x37, + 0x5f, + 0x31, + 0x33, + 0x31, + 0x36, + 0x39, + 0x31, + 0x32, + 0x37, + 0x34, + 0x31, + 0x35, + 0x35, + 0x37, + 0x0a, + 0x00, + 0x00, + 0x00, + 0x6c, + 0x61, + 0x74, + 0x63, + 0x68, + 0x69, + 0x6e, + 0x67, + 0x3d, + 0x31, + 0x27, + 0x00, + 0x00, + 0x00, + 0x6d, + 0x64, + 0x35, + 0x73, + 0x75, + 0x6d, + 0x3d, + 0x39, + 0x39, + 0x32, + 0x63, + 0x65, + 0x38, + 0x61, + 0x31, + 0x36, + 0x38, + 0x37, + 0x63, + 0x65, + 0x63, + 0x38, + 0x63, + 0x38, + 0x62, + 0x64, + 0x38, + 0x38, + 0x33, + 0x65, + 0x63, + 0x37, + 0x33, + 0x63, + 0x61, + 0x34, + 0x31, + 0x64, + 0x31, + 0x0e, + 0x00, + 0x00, + 0x00, + 0x74, + 0x6f, + 0x70, + 0x69, + 0x63, + 0x3d, + 0x2f, + 0x63, + 0x68, + 0x61, + 0x74, + 0x74, + 0x65, + 0x72, + 0x14, + 0x00, + 0x00, + 0x00, + 0x74, + 0x79, + 0x70, + 0x65, + 0x3d, + 0x73, + 0x74, + 0x64, + 0x5f, + 0x6d, + 0x73, + 0x67, + 0x73, + 0x2f, + 0x53, + 0x74, + 0x72, + 0x69, + 0x6e, + 0x67, + ]; let data = decode(&mut std::io::Cursor::new(input)).expect(FAILED_TO_DECODE); assert_eq!(6, data.len()); - assert_eq!(Some(&String::from("string data\n\n")), - data.get("message_definition")); - assert_eq!(Some(&String::from("/rostopic_4767_1316912741557")), - data.get("callerid")); + assert_eq!( + Some(&String::from("string data\n\n")), + data.get("message_definition") + ); + assert_eq!( + Some(&String::from("/rostopic_4767_1316912741557")), + data.get("callerid") + ); assert_eq!(Some(&String::from("1")), data.get("latching")); - assert_eq!(Some(&String::from("992ce8a1687cec8c8bd883ec73ca41d1")), - data.get("md5sum")); + assert_eq!( + Some(&String::from("992ce8a1687cec8c8bd883ec73ca41d1")), + data.get("md5sum") + ); assert_eq!(Some(&String::from("/chatter")), data.get("topic")); assert_eq!(Some(&String::from("std_msgs/String")), data.get("type")); } diff --git a/src/tcpros/publisher.rs b/src/tcpros/publisher.rs index c154381..0dd8710 100644 --- a/src/tcpros/publisher.rs +++ b/src/tcpros/publisher.rs @@ -40,22 +40,25 @@ fn write_response(mut stream: &mut U) -> Result<( } fn exchange_headers(mut stream: &mut U, topic: &str) -> Result<()> - where T: Message, - U: std::io::Write + std::io::Read +where + T: Message, + U: std::io::Write + std::io::Read, { read_request::(&mut stream, &topic)?; write_response::(&mut stream) } fn listen_for_subscribers(topic: &str, listener: V, targets: TargetList) - where T: Message, - U: std::io::Read + std::io::Write + Send, - V: Iterator +where + T: Message, + U: std::io::Read + std::io::Write + Send, + V: Iterator, { // This listener stream never breaks by itself since it's a TcpListener for mut stream in listener { - let result = exchange_headers::(&mut stream, topic) - .chain_err(|| ErrorKind::TopicConnectionFail(topic.into())); + let result = exchange_headers::(&mut stream, topic).chain_err(|| { + ErrorKind::TopicConnectionFail(topic.into()) + }); if let Err(err) = result { let info = err.iter() .map(|v| format!("{}", v)) @@ -75,27 +78,37 @@ fn listen_for_subscribers(topic: &str, listener: V, targets: TargetList impl Publisher { pub fn new(address: U, topic: &str) -> Result - where T: Message, - U: ToSocketAddrs + where + T: Message, + U: ToSocketAddrs, { let listener = TcpListener::bind(address)?; let socket_address = listener.local_addr()?; let (raii, listener) = tcpconnection::iterate(listener, format!("topic '{}'", topic)); - Ok(Publisher::wrap_stream::(topic, raii, listener, socket_address.port())) + Ok(Publisher::wrap_stream::( + topic, + raii, + listener, + socket_address.port(), + )) } - fn wrap_stream(topic: &str, - raii: tcpconnection::Raii, - listener: V, - port: u16) - -> Publisher - where T: Message, - U: std::io::Read + std::io::Write + Send + 'static, - V: Iterator + Send + 'static + fn wrap_stream( + topic: &str, + raii: tcpconnection::Raii, + listener: V, + port: u16, + ) -> Publisher + where + T: Message, + U: std::io::Read + std::io::Write + Send + 'static, + V: Iterator + Send + 'static, { let (targets, data) = fork(); let topic_name = String::from(topic); - thread::spawn(move || listen_for_subscribers::(&topic_name, listener, targets)); + thread::spawn(move || { + listen_for_subscribers::(&topic_name, listener, targets) + }); Publisher { subscriptions: data, port: port, @@ -123,12 +136,15 @@ impl PublisherStream { fn new(publisher: &Publisher) -> Result> { let msg_type = T::msg_type(); if publisher.msg_type != msg_type { - bail!(ErrorKind::MessageTypeMismatch(publisher.msg_type.clone(), msg_type)); + bail!(ErrorKind::MessageTypeMismatch( + publisher.msg_type.clone(), + msg_type, + )); } Ok(PublisherStream { - stream: publisher.subscriptions.clone(), - datatype: std::marker::PhantomData, - }) + stream: publisher.subscriptions.clone(), + datatype: std::marker::PhantomData, + }) } pub fn send(&mut self, message: T) -> Result<()> { diff --git a/src/tcpros/service.rs b/src/tcpros/service.rs index bfb5abf..64042dc 100644 --- a/src/tcpros/service.rs +++ b/src/tcpros/service.rs @@ -18,42 +18,50 @@ pub struct Service { } impl Service { - pub fn new(hostname: &str, - port: u16, - service: &str, - node_name: &str, - handler: F) - -> Result - where T: ServicePair, - F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static + pub fn new( + hostname: &str, + port: u16, + service: &str, + node_name: &str, + handler: F, + ) -> Result + where + T: ServicePair, + F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static, { let listener = TcpListener::bind((hostname, port))?; let socket_address = listener.local_addr()?; let api = format!("rosrpc://{}:{}", hostname, socket_address.port()); let (raii, listener) = tcpconnection::iterate(listener, format!("service '{}'", service)); - Ok(Service::wrap_stream::(service, node_name, handler, raii, listener, &api)) + Ok(Service::wrap_stream::( + service, + node_name, + handler, + raii, + listener, + &api, + )) } - fn wrap_stream(service: &str, - node_name: &str, - handler: F, - raii: tcpconnection::Raii, - listener: V, - api: &str) - -> Service - where T: ServicePair, - U: std::io::Read + std::io::Write + Send + 'static, - V: Iterator + Send + 'static, - F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static + fn wrap_stream( + service: &str, + node_name: &str, + handler: F, + raii: tcpconnection::Raii, + listener: V, + api: &str, + ) -> Service + where + T: ServicePair, + U: std::io::Read + std::io::Write + Send + 'static, + V: Iterator + Send + 'static, + F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static, { let service_name = String::from(service); let node_name = String::from(node_name); thread::spawn(move || { - listen_for_clients::(service_name, - node_name, - handler, - listener) - }); + listen_for_clients::(service_name, node_name, handler, listener) + }); Service { api: String::from(api), msg_type: T::msg_type(), @@ -64,18 +72,21 @@ impl Service { } fn listen_for_clients(service: String, node_name: String, handler: F, connections: V) - where T: ServicePair, - U: std::io::Read + std::io::Write + Send + 'static, - V: Iterator, - F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static +where + T: ServicePair, + U: std::io::Read + std::io::Write + Send + 'static, + V: Iterator, + F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static, { let handler = Arc::new(handler); for mut stream in connections { // Service request starts by exchanging connection headers if let Err(err) = exchange_headers::(&mut stream, &service, &node_name) { - error!("Failed to exchange headers for service '{}': {}", - service, - err); + error!( + "Failed to exchange headers for service '{}': {}", + service, + err + ); continue; } @@ -85,8 +96,9 @@ fn listen_for_clients(service: String, node_name: String, handler: F } fn exchange_headers(mut stream: &mut U, service: &str, node_name: &str) -> Result<()> - where T: ServicePair, - U: std::io::Write + std::io::Read +where + T: ServicePair, + U: std::io::Write + std::io::Read, { read_request::(stream, service)?; write_response::(stream, node_name) @@ -105,8 +117,9 @@ fn read_request(mut stream: &mut U, service: & } fn write_response(mut stream: &mut U, node_name: &str) -> Result<()> - where T: ServicePair, - U: std::io::Write +where + T: ServicePair, + U: std::io::Write, { let mut fields = HashMap::::new(); fields.insert(String::from("callerid"), String::from(node_name)); @@ -117,23 +130,29 @@ fn write_response(mut stream: &mut U, node_name: &str) -> Result<()> } fn spawn_request_handler(stream: U, handler: Arc) - where T: ServicePair, - U: std::io::Read + std::io::Write + Send + 'static, - F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static +where + T: ServicePair, + U: std::io::Read + std::io::Write + Send + 'static, + F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static, { - thread::spawn(move || if let Err(err) = handle_request_loop::(stream, handler) { - let info = err.iter() - .map(|v| format!("{}", v)) - .collect::>() - .join("\nCaused by:"); - error!("{}", info); - }); + thread::spawn(move || if let Err(err) = handle_request_loop::( + stream, + handler, + ) + { + let info = err.iter() + .map(|v| format!("{}", v)) + .collect::>() + .join("\nCaused by:"); + error!("{}", info); + }); } fn handle_request_loop(mut stream: U, handler: Arc) -> Result<()> - where T: ServicePair, - U: std::io::Read + std::io::Write, - F: Fn(T::Request) -> ServiceResult +where + T: ServicePair, + U: std::io::Read + std::io::Write, + F: Fn(T::Request) -> ServiceResult, { loop { // Receive request from client diff --git a/src/tcpros/subscriber.rs b/src/tcpros/subscriber.rs index 2a594e9..ed9c525 100644 --- a/src/tcpros/subscriber.rs +++ b/src/tcpros/subscriber.rs @@ -18,15 +18,18 @@ pub struct Subscriber { impl Subscriber { pub fn new(caller_id: &str, topic: &str, callback: F) -> Subscriber - where T: Message, - F: Fn(T) -> () + Send + 'static + where + T: Message, + F: Fn(T) -> () + Send + 'static, { let (data_tx, data_rx) = channel(); let (pub_tx, pub_rx) = channel(); let caller_id = String::from(caller_id); let topic_name = String::from(topic); let data_stream = data_tx.clone(); - thread::spawn(move || join_connections::(data_tx, pub_rx, &caller_id, &topic_name)); + thread::spawn(move || { + join_connections::(data_tx, pub_rx, &caller_id, &topic_name) + }); thread::spawn(move || handle_data::(data_rx, callback)); Subscriber { data_stream: data_stream, @@ -42,9 +45,9 @@ impl Subscriber { // Failure could only be caused by the join_connections // thread not running, which only happens after // Subscriber has been deconstructed - self.publishers_stream - .send(address) - .expect("Connected thread died"); + self.publishers_stream.send(address).expect( + "Connected thread died", + ); } Ok(()) } @@ -53,15 +56,18 @@ impl Subscriber { impl Drop for Subscriber { fn drop(&mut self) { if self.data_stream.send(None).is_err() { - error!("Subscriber data stream to topic '{}' has already been killed", - self.topic); + error!( + "Subscriber data stream to topic '{}' has already been killed", + self.topic + ); } } } fn handle_data(data: Receiver>>, callback: F) - where T: Message, - F: Fn(T) -> () +where + T: Message, + F: Fn(T) -> (), { for buffer_option in data { let buffer = match buffer_option { @@ -75,11 +81,13 @@ fn handle_data(data: Receiver>>, callback: F) } } -fn join_connections(data_stream: Sender>>, - publishers: Receiver, - caller_id: &str, - topic: &str) - where T: Message +fn join_connections( + data_stream: Sender>>, + publishers: Receiver, + caller_id: &str, + topic: &str, +) where + T: Message, { // Ends when publisher sender is destroyed, which happens at Subscriber destruction for publisher in publishers { @@ -95,12 +103,14 @@ fn join_connections(data_stream: Sender>>, } } -fn join_connection(data_stream: &Sender>>, - publisher: &SocketAddr, - caller_id: &str, - topic: &str) - -> Result<()> - where T: Message +fn join_connection( + data_stream: &Sender>>, + publisher: &SocketAddr, + caller_id: &str, + topic: &str, +) -> Result<()> +where + T: Message, { let mut stream = TcpStream::connect(publisher)?; exchange_headers::(&mut stream, caller_id, topic)?; @@ -117,10 +127,11 @@ fn join_connection(data_stream: &Sender>>, Ok(()) } -fn write_request(mut stream: &mut U, - caller_id: &str, - topic: &str) - -> Result<()> { +fn write_request( + mut stream: &mut U, + caller_id: &str, + topic: &str, +) -> Result<()> { let mut fields = HashMap::::new(); fields.insert(String::from("message_definition"), T::msg_definition()); fields.insert(String::from("callerid"), String::from(caller_id)); @@ -138,8 +149,9 @@ fn read_response(mut stream: &mut U) -> Result<()> } fn exchange_headers(mut stream: &mut U, caller_id: &str, topic: &str) -> Result<()> - where T: Message, - U: std::io::Write + std::io::Read +where + T: Message, + U: std::io::Write + std::io::Read, { write_request::(stream, caller_id, topic)?; read_response::(stream) @@ -177,16 +189,16 @@ mod tests { #[test] fn package_to_vector_creates_right_buffer_from_reader() { let input = [7, 0, 0, 0, 1, 2, 3, 4, 5, 6, 7]; - let data = package_to_vector(&mut std::io::Cursor::new(input)) - .expect(FAILED_TO_READ_WRITE_VECTOR); + let data = + package_to_vector(&mut std::io::Cursor::new(input)).expect(FAILED_TO_READ_WRITE_VECTOR); assert_eq!(data, [7, 0, 0, 0, 1, 2, 3, 4, 5, 6, 7]); } #[test] fn package_to_vector_respects_provided_length() { let input = [7, 0, 0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12]; - let data = package_to_vector(&mut std::io::Cursor::new(input)) - .expect(FAILED_TO_READ_WRITE_VECTOR); + let data = + package_to_vector(&mut std::io::Cursor::new(input)).expect(FAILED_TO_READ_WRITE_VECTOR); assert_eq!(data, [7, 0, 0, 0, 1, 2, 3, 4, 5, 6, 7]); } diff --git a/src/tcpros/util/streamfork.rs b/src/tcpros/util/streamfork.rs index d1cfaed..f374a78 100644 --- a/src/tcpros/util/streamfork.rs +++ b/src/tcpros/util/streamfork.rs @@ -18,9 +18,9 @@ fn fork_thread(streams: Receiver, data: Receiver Some(target), - Err(_) => None, - }) + Ok(()) => Some(target), + Err(_) => None, + }) .collect() } } From bc30656fa9afdf56a1178f2d4bb988db89b96fab Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 23 Aug 2017 02:48:30 +0200 Subject: [PATCH 02/50] Create initial XML-RPC client switch --- Cargo.toml | 2 +- src/api/error.rs | 9 ++ src/api/master.rs | 253 +++++++++++++++++---------------- src/api/ros.rs | 16 ++- src/api/slavehandler.rs | 22 +-- src/lib.rs | 1 + src/rosxmlrpc/client.rs | 73 ---------- src/rosxmlrpc/mod.rs | 2 - src/rosxmlrpc/serde/decoder.rs | 45 ------ src/rosxmlrpc/serde/encoder.rs | 43 ------ src/rosxmlrpc/serde/value.rs | 47 ------ 11 files changed, 165 insertions(+), 348 deletions(-) delete mode 100644 src/rosxmlrpc/client.rs diff --git a/Cargo.toml b/Cargo.toml index 8d8d5a8..b620bf6 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -6,7 +6,6 @@ name = "rosrust" readme = "README.md" repository = "https://github.com/adnanademovic/rosrust" version = "0.5.0" - [dependencies] byteorder = "1.0.0" error-chain = "0.10.0" @@ -20,6 +19,7 @@ rustc-serialize = "0.3" serde = "1.0.2" serde_derive = "1.0.2" serde_rosmsg = "0.2.0" +xml-rpc = "0.0.1" xml-rs = "0.3" [dependencies.hyper] diff --git a/src/api/error.rs b/src/api/error.rs index b49206b..a3a5b3e 100644 --- a/src/api/error.rs +++ b/src/api/error.rs @@ -38,10 +38,19 @@ pub mod api { } pub mod master { + use xml_rpc; + error_chain! { links { + XmlRpcErr(xml_rpc::error::Error, xml_rpc::error::ErrorKind); XmlRpc(super::rosxmlrpc::Error, super::rosxmlrpc::ErrorKind); Api(::error::api::Error, ::error::api::ErrorKind); } + errors { + Fault(fault: xml_rpc::Fault) { + description("Queried XML-RPC server returned a fault") + display("Fault #{}: {}", fault.code, fault.message) + } + } } } diff --git a/src/api/master.rs b/src/api/master.rs index 9bdc279..3d68859 100644 --- a/src/api/master.rs +++ b/src/api/master.rs @@ -1,112 +1,103 @@ -use rosxmlrpc; -use error::rosxmlrpc::{Error as ReError, ErrorKind as ReErrorKind}; -use error::rosxmlrpc::serde::ErrorKind as SeErrorKind; +use std; +use std::sync::Mutex; use super::error::master::{Result, Error, ErrorKind}; -use rustc_serialize::{Decodable, Decoder, Encodable}; +use serde::{Deserialize, Serialize}; use super::value::Topic; +use xml_rpc; pub struct Master { - client: rosxmlrpc::Client, + client: Mutex, client_id: String, caller_api: String, + master_uri: String, +} + +const ERROR_CODE: i32 = -1; +const FAILURE_CODE: i32 = 0; +const SUCCESS_CODE: i32 = 1; + +fn parse_response(params: xml_rpc::Params) -> std::result::Result { + let mut param_iter = params.into_iter(); + let code = param_iter.next().ok_or_else(|| { + xml_rpc::Fault::new(FAILURE_CODE, "Server response missing arguments.") + })?; + let message = param_iter.next().ok_or_else(|| { + xml_rpc::Fault::new(FAILURE_CODE, "Server response missing arguments.") + })?; + let value = param_iter.next().ok_or_else(|| { + xml_rpc::Fault::new(FAILURE_CODE, "Server response missing arguments.") + })?; + let code = match code { + xml_rpc::Value::Int(v) => v, + _ => { + return Err(xml_rpc::Fault::new( + FAILURE_CODE, + "First response argument is expected to be int.", + )) + } + }; + let message = match message { + xml_rpc::Value::String(v) => v, + _ => { + return Err(xml_rpc::Fault::new( + FAILURE_CODE, + "Second response argument is expected to be string.", + )) + } + }; + if code != SUCCESS_CODE { + return Err(xml_rpc::Fault::new(code, message)); + } + Ok(value) } macro_rules! request { ($s:expr; $name:ident; $($item:expr),*)=> ({ - let mut request = rosxmlrpc::client::Request::new(stringify!($name)); - request.add(&$s.client_id).map_err(|v| ReError::from(v))?; - $( - request.add(&$item).map_err(|v| ReError::from(v))?; - )* - let data : ResponseData<_> = $s.client.request(request)?; - data.0.map_err(to_api_error) + let params = xml_rpc::into_params(&(&$s.client_id, + $( + $item, + )* + )) + .map_err(xml_rpc::error::Error::from)?; + let response = $s.client.lock().unwrap() + .call_value(&$s.master_uri.parse().unwrap(), stringify!($name), params)? + .map_err(to_api_error)?; + let data = parse_response(response).map_err(to_api_error)?; + Deserialize::deserialize(data).map_err(|v| { + to_api_error(xml_rpc::Fault::new( + FAILURE_CODE, + format!("Third response argument has unexpected structure: {}", v), + ))}) }) } macro_rules! request_tree { ($s:expr; $name:ident; $($item:expr),*)=> ({ - let mut request = rosxmlrpc::client::Request::new(stringify!($name)); - request.add(&$s.client_id).map_err(|v| ReError::from(v))?; - $( - request.add(&$item).map_err(|v| ReError::from(v))?; - )* - $s.client.request_tree(request) - .map_err(|v|v.into()) - .and_then(Master::remove_tree_wrap) + let params = xml_rpc::into_params(&(&$s.client_id, + $( + $item, + )* + )) + .map_err(xml_rpc::error::Error::from)?; + let response = $s.client.lock().unwrap() + .call_value(&$s.master_uri.parse().unwrap(), stringify!($name), params)? + .map_err(ErrorKind::Fault)?; + parse_response(response).map_err(to_api_error) }) } impl Master { pub fn new(master_uri: &str, client_id: &str, caller_api: &str) -> Master { Master { - client: rosxmlrpc::Client::new(&master_uri), + client: Mutex::new(xml_rpc::Client::new().unwrap()), client_id: client_id.to_owned(), caller_api: caller_api.to_owned(), + master_uri: master_uri.to_owned(), } } - fn remove_tree_wrap(data: rosxmlrpc::XmlRpcValue) -> Result { - let values = match data { - rosxmlrpc::XmlRpcValue::Array(values) => values, - _ => { - bail!(ErrorKind::XmlRpc( - ReErrorKind::Serde(SeErrorKind::MismatchedDataFormat( - "while handling request".into(), - )), - )) - } - }; - if values.len() != 3 { - bail!(ErrorKind::XmlRpc( - ReErrorKind::Serde(SeErrorKind::MismatchedDataFormat( - "while \ - handling \ - request" - .into(), - )), - )) - } - let mut values = values.into_iter(); - let code = match values.next() { - Some(rosxmlrpc::XmlRpcValue::Int(v)) => v, - _ => { - bail!(ErrorKind::XmlRpc( - ReErrorKind::Serde(SeErrorKind::MismatchedDataFormat( - "while handling request".into(), - )), - )) - } - }; - let message = match values.next() { - Some(rosxmlrpc::XmlRpcValue::String(v)) => v, - _ => { - bail!(ErrorKind::XmlRpc( - ReErrorKind::Serde(SeErrorKind::MismatchedDataFormat( - "while handling request".into(), - )), - )) - } - }; - let value = match values.next() { - Some(v) => v, - _ => { - bail!(ErrorKind::XmlRpc( - ReErrorKind::Serde(SeErrorKind::MismatchedDataFormat( - "while handling request".into(), - )), - )) - } - }; - if code != 1 { - bail!(ErrorKind::XmlRpc( - ReErrorKind::Serde(SeErrorKind::Msg(message)), - )); - } - Ok(value) - } - pub fn register_service(&self, service: &str, service_api: &str) -> Result { - request!(self; registerService; service, service_api, self.caller_api) + request!(self; registerService; service, service_api, &self.caller_api) } pub fn unregister_service(&self, service: &str, service_api: &str) -> Result { @@ -114,19 +105,19 @@ impl Master { } pub fn register_subscriber(&self, topic: &str, topic_type: &str) -> Result> { - request!(self; registerSubscriber; topic, topic_type, self.caller_api) + request!(self; registerSubscriber; topic, topic_type, &self.caller_api) } pub fn unregister_subscriber(&self, topic: &str) -> Result { - request!(self; registerSubscriber; topic, self.caller_api) + request!(self; registerSubscriber; topic, &self.caller_api) } pub fn register_publisher(&self, topic: &str, topic_type: &str) -> Result> { - request!(self; registerPublisher; topic, topic_type, self.caller_api) + request!(self; registerPublisher; topic, topic_type, &self.caller_api) } pub fn unregister_publisher(&self, topic: &str) -> Result { - request!(self; unregisterPublisher; topic, self.caller_api) + request!(self; unregisterPublisher; topic, &self.caller_api) } #[allow(dead_code)] @@ -139,11 +130,11 @@ impl Master { request!(self; getPublishedTopics; subgraph) } - pub fn get_topic_types(&self) -> Result> { + pub fn get_topic_types(&self) -> Result> { request!(self; getTopicTypes;) } - pub fn get_system_state(&self) -> Result { + pub fn get_system_state(&self) -> Result { request!(self; getSystemState;) } @@ -160,15 +151,15 @@ impl Master { request!(self; deleteParam; key) } - pub fn set_param(&self, key: &str, value: &T) -> Result { + pub fn set_param(&self, key: &str, value: &T) -> Result { request!(self; setParam; key, value) } - pub fn get_param(&self, key: &str) -> Result { + pub fn get_param<'a, T: Deserialize<'a>>(&self, key: &str) -> Result { request!(self; getParam; key) } - pub fn get_param_any(&self, key: &str) -> Result { + pub fn get_param_any(&self, key: &str) -> Result { request_tree!(self; getParam; key) } @@ -177,18 +168,18 @@ impl Master { } #[allow(dead_code)] - pub fn subscribe_param(&self, key: &str) -> Result { - request!(self; subscribeParam; self.caller_api, key) + pub fn subscribe_param<'a, T: Deserialize<'a>>(&self, key: &str) -> Result { + request!(self; subscribeParam; &self.caller_api, key) } #[allow(dead_code)] - pub fn subscribe_param_any(&self, key: &str) -> Result { - request_tree!(self; subscribeParam; self.caller_api, key) + pub fn subscribe_param_any(&self, key: &str) -> Result { + request_tree!(self; subscribeParam; &self.caller_api, key) } #[allow(dead_code)] pub fn unsubscribe_param(&self, key: &str) -> Result { - request!(self; unsubscribeParam; self.caller_api, key) + request!(self; unsubscribeParam; &self.caller_api, key) } pub fn has_param(&self, key: &str) -> Result { @@ -200,42 +191,64 @@ impl Master { } } -fn to_api_error(v: (bool, String)) -> Error { +fn to_api_error(v: xml_rpc::Fault) -> Error { use super::error::api::ErrorKind as ApiErrorKind; - match v.0 { - false => ErrorKind::Api(ApiErrorKind::SystemFail(v.1)), - true => ErrorKind::Api(ApiErrorKind::BadData(v.1)), + match v.code { + FAILURE_CODE => ErrorKind::Api(ApiErrorKind::SystemFail(v.message)), + ERROR_CODE => ErrorKind::Api(ApiErrorKind::BadData(v.message)), + x => ErrorKind::Api(ApiErrorKind::SystemFail(format!( + "Bad error code #{} returned with message: {}", + x, + v.message + ))), }.into() } #[derive(Debug)] -struct ResponseData(::std::result::Result); - -impl Decodable for ResponseData { - fn decode(d: &mut D) -> ::std::result::Result, D::Error> { - d.read_struct("ResponseData", 3, |d| { - let code = d.read_struct_field("status_code", 0, |d| d.read_i32())?; - let message = d.read_struct_field("status_message", 1, |d| d.read_str())?; - match code { - 0 | -1 => Ok(ResponseData(Err((code != 0, message)))), - 1 => Ok(ResponseData( - Ok(d.read_struct_field("data", 2, |d| T::decode(d))?), - )), - _ => Err(d.error("Invalid response code returned by ROS")), - } - }) - } -} - -#[derive(Debug, RustcDecodable)] pub struct TopicData { pub name: String, pub connections: Vec, } -#[derive(Debug, RustcDecodable)] +#[derive(Debug)] pub struct SystemState { pub publishers: Vec, pub subscribers: Vec, pub services: Vec, } + +#[derive(Debug, Deserialize)] +pub struct TopicDataTuple(String, Vec); +#[derive(Debug, Deserialize)] +pub struct SystemStateTuple(Vec, Vec, Vec); + +impl Into for SystemStateTuple { + fn into(self) -> SystemState { + SystemState { + publishers: self.0.into_iter().map(Into::into).collect(), + subscribers: self.1.into_iter().map(Into::into).collect(), + services: self.2.into_iter().map(Into::into).collect(), + } + } +} + +impl Into for TopicDataTuple { + fn into(self) -> TopicData { + TopicData { + name: self.0, + connections: self.1, + } + } +} + +#[derive(Debug, Deserialize)] +pub struct TopicTuple(String, String); + +impl Into for TopicTuple { + fn into(self) -> Topic { + Topic { + name: self.0, + datatype: self.1, + } + } +} diff --git a/src/api/ros.rs b/src/api/ros.rs index 948d478..903e060 100644 --- a/src/api/ros.rs +++ b/src/api/ros.rs @@ -1,4 +1,4 @@ -use rustc_serialize::{Encodable, Decodable}; +use serde::{Serialize, Deserialize}; use super::master::{self, Master}; use super::slave::Slave; use super::error::{ErrorKind, Result}; @@ -7,7 +7,7 @@ use super::value::Topic; use super::naming::{self, Resolver}; use super::resolve; use tcpros::{Client, Message, PublisherStream, ServicePair, ServiceResult}; -use rosxmlrpc::serde::XmlRpcValue; +use xml_rpc; pub struct Ros { master: Master, @@ -75,11 +75,13 @@ impl Ros { } pub fn state(&self) -> MasterResult { - self.master.get_system_state() + self.master.get_system_state().map(Into::into) } pub fn topics(&self) -> MasterResult> { - self.master.get_topic_types() + self.master.get_topic_types().map(|v| { + v.into_iter().map(Into::into).collect() + }) } pub fn client(&self, service: &str) -> Result> { @@ -173,15 +175,15 @@ impl<'a> Parameter<'a> { &self.name } - pub fn get(&self) -> MasterResult { + pub fn get<'b, T: Deserialize<'b>>(&self) -> MasterResult { self.master.get_param::(&self.name) } - pub fn get_raw(&self) -> MasterResult { + pub fn get_raw(&self) -> MasterResult { self.master.get_param_any(&self.name) } - pub fn set(&self, value: &T) -> MasterResult<()> { + pub fn set(&self, value: &T) -> MasterResult<()> { self.master.set_param::(&self.name, value).and(Ok(())) } diff --git a/src/api/slavehandler.rs b/src/api/slavehandler.rs index 09df35e..c0c255e 100644 --- a/src/api/slavehandler.rs +++ b/src/api/slavehandler.rs @@ -1,5 +1,5 @@ use nix::unistd::getpid; -use rosxmlrpc::{self, XmlRpcValue}; +use rosxmlrpc::XmlRpcValue; use rosxmlrpc::server::{Answer, ParameterIterator, XmlRpcServer}; use rustc_serialize::{Decodable, Encodable}; use std::collections::HashMap; @@ -8,6 +8,7 @@ use std::sync::mpsc::Sender; use super::error::{self, ErrorKind, Result}; use super::value::Topic; use tcpros::{Publisher, Subscriber, Service}; +use xml_rpc; pub struct SlaveHandler { pub subscriptions: Arc>>, @@ -319,15 +320,16 @@ fn request_topic( caller_id: &str, topic: &str, ) -> error::rosxmlrpc::Result<(String, String, i32)> { - let mut request = rosxmlrpc::client::Request::new("requestTopic"); - request.add(&caller_id)?; - request.add(&topic)?; - request.add(&[["TCPROS"]])?; - let client = rosxmlrpc::Client::new(publisher_uri); - let protocols = client.request::<(i32, String, (String, String, i32))>( - request, - )?; - Ok(protocols.2) + let (_code, _message, protocols): (i32, String, (String, String, i32)) = xml_rpc::Client::new() + .unwrap() + .call(&publisher_uri.parse().unwrap(), "requestTopic", &( + caller_id, + topic, + [["TCPROS"]], + )) + .unwrap() + .unwrap(); + Ok(protocols) } #[derive(RustcEncodable)] diff --git a/src/lib.rs b/src/lib.rs index db73317..5b72341 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -18,6 +18,7 @@ extern crate serde; extern crate serde_derive; extern crate serde_rosmsg; extern crate xml; +extern crate xml_rpc; pub use api::Ros; pub use tcpros::{Client, PublisherStream, Message, ServicePair as Service}; diff --git a/src/rosxmlrpc/client.rs b/src/rosxmlrpc/client.rs deleted file mode 100644 index 8ef08af..0000000 --- a/src/rosxmlrpc/client.rs +++ /dev/null @@ -1,73 +0,0 @@ -use hyper; -use rustc_serialize::{Encodable, Decodable}; -use super::error::{self, ErrorKind, Result}; -use super::serde; - -pub struct Client { - http_client: hyper::Client, - server_uri: String, -} - -impl Client { - pub fn new(server_uri: &str) -> Client { - Client { - http_client: hyper::Client::new(), - server_uri: String::from(server_uri), - } - } - - pub fn request_tree(&self, request: Request) -> Result { - let mut body = Vec::::new(); - request.encoder.write_request(&request.name, &mut body)?; - - let body = String::from_utf8(body)?; - let res = self.http_client.post(&self.server_uri).body(&body).send()?; - - let mut res = serde::Decoder::new_response(res)?; - match res.pop() { - Some(v) => Ok(v.value()), - None => { - bail!(ErrorKind::Serde( - error::serde::ErrorKind::Decoding("request tree".into()), - )) - } - } - } - - pub fn request(&self, request: Request) -> Result { - let mut body = Vec::::new(); - request.encoder.write_request(&request.name, &mut body)?; - - let body = String::from_utf8(body)?; - let res = self.http_client.post(&self.server_uri).body(&body).send()?; - - let mut res = serde::Decoder::new_response(res)?; - let mut value = match res.pop() { - Some(v) => v, - None => { - bail!(ErrorKind::Serde( - error::serde::ErrorKind::Decoding("request".into()), - )) - } - }; - T::decode(&mut value).map_err(|v| v.into()) - } -} - -pub struct Request { - name: String, - encoder: serde::Encoder, -} - -impl Request { - pub fn new(function_name: &str) -> Request { - Request { - name: String::from(function_name), - encoder: serde::Encoder::new(), - } - } - - pub fn add(&mut self, parameter: &T) -> error::serde::Result<()> { - parameter.encode(&mut self.encoder) - } -} diff --git a/src/rosxmlrpc/mod.rs b/src/rosxmlrpc/mod.rs index fd99731..9d70aef 100644 --- a/src/rosxmlrpc/mod.rs +++ b/src/rosxmlrpc/mod.rs @@ -1,8 +1,6 @@ -pub use self::client::Client; pub use self::server::Server; pub use self::serde::XmlRpcValue; -pub mod client; pub mod error; pub mod serde; pub mod server; diff --git a/src/rosxmlrpc/serde/decoder.rs b/src/rosxmlrpc/serde/decoder.rs index f9616e7..e97e398 100644 --- a/src/rosxmlrpc/serde/decoder.rs +++ b/src/rosxmlrpc/serde/decoder.rs @@ -29,14 +29,6 @@ impl Decoder { }) } - pub fn new_response(body: T) -> Result> { - value::XmlRpcResponse::new(body) - .chain_err(|| ErrorKind::XmlRpcReading("response".into())) - .map(|value| { - value.parameters.into_iter().map(Decoder::new).collect() - }) - } - pub fn value(self) -> value::XmlRpcValue { self.value } @@ -457,43 +449,6 @@ mod tests { ); } - #[test] - fn handles_responses() { - let data = r#" - - - - 33 - - - - 41 - 1 - - Hello - 0.5 - - - - -"#; - let mut cursor = std::io::Cursor::new(data.as_bytes()); - let mut parameters = Decoder::new_response(&mut cursor).expect(FAILED_TO_DECODE); - assert_eq!(2, parameters.len()); - assert_eq!(33, i32::decode(&mut parameters[0]).expect(FAILED_TO_DECODE)); - assert_eq!( - ExampleRequestStruct { - a: 41, - b: true, - c: ExampleRequestStructChild { - a: String::from("Hello"), - b: 0.5, - }, - }, - ExampleRequestStruct::decode(&mut parameters[1]).expect(FAILED_TO_DECODE) - ); - } - #[test] fn decoders_value_field_matches_data() { let data = r#" diff --git a/src/rosxmlrpc/serde/encoder.rs b/src/rosxmlrpc/serde/encoder.rs index 820253f..3ad1df0 100644 --- a/src/rosxmlrpc/serde/encoder.rs +++ b/src/rosxmlrpc/serde/encoder.rs @@ -42,21 +42,6 @@ impl Encoder { } } - pub fn write_request( - self, - method: &str, - mut body: &mut T, - ) -> std::io::Result<()> { - write!( - &mut body, - "{}", - value::XmlRpcRequest { - method: String::from(method), - parameters: self.form_tree(), - } - ) - } - pub fn write_response(self, mut body: &mut T) -> std::io::Result<()> { write!( &mut body, @@ -315,34 +300,6 @@ mod tests { ); } - #[test] - fn writes_request() { - let mut data = vec![]; - let mut encoder = Encoder::new(); - String::from("Hello").encode(&mut encoder).expect( - FAILED_TO_ENCODE, - ); - encoder.write_request("something", &mut data).expect( - FAILED_TO_ENCODE, - ); - assert_eq!( - concat!( - r#""#, - r#""#, - r#""#, - r#"something"#, - r#""#, - r#""#, - r#""#, - r#"Hello"#, - r#""#, - r#""#, - r#""# - ), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) - ); - } - #[test] fn writes_string() { let mut data = vec![]; diff --git a/src/rosxmlrpc/serde/value.rs b/src/rosxmlrpc/serde/value.rs index c16dc15..ae50662 100644 --- a/src/rosxmlrpc/serde/value.rs +++ b/src/rosxmlrpc/serde/value.rs @@ -113,15 +113,6 @@ impl XmlRpcRequest { } } -impl XmlRpcResponse { - pub fn new(body: T) -> Result { - extract_parameters(Tree::new(body) - .chain_err(|| "Failed to transform XML data into tree")? - .peel_layer("methodResponse")?) - .map(|parameters| XmlRpcResponse { parameters: parameters }) - } -} - fn extract_parameters(parameters: Tree) -> Result> { if let Tree::Node(key, parameters) = parameters { if key == "params" { @@ -476,44 +467,6 @@ mod tests { ); } - #[test] - fn reads_response() { - let data = r#" - - - - 33 - - - - 41 - 1 - - Hello - 0.5 - - - - -"#; - let mut cursor = std::io::Cursor::new(data.as_bytes()); - let value = XmlRpcResponse::new(&mut cursor).expect(BAD_DATA); - assert_eq!( - vec![ - XmlRpcValue::Int(33), - XmlRpcValue::Array(vec![ - XmlRpcValue::Int(41), - XmlRpcValue::Bool(true), - XmlRpcValue::Array(vec![ - XmlRpcValue::String(String::from("Hello")), - XmlRpcValue::Double(0.5), - ]), - ]), - ], - value.parameters - ); - } - #[test] fn writes_string() { assert_eq!( From 6c3ea12467b794e52e24678752c5ea024bf0dcd7 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Fri, 25 Aug 2017 02:09:34 +0200 Subject: [PATCH 03/50] Wrap ROS XML-RPC client into own layer --- src/api/error.rs | 21 +---------- src/api/master.rs | 81 +++-------------------------------------- src/api/ros.rs | 20 +++++----- src/rosxmlrpc/client.rs | 74 +++++++++++++++++++++++++++++++++++++ src/rosxmlrpc/mod.rs | 37 +++++++++++++++++++ 5 files changed, 128 insertions(+), 105 deletions(-) create mode 100644 src/rosxmlrpc/client.rs diff --git a/src/api/error.rs b/src/api/error.rs index a3a5b3e..e5fd5e3 100644 --- a/src/api/error.rs +++ b/src/api/error.rs @@ -1,18 +1,19 @@ pub use api::naming::error as naming; pub use rosxmlrpc::error as rosxmlrpc; pub use tcpros::error as tcpros; +pub use rosxmlrpc::ResponseError; error_chain! { foreign_links { Io(::std::io::Error); Nix(::nix::Error); FromUTF8(::std::string::FromUtf8Error); + Response(ResponseError); } links { XmlRpc(rosxmlrpc::Error, rosxmlrpc::ErrorKind); Tcpros(tcpros::Error, tcpros::ErrorKind); Naming(naming::Error, naming::ErrorKind); - Master(self::master::Error, self::master::ErrorKind); } errors { Duplicate(t: String) { @@ -36,21 +37,3 @@ pub mod api { } } } - -pub mod master { - use xml_rpc; - - error_chain! { - links { - XmlRpcErr(xml_rpc::error::Error, xml_rpc::error::ErrorKind); - XmlRpc(super::rosxmlrpc::Error, super::rosxmlrpc::ErrorKind); - Api(::error::api::Error, ::error::api::ErrorKind); - } - errors { - Fault(fault: xml_rpc::Fault) { - description("Queried XML-RPC server returned a fault") - display("Fault #{}: {}", fault.code, fault.message) - } - } - } -} diff --git a/src/api/master.rs b/src/api/master.rs index 3d68859..e4886a6 100644 --- a/src/api/master.rs +++ b/src/api/master.rs @@ -1,98 +1,40 @@ -use std; -use std::sync::Mutex; -use super::error::master::{Result, Error, ErrorKind}; use serde::{Deserialize, Serialize}; use super::value::Topic; +use super::super::rosxmlrpc::{self, Response as Result}; use xml_rpc; pub struct Master { - client: Mutex, + client: rosxmlrpc::Client, client_id: String, caller_api: String, - master_uri: String, -} - -const ERROR_CODE: i32 = -1; -const FAILURE_CODE: i32 = 0; -const SUCCESS_CODE: i32 = 1; - -fn parse_response(params: xml_rpc::Params) -> std::result::Result { - let mut param_iter = params.into_iter(); - let code = param_iter.next().ok_or_else(|| { - xml_rpc::Fault::new(FAILURE_CODE, "Server response missing arguments.") - })?; - let message = param_iter.next().ok_or_else(|| { - xml_rpc::Fault::new(FAILURE_CODE, "Server response missing arguments.") - })?; - let value = param_iter.next().ok_or_else(|| { - xml_rpc::Fault::new(FAILURE_CODE, "Server response missing arguments.") - })?; - let code = match code { - xml_rpc::Value::Int(v) => v, - _ => { - return Err(xml_rpc::Fault::new( - FAILURE_CODE, - "First response argument is expected to be int.", - )) - } - }; - let message = match message { - xml_rpc::Value::String(v) => v, - _ => { - return Err(xml_rpc::Fault::new( - FAILURE_CODE, - "Second response argument is expected to be string.", - )) - } - }; - if code != SUCCESS_CODE { - return Err(xml_rpc::Fault::new(code, message)); - } - Ok(value) } macro_rules! request { ($s:expr; $name:ident; $($item:expr),*)=> ({ - let params = xml_rpc::into_params(&(&$s.client_id, + $s.client.request(stringify!($name),&(&$s.client_id, $( $item, )* )) - .map_err(xml_rpc::error::Error::from)?; - let response = $s.client.lock().unwrap() - .call_value(&$s.master_uri.parse().unwrap(), stringify!($name), params)? - .map_err(to_api_error)?; - let data = parse_response(response).map_err(to_api_error)?; - Deserialize::deserialize(data).map_err(|v| { - to_api_error(xml_rpc::Fault::new( - FAILURE_CODE, - format!("Third response argument has unexpected structure: {}", v), - ))}) }) } macro_rules! request_tree { ($s:expr; $name:ident; $($item:expr),*)=> ({ - let params = xml_rpc::into_params(&(&$s.client_id, + $s.client.request_tree(stringify!($name),&(&$s.client_id, $( $item, )* )) - .map_err(xml_rpc::error::Error::from)?; - let response = $s.client.lock().unwrap() - .call_value(&$s.master_uri.parse().unwrap(), stringify!($name), params)? - .map_err(ErrorKind::Fault)?; - parse_response(response).map_err(to_api_error) }) } impl Master { pub fn new(master_uri: &str, client_id: &str, caller_api: &str) -> Master { Master { - client: Mutex::new(xml_rpc::Client::new().unwrap()), + client: rosxmlrpc::Client::new(master_uri).unwrap(), client_id: client_id.to_owned(), caller_api: caller_api.to_owned(), - master_uri: master_uri.to_owned(), } } @@ -191,19 +133,6 @@ impl Master { } } -fn to_api_error(v: xml_rpc::Fault) -> Error { - use super::error::api::ErrorKind as ApiErrorKind; - match v.code { - FAILURE_CODE => ErrorKind::Api(ApiErrorKind::SystemFail(v.message)), - ERROR_CODE => ErrorKind::Api(ApiErrorKind::BadData(v.message)), - x => ErrorKind::Api(ApiErrorKind::SystemFail(format!( - "Bad error code #{} returned with message: {}", - x, - v.message - ))), - }.into() -} - #[derive(Debug)] pub struct TopicData { pub name: String, diff --git a/src/api/ros.rs b/src/api/ros.rs index 903e060..80c7fc7 100644 --- a/src/api/ros.rs +++ b/src/api/ros.rs @@ -2,7 +2,7 @@ use serde::{Serialize, Deserialize}; use super::master::{self, Master}; use super::slave::Slave; use super::error::{ErrorKind, Result}; -use super::error::master::Result as MasterResult; +use super::super::rosxmlrpc::Response; use super::value::Topic; use super::naming::{self, Resolver}; use super::resolve; @@ -70,15 +70,15 @@ impl Ros { }) } - pub fn parameters(&self) -> MasterResult> { + pub fn parameters(&self) -> Response> { self.master.get_param_names() } - pub fn state(&self) -> MasterResult { + pub fn state(&self) -> Response { self.master.get_system_state().map(Into::into) } - pub fn topics(&self) -> MasterResult> { + pub fn topics(&self) -> Response> { self.master.get_topic_types().map(|v| { v.into_iter().map(Into::into).collect() }) @@ -175,27 +175,27 @@ impl<'a> Parameter<'a> { &self.name } - pub fn get<'b, T: Deserialize<'b>>(&self) -> MasterResult { + pub fn get<'b, T: Deserialize<'b>>(&self) -> Response { self.master.get_param::(&self.name) } - pub fn get_raw(&self) -> MasterResult { + pub fn get_raw(&self) -> Response { self.master.get_param_any(&self.name) } - pub fn set(&self, value: &T) -> MasterResult<()> { + pub fn set(&self, value: &T) -> Response<()> { self.master.set_param::(&self.name, value).and(Ok(())) } - pub fn delete(&self) -> MasterResult<()> { + pub fn delete(&self) -> Response<()> { self.master.delete_param(&self.name).and(Ok(())) } - pub fn exists(&self) -> MasterResult { + pub fn exists(&self) -> Response { self.master.has_param(&self.name) } - pub fn search(&self) -> MasterResult { + pub fn search(&self) -> Response { self.master.search_param(&self.name) } } diff --git a/src/rosxmlrpc/client.rs b/src/rosxmlrpc/client.rs new file mode 100644 index 0000000..b64498b --- /dev/null +++ b/src/rosxmlrpc/client.rs @@ -0,0 +1,74 @@ +use std::sync::Mutex; +use serde::{Deserialize, Serialize}; +use xml_rpc::{self, Value, Params}; +use super::{Response, ResponseError, ERROR_CODE, FAILURE_CODE, SUCCESS_CODE}; + +pub struct Client { + client: Mutex, + master_uri: String, +} + +impl Client { + pub fn new(master_uri: &str) -> Result { + Ok(Client { + client: Mutex::new(xml_rpc::Client::new()?), + master_uri: master_uri.to_owned(), + }) + } + + pub fn request_tree_with_tree(&self, name: &str, params: Params) -> Response { + let mut client = self.client.lock().map_err(|err| { + ResponseError::Client(format!("Failed to acquire lock on socket: {}", err)) + })?; + let mut response = client + .call_value(&self.master_uri.parse().unwrap(), name, params) + .map_err(|err| { + ResponseError::Client(format!("Failed to perform call to server: {}", err)) + })? + .map_err(|fault| { + ResponseError::Client(format!( + "Unexpected fault #{} received from server: {}", + fault.code, + fault.message + )) + })? + .into_iter(); + if let (Some(Value::Int(code)), Some(Value::String(message)), Some(data)) = + (response.next(), response.next(), response.next()) + { + match code { + ERROR_CODE => Err(ResponseError::Client(message)), + FAILURE_CODE => Err(ResponseError::Server(message)), + SUCCESS_CODE => Ok(data), + _ => Err(ResponseError::Server( + format!("Bad response code returned from server"), + )), + } + } else { + Err(ResponseError::Server(format!( + "Response with three parameters (int code, str msg, value) expected from server" + ))) + } + } + + pub fn request_tree(&self, name: &str, params: &S) -> Response + where + S: Serialize, + { + let params = xml_rpc::into_params(params).map_err(|err| { + ResponseError::Client(format!("Failed to serialize parameters: {}", err)) + })?; + self.request_tree_with_tree(name, params) + } + + pub fn request<'a, S, D>(&self, name: &str, params: &S) -> Response + where + S: Serialize, + D: Deserialize<'a>, + { + let data = self.request_tree(name, params)?; + Deserialize::deserialize(data).map_err(|err| { + ResponseError::Server(format!("Response data has unexpected structure: {}", err)) + }) + } +} diff --git a/src/rosxmlrpc/mod.rs b/src/rosxmlrpc/mod.rs index 9d70aef..177474e 100644 --- a/src/rosxmlrpc/mod.rs +++ b/src/rosxmlrpc/mod.rs @@ -1,6 +1,43 @@ +use std; +pub use self::client::Client; pub use self::server::Server; pub use self::serde::XmlRpcValue; +pub mod client; pub mod error; pub mod serde; pub mod server; + +pub type Response = Result; + +#[derive(Debug)] +pub enum ResponseError { + Client(String), + Server(String), +} + +impl std::fmt::Display for ResponseError { + fn fmt(&self, f: &mut std::fmt::Formatter) -> Result<(), std::fmt::Error> { + match *self { + ResponseError::Client(ref v) => write!(f, "Client error: {}", v), + ResponseError::Server(ref v) => write!(f, "Server error: {}", v), + } + } +} + +impl std::error::Error for ResponseError { + fn description(&self) -> &str { + match *self { + ResponseError::Client(ref v) => &v, + ResponseError::Server(ref v) => &v, + } + } + + fn cause(&self) -> Option<&std::error::Error> { + None + } +} + +const ERROR_CODE: i32 = -1; +const FAILURE_CODE: i32 = 0; +const SUCCESS_CODE: i32 = 1; From ff3870dea372c985da9de908f9784b8612080edf Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sun, 24 Sep 2017 14:55:59 +0200 Subject: [PATCH 04/50] Update XML-RPC dependency --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index b620bf6..5877fd7 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -19,7 +19,7 @@ rustc-serialize = "0.3" serde = "1.0.2" serde_derive = "1.0.2" serde_rosmsg = "0.2.0" -xml-rpc = "0.0.1" +xml-rpc = "0.0.2" xml-rs = "0.3" [dependencies.hyper] From d822a6b2e0d300935ac76d0688673b401c027397 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Mon, 25 Sep 2017 00:16:57 +0200 Subject: [PATCH 05/50] Switch Slave API to XML-RPC library --- src/api/slave.rs | 23 ++- src/api/slavehandler.rs | 397 ++++++++++++++++------------------------ src/rosxmlrpc/server.rs | 138 +++++--------- 3 files changed, 219 insertions(+), 339 deletions(-) diff --git a/src/api/slave.rs b/src/api/slave.rs index d2d771a..16482db 100644 --- a/src/api/slave.rs +++ b/src/api/slave.rs @@ -1,4 +1,3 @@ -use rosxmlrpc; use std::collections::HashMap; use std::sync::{Arc, Mutex}; use std::sync::mpsc::channel; @@ -19,21 +18,21 @@ type SerdeResult = Result; impl Slave { pub fn new(master_uri: &str, hostname: &str, port: u16, name: &str) -> Result { - let (shutdown_tx, shutdown_rx) = channel(); + use std::net::ToSocketAddrs; + + let (shutdown_tx, _shutdown_rx) = channel(); let handler = SlaveHandler::new(master_uri, hostname, name, shutdown_tx); let pubs = handler.publications.clone(); let subs = handler.subscriptions.clone(); let services = handler.services.clone(); - let mut server = rosxmlrpc::Server::new(hostname, port, handler)?; - let uri = server.uri.clone(); - thread::spawn(move || { - match shutdown_rx.recv() { - Ok(..) => info!("ROS Slave API shutdown by remote request"), - Err(..) => info!("ROS Slave API shutdown by ROS client destruction"), - }; - if let Err(err) = server.shutdown() { - info!("Error during ROS Slave API shutdown: {}", err); - } + // TODO: allow OS assigned port numbers + let uri = format!("http://{}:{}/", hostname, port); + let socket_addr = match (hostname, port).to_socket_addrs()?.next() { + Some(socket_addr) => socket_addr, + None => bail!("Bad address provided: {}:{}", hostname, port), + }; + thread::spawn(move || if let Err(err) = handler.run(&socket_addr) { + info!("Error during ROS Slave API initiation: {}", err); }); Ok(Slave { name: String::from(name), diff --git a/src/api/slavehandler.rs b/src/api/slavehandler.rs index c0c255e..22c792e 100644 --- a/src/api/slavehandler.rs +++ b/src/api/slavehandler.rs @@ -1,45 +1,18 @@ use nix::unistd::getpid; -use rosxmlrpc::XmlRpcValue; -use rosxmlrpc::server::{Answer, ParameterIterator, XmlRpcServer}; -use rustc_serialize::{Decodable, Encodable}; +use rosxmlrpc::{Response, ResponseError, Server}; +use std::net::SocketAddr; use std::collections::HashMap; use std::sync::{Arc, Mutex}; use std::sync::mpsc::Sender; use super::error::{self, ErrorKind, Result}; -use super::value::Topic; use tcpros::{Publisher, Subscriber, Service}; -use xml_rpc; +use xml_rpc::{self, Value}; pub struct SlaveHandler { pub subscriptions: Arc>>, pub publications: Arc>>, pub services: Arc>>, - hostname: String, - shutdown_signal: Arc>>, - master_uri: String, - name: String, -} - -impl XmlRpcServer for SlaveHandler { - fn handle( - &self, - method_name: &str, - mut req: ParameterIterator, - ) -> error::rosxmlrpc::serde::Result { - info!("Slave API method called: {}", method_name); - self.handle_call(method_name, &mut req) - } -} - -type HandleResult = Result<::std::result::Result>; - -macro_rules! pop{ - ($src:expr; $t:ty) => ({ - match pop::<$t>($src)? { - Ok(v) => v, - Err(err) => return Ok(Err(err)), - } - }) + server: Server, } impl SlaveHandler { @@ -47,200 +20,177 @@ impl SlaveHandler { master_uri: &str, hostname: &str, name: &str, - shutdown_signal: Sender<()>, + _shutdown_signal: Sender<()>, ) -> SlaveHandler { - SlaveHandler { - subscriptions: Arc::new(Mutex::new(HashMap::new())), - publications: Arc::new(Mutex::new(HashMap::new())), - services: Arc::new(Mutex::new(HashMap::new())), - master_uri: String::from(master_uri), - hostname: String::from(hostname), - name: String::from(name), - shutdown_signal: Arc::new(Mutex::new(shutdown_signal)), - } - } + let mut server = Server::default(); - fn handle_call( - &self, - method_name: &str, - req: &mut ParameterIterator, - ) -> error::rosxmlrpc::serde::Result { - match method_name { - "getBusStats" => encode_response(self.get_bus_stats(req), "Bus stats"), - "getBusInfo" => encode_response(self.get_bus_info(req), "Bus stats"), - "getMasterUri" => encode_response(self.get_master_uri(req), "Master URI"), - "shutdown" => encode_response(self.shutdown(req), "Shutdown"), - "getPid" => encode_response(self.get_pid(req), "PID"), - "getSubscriptions" => { - encode_response(self.get_subscriptions(req), "List of subscriptions") - } - "getPublications" => { - encode_response(self.get_publications(req), "List of publications") - } - "paramUpdate" => encode_response(self.param_update(req), "Parameter updated"), - "publisherUpdate" => encode_response(self.publisher_update(req), "Publishers updated"), - "requestTopic" => encode_response(self.request_topic(req), "Chosen protocol"), - name => encode_response::(Ok(Err(format!("Unimplemented method: {}", name))), ""), - } - } - fn get_bus_stats(&self, req: &mut ParameterIterator) -> HandleResult { - let caller_id = pop!(req; String); - if caller_id == "" { - return Ok(Err("Empty strings given".into())); - } - // TODO: implement actual stats displaying - Err("Method not implemented".into()) - } + server.register_value("getBusStats", "Bus stats", |_args| { + // TODO: implement actual stats displaying + Err(ResponseError::Server("Method not implemented".into())) + }); - fn get_bus_info(&self, req: &mut ParameterIterator) -> HandleResult> { - let caller_id = pop!(req; String); - if caller_id == "" { - return Ok(Err("Empty strings given".into())); - } - // TODO: implement actual info displaying - Err("Method not implemented".into()) - } + server.register_value("getBusInfo", "Bus info", |_args| { + // TODO: implement actual info displaying + Err(ResponseError::Server("Method not implemented".into())) + }); - fn param_update(&self, req: &mut ParameterIterator) -> HandleResult { - let caller_id = pop!(req; String); - let key = pop!(req; String); - // We don't do anything with parameter updates - let value = req.next(); - if let None = value { - return Ok(Err("Missing parameter".into())); - } - if caller_id == "" || key == "" { - return Ok(Err("Empty strings given".into())); - } - Ok(Ok(0)) - } + let master_uri_string = String::from(master_uri); - fn get_pid(&self, req: &mut ParameterIterator) -> HandleResult { - let caller_id = pop!(req; String); - if caller_id == "" { - return Ok(Err("Empty strings given".into())); - } - Ok(Ok(getpid())) - } + server.register_value("getMasterUri", "Master URI", move |_args| { + Ok(Value::String(master_uri_string.clone())) + }); - fn shutdown(&self, req: &mut ParameterIterator) -> HandleResult { - let caller_id = pop!(req; String); - let message = pop::(req).unwrap_or(Err("".into())).unwrap_or( - "".into(), - ); - if caller_id == "" { - return Ok(Err("Empty strings given".into())); - } - info!("Server is shutting down because: {}", message); - if let Err(..) = self.shutdown_signal.lock().expect(FAILED_TO_LOCK).send(()) { - bail!("Slave API is down already"); - } - Ok(Ok(0)) - } + server.register_value("shutdown", "Shutdown", |_args| { + // TODO: implement shutdown + Err(ResponseError::Server("Method not implemented".into())) + }); - fn get_publications(&self, req: &mut ParameterIterator) -> HandleResult> { - let caller_id = pop!(req; String); - if caller_id == "" { - return Ok(Err("Empty strings given".into())); - } - Ok(Ok( - self.publications - .lock() - .expect(FAILED_TO_LOCK) - .values() - .map(|ref v| { - return Topic { - name: v.topic.clone(), - datatype: v.msg_type.clone(), - }; - }) - .collect(), - )) - } + server.register_value("getPid", "PID", |_args| Ok(Value::Int(getpid()))); - fn get_subscriptions(&self, req: &mut ParameterIterator) -> HandleResult> { - let caller_id = pop!(req; String); - if caller_id == "" { - return Ok(Err("Empty strings given".into())); - } - Ok(Ok( - self.subscriptions - .lock() - .expect(FAILED_TO_LOCK) - .values() - .map(|ref v| { - return Topic { - name: v.topic.clone(), - datatype: v.msg_type.clone(), - }; + let subscriptions = Arc::new(Mutex::new(HashMap::::new())); + let subs = subscriptions.clone(); + + server.register_value("getSubscriptions", "List of subscriptions", move |_args| { + Ok(Value::Array( + subs.lock() + .expect(FAILED_TO_LOCK) + .values() + .map(|ref v| { + Value::Array(vec![ + Value::String(v.topic.clone()), + Value::String(v.msg_type.clone()), + ]) + }) + .collect(), + )) + }); + + let publications = Arc::new(Mutex::new(HashMap::::new())); + let pubs = publications.clone(); + + server.register_value("getPublications", "List of publications", move |_args| { + Ok(Value::Array( + pubs.lock() + .expect(FAILED_TO_LOCK) + .values() + .map(|ref v| { + Value::Array(vec![ + Value::String(v.topic.clone()), + Value::String(v.msg_type.clone()), + ]) + }) + .collect(), + )) + }); + + server.register_value("paramUpdate", "Parameter updated", |_args| { + // We don't do anything with parameter updates + Ok(Value::Int(0)) + }); + + let name_string = String::from(name); + let subs = subscriptions.clone(); + + server.register_value("publisherUpdate", "Publishers updated", move |args| { + let mut args = args.into_iter(); + let _caller_id = args.next().ok_or(ResponseError::Client( + "Missing argument 'caller_id'".into(), + ))?; + let topic = match args.next() { + Some(Value::String(topic)) => topic, + _ => return Err(ResponseError::Client("Missing argument 'topic'".into())), + }; + let publishers = match args.next() { + Some(Value::Array(publishers)) => publishers, + _ => { + return Err(ResponseError::Client( + "Missing argument 'publishers'".into(), + )) + } + }; + let publishers = publishers + .into_iter() + .map(|v| match v { + Value::String(x) => Ok(x), + _ => Err(ResponseError::Client( + "Publishers need to be strings".into(), + )), }) - .collect(), - )) - } + .collect::>>()?; - fn publisher_update(&self, req: &mut ParameterIterator) -> HandleResult { - let caller_id = pop!(req; String); - let topic = pop!(req; String); - let publishers = pop!(req; Vec); - if caller_id == "" || topic == "" || publishers.iter().any(|ref x| x.as_str() == "") { - return Ok(Err("Empty strings given".into())); - } - add_publishers_to_subscription( - &mut self.subscriptions.lock().expect(FAILED_TO_LOCK), - &self.name, - &topic, - publishers.into_iter(), - ).and(Ok(Ok(0))) - } + add_publishers_to_subscription( + &mut subs.lock().expect(FAILED_TO_LOCK), + &name_string, + &topic, + publishers.into_iter(), + ).map_err(|v| { + ResponseError::Server(format!("Failed to handle publishers: {}", v)) + })?; + Ok(Value::Int(0)) + }); - fn get_master_uri(&self, req: &mut ParameterIterator) -> HandleResult<&str> { - let caller_id = pop!(req; String); - if caller_id == "" { - return Ok(Err("Empty strings given".into())); - } - Ok(Ok(&self.master_uri)) - } + let hostname_string = String::from(hostname); + let pubs = publications.clone(); - fn request_topic(&self, req: &mut ParameterIterator) -> HandleResult<(String, String, i32)> { - let caller_id = pop!(req; String); - let topic = pop!(req; String); - let protocols = match req.next() { - Some(v) => v.value(), - None => return Ok(Err("Missing parameter".into())), - }; - let (ip, port) = match self.publications.lock().expect(FAILED_TO_LOCK).get(&topic) { - Some(publisher) => (self.hostname.clone(), publisher.port as i32), - None => { - return Ok(Err("Requested topic not published by node".into())); - } - }; - if caller_id == "" || topic == "" { - return Ok(Err("Empty strings given".into())); - } - let protocols = match protocols { - XmlRpcValue::Array(protocols) => protocols, - _ => { - return Ok(Err( - "Protocols need to be provided as [ [String, \ - XmlRpcLegalValue] ]" - .into(), - )); - } - }; - let mut has_tcpros = false; - for protocol in protocols { - if let XmlRpcValue::Array(protocol) = protocol { - if let Some(&XmlRpcValue::String(ref name)) = protocol.get(0) { - has_tcpros |= name == "TCPROS"; + server.register_value("requestTopic", "Chosen protocol", move |args| { + let mut args = args.into_iter(); + let _caller_id = args.next().ok_or(ResponseError::Client( + "Missing argument 'caller_id'".into(), + ))?; + let topic = match args.next() { + Some(Value::String(topic)) => topic, + _ => return Err(ResponseError::Client("Missing argument 'topic'".into())), + }; + let protocols = match args.next() { + Some(Value::Array(protocols)) => protocols, + Some(_) => { + return Err(ResponseError::Client( + "Protocols need to be provided as [ [String, XmlRpcLegalValue] ]" + .into(), + )) + } + None => return Err(ResponseError::Client("Missing argument 'protocols'".into())), + }; + let (ip, port) = match pubs.lock().expect(FAILED_TO_LOCK).get(&topic) { + Some(publisher) => (hostname_string.clone(), publisher.port as i32), + None => { + return Err(ResponseError::Client( + "Requested topic not published by node".into(), + )); + } + }; + let mut has_tcpros = false; + for protocol in protocols { + if let Value::Array(protocol) = protocol { + if let Some(&Value::String(ref name)) = protocol.get(0) { + has_tcpros |= name == "TCPROS"; + } } } + if has_tcpros { + Ok(Value::Array(vec![ + Value::String("TCPROS".into()), + Value::String(ip), + Value::Int(port), + ])) + } else { + Err(ResponseError::Server( + "No matching protocols available".into(), + )) + } + }); + + SlaveHandler { + subscriptions: subscriptions, + publications: publications, + services: Arc::new(Mutex::new(HashMap::new())), + server: server, } - if has_tcpros { - Ok(Ok((String::from("TCPROS"), ip, port))) - } else { - Ok(Err("No matching protocols available".into())) - } + } + + pub fn run(self, addr: &SocketAddr) -> xml_rpc::error::Result<()> { + self.server.run(addr) } } @@ -268,35 +218,6 @@ where Ok(()) } -fn encode_response( - response: HandleResult, - message: &str, -) -> error::rosxmlrpc::serde::Result { - use std::error::Error; - let mut res = Answer::new(); - match response { - Ok(value) => { - match value { - // Success - Ok(value) => res.add(&(1i32, message, value)), - // Bad request provided - Err(err) => res.add(&(-1i32, err, 0)), - } - } - // System failure while handling request - Err(err) => res.add(&(0i32, err.description(), 0)), - }.map(|_| res) -} - - -fn pop(req: &mut ParameterIterator) -> HandleResult { - Ok(Ok(match req.next() { - Some(v) => v, - None => return Ok(Err("Missing parameter".into())), - }.read::() - .map_err(|v| error::rosxmlrpc::Error::from(v))?)) -} - fn connect_to_publisher( subscriber: &mut Subscriber, caller_id: &str, diff --git a/src/rosxmlrpc/server.rs b/src/rosxmlrpc/server.rs index 713ab9b..adc1225 100644 --- a/src/rosxmlrpc/server.rs +++ b/src/rosxmlrpc/server.rs @@ -1,109 +1,69 @@ -use hyper; -use hyper::server::{Request, Response, Handler}; -use rustc_serialize::{Decodable, Encodable}; -use std; -use super::serde; -use super::error::{self, Result}; +use std::net::SocketAddr; +use xml_rpc; + +use super::{ERROR_CODE, FAILURE_CODE, SUCCESS_CODE, Response, ResponseError}; pub struct Server { - listener: hyper::server::Listening, - pub uri: String, + server: xml_rpc::Server, +} + +impl Default for Server { + fn default() -> Self { + let mut server = xml_rpc::Server::default(); + server.set_on_missing(on_missing); + Server { server: server } + } } impl Server { - pub fn new(hostname: &str, port: u16, responder: T) -> Result + pub fn register_value(&mut self, name: K, msg: &'static str, handler: T) where - T: 'static + XmlRpcServer + Send + Sync, + K: Into, + T: Fn(xml_rpc::Params) -> Response + Send + Sync + 'static, { - let listener = hyper::Server::http((hostname, port))?.handle( - XmlRpcHandler::new( - responder, - ), - )?; - let uri = format!("http://{}:{}/", hostname, listener.socket.port()); - Ok(Server { - listener: listener, - uri: uri, + self.server.register_value(name, move |args| { + Ok(response_to_params(msg, handler(args))) }) } - pub fn shutdown(&mut self) -> hyper::Result<()> { - self.listener.close() + pub fn run(self, uri: &SocketAddr) -> xml_rpc::error::Result<()> { + self.server.run(uri) } } -pub type ParameterIterator = std::iter::Map< - std::vec::IntoIter, - fn(serde::decoder::Decoder) -> Parameter, ->; - -pub trait XmlRpcServer { - fn handle(&self, method_name: &str, params: ParameterIterator) -> error::serde::Result; -} - -struct XmlRpcHandler { - handler: T, -} - -impl XmlRpcHandler { - fn new(handler: T) -> XmlRpcHandler { - XmlRpcHandler { handler: handler } - } - - fn process(&self, req: Request, res: Response) -> Result<()> { - let (method_name, parameters) = serde::Decoder::new_request(req)?; - res.send(&self.handler - .handle(&method_name, parameters.into_iter().map(Parameter::new))? - .write_response()?)?; - Ok(()) - } -} - -impl Handler for XmlRpcHandler { - fn handle(&self, req: Request, res: Response) { - if let Err(err) = self.process(req, res) { - let info = err.iter() - .map(|v| format!("{}", v)) - .collect::>() - .join("\nCaused by:"); - error!("Server handler error: {}", info); +fn response_to_params(msg: &str, response: Response) -> xml_rpc::Params { + use xml_rpc::Value; + match response { + Ok(v) => vec![Value::Int(SUCCESS_CODE), Value::String(msg.into()), v], + Err(ResponseError::Client(err)) => { + vec![ + Value::Int(ERROR_CODE), + Value::String(err.into()), + Value::Int(0), + ] + } + Err(ResponseError::Server(err)) => { + vec![ + Value::Int(FAILURE_CODE), + Value::String(err.into()), + Value::Int(0), + ] } } } -pub struct Answer { - encoder: serde::Encoder, -} - -impl Answer { - pub fn new() -> Answer { - Answer { encoder: serde::Encoder::new() } - } - - pub fn add(&mut self, data: &T) -> error::serde::Result<()> { - data.encode(&mut self.encoder) - } - - fn write_response(self) -> std::io::Result> { - let mut data = vec![]; - self.encoder.write_response(&mut data).and(Ok(data)) - } -} - -pub struct Parameter { - decoder: serde::Decoder, +fn error_response(message: T) -> xml_rpc::Params +where + T: Into, +{ + use xml_rpc::Value; + vec![ + Value::Int(ERROR_CODE), + Value::String(message.into()), + Value::Int(0), + ] } -impl Parameter { - fn new(decoder: serde::Decoder) -> Parameter { - Parameter { decoder: decoder } - } - - pub fn read(mut self) -> error::serde::Result { - T::decode(&mut self.decoder) - } - - pub fn value(self) -> serde::XmlRpcValue { - self.decoder.value() - } +fn on_missing(_params: xml_rpc::Params) -> xml_rpc::Response { + Ok(error_response("Bad method requested")) } From 0987a5466a253b4344ca8caa7152b8076178f790 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Fri, 29 Sep 2017 20:21:33 +0200 Subject: [PATCH 06/50] Allow OS assigned port numbers --- Cargo.toml | 2 +- src/api/slave.rs | 27 ++++++++++++++++++++++----- src/api/slavehandler.rs | 4 ++-- src/rosxmlrpc/server.rs | 4 ++-- 4 files changed, 27 insertions(+), 10 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 5877fd7..006d754 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -19,7 +19,7 @@ rustc-serialize = "0.3" serde = "1.0.2" serde_derive = "1.0.2" serde_rosmsg = "0.2.0" -xml-rpc = "0.0.2" +xml-rpc = "0.0.3" xml-rs = "0.3" [dependencies.hyper] diff --git a/src/api/slave.rs b/src/api/slave.rs index 16482db..cd3fc2a 100644 --- a/src/api/slave.rs +++ b/src/api/slave.rs @@ -2,7 +2,7 @@ use std::collections::HashMap; use std::sync::{Arc, Mutex}; use std::sync::mpsc::channel; use std::thread; -use super::error::{self, ErrorKind, Result}; +use super::error::{self, ErrorKind, Result, ResultExt}; use super::slavehandler::{add_publishers_to_subscription, SlaveHandler}; use tcpros::{Message, Publisher, PublisherStream, Subscriber, Service, ServicePair, ServiceResult}; @@ -25,15 +25,32 @@ impl Slave { let pubs = handler.publications.clone(); let subs = handler.subscriptions.clone(); let services = handler.services.clone(); - // TODO: allow OS assigned port numbers - let uri = format!("http://{}:{}/", hostname, port); + let (port_tx, port_rx) = channel(); let socket_addr = match (hostname, port).to_socket_addrs()?.next() { Some(socket_addr) => socket_addr, None => bail!("Bad address provided: {}:{}", hostname, port), }; - thread::spawn(move || if let Err(err) = handler.run(&socket_addr) { - info!("Error during ROS Slave API initiation: {}", err); + + thread::spawn(move || { + let bound_handler = match handler.bind(&socket_addr) { + Ok(v) => v, + Err(err) => { + port_tx.send(Err(err)).expect(FAILED_TO_LOCK); + return; + } + }; + let port = bound_handler.local_addr().map(|v| v.port()); + port_tx.send(port).expect(FAILED_TO_LOCK); + if let Err(err) = bound_handler.run() { + info!("Error during ROS Slave API initiation: {}", err); + } }); + + let port = port_rx.recv().expect(FAILED_TO_LOCK).chain_err( + || "Failed to get port", + )?; + let uri = format!("http://{}:{}/", hostname, port); + Ok(Slave { name: String::from(name), uri: uri, diff --git a/src/api/slavehandler.rs b/src/api/slavehandler.rs index 22c792e..6ab038b 100644 --- a/src/api/slavehandler.rs +++ b/src/api/slavehandler.rs @@ -189,8 +189,8 @@ impl SlaveHandler { } } - pub fn run(self, addr: &SocketAddr) -> xml_rpc::error::Result<()> { - self.server.run(addr) + pub fn bind(self, addr: &SocketAddr) -> xml_rpc::error::Result { + self.server.bind(addr) } } diff --git a/src/rosxmlrpc/server.rs b/src/rosxmlrpc/server.rs index adc1225..052395a 100644 --- a/src/rosxmlrpc/server.rs +++ b/src/rosxmlrpc/server.rs @@ -26,8 +26,8 @@ impl Server { }) } - pub fn run(self, uri: &SocketAddr) -> xml_rpc::error::Result<()> { - self.server.run(uri) + pub fn bind(self, uri: &SocketAddr) -> xml_rpc::error::Result { + self.server.bind(uri) } } From d4e76911f26cc78628fabfb8226ef01ae8bbaf83 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Fri, 29 Sep 2017 21:06:18 +0200 Subject: [PATCH 07/50] Implement shutdown method --- Cargo.toml | 2 ++ src/api/slave.rs | 7 +++++-- src/api/slavehandler.rs | 26 +++++++++++++++++++++----- src/lib.rs | 1 + 4 files changed, 29 insertions(+), 7 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 006d754..43b770b 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -6,9 +6,11 @@ name = "rosrust" readme = "README.md" repository = "https://github.com/adnanademovic/rosrust" version = "0.5.0" + [dependencies] byteorder = "1.0.0" error-chain = "0.10.0" +futures = "0.1.16" itertools = "0.6.0" lazy_static = "0.2" log = "0.3" diff --git a/src/api/slave.rs b/src/api/slave.rs index cd3fc2a..2f77e5a 100644 --- a/src/api/slave.rs +++ b/src/api/slave.rs @@ -1,3 +1,4 @@ +use futures::sync::mpsc::channel as futures_channel; use std::collections::HashMap; use std::sync::{Arc, Mutex}; use std::sync::mpsc::channel; @@ -19,8 +20,9 @@ type SerdeResult = Result; impl Slave { pub fn new(master_uri: &str, hostname: &str, port: u16, name: &str) -> Result { use std::net::ToSocketAddrs; + use futures::{Future, Stream}; - let (shutdown_tx, _shutdown_rx) = channel(); + let (shutdown_tx, shutdown_rx) = futures_channel(1); let handler = SlaveHandler::new(master_uri, hostname, name, shutdown_tx); let pubs = handler.publications.clone(); let subs = handler.subscriptions.clone(); @@ -41,7 +43,8 @@ impl Slave { }; let port = bound_handler.local_addr().map(|v| v.port()); port_tx.send(port).expect(FAILED_TO_LOCK); - if let Err(err) = bound_handler.run() { + let shutdown_future = shutdown_rx.into_future().map(|_| ()).map_err(|_| ()); + if let Err(err) = bound_handler.run_until(shutdown_future) { info!("Error during ROS Slave API initiation: {}", err); } }); diff --git a/src/api/slavehandler.rs b/src/api/slavehandler.rs index 6ab038b..b02f9c9 100644 --- a/src/api/slavehandler.rs +++ b/src/api/slavehandler.rs @@ -3,7 +3,7 @@ use rosxmlrpc::{Response, ResponseError, Server}; use std::net::SocketAddr; use std::collections::HashMap; use std::sync::{Arc, Mutex}; -use std::sync::mpsc::Sender; +use futures::sync::mpsc::Sender; use super::error::{self, ErrorKind, Result}; use tcpros::{Publisher, Subscriber, Service}; use xml_rpc::{self, Value}; @@ -20,8 +20,10 @@ impl SlaveHandler { master_uri: &str, hostname: &str, name: &str, - _shutdown_signal: Sender<()>, + shutdown_signal: Sender<()>, ) -> SlaveHandler { + use futures::Sink; + let mut server = Server::default(); @@ -41,9 +43,23 @@ impl SlaveHandler { Ok(Value::String(master_uri_string.clone())) }); - server.register_value("shutdown", "Shutdown", |_args| { - // TODO: implement shutdown - Err(ResponseError::Server("Method not implemented".into())) + server.register_value("shutdown", "Shutdown", move |args| { + let mut args = args.into_iter(); + let _caller_id = args.next().ok_or(ResponseError::Client( + "Missing argument 'caller_id'".into(), + ))?; + let message = match args.next() { + Some(Value::String(message)) => message, + _ => return Err(ResponseError::Client("Missing argument 'message'".into())), + }; + info!("Server is shutting down because: {}", message); + match shutdown_signal.clone().wait().send(()) { + Ok(()) => Ok(Value::Int(0)), + Err(err) => { + error!("Shutdown error: {:?}", err); + Err(ResponseError::Server("Failed to shut down".into())) + } + } }); server.register_value("getPid", "PID", |_args| Ok(Value::Int(getpid()))); diff --git a/src/lib.rs b/src/lib.rs index 5b72341..2c169b3 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -4,6 +4,7 @@ extern crate byteorder; extern crate crypto; #[macro_use] extern crate error_chain; +extern crate futures; extern crate hyper; extern crate itertools; #[macro_use] From 6b9c724faedfbc7b1f0c6ee59e047cffbeb40b5a Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Fri, 29 Sep 2017 21:17:31 +0200 Subject: [PATCH 08/50] Remove dead code Fixes #19, as it's one of the dropped dependencies --- Cargo.toml | 6 - src/api/master.rs | 6 +- src/api/mod.rs | 1 - src/api/ros.rs | 3 +- src/api/slavehandler.rs | 14 +- src/api/value.rs | 5 - src/lib.rs | 4 - src/rosxmlrpc/error.rs | 6 - src/rosxmlrpc/mod.rs | 2 - src/rosxmlrpc/serde/decoder.rs | 493 ------------------------ src/rosxmlrpc/serde/encoder.rs | 586 ---------------------------- src/rosxmlrpc/serde/error.rs | 20 - src/rosxmlrpc/serde/mod.rs | 8 - src/rosxmlrpc/serde/value.rs | 670 --------------------------------- 14 files changed, 13 insertions(+), 1811 deletions(-) delete mode 100644 src/api/value.rs delete mode 100644 src/rosxmlrpc/serde/decoder.rs delete mode 100644 src/rosxmlrpc/serde/encoder.rs delete mode 100644 src/rosxmlrpc/serde/error.rs delete mode 100644 src/rosxmlrpc/serde/mod.rs delete mode 100644 src/rosxmlrpc/serde/value.rs diff --git a/Cargo.toml b/Cargo.toml index 43b770b..0eb5f95 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -17,13 +17,7 @@ log = "0.3" nix = "0.7" regex = "0.2.1" rust-crypto = "0.2.36" -rustc-serialize = "0.3" serde = "1.0.2" serde_derive = "1.0.2" serde_rosmsg = "0.2.0" xml-rpc = "0.0.3" -xml-rs = "0.3" - -[dependencies.hyper] -default-features = false -version = "0.9" diff --git a/src/api/master.rs b/src/api/master.rs index e4886a6..3c61006 100644 --- a/src/api/master.rs +++ b/src/api/master.rs @@ -1,5 +1,4 @@ use serde::{Deserialize, Serialize}; -use super::value::Topic; use super::super::rosxmlrpc::{self, Response as Result}; use xml_rpc; @@ -181,3 +180,8 @@ impl Into for TopicTuple { } } } + +pub struct Topic { + pub name: String, + pub datatype: String, +} diff --git a/src/api/mod.rs b/src/api/mod.rs index 5dc8992..e984897 100644 --- a/src/api/mod.rs +++ b/src/api/mod.rs @@ -1,6 +1,5 @@ pub use self::ros::Ros; -mod value; pub mod error; mod master; mod slave; diff --git a/src/api/ros.rs b/src/api/ros.rs index 80c7fc7..2cbe41b 100644 --- a/src/api/ros.rs +++ b/src/api/ros.rs @@ -1,9 +1,8 @@ use serde::{Serialize, Deserialize}; -use super::master::{self, Master}; +use super::master::{self, Master, Topic}; use super::slave::Slave; use super::error::{ErrorKind, Result}; use super::super::rosxmlrpc::Response; -use super::value::Topic; use super::naming::{self, Resolver}; use super::resolve; use tcpros::{Client, Message, PublisherStream, ServicePair, ServiceResult}; diff --git a/src/api/slavehandler.rs b/src/api/slavehandler.rs index b02f9c9..cffaf71 100644 --- a/src/api/slavehandler.rs +++ b/src/api/slavehandler.rs @@ -269,21 +269,21 @@ fn request_topic( Ok(protocols) } -#[derive(RustcEncodable)] +#[allow(dead_code)] pub struct BusStats { pub publish: Vec, pub subscribe: Vec, pub service: ServiceStats, } -#[derive(RustcEncodable)] +#[allow(dead_code)] pub struct PublishStats { pub name: String, pub data_sent: String, pub connection_data: PublishConnectionData, } -#[derive(RustcEncodable)] +#[allow(dead_code)] pub struct PublishConnectionData { pub connection_id: String, pub bytes_sent: i32, @@ -291,13 +291,13 @@ pub struct PublishConnectionData { pub connected: bool, } -#[derive(RustcEncodable)] +#[allow(dead_code)] pub struct SubscribeStats { pub name: String, pub connection_data: SubscribeConnectionData, } -#[derive(RustcEncodable)] +#[allow(dead_code)] pub struct SubscribeConnectionData { pub connection_id: String, pub bytes_received: i32, @@ -305,14 +305,14 @@ pub struct SubscribeConnectionData { pub connected: bool, } -#[derive(RustcEncodable)] +#[allow(dead_code)] pub struct ServiceStats { pub number_of_requests: i32, pub bytes_received: i32, pub bytes_sent: i32, } -#[derive(RustcEncodable)] +#[allow(dead_code)] pub struct BusInfo { pub connection_id: String, pub destination_id: String, diff --git a/src/api/value.rs b/src/api/value.rs deleted file mode 100644 index 42f4380..0000000 --- a/src/api/value.rs +++ /dev/null @@ -1,5 +0,0 @@ -#[derive(Debug, RustcDecodable, RustcEncodable)] -pub struct Topic { - pub name: String, - pub datatype: String, -} diff --git a/src/lib.rs b/src/lib.rs index 2c169b3..057d8d5 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -5,7 +5,6 @@ extern crate crypto; #[macro_use] extern crate error_chain; extern crate futures; -extern crate hyper; extern crate itertools; #[macro_use] extern crate lazy_static; @@ -13,18 +12,15 @@ extern crate lazy_static; extern crate log; extern crate nix; extern crate regex; -extern crate rustc_serialize; extern crate serde; #[macro_use] extern crate serde_derive; extern crate serde_rosmsg; -extern crate xml; extern crate xml_rpc; pub use api::Ros; pub use tcpros::{Client, PublisherStream, Message, ServicePair as Service}; pub use api::error; -pub use rosxmlrpc::XmlRpcValue; mod api; #[macro_use] diff --git a/src/rosxmlrpc/error.rs b/src/rosxmlrpc/error.rs index f487ef1..b7f9385 100644 --- a/src/rosxmlrpc/error.rs +++ b/src/rosxmlrpc/error.rs @@ -1,12 +1,6 @@ -pub use rosxmlrpc::serde::error as serde; - error_chain! { foreign_links { - Fmt(::hyper::error::Error); Io(::std::io::Error); Utf8(::std::string::FromUtf8Error); } - links { - Serde(serde::Error, serde::ErrorKind); - } } diff --git a/src/rosxmlrpc/mod.rs b/src/rosxmlrpc/mod.rs index 177474e..cce8644 100644 --- a/src/rosxmlrpc/mod.rs +++ b/src/rosxmlrpc/mod.rs @@ -1,11 +1,9 @@ use std; pub use self::client::Client; pub use self::server::Server; -pub use self::serde::XmlRpcValue; pub mod client; pub mod error; -pub mod serde; pub mod server; pub type Response = Result; diff --git a/src/rosxmlrpc/serde/decoder.rs b/src/rosxmlrpc/serde/decoder.rs deleted file mode 100644 index e97e398..0000000 --- a/src/rosxmlrpc/serde/decoder.rs +++ /dev/null @@ -1,493 +0,0 @@ -use rustc_serialize; -use std; -use super::error::{ErrorKind, ResultExt, Result}; -use super::value; - -pub struct Decoder { - value: value::XmlRpcValue, - chain: std::vec::IntoIter<(value::XmlRpcValue, usize)>, -} - -impl Decoder { - pub fn new(body: value::XmlRpcValue) -> Decoder { - let mut chain = vec![]; - append_elements(&mut chain, &body); - Decoder { - value: body, - chain: chain.into_iter(), - } - } - - pub fn new_request(body: T) -> Result<(String, Vec)> { - value::XmlRpcRequest::new(body) - .chain_err(|| ErrorKind::XmlRpcReading("request".into())) - .map(|value| { - ( - value.method, - value.parameters.into_iter().map(Decoder::new).collect(), - ) - }) - } - - pub fn value(self) -> value::XmlRpcValue { - self.value - } - - fn read_tuple_helper(&mut self, l: usize, f: F) -> Result - where - F: FnOnce(&mut Self) -> Result, - { - if let Some((value::XmlRpcValue::Array(_), len)) = self.chain.next() { - if l == len { - f(self) - } else { - Err( - ErrorKind::MismatchedDataFormat(format!("an array of length {}", len)).into(), - ) - } - } else { - Err( - ErrorKind::MismatchedDataFormat("an array field".into()).into(), - ) - } - } -} - -fn append_elements(array: &mut Vec<(value::XmlRpcValue, usize)>, v: &value::XmlRpcValue) { - if let &value::XmlRpcValue::Array(ref children) = v { - array.push((value::XmlRpcValue::Array(vec![]), children.len())); - for child in children { - append_elements(array, child); - } - } else { - array.push((v.clone(), 0)); - } -} - -impl rustc_serialize::Decoder for Decoder { - type Error = super::error::Error; - - fn read_nil(&mut self) -> Result<()> { - bail!(ErrorKind::UnsupportedDataType("nil".into())) - } - - fn read_usize(&mut self) -> Result { - bail!(ErrorKind::UnsupportedDataType("usize".into())) - } - - fn read_u64(&mut self) -> Result { - bail!(ErrorKind::UnsupportedDataType("u64".into())) - } - - fn read_u32(&mut self) -> Result { - bail!(ErrorKind::UnsupportedDataType("u32".into())) - } - - fn read_u16(&mut self) -> Result { - bail!(ErrorKind::UnsupportedDataType("u16".into())) - } - - fn read_u8(&mut self) -> Result { - bail!(ErrorKind::UnsupportedDataType("u8".into())) - } - - fn read_isize(&mut self) -> Result { - bail!(ErrorKind::UnsupportedDataType("isize".into())) - } - - fn read_i64(&mut self) -> Result { - bail!(ErrorKind::UnsupportedDataType("i64".into())) - } - - fn read_i32(&mut self) -> Result { - if let Some((value::XmlRpcValue::Int(v), _)) = self.chain.next() { - Ok(v) - } else { - Err( - ErrorKind::MismatchedDataFormat("an integer (i32) field".into()).into(), - ) - } - } - - fn read_i16(&mut self) -> Result { - bail!(ErrorKind::UnsupportedDataType("i16".into())) - } - - fn read_i8(&mut self) -> Result { - bail!(ErrorKind::UnsupportedDataType("i8".into())) - } - - fn read_bool(&mut self) -> Result { - if let Some((value::XmlRpcValue::Bool(v), _)) = self.chain.next() { - Ok(v) - } else { - Err( - ErrorKind::MismatchedDataFormat("a boolean field".into()).into(), - ) - } - } - - fn read_f64(&mut self) -> Result { - if let Some((value::XmlRpcValue::Double(v), _)) = self.chain.next() { - Ok(v) - } else { - Err( - ErrorKind::MismatchedDataFormat("a double (f64) field".into()).into(), - ) - } - } - - fn read_f32(&mut self) -> Result { - bail!(ErrorKind::UnsupportedDataType("f32".into())) - } - - fn read_char(&mut self) -> Result { - bail!(ErrorKind::UnsupportedDataType("char".into())) - } - - fn read_str(&mut self) -> Result { - if let Some((value::XmlRpcValue::String(v), _)) = self.chain.next() { - Ok(v) - } else { - Err( - ErrorKind::MismatchedDataFormat("a string field".into()).into(), - ) - } - } - - fn read_enum(&mut self, _: &str, _: F) -> Result - where - F: FnOnce(&mut Self) -> Result, - { - bail!(ErrorKind::UnsupportedDataType("enum".into())) - } - - fn read_enum_variant(&mut self, _: &[&str], _: F) -> Result - where - F: FnMut(&mut Self, usize) -> Result, - { - bail!(ErrorKind::UnsupportedDataType("enum variant".into())) - } - - fn read_enum_variant_arg(&mut self, _: usize, _: F) -> Result - where - F: FnOnce(&mut Self) -> Result, - { - bail!(ErrorKind::UnsupportedDataType( - "enum variant argument".into(), - )) - } - - fn read_enum_struct_variant(&mut self, _: &[&str], _: F) -> Result - where - F: FnMut(&mut Self, usize) -> Result, - { - bail!(ErrorKind::UnsupportedDataType("enum struct variant".into())) - } - - fn read_enum_struct_variant_field(&mut self, _: &str, _: usize, _: F) -> Result - where - F: FnOnce(&mut Self) -> Result, - { - bail!(ErrorKind::UnsupportedDataType( - "enum struct variant field".into(), - )) - } - - fn read_struct(&mut self, name: &str, size: usize, f: F) -> Result - where - F: FnOnce(&mut Self) -> Result, - { - self.read_tuple_helper(size, f).chain_err(|| { - ErrorKind::Decoding(format!("struct {}", name)) - }) - } - - fn read_struct_field(&mut self, name: &str, _: usize, f: F) -> Result - where - F: FnOnce(&mut Self) -> Result, - { - f(self).chain_err(|| ErrorKind::Decoding(format!("field {}", name))) - } - - fn read_tuple(&mut self, size: usize, f: F) -> Result - where - F: FnOnce(&mut Self) -> Result, - { - self.read_tuple_helper(size, f).chain_err(|| { - ErrorKind::Decoding("tuple".into()) - }) - } - - fn read_tuple_arg(&mut self, n: usize, f: F) -> Result - where - F: FnOnce(&mut Self) -> Result, - { - f(self).chain_err(|| ErrorKind::Decoding(format!("field number {}", n))) - } - - fn read_tuple_struct(&mut self, name: &str, size: usize, f: F) -> Result - where - F: FnOnce(&mut Self) -> Result, - { - self.read_tuple_helper(size, f).chain_err(|| { - ErrorKind::Decoding(format!("tuple struct {}", name)) - }) - } - - fn read_tuple_struct_arg(&mut self, n: usize, f: F) -> Result - where - F: FnOnce(&mut Self) -> Result, - { - f(self).chain_err(|| ErrorKind::Decoding(format!("field number {}", n))) - } - - fn read_option(&mut self, _: F) -> Result - where - F: FnMut(&mut Self, bool) -> Result, - { - bail!(ErrorKind::UnsupportedDataType("option".into())) - } - - fn read_seq(&mut self, f: F) -> Result - where - F: FnOnce(&mut Self, usize) -> Result, - { - if let Some((value::XmlRpcValue::Array(_), len)) = self.chain.next() { - f(self, len).chain_err(|| ErrorKind::Decoding("array".into())) - } else { - Err( - ErrorKind::MismatchedDataFormat("an array field".into()).into(), - ) - } - } - - fn read_seq_elt(&mut self, n: usize, f: F) -> Result - where - F: FnOnce(&mut Self) -> Result, - { - f(self).chain_err(|| ErrorKind::Decoding(format!("element number {}", n))) - } - - fn read_map(&mut self, _: F) -> Result - where - F: FnOnce(&mut Self, usize) -> Result, - { - bail!(ErrorKind::UnsupportedDataType("map".into())) - } - - fn read_map_elt_key(&mut self, _: usize, _: F) -> Result - where - F: FnOnce(&mut Self) -> Result, - { - bail!(ErrorKind::UnsupportedDataType("map element key".into())) - } - - fn read_map_elt_val(&mut self, _: usize, _: F) -> Result - where - F: FnOnce(&mut Self) -> Result, - { - bail!(ErrorKind::UnsupportedDataType("map element value".into())) - } - - fn error(&mut self, s: &str) -> Self::Error { - s.into() - } -} - -#[cfg(test)] -mod tests { - use super::*; - use super::super::XmlRpcValue; - use rustc_serialize::Decodable; - use std; - - static FAILED_TO_DECODE: &'static str = "Failed to decode"; - - #[test] - fn reads_string() { - assert_eq!( - "First test", - String::decode(&mut Decoder::new( - XmlRpcValue::String(String::from("First test")), - )).expect(FAILED_TO_DECODE) - ); - } - - #[test] - fn reads_int() { - assert_eq!( - 41, - i32::decode(&mut Decoder::new(XmlRpcValue::Int(41))).expect(FAILED_TO_DECODE) - ); - } - - #[test] - fn reads_float() { - assert_eq!( - 32.5, - f64::decode(&mut Decoder::new(XmlRpcValue::Double(32.5))).expect(FAILED_TO_DECODE) - ); - } - - #[test] - fn reads_bool() { - assert_eq!( - true, - bool::decode(&mut Decoder::new(XmlRpcValue::Bool(true))).expect(FAILED_TO_DECODE) - ); - assert_eq!( - false, - bool::decode(&mut Decoder::new(XmlRpcValue::Bool(false))).expect(FAILED_TO_DECODE) - ); - } - - #[test] - fn reads_array() { - assert_eq!( - vec![1, 2, 3, 4, 5], - Vec::::decode(&mut Decoder::new(XmlRpcValue::Array(vec![ - XmlRpcValue::Int(1), - XmlRpcValue::Int(2), - XmlRpcValue::Int(3), - XmlRpcValue::Int(4), - XmlRpcValue::Int(5), - ]))).expect(FAILED_TO_DECODE) - ); - } - - #[derive(Debug, PartialEq, RustcDecodable)] - struct ExampleTuple(i32, f64, String, bool); - - #[test] - fn reads_tuple() { - assert_eq!( - ExampleTuple(5, 0.5, String::from("abc"), false), - ExampleTuple::decode(&mut Decoder::new(XmlRpcValue::Array(vec![ - XmlRpcValue::Int(5), - XmlRpcValue::Double(0.5), - XmlRpcValue::String(String::from("abc")), - XmlRpcValue::Bool(false), - ]))).expect(FAILED_TO_DECODE) - ); - } - - #[derive(Debug, PartialEq, RustcDecodable)] - struct ExampleStruct { - a: i32, - b: ExampleTuple, - } - - #[test] - fn reads_struct() { - assert_eq!( - ExampleStruct { - a: 11, - b: ExampleTuple(5, 0.5, String::from("abc"), false), - }, - ExampleStruct::decode(&mut Decoder::new(XmlRpcValue::Array(vec![ - XmlRpcValue::Int(11), - XmlRpcValue::Array(vec![ - XmlRpcValue::Int(5), - XmlRpcValue::Double(0.5), - XmlRpcValue::String(String::from("abc")), - XmlRpcValue::Bool(false), - ]), - ]))).expect(FAILED_TO_DECODE) - ); - } - - #[derive(Debug, PartialEq, RustcDecodable)] - struct ExampleRequestStruct { - a: i32, - b: bool, - c: ExampleRequestStructChild, - } - - #[derive(Debug, PartialEq, RustcDecodable)] - struct ExampleRequestStructChild { - a: String, - b: f64, - } - - #[test] - fn handles_requests() { - let data = r#" - - mytype.mymethod - - - 33 - - - - 41 - 1 - - Hello - 0.5 - - - - -"#; - let mut cursor = std::io::Cursor::new(data.as_bytes()); - let (method, mut parameters) = Decoder::new_request(&mut cursor).expect(FAILED_TO_DECODE); - assert_eq!("mytype.mymethod", method); - assert_eq!(2, parameters.len()); - assert_eq!(33, i32::decode(&mut parameters[0]).expect(FAILED_TO_DECODE)); - assert_eq!( - ExampleRequestStruct { - a: 41, - b: true, - c: ExampleRequestStructChild { - a: String::from("Hello"), - b: 0.5, - }, - }, - ExampleRequestStruct::decode(&mut parameters[1]).expect(FAILED_TO_DECODE) - ); - } - - #[test] - fn decoders_value_field_matches_data() { - let data = r#" - - mytype.mymethod - - - 33 - - - - 41 - 1 - - Hello - 0.5 - - - - -"#; - let mut cursor = std::io::Cursor::new(data.as_bytes()); - let (method, mut parameters) = Decoder::new_request(&mut cursor).expect(FAILED_TO_DECODE); - assert_eq!("mytype.mymethod", method); - assert_eq!(2, parameters.len()); - assert_eq!( - XmlRpcValue::Array(vec![ - XmlRpcValue::Int(41), - XmlRpcValue::Bool(true), - XmlRpcValue::Array(vec![ - XmlRpcValue::String(String::from("Hello")), - XmlRpcValue::Double(0.5), - ]), - ]), - parameters.pop().expect(FAILED_TO_DECODE).value() - ); - assert_eq!( - XmlRpcValue::Int(33), - parameters.pop().expect(FAILED_TO_DECODE).value() - ); - } -} diff --git a/src/rosxmlrpc/serde/encoder.rs b/src/rosxmlrpc/serde/encoder.rs deleted file mode 100644 index 3ad1df0..0000000 --- a/src/rosxmlrpc/serde/encoder.rs +++ /dev/null @@ -1,586 +0,0 @@ -use rustc_serialize; -use std; -use super::value::{self, XmlRpcValue}; -use super::error::{ErrorKind, Result, ResultExt}; - -pub struct Encoder { - data: Vec<(XmlRpcValue, usize)>, -} - -impl Encoder { - pub fn new() -> Encoder { - Encoder { data: vec![] } - } - - fn form_tree(self) -> Vec { - let mut retval = vec![]; - let mut data = self.data.into_iter(); - while let Some(value) = Encoder::form_subtree(&mut data) { - retval.push(value); - } - retval - } - - fn form_subtree( - mut data: &mut std::vec::IntoIter<(XmlRpcValue, usize)>, - ) -> Option { - match data.next() { - Some((node, count)) => { - Some(match node { - XmlRpcValue::Array(_) => { - XmlRpcValue::Array( - (0..count) - .into_iter() - .filter_map(|_| Encoder::form_subtree(&mut data)) - .collect(), - ) - } - _ => node, - }) - } - None => None, - } - } - - pub fn write_response(self, mut body: &mut T) -> std::io::Result<()> { - write!( - &mut body, - "{}", - value::XmlRpcResponse { parameters: self.form_tree() } - ) - } -} - -type EncoderResult = Result<()>; - -impl rustc_serialize::Encoder for Encoder { - type Error = super::error::Error; - - fn emit_nil(&mut self) -> EncoderResult { - bail!(ErrorKind::UnsupportedDataType("nil".into())) - } - - fn emit_usize(&mut self, _: usize) -> EncoderResult { - bail!(ErrorKind::UnsupportedDataType("usize".into())) - } - - fn emit_u64(&mut self, _: u64) -> EncoderResult { - bail!(ErrorKind::UnsupportedDataType("u64".into())) - } - - fn emit_u32(&mut self, _: u32) -> EncoderResult { - bail!(ErrorKind::UnsupportedDataType("u32".into())) - } - - fn emit_u16(&mut self, _: u16) -> EncoderResult { - bail!(ErrorKind::UnsupportedDataType("u16".into())) - } - - fn emit_u8(&mut self, _: u8) -> EncoderResult { - bail!(ErrorKind::UnsupportedDataType("u8".into())) - } - - fn emit_isize(&mut self, _: isize) -> EncoderResult { - bail!(ErrorKind::UnsupportedDataType("isize".into())) - } - - fn emit_i64(&mut self, _: i64) -> EncoderResult { - bail!(ErrorKind::UnsupportedDataType("i64".into())) - } - - fn emit_i32(&mut self, v: i32) -> EncoderResult { - Ok(self.data.push((XmlRpcValue::Int(v), 0))) - } - - fn emit_i16(&mut self, _: i16) -> EncoderResult { - bail!(ErrorKind::UnsupportedDataType("i16".into())) - } - - fn emit_i8(&mut self, _: i8) -> EncoderResult { - bail!(ErrorKind::UnsupportedDataType("i8".into())) - } - - fn emit_bool(&mut self, v: bool) -> EncoderResult { - Ok(self.data.push((XmlRpcValue::Bool(v), 0))) - } - - fn emit_f64(&mut self, v: f64) -> EncoderResult { - Ok(self.data.push((XmlRpcValue::Double(v), 0))) - } - - fn emit_f32(&mut self, _: f32) -> EncoderResult { - bail!(ErrorKind::UnsupportedDataType("f32".into())) - } - - fn emit_char(&mut self, _: char) -> EncoderResult { - bail!(ErrorKind::UnsupportedDataType("char".into())) - } - - fn emit_str(&mut self, v: &str) -> EncoderResult { - Ok(self.data.push((XmlRpcValue::String(String::from(v)), 0))) - } - - fn emit_enum(&mut self, _: &str, _: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - bail!(ErrorKind::UnsupportedDataType("enum".into())) - } - - fn emit_enum_variant(&mut self, _: &str, _: usize, _: usize, _: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - bail!(ErrorKind::UnsupportedDataType("enum variant".into())) - } - - fn emit_enum_variant_arg(&mut self, _: usize, _: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - bail!(ErrorKind::UnsupportedDataType( - "enum variant argument".into(), - )) - } - - fn emit_enum_struct_variant(&mut self, _: &str, _: usize, _: usize, _: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - bail!(ErrorKind::UnsupportedDataType("enum struct variant".into())) - } - - fn emit_enum_struct_variant_field(&mut self, _: &str, _: usize, _: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - bail!(ErrorKind::UnsupportedDataType( - "enum struct variant field".into(), - )) - } - - fn emit_struct(&mut self, name: &str, l: usize, f: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - self.data.push((XmlRpcValue::Array(vec![]), l)); - f(self).chain_err(|| { - ErrorKind::UnsupportedDataType(format!("struct {}", name)) - }) - } - - fn emit_struct_field(&mut self, name: &str, _: usize, f: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - f(self).chain_err(|| ErrorKind::UnsupportedDataType(format!("field {}", name))) - } - - fn emit_tuple(&mut self, l: usize, f: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - self.data.push((XmlRpcValue::Array(vec![]), l)); - f(self).chain_err(|| ErrorKind::UnsupportedDataType("tuple".into())) - } - - fn emit_tuple_arg(&mut self, n: usize, f: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - f(self).chain_err(|| { - ErrorKind::UnsupportedDataType(format!("field number {}", n)) - }) - } - - fn emit_tuple_struct(&mut self, name: &str, l: usize, f: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - self.data.push((XmlRpcValue::Array(vec![]), l)); - f(self).chain_err(|| { - ErrorKind::UnsupportedDataType(format!("tuple struct {}", name)) - }) - } - - fn emit_tuple_struct_arg(&mut self, n: usize, f: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - f(self).chain_err(|| { - ErrorKind::UnsupportedDataType(format!("field number {}", n)) - }) - } - - fn emit_option(&mut self, _: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - bail!(ErrorKind::UnsupportedDataType("option".into())) - } - - fn emit_option_none(&mut self) -> EncoderResult { - bail!(ErrorKind::UnsupportedDataType("none".into())) - } - - fn emit_option_some(&mut self, _: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - bail!(ErrorKind::UnsupportedDataType("some".into())) - } - - fn emit_seq(&mut self, l: usize, f: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - self.data.push((XmlRpcValue::Array(vec![]), l)); - f(self).chain_err(|| ErrorKind::UnsupportedDataType("array".into())) - } - - fn emit_seq_elt(&mut self, n: usize, f: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - f(self).chain_err(|| { - ErrorKind::UnsupportedDataType(format!("element number {}", n)) - }) - } - - fn emit_map(&mut self, _: usize, _: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - bail!(ErrorKind::UnsupportedDataType("map".into())) - } - - fn emit_map_elt_key(&mut self, _: usize, _: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - bail!(ErrorKind::UnsupportedDataType("map element key".into())) - } - - fn emit_map_elt_val(&mut self, _: usize, _: F) -> EncoderResult - where - F: FnOnce(&mut Self) -> EncoderResult, - { - bail!(ErrorKind::UnsupportedDataType("map element value".into())) - } -} - -#[cfg(test)] -mod tests { - use super::*; - use rustc_serialize::Encodable; - use std; - - static FAILED_TO_ENCODE: &'static str = "Failed to encode"; - - #[test] - fn writes_response() { - let mut data = vec![]; - let mut encoder = Encoder::new(); - String::from("Hello").encode(&mut encoder).expect( - FAILED_TO_ENCODE, - ); - encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!( - concat!( - r#""#, - r#""#, - r#""#, - r#""#, - r#"Hello"#, - r#""#, - r#""#, - r#""# - ), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) - ); - } - - #[test] - fn writes_string() { - let mut data = vec![]; - let mut encoder = Encoder::new(); - String::from("Hello").encode(&mut encoder).expect( - FAILED_TO_ENCODE, - ); - String::from("There").encode(&mut encoder).expect( - FAILED_TO_ENCODE, - ); - String::from("Friend").encode(&mut encoder).expect( - FAILED_TO_ENCODE, - ); - encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!( - concat!( - r#""#, - r#""#, - r#""#, - r#""#, - r#"Hello"#, - r#""#, - r#""#, - r#"There"#, - r#""#, - r#""#, - r#"Friend"#, - r#""#, - r#""#, - r#""# - ), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) - ); - } - - #[test] - fn writes_int() { - let mut data = vec![]; - let mut encoder = Encoder::new(); - 43i32.encode(&mut encoder).expect(FAILED_TO_ENCODE); - 27i32.encode(&mut encoder).expect(FAILED_TO_ENCODE); - 12i32.encode(&mut encoder).expect(FAILED_TO_ENCODE); - encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!( - concat!( - r#""#, - r#""#, - r#""#, - r#""#, - r#"43"#, - r#""#, - r#""#, - r#"27"#, - r#""#, - r#""#, - r#"12"#, - r#""#, - r#""#, - r#""# - ), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) - ); - } - - #[test] - fn writes_float() { - let mut data = vec![]; - let mut encoder = Encoder::new(); - 33.5f64.encode(&mut encoder).expect(FAILED_TO_ENCODE); - 11.25f64.encode(&mut encoder).expect(FAILED_TO_ENCODE); - 77.125f64.encode(&mut encoder).expect(FAILED_TO_ENCODE); - encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!( - concat!( - r#""#, - r#""#, - r#""#, - r#""#, - r#"33.5"#, - r#""#, - r#""#, - r#"11.25"#, - r#""#, - r#""#, - r#"77.125"#, - r#""#, - r#""#, - r#""# - ), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) - ); - } - - #[test] - fn writes_bool() { - let mut data = vec![]; - let mut encoder = Encoder::new(); - true.encode(&mut encoder).expect(FAILED_TO_ENCODE); - false.encode(&mut encoder).expect(FAILED_TO_ENCODE); - encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!( - concat!( - r#""#, - r#""#, - r#""#, - r#""#, - r#"1"#, - r#""#, - r#""#, - r#"0"#, - r#""#, - r#""#, - r#""# - ), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) - ); - } - - #[test] - fn writes_array() { - let mut data = vec![]; - let mut encoder = Encoder::new(); - vec![1i32, 2, 3, 4, 5].encode(&mut encoder).expect( - FAILED_TO_ENCODE, - ); - encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!( - concat!( - r#""#, - r#""#, - r#""#, - r#""#, - r#""#, - r#"1"#, - r#"2"#, - r#"3"#, - r#"4"#, - r#"5"#, - r#""#, - r#""#, - r#""#, - r#""# - ), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) - ); - } - - #[derive(Debug, PartialEq, RustcEncodable)] - struct ExampleTuple(i32, f64, String, bool); - - #[test] - fn writes_tuple() { - let mut data = vec![]; - let mut encoder = Encoder::new(); - ExampleTuple(5, 0.5, String::from("abc"), false) - .encode(&mut encoder) - .expect(FAILED_TO_ENCODE); - encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!( - concat!( - r#""#, - r#""#, - r#""#, - r#""#, - r#""#, - r#"5"#, - r#"0.5"#, - r#"abc"#, - r#"0"#, - r#""#, - r#""#, - r#""#, - r#""# - ), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) - ); - } - - #[derive(Debug, PartialEq, RustcEncodable)] - struct ExampleRequestStruct { - a: i32, - b: bool, - c: ExampleRequestStructChild, - } - - #[derive(Debug, PartialEq, RustcEncodable)] - struct ExampleRequestStructChild { - a: String, - b: f64, - } - - #[test] - fn writes_struct() { - let mut data = vec![]; - let mut encoder = Encoder::new(); - ExampleRequestStruct { - a: 41, - b: true, - c: ExampleRequestStructChild { - a: String::from("Hello"), - b: 0.5, - }, - }.encode(&mut encoder) - .expect(FAILED_TO_ENCODE); - encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!( - concat!( - r#""#, - r#""#, - r#""#, - r#""#, - r#""#, - r#"41"#, - r#"1"#, - r#""#, - r#"Hello"#, - r#"0.5"#, - r#""#, - r#""#, - r#""#, - r#""#, - r#""# - ), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) - ); - } - - #[test] - fn writes_multiple_parameters() { - let mut data = vec![]; - let mut encoder = Encoder::new(); - ExampleTuple(5, 0.5, String::from("abc"), false) - .encode(&mut encoder) - .expect(FAILED_TO_ENCODE); - 27i32.encode(&mut encoder).expect(FAILED_TO_ENCODE); - String::from("Hello").encode(&mut encoder).expect( - FAILED_TO_ENCODE, - ); - ExampleRequestStruct { - a: 41, - b: true, - c: ExampleRequestStructChild { - a: String::from("Hello"), - b: 0.5, - }, - }.encode(&mut encoder) - .expect(FAILED_TO_ENCODE); - encoder.write_response(&mut data).expect(FAILED_TO_ENCODE); - assert_eq!( - concat!( - r#""#, - r#""#, - r#""#, - r#""#, - r#""#, - r#"5"#, - r#"0.5"#, - r#"abc"#, - r#"0"#, - r#""#, - r#""#, - r#""#, - r#"27"#, - r#""#, - r#""#, - r#"Hello"#, - r#""#, - r#""#, - r#""#, - r#"41"#, - r#"1"#, - r#""#, - r#"Hello"#, - r#"0.5"#, - r#""#, - r#""#, - r#""#, - r#""#, - r#""# - ), - std::str::from_utf8(&data).expect(FAILED_TO_ENCODE) - ); - } -} diff --git a/src/rosxmlrpc/serde/error.rs b/src/rosxmlrpc/serde/error.rs deleted file mode 100644 index 80700fb..0000000 --- a/src/rosxmlrpc/serde/error.rs +++ /dev/null @@ -1,20 +0,0 @@ -error_chain!{ - errors { - XmlRpcReading(t:String) { - description("Issue while reading XML-RPC data") - display("Issue while reading XML-RPC data, for {}", t) - } - Decoding(t: String) { - description("Issue while decoding data structure") - display("Issue while decoding data structure, within {}", t) - } - UnsupportedDataType(t: String) { - description("Datatype is not supported") - display("Datatype is not supported, issue within {}", t) - } - MismatchedDataFormat(t: String) { - description("Provided XML-RPC tree does not match target format") - display("Provided XML-RPC tree does not match target format, {} was expected", t) - } - } -} diff --git a/src/rosxmlrpc/serde/mod.rs b/src/rosxmlrpc/serde/mod.rs deleted file mode 100644 index d0235fe..0000000 --- a/src/rosxmlrpc/serde/mod.rs +++ /dev/null @@ -1,8 +0,0 @@ -pub use self::encoder::Encoder; -pub use self::decoder::Decoder; -pub use self::value::XmlRpcValue; - -pub mod encoder; -pub mod decoder; -pub mod value; -pub mod error; diff --git a/src/rosxmlrpc/serde/value.rs b/src/rosxmlrpc/serde/value.rs deleted file mode 100644 index ae50662..0000000 --- a/src/rosxmlrpc/serde/value.rs +++ /dev/null @@ -1,670 +0,0 @@ -use std; -use xml; -use self::error::{ErrorKind, Result, ResultExt}; - -#[derive(Clone, Debug, PartialEq)] -pub enum XmlRpcValue { - Int(i32), - Bool(bool), - String(String), - Double(f64), - Array(Vec), - Struct(Vec<(String, XmlRpcValue)>), -} - -#[derive(Debug)] -pub struct XmlRpcRequest { - pub method: String, - pub parameters: Vec, -} - -#[derive(Debug)] -pub struct XmlRpcResponse { - pub parameters: Vec, -} - -impl std::fmt::Display for XmlRpcValue { - fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result { - match *self { - XmlRpcValue::Int(v) => write!(f, "{}", v), - XmlRpcValue::Bool(v) => { - write!( - f, - "{}", - if v { 1 } else { 0 } - ) - } - XmlRpcValue::String(ref v) => write!(f, "{}", v), - XmlRpcValue::Double(v) => write!(f, "{}", v), - XmlRpcValue::Array(ref v) => { - write!(f, "")?; - for item in v { - item.fmt(f)?; - } - write!(f, "") - } - XmlRpcValue::Struct(ref v) => { - write!(f, "")?; - for &(ref name, ref item) in v { - write!(f, "{}", name)?; - item.fmt(f)?; - write!(f, "")?; - } - write!(f, "") - } - } - } -} - -impl std::fmt::Display for XmlRpcRequest { - fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result { - write!( - f, - "{}", - self.method - )?; - for parameter in &self.parameters { - write!(f, "{}", parameter)?; - } - write!(f, "") - } -} - -impl std::fmt::Display for XmlRpcResponse { - fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result { - write!(f, "")?; - for parameter in &self.parameters { - write!(f, "{}", parameter)?; - } - write!(f, "") - } -} - -impl XmlRpcRequest { - pub fn new(body: T) -> Result { - let tree = Tree::new(body).chain_err( - || "Failed to transform XML data into tree", - )?; - let (key, mut children) = match tree { - Tree::Node(key, children) => (key, children), - Tree::Leaf(_) => { - bail!("XML-RPC request should contain a node called 'methodCall' with 2 children"); - } - }; - if key != "methodCall" || children.len() != 2 { - bail!("XML-RPC request should contain a node called 'methodCall' with 2 children") - } - // We checked the array length, so it's safe to pop - let parameters_tree = children.pop().expect(UNEXPECTED_EMPTY_ARRAY); - let method = children.pop().expect(UNEXPECTED_EMPTY_ARRAY); - match method.peel_layer("methodName").chain_err( - || "Bad XML-RPC Request method name value", - )? { - Tree::Leaf(method_name) => { - Ok(XmlRpcRequest { - method: method_name, - parameters: extract_parameters(parameters_tree)?, - }) - } - Tree::Node(_, _) => { - bail!("Node 'methodName' contain a string representing the method name"); - } - } - } -} - -fn extract_parameters(parameters: Tree) -> Result> { - if let Tree::Node(key, parameters) = parameters { - if key == "params" { - return parameters - .into_iter() - .map(XmlRpcValue::from_parameter) - .collect::>() - .chain_err(|| "Failed to parse parameters"); - } - } - bail!("Parameters need to be contained within a node called params") -} - -impl XmlRpcValue { - pub fn new(body: T) -> Result { - XmlRpcValue::from_tree(Tree::new(body).chain_err( - || "Couldn't generate XML tree to form value", - )?) - } - - fn read_member(tree: Tree) -> Result<(String, XmlRpcValue)> { - let (key, mut children) = match tree { - Tree::Node(key, children) => (key, children), - Tree::Leaf(_) => { - bail!( - "Structure member node should contain a node called 'member' with two \ - children" - ) - } - }; - if key != "member" || children.len() != 2 { - bail!("Structure member node should contain a node called 'member' with two children") - } - // We tested the vector length already, so it's safe to pop - let value = children.pop().expect(UNEXPECTED_EMPTY_ARRAY); - let name_node = children.pop().expect(UNEXPECTED_EMPTY_ARRAY); - let name = match name_node.peel_layer("name").chain_err( - || "First struct member field should be a node called 'name'", - )? { - Tree::Leaf(name) => name, - Tree::Node(_, _) => { - bail!("Struct member's name node should just contain the member's name"); - } - }; - - XmlRpcValue::from_tree(value) - .chain_err(|| { - format!("Failed to parse subtree of struct field {}", name) - }) - .map(|v| (name, v)) - } - - fn from_parameter(tree: Tree) -> Result { - XmlRpcValue::from_tree(tree.peel_layer("param").chain_err( - || "Parameters should be contained within node 'param'", - )?).chain_err(|| "Failed to parse XML RPC parameter") - } - - fn from_tree(tree: Tree) -> Result { - let (key, mut values) = match tree.peel_layer("value")? { - Tree::Node(key, values) => (key, values), - Tree::Leaf(_) => bail!("Value node should contain one node representing its data type"), - }; - if key == "struct" { - return Ok(XmlRpcValue::Struct(values - .into_iter() - .map(XmlRpcValue::read_member) - .collect::>>() - .chain_err(|| "Couldn't parse struct")?)); - } - if values.len() > 1 { - bail!("Node '{}' can't have more than one child", key); - } - if key == "array" { - return if let Some(Tree::Node(key, children)) = values.pop() { - if key != "data" { - bail!( - "Node 'array' must contain 'data' node, but '{}' detected", - key - ); - } - Ok(XmlRpcValue::Array(children - .into_iter() - .map(XmlRpcValue::from_tree) - .collect::>() - .chain_err(|| "Failed to parse array's children")?)) - } else { - bail!("Node 'array' must contain 'data' node with child values"); - }; - } - let value = match values.pop().unwrap_or(Tree::Leaf(String::from(""))) { - Tree::Leaf(value) => value, - Tree::Node(_, _) => bail!("Value field for type '{}' must contain just the value", key), - }; - match key.as_str() { - "i4" | "int" => { - Ok(XmlRpcValue::Int(value.parse().chain_err(|| { - format!("Failed to parse integer (i32) {}", value) - })?)) - } - "boolean" => { - Ok(XmlRpcValue::Bool( - value.parse::().chain_err(|| { - format!("Expected 0 or 1 for boolean, got {}", value) - })? != 0, - )) - } - "string" => Ok(XmlRpcValue::String(value)), - "double" => { - Ok(XmlRpcValue::Double(value.parse().chain_err(|| { - format!("Failed to parse double (f64) {}", value) - })?)) - } - _ => bail!("Unsupported data type '{}'", key), - } - } -} - -enum Tree { - Leaf(String), - Node(String, Vec), -} - -impl Tree { - fn new(body: T) -> Result { - parse_tree(&mut xml::EventReader::new(body))?.ok_or( - "XML data started with a closing tag" - .into(), - ) - } - - fn peel_layer(self, name: &str) -> Result { - if let Tree::Node(key, mut children) = self { - if key == name && children.len() == 1 { - return Ok(children.pop().expect(UNEXPECTED_EMPTY_ARRAY)); - } - } - bail!("Expected a node named '{}' with 1 child", name) - } -} - -enum Node { - Open(String), - Data(String), - Close(String), -} - -fn parse_tree(reader: &mut xml::EventReader) -> Result> { - match next_node(reader).chain_err(|| "Unexpected end of XML data")? { - Node::Close(..) => Ok(None), - Node::Data(value) => Ok(Some(Tree::Leaf(value))), - Node::Open(name) => { - let mut children = Vec::::new(); - while let Some(node) = parse_tree(reader).chain_err( - || ErrorKind::TreeParsing(name.clone()), - )? - { - children.push(node); - } - Ok(Some(Tree::Node(name, children))) - } - } -} - -fn next_node(reader: &mut xml::EventReader) -> Result { - match reader.next().chain_err(|| "Couldn't obtain XML token")? { - xml::reader::XmlEvent::StartElement { name, .. } => Ok(Node::Open(name.local_name)), - xml::reader::XmlEvent::Characters(value) => Ok(Node::Data(value)), - xml::reader::XmlEvent::EndElement { name } => Ok(Node::Close(name.local_name)), - _ => next_node(reader), - } -} - -mod error { - error_chain!{ - errors { - TreeParsing(node_name: String) { - description("Error while building tree out of XML data") - display("XML tree building error within node {}", node_name) - } - } - } -} - -static UNEXPECTED_EMPTY_ARRAY: &'static str = "Popping failure from this array is impossible"; - -#[cfg(test)] -mod tests { - use super::*; - use std; - - static BAD_DATA: &'static str = "Bad data provided"; - - #[test] - fn reads_string() { - let data = r#"First test"#; - let mut cursor = std::io::Cursor::new(data.as_bytes()); - let value = XmlRpcValue::new(&mut cursor).expect(BAD_DATA); - assert_eq!(XmlRpcValue::String(String::from("First test")), value); - let data = r#""#; - let mut cursor = std::io::Cursor::new(data.as_bytes()); - let value = XmlRpcValue::new(&mut cursor).expect(BAD_DATA); - assert_eq!(XmlRpcValue::String(String::from("")), value); - let data = r#""#; - let mut cursor = std::io::Cursor::new(data.as_bytes()); - let value = XmlRpcValue::new(&mut cursor).expect(BAD_DATA); - assert_eq!(XmlRpcValue::String(String::from("")), value); - } - - #[test] - fn reads_int() { - let data = r#"41"#; - let mut cursor = std::io::Cursor::new(data.as_bytes()); - let value = XmlRpcValue::new(&mut cursor).expect(BAD_DATA); - assert_eq!(XmlRpcValue::Int(41), value); - let data = r#"14"#; - let mut cursor = std::io::Cursor::new(data.as_bytes()); - let value = XmlRpcValue::new(&mut cursor).expect(BAD_DATA); - assert_eq!(XmlRpcValue::Int(14), value); - } - - #[test] - fn reads_float() { - let data = r#"33.25"#; - let mut cursor = std::io::Cursor::new(data.as_bytes()); - let value = XmlRpcValue::new(&mut cursor).expect(BAD_DATA); - assert_eq!(XmlRpcValue::Double(33.25), value); - } - - #[test] - fn reads_bool() { - let data = r#"1"#; - let mut cursor = std::io::Cursor::new(data.as_bytes()); - let value = XmlRpcValue::new(&mut cursor).expect(BAD_DATA); - assert_eq!(XmlRpcValue::Bool(true), value); - let data = r#"0"#; - let mut cursor = std::io::Cursor::new(data.as_bytes()); - let value = XmlRpcValue::new(&mut cursor).expect(BAD_DATA); - assert_eq!(XmlRpcValue::Bool(false), value); - } - - #[test] - fn reads_array() { - let data = r#" - - 41 - 1 - - Hello - 0.5 - -"#; - let mut cursor = std::io::Cursor::new(data.as_bytes()); - let value = XmlRpcValue::new(&mut cursor).expect(BAD_DATA); - assert_eq!( - XmlRpcValue::Array(vec![ - XmlRpcValue::Int(41), - XmlRpcValue::Bool(true), - XmlRpcValue::Array(vec![ - XmlRpcValue::String(String::from("Hello")), - XmlRpcValue::Double(0.5), - ]), - ]), - value - ); - } - - #[test] - fn reads_struct() { - let data = r#" - - - a - 41 - - - b - 1 - - - c - - - xxx - Hello - - - yyy - 0.5 - - - -"#; - let mut cursor = std::io::Cursor::new(data.as_bytes()); - let value = XmlRpcValue::new(&mut cursor).expect(BAD_DATA); - assert_eq!( - XmlRpcValue::Struct(vec![ - (String::from("a"), XmlRpcValue::Int(41)), - (String::from("b"), XmlRpcValue::Bool(true)), - ( - String::from("c"), - XmlRpcValue::Struct(vec![ - ( - String::from("xxx"), - XmlRpcValue::String(String::from("Hello")) - ), - (String::from("yyy"), XmlRpcValue::Double(0.5)), - ]) - ), - ]), - value - ); - } - - #[test] - fn reads_request() { - let data = r#" - - mytype.mymethod - - - 33 - - - - 41 - 1 - - Hello - 0.5 - - - - -"#; - let mut cursor = std::io::Cursor::new(data.as_bytes()); - let value = XmlRpcRequest::new(&mut cursor).expect(BAD_DATA); - assert_eq!("mytype.mymethod", value.method); - assert_eq!( - vec![ - XmlRpcValue::Int(33), - XmlRpcValue::Array(vec![ - XmlRpcValue::Int(41), - XmlRpcValue::Bool(true), - XmlRpcValue::Array(vec![ - XmlRpcValue::String(String::from("Hello")), - XmlRpcValue::Double(0.5), - ]), - ]), - ], - value.parameters - ); - } - - #[test] - fn writes_string() { - assert_eq!( - r#"First test"#, - format!("{}", XmlRpcValue::String(String::from("First test"))) - ); - assert_eq!( - r#""#, - format!("{}", XmlRpcValue::String(String::from(""))) - ); - } - - #[test] - fn writes_int() { - assert_eq!( - r#"41"#, - format!("{}", XmlRpcValue::Int(41)) - ); - } - - #[test] - fn writes_float() { - assert_eq!( - r#"33.25"#, - format!("{}", XmlRpcValue::Double(33.25)) - ); - } - - #[test] - fn writes_bool() { - assert_eq!( - r#"1"#, - format!("{}", XmlRpcValue::Bool(true)) - ); - assert_eq!( - r#"0"#, - format!("{}", XmlRpcValue::Bool(false)) - ); - } - - #[test] - fn writes_array() { - assert_eq!( - concat!( - r#""#, - r#"41"#, - r#"1"#, - r#""#, - r#"Hello"#, - r#"0.5"#, - r#""#, - r#""# - ), - format!( - "{}", - XmlRpcValue::Array(vec![ - XmlRpcValue::Int(41), - XmlRpcValue::Bool(true), - XmlRpcValue::Array(vec![ - XmlRpcValue::String(String::from("Hello")), - XmlRpcValue::Double(0.5), - ]), - ]) - ) - ); - } - - #[test] - fn writes_struct() { - assert_eq!( - concat!( - r#""#, - r#""#, - r#"a"#, - r#"41"#, - r#""#, - r#""#, - r#"b"#, - r#"1"#, - r#""#, - r#""#, - r#"c"#, - r#""#, - r#""#, - r#"xxx"#, - r#"Hello"#, - r#""#, - r#""#, - r#"yyy"#, - r#"0.5"#, - r#""#, - r#""#, - r#""#, - r#""# - ), - format!( - "{}", - XmlRpcValue::Struct(vec![ - (String::from("a"), XmlRpcValue::Int(41)), - (String::from("b"), XmlRpcValue::Bool(true)), - ( - String::from("c"), - XmlRpcValue::Struct(vec![ - ( - String::from("xxx"), - XmlRpcValue::String(String::from("Hello")) - ), - (String::from("yyy"), XmlRpcValue::Double(0.5)), - ]) - ), - ]) - ) - ); - } - - #[test] - fn writes_request() { - assert_eq!( - concat!( - r#""#, - r#""#, - r#"mytype.mymethod"#, - r#""#, - r#""#, - r#"33"#, - r#""#, - r#""#, - r#""#, - r#"41"#, - r#"1"#, - r#""#, - r#"Hello"#, - r#"0.5"#, - r#""#, - r#""#, - r#""#, - r#""#, - r#""# - ), - format!( - "{}", - XmlRpcRequest { - method: String::from("mytype.mymethod"), - parameters: vec![ - XmlRpcValue::Int(33), - XmlRpcValue::Array(vec![ - XmlRpcValue::Int(41), - XmlRpcValue::Bool(true), - XmlRpcValue::Array(vec![ - XmlRpcValue::String(String::from("Hello")), - XmlRpcValue::Double(0.5), - ]), - ]), - ], - } - ) - ); - } - - #[test] - fn writes_response() { - assert_eq!( - concat!( - r#""#, - r#""#, - r#""#, - r#""#, - r#"33"#, - r#""#, - r#""#, - r#""#, - r#"41"#, - r#"1"#, - r#""#, - r#"Hello"#, - r#"0.5"#, - r#""#, - r#""#, - r#""#, - r#""#, - r#""# - ), - format!( - "{}", - XmlRpcResponse { - parameters: vec![ - XmlRpcValue::Int(33), - XmlRpcValue::Array(vec![ - XmlRpcValue::Int(41), - XmlRpcValue::Bool(true), - XmlRpcValue::Array(vec![ - XmlRpcValue::String(String::from("Hello")), - XmlRpcValue::Double(0.5), - ]), - ]), - ], - } - ) - ); - } -} From ea06c7f5fa133c9d0763ddbcd16105ce5f54d361 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Thu, 5 Oct 2017 01:22:26 +0200 Subject: [PATCH 09/50] Handle when parameters are sent as one array --- src/api/slavehandler.rs | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/api/slavehandler.rs b/src/api/slavehandler.rs index cffaf71..37e5453 100644 --- a/src/api/slavehandler.rs +++ b/src/api/slavehandler.rs @@ -6,7 +6,7 @@ use std::sync::{Arc, Mutex}; use futures::sync::mpsc::Sender; use super::error::{self, ErrorKind, Result}; use tcpros::{Publisher, Subscriber, Service}; -use xml_rpc::{self, Value}; +use xml_rpc::{self, Value, Params}; pub struct SlaveHandler { pub subscriptions: Arc>>, @@ -15,6 +15,13 @@ pub struct SlaveHandler { server: Server, } +fn unwrap_array_case(params: Params) -> Params { + if let Some(&Value::Array(ref items)) = params.get(0) { + return items.clone(); + } + params +} + impl SlaveHandler { pub fn new( master_uri: &str, @@ -44,7 +51,7 @@ impl SlaveHandler { }); server.register_value("shutdown", "Shutdown", move |args| { - let mut args = args.into_iter(); + let mut args = unwrap_array_case(args).into_iter(); let _caller_id = args.next().ok_or(ResponseError::Client( "Missing argument 'caller_id'".into(), ))?; @@ -109,7 +116,7 @@ impl SlaveHandler { let subs = subscriptions.clone(); server.register_value("publisherUpdate", "Publishers updated", move |args| { - let mut args = args.into_iter(); + let mut args = unwrap_array_case(args).into_iter(); let _caller_id = args.next().ok_or(ResponseError::Client( "Missing argument 'caller_id'".into(), ))?; @@ -150,7 +157,7 @@ impl SlaveHandler { let pubs = publications.clone(); server.register_value("requestTopic", "Chosen protocol", move |args| { - let mut args = args.into_iter(); + let mut args = unwrap_array_case(args).into_iter(); let _caller_id = args.next().ok_or(ResponseError::Client( "Missing argument 'caller_id'".into(), ))?; From 975363145abd7a4c6fe8f6c81c374e46ed2ecce3 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 5 Dec 2017 16:07:45 +0100 Subject: [PATCH 10/50] Improve XML-RPC support --- Cargo.toml | 2 +- src/rosxmlrpc/client.rs | 38 ++++++++++++++++++++++++-------------- 2 files changed, 25 insertions(+), 15 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 0eb5f95..d2ec910 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -20,4 +20,4 @@ rust-crypto = "0.2.36" serde = "1.0.2" serde_derive = "1.0.2" serde_rosmsg = "0.2.0" -xml-rpc = "0.0.3" +xml-rpc = "0.0.4" diff --git a/src/rosxmlrpc/client.rs b/src/rosxmlrpc/client.rs index b64498b..ad77f87 100644 --- a/src/rosxmlrpc/client.rs +++ b/src/rosxmlrpc/client.rs @@ -33,21 +33,31 @@ impl Client { )) })? .into_iter(); - if let (Some(Value::Int(code)), Some(Value::String(message)), Some(data)) = - (response.next(), response.next(), response.next()) - { - match code { - ERROR_CODE => Err(ResponseError::Client(message)), - FAILURE_CODE => Err(ResponseError::Server(message)), - SUCCESS_CODE => Ok(data), - _ => Err(ResponseError::Server( - format!("Bad response code returned from server"), - )), + let mut first_item = response.next(); + while let Some(Value::Array(v)) = first_item { + response = v.into_iter(); + first_item = response.next(); + } + match (first_item, response.next(), response.next()) { + (Some(Value::Int(code)), Some(Value::String(message)), Some(data)) => { + match code { + ERROR_CODE => Err(ResponseError::Client(message)), + FAILURE_CODE => Err(ResponseError::Server(message)), + SUCCESS_CODE => Ok(data), + _ => Err(ResponseError::Server( + format!("Bad response code returned from server"), + )), + } + } + (code, message, data) => { + Err(ResponseError::Server(format!( + "Response with three parameters (int code, str msg, value) \ + expected from server, received: ({:?}, {:?}, {:?})", + code, + message, + data + ))) } - } else { - Err(ResponseError::Server(format!( - "Response with three parameters (int code, str msg, value) expected from server" - ))) } } From 94791bb37169f8bef4390e5bc7d9f64b3b3ee0a9 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Thu, 7 Dec 2017 00:28:10 +0100 Subject: [PATCH 11/50] Update XML-RPC --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index d2ec910..bac9fe2 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -20,4 +20,4 @@ rust-crypto = "0.2.36" serde = "1.0.2" serde_derive = "1.0.2" serde_rosmsg = "0.2.0" -xml-rpc = "0.0.4" +xml-rpc = "0.0.5" From 8ec6be8ba16a0426c28be1bc37b1c87fa1de5579 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Thu, 7 Dec 2017 10:57:28 +0100 Subject: [PATCH 12/50] Update XML-RPC --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index bac9fe2..dfb5312 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -20,4 +20,4 @@ rust-crypto = "0.2.36" serde = "1.0.2" serde_derive = "1.0.2" serde_rosmsg = "0.2.0" -xml-rpc = "0.0.5" +xml-rpc = "0.0.6" From 9a6ce92a85555811290d5cc4797c10a3868603d3 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sun, 10 Dec 2017 16:30:01 +0100 Subject: [PATCH 13/50] Update XML-RPC --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index dfb5312..77be239 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -20,4 +20,4 @@ rust-crypto = "0.2.36" serde = "1.0.2" serde_derive = "1.0.2" serde_rosmsg = "0.2.0" -xml-rpc = "0.0.6" +xml-rpc = "0.0.7" From c273e80ae462b47697fbf80c31ebbfe3cdbe6761 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 13 Dec 2017 11:04:21 +0100 Subject: [PATCH 14/50] Create workspace --- .rustfmt.toml | 1 + Cargo.toml | 28 ++++--------------- rosrust/Cargo.toml | 23 +++++++++++++++ {src => rosrust/src}/api/error.rs | 0 {src => rosrust/src}/api/master.rs | 0 {src => rosrust/src}/api/mod.rs | 0 {src => rosrust/src}/api/naming/error.rs | 0 {src => rosrust/src}/api/naming/mapper.rs | 0 {src => rosrust/src}/api/naming/mod.rs | 0 {src => rosrust/src}/api/naming/path.rs | 0 {src => rosrust/src}/api/resolve.rs | 0 {src => rosrust/src}/api/ros.rs | 0 {src => rosrust/src}/api/slave.rs | 0 {src => rosrust/src}/api/slavehandler.rs | 0 {src => rosrust/src}/build_tools/error.rs | 0 {src => rosrust/src}/build_tools/genmsg.rs | 0 {src => rosrust/src}/build_tools/helpers.rs | 0 {src => rosrust/src}/build_tools/mod.rs | 0 {src => rosrust/src}/build_tools/msg.rs | 0 .../benchmark_msgs/msg/Overall.msg | 0 .../diagnostic_msgs/srv/AddDiagnostics.srv | 0 .../empty_req_srv/srv/EmptyRequest.srv | 0 .../msg_examples/geometry_msgs/msg/Point.msg | 0 .../msg_examples/geometry_msgs/msg/Pose.msg | 0 .../geometry_msgs/msg/PoseStamped.msg | 0 .../geometry_msgs/msg/Quaternion.msg | 0 .../geometry_msgs/msg/TwistWithCovariance.msg | 0 .../geometry_msgs/msg/Vector3.msg | 0 .../msg_examples/sensor_msgs/msg/Imu.msg | 0 .../msg_examples/simple_srv/srv/Something.srv | 0 .../msg_examples/std_msgs/msg/Header.msg | 0 .../tricky_comment_srv/srv/TrickyComment.srv | 0 {src => rosrust/src}/lib.rs | 0 {src => rosrust/src}/msg.rs | 0 {src => rosrust/src}/rosxmlrpc/client.rs | 0 {src => rosrust/src}/rosxmlrpc/error.rs | 0 {src => rosrust/src}/rosxmlrpc/mod.rs | 0 {src => rosrust/src}/rosxmlrpc/server.rs | 0 {src => rosrust/src}/tcpros/client.rs | 0 {src => rosrust/src}/tcpros/error.rs | 0 {src => rosrust/src}/tcpros/header.rs | 0 {src => rosrust/src}/tcpros/mod.rs | 0 {src => rosrust/src}/tcpros/publisher.rs | 0 {src => rosrust/src}/tcpros/service.rs | 0 {src => rosrust/src}/tcpros/subscriber.rs | 0 {src => rosrust/src}/tcpros/util/mod.rs | 0 .../src}/tcpros/util/streamfork.rs | 0 .../src}/tcpros/util/tcpconnection.rs | 0 48 files changed, 30 insertions(+), 22 deletions(-) create mode 100644 .rustfmt.toml create mode 100644 rosrust/Cargo.toml rename {src => rosrust/src}/api/error.rs (100%) rename {src => rosrust/src}/api/master.rs (100%) rename {src => rosrust/src}/api/mod.rs (100%) rename {src => rosrust/src}/api/naming/error.rs (100%) rename {src => rosrust/src}/api/naming/mapper.rs (100%) rename {src => rosrust/src}/api/naming/mod.rs (100%) rename {src => rosrust/src}/api/naming/path.rs (100%) rename {src => rosrust/src}/api/resolve.rs (100%) rename {src => rosrust/src}/api/ros.rs (100%) rename {src => rosrust/src}/api/slave.rs (100%) rename {src => rosrust/src}/api/slavehandler.rs (100%) rename {src => rosrust/src}/build_tools/error.rs (100%) rename {src => rosrust/src}/build_tools/genmsg.rs (100%) rename {src => rosrust/src}/build_tools/helpers.rs (100%) rename {src => rosrust/src}/build_tools/mod.rs (100%) rename {src => rosrust/src}/build_tools/msg.rs (100%) rename {src => rosrust/src}/build_tools/msg_examples/benchmark_msgs/msg/Overall.msg (100%) rename {src => rosrust/src}/build_tools/msg_examples/diagnostic_msgs/srv/AddDiagnostics.srv (100%) rename {src => rosrust/src}/build_tools/msg_examples/empty_req_srv/srv/EmptyRequest.srv (100%) rename {src => rosrust/src}/build_tools/msg_examples/geometry_msgs/msg/Point.msg (100%) rename {src => rosrust/src}/build_tools/msg_examples/geometry_msgs/msg/Pose.msg (100%) rename {src => rosrust/src}/build_tools/msg_examples/geometry_msgs/msg/PoseStamped.msg (100%) rename {src => rosrust/src}/build_tools/msg_examples/geometry_msgs/msg/Quaternion.msg (100%) rename {src => rosrust/src}/build_tools/msg_examples/geometry_msgs/msg/TwistWithCovariance.msg (100%) rename {src => rosrust/src}/build_tools/msg_examples/geometry_msgs/msg/Vector3.msg (100%) rename {src => rosrust/src}/build_tools/msg_examples/sensor_msgs/msg/Imu.msg (100%) rename {src => rosrust/src}/build_tools/msg_examples/simple_srv/srv/Something.srv (100%) rename {src => rosrust/src}/build_tools/msg_examples/std_msgs/msg/Header.msg (100%) rename {src => rosrust/src}/build_tools/msg_examples/tricky_comment_srv/srv/TrickyComment.srv (100%) rename {src => rosrust/src}/lib.rs (100%) rename {src => rosrust/src}/msg.rs (100%) rename {src => rosrust/src}/rosxmlrpc/client.rs (100%) rename {src => rosrust/src}/rosxmlrpc/error.rs (100%) rename {src => rosrust/src}/rosxmlrpc/mod.rs (100%) rename {src => rosrust/src}/rosxmlrpc/server.rs (100%) rename {src => rosrust/src}/tcpros/client.rs (100%) rename {src => rosrust/src}/tcpros/error.rs (100%) rename {src => rosrust/src}/tcpros/header.rs (100%) rename {src => rosrust/src}/tcpros/mod.rs (100%) rename {src => rosrust/src}/tcpros/publisher.rs (100%) rename {src => rosrust/src}/tcpros/service.rs (100%) rename {src => rosrust/src}/tcpros/subscriber.rs (100%) rename {src => rosrust/src}/tcpros/util/mod.rs (100%) rename {src => rosrust/src}/tcpros/util/streamfork.rs (100%) rename {src => rosrust/src}/tcpros/util/tcpconnection.rs (100%) diff --git a/.rustfmt.toml b/.rustfmt.toml new file mode 100644 index 0000000..ad0225f --- /dev/null +++ b/.rustfmt.toml @@ -0,0 +1 @@ +error_on_line_overflow = false diff --git a/Cargo.toml b/Cargo.toml index 79c203d..736083b 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,23 +1,7 @@ -[package] -authors = ["Adnan Ademovic "] -description = "Pure Rust implementation of a ROS client library" -license = "MIT" -name = "rosrust" -readme = "README.md" -repository = "https://github.com/adnanademovic/rosrust" -version = "0.5.1" +[workspace] +members = [ + "rosrust", +] -[dependencies] -byteorder = "1.0.0" -error-chain = "0.10.0" -futures = "0.1.16" -itertools = "0.6.0" -lazy_static = "0.2" -log = "0.3" -nix = "0.7" -regex = "0.2.1" -rust-crypto = "0.2.36" -serde = "1.0.2" -serde_derive = "1.0.2" -serde_rosmsg = "0.2.0" -xml-rpc = "0.0.7" +[patch.crates-io] +"rosrust" = { path = "rosrust" } diff --git a/rosrust/Cargo.toml b/rosrust/Cargo.toml new file mode 100644 index 0000000..79c203d --- /dev/null +++ b/rosrust/Cargo.toml @@ -0,0 +1,23 @@ +[package] +authors = ["Adnan Ademovic "] +description = "Pure Rust implementation of a ROS client library" +license = "MIT" +name = "rosrust" +readme = "README.md" +repository = "https://github.com/adnanademovic/rosrust" +version = "0.5.1" + +[dependencies] +byteorder = "1.0.0" +error-chain = "0.10.0" +futures = "0.1.16" +itertools = "0.6.0" +lazy_static = "0.2" +log = "0.3" +nix = "0.7" +regex = "0.2.1" +rust-crypto = "0.2.36" +serde = "1.0.2" +serde_derive = "1.0.2" +serde_rosmsg = "0.2.0" +xml-rpc = "0.0.7" diff --git a/src/api/error.rs b/rosrust/src/api/error.rs similarity index 100% rename from src/api/error.rs rename to rosrust/src/api/error.rs diff --git a/src/api/master.rs b/rosrust/src/api/master.rs similarity index 100% rename from src/api/master.rs rename to rosrust/src/api/master.rs diff --git a/src/api/mod.rs b/rosrust/src/api/mod.rs similarity index 100% rename from src/api/mod.rs rename to rosrust/src/api/mod.rs diff --git a/src/api/naming/error.rs b/rosrust/src/api/naming/error.rs similarity index 100% rename from src/api/naming/error.rs rename to rosrust/src/api/naming/error.rs diff --git a/src/api/naming/mapper.rs b/rosrust/src/api/naming/mapper.rs similarity index 100% rename from src/api/naming/mapper.rs rename to rosrust/src/api/naming/mapper.rs diff --git a/src/api/naming/mod.rs b/rosrust/src/api/naming/mod.rs similarity index 100% rename from src/api/naming/mod.rs rename to rosrust/src/api/naming/mod.rs diff --git a/src/api/naming/path.rs b/rosrust/src/api/naming/path.rs similarity index 100% rename from src/api/naming/path.rs rename to rosrust/src/api/naming/path.rs diff --git a/src/api/resolve.rs b/rosrust/src/api/resolve.rs similarity index 100% rename from src/api/resolve.rs rename to rosrust/src/api/resolve.rs diff --git a/src/api/ros.rs b/rosrust/src/api/ros.rs similarity index 100% rename from src/api/ros.rs rename to rosrust/src/api/ros.rs diff --git a/src/api/slave.rs b/rosrust/src/api/slave.rs similarity index 100% rename from src/api/slave.rs rename to rosrust/src/api/slave.rs diff --git a/src/api/slavehandler.rs b/rosrust/src/api/slavehandler.rs similarity index 100% rename from src/api/slavehandler.rs rename to rosrust/src/api/slavehandler.rs diff --git a/src/build_tools/error.rs b/rosrust/src/build_tools/error.rs similarity index 100% rename from src/build_tools/error.rs rename to rosrust/src/build_tools/error.rs diff --git a/src/build_tools/genmsg.rs b/rosrust/src/build_tools/genmsg.rs similarity index 100% rename from src/build_tools/genmsg.rs rename to rosrust/src/build_tools/genmsg.rs diff --git a/src/build_tools/helpers.rs b/rosrust/src/build_tools/helpers.rs similarity index 100% rename from src/build_tools/helpers.rs rename to rosrust/src/build_tools/helpers.rs diff --git a/src/build_tools/mod.rs b/rosrust/src/build_tools/mod.rs similarity index 100% rename from src/build_tools/mod.rs rename to rosrust/src/build_tools/mod.rs diff --git a/src/build_tools/msg.rs b/rosrust/src/build_tools/msg.rs similarity index 100% rename from src/build_tools/msg.rs rename to rosrust/src/build_tools/msg.rs diff --git a/src/build_tools/msg_examples/benchmark_msgs/msg/Overall.msg b/rosrust/src/build_tools/msg_examples/benchmark_msgs/msg/Overall.msg similarity index 100% rename from src/build_tools/msg_examples/benchmark_msgs/msg/Overall.msg rename to rosrust/src/build_tools/msg_examples/benchmark_msgs/msg/Overall.msg diff --git a/src/build_tools/msg_examples/diagnostic_msgs/srv/AddDiagnostics.srv b/rosrust/src/build_tools/msg_examples/diagnostic_msgs/srv/AddDiagnostics.srv similarity index 100% rename from src/build_tools/msg_examples/diagnostic_msgs/srv/AddDiagnostics.srv rename to rosrust/src/build_tools/msg_examples/diagnostic_msgs/srv/AddDiagnostics.srv diff --git a/src/build_tools/msg_examples/empty_req_srv/srv/EmptyRequest.srv b/rosrust/src/build_tools/msg_examples/empty_req_srv/srv/EmptyRequest.srv similarity index 100% rename from src/build_tools/msg_examples/empty_req_srv/srv/EmptyRequest.srv rename to rosrust/src/build_tools/msg_examples/empty_req_srv/srv/EmptyRequest.srv diff --git a/src/build_tools/msg_examples/geometry_msgs/msg/Point.msg b/rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Point.msg similarity index 100% rename from src/build_tools/msg_examples/geometry_msgs/msg/Point.msg rename to rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Point.msg diff --git a/src/build_tools/msg_examples/geometry_msgs/msg/Pose.msg b/rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Pose.msg similarity index 100% rename from src/build_tools/msg_examples/geometry_msgs/msg/Pose.msg rename to rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Pose.msg diff --git a/src/build_tools/msg_examples/geometry_msgs/msg/PoseStamped.msg b/rosrust/src/build_tools/msg_examples/geometry_msgs/msg/PoseStamped.msg similarity index 100% rename from src/build_tools/msg_examples/geometry_msgs/msg/PoseStamped.msg rename to rosrust/src/build_tools/msg_examples/geometry_msgs/msg/PoseStamped.msg diff --git a/src/build_tools/msg_examples/geometry_msgs/msg/Quaternion.msg b/rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Quaternion.msg similarity index 100% rename from src/build_tools/msg_examples/geometry_msgs/msg/Quaternion.msg rename to rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Quaternion.msg diff --git a/src/build_tools/msg_examples/geometry_msgs/msg/TwistWithCovariance.msg b/rosrust/src/build_tools/msg_examples/geometry_msgs/msg/TwistWithCovariance.msg similarity index 100% rename from src/build_tools/msg_examples/geometry_msgs/msg/TwistWithCovariance.msg rename to rosrust/src/build_tools/msg_examples/geometry_msgs/msg/TwistWithCovariance.msg diff --git a/src/build_tools/msg_examples/geometry_msgs/msg/Vector3.msg b/rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Vector3.msg similarity index 100% rename from src/build_tools/msg_examples/geometry_msgs/msg/Vector3.msg rename to rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Vector3.msg diff --git a/src/build_tools/msg_examples/sensor_msgs/msg/Imu.msg b/rosrust/src/build_tools/msg_examples/sensor_msgs/msg/Imu.msg similarity index 100% rename from src/build_tools/msg_examples/sensor_msgs/msg/Imu.msg rename to rosrust/src/build_tools/msg_examples/sensor_msgs/msg/Imu.msg diff --git a/src/build_tools/msg_examples/simple_srv/srv/Something.srv b/rosrust/src/build_tools/msg_examples/simple_srv/srv/Something.srv similarity index 100% rename from src/build_tools/msg_examples/simple_srv/srv/Something.srv rename to rosrust/src/build_tools/msg_examples/simple_srv/srv/Something.srv diff --git a/src/build_tools/msg_examples/std_msgs/msg/Header.msg b/rosrust/src/build_tools/msg_examples/std_msgs/msg/Header.msg similarity index 100% rename from src/build_tools/msg_examples/std_msgs/msg/Header.msg rename to rosrust/src/build_tools/msg_examples/std_msgs/msg/Header.msg diff --git a/src/build_tools/msg_examples/tricky_comment_srv/srv/TrickyComment.srv b/rosrust/src/build_tools/msg_examples/tricky_comment_srv/srv/TrickyComment.srv similarity index 100% rename from src/build_tools/msg_examples/tricky_comment_srv/srv/TrickyComment.srv rename to rosrust/src/build_tools/msg_examples/tricky_comment_srv/srv/TrickyComment.srv diff --git a/src/lib.rs b/rosrust/src/lib.rs similarity index 100% rename from src/lib.rs rename to rosrust/src/lib.rs diff --git a/src/msg.rs b/rosrust/src/msg.rs similarity index 100% rename from src/msg.rs rename to rosrust/src/msg.rs diff --git a/src/rosxmlrpc/client.rs b/rosrust/src/rosxmlrpc/client.rs similarity index 100% rename from src/rosxmlrpc/client.rs rename to rosrust/src/rosxmlrpc/client.rs diff --git a/src/rosxmlrpc/error.rs b/rosrust/src/rosxmlrpc/error.rs similarity index 100% rename from src/rosxmlrpc/error.rs rename to rosrust/src/rosxmlrpc/error.rs diff --git a/src/rosxmlrpc/mod.rs b/rosrust/src/rosxmlrpc/mod.rs similarity index 100% rename from src/rosxmlrpc/mod.rs rename to rosrust/src/rosxmlrpc/mod.rs diff --git a/src/rosxmlrpc/server.rs b/rosrust/src/rosxmlrpc/server.rs similarity index 100% rename from src/rosxmlrpc/server.rs rename to rosrust/src/rosxmlrpc/server.rs diff --git a/src/tcpros/client.rs b/rosrust/src/tcpros/client.rs similarity index 100% rename from src/tcpros/client.rs rename to rosrust/src/tcpros/client.rs diff --git a/src/tcpros/error.rs b/rosrust/src/tcpros/error.rs similarity index 100% rename from src/tcpros/error.rs rename to rosrust/src/tcpros/error.rs diff --git a/src/tcpros/header.rs b/rosrust/src/tcpros/header.rs similarity index 100% rename from src/tcpros/header.rs rename to rosrust/src/tcpros/header.rs diff --git a/src/tcpros/mod.rs b/rosrust/src/tcpros/mod.rs similarity index 100% rename from src/tcpros/mod.rs rename to rosrust/src/tcpros/mod.rs diff --git a/src/tcpros/publisher.rs b/rosrust/src/tcpros/publisher.rs similarity index 100% rename from src/tcpros/publisher.rs rename to rosrust/src/tcpros/publisher.rs diff --git a/src/tcpros/service.rs b/rosrust/src/tcpros/service.rs similarity index 100% rename from src/tcpros/service.rs rename to rosrust/src/tcpros/service.rs diff --git a/src/tcpros/subscriber.rs b/rosrust/src/tcpros/subscriber.rs similarity index 100% rename from src/tcpros/subscriber.rs rename to rosrust/src/tcpros/subscriber.rs diff --git a/src/tcpros/util/mod.rs b/rosrust/src/tcpros/util/mod.rs similarity index 100% rename from src/tcpros/util/mod.rs rename to rosrust/src/tcpros/util/mod.rs diff --git a/src/tcpros/util/streamfork.rs b/rosrust/src/tcpros/util/streamfork.rs similarity index 100% rename from src/tcpros/util/streamfork.rs rename to rosrust/src/tcpros/util/streamfork.rs diff --git a/src/tcpros/util/tcpconnection.rs b/rosrust/src/tcpros/util/tcpconnection.rs similarity index 100% rename from src/tcpros/util/tcpconnection.rs rename to rosrust/src/tcpros/util/tcpconnection.rs From 67ddbfbe655dcb2fd0adb458ff1eae546a7b4b2c Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 13 Dec 2017 11:35:29 +0100 Subject: [PATCH 15/50] Add publisher/subscriber example --- Cargo.toml | 1 + examples/pubsub/Cargo.toml | 24 ++++++++++++++++++++++++ examples/pubsub/README.md | 5 +++++ examples/pubsub/build.rs | 4 ++++ examples/pubsub/src/publisher.rs | 28 ++++++++++++++++++++++++++++ examples/pubsub/src/subscriber.rs | 21 +++++++++++++++++++++ 6 files changed, 83 insertions(+) create mode 100644 examples/pubsub/Cargo.toml create mode 100644 examples/pubsub/README.md create mode 100644 examples/pubsub/build.rs create mode 100644 examples/pubsub/src/publisher.rs create mode 100644 examples/pubsub/src/subscriber.rs diff --git a/Cargo.toml b/Cargo.toml index 736083b..fd0e2ec 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,5 +1,6 @@ [workspace] members = [ + "examples/pubsub", "rosrust", ] diff --git a/examples/pubsub/Cargo.toml b/examples/pubsub/Cargo.toml new file mode 100644 index 0000000..4722c1c --- /dev/null +++ b/examples/pubsub/Cargo.toml @@ -0,0 +1,24 @@ +[package] +authors = ["Adnan Ademovic "] +license = "MIT" +name = "pubsub" +repository = "https://github.com/adnanademovic/rosrust" +version = "0.0.1" + +[dependencies] +env_logger = "0.4.3" +rosrust = "*" +serde = "1.0.24" +serde_derive = "1.0.24" + +[[bin]] +name = "publisher" +path = "src/publisher.rs" + +[[bin]] +name = "subscriber" +path = "src/subscriber.rs" + +[build-dependencies] +env_logger = "0.4.3" +rosrust = "*" diff --git a/examples/pubsub/README.md b/examples/pubsub/README.md new file mode 100644 index 0000000..e366166 --- /dev/null +++ b/examples/pubsub/README.md @@ -0,0 +1,5 @@ +# Publisher and Subscriber example + +Rust version of the publisher/subscriber tutorial, similar to the [Python version](http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28python%29) and [C++ version](http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29). + +The two versions have minor differences, so we went with the C++ version to immitate. diff --git a/examples/pubsub/build.rs b/examples/pubsub/build.rs new file mode 100644 index 0000000..72dc60e --- /dev/null +++ b/examples/pubsub/build.rs @@ -0,0 +1,4 @@ +#[macro_use] +extern crate rosrust; + +rosmsg_main!("std_msgs/String"); diff --git a/examples/pubsub/src/publisher.rs b/examples/pubsub/src/publisher.rs new file mode 100644 index 0000000..a3ebf76 --- /dev/null +++ b/examples/pubsub/src/publisher.rs @@ -0,0 +1,28 @@ +extern crate env_logger; +#[macro_use] +extern crate rosrust; + +use rosrust::Ros; +use std::{thread, time}; + +rosmsg_include!(); + +fn main() { + env_logger::init().unwrap(); + + let mut ros = Ros::new("talker").unwrap(); + let mut chatter_pub = ros.publish("chatter").unwrap(); + + let mut count = 0; + + loop { + let mut msg = msg::std_msgs::String::new(); + msg.data = format!("hello world {}", count); + + chatter_pub.send(msg).unwrap(); + + thread::sleep(time::Duration::from_millis(100)); + + count += 1; + } +} diff --git a/examples/pubsub/src/subscriber.rs b/examples/pubsub/src/subscriber.rs new file mode 100644 index 0000000..b89cb8b --- /dev/null +++ b/examples/pubsub/src/subscriber.rs @@ -0,0 +1,21 @@ +extern crate env_logger; +#[macro_use] +extern crate rosrust; + +use rosrust::Ros; +use std::{thread, time}; + +rosmsg_include!(); + +fn main() { + env_logger::init().unwrap(); + + let mut ros = Ros::new("listener").unwrap(); + + ros.subscribe("chatter", |v: msg::std_msgs::String| println!("{}", v.data)) + .unwrap(); + + loop { + thread::sleep(time::Duration::from_secs(100)); + } +} From 332ab53b28f850b9807c2a7187c4d187fd496d03 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 13 Dec 2017 11:42:49 +0100 Subject: [PATCH 16/50] Update dependencies --- rosrust/Cargo.toml | 20 ++++++++++---------- rosrust/src/api/slavehandler.rs | 2 +- rosrust/src/tcpros/error.rs | 4 +--- 3 files changed, 12 insertions(+), 14 deletions(-) diff --git a/rosrust/Cargo.toml b/rosrust/Cargo.toml index 79c203d..6c60208 100644 --- a/rosrust/Cargo.toml +++ b/rosrust/Cargo.toml @@ -8,16 +8,16 @@ repository = "https://github.com/adnanademovic/rosrust" version = "0.5.1" [dependencies] -byteorder = "1.0.0" -error-chain = "0.10.0" -futures = "0.1.16" -itertools = "0.6.0" -lazy_static = "0.2" -log = "0.3" -nix = "0.7" -regex = "0.2.1" +byteorder = "1.2.1" +error-chain = "0.11.0" +futures = "0.1.17" +itertools = "0.7.4" +lazy_static = "1.0.0" +log = "0.3.8" +nix = "0.9.0" +regex = "0.2.3" rust-crypto = "0.2.36" -serde = "1.0.2" -serde_derive = "1.0.2" +serde = "1.0.24" +serde_derive = "1.0.24" serde_rosmsg = "0.2.0" xml-rpc = "0.0.7" diff --git a/rosrust/src/api/slavehandler.rs b/rosrust/src/api/slavehandler.rs index 37e5453..8a1c0ca 100644 --- a/rosrust/src/api/slavehandler.rs +++ b/rosrust/src/api/slavehandler.rs @@ -69,7 +69,7 @@ impl SlaveHandler { } }); - server.register_value("getPid", "PID", |_args| Ok(Value::Int(getpid()))); + server.register_value("getPid", "PID", |_args| Ok(Value::Int(getpid().into()))); let subscriptions = Arc::new(Mutex::new(HashMap::::new())); let subs = subscriptions.clone(); diff --git a/rosrust/src/tcpros/error.rs b/rosrust/src/tcpros/error.rs index 4a0ab22..ebb3910 100644 --- a/rosrust/src/tcpros/error.rs +++ b/rosrust/src/tcpros/error.rs @@ -1,9 +1,7 @@ error_chain! { foreign_links { Io(::std::io::Error); - } - links { - SerdeRosmsg(::serde_rosmsg::error::Error, ::serde_rosmsg::error::ErrorKind); + SerdeRosmsg(::serde_rosmsg::error::Error); } errors { ServiceConnectionFail(service: String, uri: String) { From 5e5c14c16e1c5ae9d0d440e1626a32fe3762ce25 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 13 Dec 2017 12:06:06 +0100 Subject: [PATCH 17/50] Add service and client example --- Cargo.toml | 1 + examples/serviceclient/Cargo.toml | 24 +++++++++++++++ examples/serviceclient/README.md | 5 +++ examples/serviceclient/build.rs | 4 +++ examples/serviceclient/src/client.rs | 44 +++++++++++++++++++++++++++ examples/serviceclient/src/service.rs | 22 ++++++++++++++ 6 files changed, 100 insertions(+) create mode 100644 examples/serviceclient/Cargo.toml create mode 100644 examples/serviceclient/README.md create mode 100644 examples/serviceclient/build.rs create mode 100644 examples/serviceclient/src/client.rs create mode 100644 examples/serviceclient/src/service.rs diff --git a/Cargo.toml b/Cargo.toml index fd0e2ec..4a33fce 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,7 @@ [workspace] members = [ "examples/pubsub", + "examples/serviceclient", "rosrust", ] diff --git a/examples/serviceclient/Cargo.toml b/examples/serviceclient/Cargo.toml new file mode 100644 index 0000000..84394f5 --- /dev/null +++ b/examples/serviceclient/Cargo.toml @@ -0,0 +1,24 @@ +[package] +authors = ["Adnan Ademovic "] +license = "MIT" +name = "serviceclient" +repository = "https://github.com/adnanademovic/rosrust" +version = "0.0.1" + +[dependencies] +env_logger = "0.4.3" +rosrust = "*" +serde = "1.0.24" +serde_derive = "1.0.24" + +[[bin]] +name = "client" +path = "src/client.rs" + +[[bin]] +name = "service" +path = "src/service.rs" + +[build-dependencies] +env_logger = "0.4.3" +rosrust = "*" diff --git a/examples/serviceclient/README.md b/examples/serviceclient/README.md new file mode 100644 index 0000000..f6f6948 --- /dev/null +++ b/examples/serviceclient/README.md @@ -0,0 +1,5 @@ +# Service and Client example + +Rust version of the service/client tutorial, similar to the [Python version](http://wiki.ros.org/ROS/Tutorials/WritingServiceClient%28python%29) and [C++ version](http://wiki.ros.org/ROS/Tutorials/WritingServiceClient%28c%2B%2B%29). + +The two versions have minor differences, so we went with the C++ version to immitate. diff --git a/examples/serviceclient/build.rs b/examples/serviceclient/build.rs new file mode 100644 index 0000000..b4f9463 --- /dev/null +++ b/examples/serviceclient/build.rs @@ -0,0 +1,4 @@ +#[macro_use] +extern crate rosrust; + +rosmsg_main!("roscpp_tutorials/TwoInts"); diff --git a/examples/serviceclient/src/client.rs b/examples/serviceclient/src/client.rs new file mode 100644 index 0000000..4cff760 --- /dev/null +++ b/examples/serviceclient/src/client.rs @@ -0,0 +1,44 @@ +extern crate env_logger; +#[macro_use] +extern crate rosrust; + +use rosrust::Ros; +use std::{env, thread, time}; + +rosmsg_include!(); + +fn main() { + env_logger::init().unwrap(); + + let args: Vec<_> = env::args().collect(); + + if args.len() != 3 { + println!("usage: client X Y"); + return; + } + + let a = args[1].parse::().unwrap(); + let b = args[2].parse::().unwrap(); + + let ros = Ros::new("add_two_ints_client").unwrap(); + let client = ros.client::("add_two_ints") + .unwrap(); + + // Sync approach + println!( + "{} + {} = {}", + a, + b, + client + .req(&msg::roscpp_tutorials::TwoIntsReq { a, b }) + .unwrap() + .unwrap() + .sum + ); + + // Async approach + let retval = client.req_async(msg::roscpp_tutorials::TwoIntsReq { a, b }); + println!("{} + {} = {}", a, b, retval.read().unwrap().unwrap().sum); + + thread::sleep(time::Duration::from_secs(1)); +} diff --git a/examples/serviceclient/src/service.rs b/examples/serviceclient/src/service.rs new file mode 100644 index 0000000..e14a9f2 --- /dev/null +++ b/examples/serviceclient/src/service.rs @@ -0,0 +1,22 @@ +extern crate env_logger; +#[macro_use] +extern crate rosrust; + +use rosrust::Ros; +use std::{thread, time}; + +rosmsg_include!(); + +fn main() { + env_logger::init().unwrap(); + + let mut ros = Ros::new("add_two_ints_server").unwrap(); + + ros.service::("add_two_ints", |req| { + Ok(msg::roscpp_tutorials::TwoIntsRes { sum: req.a + req.b }) + }).unwrap(); + + loop { + thread::sleep(time::Duration::from_secs(100)); + } +} From 9d8a16a90daf68b7a10e37c605186398cc7b2f44 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 13 Dec 2017 13:39:42 +0100 Subject: [PATCH 18/50] Get rid of warnings --- rosrust/src/api/ros.rs | 2 -- rosrust/src/tcpros/client.rs | 2 +- rosrust/src/tcpros/service.rs | 8 ++++---- rosrust/src/tcpros/subscriber.rs | 4 ++-- 4 files changed, 7 insertions(+), 9 deletions(-) diff --git a/rosrust/src/api/ros.rs b/rosrust/src/api/ros.rs index f8aa73b..759317b 100644 --- a/rosrust/src/api/ros.rs +++ b/rosrust/src/api/ros.rs @@ -109,7 +109,6 @@ impl Ros { Ok(_) => return Ok(()), Err(e) => e, }; - println!("{:?}", e); match e { ResponseError::Client(ref m) if m == "no provider" => { if let Some(ref timeout) = timeout { @@ -124,7 +123,6 @@ impl Ros { } return Err(e.into()); } - unreachable!(); } pub fn service(&mut self, service: &str, handler: F) -> Result<()> diff --git a/rosrust/src/tcpros/client.rs b/rosrust/src/tcpros/client.rs index 69195bd..6981778 100644 --- a/rosrust/src/tcpros/client.rs +++ b/rosrust/src/tcpros/client.rs @@ -132,7 +132,7 @@ where Ok(()) } -fn exchange_headers(mut stream: &mut U, caller_id: &str, service: &str) -> Result<()> +fn exchange_headers(stream: &mut U, caller_id: &str, service: &str) -> Result<()> where T: ServicePair, U: std::io::Write + std::io::Read, diff --git a/rosrust/src/tcpros/service.rs b/rosrust/src/tcpros/service.rs index 64042dc..5d08af7 100644 --- a/rosrust/src/tcpros/service.rs +++ b/rosrust/src/tcpros/service.rs @@ -95,7 +95,7 @@ where } } -fn exchange_headers(mut stream: &mut U, service: &str, node_name: &str) -> Result<()> +fn exchange_headers(stream: &mut U, service: &str, node_name: &str) -> Result<()> where T: ServicePair, U: std::io::Write + std::io::Read, @@ -104,7 +104,7 @@ where write_response::(stream, node_name) } -fn read_request(mut stream: &mut U, service: &str) -> Result<()> { +fn read_request(stream: &mut U, service: &str) -> Result<()> { let fields = header::decode(stream)?; header::match_field(&fields, "service", service)?; if fields.get("callerid").is_none() { @@ -116,7 +116,7 @@ fn read_request(mut stream: &mut U, service: & header::match_field(&fields, "md5sum", &T::md5sum()) } -fn write_response(mut stream: &mut U, node_name: &str) -> Result<()> +fn write_response(stream: &mut U, node_name: &str) -> Result<()> where T: ServicePair, U: std::io::Write, @@ -125,7 +125,7 @@ where fields.insert(String::from("callerid"), String::from(node_name)); fields.insert(String::from("md5sum"), T::md5sum()); fields.insert(String::from("type"), T::msg_type()); - header::encode(&mut stream, &fields)?; + header::encode(stream, &fields)?; Ok(()) } diff --git a/rosrust/src/tcpros/subscriber.rs b/rosrust/src/tcpros/subscriber.rs index ed9c525..445e5a2 100644 --- a/rosrust/src/tcpros/subscriber.rs +++ b/rosrust/src/tcpros/subscriber.rs @@ -148,7 +148,7 @@ fn read_response(mut stream: &mut U) -> Result<()> match_field(&fields, "type", &T::msg_type()) } -fn exchange_headers(mut stream: &mut U, caller_id: &str, topic: &str) -> Result<()> +fn exchange_headers(stream: &mut U, caller_id: &str, topic: &str) -> Result<()> where T: Message, U: std::io::Write + std::io::Read, @@ -158,7 +158,7 @@ where } #[inline] -fn package_to_vector(mut stream: &mut R) -> std::io::Result> { +fn package_to_vector(stream: &mut R) -> std::io::Result> { let length = stream.read_u32::()?; let mut buffer = length_vector(length)?; buffer.resize(length as usize + 4, 0); From 31b7e90d5d20e4f1d30525bdc801c3028e614661 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 13 Dec 2017 14:57:43 +0100 Subject: [PATCH 19/50] Fix md5 hashes --- rosrust/src/build_tools/helpers.rs | 1 - rosrust/src/build_tools/msg.rs | 21 ++++++++++++++++----- 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/rosrust/src/build_tools/helpers.rs b/rosrust/src/build_tools/helpers.rs index 609e1af..e4ae2a9 100644 --- a/rosrust/src/build_tools/helpers.rs +++ b/rosrust/src/build_tools/helpers.rs @@ -35,7 +35,6 @@ pub fn calculate_md5(message_map: &MessageMap) -> Result v, None => bail!("Message map does not contain all needed elements"), }; - println!("|{}|{}|", req, res); hashes.insert( (pack.clone(), name.clone()), calculate_md5_from_representation(&format!("{}{}", req, res)), diff --git a/rosrust/src/build_tools/msg.rs b/rosrust/src/build_tools/msg.rs index b45f6e8..bce114c 100644 --- a/rosrust/src/build_tools/msg.rs +++ b/rosrust/src/build_tools/msg.rs @@ -338,11 +338,12 @@ impl FieldInfo { hashes: &HashMap<(String, String), String>, ) -> ::std::result::Result { let datatype = self.datatype.md5_string(package, hashes)?; - Ok(match self.case { - FieldCase::Unit => format!("{} {}", datatype, self.name), - FieldCase::Vector => format!("{}[] {}", datatype, self.name), - FieldCase::Array(l) => format!("{}[{}] {}", datatype, l, self.name), - FieldCase::Const(ref v) => format!("{} {}={}", datatype, self.name, v), + Ok(match (self.datatype.is_builtin(), &self.case) { + (_, &FieldCase::Const(ref v)) => format!("{} {}={}", datatype, self.name, v), + (false, _) | + (_, &FieldCase::Unit) => format!("{} {}", datatype, self.name), + (true, &FieldCase::Vector) => format!("{}[] {}", datatype, self.name), + (true, &FieldCase::Array(l)) => format!("{}[{}] {}", datatype, l, self.name), }) } @@ -434,6 +435,16 @@ impl DataType { } } + fn is_builtin(&self) -> bool { + match *self { + DataType::Bool | DataType::I8 | DataType::I16 | DataType::I32 | DataType::I64 | + DataType::U8 | DataType::U16 | DataType::U32 | DataType::U64 | DataType::F32 | + DataType::F64 | DataType::String | DataType::Time | DataType::Duration => true, + DataType::LocalStruct(_) | + DataType::RemoteStruct(_, _) => false, + } + } + fn rust_newtype(&self) -> String { match *self { DataType::Bool => "false".into(), From 9282af14b6504f3e64f0d5213ab51ce8d0f248f8 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 13 Dec 2017 15:02:06 +0100 Subject: [PATCH 20/50] Fix tests for hashes --- rosrust/src/build_tools/msg.rs | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/rosrust/src/build_tools/msg.rs b/rosrust/src/build_tools/msg.rs index bce114c..cb721c1 100644 --- a/rosrust/src/build_tools/msg.rs +++ b/rosrust/src/build_tools/msg.rs @@ -592,6 +592,13 @@ mod tests { .unwrap(), "float32[3] abc".to_owned() ); + assert_eq!( + FieldInfo::new("int32", "abc", FieldCase::Vector) + .unwrap() + .md5_string("", &hashes) + .unwrap(), + "int32[] abc".to_owned() + ); assert_eq!( FieldInfo::new("string", "abc", FieldCase::Const("something".into())) .unwrap() @@ -604,7 +611,14 @@ mod tests { .unwrap() .md5_string("p1", &hashes) .unwrap(), - "ABCD[] abc".to_owned() + "ABCD abc".to_owned() + ); + assert_eq!( + FieldInfo::new("xx", "abc", FieldCase::Array(3)) + .unwrap() + .md5_string("p1", &hashes) + .unwrap(), + "ABCD abc".to_owned() ); assert_eq!( FieldInfo::new("p2/xx", "abc", FieldCase::Unit) From 23608cd6d21f99cba73570b366a756bc57324a2b Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 13 Dec 2017 15:22:19 +0100 Subject: [PATCH 21/50] Fix all tests --- rosrust/src/build_tools/genmsg.rs | 15 ++++----------- rosrust/src/build_tools/helpers.rs | 26 ++++++++++---------------- 2 files changed, 14 insertions(+), 27 deletions(-) diff --git a/rosrust/src/build_tools/genmsg.rs b/rosrust/src/build_tools/genmsg.rs index a69f0fe..d15e71a 100644 --- a/rosrust/src/build_tools/genmsg.rs +++ b/rosrust/src/build_tools/genmsg.rs @@ -127,20 +127,13 @@ fn string_into_pair<'a>(input: &'a str) -> Result<(&'a str, &'a str)> { #[cfg(test)] mod tests { use super::*; - use std::path::Path; - lazy_static! { - static ref FILEPATH: String = Path::new(file!()) - .parent().unwrap() - .join("msg_examples") - .to_str().unwrap() - .into(); - } + static FILEPATH: &'static str = "src/build_tools/msg_examples"; #[test] fn depend_on_messages_printout() { let data = depend_on_messages( - &[&FILEPATH], + &[FILEPATH], &["geometry_msgs/PoseStamped", "sensor_msgs/Imu"], ).unwrap(); println!("{}", data); @@ -149,14 +142,14 @@ mod tests { #[test] fn benchmark_genmsg() { - let data = depend_on_messages(&[&FILEPATH], &["benchmark_msgs/Overall"]).unwrap(); + let data = depend_on_messages(&[FILEPATH], &["benchmark_msgs/Overall"]).unwrap(); println!("{}", data); // TODO: actually test this output data } #[test] fn benchmark_genmsg_service() { - let data = depend_on_messages(&[&FILEPATH], &["simple_srv/Something"]).unwrap(); + let data = depend_on_messages(&[FILEPATH], &["simple_srv/Something"]).unwrap(); println!("{}", data); // TODO: actually test this output data } diff --git a/rosrust/src/build_tools/helpers.rs b/rosrust/src/build_tools/helpers.rs index e4ae2a9..ec9da6d 100644 --- a/rosrust/src/build_tools/helpers.rs +++ b/rosrust/src/build_tools/helpers.rs @@ -187,17 +187,11 @@ fn get_message(folders: &[&str], package: &str, name: &str) -> Result Date: Thu, 14 Dec 2017 11:47:55 +0100 Subject: [PATCH 22/50] Update client example --- examples/serviceclient/src/client.rs | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/examples/serviceclient/src/client.rs b/examples/serviceclient/src/client.rs index 4cff760..ebbf281 100644 --- a/examples/serviceclient/src/client.rs +++ b/examples/serviceclient/src/client.rs @@ -3,7 +3,7 @@ extern crate env_logger; extern crate rosrust; use rosrust::Ros; -use std::{env, thread, time}; +use std::{env, time}; rosmsg_include!(); @@ -21,6 +21,10 @@ fn main() { let b = args[2].parse::().unwrap(); let ros = Ros::new("add_two_ints_client").unwrap(); + + ros.wait_for_service("add_two_ints", Some(time::Duration::from_secs(10))) + .unwrap(); + let client = ros.client::("add_two_ints") .unwrap(); @@ -39,6 +43,4 @@ fn main() { // Async approach let retval = client.req_async(msg::roscpp_tutorials::TwoIntsReq { a, b }); println!("{} + {} = {}", a, b, retval.read().unwrap().unwrap().sum); - - thread::sleep(time::Duration::from_secs(1)); } From a04dbc47f700de6b13ba64b2b236e3d89fa2bab1 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Thu, 14 Dec 2017 12:19:06 +0100 Subject: [PATCH 23/50] Add parameters example --- Cargo.toml | 1 + examples/parameters/Cargo.toml | 20 +++++++ examples/parameters/README.md | 5 ++ examples/parameters/src/parameters.rs | 78 +++++++++++++++++++++++++++ 4 files changed, 104 insertions(+) create mode 100644 examples/parameters/Cargo.toml create mode 100644 examples/parameters/README.md create mode 100644 examples/parameters/src/parameters.rs diff --git a/Cargo.toml b/Cargo.toml index 4a33fce..8d45041 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,5 +1,6 @@ [workspace] members = [ + "examples/parameters", "examples/pubsub", "examples/serviceclient", "rosrust", diff --git a/examples/parameters/Cargo.toml b/examples/parameters/Cargo.toml new file mode 100644 index 0000000..00bb5e0 --- /dev/null +++ b/examples/parameters/Cargo.toml @@ -0,0 +1,20 @@ +[package] +authors = ["Adnan Ademovic "] +license = "MIT" +name = "parameters" +repository = "https://github.com/adnanademovic/rosrust" +version = "0.0.1" + +[dependencies] +env_logger = "0.4.3" +rosrust = "*" +serde = "1.0.24" +serde_derive = "1.0.24" + +[[bin]] +name = "parameters" +path = "src/parameters.rs" + +[build-dependencies] +env_logger = "0.4.3" +rosrust = "*" diff --git a/examples/parameters/README.md b/examples/parameters/README.md new file mode 100644 index 0000000..488b859 --- /dev/null +++ b/examples/parameters/README.md @@ -0,0 +1,5 @@ +# Parameters example + +Creates a node, creates parameters, prints them, and prints the whole parameter tree. + +Comparing the results with `rosparam` is recommended. diff --git a/examples/parameters/src/parameters.rs b/examples/parameters/src/parameters.rs new file mode 100644 index 0000000..0ca2a81 --- /dev/null +++ b/examples/parameters/src/parameters.rs @@ -0,0 +1,78 @@ +extern crate env_logger; +extern crate rosrust; +#[macro_use] +extern crate serde_derive; + +use rosrust::Ros; + +fn main() { + env_logger::init().unwrap(); + + let ros = Ros::new("param_test").unwrap(); + + // Create parameter, go through all methods, and delete it + let param = ros.param("~foo").unwrap(); + println!("Handling ~foo:"); + println!("Exists? {:?}", param.exists()); // false + param.set(&42u64).unwrap(); + println!("Get: {:?}", param.get::().unwrap()); + println!("Get raw: {:?}", param.get_raw().unwrap()); + println!("Search: {:?}", param.search().unwrap()); + println!("Exists? {}", param.exists().unwrap()); + param.delete().unwrap(); + println!("Get {:?}", param.get::().unwrap_err()); + println!("Exists? {}", param.exists().unwrap()); + + // Same as before, but don't delete it + let param = ros.param("bar").unwrap(); + println!("Handling bar:"); + param.set(&"string data").unwrap(); + println!("Get: {:?}", param.get::().unwrap()); + println!("Get raw: {:?}", param.get_raw().unwrap()); + println!("Search: {:?}", param.search().unwrap()); + println!("Exists? {}", param.exists().unwrap()); + + // Access existing parameter + let param = ros.param("/baz").unwrap(); + println!("Handling /baz:"); + if param.exists().unwrap() { + println!("Get raw: {:?}", param.get_raw().unwrap()); + println!("Search: {:?}", param.search().unwrap()); + } else { + println!("Create own parameter /baz with 'rosparam' to observe interaction."); + } + + #[derive(Debug, Deserialize, Serialize)] + struct TestStruct { + foo: String, + bar: i32, + baz: bool, + } + + // Create tree and output the end result + println!("Handling /qux:"); + println!("Setting /qux/alpha"); + ros.param("/qux/alpha").unwrap().set(&"meow").unwrap(); + println!("Setting /qux/beta"); + ros.param("/qux/beta").unwrap().set(&44).unwrap(); + println!("Setting /qux/gamma/x"); + ros.param("/qux/gamma/x").unwrap().set(&3.0).unwrap(); + println!("Setting /qux/gamma/y"); + ros.param("/qux/gamma/y").unwrap().set(&2).unwrap(); + println!("Setting /qux/gamma/z"); + ros.param("/qux/gamma/z").unwrap().set(&true).unwrap(); + println!("Setting /qux/delta"); + ros.param("/qux/delta").unwrap().set(&[1, 2, 3]).unwrap(); + ros.param("/qux/epsilon") + .unwrap() + .set(&TestStruct { + foo: "x".into(), + bar: 42, + baz: false, + }) + .unwrap(); + println!( + "Get raw: {:?}", + ros.param("/qux").unwrap().get_raw().unwrap() + ); +} From 11042a96997ba89f36466a19fecf9739da9bd8c3 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 20 Dec 2017 11:43:21 +0100 Subject: [PATCH 24/50] Add RAII to pub/sub/service --- examples/pubsub/src/subscriber.rs | 6 +- examples/serviceclient/src/service.rs | 3 +- rosrust/src/api/mod.rs | 1 + rosrust/src/api/raii.rs | 195 ++++++++++++++++++++++++++ rosrust/src/api/ros.rs | 63 ++------- rosrust/src/api/slave.rs | 6 +- rosrust/src/tcpros/publisher.rs | 1 + 7 files changed, 217 insertions(+), 58 deletions(-) create mode 100644 rosrust/src/api/raii.rs diff --git a/examples/pubsub/src/subscriber.rs b/examples/pubsub/src/subscriber.rs index b89cb8b..7e13535 100644 --- a/examples/pubsub/src/subscriber.rs +++ b/examples/pubsub/src/subscriber.rs @@ -12,8 +12,10 @@ fn main() { let mut ros = Ros::new("listener").unwrap(); - ros.subscribe("chatter", |v: msg::std_msgs::String| println!("{}", v.data)) - .unwrap(); + // The subscriber is stopped when the returned object is destroyed + let _subscriber_raii = + ros.subscribe("chatter", |v: msg::std_msgs::String| println!("{}", v.data)) + .unwrap(); loop { thread::sleep(time::Duration::from_secs(100)); diff --git a/examples/serviceclient/src/service.rs b/examples/serviceclient/src/service.rs index e14a9f2..ad3a3af 100644 --- a/examples/serviceclient/src/service.rs +++ b/examples/serviceclient/src/service.rs @@ -12,7 +12,8 @@ fn main() { let mut ros = Ros::new("add_two_ints_server").unwrap(); - ros.service::("add_two_ints", |req| { + // The service is stopped when the returned object is destroyed + let _service_raii = ros.service::("add_two_ints", |req| { Ok(msg::roscpp_tutorials::TwoIntsRes { sum: req.a + req.b }) }).unwrap(); diff --git a/rosrust/src/api/mod.rs b/rosrust/src/api/mod.rs index e984897..b6edb53 100644 --- a/rosrust/src/api/mod.rs +++ b/rosrust/src/api/mod.rs @@ -6,4 +6,5 @@ mod slave; mod ros; mod slavehandler; mod naming; +mod raii; mod resolve; diff --git a/rosrust/src/api/raii.rs b/rosrust/src/api/raii.rs new file mode 100644 index 0000000..51abb46 --- /dev/null +++ b/rosrust/src/api/raii.rs @@ -0,0 +1,195 @@ +use super::error::Result; +use super::master::Master; +use super::slave::Slave; +use rosxmlrpc::Response; +use std::sync::Arc; +use tcpros::{Message, PublisherStream, ServicePair, ServiceResult}; + +#[derive(Clone)] +pub struct Publisher<'a, T: Message> { + stream: PublisherStream, + _raii: Arc>>, +} + +impl<'a, T: Message> Publisher<'a, T> { + pub(crate) fn new( + master: &'a Master, + slave: &'a mut Slave, + hostname: &str, + name: &str, + ) -> Result { + let stream = slave.add_publication::(hostname, name)?; + let mut info = PublisherInfo { + master, + slave, + name: name.into(), + }; + + match master.register_publisher(&name, &T::msg_type()) { + Ok(_) => Ok(Self { + stream, + _raii: Arc::new(InteractorRaii::new(info)), + }), + Err(error) => { + error!( + "Failed to register publisher for topic '{}': {}", + name, + error + ); + info.unregister()?; + Err(error.into()) + } + } + } + + #[inline] + pub fn send(&mut self, message: T) -> Result<()> { + self.stream.send(message).map_err(|v| v.into()) + } +} + +struct PublisherInfo<'a> { + master: &'a Master, + slave: &'a Slave, + name: String, +} + +impl<'a> Interactor for PublisherInfo<'a> { + fn unregister(&mut self) -> Response<()> { + self.slave.remove_publication(&self.name); + self.master.unregister_publisher(&self.name).map(|_| ()) + } +} + +#[derive(Clone)] +pub struct Subscriber<'a> { + _raii: Arc>>, +} + +impl<'a> Subscriber<'a> { + pub(crate) fn new () + Send + 'static>( + master: &'a Master, + slave: &'a mut Slave, + name: &str, + callback: F, + ) -> Result { + slave.add_subscription::(name, callback)?; + + match master.register_subscriber(name, &T::msg_type()) { + Ok(publishers) => { + if let Err(err) = slave.add_publishers_to_subscription( + &name, + publishers.into_iter(), + ) + { + error!( + "Failed to subscribe to all publishers of topic '{}': {}", + name, + err + ); + } + Ok(Self { + _raii: Arc::new(InteractorRaii::new(SubscriberInfo { + master, + slave, + name: name.into(), + })), + }) + } + Err(err) => { + SubscriberInfo { + master, + slave, + name: name.into(), + }.unregister()?; + Err(err.into()) + } + } + } +} + +struct SubscriberInfo<'a> { + master: &'a Master, + slave: &'a Slave, + name: String, +} + +impl<'a> Interactor for SubscriberInfo<'a> { + fn unregister(&mut self) -> Response<()> { + self.slave.remove_subscription(&self.name); + self.master.unregister_subscriber(&self.name).map(|_| ()) + } +} + +#[derive(Clone)] +pub struct Service<'a> { + _raii: Arc>>, +} + +impl<'a> Service<'a> { + pub(crate) fn new( + master: &'a Master, + slave: &'a mut Slave, + hostname: &str, + name: &str, + handler: F, + ) -> Result + where + T: ServicePair, + F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static, + { + let api = slave.add_service::(hostname, name, handler)?; + + let mut info = ServiceInfo { + master, + slave, + api, + name: name.into(), + }; + + if let Err(err) = master.register_service(name, &info.api) { + info.unregister()?; + Err(err.into()) + } else { + Ok(Self { _raii: Arc::new(InteractorRaii::new(info)) }) + } + } +} + +struct ServiceInfo<'a> { + master: &'a Master, + slave: &'a Slave, + name: String, + api: String, +} + +impl<'a> Interactor for ServiceInfo<'a> { + fn unregister(&mut self) -> Response<()> { + self.slave.remove_service(&self.name); + self.master.unregister_service(&self.name, &self.api).map( + |_| (), + ) + } +} + +trait Interactor { + fn unregister(&mut self) -> Response<()>; +} + +struct InteractorRaii { + interactor: I, +} + +impl InteractorRaii { + pub fn new(interactor: I) -> InteractorRaii { + Self { interactor } + } +} + +impl Drop for InteractorRaii { + fn drop(&mut self) { + if let Err(e) = self.interactor.unregister() { + error!("Error while unloading: {:?}", e); + } + } +} diff --git a/rosrust/src/api/ros.rs b/rosrust/src/api/ros.rs index 759317b..beaa1b8 100644 --- a/rosrust/src/api/ros.rs +++ b/rosrust/src/api/ros.rs @@ -4,8 +4,9 @@ use super::slave::Slave; use super::error::{ErrorKind, Result}; use super::super::rosxmlrpc::Response; use super::naming::{self, Resolver}; +use super::raii::{Publisher, Subscriber, Service}; use super::resolve; -use tcpros::{Client, Message, PublisherStream, ServicePair, ServiceResult}; +use tcpros::{Client, Message, ServicePair, ServiceResult}; use xml_rpc; use std::time::Duration; @@ -125,78 +126,36 @@ impl Ros { } } - pub fn service(&mut self, service: &str, handler: F) -> Result<()> + pub fn service(&mut self, service: &str, handler: F) -> Result where T: ServicePair, F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static, { let name = self.resolver.translate(service)?; - let api = self.slave.add_service::( + Service::new::( + &self.master, + &mut self.slave, &self.hostname, &name, handler, - )?; - - if let Err(err) = self.master.register_service(&name, &api) { - self.slave.remove_service(&name); - self.master.unregister_service(&name, &api)?; - Err(err.into()) - } else { - Ok(()) - } - + ) } - pub fn subscribe(&mut self, topic: &str, callback: F) -> Result<()> + pub fn subscribe(&mut self, topic: &str, callback: F) -> Result where T: Message, F: Fn(T) -> () + Send + 'static, { let name = self.resolver.translate(topic)?; - self.slave.add_subscription::(&name, callback)?; - - match self.master.register_subscriber(&name, &T::msg_type()) { - Ok(publishers) => { - if let Err(err) = self.slave.add_publishers_to_subscription( - &name, - publishers.into_iter(), - ) - { - error!( - "Failed to subscribe to all publishers of topic '{}': {}", - name, - err - ); - } - Ok(()) - } - Err(err) => { - self.slave.remove_subscription(&name); - self.master.unregister_subscriber(&name)?; - Err(err.into()) - } - } + Subscriber::new::(&self.master, &mut self.slave, &name, callback) } - pub fn publish(&mut self, topic: &str) -> Result> + pub fn publish(&mut self, topic: &str) -> Result> where T: Message, { let name = self.resolver.translate(topic)?; - let stream = self.slave.add_publication::(&self.hostname, &name)?; - match self.master.register_publisher(&name, &T::msg_type()) { - Ok(_) => Ok(stream), - Err(error) => { - error!( - "Failed to register publisher for topic '{}': {}", - name, - error - ); - self.slave.remove_publication(&name); - self.master.unregister_publisher(&name)?; - Err(error.into()) - } - } + Publisher::new(&self.master, &mut self.slave, &self.hostname, &name) } } diff --git a/rosrust/src/api/slave.rs b/rosrust/src/api/slave.rs index 2f77e5a..cb9edb4 100644 --- a/rosrust/src/api/slave.rs +++ b/rosrust/src/api/slave.rs @@ -112,7 +112,7 @@ impl Slave { } } - pub fn remove_service(&mut self, service: &str) { + pub fn remove_service(&self, service: &str) { self.services.lock().expect(FAILED_TO_LOCK).remove(service); } @@ -136,7 +136,7 @@ impl Slave { } } - pub fn remove_publication(&mut self, topic: &str) { + pub fn remove_publication(&self, topic: &str) { self.publications.lock().expect(FAILED_TO_LOCK).remove( topic, ); @@ -163,7 +163,7 @@ impl Slave { } } - pub fn remove_subscription(&mut self, topic: &str) { + pub fn remove_subscription(&self, topic: &str) { self.subscriptions.lock().expect(FAILED_TO_LOCK).remove( topic, ); diff --git a/rosrust/src/tcpros/publisher.rs b/rosrust/src/tcpros/publisher.rs index 0dd8710..3473ed6 100644 --- a/rosrust/src/tcpros/publisher.rs +++ b/rosrust/src/tcpros/publisher.rs @@ -127,6 +127,7 @@ impl Publisher { // publisher streams are gone. This should be done with a RAII Arc, residing next todo // the datastream. So maybe replace DataStream with a wrapper that holds that Arc too +#[derive(Clone)] pub struct PublisherStream { stream: DataStream, datatype: std::marker::PhantomData, From 400260bfe36f41320ae67e1e4d6782fed38bf23f Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 20 Dec 2017 12:00:17 +0100 Subject: [PATCH 25/50] Fix missing message files --- rosrust/src/build_tools/mod.rs | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/rosrust/src/build_tools/mod.rs b/rosrust/src/build_tools/mod.rs index b74f1b2..c25d983 100644 --- a/rosrust/src/build_tools/mod.rs +++ b/rosrust/src/build_tools/mod.rs @@ -32,6 +32,11 @@ pub fn depend_on_messages(messages: &[&str]) { .split(":") .filter_map(append_share_folder) .collect::>(); + let cmake_alt_paths = env::var("CMAKE_PREFIX_PATH") + .unwrap_or(String::new()) + .split(":") + .filter_map(append_src_folder) + .collect::>(); let extra_paths = env::var("ROSRUST_MSG_PATH") .unwrap_or(String::new()) .split(":") @@ -39,6 +44,7 @@ pub fn depend_on_messages(messages: &[&str]) { .collect::>(); let paths = cmake_paths .iter() + .chain(cmake_alt_paths.iter()) .chain(extra_paths.iter()) .map(|v| v.as_str()) .collect::>(); @@ -52,3 +58,9 @@ pub fn depend_on_messages(messages: &[&str]) { fn append_share_folder(path: &str) -> Option { Path::new(path).join("share").to_str().map(|v| v.to_owned()) } + +fn append_src_folder(path: &str) -> Option { + Path::new(path).join("..").join("src").to_str().map(|v| { + v.to_owned() + }) +} From 61072ec45bea467aae33c2b377895977b2be8a7c Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 20 Dec 2017 12:01:17 +0100 Subject: [PATCH 26/50] Fix CI --- .travis.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.travis.yml b/.travis.yml index 8c91a74..e573638 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,4 +1,6 @@ language: rust +env: + - ROSRUST_MSG_PATH=/home/travis/build/adnanademovic/rosrust/rosrust/src/build_tools/msg_examples rust: - stable - beta From 31aa7630c7335269d35916a749e55dfc3038fa59 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 20 Dec 2017 12:08:46 +0100 Subject: [PATCH 27/50] Fix service example --- .../build_tools/msg_examples/roscpp_tutorials/srv/TwoInts.srv | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 rosrust/src/build_tools/msg_examples/roscpp_tutorials/srv/TwoInts.srv diff --git a/rosrust/src/build_tools/msg_examples/roscpp_tutorials/srv/TwoInts.srv b/rosrust/src/build_tools/msg_examples/roscpp_tutorials/srv/TwoInts.srv new file mode 100644 index 0000000..3a68808 --- /dev/null +++ b/rosrust/src/build_tools/msg_examples/roscpp_tutorials/srv/TwoInts.srv @@ -0,0 +1,4 @@ +int64 a +int64 b +--- +int64 sum From e2e1ccf3273c15c52cf5ed3f1ce68b561d676993 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 20 Dec 2017 12:16:14 +0100 Subject: [PATCH 28/50] Fix pubsub example --- rosrust/src/build_tools/msg_examples/std_msgs/msg/String.msg | 1 + 1 file changed, 1 insertion(+) create mode 100644 rosrust/src/build_tools/msg_examples/std_msgs/msg/String.msg diff --git a/rosrust/src/build_tools/msg_examples/std_msgs/msg/String.msg b/rosrust/src/build_tools/msg_examples/std_msgs/msg/String.msg new file mode 100644 index 0000000..ae72173 --- /dev/null +++ b/rosrust/src/build_tools/msg_examples/std_msgs/msg/String.msg @@ -0,0 +1 @@ +string data From d8d6eba6da12a73b6a4b8901c58aada39d5916c1 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sat, 23 Dec 2017 12:07:43 +0100 Subject: [PATCH 29/50] Use proper values for private topic renaming --- rosrust/src/api/resolve.rs | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/rosrust/src/api/resolve.rs b/rosrust/src/api/resolve.rs index cb78ea6..79300e3 100644 --- a/rosrust/src/api/resolve.rs +++ b/rosrust/src/api/resolve.rs @@ -37,10 +37,10 @@ pub fn name(default: &str) -> String { pub fn mappings() -> Vec<(String, String)> { args() .skip(1) - .filter(|v| !v.starts_with("__")) + .filter(|v| !v.starts_with("_")) .map(|v| v.split(":=").map(String::from).collect::>()) .filter(|v| v.len() == 2) - .map(|v| v.into_iter().map(fix_prefix)) + .map(|v| v.into_iter()) .map(|mut v| { ( v.next().expect(UNEXPECTED_EMPTY_ARRAY), @@ -50,14 +50,6 @@ pub fn mappings() -> Vec<(String, String)> { .collect() } -fn fix_prefix(v: String) -> String { - if v.starts_with("_") { - format!("~{}", v.trim_left_matches("_")) - } else { - v - } -} - fn find_with_prefix(prefix: &str) -> Option { args().skip(1).find(|v| v.starts_with(prefix)).map(|v| { String::from(v.trim_left_matches(prefix)) @@ -142,9 +134,10 @@ mod tests { "/c:=d", "e:=/f_g", "__name:=something", + "_param:=something", "a:=b:=c", - "_oo_e:=/ab_c", - "/x_y:=_i", + "~oo_e:=/ab_c", + "/x_y:=~i", ]); assert_eq!( vec![ From 74e7f94c7e42a5d120af378d3f9160f8c97f710b Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sat, 23 Dec 2017 13:31:55 +0100 Subject: [PATCH 30/50] Parse parameter assignments from command line --- examples/parameters/README.md | 13 ++++++ examples/parameters/src/parameters.rs | 10 +++++ rosrust/Cargo.toml | 1 + rosrust/src/api/master.rs | 13 ++++++ rosrust/src/api/resolve.rs | 60 ++++++++++++++++++++++++++- rosrust/src/api/ros.rs | 46 +++++++++++++++++++- rosrust/src/lib.rs | 1 + 7 files changed, 142 insertions(+), 2 deletions(-) diff --git a/examples/parameters/README.md b/examples/parameters/README.md index 488b859..6d7f0e2 100644 --- a/examples/parameters/README.md +++ b/examples/parameters/README.md @@ -3,3 +3,16 @@ Creates a node, creates parameters, prints them, and prints the whole parameter tree. Comparing the results with `rosparam` is recommended. + +You can pass a value for ~privbaz. Running +`cargo run _privbaz:="[1,2,3.3,'foo',"bar",[7,8,9],{x:5,y:2}]"` +will yield within the output: + +``` +(..) +Handling ~privbaz: +Get raw: Array([Int(1), Int(2), Double(3.3), String("foo"), String("bar"), Array([Int(7), Int(8), Int(9)]), Struct({"x": Int(5), "y": Int(2)})]) +(..) +``` + +This is even more tolerant than `rospy`, which would require quotation marks hash keys. diff --git a/examples/parameters/src/parameters.rs b/examples/parameters/src/parameters.rs index 0ca2a81..c909674 100644 --- a/examples/parameters/src/parameters.rs +++ b/examples/parameters/src/parameters.rs @@ -42,6 +42,16 @@ fn main() { println!("Create own parameter /baz with 'rosparam' to observe interaction."); } + // Access existing parameter + let param = ros.param("~privbaz").unwrap(); + println!("Handling ~privbaz:"); + if param.exists().unwrap() { + println!("Get raw: {:?}", param.get_raw().unwrap()); + println!("Search: {:?}", param.search().unwrap()); + } else { + println!("Create ~privbaz by passing _privbaz:=value to observe interaction."); + } + #[derive(Debug, Deserialize, Serialize)] struct TestStruct { foo: String, diff --git a/rosrust/Cargo.toml b/rosrust/Cargo.toml index 6c60208..21ca717 100644 --- a/rosrust/Cargo.toml +++ b/rosrust/Cargo.toml @@ -21,3 +21,4 @@ serde = "1.0.24" serde_derive = "1.0.24" serde_rosmsg = "0.2.0" xml-rpc = "0.0.7" +yaml-rust = "0.4.0" diff --git a/rosrust/src/api/master.rs b/rosrust/src/api/master.rs index 3c61006..929192b 100644 --- a/rosrust/src/api/master.rs +++ b/rosrust/src/api/master.rs @@ -96,6 +96,19 @@ impl Master { request!(self; setParam; key, value) } + pub fn set_param_any(&self, key: &str, value: xml_rpc::Value) -> Result<()> { + self.client + .request_tree_with_tree( + "setParam", + vec![ + xml_rpc::Value::String(self.client_id.clone()), + xml_rpc::Value::String(key.into()), + value, + ], + ) + .and(Ok(())) + } + pub fn get_param<'a, T: Deserialize<'a>>(&self, key: &str) -> Result { request!(self; getParam; key) } diff --git a/rosrust/src/api/resolve.rs b/rosrust/src/api/resolve.rs index 79300e3..d8938b6 100644 --- a/rosrust/src/api/resolve.rs +++ b/rosrust/src/api/resolve.rs @@ -50,6 +50,29 @@ pub fn mappings() -> Vec<(String, String)> { .collect() } +pub fn params() -> Vec<(String, String)> { + args() + .skip(1) + .filter(|v| v.starts_with("_")) + .filter(|v| !v.starts_with("__")) + .map(|v| { + v.splitn(2, ":=").map(String::from).collect::>() + }) + .filter(|v| v.len() == 2) + .map(|v| v.into_iter()) + .map(|mut v| { + ( + v.next().expect(UNEXPECTED_EMPTY_ARRAY).replacen( + '_', + "~", + 1, + ), + v.next().expect(UNEXPECTED_EMPTY_ARRAY), + ) + }) + .collect() +} + fn find_with_prefix(prefix: &str) -> Option { args().skip(1).find(|v| v.starts_with(prefix)).map(|v| { String::from(v.trim_left_matches(prefix)) @@ -124,7 +147,7 @@ mod tests { #[test] #[allow(unused_variables)] - fn maps_good_and_ignores_everything_else() { + fn mappings_maps_good_and_ignores_everything_else() { let testcase = TESTCASE.lock().expect(FAILED_TO_LOCK); set_args(&vec![]); assert_eq!(Vec::<(String, String)>::new(), mappings()); @@ -151,6 +174,41 @@ mod tests { ); } + #[test] + #[allow(unused_variables)] + fn params_default_to_empty_vector() { + let testcase = TESTCASE.lock().expect(FAILED_TO_LOCK); + set_args(&vec![]); + assert_eq!(Vec::<(String, String)>::new(), params()); + } + + #[test] + #[allow(unused_variables)] + fn params_maps_good_and_ignores_everything_else() { + let testcase = TESTCASE.lock().expect(FAILED_TO_LOCK); + set_args(&vec![]); + assert_eq!(Vec::<(String, String)>::new(), params()); + set_args(&vec![ + "a:=x", + "b=e", + "/c:=d", + "e:=/f_g", + "__name:=something", + "_param:=something", + "a:=b:=c", + "~oo_e:=/ab_c", + "_foo:=123:=456", + "/x_y:=~i", + ]); + assert_eq!( + vec![ + (String::from("~param"), String::from("something")), + (String::from("~foo"), String::from("123:=456")), + ], + params() + ); + } + #[test] #[allow(unused_variables)] fn name_uses_passed_value_by_default() { diff --git a/rosrust/src/api/ros.rs b/rosrust/src/api/ros.rs index beaa1b8..5cf5d68 100644 --- a/rosrust/src/api/ros.rs +++ b/rosrust/src/api/ros.rs @@ -1,7 +1,7 @@ use serde::{Serialize, Deserialize}; use super::master::{self, Master, Topic}; use super::slave::Slave; -use super::error::{ErrorKind, Result}; +use super::error::{ErrorKind, Result, ResultExt}; use super::super::rosxmlrpc::Response; use super::naming::{self, Resolver}; use super::raii::{Publisher, Subscriber, Service}; @@ -9,6 +9,7 @@ use super::resolve; use tcpros::{Client, Message, ServicePair, ServiceResult}; use xml_rpc; use std::time::Duration; +use yaml_rust::{Yaml, YamlLoader}; pub struct Ros { master: Master, @@ -28,6 +29,17 @@ impl Ros { for (src, dest) in resolve::mappings() { ros.map(&src, &dest)?; } + for (src, dest) in resolve::params() { + let data = YamlLoader::load_from_str(&dest) + .chain_err(|| format!("Failed to load YAML: {}", dest))? + .into_iter() + .next() + .ok_or_else(|| format!("Failed to load YAML: {}", dest))?; + let param = ros.param(&src).ok_or_else( + || format!("Failed to resolve name: {}", src), + )?; + param.set_raw(yaml_to_xmlrpc(data)?)?; + } Ok(ros) } @@ -181,6 +193,10 @@ impl<'a> Parameter<'a> { self.master.set_param::(&self.name, value).and(Ok(())) } + pub fn set_raw(&self, value: xml_rpc::Value) -> Response<()> { + self.master.set_param_any(&self.name, value).and(Ok(())) + } + pub fn delete(&self) -> Response<()> { self.master.delete_param(&self.name).and(Ok(())) } @@ -193,3 +209,31 @@ impl<'a> Parameter<'a> { self.master.search_param(&self.name) } } + +fn yaml_to_xmlrpc(val: Yaml) -> Result { + Ok(match val { + Yaml::Real(v) => xml_rpc::Value::Double(v.parse().chain_err(|| "Failed to parse float")?), + Yaml::Integer(v) => xml_rpc::Value::Int(v as i32), + Yaml::String(v) => xml_rpc::Value::String(v), + Yaml::Boolean(v) => xml_rpc::Value::Bool(v), + Yaml::Array(v) => xml_rpc::Value::Array( + v.into_iter().map(yaml_to_xmlrpc).collect::>()?, + ), + Yaml::Hash(v) => xml_rpc::Value::Struct(v.into_iter() + .map(|(k, v)| Ok((yaml_to_string(k)?, yaml_to_xmlrpc(v)?))) + .collect::>()?), + Yaml::Alias(_) => bail!("Alias is not supported"), + Yaml::Null => bail!("Illegal null value"), + Yaml::BadValue => bail!("Bad value provided"), + }) +} + +fn yaml_to_string(val: Yaml) -> Result { + Ok(match val { + Yaml::Real(v) | Yaml::String(v) => v, + Yaml::Integer(v) => v.to_string(), + Yaml::Boolean(true) => "true".into(), + Yaml::Boolean(false) => "false".into(), + _ => bail!("Hash keys need to be strings"), + }) +} diff --git a/rosrust/src/lib.rs b/rosrust/src/lib.rs index 057d8d5..62e4c3b 100644 --- a/rosrust/src/lib.rs +++ b/rosrust/src/lib.rs @@ -17,6 +17,7 @@ extern crate serde; extern crate serde_derive; extern crate serde_rosmsg; extern crate xml_rpc; +extern crate yaml_rust; pub use api::Ros; pub use tcpros::{Client, PublisherStream, Message, ServicePair as Service}; From 9bf6ed420f23afdbc75fdb1f094039bd7fcf660a Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sat, 23 Dec 2017 13:33:50 +0100 Subject: [PATCH 31/50] Fix typos --- examples/parameters/README.md | 2 +- examples/parameters/src/parameters.rs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/parameters/README.md b/examples/parameters/README.md index 6d7f0e2..228f409 100644 --- a/examples/parameters/README.md +++ b/examples/parameters/README.md @@ -15,4 +15,4 @@ Get raw: Array([Int(1), Int(2), Double(3.3), String("foo"), String("bar"), Array (..) ``` -This is even more tolerant than `rospy`, which would require quotation marks hash keys. +This is even more tolerant than `rospy`, which would require quotation marks around hash keys. diff --git a/examples/parameters/src/parameters.rs b/examples/parameters/src/parameters.rs index c909674..c87af87 100644 --- a/examples/parameters/src/parameters.rs +++ b/examples/parameters/src/parameters.rs @@ -42,7 +42,7 @@ fn main() { println!("Create own parameter /baz with 'rosparam' to observe interaction."); } - // Access existing parameter + // Access command line parameter let param = ros.param("~privbaz").unwrap(); println!("Handling ~privbaz:"); if param.exists().unwrap() { From 388188bfbbe40448822329480371877036609a24 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sat, 23 Dec 2017 19:52:05 +0100 Subject: [PATCH 32/50] Support clock simulation and blocking --- examples/pubsub/build.rs | 2 +- examples/pubsub/src/publisher.rs | 6 +- examples/pubsub/src/subscriber.rs | 5 +- examples/serviceclient/src/service.rs | 5 +- rosrust/src/api/clock.rs | 180 ++++++++++++++++++++++++++ rosrust/src/api/mod.rs | 2 + rosrust/src/api/raii.rs | 58 ++++----- rosrust/src/api/ros.rs | 80 ++++++++++-- rosrust/src/api/slave.rs | 23 ++-- rosrust/src/lib.rs | 6 - rosrust/src/msg.rs | 59 ++++++++- 11 files changed, 351 insertions(+), 75 deletions(-) create mode 100644 rosrust/src/api/clock.rs diff --git a/examples/pubsub/build.rs b/examples/pubsub/build.rs index 72dc60e..e570e2b 100644 --- a/examples/pubsub/build.rs +++ b/examples/pubsub/build.rs @@ -1,4 +1,4 @@ #[macro_use] extern crate rosrust; -rosmsg_main!("std_msgs/String"); +rosmsg_main!("std_msgs/String", "rosgraph_msgs/Clock"); diff --git a/examples/pubsub/src/publisher.rs b/examples/pubsub/src/publisher.rs index a3ebf76..83ff39a 100644 --- a/examples/pubsub/src/publisher.rs +++ b/examples/pubsub/src/publisher.rs @@ -3,7 +3,6 @@ extern crate env_logger; extern crate rosrust; use rosrust::Ros; -use std::{thread, time}; rosmsg_include!(); @@ -15,13 +14,14 @@ fn main() { let mut count = 0; - loop { + let mut rate = ros.rate(10.0); + while ros.is_ok() { let mut msg = msg::std_msgs::String::new(); msg.data = format!("hello world {}", count); chatter_pub.send(msg).unwrap(); - thread::sleep(time::Duration::from_millis(100)); + rate.sleep(); count += 1; } diff --git a/examples/pubsub/src/subscriber.rs b/examples/pubsub/src/subscriber.rs index 7e13535..e14c126 100644 --- a/examples/pubsub/src/subscriber.rs +++ b/examples/pubsub/src/subscriber.rs @@ -3,7 +3,6 @@ extern crate env_logger; extern crate rosrust; use rosrust::Ros; -use std::{thread, time}; rosmsg_include!(); @@ -17,7 +16,5 @@ fn main() { ros.subscribe("chatter", |v: msg::std_msgs::String| println!("{}", v.data)) .unwrap(); - loop { - thread::sleep(time::Duration::from_secs(100)); - } + ros.spin(); } diff --git a/examples/serviceclient/src/service.rs b/examples/serviceclient/src/service.rs index ad3a3af..33ed899 100644 --- a/examples/serviceclient/src/service.rs +++ b/examples/serviceclient/src/service.rs @@ -3,7 +3,6 @@ extern crate env_logger; extern crate rosrust; use rosrust::Ros; -use std::{thread, time}; rosmsg_include!(); @@ -17,7 +16,5 @@ fn main() { Ok(msg::roscpp_tutorials::TwoIntsRes { sum: req.a + req.b }) }).unwrap(); - loop { - thread::sleep(time::Duration::from_secs(100)); - } + ros.spin(); } diff --git a/rosrust/src/api/clock.rs b/rosrust/src/api/clock.rs new file mode 100644 index 0000000..cce29fb --- /dev/null +++ b/rosrust/src/api/clock.rs @@ -0,0 +1,180 @@ +use msg::{Duration, Time}; +use std::cmp; +use std::collections::BinaryHeap; +use std::sync::{Arc, Mutex}; +use std::sync::mpsc::{channel, Sender}; +use std::thread::sleep; +use std::time::{Duration as StdDuration, SystemTime, UNIX_EPOCH}; + +static BEFORE_EPOCH: &'static str = "Requested time is before UNIX epoch."; + +pub struct Rate { + clock: Arc, + next: Time, + delay: Duration, +} + +impl Rate { + pub fn new(clock: Arc, start: Time, delay: Duration) -> Rate { + Rate { + clock, + next: start, + delay, + } + } + + pub fn sleep(&mut self) { + self.next = self.next.clone() + self.delay.clone(); + self.clock.wait_until(self.next.clone()); + } +} + +pub trait Clock { + fn now(&self) -> Time; + fn sleep(&self, d: Duration); + fn wait_until(&self, t: Time); +} + +#[derive(Clone, Default)] +pub struct RealClock {} + +impl Clock for RealClock { + #[inline] + fn now(&self) -> Time { + let time = SystemTime::now().duration_since(UNIX_EPOCH).expect( + BEFORE_EPOCH, + ); + Time { + sec: time.as_secs() as i32, + nsec: time.subsec_nanos() as i32, + } + } + + #[inline] + fn sleep(&self, d: Duration) { + if d < Duration::default() { + return; + } + sleep(StdDuration::new(d.sec as u64, d.nsec as u32)); + } + + #[inline] + fn wait_until(&self, t: Time) { + self.sleep(t - self.now()); + } +} + +struct Timeout { + timestamp: Time, + tx: Sender<()>, +} + +impl cmp::PartialEq for Timeout { + fn eq(&self, other: &Self) -> bool { + self.timestamp == other.timestamp + } +} + +impl cmp::PartialOrd for Timeout { + fn partial_cmp(&self, other: &Self) -> Option { + self.timestamp.partial_cmp(&other.timestamp).map( + cmp::Ordering::reverse, + ) + } +} + +impl cmp::Eq for Timeout {} + +impl cmp::Ord for Timeout { + fn cmp(&self, other: &Self) -> cmp::Ordering { + self.timestamp.cmp(&other.timestamp).reverse() + } +} + +#[derive(Default)] +pub struct SimData { + current: Time, + timeouts: BinaryHeap, +} + +#[derive(Default)] +pub struct SimulatedClock { + pub data: Mutex, +} + +impl SimulatedClock { + pub fn trigger(&self, time: Time) { + let mut data = self.data.lock().expect(FAILED_TO_LOCK); + data.current = time; + loop { + if let Some(next) = data.timeouts.peek() { + if next.timestamp > data.current { + return; + } + } + if let Some(next) = data.timeouts.pop() { + next.tx.send(()).expect(SLEEPING_THREAD_MISSING); + } + } + } +} + +impl Clock for SimulatedClock { + #[inline] + fn now(&self) -> Time { + self.data.lock().expect(FAILED_TO_LOCK).current.clone() + } + + #[inline] + fn sleep(&self, d: Duration) { + if d.sec < 0 || d.nsec < 0 { + return; + } + let current = { + self.data.lock().expect(FAILED_TO_LOCK).current.clone() + }; + self.wait_until(current + d); + } + + #[inline] + fn wait_until(&self, timestamp: Time) { + let (tx, rx) = channel(); + { + self.data.lock().expect(FAILED_TO_LOCK).timeouts.push( + Timeout { + timestamp, + tx, + }, + ); + } + if rx.recv().is_err() { + warn!("Sleep beyond simulated clock"); + } + } +} + +static FAILED_TO_LOCK: &'static str = "Failed to acquire lock"; +static SLEEPING_THREAD_MISSING: &'static str = "Failed to find sleeping thread"; + +pub mod rosgraph_msgs { + use Message; + use msg::Time; + + #[derive(Serialize, Deserialize, Debug)] + pub struct Clock { + pub clock: Time, + } + impl Message for Clock { + fn msg_definition() -> ::std::string::String { + "# roslib/Clock is used for publishing simulated time in ROS. \n# This message simply communicates the current time.\n# For more information, see http://www.ros.org/wiki/Clock\ntime clock\n".into() + } + + fn md5sum() -> ::std::string::String { + "a9c97c1d230cfc112e270351a944ee47".into() + } + + fn msg_type() -> ::std::string::String { + "rosgraph_msgs/Clock".into() + } + } +} diff --git a/rosrust/src/api/mod.rs b/rosrust/src/api/mod.rs index b6edb53..6bca0d1 100644 --- a/rosrust/src/api/mod.rs +++ b/rosrust/src/api/mod.rs @@ -1,5 +1,7 @@ pub use self::ros::Ros; +pub use self::clock::Rate; +mod clock; pub mod error; mod master; mod slave; diff --git a/rosrust/src/api/raii.rs b/rosrust/src/api/raii.rs index 51abb46..8c900b0 100644 --- a/rosrust/src/api/raii.rs +++ b/rosrust/src/api/raii.rs @@ -6,15 +6,15 @@ use std::sync::Arc; use tcpros::{Message, PublisherStream, ServicePair, ServiceResult}; #[derive(Clone)] -pub struct Publisher<'a, T: Message> { +pub struct Publisher { stream: PublisherStream, - _raii: Arc>>, + _raii: Arc>, } -impl<'a, T: Message> Publisher<'a, T> { +impl Publisher { pub(crate) fn new( - master: &'a Master, - slave: &'a mut Slave, + master: Arc, + slave: Arc, hostname: &str, name: &str, ) -> Result { @@ -25,7 +25,7 @@ impl<'a, T: Message> Publisher<'a, T> { name: name.into(), }; - match master.register_publisher(&name, &T::msg_type()) { + match info.master.register_publisher(&name, &T::msg_type()) { Ok(_) => Ok(Self { stream, _raii: Arc::new(InteractorRaii::new(info)), @@ -48,13 +48,13 @@ impl<'a, T: Message> Publisher<'a, T> { } } -struct PublisherInfo<'a> { - master: &'a Master, - slave: &'a Slave, +struct PublisherInfo { + master: Arc, + slave: Arc, name: String, } -impl<'a> Interactor for PublisherInfo<'a> { +impl Interactor for PublisherInfo { fn unregister(&mut self) -> Response<()> { self.slave.remove_publication(&self.name); self.master.unregister_publisher(&self.name).map(|_| ()) @@ -62,14 +62,14 @@ impl<'a> Interactor for PublisherInfo<'a> { } #[derive(Clone)] -pub struct Subscriber<'a> { - _raii: Arc>>, +pub struct Subscriber { + _raii: Arc>, } -impl<'a> Subscriber<'a> { +impl Subscriber { pub(crate) fn new () + Send + 'static>( - master: &'a Master, - slave: &'a mut Slave, + master: Arc, + slave: Arc, name: &str, callback: F, ) -> Result { @@ -108,13 +108,13 @@ impl<'a> Subscriber<'a> { } } -struct SubscriberInfo<'a> { - master: &'a Master, - slave: &'a Slave, +struct SubscriberInfo { + master: Arc, + slave: Arc, name: String, } -impl<'a> Interactor for SubscriberInfo<'a> { +impl Interactor for SubscriberInfo { fn unregister(&mut self) -> Response<()> { self.slave.remove_subscription(&self.name); self.master.unregister_subscriber(&self.name).map(|_| ()) @@ -122,14 +122,14 @@ impl<'a> Interactor for SubscriberInfo<'a> { } #[derive(Clone)] -pub struct Service<'a> { - _raii: Arc>>, +pub struct Service { + _raii: Arc>, } -impl<'a> Service<'a> { +impl Service { pub(crate) fn new( - master: &'a Master, - slave: &'a mut Slave, + master: Arc, + slave: Arc, hostname: &str, name: &str, handler: F, @@ -147,7 +147,7 @@ impl<'a> Service<'a> { name: name.into(), }; - if let Err(err) = master.register_service(name, &info.api) { + if let Err(err) = info.master.register_service(name, &info.api) { info.unregister()?; Err(err.into()) } else { @@ -156,14 +156,14 @@ impl<'a> Service<'a> { } } -struct ServiceInfo<'a> { - master: &'a Master, - slave: &'a Slave, +struct ServiceInfo { + master: Arc, + slave: Arc, name: String, api: String, } -impl<'a> Interactor for ServiceInfo<'a> { +impl Interactor for ServiceInfo { fn unregister(&mut self) -> Response<()> { self.slave.remove_service(&self.name); self.master.unregister_service(&self.name, &self.api).map( diff --git a/rosrust/src/api/ros.rs b/rosrust/src/api/ros.rs index 5cf5d68..f2f1676 100644 --- a/rosrust/src/api/ros.rs +++ b/rosrust/src/api/ros.rs @@ -1,4 +1,9 @@ +use msg::{Duration, Time}; use serde::{Serialize, Deserialize}; +use std::sync::{Arc, mpsc}; +use std::time; +use super::clock::{Clock, Rate, RealClock, SimulatedClock}; +use super::clock::rosgraph_msgs::Clock as ClockMsg; use super::master::{self, Master, Topic}; use super::slave::Slave; use super::error::{ErrorKind, Result, ResultExt}; @@ -8,15 +13,17 @@ use super::raii::{Publisher, Subscriber, Service}; use super::resolve; use tcpros::{Client, Message, ServicePair, ServiceResult}; use xml_rpc; -use std::time::Duration; use yaml_rust::{Yaml, YamlLoader}; pub struct Ros { - master: Master, - slave: Slave, + master: Arc, + slave: Arc, hostname: String, resolver: Resolver, name: String, + clock: Arc, + static_subs: Vec, + shutdown_rx: mpsc::Receiver<()>, } impl Ros { @@ -40,6 +47,19 @@ impl Ros { )?; param.set_raw(yaml_to_xmlrpc(data)?)?; } + + if ros.param("/use_sim_time") + .and_then(|v| v.get().ok()) + .unwrap_or(false) + { + let clock = Arc::new(SimulatedClock::default()); + let ros_clock = clock.clone(); + let sub = ros.subscribe::("/clock", move |v| clock.trigger(v.clock)) + .chain_err(|| "Failed to subscribe to simulated clock")?; + ros.static_subs.push(sub); + ros.clock = ros_clock; + } + Ok(ros) } @@ -55,14 +75,20 @@ impl Ros { let name = format!("{}/{}", namespace, name); let resolver = Resolver::new(&name)?; - let slave = Slave::new(&master_uri, &hostname, 0, &name)?; + let (shutdown_tx, shutdown_rx) = mpsc::channel(); + + let slave = Slave::new(&master_uri, &hostname, 0, &name, shutdown_tx)?; let master = Master::new(&master_uri, &name, &slave.uri()); + Ok(Ros { - master: master, - slave: slave, + master: Arc::new(master), + slave: Arc::new(slave), hostname: String::from(hostname), resolver: resolver, name: name, + clock: Arc::new(RealClock::default()), + static_subs: Vec::new(), + shutdown_rx, }) } @@ -82,6 +108,31 @@ impl Ros { return &self.hostname; } + pub fn now(&self) -> Time { + self.clock.now() + } + + pub fn sleep(&self, d: Duration) { + self.clock.sleep(d); + } + + pub fn rate(&self, rate: f64) -> Rate { + let nanos = 1_000_000_000.0 / rate; + Rate::new( + self.clock.clone(), + self.now(), + Duration::from_nanos(nanos as i64), + ) + } + + pub fn is_ok(&self) -> bool { + self.shutdown_rx.try_recv() == Err(mpsc::TryRecvError::Empty) + } + + pub fn spin(&self) { + self.shutdown_rx.recv().is_ok(); + } + pub fn param<'a, 'b>(&'a self, name: &'b str) -> Option> { self.resolver.translate(name).ok().map(|v| { Parameter { @@ -111,7 +162,7 @@ impl Ros { Ok(Client::new(&self.name, &uri, &name)) } - pub fn wait_for_service(&self, service: &str, timeout: Option) -> Result<()> { + pub fn wait_for_service(&self, service: &str, timeout: Option) -> Result<()> { use rosxmlrpc::ResponseError; use std::thread::sleep; @@ -129,7 +180,7 @@ impl Ros { return Err(ErrorKind::TimeoutError.into()); } } - sleep(Duration::from_millis(100)); + sleep(time::Duration::from_millis(100)); continue; } _ => {} @@ -145,8 +196,8 @@ impl Ros { { let name = self.resolver.translate(service)?; Service::new::( - &self.master, - &mut self.slave, + self.master.clone(), + self.slave.clone(), &self.hostname, &name, handler, @@ -159,7 +210,7 @@ impl Ros { F: Fn(T) -> () + Send + 'static, { let name = self.resolver.translate(topic)?; - Subscriber::new::(&self.master, &mut self.slave, &name, callback) + Subscriber::new::(self.master.clone(), self.slave.clone(), &name, callback) } pub fn publish(&mut self, topic: &str) -> Result> @@ -167,7 +218,12 @@ impl Ros { T: Message, { let name = self.resolver.translate(topic)?; - Publisher::new(&self.master, &mut self.slave, &self.hostname, &name) + Publisher::new( + self.master.clone(), + self.slave.clone(), + &self.hostname, + &name, + ) } } diff --git a/rosrust/src/api/slave.rs b/rosrust/src/api/slave.rs index cb9edb4..c1185fb 100644 --- a/rosrust/src/api/slave.rs +++ b/rosrust/src/api/slave.rs @@ -1,7 +1,7 @@ use futures::sync::mpsc::channel as futures_channel; use std::collections::HashMap; use std::sync::{Arc, Mutex}; -use std::sync::mpsc::channel; +use std::sync::mpsc::{channel, Sender}; use std::thread; use super::error::{self, ErrorKind, Result, ResultExt}; use super::slavehandler::{add_publishers_to_subscription, SlaveHandler}; @@ -18,7 +18,13 @@ pub struct Slave { type SerdeResult = Result; impl Slave { - pub fn new(master_uri: &str, hostname: &str, port: u16, name: &str) -> Result { + pub fn new( + master_uri: &str, + hostname: &str, + port: u16, + name: &str, + outer_shutdown_tx: Sender<()>, + ) -> Result { use std::net::ToSocketAddrs; use futures::{Future, Stream}; @@ -47,6 +53,7 @@ impl Slave { if let Err(err) = bound_handler.run_until(shutdown_future) { info!("Error during ROS Slave API initiation: {}", err); } + outer_shutdown_tx.send(()).is_ok(); }); let port = port_rx.recv().expect(FAILED_TO_LOCK).chain_err( @@ -67,11 +74,7 @@ impl Slave { return &self.uri; } - pub fn add_publishers_to_subscription( - &mut self, - topic: &str, - publishers: T, - ) -> SerdeResult<()> + pub fn add_publishers_to_subscription(&self, topic: &str, publishers: T) -> SerdeResult<()> where T: Iterator, { @@ -84,7 +87,7 @@ impl Slave { } pub fn add_service( - &mut self, + &self, hostname: &str, service: &str, handler: F, @@ -117,7 +120,7 @@ impl Slave { } pub fn add_publication( - &mut self, + &self, hostname: &str, topic: &str, ) -> error::tcpros::Result> @@ -142,7 +145,7 @@ impl Slave { ); } - pub fn add_subscription(&mut self, topic: &str, callback: F) -> Result<()> + pub fn add_subscription(&self, topic: &str, callback: F) -> Result<()> where T: Message, F: Fn(T) -> () + Send + 'static, diff --git a/rosrust/src/lib.rs b/rosrust/src/lib.rs index 62e4c3b..386f72b 100644 --- a/rosrust/src/lib.rs +++ b/rosrust/src/lib.rs @@ -29,9 +29,3 @@ pub mod build_tools; mod rosxmlrpc; mod tcpros; pub mod msg; - -#[cfg(test)] -mod tests { - #[test] - fn it_works() {} -} diff --git a/rosrust/src/msg.rs b/rosrust/src/msg.rs index 35aa6ed..cf213a3 100644 --- a/rosrust/src/msg.rs +++ b/rosrust/src/msg.rs @@ -1,18 +1,21 @@ +use std::cmp; use std::ops; const BILLION: i64 = 1000000000; -#[derive(Serialize, Deserialize, Debug)] +#[derive(Clone, Default, Serialize, Deserialize, Debug)] pub struct Time { pub sec: i32, pub nsec: i32, } impl Time { + #[inline] pub fn new() -> Time { - Time { sec: 0, nsec: 0 } + Self::default() } + #[inline] pub fn from_nanos(t: i64) -> Time { Time { sec: (t / BILLION) as i32, @@ -20,22 +23,45 @@ impl Time { } } - fn nanos(self) -> i64 { + #[inline] + fn nanos(&self) -> i64 { self.sec as i64 * BILLION + self.nsec as i64 } } -#[derive(Serialize, Deserialize, Debug)] +impl cmp::PartialEq for Time { + fn eq(&self, other: &Self) -> bool { + self.nanos() == other.nanos() + } +} + +impl cmp::PartialOrd for Time { + fn partial_cmp(&self, other: &Self) -> Option { + self.nanos().partial_cmp(&other.nanos()) + } +} + +impl cmp::Eq for Time {} + +impl cmp::Ord for Time { + fn cmp(&self, other: &Self) -> cmp::Ordering { + self.nanos().cmp(&other.nanos()) + } +} + +#[derive(Clone, Default, Serialize, Deserialize, Debug)] pub struct Duration { pub sec: i32, pub nsec: i32, } impl Duration { + #[inline] pub fn new() -> Duration { - Duration { sec: 0, nsec: 0 } + Self::default() } + #[inline] pub fn from_nanos(t: i64) -> Duration { Duration { sec: (t / BILLION) as i32, @@ -43,11 +69,32 @@ impl Duration { } } - fn nanos(self) -> i64 { + #[inline] + fn nanos(&self) -> i64 { self.sec as i64 * BILLION + self.nsec as i64 } } +impl cmp::PartialEq for Duration { + fn eq(&self, other: &Self) -> bool { + self.nanos() == other.nanos() + } +} + +impl cmp::PartialOrd for Duration { + fn partial_cmp(&self, other: &Self) -> Option { + self.nanos().partial_cmp(&other.nanos()) + } +} + +impl cmp::Eq for Duration {} + +impl cmp::Ord for Duration { + fn cmp(&self, other: &Self) -> cmp::Ordering { + self.nanos().cmp(&other.nanos()) + } +} + impl ops::Add for Time { type Output = Time; fn add(self, rhs: Duration) -> Self::Output { From 6ec413819092de0e2a162eea2e6be62c12bdce6d Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sat, 23 Dec 2017 21:02:31 +0100 Subject: [PATCH 33/50] Remove unused message dependency --- examples/pubsub/build.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/pubsub/build.rs b/examples/pubsub/build.rs index e570e2b..72dc60e 100644 --- a/examples/pubsub/build.rs +++ b/examples/pubsub/build.rs @@ -1,4 +1,4 @@ #[macro_use] extern crate rosrust; -rosmsg_main!("std_msgs/String", "rosgraph_msgs/Clock"); +rosmsg_main!("std_msgs/String"); From ce4bf04692de1d875148fb932f7e5346ec029c25 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sun, 24 Dec 2017 00:20:34 +0100 Subject: [PATCH 34/50] Extract build tools --- Cargo.toml | 2 ++ rosrust/src/lib.rs | 4 ++-- rosrust_codegen/Cargo.toml | 14 ++++++++++++++ .../build_tools => rosrust_codegen/src}/error.rs | 0 .../build_tools => rosrust_codegen/src}/genmsg.rs | 4 ++-- .../build_tools => rosrust_codegen/src}/helpers.rs | 4 ++-- .../mod.rs => rosrust_codegen/src/lib.rs | 10 ++++++++++ .../src/build_tools => rosrust_codegen/src}/msg.rs | 2 +- .../msg_examples/benchmark_msgs/msg/Overall.msg | 0 .../diagnostic_msgs/srv/AddDiagnostics.srv | 0 .../empty_req_srv/srv/EmptyRequest.srv | 0 .../src}/msg_examples/geometry_msgs/msg/Point.msg | 0 .../src}/msg_examples/geometry_msgs/msg/Pose.msg | 0 .../msg_examples/geometry_msgs/msg/PoseStamped.msg | 0 .../msg_examples/geometry_msgs/msg/Quaternion.msg | 0 .../geometry_msgs/msg/TwistWithCovariance.msg | 0 .../msg_examples/geometry_msgs/msg/Vector3.msg | 0 .../msg_examples/roscpp_tutorials/srv/TwoInts.srv | 0 .../src}/msg_examples/sensor_msgs/msg/Imu.msg | 0 .../src}/msg_examples/simple_srv/srv/Something.srv | 0 .../src}/msg_examples/std_msgs/msg/Header.msg | 0 .../src}/msg_examples/std_msgs/msg/String.msg | 0 .../tricky_comment_srv/srv/TrickyComment.srv | 0 23 files changed, 33 insertions(+), 7 deletions(-) create mode 100644 rosrust_codegen/Cargo.toml rename {rosrust/src/build_tools => rosrust_codegen/src}/error.rs (100%) rename {rosrust/src/build_tools => rosrust_codegen/src}/genmsg.rs (99%) rename {rosrust/src/build_tools => rosrust_codegen/src}/helpers.rs (99%) rename rosrust/src/build_tools/mod.rs => rosrust_codegen/src/lib.rs (91%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg.rs (99%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg_examples/benchmark_msgs/msg/Overall.msg (100%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg_examples/diagnostic_msgs/srv/AddDiagnostics.srv (100%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg_examples/empty_req_srv/srv/EmptyRequest.srv (100%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg_examples/geometry_msgs/msg/Point.msg (100%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg_examples/geometry_msgs/msg/Pose.msg (100%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg_examples/geometry_msgs/msg/PoseStamped.msg (100%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg_examples/geometry_msgs/msg/Quaternion.msg (100%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg_examples/geometry_msgs/msg/TwistWithCovariance.msg (100%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg_examples/geometry_msgs/msg/Vector3.msg (100%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg_examples/roscpp_tutorials/srv/TwoInts.srv (100%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg_examples/sensor_msgs/msg/Imu.msg (100%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg_examples/simple_srv/srv/Something.srv (100%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg_examples/std_msgs/msg/Header.msg (100%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg_examples/std_msgs/msg/String.msg (100%) rename {rosrust/src/build_tools => rosrust_codegen/src}/msg_examples/tricky_comment_srv/srv/TrickyComment.srv (100%) diff --git a/Cargo.toml b/Cargo.toml index 8d45041..cf77042 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -4,7 +4,9 @@ members = [ "examples/pubsub", "examples/serviceclient", "rosrust", + "rosrust_codegen", ] [patch.crates-io] "rosrust" = { path = "rosrust" } +"rosrust_codegen" = { path = "rosrust_codegen" } diff --git a/rosrust/src/lib.rs b/rosrust/src/lib.rs index 386f72b..fe6c845 100644 --- a/rosrust/src/lib.rs +++ b/rosrust/src/lib.rs @@ -12,6 +12,8 @@ extern crate lazy_static; extern crate log; extern crate nix; extern crate regex; +#[macro_use] +extern crate rosrust_codegen; extern crate serde; #[macro_use] extern crate serde_derive; @@ -24,8 +26,6 @@ pub use tcpros::{Client, PublisherStream, Message, ServicePair as Service}; pub use api::error; mod api; -#[macro_use] -pub mod build_tools; mod rosxmlrpc; mod tcpros; pub mod msg; diff --git a/rosrust_codegen/Cargo.toml b/rosrust_codegen/Cargo.toml new file mode 100644 index 0000000..94f8dd2 --- /dev/null +++ b/rosrust_codegen/Cargo.toml @@ -0,0 +1,14 @@ +[package] +authors = ["Adnan Ademovic "] +description = "Message generation for rosrust" +license = "MIT" +name = "rosrust_codegen" +repository = "https://github.com/adnanademovic/rosrust" +version = "0.0.1" + +[dependencies] +error-chain = "0.11.0" +itertools = "0.7.4" +lazy_static = "1.0.0" +regex = "0.2.3" +rust-crypto = "0.2.36" diff --git a/rosrust/src/build_tools/error.rs b/rosrust_codegen/src/error.rs similarity index 100% rename from rosrust/src/build_tools/error.rs rename to rosrust_codegen/src/error.rs diff --git a/rosrust/src/build_tools/genmsg.rs b/rosrust_codegen/src/genmsg.rs similarity index 99% rename from rosrust/src/build_tools/genmsg.rs rename to rosrust_codegen/src/genmsg.rs index d15e71a..feda3f8 100644 --- a/rosrust/src/build_tools/genmsg.rs +++ b/rosrust_codegen/src/genmsg.rs @@ -1,6 +1,6 @@ use std::collections::HashSet; -use super::helpers; -use super::error::Result; +use helpers; +use error::Result; pub fn depend_on_messages(folders: &[&str], messages: &[&str]) -> Result { let mut output = Vec::::new(); diff --git a/rosrust/src/build_tools/helpers.rs b/rosrust_codegen/src/helpers.rs similarity index 99% rename from rosrust/src/build_tools/helpers.rs rename to rosrust_codegen/src/helpers.rs index ec9da6d..dc89039 100644 --- a/rosrust/src/build_tools/helpers.rs +++ b/rosrust_codegen/src/helpers.rs @@ -1,6 +1,6 @@ use std::collections::{LinkedList, HashMap, HashSet}; -use super::msg::Msg; -use super::error::{Result, ResultExt}; +use msg::Msg; +use error::{Result, ResultExt}; use std::fs::File; use std::path::Path; use regex::RegexBuilder; diff --git a/rosrust/src/build_tools/mod.rs b/rosrust_codegen/src/lib.rs similarity index 91% rename from rosrust/src/build_tools/mod.rs rename to rosrust_codegen/src/lib.rs index c25d983..a5cac7f 100644 --- a/rosrust/src/build_tools/mod.rs +++ b/rosrust_codegen/src/lib.rs @@ -1,3 +1,13 @@ +#![recursion_limit = "1024"] + +extern crate crypto; +#[macro_use] +extern crate error_chain; +extern crate itertools; +#[macro_use] +extern crate lazy_static; +extern crate regex; + pub mod msg; pub mod helpers; pub mod error; diff --git a/rosrust/src/build_tools/msg.rs b/rosrust_codegen/src/msg.rs similarity index 99% rename from rosrust/src/build_tools/msg.rs rename to rosrust_codegen/src/msg.rs index cb721c1..bbbaa45 100644 --- a/rosrust/src/build_tools/msg.rs +++ b/rosrust_codegen/src/msg.rs @@ -1,6 +1,6 @@ use itertools::{self, Itertools}; use regex::Regex; -use super::error::{Result, ResultExt}; +use error::{Result, ResultExt}; use std::collections::HashMap; pub struct Msg { diff --git a/rosrust/src/build_tools/msg_examples/benchmark_msgs/msg/Overall.msg b/rosrust_codegen/src/msg_examples/benchmark_msgs/msg/Overall.msg similarity index 100% rename from rosrust/src/build_tools/msg_examples/benchmark_msgs/msg/Overall.msg rename to rosrust_codegen/src/msg_examples/benchmark_msgs/msg/Overall.msg diff --git a/rosrust/src/build_tools/msg_examples/diagnostic_msgs/srv/AddDiagnostics.srv b/rosrust_codegen/src/msg_examples/diagnostic_msgs/srv/AddDiagnostics.srv similarity index 100% rename from rosrust/src/build_tools/msg_examples/diagnostic_msgs/srv/AddDiagnostics.srv rename to rosrust_codegen/src/msg_examples/diagnostic_msgs/srv/AddDiagnostics.srv diff --git a/rosrust/src/build_tools/msg_examples/empty_req_srv/srv/EmptyRequest.srv b/rosrust_codegen/src/msg_examples/empty_req_srv/srv/EmptyRequest.srv similarity index 100% rename from rosrust/src/build_tools/msg_examples/empty_req_srv/srv/EmptyRequest.srv rename to rosrust_codegen/src/msg_examples/empty_req_srv/srv/EmptyRequest.srv diff --git a/rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Point.msg b/rosrust_codegen/src/msg_examples/geometry_msgs/msg/Point.msg similarity index 100% rename from rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Point.msg rename to rosrust_codegen/src/msg_examples/geometry_msgs/msg/Point.msg diff --git a/rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Pose.msg b/rosrust_codegen/src/msg_examples/geometry_msgs/msg/Pose.msg similarity index 100% rename from rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Pose.msg rename to rosrust_codegen/src/msg_examples/geometry_msgs/msg/Pose.msg diff --git a/rosrust/src/build_tools/msg_examples/geometry_msgs/msg/PoseStamped.msg b/rosrust_codegen/src/msg_examples/geometry_msgs/msg/PoseStamped.msg similarity index 100% rename from rosrust/src/build_tools/msg_examples/geometry_msgs/msg/PoseStamped.msg rename to rosrust_codegen/src/msg_examples/geometry_msgs/msg/PoseStamped.msg diff --git a/rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Quaternion.msg b/rosrust_codegen/src/msg_examples/geometry_msgs/msg/Quaternion.msg similarity index 100% rename from rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Quaternion.msg rename to rosrust_codegen/src/msg_examples/geometry_msgs/msg/Quaternion.msg diff --git a/rosrust/src/build_tools/msg_examples/geometry_msgs/msg/TwistWithCovariance.msg b/rosrust_codegen/src/msg_examples/geometry_msgs/msg/TwistWithCovariance.msg similarity index 100% rename from rosrust/src/build_tools/msg_examples/geometry_msgs/msg/TwistWithCovariance.msg rename to rosrust_codegen/src/msg_examples/geometry_msgs/msg/TwistWithCovariance.msg diff --git a/rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Vector3.msg b/rosrust_codegen/src/msg_examples/geometry_msgs/msg/Vector3.msg similarity index 100% rename from rosrust/src/build_tools/msg_examples/geometry_msgs/msg/Vector3.msg rename to rosrust_codegen/src/msg_examples/geometry_msgs/msg/Vector3.msg diff --git a/rosrust/src/build_tools/msg_examples/roscpp_tutorials/srv/TwoInts.srv b/rosrust_codegen/src/msg_examples/roscpp_tutorials/srv/TwoInts.srv similarity index 100% rename from rosrust/src/build_tools/msg_examples/roscpp_tutorials/srv/TwoInts.srv rename to rosrust_codegen/src/msg_examples/roscpp_tutorials/srv/TwoInts.srv diff --git a/rosrust/src/build_tools/msg_examples/sensor_msgs/msg/Imu.msg b/rosrust_codegen/src/msg_examples/sensor_msgs/msg/Imu.msg similarity index 100% rename from rosrust/src/build_tools/msg_examples/sensor_msgs/msg/Imu.msg rename to rosrust_codegen/src/msg_examples/sensor_msgs/msg/Imu.msg diff --git a/rosrust/src/build_tools/msg_examples/simple_srv/srv/Something.srv b/rosrust_codegen/src/msg_examples/simple_srv/srv/Something.srv similarity index 100% rename from rosrust/src/build_tools/msg_examples/simple_srv/srv/Something.srv rename to rosrust_codegen/src/msg_examples/simple_srv/srv/Something.srv diff --git a/rosrust/src/build_tools/msg_examples/std_msgs/msg/Header.msg b/rosrust_codegen/src/msg_examples/std_msgs/msg/Header.msg similarity index 100% rename from rosrust/src/build_tools/msg_examples/std_msgs/msg/Header.msg rename to rosrust_codegen/src/msg_examples/std_msgs/msg/Header.msg diff --git a/rosrust/src/build_tools/msg_examples/std_msgs/msg/String.msg b/rosrust_codegen/src/msg_examples/std_msgs/msg/String.msg similarity index 100% rename from rosrust/src/build_tools/msg_examples/std_msgs/msg/String.msg rename to rosrust_codegen/src/msg_examples/std_msgs/msg/String.msg diff --git a/rosrust/src/build_tools/msg_examples/tricky_comment_srv/srv/TrickyComment.srv b/rosrust_codegen/src/msg_examples/tricky_comment_srv/srv/TrickyComment.srv similarity index 100% rename from rosrust/src/build_tools/msg_examples/tricky_comment_srv/srv/TrickyComment.srv rename to rosrust_codegen/src/msg_examples/tricky_comment_srv/srv/TrickyComment.srv From 132bf596be53f269dcef770e2d316dcd91fa59ae Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sun, 24 Dec 2017 00:28:48 +0100 Subject: [PATCH 35/50] Fix builds --- .travis.yml | 2 +- examples/pubsub/Cargo.toml | 3 ++- examples/pubsub/build.rs | 2 +- examples/pubsub/src/publisher.rs | 3 ++- examples/pubsub/src/subscriber.rs | 3 ++- examples/serviceclient/Cargo.toml | 3 ++- examples/serviceclient/build.rs | 2 +- examples/serviceclient/src/client.rs | 3 ++- examples/serviceclient/src/service.rs | 3 ++- rosrust/Cargo.toml | 1 + rosrust_codegen/src/genmsg.rs | 2 +- rosrust_codegen/src/helpers.rs | 2 +- rosrust_codegen/src/lib.rs | 2 +- 13 files changed, 19 insertions(+), 12 deletions(-) diff --git a/.travis.yml b/.travis.yml index e573638..5609cf4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,6 +1,6 @@ language: rust env: - - ROSRUST_MSG_PATH=/home/travis/build/adnanademovic/rosrust/rosrust/src/build_tools/msg_examples + - ROSRUST_MSG_PATH=/home/travis/build/adnanademovic/rosrust/rosrust_codegen/src/msg_examples rust: - stable - beta diff --git a/examples/pubsub/Cargo.toml b/examples/pubsub/Cargo.toml index 4722c1c..4845556 100644 --- a/examples/pubsub/Cargo.toml +++ b/examples/pubsub/Cargo.toml @@ -8,6 +8,7 @@ version = "0.0.1" [dependencies] env_logger = "0.4.3" rosrust = "*" +rosrust_codegen = "*" serde = "1.0.24" serde_derive = "1.0.24" @@ -21,4 +22,4 @@ path = "src/subscriber.rs" [build-dependencies] env_logger = "0.4.3" -rosrust = "*" +rosrust_codegen = "*" diff --git a/examples/pubsub/build.rs b/examples/pubsub/build.rs index 72dc60e..804ea27 100644 --- a/examples/pubsub/build.rs +++ b/examples/pubsub/build.rs @@ -1,4 +1,4 @@ #[macro_use] -extern crate rosrust; +extern crate rosrust_codegen; rosmsg_main!("std_msgs/String"); diff --git a/examples/pubsub/src/publisher.rs b/examples/pubsub/src/publisher.rs index 83ff39a..b2b43e4 100644 --- a/examples/pubsub/src/publisher.rs +++ b/examples/pubsub/src/publisher.rs @@ -1,6 +1,7 @@ extern crate env_logger; -#[macro_use] extern crate rosrust; +#[macro_use] +extern crate rosrust_codegen; use rosrust::Ros; diff --git a/examples/pubsub/src/subscriber.rs b/examples/pubsub/src/subscriber.rs index e14c126..1ef1861 100644 --- a/examples/pubsub/src/subscriber.rs +++ b/examples/pubsub/src/subscriber.rs @@ -1,6 +1,7 @@ extern crate env_logger; -#[macro_use] extern crate rosrust; +#[macro_use] +extern crate rosrust_codegen; use rosrust::Ros; diff --git a/examples/serviceclient/Cargo.toml b/examples/serviceclient/Cargo.toml index 84394f5..b22112b 100644 --- a/examples/serviceclient/Cargo.toml +++ b/examples/serviceclient/Cargo.toml @@ -8,6 +8,7 @@ version = "0.0.1" [dependencies] env_logger = "0.4.3" rosrust = "*" +rosrust_codegen = "*" serde = "1.0.24" serde_derive = "1.0.24" @@ -21,4 +22,4 @@ path = "src/service.rs" [build-dependencies] env_logger = "0.4.3" -rosrust = "*" +rosrust_codegen = "*" diff --git a/examples/serviceclient/build.rs b/examples/serviceclient/build.rs index b4f9463..4ec2752 100644 --- a/examples/serviceclient/build.rs +++ b/examples/serviceclient/build.rs @@ -1,4 +1,4 @@ #[macro_use] -extern crate rosrust; +extern crate rosrust_codegen; rosmsg_main!("roscpp_tutorials/TwoInts"); diff --git a/examples/serviceclient/src/client.rs b/examples/serviceclient/src/client.rs index ebbf281..36d2bfc 100644 --- a/examples/serviceclient/src/client.rs +++ b/examples/serviceclient/src/client.rs @@ -1,6 +1,7 @@ extern crate env_logger; -#[macro_use] extern crate rosrust; +#[macro_use] +extern crate rosrust_codegen; use rosrust::Ros; use std::{env, time}; diff --git a/examples/serviceclient/src/service.rs b/examples/serviceclient/src/service.rs index 33ed899..fa7f053 100644 --- a/examples/serviceclient/src/service.rs +++ b/examples/serviceclient/src/service.rs @@ -1,6 +1,7 @@ extern crate env_logger; -#[macro_use] extern crate rosrust; +#[macro_use] +extern crate rosrust_codegen; use rosrust::Ros; diff --git a/rosrust/Cargo.toml b/rosrust/Cargo.toml index 21ca717..d5feb66 100644 --- a/rosrust/Cargo.toml +++ b/rosrust/Cargo.toml @@ -16,6 +16,7 @@ lazy_static = "1.0.0" log = "0.3.8" nix = "0.9.0" regex = "0.2.3" +rosrust_codegen = "0.0.1" rust-crypto = "0.2.36" serde = "1.0.24" serde_derive = "1.0.24" diff --git a/rosrust_codegen/src/genmsg.rs b/rosrust_codegen/src/genmsg.rs index feda3f8..f3f4c08 100644 --- a/rosrust_codegen/src/genmsg.rs +++ b/rosrust_codegen/src/genmsg.rs @@ -128,7 +128,7 @@ fn string_into_pair<'a>(input: &'a str) -> Result<(&'a str, &'a str)> { mod tests { use super::*; - static FILEPATH: &'static str = "src/build_tools/msg_examples"; + static FILEPATH: &'static str = "src/msg_examples"; #[test] fn depend_on_messages_printout() { diff --git a/rosrust_codegen/src/helpers.rs b/rosrust_codegen/src/helpers.rs index dc89039..316bc11 100644 --- a/rosrust_codegen/src/helpers.rs +++ b/rosrust_codegen/src/helpers.rs @@ -187,7 +187,7 @@ fn get_message(folders: &[&str], package: &str, name: &str) -> Result { fn main() { - $crate::build_tools::depend_on_messages(&[ + $crate::depend_on_messages(&[ $( $msg, )* From 19aa3995e21126b20800be0590398eafc2cbc57b Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sun, 24 Dec 2017 01:32:36 +0100 Subject: [PATCH 36/50] Fix aliased typenames --- rosrust_codegen/src/helpers.rs | 34 ++++++++++++++++--- rosrust_codegen/src/msg.rs | 30 +++++++++------- .../msg_examples/rosgraph_msgs/msg/Clock.msg | 4 +++ .../msg_examples/rosgraph_msgs/msg/Log.msg | 19 +++++++++++ 4 files changed, 70 insertions(+), 17 deletions(-) create mode 100644 rosrust_codegen/src/msg_examples/rosgraph_msgs/msg/Clock.msg create mode 100644 rosrust_codegen/src/msg_examples/rosgraph_msgs/msg/Log.msg diff --git a/rosrust_codegen/src/helpers.rs b/rosrust_codegen/src/helpers.rs index 316bc11..8c70284 100644 --- a/rosrust_codegen/src/helpers.rs +++ b/rosrust_codegen/src/helpers.rs @@ -244,10 +244,15 @@ mod tests { fn get_message_map_traverses_all_passed_messages_dependency_tree() { let message_map = get_message_map( &[FILEPATH], - &[("geometry_msgs", "PoseStamped"), ("sensor_msgs", "Imu")], + &[ + ("geometry_msgs", "PoseStamped"), + ("sensor_msgs", "Imu"), + ("rosgraph_msgs", "Clock"), + ("rosgraph_msgs", "Log"), + ], ).unwrap() .messages; - assert_eq!(message_map.len(), 7); + assert_eq!(message_map.len(), 9); assert!(message_map.contains_key( &("geometry_msgs".into(), "Vector3".into()), )); @@ -269,16 +274,27 @@ mod tests { assert!(message_map.contains_key( &("std_msgs".into(), "Header".into()), )); + assert!(message_map.contains_key( + &("rosgraph_msgs".into(), "Clock".into()), + )); + assert!(message_map.contains_key( + &("rosgraph_msgs".into(), "Log".into()), + )); } #[test] fn calculate_md5_works() { let message_map = get_message_map( &[FILEPATH], - &[("geometry_msgs", "PoseStamped"), ("sensor_msgs", "Imu")], + &[ + ("geometry_msgs", "PoseStamped"), + ("sensor_msgs", "Imu"), + ("rosgraph_msgs", "Clock"), + ("rosgraph_msgs", "Log"), + ], ).unwrap(); let hashes = calculate_md5(&message_map).unwrap(); - assert_eq!(hashes.len(), 7); + assert_eq!(hashes.len(), 9); assert_eq!( *hashes .get(&("geometry_msgs".into(), "Vector3".into())) @@ -317,6 +333,16 @@ mod tests { *hashes.get(&("sensor_msgs".into(), "Imu".into())).unwrap(), "6a62c6daae103f4ff57a132d6f95cec2".to_owned() ); + assert_eq!( + *hashes + .get(&("rosgraph_msgs".into(), "Clock".into())) + .unwrap(), + "a9c97c1d230cfc112e270351a944ee47".to_owned() + ); + assert_eq!( + *hashes.get(&("rosgraph_msgs".into(), "Log".into())).unwrap(), + "acffd30cd6b6de30f120938c17c593fb".to_owned() + ); } #[test] diff --git a/rosrust_codegen/src/msg.rs b/rosrust_codegen/src/msg.rs index bbbaa45..ad33121 100644 --- a/rosrust_codegen/src/msg.rs +++ b/rosrust_codegen/src/msg.rs @@ -396,11 +396,11 @@ impl FieldInfo { #[derive(Debug, PartialEq)] enum DataType { Bool, - I8, + I8(bool), I16, I32, I64, - U8, + U8(bool), U16, U32, U64, @@ -417,11 +417,11 @@ impl DataType { fn rust_type(&self) -> String { match *self { DataType::Bool => "bool".into(), - DataType::I8 => "i8".into(), + DataType::I8(_) => "i8".into(), DataType::I16 => "i16".into(), DataType::I32 => "i32".into(), DataType::I64 => "i64".into(), - DataType::U8 => "u8".into(), + DataType::U8(_) => "u8".into(), DataType::U16 => "u16".into(), DataType::U32 => "u32".into(), DataType::U64 => "u64".into(), @@ -437,8 +437,8 @@ impl DataType { fn is_builtin(&self) -> bool { match *self { - DataType::Bool | DataType::I8 | DataType::I16 | DataType::I32 | DataType::I64 | - DataType::U8 | DataType::U16 | DataType::U32 | DataType::U64 | DataType::F32 | + DataType::Bool | DataType::I8(_) | DataType::I16 | DataType::I32 | DataType::I64 | + DataType::U8(_) | DataType::U16 | DataType::U32 | DataType::U64 | DataType::F32 | DataType::F64 | DataType::String | DataType::Time | DataType::Duration => true, DataType::LocalStruct(_) | DataType::RemoteStruct(_, _) => false, @@ -448,11 +448,11 @@ impl DataType { fn rust_newtype(&self) -> String { match *self { DataType::Bool => "false".into(), - DataType::I8 => "0i8".into(), + DataType::I8(_) => "0i8".into(), DataType::I16 => "0i16".into(), DataType::I32 => "0i32".into(), DataType::I64 => "0i64".into(), - DataType::U8 => "0u8".into(), + DataType::U8(_) => "0u8".into(), DataType::U16 => "0u16".into(), DataType::U32 => "0u32".into(), DataType::U64 => "0u64".into(), @@ -470,11 +470,13 @@ impl DataType { Ok( match *self { DataType::Bool => "bool", - DataType::I8 => "int8", + DataType::I8(true) => "int8", + DataType::I8(false) => "byte", DataType::I16 => "int16", DataType::I32 => "int32", DataType::I64 => "int64", - DataType::U8 => "uint8", + DataType::U8(true) => "uint8", + DataType::U8(false) => "uint8", DataType::U16 => "uint16", DataType::U32 => "uint32", DataType::U64 => "uint64", @@ -500,11 +502,13 @@ impl DataType { fn parse_datatype(datatype: &str) -> Option { match datatype { "bool" => Some(DataType::Bool), - "int8" | "byte" => Some(DataType::I8), + "int8" => Some(DataType::I8(true)), + "byte" => Some(DataType::I8(false)), "int16" => Some(DataType::I16), "int32" => Some(DataType::I32), "int64" => Some(DataType::I64), - "uint8" | "char" => Some(DataType::U8), + "uint8" => Some(DataType::U8(true)), + "char" => Some(DataType::U8(false)), "uint16" => Some(DataType::U16), "uint32" => Some(DataType::U32), "uint64" => Some(DataType::U64), @@ -774,7 +778,7 @@ mod tests { assert_eq!( FieldInfo { - datatype: DataType::U8, + datatype: DataType::U8(false), name: "myname".into(), case: FieldCase::Array(127), }, diff --git a/rosrust_codegen/src/msg_examples/rosgraph_msgs/msg/Clock.msg b/rosrust_codegen/src/msg_examples/rosgraph_msgs/msg/Clock.msg new file mode 100644 index 0000000..423872f --- /dev/null +++ b/rosrust_codegen/src/msg_examples/rosgraph_msgs/msg/Clock.msg @@ -0,0 +1,4 @@ +# roslib/Clock is used for publishing simulated time in ROS. +# This message simply communicates the current time. +# For more information, see http://www.ros.org/wiki/Clock +time clock diff --git a/rosrust_codegen/src/msg_examples/rosgraph_msgs/msg/Log.msg b/rosrust_codegen/src/msg_examples/rosgraph_msgs/msg/Log.msg new file mode 100644 index 0000000..9a9597c --- /dev/null +++ b/rosrust_codegen/src/msg_examples/rosgraph_msgs/msg/Log.msg @@ -0,0 +1,19 @@ +## +## Severity level constants +## +byte DEBUG=1 #debug level +byte INFO=2 #general level +byte WARN=4 #warning level +byte ERROR=8 #error level +byte FATAL=16 #fatal/critical level +## +## Fields +## +Header header +byte level +string name # name of the node +string msg # message +string file # file the message came from +string function # function the message came from +uint32 line # line the message came from +string[] topics # topic names that the node publishes From d676a1fa6a2bdf45c8ab88996f52845e265ed5b3 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sun, 24 Dec 2017 01:47:38 +0100 Subject: [PATCH 37/50] Implement const fields as associated constants Fixes #22 --- rosrust_codegen/src/genmsg.rs | 16 ++-------------- rosrust_codegen/src/msg.rs | 19 +++++-------------- 2 files changed, 7 insertions(+), 28 deletions(-) diff --git a/rosrust_codegen/src/genmsg.rs b/rosrust_codegen/src/genmsg.rs index f3f4c08..bf54d02 100644 --- a/rosrust_codegen/src/genmsg.rs +++ b/rosrust_codegen/src/genmsg.rs @@ -48,18 +48,9 @@ pub fn depend_on_messages(folders: &[&str], messages: &[&str]) -> Result output.push(" }".into()); output.push(format!(" impl {} {{", message.name)); output.push(message.new_string()); - output.push(" }".into()); - } - output.push(" #[allow(non_snake_case)]".into()); - output.push(" pub mod CONST {".into()); - for name in &names { - let message = message_map - .messages - .get(&(package.clone(), name.clone())) - .expect("Internal implementation contains mismatch in map keys"); output.push(message.const_string()); + output.push(" }".into()); } - output.push(" }".into()); let names = message_map .services .iter() @@ -132,10 +123,7 @@ mod tests { #[test] fn depend_on_messages_printout() { - let data = depend_on_messages( - &[FILEPATH], - &["geometry_msgs/PoseStamped", "sensor_msgs/Imu"], - ).unwrap(); + let data = depend_on_messages(&[FILEPATH], &["rosgraph_msgs/Log"]).unwrap(); println!("{}", data); // TODO: actually test this output data } diff --git a/rosrust_codegen/src/msg.rs b/rosrust_codegen/src/msg.rs index ad33121..7e686da 100644 --- a/rosrust_codegen/src/msg.rs +++ b/rosrust_codegen/src/msg.rs @@ -103,17 +103,14 @@ impl Msg { pub fn const_string(&self) -> String { let mut output = Vec::::new(); - output.push(" #[allow(non_snake_case)]".into()); - output.push(format!(" pub mod {} {{", self.name)); for field in &self.fields { if let Some(s) = field.to_const_string() { output.push( - " #[allow(dead_code,non_upper_case_globals)]".into(), + " #[allow(dead_code,non_upper_case_globals)]".into(), ); - output.push(format!(" pub {}", s)); + output.push(format!(" pub const {}", s)); } } - output.push(" }".into()); output.join("\n") } @@ -363,21 +360,15 @@ impl FieldInfo { _ => return None, }; Some(match self.datatype { - DataType::Bool => format!("const {}: bool = {:?};", self.name, value != "0"), - DataType::String => format!("static {}: &'static str = {:?};", self.name, value), + DataType::Bool => format!("{}: bool = {:?};", self.name, value != "0"), + DataType::String => format!("{}: &'static str = {:?};", self.name, value), DataType::Time => return None, DataType::Duration => return None, DataType::LocalStruct(..) => return None, DataType::RemoteStruct(..) => return None, _ => { let datatype = self.datatype.rust_type(); - format!( - "const {}: {} = {} as {};", - self.name, - datatype, - value, - datatype - ) + format!("{}: {} = {} as {};", self.name, datatype, value, datatype) } }) } From a27e10506e0b1a537998df5bcd3a337140e131c0 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sun, 24 Dec 2017 01:53:30 +0100 Subject: [PATCH 38/50] Implement default for messages --- rosrust_codegen/src/genmsg.rs | 5 +++-- rosrust_codegen/src/msg.rs | 4 +++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/rosrust_codegen/src/genmsg.rs b/rosrust_codegen/src/genmsg.rs index bf54d02..fc12712 100644 --- a/rosrust_codegen/src/genmsg.rs +++ b/rosrust_codegen/src/genmsg.rs @@ -47,8 +47,8 @@ pub fn depend_on_messages(folders: &[&str], messages: &[&str]) -> Result output.push(create_function("msg_type", &message.get_type())); output.push(" }".into()); output.push(format!(" impl {} {{", message.name)); - output.push(message.new_string()); output.push(message.const_string()); + output.push(" fn new() -> Self { Self::default() }".into()); output.push(" }".into()); } let names = message_map @@ -123,7 +123,8 @@ mod tests { #[test] fn depend_on_messages_printout() { - let data = depend_on_messages(&[FILEPATH], &["rosgraph_msgs/Log"]).unwrap(); + let data = depend_on_messages(&[FILEPATH], &["rosgraph_msgs/Clock", "rosgraph_msgs/Log"]) + .unwrap(); println!("{}", data); // TODO: actually test this output data } diff --git a/rosrust_codegen/src/msg.rs b/rosrust_codegen/src/msg.rs index 7e686da..600ae56 100644 --- a/rosrust_codegen/src/msg.rs +++ b/rosrust_codegen/src/msg.rs @@ -119,7 +119,9 @@ impl Msg { output.push( " #[allow(dead_code,non_camel_case_types,non_snake_case)]".into(), ); - output.push(" #[derive(Serialize,Deserialize,Debug)]".into()); + output.push( + " #[derive(Serialize,Deserialize,Debug,Default)]".into(), + ); output.push(format!(" pub struct {} {{", self.name)); for field in &self.fields { if let Some(s) = field.to_string() { From cf7f7ed514645660ccdf44bb04cfef79644f7350 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sun, 24 Dec 2017 02:31:34 +0100 Subject: [PATCH 39/50] Use codegen within rosrust --- rosrust/Cargo.toml | 4 +- rosrust/build.rs | 4 ++ rosrust/src/api/clock.rs | 25 +----------- rosrust/src/api/ros.rs | 4 +- rosrust/src/lib.rs | 9 ++--- rosrust/src/{msg.rs => time.rs} | 0 rosrust_codegen/Cargo.toml | 1 - rosrust_codegen/src/genmsg.rs | 45 ++++++++++++++++----- rosrust_codegen/src/lib.rs | 22 +++++++--- rosrust_codegen/src/msg.rs | 72 +++++---------------------------- 10 files changed, 76 insertions(+), 110 deletions(-) create mode 100644 rosrust/build.rs rename rosrust/src/{msg.rs => time.rs} (100%) diff --git a/rosrust/Cargo.toml b/rosrust/Cargo.toml index d5feb66..aa9089f 100644 --- a/rosrust/Cargo.toml +++ b/rosrust/Cargo.toml @@ -12,7 +12,6 @@ byteorder = "1.2.1" error-chain = "0.11.0" futures = "0.1.17" itertools = "0.7.4" -lazy_static = "1.0.0" log = "0.3.8" nix = "0.9.0" regex = "0.2.3" @@ -23,3 +22,6 @@ serde_derive = "1.0.24" serde_rosmsg = "0.2.0" xml-rpc = "0.0.7" yaml-rust = "0.4.0" + +[build-dependencies] +rosrust_codegen = "0.0.1" diff --git a/rosrust/build.rs b/rosrust/build.rs new file mode 100644 index 0000000..3410729 --- /dev/null +++ b/rosrust/build.rs @@ -0,0 +1,4 @@ +#[macro_use] +extern crate rosrust_codegen; + +rosmsg_main_no_prefix!("rosgraph_msgs/Clock"); diff --git a/rosrust/src/api/clock.rs b/rosrust/src/api/clock.rs index cce29fb..c89637c 100644 --- a/rosrust/src/api/clock.rs +++ b/rosrust/src/api/clock.rs @@ -1,10 +1,10 @@ -use msg::{Duration, Time}; use std::cmp; use std::collections::BinaryHeap; use std::sync::{Arc, Mutex}; use std::sync::mpsc::{channel, Sender}; use std::thread::sleep; use std::time::{Duration as StdDuration, SystemTime, UNIX_EPOCH}; +use time::{Duration, Time}; static BEFORE_EPOCH: &'static str = "Requested time is before UNIX epoch."; @@ -155,26 +155,3 @@ impl Clock for SimulatedClock { static FAILED_TO_LOCK: &'static str = "Failed to acquire lock"; static SLEEPING_THREAD_MISSING: &'static str = "Failed to find sleeping thread"; - -pub mod rosgraph_msgs { - use Message; - use msg::Time; - - #[derive(Serialize, Deserialize, Debug)] - pub struct Clock { - pub clock: Time, - } - impl Message for Clock { - fn msg_definition() -> ::std::string::String { - "# roslib/Clock is used for publishing simulated time in ROS. \n# This message simply communicates the current time.\n# For more information, see http://www.ros.org/wiki/Clock\ntime clock\n".into() - } - - fn md5sum() -> ::std::string::String { - "a9c97c1d230cfc112e270351a944ee47".into() - } - - fn msg_type() -> ::std::string::String { - "rosgraph_msgs/Clock".into() - } - } -} diff --git a/rosrust/src/api/ros.rs b/rosrust/src/api/ros.rs index f2f1676..bbb8562 100644 --- a/rosrust/src/api/ros.rs +++ b/rosrust/src/api/ros.rs @@ -1,9 +1,9 @@ -use msg::{Duration, Time}; +use msg::rosgraph_msgs::Clock as ClockMsg; +use time::{Duration, Time}; use serde::{Serialize, Deserialize}; use std::sync::{Arc, mpsc}; use std::time; use super::clock::{Clock, Rate, RealClock, SimulatedClock}; -use super::clock::rosgraph_msgs::Clock as ClockMsg; use super::master::{self, Master, Topic}; use super::slave::Slave; use super::error::{ErrorKind, Result, ResultExt}; diff --git a/rosrust/src/lib.rs b/rosrust/src/lib.rs index fe6c845..c162b8d 100644 --- a/rosrust/src/lib.rs +++ b/rosrust/src/lib.rs @@ -7,16 +7,12 @@ extern crate error_chain; extern crate futures; extern crate itertools; #[macro_use] -extern crate lazy_static; -#[macro_use] extern crate log; extern crate nix; extern crate regex; #[macro_use] extern crate rosrust_codegen; extern crate serde; -#[macro_use] -extern crate serde_derive; extern crate serde_rosmsg; extern crate xml_rpc; extern crate yaml_rust; @@ -24,8 +20,11 @@ extern crate yaml_rust; pub use api::Ros; pub use tcpros::{Client, PublisherStream, Message, ServicePair as Service}; pub use api::error; +pub use time::{Duration, Time}; mod api; mod rosxmlrpc; mod tcpros; -pub mod msg; +mod time; + +rosmsg_include!(); diff --git a/rosrust/src/msg.rs b/rosrust/src/time.rs similarity index 100% rename from rosrust/src/msg.rs rename to rosrust/src/time.rs diff --git a/rosrust_codegen/Cargo.toml b/rosrust_codegen/Cargo.toml index 94f8dd2..e1af7dc 100644 --- a/rosrust_codegen/Cargo.toml +++ b/rosrust_codegen/Cargo.toml @@ -8,7 +8,6 @@ version = "0.0.1" [dependencies] error-chain = "0.11.0" -itertools = "0.7.4" lazy_static = "1.0.0" regex = "0.2.3" rust-crypto = "0.2.36" diff --git a/rosrust_codegen/src/genmsg.rs b/rosrust_codegen/src/genmsg.rs index fc12712..f077841 100644 --- a/rosrust_codegen/src/genmsg.rs +++ b/rosrust_codegen/src/genmsg.rs @@ -2,7 +2,11 @@ use std::collections::HashSet; use helpers; use error::Result; -pub fn depend_on_messages(folders: &[&str], messages: &[&str]) -> Result { +pub fn depend_on_messages( + folders: &[&str], + messages: &[&str], + crate_prefix: &str, +) -> Result { let mut output = Vec::::new(); output.push("#[macro_use]\nextern crate serde_derive;".into()); output.push("pub mod msg {".into()); @@ -37,9 +41,10 @@ pub fn depend_on_messages(folders: &[&str], messages: &[&str]) -> Result "Internal implementation contains mismatch in map keys", ); let definition = helpers::generate_message_definition(&message_map.messages, &message)?; - output.push(message.struct_string()); + output.push(message.struct_string(crate_prefix)); output.push(format!( - " impl ::rosrust::Message for {} {{", + " impl {}Message for {} {{", + crate_prefix, message.name )); output.push(create_function("msg_definition", &definition)); @@ -47,8 +52,12 @@ pub fn depend_on_messages(folders: &[&str], messages: &[&str]) -> Result output.push(create_function("msg_type", &message.get_type())); output.push(" }".into()); output.push(format!(" impl {} {{", message.name)); - output.push(message.const_string()); - output.push(" fn new() -> Self { Self::default() }".into()); + output.push(message.const_string(crate_prefix)); + output.push(" #[allow(dead_code)]".into()); + output.push(" #[inline]".into()); + output.push( + " pub fn new() -> Self { Self::default() }".into(), + ); output.push(" }".into()); } let names = message_map @@ -66,7 +75,11 @@ pub fn depend_on_messages(folders: &[&str], messages: &[&str]) -> Result ); output.push(" #[derive(Serialize,Deserialize,Debug)]".into()); output.push(format!(" pub struct {} {{}}", name)); - output.push(format!(" impl ::rosrust::Message for {} {{", name)); + output.push(format!( + " impl {}Message for {} {{", + crate_prefix, + name + )); output.push(create_function("msg_definition", "")); output.push(create_function("md5sum", &hash)); output.push(create_function( @@ -75,7 +88,11 @@ pub fn depend_on_messages(folders: &[&str], messages: &[&str]) -> Result )); output.push(" }".into()); - output.push(format!(" impl ::rosrust::Service for {} {{", name)); + output.push(format!( + " impl {}Service for {} {{", + crate_prefix, + name + )); output.push(format!(" type Request = {}Req;", name)); output.push(format!(" type Response = {}Res;", name)); output.push(" }".into()); @@ -89,6 +106,7 @@ pub fn depend_on_messages(folders: &[&str], messages: &[&str]) -> Result fn create_function(name: &str, value: &str) -> String { format!( r#" + #[inline] fn {}() -> ::std::string::String {{ {:?}.into() }}"#, @@ -123,22 +141,27 @@ mod tests { #[test] fn depend_on_messages_printout() { - let data = depend_on_messages(&[FILEPATH], &["rosgraph_msgs/Clock", "rosgraph_msgs/Log"]) - .unwrap(); + let data = depend_on_messages( + &[FILEPATH], + &["rosgraph_msgs/Clock", "rosgraph_msgs/Log"], + "::rosrust::", + ).unwrap(); println!("{}", data); // TODO: actually test this output data } #[test] fn benchmark_genmsg() { - let data = depend_on_messages(&[FILEPATH], &["benchmark_msgs/Overall"]).unwrap(); + let data = depend_on_messages(&[FILEPATH], &["benchmark_msgs/Overall"], "::rosrust::") + .unwrap(); println!("{}", data); // TODO: actually test this output data } #[test] fn benchmark_genmsg_service() { - let data = depend_on_messages(&[FILEPATH], &["simple_srv/Something"]).unwrap(); + let data = depend_on_messages(&[FILEPATH], &["simple_srv/Something"], "::rosrust::") + .unwrap(); println!("{}", data); // TODO: actually test this output data } diff --git a/rosrust_codegen/src/lib.rs b/rosrust_codegen/src/lib.rs index 520e024..1e49d16 100644 --- a/rosrust_codegen/src/lib.rs +++ b/rosrust_codegen/src/lib.rs @@ -3,7 +3,6 @@ extern crate crypto; #[macro_use] extern crate error_chain; -extern crate itertools; #[macro_use] extern crate lazy_static; extern crate regex; @@ -20,13 +19,26 @@ use std::path::Path; #[macro_export] macro_rules! rosmsg_main { - ($($msg:expr),*)=> { + ($($msg:expr),*) => { fn main() { $crate::depend_on_messages(&[ $( $msg, )* - ]); + ], "::rosrust::"); + } + } +} + +#[macro_export] +macro_rules! rosmsg_main_no_prefix { + ($($msg:expr),*) => { + fn main() { + $crate::depend_on_messages(&[ + $( + $msg, + )* + ], "::"); } } } @@ -36,7 +48,7 @@ macro_rules! rosmsg_include { () => {include!(concat!(env!("OUT_DIR"), "/msg.rs"));} } -pub fn depend_on_messages(messages: &[&str]) { +pub fn depend_on_messages(messages: &[&str], crate_prefix: &str) { let cmake_paths = env::var("CMAKE_PREFIX_PATH") .unwrap_or(String::new()) .split(":") @@ -58,7 +70,7 @@ pub fn depend_on_messages(messages: &[&str]) { .chain(extra_paths.iter()) .map(|v| v.as_str()) .collect::>(); - let output = genmsg::depend_on_messages(paths.as_slice(), messages).unwrap(); + let output = genmsg::depend_on_messages(paths.as_slice(), messages, crate_prefix).unwrap(); let out_dir = env::var("OUT_DIR").unwrap(); let dest_path = Path::new(&out_dir).join("msg.rs"); let mut f = File::create(&dest_path).unwrap(); diff --git a/rosrust_codegen/src/msg.rs b/rosrust_codegen/src/msg.rs index 600ae56..5557e74 100644 --- a/rosrust_codegen/src/msg.rs +++ b/rosrust_codegen/src/msg.rs @@ -1,4 +1,3 @@ -use itertools::{self, Itertools}; use regex::Regex; use error::{Result, ResultExt}; use std::collections::HashMap; @@ -21,38 +20,6 @@ impl Msg { }) } - pub fn new_string(&self) -> String { - let mut lines = Vec::new(); - lines.push(format!(" pub fn new() -> {} {{", self.name)); - lines.push(format!(" {} {{", self.name)); - for field in self.fields.iter() { - match field.case { - FieldCase::Unit => { - lines.push(format!( - " {}: {},", - field.name, - field.datatype.rust_newtype() - )); - } - FieldCase::Vector => { - lines.push(format!(" {}: Vec::new(),", field.name)); - } - FieldCase::Array(l) => { - let newtype = field.datatype.rust_newtype(); - lines.push(format!( - " {}: [{}],", - field.name, - itertools::repeat_n(newtype.as_str(), l).join(", ") - )); - } - FieldCase::Const(..) => {} - } - } - lines.push(" }".into()); - lines.push(" }".into()); - lines.join("\n") - } - pub fn get_type(&self) -> String { format!("{}/{}", self.package, self.name) } @@ -101,10 +68,10 @@ impl Msg { Ok(representation) } - pub fn const_string(&self) -> String { + pub fn const_string(&self, crate_prefix: &str) -> String { let mut output = Vec::::new(); for field in &self.fields { - if let Some(s) = field.to_const_string() { + if let Some(s) = field.to_const_string(crate_prefix) { output.push( " #[allow(dead_code,non_upper_case_globals)]".into(), ); @@ -114,7 +81,7 @@ impl Msg { output.join("\n") } - pub fn struct_string(&self) -> String { + pub fn struct_string(&self, crate_prefix: &str) -> String { let mut output = Vec::::new(); output.push( " #[allow(dead_code,non_camel_case_types,non_snake_case)]".into(), @@ -124,7 +91,7 @@ impl Msg { ); output.push(format!(" pub struct {} {{", self.name)); for field in &self.fields { - if let Some(s) = field.to_string() { + if let Some(s) = field.to_string(crate_prefix) { output.push(format!(" pub {}", s)); } } @@ -346,8 +313,8 @@ impl FieldInfo { }) } - fn to_string(&self) -> Option { - let datatype = self.datatype.rust_type(); + fn to_string(&self, crate_prefix: &str) -> Option { + let datatype = self.datatype.rust_type(crate_prefix); match self.case { FieldCase::Unit => Some(format!("{}: {},", self.name, datatype)), FieldCase::Vector => Some(format!("{}: Vec<{}>,", self.name, datatype)), @@ -356,7 +323,7 @@ impl FieldInfo { } } - fn to_const_string(&self) -> Option { + fn to_const_string(&self, crate_prefix: &str) -> Option { let value = match self.case { FieldCase::Const(ref value) => value, _ => return None, @@ -369,7 +336,7 @@ impl FieldInfo { DataType::LocalStruct(..) => return None, DataType::RemoteStruct(..) => return None, _ => { - let datatype = self.datatype.rust_type(); + let datatype = self.datatype.rust_type(crate_prefix); format!("{}: {} = {} as {};", self.name, datatype, value, datatype) } }) @@ -407,7 +374,7 @@ enum DataType { } impl DataType { - fn rust_type(&self) -> String { + fn rust_type(&self, crate_prefix: &str) -> String { match *self { DataType::Bool => "bool".into(), DataType::I8(_) => "i8".into(), @@ -421,8 +388,8 @@ impl DataType { DataType::F32 => "f32".into(), DataType::F64 => "f64".into(), DataType::String => "::std::string::String".into(), - DataType::Time => "::rosrust::msg::Time".into(), - DataType::Duration => "::rosrust::msg::Duration".into(), + DataType::Time => format!("{}Time", crate_prefix), + DataType::Duration => format!("{}Duration", crate_prefix), DataType::LocalStruct(ref name) => name.clone(), DataType::RemoteStruct(ref pkg, ref name) => format!("super::{}::{}", pkg, name), } @@ -438,23 +405,6 @@ impl DataType { } } - fn rust_newtype(&self) -> String { - match *self { - DataType::Bool => "false".into(), - DataType::I8(_) => "0i8".into(), - DataType::I16 => "0i16".into(), - DataType::I32 => "0i32".into(), - DataType::I64 => "0i64".into(), - DataType::U8(_) => "0u8".into(), - DataType::U16 => "0u16".into(), - DataType::U32 => "0u32".into(), - DataType::U64 => "0u64".into(), - DataType::F32 => "0f32".into(), - DataType::F64 => "0f64".into(), - _ => format!("{}::new()", self.rust_type()), - } - } - fn md5_string( &self, package: &str, From b9ff13109640e3b10c1986c54f7848e33a9cc244 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sun, 24 Dec 2017 02:35:21 +0100 Subject: [PATCH 40/50] Remove new() from generated messages --- examples/pubsub/src/publisher.rs | 2 +- rosrust/Cargo.toml | 1 + rosrust/src/lib.rs | 3 +++ rosrust_codegen/src/genmsg.rs | 5 ----- 4 files changed, 5 insertions(+), 6 deletions(-) diff --git a/examples/pubsub/src/publisher.rs b/examples/pubsub/src/publisher.rs index b2b43e4..5bae281 100644 --- a/examples/pubsub/src/publisher.rs +++ b/examples/pubsub/src/publisher.rs @@ -17,7 +17,7 @@ fn main() { let mut rate = ros.rate(10.0); while ros.is_ok() { - let mut msg = msg::std_msgs::String::new(); + let mut msg = msg::std_msgs::String::default(); msg.data = format!("hello world {}", count); chatter_pub.send(msg).unwrap(); diff --git a/rosrust/Cargo.toml b/rosrust/Cargo.toml index aa9089f..8438a18 100644 --- a/rosrust/Cargo.toml +++ b/rosrust/Cargo.toml @@ -12,6 +12,7 @@ byteorder = "1.2.1" error-chain = "0.11.0" futures = "0.1.17" itertools = "0.7.4" +lazy_static = "1.0.0" log = "0.3.8" nix = "0.9.0" regex = "0.2.3" diff --git a/rosrust/src/lib.rs b/rosrust/src/lib.rs index c162b8d..c09b0e0 100644 --- a/rosrust/src/lib.rs +++ b/rosrust/src/lib.rs @@ -6,6 +6,9 @@ extern crate crypto; extern crate error_chain; extern crate futures; extern crate itertools; +#[cfg(test)] +#[macro_use] +extern crate lazy_static; #[macro_use] extern crate log; extern crate nix; diff --git a/rosrust_codegen/src/genmsg.rs b/rosrust_codegen/src/genmsg.rs index f077841..8fafdee 100644 --- a/rosrust_codegen/src/genmsg.rs +++ b/rosrust_codegen/src/genmsg.rs @@ -53,11 +53,6 @@ pub fn depend_on_messages( output.push(" }".into()); output.push(format!(" impl {} {{", message.name)); output.push(message.const_string(crate_prefix)); - output.push(" #[allow(dead_code)]".into()); - output.push(" #[inline]".into()); - output.push( - " pub fn new() -> Self { Self::default() }".into(), - ); output.push(" }".into()); } let names = message_map From 78a619e1105143f552c6f80950859285826faf73 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sun, 24 Dec 2017 03:00:05 +0100 Subject: [PATCH 41/50] Address all codegen warnings --- .travis.yml | 2 -- rosrust_codegen/src/genmsg.rs | 14 +++++++------- rosrust_codegen/src/helpers.rs | 12 +++++++----- rosrust_codegen/src/lib.rs | 22 ++++++++++++++++------ rosrust_codegen/src/msg.rs | 22 +++++++++++----------- 5 files changed, 41 insertions(+), 31 deletions(-) diff --git a/.travis.yml b/.travis.yml index 5609cf4..8c91a74 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,6 +1,4 @@ language: rust -env: - - ROSRUST_MSG_PATH=/home/travis/build/adnanademovic/rosrust/rosrust_codegen/src/msg_examples rust: - stable - beta diff --git a/rosrust_codegen/src/genmsg.rs b/rosrust_codegen/src/genmsg.rs index 8fafdee..16a57bf 100644 --- a/rosrust_codegen/src/genmsg.rs +++ b/rosrust_codegen/src/genmsg.rs @@ -19,7 +19,7 @@ pub fn depend_on_messages( let packages = message_map .messages .iter() - .map(|(&(ref pack, ref _name), ref _value)| pack.clone()) + .map(|(&(ref pack, ref _name), _value)| pack.clone()) .chain(message_map.services.iter().map(|&(ref pack, ref _name)| { pack.clone() })) @@ -29,8 +29,8 @@ pub fn depend_on_messages( let names = message_map .messages .iter() - .filter(|&(&(ref pack, ref _name), ref _value)| pack == &package) - .map(|(&(ref _pack, ref name), ref _value)| name.clone()) + .filter(|&(&(ref pack, ref _name), _value)| pack == &package) + .map(|(&(ref _pack, ref name), _value)| name.clone()) .collect::>(); for name in &names { let key = (package.clone(), name.clone()); @@ -40,7 +40,7 @@ pub fn depend_on_messages( let hash = hashes.get(&key).expect( "Internal implementation contains mismatch in map keys", ); - let definition = helpers::generate_message_definition(&message_map.messages, &message)?; + let definition = helpers::generate_message_definition(&message_map.messages, message)?; output.push(message.struct_string(crate_prefix)); output.push(format!( " impl {}Message for {} {{", @@ -48,7 +48,7 @@ pub fn depend_on_messages( message.name )); output.push(create_function("msg_definition", &definition)); - output.push(create_function("md5sum", &hash)); + output.push(create_function("md5sum", hash)); output.push(create_function("msg_type", &message.get_type())); output.push(" }".into()); output.push(format!(" impl {} {{", message.name)); @@ -76,7 +76,7 @@ pub fn depend_on_messages( name )); output.push(create_function("msg_definition", "")); - output.push(create_function("md5sum", &hash)); + output.push(create_function("md5sum", hash)); output.push(create_function( "msg_type", &format!("{}/{}", package, name), @@ -110,7 +110,7 @@ fn create_function(name: &str, value: &str) -> String { ) } -fn string_into_pair<'a>(input: &'a str) -> Result<(&'a str, &'a str)> { +fn string_into_pair(input: &str) -> Result<(&str, &str)> { let mut parts = input.splitn(2, '/'); let package = match parts.next() { Some(v) => v, diff --git a/rosrust_codegen/src/helpers.rs b/rosrust_codegen/src/helpers.rs index 8c70284..4431770 100644 --- a/rosrust_codegen/src/helpers.rs +++ b/rosrust_codegen/src/helpers.rs @@ -1,6 +1,7 @@ use std::collections::{LinkedList, HashMap, HashSet}; use msg::Msg; use error::{Result, ResultExt}; +use std; use std::fs::File; use std::path::Path; use regex::RegexBuilder; @@ -54,8 +55,8 @@ fn calculate_md5_from_representation(v: &str) -> String { hasher.result_str() } -pub fn generate_message_definition( - message_map: &HashMap<(String, String), Msg>, +pub fn generate_message_definition( + message_map: &HashMap<(String, String), Msg, S>, message: &Msg, ) -> Result { let mut handled_messages = HashSet::<(String, String)>::new(); @@ -133,6 +134,7 @@ enum MessageCase { Service(String, Msg, Msg), } +#[allow(unknown_lints, trivial_regex)] fn get_message(folders: &[&str], package: &str, name: &str) -> Result { use std::io::Read; for folder in folders { @@ -146,7 +148,7 @@ fn get_message(folders: &[&str], package: &str, name: &str) -> Result Result>(); let cmake_alt_paths = env::var("CMAKE_PREFIX_PATH") - .unwrap_or(String::new()) - .split(":") + .unwrap_or_default() + .split(':') .filter_map(append_src_folder) .collect::>(); + let local_paths = vec![ + Path::new(file!()) + .parent() + .expect("Unexpected path") + .join("msg_examples") + .to_str() + .map(String::from) + .expect("Unexpected path"), + ]; let extra_paths = env::var("ROSRUST_MSG_PATH") - .unwrap_or(String::new()) - .split(":") + .unwrap_or_default() + .split(':') .map(String::from) .collect::>(); let paths = cmake_paths .iter() .chain(cmake_alt_paths.iter()) + .chain(local_paths.iter()) .chain(extra_paths.iter()) .map(|v| v.as_str()) .collect::>(); diff --git a/rosrust_codegen/src/msg.rs b/rosrust_codegen/src/msg.rs index 5557e74..67c32dd 100644 --- a/rosrust_codegen/src/msg.rs +++ b/rosrust_codegen/src/msg.rs @@ -52,13 +52,13 @@ impl Msg { ) -> ::std::result::Result { let constants = self.fields .iter() - .filter(|ref v| v.is_constant()) - .map(|ref v| v.md5_string(&self.package, hashes)) + .filter(|v| v.is_constant()) + .map(|v| v.md5_string(&self.package, hashes)) .collect::<::std::result::Result, ()>>()?; let fields = self.fields .iter() - .filter(|ref v| !v.is_constant()) - .map(|ref v| v.md5_string(&self.package, hashes)) + .filter(|v| !v.is_constant()) + .map(|v| v.md5_string(&self.package, hashes)) .collect::<::std::result::Result, ()>>()?; let representation = constants .into_iter() @@ -247,7 +247,7 @@ fn match_line(data: &str) -> Option> { } #[inline] -fn strip_useless<'a>(data: &'a str) -> Result<&'a str> { +fn strip_useless(data: &str) -> Result<&str> { Ok( data.splitn(2, '#') .next() @@ -331,9 +331,9 @@ impl FieldInfo { Some(match self.datatype { DataType::Bool => format!("{}: bool = {:?};", self.name, value != "0"), DataType::String => format!("{}: &'static str = {:?};", self.name, value), - DataType::Time => return None, - DataType::Duration => return None, - DataType::LocalStruct(..) => return None, + DataType::Time | + DataType::Duration | + DataType::LocalStruct(..) | DataType::RemoteStruct(..) => return None, _ => { let datatype = self.datatype.rust_type(crate_prefix); @@ -344,7 +344,7 @@ impl FieldInfo { fn new(datatype: &str, name: &str, case: FieldCase) -> Result { Ok(FieldInfo { - datatype: parse_datatype(&datatype).ok_or_else(|| { + datatype: parse_datatype(datatype).ok_or_else(|| { format!("Unsupported datatype: {}", datatype) })?, name: name.to_owned(), @@ -419,7 +419,7 @@ impl DataType { DataType::I32 => "int32", DataType::I64 => "int64", DataType::U8(true) => "uint8", - DataType::U8(false) => "uint8", + DataType::U8(false) => "char", DataType::U16 => "uint16", DataType::U32 => "uint32", DataType::U64 => "uint64", @@ -463,7 +463,7 @@ fn parse_datatype(datatype: &str) -> Option { "Header" => Some(DataType::RemoteStruct("std_msgs".into(), "Header".into())), _ => { let parts = datatype.split('/').collect::>(); - if parts.iter().any(|v| v.len() == 0) { + if parts.iter().any(|v| v.is_empty()) { return None; } match parts.len() { From 5c4857a2917c92f0022ff88aaf7dfce0dbb43a0f Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sun, 24 Dec 2017 03:11:46 +0100 Subject: [PATCH 42/50] Stop using local paths --- .travis.yml | 2 ++ rosrust_codegen/src/lib.rs | 10 ---------- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/.travis.yml b/.travis.yml index 8c91a74..5609cf4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,4 +1,6 @@ language: rust +env: + - ROSRUST_MSG_PATH=/home/travis/build/adnanademovic/rosrust/rosrust_codegen/src/msg_examples rust: - stable - beta diff --git a/rosrust_codegen/src/lib.rs b/rosrust_codegen/src/lib.rs index ef188a3..4b3b6ff 100644 --- a/rosrust_codegen/src/lib.rs +++ b/rosrust_codegen/src/lib.rs @@ -59,15 +59,6 @@ pub fn depend_on_messages(messages: &[&str], crate_prefix: &str) { .split(':') .filter_map(append_src_folder) .collect::>(); - let local_paths = vec![ - Path::new(file!()) - .parent() - .expect("Unexpected path") - .join("msg_examples") - .to_str() - .map(String::from) - .expect("Unexpected path"), - ]; let extra_paths = env::var("ROSRUST_MSG_PATH") .unwrap_or_default() .split(':') @@ -76,7 +67,6 @@ pub fn depend_on_messages(messages: &[&str], crate_prefix: &str) { let paths = cmake_paths .iter() .chain(cmake_alt_paths.iter()) - .chain(local_paths.iter()) .chain(extra_paths.iter()) .map(|v| v.as_str()) .collect::>(); From 006f2effc7d0155c98135d3823808a32cff20c74 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Sun, 24 Dec 2017 12:54:25 +0100 Subject: [PATCH 43/50] Fix linter warnings on rosrust --- examples/pubsub/src/publisher.rs | 2 +- rosrust/src/api/naming/mapper.rs | 2 +- rosrust/src/api/naming/path.rs | 2 +- rosrust/src/api/raii.rs | 6 ++-- rosrust/src/api/resolve.rs | 12 +++---- rosrust/src/api/ros.rs | 40 +++++++++++++++--------- rosrust/src/api/slave.rs | 9 +++--- rosrust/src/api/slavehandler.rs | 34 ++++++++++---------- rosrust/src/rosxmlrpc/client.rs | 2 +- rosrust/src/rosxmlrpc/mod.rs | 4 +-- rosrust/src/rosxmlrpc/server.rs | 13 ++------ rosrust/src/tcpros/client.rs | 9 +++--- rosrust/src/tcpros/header.rs | 2 +- rosrust/src/tcpros/publisher.rs | 10 +++--- rosrust/src/tcpros/service.rs | 23 ++++++-------- rosrust/src/tcpros/subscriber.rs | 6 ++-- rosrust/src/tcpros/util/streamfork.rs | 4 +-- rosrust/src/tcpros/util/tcpconnection.rs | 4 +-- rosrust/src/time.rs | 6 ++-- 19 files changed, 96 insertions(+), 94 deletions(-) diff --git a/examples/pubsub/src/publisher.rs b/examples/pubsub/src/publisher.rs index 5bae281..b5a25d5 100644 --- a/examples/pubsub/src/publisher.rs +++ b/examples/pubsub/src/publisher.rs @@ -20,7 +20,7 @@ fn main() { let mut msg = msg::std_msgs::String::default(); msg.data = format!("hello world {}", count); - chatter_pub.send(msg).unwrap(); + chatter_pub.send(&msg).unwrap(); rate.sleep(); diff --git a/rosrust/src/api/naming/mapper.rs b/rosrust/src/api/naming/mapper.rs index 924ad64..55274a8 100644 --- a/rosrust/src/api/naming/mapper.rs +++ b/rosrust/src/api/naming/mapper.rs @@ -22,7 +22,7 @@ impl Mapper { Some((key, child_keys)) => { self.children .entry(key.clone()) - .or_insert(Mapper::new()) + .or_insert_with(Mapper::new) .add(child_keys, value); } } diff --git a/rosrust/src/api/naming/path.rs b/rosrust/src/api/naming/path.rs index 03dd489..6bb1872 100644 --- a/rosrust/src/api/naming/path.rs +++ b/rosrust/src/api/naming/path.rs @@ -87,7 +87,7 @@ impl Add for Buffer { impl AddAssign for Buffer { fn add_assign(&mut self, rhs: T) { - self.chain.extend_from_slice(&mut rhs.get()); + self.chain.extend_from_slice(rhs.get()); } } diff --git a/rosrust/src/api/raii.rs b/rosrust/src/api/raii.rs index 8c900b0..ffaf7a9 100644 --- a/rosrust/src/api/raii.rs +++ b/rosrust/src/api/raii.rs @@ -25,7 +25,7 @@ impl Publisher { name: name.into(), }; - match info.master.register_publisher(&name, &T::msg_type()) { + match info.master.register_publisher(name, &T::msg_type()) { Ok(_) => Ok(Self { stream, _raii: Arc::new(InteractorRaii::new(info)), @@ -43,7 +43,7 @@ impl Publisher { } #[inline] - pub fn send(&mut self, message: T) -> Result<()> { + pub fn send(&mut self, message: &T) -> Result<()> { self.stream.send(message).map_err(|v| v.into()) } } @@ -78,7 +78,7 @@ impl Subscriber { match master.register_subscriber(name, &T::msg_type()) { Ok(publishers) => { if let Err(err) = slave.add_publishers_to_subscription( - &name, + name, publishers.into_iter(), ) { diff --git a/rosrust/src/api/resolve.rs b/rosrust/src/api/resolve.rs index d8938b6..a36a171 100644 --- a/rosrust/src/api/resolve.rs +++ b/rosrust/src/api/resolve.rs @@ -4,7 +4,7 @@ pub fn master() -> String { if let Some(v) = find_with_prefix("__master:=") { return v; } - env::var("ROS_MASTER_URI").unwrap_or(String::from("http://localhost:11311/")) + env::var("ROS_MASTER_URI").unwrap_or_else(|_| String::from("http://localhost:11311/")) } pub fn hostname() -> String { @@ -27,17 +27,17 @@ pub fn namespace() -> String { if let Some(v) = find_with_prefix("__ns:=") { return v; } - env::var("ROS_NAMESPACE").unwrap_or(String::new()) + env::var("ROS_NAMESPACE").unwrap_or_default() } pub fn name(default: &str) -> String { - find_with_prefix("__name:=").unwrap_or(String::from(default)) + find_with_prefix("__name:=").unwrap_or_else(|| String::from(default)) } pub fn mappings() -> Vec<(String, String)> { args() .skip(1) - .filter(|v| !v.starts_with("_")) + .filter(|v| !v.starts_with('_')) .map(|v| v.split(":=").map(String::from).collect::>()) .filter(|v| v.len() == 2) .map(|v| v.into_iter()) @@ -53,7 +53,7 @@ pub fn mappings() -> Vec<(String, String)> { pub fn params() -> Vec<(String, String)> { args() .skip(1) - .filter(|v| v.starts_with("_")) + .filter(|v| v.starts_with('_')) .filter(|v| !v.starts_with("__")) .map(|v| { v.splitn(2, ":=").map(String::from).collect::>() @@ -89,7 +89,7 @@ fn system_hostname() -> String { let hostname = hostname .into_iter() .take_while(|&v| *v != 0u8) - .map(|v| *v) + .cloned() .collect::>(); String::from_utf8(hostname).expect("Hostname is not legal UTF-8") } diff --git a/rosrust/src/api/ros.rs b/rosrust/src/api/ros.rs index bbb8562..33c97e9 100644 --- a/rosrust/src/api/ros.rs +++ b/rosrust/src/api/ros.rs @@ -53,7 +53,7 @@ impl Ros { .unwrap_or(false) { let clock = Arc::new(SimulatedClock::default()); - let ros_clock = clock.clone(); + let ros_clock = Arc::clone(&clock); let sub = ros.subscribe::("/clock", move |v| clock.trigger(v.clock)) .chain_err(|| "Failed to subscribe to simulated clock")?; ros.static_subs.push(sub); @@ -64,9 +64,9 @@ impl Ros { } fn new_raw(master_uri: &str, hostname: &str, namespace: &str, name: &str) -> Result { - let namespace = namespace.trim_right_matches("/"); + let namespace = namespace.trim_right_matches('/'); - if name.contains("/") { + if name.contains('/') { bail!(ErrorKind::Naming( naming::error::ErrorKind::IllegalCharacter(name.into()), )); @@ -77,8 +77,8 @@ impl Ros { let (shutdown_tx, shutdown_rx) = mpsc::channel(); - let slave = Slave::new(&master_uri, &hostname, 0, &name, shutdown_tx)?; - let master = Master::new(&master_uri, &name, &slave.uri()); + let slave = Slave::new(master_uri, hostname, 0, &name, shutdown_tx)?; + let master = Master::new(master_uri, &name, slave.uri()); Ok(Ros { master: Arc::new(master), @@ -96,22 +96,27 @@ impl Ros { self.resolver.map(source, destination).map_err(|v| v.into()) } + #[inline] pub fn uri(&self) -> &str { - return self.slave.uri(); + self.slave.uri() } + #[inline] pub fn name(&self) -> &str { - return &self.name; + &self.name } + #[inline] pub fn hostname(&self) -> &str { - return &self.hostname; + &self.hostname } + #[inline] pub fn now(&self) -> Time { self.clock.now() } + #[inline] pub fn sleep(&self, d: Duration) { self.clock.sleep(d); } @@ -119,16 +124,18 @@ impl Ros { pub fn rate(&self, rate: f64) -> Rate { let nanos = 1_000_000_000.0 / rate; Rate::new( - self.clock.clone(), + Arc::clone(&self.clock), self.now(), Duration::from_nanos(nanos as i64), ) } + #[inline] pub fn is_ok(&self) -> bool { self.shutdown_rx.try_recv() == Err(mpsc::TryRecvError::Empty) } + #[inline] pub fn spin(&self) { self.shutdown_rx.recv().is_ok(); } @@ -196,8 +203,8 @@ impl Ros { { let name = self.resolver.translate(service)?; Service::new::( - self.master.clone(), - self.slave.clone(), + Arc::clone(&self.master), + Arc::clone(&self.slave), &self.hostname, &name, handler, @@ -210,7 +217,12 @@ impl Ros { F: Fn(T) -> () + Send + 'static, { let name = self.resolver.translate(topic)?; - Subscriber::new::(self.master.clone(), self.slave.clone(), &name, callback) + Subscriber::new::( + Arc::clone(&self.master), + Arc::clone(&self.slave), + &name, + callback, + ) } pub fn publish(&mut self, topic: &str) -> Result> @@ -219,8 +231,8 @@ impl Ros { { let name = self.resolver.translate(topic)?; Publisher::new( - self.master.clone(), - self.slave.clone(), + Arc::clone(&self.master), + Arc::clone(&self.slave), &self.hostname, &name, ) diff --git a/rosrust/src/api/slave.rs b/rosrust/src/api/slave.rs index c1185fb..d84b653 100644 --- a/rosrust/src/api/slave.rs +++ b/rosrust/src/api/slave.rs @@ -30,9 +30,9 @@ impl Slave { let (shutdown_tx, shutdown_rx) = futures_channel(1); let handler = SlaveHandler::new(master_uri, hostname, name, shutdown_tx); - let pubs = handler.publications.clone(); - let subs = handler.subscriptions.clone(); - let services = handler.services.clone(); + let pubs = Arc::clone(&handler.publications); + let subs = Arc::clone(&handler.subscriptions); + let services = Arc::clone(&handler.services); let (port_tx, port_rx) = channel(); let socket_addr = match (hostname, port).to_socket_addrs()?.next() { Some(socket_addr) => socket_addr, @@ -70,8 +70,9 @@ impl Slave { }) } + #[inline] pub fn uri(&self) -> &str { - return &self.uri; + &self.uri } pub fn add_publishers_to_subscription(&self, topic: &str, publishers: T) -> SerdeResult<()> diff --git a/rosrust/src/api/slavehandler.rs b/rosrust/src/api/slavehandler.rs index 8a1c0ca..6a98ffd 100644 --- a/rosrust/src/api/slavehandler.rs +++ b/rosrust/src/api/slavehandler.rs @@ -52,9 +52,9 @@ impl SlaveHandler { server.register_value("shutdown", "Shutdown", move |args| { let mut args = unwrap_array_case(args).into_iter(); - let _caller_id = args.next().ok_or(ResponseError::Client( - "Missing argument 'caller_id'".into(), - ))?; + let _caller_id = args.next().ok_or_else(|| { + ResponseError::Client("Missing argument 'caller_id'".into()) + })?; let message = match args.next() { Some(Value::String(message)) => message, _ => return Err(ResponseError::Client("Missing argument 'message'".into())), @@ -72,14 +72,14 @@ impl SlaveHandler { server.register_value("getPid", "PID", |_args| Ok(Value::Int(getpid().into()))); let subscriptions = Arc::new(Mutex::new(HashMap::::new())); - let subs = subscriptions.clone(); + let subs = Arc::clone(&subscriptions); server.register_value("getSubscriptions", "List of subscriptions", move |_args| { Ok(Value::Array( subs.lock() .expect(FAILED_TO_LOCK) .values() - .map(|ref v| { + .map(|v| { Value::Array(vec![ Value::String(v.topic.clone()), Value::String(v.msg_type.clone()), @@ -90,14 +90,14 @@ impl SlaveHandler { }); let publications = Arc::new(Mutex::new(HashMap::::new())); - let pubs = publications.clone(); + let pubs = Arc::clone(&publications); server.register_value("getPublications", "List of publications", move |_args| { Ok(Value::Array( pubs.lock() .expect(FAILED_TO_LOCK) .values() - .map(|ref v| { + .map(|v| { Value::Array(vec![ Value::String(v.topic.clone()), Value::String(v.msg_type.clone()), @@ -113,13 +113,13 @@ impl SlaveHandler { }); let name_string = String::from(name); - let subs = subscriptions.clone(); + let subs = Arc::clone(&subscriptions); server.register_value("publisherUpdate", "Publishers updated", move |args| { let mut args = unwrap_array_case(args).into_iter(); - let _caller_id = args.next().ok_or(ResponseError::Client( - "Missing argument 'caller_id'".into(), - ))?; + let _caller_id = args.next().ok_or_else(|| { + ResponseError::Client("Missing argument 'caller_id'".into()) + })?; let topic = match args.next() { Some(Value::String(topic)) => topic, _ => return Err(ResponseError::Client("Missing argument 'topic'".into())), @@ -154,13 +154,13 @@ impl SlaveHandler { }); let hostname_string = String::from(hostname); - let pubs = publications.clone(); + let pubs = Arc::clone(&publications); server.register_value("requestTopic", "Chosen protocol", move |args| { let mut args = unwrap_array_case(args).into_iter(); - let _caller_id = args.next().ok_or(ResponseError::Client( - "Missing argument 'caller_id'".into(), - ))?; + let _caller_id = args.next().ok_or_else(|| { + ResponseError::Client("Missing argument 'caller_id'".into()) + })?; let topic = match args.next() { Some(Value::String(topic)) => topic, _ => return Err(ResponseError::Client("Missing argument 'topic'".into())), @@ -176,7 +176,7 @@ impl SlaveHandler { None => return Err(ResponseError::Client("Missing argument 'protocols'".into())), }; let (ip, port) = match pubs.lock().expect(FAILED_TO_LOCK).get(&topic) { - Some(publisher) => (hostname_string.clone(), publisher.port as i32), + Some(publisher) => (hostname_string.clone(), i32::from(publisher.port)), None => { return Err(ResponseError::Client( "Requested topic not published by node".into(), @@ -228,7 +228,7 @@ where { if let Some(mut subscription) = subscriptions.get_mut(topic) { for publisher in publishers { - if let Err(err) = connect_to_publisher(&mut subscription, &name, &publisher, &topic) { + if let Err(err) = connect_to_publisher(&mut subscription, name, &publisher, topic) { let info = err.iter() .map(|v| format!("{}", v)) .collect::>() diff --git a/rosrust/src/rosxmlrpc/client.rs b/rosrust/src/rosxmlrpc/client.rs index ad77f87..200af1a 100644 --- a/rosrust/src/rosxmlrpc/client.rs +++ b/rosrust/src/rosxmlrpc/client.rs @@ -45,7 +45,7 @@ impl Client { FAILURE_CODE => Err(ResponseError::Server(message)), SUCCESS_CODE => Ok(data), _ => Err(ResponseError::Server( - format!("Bad response code returned from server"), + "Bad response code returned from server".into(), )), } } diff --git a/rosrust/src/rosxmlrpc/mod.rs b/rosrust/src/rosxmlrpc/mod.rs index cce8644..4d60c65 100644 --- a/rosrust/src/rosxmlrpc/mod.rs +++ b/rosrust/src/rosxmlrpc/mod.rs @@ -26,8 +26,8 @@ impl std::fmt::Display for ResponseError { impl std::error::Error for ResponseError { fn description(&self) -> &str { match *self { - ResponseError::Client(ref v) => &v, - ResponseError::Server(ref v) => &v, + ResponseError::Client(ref v) | + ResponseError::Server(ref v) => v, } } diff --git a/rosrust/src/rosxmlrpc/server.rs b/rosrust/src/rosxmlrpc/server.rs index 052395a..8526c3c 100644 --- a/rosrust/src/rosxmlrpc/server.rs +++ b/rosrust/src/rosxmlrpc/server.rs @@ -36,18 +36,10 @@ fn response_to_params(msg: &str, response: Response) -> xml_rpc: match response { Ok(v) => vec![Value::Int(SUCCESS_CODE), Value::String(msg.into()), v], Err(ResponseError::Client(err)) => { - vec![ - Value::Int(ERROR_CODE), - Value::String(err.into()), - Value::Int(0), - ] + vec![Value::Int(ERROR_CODE), Value::String(err), Value::Int(0)] } Err(ResponseError::Server(err)) => { - vec![ - Value::Int(FAILURE_CODE), - Value::String(err.into()), - Value::Int(0), - ] + vec![Value::Int(FAILURE_CODE), Value::String(err), Value::Int(0)] } } } @@ -64,6 +56,7 @@ where ] } +#[allow(unknown_lints, needless_pass_by_value)] fn on_missing(_params: xml_rpc::Params) -> xml_rpc::Response { Ok(error_response("Bad method requested")) } diff --git a/rosrust/src/tcpros/client.rs b/rosrust/src/tcpros/client.rs index 6981778..b08fc3f 100644 --- a/rosrust/src/tcpros/client.rs +++ b/rosrust/src/tcpros/client.rs @@ -1,6 +1,7 @@ use byteorder::ReadBytesExt; use std::net::TcpStream; use std::thread; +use std::sync::Arc; use std::collections::HashMap; use std; use serde_rosmsg::{from_reader, to_writer}; @@ -14,9 +15,9 @@ pub struct ClientResponse { impl ClientResponse { pub fn read(self) -> Result> { - self.handle.join().unwrap_or(Err( - ErrorKind::ServiceResponseUnknown.into(), - )) + self.handle.join().unwrap_or_else(|_| { + Err(ErrorKind::ServiceResponseUnknown.into()) + }) } } @@ -62,7 +63,7 @@ impl Client { } pub fn req_async(&self, args: T::Request) -> ClientResponse { - let info = self.info.clone(); + let info = Arc::clone(&self.info); ClientResponse { handle: thread::spawn(move || { Self::request_body(&args, &info.uri, &info.caller_id, &info.service) diff --git a/rosrust/src/tcpros/header.rs b/rosrust/src/tcpros/header.rs index 1955383..dc997aa 100644 --- a/rosrust/src/tcpros/header.rs +++ b/rosrust/src/tcpros/header.rs @@ -23,7 +23,7 @@ pub fn match_field( Some(actual) => actual, None => bail!(ErrorKind::HeaderMissingField(field.into())), }; - if actual != &expected { + if actual != expected { bail!(ErrorKind::HeaderMismatch( field.into(), expected.into(), diff --git a/rosrust/src/tcpros/publisher.rs b/rosrust/src/tcpros/publisher.rs index 3473ed6..c406694 100644 --- a/rosrust/src/tcpros/publisher.rs +++ b/rosrust/src/tcpros/publisher.rs @@ -44,11 +44,11 @@ where T: Message, U: std::io::Write + std::io::Read, { - read_request::(&mut stream, &topic)?; + read_request::(&mut stream, topic)?; write_response::(&mut stream) } -fn listen_for_subscribers(topic: &str, listener: V, targets: TargetList) +fn listen_for_subscribers(topic: &str, listener: V, targets: &TargetList) where T: Message, U: std::io::Read + std::io::Write + Send, @@ -107,7 +107,7 @@ impl Publisher { let (targets, data) = fork(); let topic_name = String::from(topic); thread::spawn(move || { - listen_for_subscribers::(&topic_name, listener, targets) + listen_for_subscribers::(&topic_name, listener, &targets) }); Publisher { subscriptions: data, @@ -148,8 +148,8 @@ impl PublisherStream { }) } - pub fn send(&mut self, message: T) -> Result<()> { - let bytes = to_vec(&message)?; + pub fn send(&mut self, message: &T) -> Result<()> { + let bytes = to_vec(message)?; // Subscriptions can only be closed from the Publisher side // There is no way for the streamfork thread to fail by itself self.stream.send(bytes).expect("Connected thread died"); diff --git a/rosrust/src/tcpros/service.rs b/rosrust/src/tcpros/service.rs index 5d08af7..ab48954 100644 --- a/rosrust/src/tcpros/service.rs +++ b/rosrust/src/tcpros/service.rs @@ -60,7 +60,7 @@ impl Service { let service_name = String::from(service); let node_name = String::from(node_name); thread::spawn(move || { - listen_for_clients::(service_name, node_name, handler, listener) + listen_for_clients::(&service_name, &node_name, handler, listener) }); Service { api: String::from(api), @@ -71,7 +71,7 @@ impl Service { } } -fn listen_for_clients(service: String, node_name: String, handler: F, connections: V) +fn listen_for_clients(service: &str, node_name: &str, handler: F, connections: V) where T: ServicePair, U: std::io::Read + std::io::Write + Send + 'static, @@ -81,7 +81,7 @@ where let handler = Arc::new(handler); for mut stream in connections { // Service request starts by exchanging connection headers - if let Err(err) = exchange_headers::(&mut stream, &service, &node_name) { + if let Err(err) = exchange_headers::(&mut stream, service, node_name) { error!( "Failed to exchange headers for service '{}': {}", service, @@ -91,7 +91,7 @@ where } // Spawn a thread for handling requests - spawn_request_handler::(stream, handler.clone()); + spawn_request_handler::(stream, Arc::clone(&handler)); } } @@ -137,7 +137,7 @@ where { thread::spawn(move || if let Err(err) = handle_request_loop::( stream, - handler, + &handler, ) { let info = err.iter() @@ -148,20 +148,15 @@ where }); } -fn handle_request_loop(mut stream: U, handler: Arc) -> Result<()> +fn handle_request_loop(mut stream: U, handler: &Arc) -> Result<()> where T: ServicePair, U: std::io::Read + std::io::Write, F: Fn(T::Request) -> ServiceResult, { - loop { - // Receive request from client - // Break out of loop in case of failure to read request - let req = match from_reader(&mut stream) { - Ok(req) => req, - Err(_) => break, - }; - + // Receive request from client + // Break out of loop in case of failure to read request + while let Ok(req) = from_reader(&mut stream) { // Call function that handles request and returns response match handler(req) { Ok(res) => { diff --git a/rosrust/src/tcpros/subscriber.rs b/rosrust/src/tcpros/subscriber.rs index 445e5a2..80d0903 100644 --- a/rosrust/src/tcpros/subscriber.rs +++ b/rosrust/src/tcpros/subscriber.rs @@ -28,7 +28,7 @@ impl Subscriber { let topic_name = String::from(topic); let data_stream = data_tx.clone(); thread::spawn(move || { - join_connections::(data_tx, pub_rx, &caller_id, &topic_name) + join_connections::(&data_tx, pub_rx, &caller_id, &topic_name) }); thread::spawn(move || handle_data::(data_rx, callback)); Subscriber { @@ -82,7 +82,7 @@ where } fn join_connections( - data_stream: Sender>>, + data_stream: &Sender>>, publishers: Receiver, caller_id: &str, topic: &str, @@ -91,7 +91,7 @@ fn join_connections( { // Ends when publisher sender is destroyed, which happens at Subscriber destruction for publisher in publishers { - let result = join_connection::(&data_stream, &publisher, caller_id, topic) + let result = join_connection::(data_stream, &publisher, caller_id, topic) .chain_err(|| ErrorKind::TopicConnectionFail(topic.into())); if let Err(err) = result { let info = err.iter() diff --git a/rosrust/src/tcpros/util/streamfork.rs b/rosrust/src/tcpros/util/streamfork.rs index f374a78..f2ee108 100644 --- a/rosrust/src/tcpros/util/streamfork.rs +++ b/rosrust/src/tcpros/util/streamfork.rs @@ -5,11 +5,11 @@ use std::thread; pub fn fork() -> (TargetList, DataStream) { let (streams_sender, streams) = channel(); let (data_sender, data) = channel(); - thread::spawn(move || fork_thread::(streams, data)); + thread::spawn(move || fork_thread::(&streams, data)); (TargetList(streams_sender), DataStream(data_sender)) } -fn fork_thread(streams: Receiver, data: Receiver>) { +fn fork_thread(streams: &Receiver, data: Receiver>) { let mut targets = Vec::new(); for buffer in data { while let Ok(target) = streams.try_recv() { diff --git a/rosrust/src/tcpros/util/tcpconnection.rs b/rosrust/src/tcpros/util/tcpconnection.rs index 6b2fa06..7d09294 100644 --- a/rosrust/src/tcpros/util/tcpconnection.rs +++ b/rosrust/src/tcpros/util/tcpconnection.rs @@ -5,11 +5,11 @@ use std::sync::mpsc::{Sender, Receiver, channel}; pub fn iterate(listener: TcpListener, tag: String) -> (Raii, TcpConnectionIterator) { let (tx, rx) = channel(); let killer = Raii { killer: tx.clone() }; - thread::spawn(move || listener_thread(listener, tag, tx)); + thread::spawn(move || listener_thread(&listener, &tag, &tx)); (killer, TcpConnectionIterator { listener: rx }) } -fn listener_thread(connections: TcpListener, tag: String, out: Sender>) { +fn listener_thread(connections: &TcpListener, tag: &str, out: &Sender>) { for stream in connections.incoming() { match stream { Ok(stream) => { diff --git a/rosrust/src/time.rs b/rosrust/src/time.rs index cf213a3..f090b4f 100644 --- a/rosrust/src/time.rs +++ b/rosrust/src/time.rs @@ -1,7 +1,7 @@ use std::cmp; use std::ops; -const BILLION: i64 = 1000000000; +const BILLION: i64 = 1_000_000_000; #[derive(Clone, Default, Serialize, Deserialize, Debug)] pub struct Time { @@ -25,7 +25,7 @@ impl Time { #[inline] fn nanos(&self) -> i64 { - self.sec as i64 * BILLION + self.nsec as i64 + i64::from(self.sec) * BILLION + i64::from(self.nsec) } } @@ -71,7 +71,7 @@ impl Duration { #[inline] fn nanos(&self) -> i64 { - self.sec as i64 * BILLION + self.nsec as i64 + i64::from(self.sec) * BILLION + i64::from(self.nsec) } } From df0caa12277da931ac4693b1b4388d8fd8393c7e Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Mon, 25 Dec 2017 11:18:08 +0100 Subject: [PATCH 44/50] Update styling --- examples/pubsub/src/subscriber.rs | 6 +- rosrust/src/api/clock.rs | 27 ++- rosrust/src/api/error.rs | 2 +- rosrust/src/api/master.rs | 6 +- rosrust/src/api/naming/mapper.rs | 2 +- rosrust/src/api/naming/mod.rs | 1 - rosrust/src/api/naming/path.rs | 7 +- rosrust/src/api/raii.rs | 21 +- rosrust/src/api/resolve.rs | 25 +-- rosrust/src/api/ros.rs | 31 ++- rosrust/src/api/slave.rs | 49 +++-- rosrust/src/api/slavehandler.rs | 37 ++-- rosrust/src/lib.rs | 2 +- rosrust/src/rosxmlrpc/client.rs | 37 ++-- rosrust/src/rosxmlrpc/mod.rs | 3 +- rosrust/src/rosxmlrpc/server.rs | 7 +- rosrust/src/tcpros/client.rs | 18 +- rosrust/src/tcpros/header.rs | 252 ++--------------------- rosrust/src/tcpros/publisher.rs | 11 +- rosrust/src/tcpros/service.rs | 21 +- rosrust/src/tcpros/subscriber.rs | 14 +- rosrust/src/tcpros/util/streamfork.rs | 2 +- rosrust/src/tcpros/util/tcpconnection.rs | 2 +- rosrust_codegen/src/genmsg.rs | 62 +++--- rosrust_codegen/src/helpers.rs | 97 +++------ rosrust_codegen/src/lib.rs | 8 +- rosrust_codegen/src/msg.rs | 145 ++++++------- 27 files changed, 299 insertions(+), 596 deletions(-) diff --git a/examples/pubsub/src/subscriber.rs b/examples/pubsub/src/subscriber.rs index 1ef1861..407ed10 100644 --- a/examples/pubsub/src/subscriber.rs +++ b/examples/pubsub/src/subscriber.rs @@ -13,9 +13,9 @@ fn main() { let mut ros = Ros::new("listener").unwrap(); // The subscriber is stopped when the returned object is destroyed - let _subscriber_raii = - ros.subscribe("chatter", |v: msg::std_msgs::String| println!("{}", v.data)) - .unwrap(); + let _subscriber_raii = ros.subscribe("chatter", |v: msg::std_msgs::String| { + println!("{}", v.data) + }).unwrap(); ros.spin(); } diff --git a/rosrust/src/api/clock.rs b/rosrust/src/api/clock.rs index c89637c..8184eda 100644 --- a/rosrust/src/api/clock.rs +++ b/rosrust/src/api/clock.rs @@ -41,9 +41,9 @@ pub struct RealClock {} impl Clock for RealClock { #[inline] fn now(&self) -> Time { - let time = SystemTime::now().duration_since(UNIX_EPOCH).expect( - BEFORE_EPOCH, - ); + let time = SystemTime::now() + .duration_since(UNIX_EPOCH) + .expect(BEFORE_EPOCH); Time { sec: time.as_secs() as i32, nsec: time.subsec_nanos() as i32, @@ -77,9 +77,9 @@ impl cmp::PartialEq for Timeout { impl cmp::PartialOrd for Timeout { fn partial_cmp(&self, other: &Self) -> Option { - self.timestamp.partial_cmp(&other.timestamp).map( - cmp::Ordering::reverse, - ) + self.timestamp + .partial_cmp(&other.timestamp) + .map(cmp::Ordering::reverse) } } @@ -130,9 +130,7 @@ impl Clock for SimulatedClock { if d.sec < 0 || d.nsec < 0 { return; } - let current = { - self.data.lock().expect(FAILED_TO_LOCK).current.clone() - }; + let current = { self.data.lock().expect(FAILED_TO_LOCK).current.clone() }; self.wait_until(current + d); } @@ -140,12 +138,11 @@ impl Clock for SimulatedClock { fn wait_until(&self, timestamp: Time) { let (tx, rx) = channel(); { - self.data.lock().expect(FAILED_TO_LOCK).timeouts.push( - Timeout { - timestamp, - tx, - }, - ); + self.data + .lock() + .expect(FAILED_TO_LOCK) + .timeouts + .push(Timeout { timestamp, tx }); } if rx.recv().is_err() { warn!("Sleep beyond simulated clock"); diff --git a/rosrust/src/api/error.rs b/rosrust/src/api/error.rs index 7b63519..ee55b1f 100644 --- a/rosrust/src/api/error.rs +++ b/rosrust/src/api/error.rs @@ -20,7 +20,7 @@ error_chain! { description("Could not add duplicate") display("Could not add duplicate {}", t) } - TimeoutError + TimeoutError } } diff --git a/rosrust/src/api/master.rs b/rosrust/src/api/master.rs index 929192b..b9ed5b8 100644 --- a/rosrust/src/api/master.rs +++ b/rosrust/src/api/master.rs @@ -161,7 +161,11 @@ pub struct SystemState { #[derive(Debug, Deserialize)] pub struct TopicDataTuple(String, Vec); #[derive(Debug, Deserialize)] -pub struct SystemStateTuple(Vec, Vec, Vec); +pub struct SystemStateTuple( + Vec, + Vec, + Vec, +); impl Into for SystemStateTuple { fn into(self) -> SystemState { diff --git a/rosrust/src/api/naming/mapper.rs b/rosrust/src/api/naming/mapper.rs index 55274a8..cd23d0b 100644 --- a/rosrust/src/api/naming/mapper.rs +++ b/rosrust/src/api/naming/mapper.rs @@ -1,5 +1,5 @@ use std::collections::HashMap; -use super::path::{Buffer, Slice, Path}; +use super::path::{Buffer, Path, Slice}; pub struct Mapper { children: HashMap, diff --git a/rosrust/src/api/naming/mod.rs b/rosrust/src/api/naming/mod.rs index c5613a9..8acb3cc 100644 --- a/rosrust/src/api/naming/mod.rs +++ b/rosrust/src/api/naming/mod.rs @@ -71,7 +71,6 @@ mod tests { assert!(Resolver::new("/a//b").is_err()); } - #[test] fn rejects_illegal_names() { let r = Resolver::new("/some/long/path").expect(FAILED_TO_RESOLVE); diff --git a/rosrust/src/api/naming/path.rs b/rosrust/src/api/naming/path.rs index 6bb1872..5e99e9c 100644 --- a/rosrust/src/api/naming/path.rs +++ b/rosrust/src/api/naming/path.rs @@ -40,7 +40,9 @@ impl<'a> Path for Slice<'a> { } fn take(self) -> Buffer { - Buffer { chain: Vec::from(self.chain) } + Buffer { + chain: Vec::from(self.chain), + } } } @@ -230,8 +232,7 @@ mod tests { ) ); assert!( - "/" - .parse::() + "/".parse::() .expect(FAILED_TO_HANDLE) .parent() .is_err() diff --git a/rosrust/src/api/raii.rs b/rosrust/src/api/raii.rs index ffaf7a9..1aa0e38 100644 --- a/rosrust/src/api/raii.rs +++ b/rosrust/src/api/raii.rs @@ -33,8 +33,7 @@ impl Publisher { Err(error) => { error!( "Failed to register publisher for topic '{}': {}", - name, - error + name, error ); info.unregister()?; Err(error.into()) @@ -77,15 +76,11 @@ impl Subscriber { match master.register_subscriber(name, &T::msg_type()) { Ok(publishers) => { - if let Err(err) = slave.add_publishers_to_subscription( - name, - publishers.into_iter(), - ) + if let Err(err) = slave.add_publishers_to_subscription(name, publishers.into_iter()) { error!( "Failed to subscribe to all publishers of topic '{}': {}", - name, - err + name, err ); } Ok(Self { @@ -151,7 +146,9 @@ impl Service { info.unregister()?; Err(err.into()) } else { - Ok(Self { _raii: Arc::new(InteractorRaii::new(info)) }) + Ok(Self { + _raii: Arc::new(InteractorRaii::new(info)), + }) } } } @@ -166,9 +163,9 @@ struct ServiceInfo { impl Interactor for ServiceInfo { fn unregister(&mut self) -> Response<()> { self.slave.remove_service(&self.name); - self.master.unregister_service(&self.name, &self.api).map( - |_| (), - ) + self.master + .unregister_service(&self.name, &self.api) + .map(|_| ()) } } diff --git a/rosrust/src/api/resolve.rs b/rosrust/src/api/resolve.rs index a36a171..9a5a8ba 100644 --- a/rosrust/src/api/resolve.rs +++ b/rosrust/src/api/resolve.rs @@ -55,18 +55,14 @@ pub fn params() -> Vec<(String, String)> { .skip(1) .filter(|v| v.starts_with('_')) .filter(|v| !v.starts_with("__")) - .map(|v| { - v.splitn(2, ":=").map(String::from).collect::>() - }) + .map(|v| v.splitn(2, ":=").map(String::from).collect::>()) .filter(|v| v.len() == 2) .map(|v| v.into_iter()) .map(|mut v| { ( - v.next().expect(UNEXPECTED_EMPTY_ARRAY).replacen( - '_', - "~", - 1, - ), + v.next() + .expect(UNEXPECTED_EMPTY_ARRAY) + .replacen('_', "~", 1), v.next().expect(UNEXPECTED_EMPTY_ARRAY), ) }) @@ -74,18 +70,18 @@ pub fn params() -> Vec<(String, String)> { } fn find_with_prefix(prefix: &str) -> Option { - args().skip(1).find(|v| v.starts_with(prefix)).map(|v| { - String::from(v.trim_left_matches(prefix)) - }) + args() + .skip(1) + .find(|v| v.starts_with(prefix)) + .map(|v| String::from(v.trim_left_matches(prefix))) } #[cfg(not(test))] fn system_hostname() -> String { use nix::unistd::gethostname; let mut hostname = [0u8; 256]; - gethostname(&mut hostname).expect( - "Hostname is either unavailable or too long to fit into buffer", - ); + gethostname(&mut hostname) + .expect("Hostname is either unavailable or too long to fit into buffer"); let hostname = hostname .into_iter() .take_while(|&v| *v != 0u8) @@ -319,7 +315,6 @@ mod tests { assert_eq!(String::from("http://somebody2:21212/"), master()); } - #[test] #[allow(unused_variables)] fn hostname_uses_default_uri_by_default() { diff --git a/rosrust/src/api/ros.rs b/rosrust/src/api/ros.rs index 33c97e9..8e78fbd 100644 --- a/rosrust/src/api/ros.rs +++ b/rosrust/src/api/ros.rs @@ -1,7 +1,7 @@ use msg::rosgraph_msgs::Clock as ClockMsg; use time::{Duration, Time}; -use serde::{Serialize, Deserialize}; -use std::sync::{Arc, mpsc}; +use serde::{Deserialize, Serialize}; +use std::sync::{mpsc, Arc}; use std::time; use super::clock::{Clock, Rate, RealClock, SimulatedClock}; use super::master::{self, Master, Topic}; @@ -9,7 +9,7 @@ use super::slave::Slave; use super::error::{ErrorKind, Result, ResultExt}; use super::super::rosxmlrpc::Response; use super::naming::{self, Resolver}; -use super::raii::{Publisher, Subscriber, Service}; +use super::raii::{Publisher, Service, Subscriber}; use super::resolve; use tcpros::{Client, Message, ServicePair, ServiceResult}; use xml_rpc; @@ -42,9 +42,8 @@ impl Ros { .into_iter() .next() .ok_or_else(|| format!("Failed to load YAML: {}", dest))?; - let param = ros.param(&src).ok_or_else( - || format!("Failed to resolve name: {}", src), - )?; + let param = ros.param(&src) + .ok_or_else(|| format!("Failed to resolve name: {}", src))?; param.set_raw(yaml_to_xmlrpc(data)?)?; } @@ -141,11 +140,9 @@ impl Ros { } pub fn param<'a, 'b>(&'a self, name: &'b str) -> Option> { - self.resolver.translate(name).ok().map(|v| { - Parameter { - master: &self.master, - name: v, - } + self.resolver.translate(name).ok().map(|v| Parameter { + master: &self.master, + name: v, }) } @@ -158,9 +155,9 @@ impl Ros { } pub fn topics(&self) -> Response> { - self.master.get_topic_types().map(|v| { - v.into_iter().map(Into::into).collect() - }) + self.master + .get_topic_types() + .map(|v| v.into_iter().map(Into::into).collect()) } pub fn client(&self, service: &str) -> Result> { @@ -284,9 +281,9 @@ fn yaml_to_xmlrpc(val: Yaml) -> Result { Yaml::Integer(v) => xml_rpc::Value::Int(v as i32), Yaml::String(v) => xml_rpc::Value::String(v), Yaml::Boolean(v) => xml_rpc::Value::Bool(v), - Yaml::Array(v) => xml_rpc::Value::Array( - v.into_iter().map(yaml_to_xmlrpc).collect::>()?, - ), + Yaml::Array(v) => { + xml_rpc::Value::Array(v.into_iter().map(yaml_to_xmlrpc).collect::>()?) + } Yaml::Hash(v) => xml_rpc::Value::Struct(v.into_iter() .map(|(k, v)| Ok((yaml_to_string(k)?, yaml_to_xmlrpc(v)?))) .collect::>()?), diff --git a/rosrust/src/api/slave.rs b/rosrust/src/api/slave.rs index d84b653..d2a7311 100644 --- a/rosrust/src/api/slave.rs +++ b/rosrust/src/api/slave.rs @@ -5,7 +5,7 @@ use std::sync::mpsc::{channel, Sender}; use std::thread; use super::error::{self, ErrorKind, Result, ResultExt}; use super::slavehandler::{add_publishers_to_subscription, SlaveHandler}; -use tcpros::{Message, Publisher, PublisherStream, Subscriber, Service, ServicePair, ServiceResult}; +use tcpros::{Message, Publisher, PublisherStream, Service, ServicePair, ServiceResult, Subscriber}; pub struct Slave { name: String, @@ -56,9 +56,10 @@ impl Slave { outer_shutdown_tx.send(()).is_ok(); }); - let port = port_rx.recv().expect(FAILED_TO_LOCK).chain_err( - || "Failed to get port", - )?; + let port = port_rx + .recv() + .expect(FAILED_TO_LOCK) + .chain_err(|| "Failed to get port")?; let uri = format!("http://{}:{}/", hostname, port); Ok(Slave { @@ -98,11 +99,11 @@ impl Slave { F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static, { use std::collections::hash_map::Entry; - match self.services.lock().expect(FAILED_TO_LOCK).entry( - String::from( - service, - ), - ) { + match self.services + .lock() + .expect(FAILED_TO_LOCK) + .entry(String::from(service)) + { Entry::Occupied(..) => { error!("Duplicate initiation of service '{}' attempted", service); Err(ErrorKind::Duplicate("service".into()).into()) @@ -129,9 +130,11 @@ impl Slave { T: Message, { use std::collections::hash_map::Entry; - match self.publications.lock().expect(FAILED_TO_LOCK).entry( - String::from(topic), - ) { + match self.publications + .lock() + .expect(FAILED_TO_LOCK) + .entry(String::from(topic)) + { Entry::Occupied(publisher_entry) => publisher_entry.get().stream(), Entry::Vacant(entry) => { let publisher = Publisher::new::(format!("{}:0", hostname).as_str(), topic)?; @@ -141,9 +144,10 @@ impl Slave { } pub fn remove_publication(&self, topic: &str) { - self.publications.lock().expect(FAILED_TO_LOCK).remove( - topic, - ); + self.publications + .lock() + .expect(FAILED_TO_LOCK) + .remove(topic); } pub fn add_subscription(&self, topic: &str, callback: F) -> Result<()> @@ -152,9 +156,11 @@ impl Slave { F: Fn(T) -> () + Send + 'static, { use std::collections::hash_map::Entry; - match self.subscriptions.lock().expect(FAILED_TO_LOCK).entry( - String::from(topic), - ) { + match self.subscriptions + .lock() + .expect(FAILED_TO_LOCK) + .entry(String::from(topic)) + { Entry::Occupied(..) => { error!("Duplicate subscription to topic '{}' attempted", topic); Err(ErrorKind::Duplicate("subscription".into()).into()) @@ -168,9 +174,10 @@ impl Slave { } pub fn remove_subscription(&self, topic: &str) { - self.subscriptions.lock().expect(FAILED_TO_LOCK).remove( - topic, - ); + self.subscriptions + .lock() + .expect(FAILED_TO_LOCK) + .remove(topic); } } diff --git a/rosrust/src/api/slavehandler.rs b/rosrust/src/api/slavehandler.rs index 6a98ffd..ffc6f4c 100644 --- a/rosrust/src/api/slavehandler.rs +++ b/rosrust/src/api/slavehandler.rs @@ -5,8 +5,8 @@ use std::collections::HashMap; use std::sync::{Arc, Mutex}; use futures::sync::mpsc::Sender; use super::error::{self, ErrorKind, Result}; -use tcpros::{Publisher, Subscriber, Service}; -use xml_rpc::{self, Value, Params}; +use tcpros::{Publisher, Service, Subscriber}; +use xml_rpc::{self, Params, Value}; pub struct SlaveHandler { pub subscriptions: Arc>>, @@ -33,7 +33,6 @@ impl SlaveHandler { let mut server = Server::default(); - server.register_value("getBusStats", "Bus stats", |_args| { // TODO: implement actual stats displaying Err(ResponseError::Server("Method not implemented".into())) @@ -52,9 +51,8 @@ impl SlaveHandler { server.register_value("shutdown", "Shutdown", move |args| { let mut args = unwrap_array_case(args).into_iter(); - let _caller_id = args.next().ok_or_else(|| { - ResponseError::Client("Missing argument 'caller_id'".into()) - })?; + let _caller_id = args.next() + .ok_or_else(|| ResponseError::Client("Missing argument 'caller_id'".into()))?; let message = match args.next() { Some(Value::String(message)) => message, _ => return Err(ResponseError::Client("Missing argument 'message'".into())), @@ -117,9 +115,8 @@ impl SlaveHandler { server.register_value("publisherUpdate", "Publishers updated", move |args| { let mut args = unwrap_array_case(args).into_iter(); - let _caller_id = args.next().ok_or_else(|| { - ResponseError::Client("Missing argument 'caller_id'".into()) - })?; + let _caller_id = args.next() + .ok_or_else(|| ResponseError::Client("Missing argument 'caller_id'".into()))?; let topic = match args.next() { Some(Value::String(topic)) => topic, _ => return Err(ResponseError::Client("Missing argument 'topic'".into())), @@ -147,9 +144,7 @@ impl SlaveHandler { &name_string, &topic, publishers.into_iter(), - ).map_err(|v| { - ResponseError::Server(format!("Failed to handle publishers: {}", v)) - })?; + ).map_err(|v| ResponseError::Server(format!("Failed to handle publishers: {}", v)))?; Ok(Value::Int(0)) }); @@ -158,9 +153,8 @@ impl SlaveHandler { server.register_value("requestTopic", "Chosen protocol", move |args| { let mut args = unwrap_array_case(args).into_iter(); - let _caller_id = args.next().ok_or_else(|| { - ResponseError::Client("Missing argument 'caller_id'".into()) - })?; + let _caller_id = args.next() + .ok_or_else(|| ResponseError::Client("Missing argument 'caller_id'".into()))?; let topic = match args.next() { Some(Value::String(topic)) => topic, _ => return Err(ResponseError::Client("Missing argument 'topic'".into())), @@ -169,8 +163,7 @@ impl SlaveHandler { Some(Value::Array(protocols)) => protocols, Some(_) => { return Err(ResponseError::Client( - "Protocols need to be provided as [ [String, XmlRpcLegalValue] ]" - .into(), + "Protocols need to be provided as [ [String, XmlRpcLegalValue] ]".into(), )) } None => return Err(ResponseError::Client("Missing argument 'protocols'".into())), @@ -266,11 +259,11 @@ fn request_topic( ) -> error::rosxmlrpc::Result<(String, String, i32)> { let (_code, _message, protocols): (i32, String, (String, String, i32)) = xml_rpc::Client::new() .unwrap() - .call(&publisher_uri.parse().unwrap(), "requestTopic", &( - caller_id, - topic, - [["TCPROS"]], - )) + .call( + &publisher_uri.parse().unwrap(), + "requestTopic", + &(caller_id, topic, [["TCPROS"]]), + ) .unwrap() .unwrap(); Ok(protocols) diff --git a/rosrust/src/lib.rs b/rosrust/src/lib.rs index c09b0e0..fbed0f7 100644 --- a/rosrust/src/lib.rs +++ b/rosrust/src/lib.rs @@ -21,7 +21,7 @@ extern crate xml_rpc; extern crate yaml_rust; pub use api::Ros; -pub use tcpros::{Client, PublisherStream, Message, ServicePair as Service}; +pub use tcpros::{Client, Message, PublisherStream, ServicePair as Service}; pub use api::error; pub use time::{Duration, Time}; diff --git a/rosrust/src/rosxmlrpc/client.rs b/rosrust/src/rosxmlrpc/client.rs index 200af1a..2436edf 100644 --- a/rosrust/src/rosxmlrpc/client.rs +++ b/rosrust/src/rosxmlrpc/client.rs @@ -1,6 +1,6 @@ use std::sync::Mutex; use serde::{Deserialize, Serialize}; -use xml_rpc::{self, Value, Params}; +use xml_rpc::{self, Params, Value}; use super::{Response, ResponseError, ERROR_CODE, FAILURE_CODE, SUCCESS_CODE}; pub struct Client { @@ -28,8 +28,7 @@ impl Client { .map_err(|fault| { ResponseError::Client(format!( "Unexpected fault #{} received from server: {}", - fault.code, - fault.message + fault.code, fault.message )) })? .into_iter(); @@ -39,25 +38,19 @@ impl Client { first_item = response.next(); } match (first_item, response.next(), response.next()) { - (Some(Value::Int(code)), Some(Value::String(message)), Some(data)) => { - match code { - ERROR_CODE => Err(ResponseError::Client(message)), - FAILURE_CODE => Err(ResponseError::Server(message)), - SUCCESS_CODE => Ok(data), - _ => Err(ResponseError::Server( - "Bad response code returned from server".into(), - )), - } - } - (code, message, data) => { - Err(ResponseError::Server(format!( - "Response with three parameters (int code, str msg, value) \ - expected from server, received: ({:?}, {:?}, {:?})", - code, - message, - data - ))) - } + (Some(Value::Int(code)), Some(Value::String(message)), Some(data)) => match code { + ERROR_CODE => Err(ResponseError::Client(message)), + FAILURE_CODE => Err(ResponseError::Server(message)), + SUCCESS_CODE => Ok(data), + _ => Err(ResponseError::Server( + "Bad response code returned from server".into(), + )), + }, + (code, message, data) => Err(ResponseError::Server(format!( + "Response with three parameters (int code, str msg, value) \ + expected from server, received: ({:?}, {:?}, {:?})", + code, message, data + ))), } } diff --git a/rosrust/src/rosxmlrpc/mod.rs b/rosrust/src/rosxmlrpc/mod.rs index 4d60c65..e0ee960 100644 --- a/rosrust/src/rosxmlrpc/mod.rs +++ b/rosrust/src/rosxmlrpc/mod.rs @@ -26,8 +26,7 @@ impl std::fmt::Display for ResponseError { impl std::error::Error for ResponseError { fn description(&self) -> &str { match *self { - ResponseError::Client(ref v) | - ResponseError::Server(ref v) => v, + ResponseError::Client(ref v) | ResponseError::Server(ref v) => v, } } diff --git a/rosrust/src/rosxmlrpc/server.rs b/rosrust/src/rosxmlrpc/server.rs index 8526c3c..8198478 100644 --- a/rosrust/src/rosxmlrpc/server.rs +++ b/rosrust/src/rosxmlrpc/server.rs @@ -1,7 +1,7 @@ use std::net::SocketAddr; use xml_rpc; -use super::{ERROR_CODE, FAILURE_CODE, SUCCESS_CODE, Response, ResponseError}; +use super::{Response, ResponseError, ERROR_CODE, FAILURE_CODE, SUCCESS_CODE}; pub struct Server { server: xml_rpc::Server, @@ -21,9 +21,8 @@ impl Server { K: Into, T: Fn(xml_rpc::Params) -> Response + Send + Sync + 'static, { - self.server.register_value(name, move |args| { - Ok(response_to_params(msg, handler(args))) - }) + self.server + .register_value(name, move |args| Ok(response_to_params(msg, handler(args)))) } pub fn bind(self, uri: &SocketAddr) -> xml_rpc::error::Result { diff --git a/rosrust/src/tcpros/client.rs b/rosrust/src/tcpros/client.rs index b08fc3f..e5d8967 100644 --- a/rosrust/src/tcpros/client.rs +++ b/rosrust/src/tcpros/client.rs @@ -6,7 +6,7 @@ use std::collections::HashMap; use std; use serde_rosmsg::{from_reader, to_writer}; use super::error::{ErrorKind, Result, ResultExt}; -use super::header::{encode, decode}; +use super::header::{decode, encode}; use super::{ServicePair, ServiceResult}; pub struct ClientResponse { @@ -15,9 +15,9 @@ pub struct ClientResponse { impl ClientResponse { pub fn read(self) -> Result> { - self.handle.join().unwrap_or_else(|_| { - Err(ErrorKind::ServiceResponseUnknown.into()) - }) + self.handle + .join() + .unwrap_or_else(|_| Err(ErrorKind::ServiceResponseUnknown.into())) } } @@ -78,9 +78,8 @@ impl Client { service: &str, ) -> Result> { let connection = TcpStream::connect(uri.trim_left_matches("rosrpc://")); - let mut stream = connection.chain_err(|| { - ErrorKind::ServiceConnectionFail(service.into(), uri.into()) - })?; + let mut stream = + connection.chain_err(|| ErrorKind::ServiceConnectionFail(service.into(), uri.into()))?; // Service request starts by exchanging connection headers exchange_headers::(&mut stream, caller_id, service)?; @@ -89,9 +88,8 @@ impl Client { to_writer(&mut stream, &args)?; // Service responds with a boolean byte, signalling success - let success = read_verification_byte(&mut stream).chain_err(|| { - ErrorKind::ServiceResponseInterruption - })?; + let success = read_verification_byte(&mut stream) + .chain_err(|| ErrorKind::ServiceResponseInterruption)?; Ok(if success { // Decode response as response type upon success Ok(from_reader(&mut stream)?) diff --git a/rosrust/src/tcpros/header.rs b/rosrust/src/tcpros/header.rs index dc997aa..e9cb56f 100644 --- a/rosrust/src/tcpros/header.rs +++ b/rosrust/src/tcpros/header.rs @@ -63,7 +63,6 @@ mod tests { ); } - #[test] fn writes_multiple_items() { let mut cursor = std::io::Cursor::new(Vec::new()); @@ -74,58 +73,12 @@ mod tests { let data = cursor.into_inner(); assert!( vec![ - 21, - 0, - 0, - 0, - 7, - 0, - 0, - 0, - 97, - 98, - 99, - 61, - 49, - 50, - 51, - 6, - 0, - 0, - 0, - 65, - 65, - 65, - 61, - 66, - 48, - ] == data || - vec![ - 21, - 0, - 0, - 0, - 6, - 0, - 0, - 0, - 65, - 65, - 65, - 61, - 66, - 48, - 7, - 0, - 0, - 0, - 97, - 98, - 99, - 61, - 49, - 50, - 51, + 21, 0, 0, 0, 7, 0, 0, 0, 97, 98, 99, 61, 49, 50, 51, 6, 0, 0, 0, 65, 65, 65, 61, + 66, 48, + ] == data + || vec![ + 21, 0, 0, 0, 6, 0, 0, 0, 65, 65, 65, 61, 66, 48, 7, 0, 0, 0, 97, 98, 99, 61, + 49, 50, 51, ] == data ); } @@ -148,186 +101,19 @@ mod tests { #[test] fn reads_typical_header() { let input = vec![ - 0xb0, - 0x00, - 0x00, - 0x00, - 0x20, - 0x00, - 0x00, - 0x00, - 0x6d, - 0x65, - 0x73, - 0x73, - 0x61, - 0x67, - 0x65, - 0x5f, - 0x64, - 0x65, - 0x66, - 0x69, - 0x6e, - 0x69, - 0x74, - 0x69, - 0x6f, - 0x6e, - 0x3d, - 0x73, - 0x74, - 0x72, - 0x69, - 0x6e, - 0x67, - 0x20, - 0x64, - 0x61, - 0x74, - 0x61, - 0x0a, - 0x0a, - 0x25, - 0x00, - 0x00, - 0x00, - 0x63, - 0x61, - 0x6c, - 0x6c, - 0x65, - 0x72, - 0x69, - 0x64, - 0x3d, - 0x2f, - 0x72, - 0x6f, - 0x73, - 0x74, - 0x6f, - 0x70, - 0x69, - 0x63, - 0x5f, - 0x34, - 0x37, - 0x36, - 0x37, - 0x5f, - 0x31, - 0x33, - 0x31, - 0x36, - 0x39, - 0x31, - 0x32, - 0x37, - 0x34, - 0x31, - 0x35, - 0x35, - 0x37, - 0x0a, - 0x00, - 0x00, - 0x00, - 0x6c, - 0x61, - 0x74, - 0x63, - 0x68, - 0x69, - 0x6e, - 0x67, - 0x3d, - 0x31, - 0x27, - 0x00, - 0x00, - 0x00, - 0x6d, - 0x64, - 0x35, - 0x73, - 0x75, - 0x6d, - 0x3d, - 0x39, - 0x39, - 0x32, - 0x63, - 0x65, - 0x38, - 0x61, - 0x31, - 0x36, - 0x38, - 0x37, - 0x63, - 0x65, - 0x63, - 0x38, - 0x63, - 0x38, - 0x62, - 0x64, - 0x38, - 0x38, - 0x33, - 0x65, - 0x63, - 0x37, - 0x33, - 0x63, - 0x61, - 0x34, - 0x31, - 0x64, - 0x31, - 0x0e, - 0x00, - 0x00, - 0x00, - 0x74, - 0x6f, - 0x70, - 0x69, - 0x63, - 0x3d, - 0x2f, - 0x63, - 0x68, - 0x61, - 0x74, - 0x74, - 0x65, - 0x72, - 0x14, - 0x00, - 0x00, - 0x00, - 0x74, - 0x79, - 0x70, - 0x65, - 0x3d, - 0x73, - 0x74, - 0x64, - 0x5f, - 0x6d, - 0x73, - 0x67, - 0x73, - 0x2f, - 0x53, - 0x74, - 0x72, - 0x69, - 0x6e, - 0x67, + 0xb0, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x6d, 0x65, 0x73, 0x73, 0x61, 0x67, + 0x65, 0x5f, 0x64, 0x65, 0x66, 0x69, 0x6e, 0x69, 0x74, 0x69, 0x6f, 0x6e, 0x3d, 0x73, + 0x74, 0x72, 0x69, 0x6e, 0x67, 0x20, 0x64, 0x61, 0x74, 0x61, 0x0a, 0x0a, 0x25, 0x00, + 0x00, 0x00, 0x63, 0x61, 0x6c, 0x6c, 0x65, 0x72, 0x69, 0x64, 0x3d, 0x2f, 0x72, 0x6f, + 0x73, 0x74, 0x6f, 0x70, 0x69, 0x63, 0x5f, 0x34, 0x37, 0x36, 0x37, 0x5f, 0x31, 0x33, + 0x31, 0x36, 0x39, 0x31, 0x32, 0x37, 0x34, 0x31, 0x35, 0x35, 0x37, 0x0a, 0x00, 0x00, + 0x00, 0x6c, 0x61, 0x74, 0x63, 0x68, 0x69, 0x6e, 0x67, 0x3d, 0x31, 0x27, 0x00, 0x00, + 0x00, 0x6d, 0x64, 0x35, 0x73, 0x75, 0x6d, 0x3d, 0x39, 0x39, 0x32, 0x63, 0x65, 0x38, + 0x61, 0x31, 0x36, 0x38, 0x37, 0x63, 0x65, 0x63, 0x38, 0x63, 0x38, 0x62, 0x64, 0x38, + 0x38, 0x33, 0x65, 0x63, 0x37, 0x33, 0x63, 0x61, 0x34, 0x31, 0x64, 0x31, 0x0e, 0x00, + 0x00, 0x00, 0x74, 0x6f, 0x70, 0x69, 0x63, 0x3d, 0x2f, 0x63, 0x68, 0x61, 0x74, 0x74, + 0x65, 0x72, 0x14, 0x00, 0x00, 0x00, 0x74, 0x79, 0x70, 0x65, 0x3d, 0x73, 0x74, 0x64, + 0x5f, 0x6d, 0x73, 0x67, 0x73, 0x2f, 0x53, 0x74, 0x72, 0x69, 0x6e, 0x67, ]; let data = decode(&mut std::io::Cursor::new(input)).expect(FAILED_TO_DECODE); assert_eq!(6, data.len()); diff --git a/rosrust/src/tcpros/publisher.rs b/rosrust/src/tcpros/publisher.rs index c406694..7fe34e1 100644 --- a/rosrust/src/tcpros/publisher.rs +++ b/rosrust/src/tcpros/publisher.rs @@ -6,7 +6,7 @@ use serde_rosmsg::to_vec; use super::error::{ErrorKind, Result, ResultExt}; use super::header; use super::Message; -use super::util::streamfork::{fork, TargetList, DataStream}; +use super::util::streamfork::{fork, DataStream, TargetList}; use super::util::tcpconnection; pub struct Publisher { @@ -56,9 +56,8 @@ where { // This listener stream never breaks by itself since it's a TcpListener for mut stream in listener { - let result = exchange_headers::(&mut stream, topic).chain_err(|| { - ErrorKind::TopicConnectionFail(topic.into()) - }); + let result = exchange_headers::(&mut stream, topic) + .chain_err(|| ErrorKind::TopicConnectionFail(topic.into())); if let Err(err) = result { let info = err.iter() .map(|v| format!("{}", v)) @@ -106,9 +105,7 @@ impl Publisher { { let (targets, data) = fork(); let topic_name = String::from(topic); - thread::spawn(move || { - listen_for_subscribers::(&topic_name, listener, &targets) - }); + thread::spawn(move || listen_for_subscribers::(&topic_name, listener, &targets)); Publisher { subscriptions: data, port: port, diff --git a/rosrust/src/tcpros/service.rs b/rosrust/src/tcpros/service.rs index ab48954..458865c 100644 --- a/rosrust/src/tcpros/service.rs +++ b/rosrust/src/tcpros/service.rs @@ -84,8 +84,7 @@ where if let Err(err) = exchange_headers::(&mut stream, service, node_name) { error!( "Failed to exchange headers for service '{}': {}", - service, - err + service, err ); continue; } @@ -135,16 +134,14 @@ where U: std::io::Read + std::io::Write + Send + 'static, F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static, { - thread::spawn(move || if let Err(err) = handle_request_loop::( - stream, - &handler, - ) - { - let info = err.iter() - .map(|v| format!("{}", v)) - .collect::>() - .join("\nCaused by:"); - error!("{}", info); + thread::spawn(move || { + if let Err(err) = handle_request_loop::(stream, &handler) { + let info = err.iter() + .map(|v| format!("{}", v)) + .collect::>() + .join("\nCaused by:"); + error!("{}", info); + } }); } diff --git a/rosrust/src/tcpros/subscriber.rs b/rosrust/src/tcpros/subscriber.rs index 80d0903..86d92ae 100644 --- a/rosrust/src/tcpros/subscriber.rs +++ b/rosrust/src/tcpros/subscriber.rs @@ -1,12 +1,12 @@ use byteorder::{LittleEndian, ReadBytesExt, WriteBytesExt}; use std::net::{SocketAddr, TcpStream, ToSocketAddrs}; -use std::sync::mpsc::{channel, Sender, Receiver}; +use std::sync::mpsc::{channel, Receiver, Sender}; use std::thread; use std::collections::HashMap; use std; use serde_rosmsg::from_slice; use super::error::{ErrorKind, Result, ResultExt}; -use super::header::{encode, decode, match_field}; +use super::header::{decode, encode, match_field}; use super::Message; pub struct Subscriber { @@ -27,9 +27,7 @@ impl Subscriber { let caller_id = String::from(caller_id); let topic_name = String::from(topic); let data_stream = data_tx.clone(); - thread::spawn(move || { - join_connections::(&data_tx, pub_rx, &caller_id, &topic_name) - }); + thread::spawn(move || join_connections::(&data_tx, pub_rx, &caller_id, &topic_name)); thread::spawn(move || handle_data::(data_rx, callback)); Subscriber { data_stream: data_stream, @@ -45,9 +43,9 @@ impl Subscriber { // Failure could only be caused by the join_connections // thread not running, which only happens after // Subscriber has been deconstructed - self.publishers_stream.send(address).expect( - "Connected thread died", - ); + self.publishers_stream + .send(address) + .expect("Connected thread died"); } Ok(()) } diff --git a/rosrust/src/tcpros/util/streamfork.rs b/rosrust/src/tcpros/util/streamfork.rs index f2ee108..fd1e437 100644 --- a/rosrust/src/tcpros/util/streamfork.rs +++ b/rosrust/src/tcpros/util/streamfork.rs @@ -1,5 +1,5 @@ use std::io::Write; -use std::sync::mpsc::{channel, Sender, Receiver}; +use std::sync::mpsc::{channel, Receiver, Sender}; use std::thread; pub fn fork() -> (TargetList, DataStream) { diff --git a/rosrust/src/tcpros/util/tcpconnection.rs b/rosrust/src/tcpros/util/tcpconnection.rs index 7d09294..b1eb5e0 100644 --- a/rosrust/src/tcpros/util/tcpconnection.rs +++ b/rosrust/src/tcpros/util/tcpconnection.rs @@ -1,6 +1,6 @@ use std::net::{TcpListener, TcpStream}; use std::thread; -use std::sync::mpsc::{Sender, Receiver, channel}; +use std::sync::mpsc::{channel, Receiver, Sender}; pub fn iterate(listener: TcpListener, tag: String) -> (Raii, TcpConnectionIterator) { let (tx, rx) = channel(); diff --git a/rosrust_codegen/src/genmsg.rs b/rosrust_codegen/src/genmsg.rs index 16a57bf..0c1da55 100644 --- a/rosrust_codegen/src/genmsg.rs +++ b/rosrust_codegen/src/genmsg.rs @@ -20,9 +20,12 @@ pub fn depend_on_messages( .messages .iter() .map(|(&(ref pack, ref _name), _value)| pack.clone()) - .chain(message_map.services.iter().map(|&(ref pack, ref _name)| { - pack.clone() - })) + .chain( + message_map + .services + .iter() + .map(|&(ref pack, ref _name)| pack.clone()), + ) .collect::>(); for package in packages { output.push(format!(" pub mod {} {{", package)); @@ -34,18 +37,18 @@ pub fn depend_on_messages( .collect::>(); for name in &names { let key = (package.clone(), name.clone()); - let message = message_map.messages.get(&key).expect( - "Internal implementation contains mismatch in map keys", - ); - let hash = hashes.get(&key).expect( - "Internal implementation contains mismatch in map keys", - ); + let message = message_map + .messages + .get(&key) + .expect("Internal implementation contains mismatch in map keys"); + let hash = hashes + .get(&key) + .expect("Internal implementation contains mismatch in map keys"); let definition = helpers::generate_message_definition(&message_map.messages, message)?; output.push(message.struct_string(crate_prefix)); output.push(format!( " impl {}Message for {} {{", - crate_prefix, - message.name + crate_prefix, message.name )); output.push(create_function("msg_definition", &definition)); output.push(create_function("md5sum", hash)); @@ -62,18 +65,15 @@ pub fn depend_on_messages( .map(|&(ref _pack, ref name)| name.clone()) .collect::>(); for name in &names { - let hash = hashes.get(&(package.clone(), name.clone())).expect( - "Internal implementation contains mismatch in map keys", - ); - output.push( - " #[allow(dead_code,non_camel_case_types,non_snake_case)]".into(), - ); + let hash = hashes + .get(&(package.clone(), name.clone())) + .expect("Internal implementation contains mismatch in map keys"); + output.push(" #[allow(dead_code,non_camel_case_types,non_snake_case)]".into()); output.push(" #[derive(Serialize,Deserialize,Debug)]".into()); output.push(format!(" pub struct {} {{}}", name)); output.push(format!( " impl {}Message for {} {{", - crate_prefix, - name + crate_prefix, name )); output.push(create_function("msg_definition", "")); output.push(create_function("md5sum", hash)); @@ -85,8 +85,7 @@ pub fn depend_on_messages( output.push(format!( " impl {}Service for {} {{", - crate_prefix, - name + crate_prefix, name )); output.push(format!(" type Request = {}Req;", name)); output.push(format!(" type Response = {}Res;", name)); @@ -105,8 +104,7 @@ fn create_function(name: &str, value: &str) -> String { fn {}() -> ::std::string::String {{ {:?}.into() }}"#, - name, - value + name, value ) } @@ -118,12 +116,10 @@ fn string_into_pair(input: &str) -> Result<(&str, &str)> { }; let name = match parts.next() { Some(v) => v, - None => { - bail!( - "Package string needs to be in package/name format: {}", - input - ) - } + None => bail!( + "Package string needs to be in package/name format: {}", + input + ), }; Ok((package, name)) } @@ -147,16 +143,16 @@ mod tests { #[test] fn benchmark_genmsg() { - let data = depend_on_messages(&[FILEPATH], &["benchmark_msgs/Overall"], "::rosrust::") - .unwrap(); + let data = + depend_on_messages(&[FILEPATH], &["benchmark_msgs/Overall"], "::rosrust::").unwrap(); println!("{}", data); // TODO: actually test this output data } #[test] fn benchmark_genmsg_service() { - let data = depend_on_messages(&[FILEPATH], &["simple_srv/Something"], "::rosrust::") - .unwrap(); + let data = + depend_on_messages(&[FILEPATH], &["simple_srv/Something"], "::rosrust::").unwrap(); println!("{}", data); // TODO: actually test this output data } diff --git a/rosrust_codegen/src/helpers.rs b/rosrust_codegen/src/helpers.rs index 4431770..ca62c58 100644 --- a/rosrust_codegen/src/helpers.rs +++ b/rosrust_codegen/src/helpers.rs @@ -1,4 +1,4 @@ -use std::collections::{LinkedList, HashMap, HashSet}; +use std::collections::{HashMap, HashSet, LinkedList}; use msg::Msg; use error::{Result, ResultExt}; use std; @@ -145,9 +145,8 @@ fn get_message(folders: &[&str], package: &str, name: &str) -> Result Result Result Option { } fn append_src_folder(path: &str) -> Option { - Path::new(path).join("..").join("src").to_str().map(|v| { - v.to_owned() - }) + Path::new(path) + .join("..") + .join("src") + .to_str() + .map(|v| v.to_owned()) } diff --git a/rosrust_codegen/src/msg.rs b/rosrust_codegen/src/msg.rs index 67c32dd..363ad1a 100644 --- a/rosrust_codegen/src/msg.rs +++ b/rosrust_codegen/src/msg.rs @@ -72,9 +72,7 @@ impl Msg { let mut output = Vec::::new(); for field in &self.fields { if let Some(s) = field.to_const_string(crate_prefix) { - output.push( - " #[allow(dead_code,non_upper_case_globals)]".into(), - ); + output.push(" #[allow(dead_code,non_upper_case_globals)]".into()); output.push(format!(" pub const {}", s)); } } @@ -83,12 +81,8 @@ impl Msg { pub fn struct_string(&self, crate_prefix: &str) -> String { let mut output = Vec::::new(); - output.push( - " #[allow(dead_code,non_camel_case_types,non_snake_case)]".into(), - ); - output.push( - " #[derive(Serialize,Deserialize,Debug,Default)]".into(), - ); + output.push(" #[allow(dead_code,non_camel_case_types,non_snake_case)]".into()); + output.push(" #[derive(Serialize,Deserialize,Debug,Default)]".into()); output.push(format!(" pub struct {} {{", self.name)); for field in &self.fields { if let Some(s) = field.to_string(crate_prefix) { @@ -248,17 +242,15 @@ fn match_line(data: &str) -> Option> { #[inline] fn strip_useless(data: &str) -> Result<&str> { - Ok( - data.splitn(2, '#') - .next() - .ok_or_else(|| { - format!( - "Somehow splitting a line resulted in 0 parts?! Happened here: {}", - data - ) - })? - .trim(), - ) + Ok(data.splitn(2, '#') + .next() + .ok_or_else(|| { + format!( + "Somehow splitting a line resulted in 0 parts?! Happened here: {}", + data + ) + })? + .trim()) } #[inline] @@ -306,8 +298,7 @@ impl FieldInfo { let datatype = self.datatype.md5_string(package, hashes)?; Ok(match (self.datatype.is_builtin(), &self.case) { (_, &FieldCase::Const(ref v)) => format!("{} {}={}", datatype, self.name, v), - (false, _) | - (_, &FieldCase::Unit) => format!("{} {}", datatype, self.name), + (false, _) | (_, &FieldCase::Unit) => format!("{} {}", datatype, self.name), (true, &FieldCase::Vector) => format!("{}[] {}", datatype, self.name), (true, &FieldCase::Array(l)) => format!("{}[{}] {}", datatype, l, self.name), }) @@ -331,10 +322,10 @@ impl FieldInfo { Some(match self.datatype { DataType::Bool => format!("{}: bool = {:?};", self.name, value != "0"), DataType::String => format!("{}: &'static str = {:?};", self.name, value), - DataType::Time | - DataType::Duration | - DataType::LocalStruct(..) | - DataType::RemoteStruct(..) => return None, + DataType::Time + | DataType::Duration + | DataType::LocalStruct(..) + | DataType::RemoteStruct(..) => return None, _ => { let datatype = self.datatype.rust_type(crate_prefix); format!("{}: {} = {} as {};", self.name, datatype, value, datatype) @@ -344,9 +335,8 @@ impl FieldInfo { fn new(datatype: &str, name: &str, case: FieldCase) -> Result { Ok(FieldInfo { - datatype: parse_datatype(datatype).ok_or_else(|| { - format!("Unsupported datatype: {}", datatype) - })?, + datatype: parse_datatype(datatype) + .ok_or_else(|| format!("Unsupported datatype: {}", datatype))?, name: name.to_owned(), case: case, }) @@ -397,11 +387,21 @@ impl DataType { fn is_builtin(&self) -> bool { match *self { - DataType::Bool | DataType::I8(_) | DataType::I16 | DataType::I32 | DataType::I64 | - DataType::U8(_) | DataType::U16 | DataType::U32 | DataType::U64 | DataType::F32 | - DataType::F64 | DataType::String | DataType::Time | DataType::Duration => true, - DataType::LocalStruct(_) | - DataType::RemoteStruct(_, _) => false, + DataType::Bool + | DataType::I8(_) + | DataType::I16 + | DataType::I32 + | DataType::I64 + | DataType::U8(_) + | DataType::U16 + | DataType::U32 + | DataType::U64 + | DataType::F32 + | DataType::F64 + | DataType::String + | DataType::Time + | DataType::Duration => true, + DataType::LocalStruct(_) | DataType::RemoteStruct(_, _) => false, } } @@ -410,35 +410,31 @@ impl DataType { package: &str, hashes: &HashMap<(String, String), String>, ) -> ::std::result::Result { - Ok( - match *self { - DataType::Bool => "bool", - DataType::I8(true) => "int8", - DataType::I8(false) => "byte", - DataType::I16 => "int16", - DataType::I32 => "int32", - DataType::I64 => "int64", - DataType::U8(true) => "uint8", - DataType::U8(false) => "char", - DataType::U16 => "uint16", - DataType::U32 => "uint32", - DataType::U64 => "uint64", - DataType::F32 => "float32", - DataType::F64 => "float64", - DataType::String => "string", - DataType::Time => "time", - DataType::Duration => "duration", - DataType::LocalStruct(ref name) => { - hashes - .get(&(package.to_owned(), name.clone())) - .ok_or(())? - .as_str() - } - DataType::RemoteStruct(ref pkg, ref name) => { - hashes.get(&(pkg.clone(), name.clone())).ok_or(())?.as_str() - } - }.into(), - ) + Ok(match *self { + DataType::Bool => "bool", + DataType::I8(true) => "int8", + DataType::I8(false) => "byte", + DataType::I16 => "int16", + DataType::I32 => "int32", + DataType::I64 => "int64", + DataType::U8(true) => "uint8", + DataType::U8(false) => "char", + DataType::U16 => "uint16", + DataType::U32 => "uint32", + DataType::U64 => "uint64", + DataType::F32 => "float32", + DataType::F64 => "float64", + DataType::String => "string", + DataType::Time => "time", + DataType::Duration => "duration", + DataType::LocalStruct(ref name) => hashes + .get(&(package.to_owned(), name.clone())) + .ok_or(())? + .as_str(), + DataType::RemoteStruct(ref pkg, ref name) => { + hashes.get(&(pkg.clone(), name.clone())).ok_or(())?.as_str() + } + }.into()) } } @@ -702,9 +698,8 @@ mod tests { name: "myname".into(), case: FieldCase::Unit, }, - match_line( - " geom_msgs/Twist myname # this clearly should succeed", - ).unwrap() + match_line(" geom_msgs/Twist myname # this clearly should succeed",) + .unwrap() .unwrap() ); @@ -755,7 +750,7 @@ mod tests { fn match_lines_parses_real_messages() { let data = match_lines(include_str!( "msg_examples/geometry_msgs/msg/TwistWithCovariance.\ - msg" + msg" )).unwrap(); assert_eq!( vec![ @@ -823,9 +818,7 @@ mod tests { ); let dependencies = get_dependency_set(&data); assert_eq!(dependencies.len(), 1); - assert!(dependencies.contains( - &("geometry_msgs".into(), "Twist".into()), - )); + assert!(dependencies.contains(&("geometry_msgs".into(), "Twist".into()),)); let data = Msg::new( "geometry_msgs", @@ -851,9 +844,7 @@ mod tests { ); let dependencies = get_dependency_set(&data); assert_eq!(dependencies.len(), 2); - assert!(dependencies.contains( - &("geometry_msgs".into(), "Pose".into()), - )); + assert!(dependencies.contains(&("geometry_msgs".into(), "Pose".into()),)); assert!(dependencies.contains(&("std_msgs".into(), "Header".into()))); let data = Msg::new( @@ -905,12 +896,8 @@ mod tests { ); let dependencies = get_dependency_set(&data); assert_eq!(dependencies.len(), 3); - assert!(dependencies.contains( - &("geometry_msgs".into(), "Vector3".into()), - )); - assert!(dependencies.contains( - &("geometry_msgs".into(), "Quaternion".into()), - )); + assert!(dependencies.contains(&("geometry_msgs".into(), "Vector3".into()),)); + assert!(dependencies.contains(&("geometry_msgs".into(), "Quaternion".into()),)); assert!(dependencies.contains(&("std_msgs".into(), "Header".into()))); } } From 311639942dd0b0133b4288e2346d3c223dd7c02e Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Mon, 25 Dec 2017 18:17:17 +0100 Subject: [PATCH 45/50] Add logging --- examples/pubsub/src/publisher.rs | 5 ++- examples/pubsub/src/subscriber.rs | 18 ++++++--- examples/serviceclient/src/service.rs | 20 +++++++--- rosrust/Cargo.toml | 2 +- rosrust/build.rs | 2 +- rosrust/src/api/clock.rs | 2 +- rosrust/src/api/logger.rs | 41 ++++++++++++++++++++ rosrust/src/api/mod.rs | 9 +++-- rosrust/src/api/raii.rs | 12 +++++- rosrust/src/api/ros.rs | 56 ++++++++++++++++++++++++--- rosrust/src/lib.rs | 3 +- rosrust/src/rosxmlrpc/client.rs | 9 +---- rosrust/src/tcpros/mod.rs | 5 +++ 13 files changed, 148 insertions(+), 36 deletions(-) create mode 100644 rosrust/src/api/logger.rs diff --git a/examples/pubsub/src/publisher.rs b/examples/pubsub/src/publisher.rs index b5a25d5..97cc399 100644 --- a/examples/pubsub/src/publisher.rs +++ b/examples/pubsub/src/publisher.rs @@ -1,4 +1,5 @@ extern crate env_logger; +#[macro_use] extern crate rosrust; #[macro_use] extern crate rosrust_codegen; @@ -20,7 +21,9 @@ fn main() { let mut msg = msg::std_msgs::String::default(); msg.data = format!("hello world {}", count); - chatter_pub.send(&msg).unwrap(); + ros_info!(ros, msg.data); + + chatter_pub.send(msg).unwrap(); rate.sleep(); diff --git a/examples/pubsub/src/subscriber.rs b/examples/pubsub/src/subscriber.rs index 407ed10..732257e 100644 --- a/examples/pubsub/src/subscriber.rs +++ b/examples/pubsub/src/subscriber.rs @@ -1,21 +1,29 @@ extern crate env_logger; +#[macro_use] extern crate rosrust; #[macro_use] extern crate rosrust_codegen; use rosrust::Ros; +use std::sync::{Arc, Mutex}; rosmsg_include!(); fn main() { env_logger::init().unwrap(); - let mut ros = Ros::new("listener").unwrap(); + let ros = Arc::new(Mutex::new(Ros::new("listener").unwrap())); + let ros_thread = Arc::clone(&ros); // The subscriber is stopped when the returned object is destroyed - let _subscriber_raii = ros.subscribe("chatter", |v: msg::std_msgs::String| { - println!("{}", v.data) - }).unwrap(); + let _subscriber_raii = ros.lock() + .unwrap() + .subscribe("chatter", move |v: msg::std_msgs::String| { + let mut ros = ros_thread.lock().unwrap(); + println!("{}", v.data); + ros_info!(ros, v.data); + }) + .unwrap(); - ros.spin(); + let spin = { ros.lock().unwrap().spin() }; } diff --git a/examples/serviceclient/src/service.rs b/examples/serviceclient/src/service.rs index fa7f053..60cc7db 100644 --- a/examples/serviceclient/src/service.rs +++ b/examples/serviceclient/src/service.rs @@ -1,21 +1,31 @@ extern crate env_logger; +#[macro_use] extern crate rosrust; #[macro_use] extern crate rosrust_codegen; use rosrust::Ros; +use std::sync::{Arc, Mutex}; rosmsg_include!(); fn main() { env_logger::init().unwrap(); - let mut ros = Ros::new("add_two_ints_server").unwrap(); + let ros = Arc::new(Mutex::new(Ros::new("add_two_ints_server").unwrap())); + + let ros_thread = Arc::clone(&ros); // The service is stopped when the returned object is destroyed - let _service_raii = ros.service::("add_two_ints", |req| { - Ok(msg::roscpp_tutorials::TwoIntsRes { sum: req.a + req.b }) - }).unwrap(); + let _service_raii = ros.lock() + .unwrap() + .service::("add_two_ints", move |req| { + let mut ros = ros_thread.lock().unwrap(); + let sum = req.a + req.b; + ros_info!(ros, format!("{} + {} = {}", req.a, req.b, sum)); + Ok(msg::roscpp_tutorials::TwoIntsRes { sum }) + }) + .unwrap(); - ros.spin(); + let spin = { ros.lock().unwrap().spin() }; } diff --git a/rosrust/Cargo.toml b/rosrust/Cargo.toml index 8438a18..f97c7cc 100644 --- a/rosrust/Cargo.toml +++ b/rosrust/Cargo.toml @@ -21,7 +21,7 @@ rust-crypto = "0.2.36" serde = "1.0.24" serde_derive = "1.0.24" serde_rosmsg = "0.2.0" -xml-rpc = "0.0.7" +xml-rpc = "0.0.8" yaml-rust = "0.4.0" [build-dependencies] diff --git a/rosrust/build.rs b/rosrust/build.rs index 3410729..c6b705f 100644 --- a/rosrust/build.rs +++ b/rosrust/build.rs @@ -1,4 +1,4 @@ #[macro_use] extern crate rosrust_codegen; -rosmsg_main_no_prefix!("rosgraph_msgs/Clock"); +rosmsg_main_no_prefix!("rosgraph_msgs/Clock", "rosgraph_msgs/Log"); diff --git a/rosrust/src/api/clock.rs b/rosrust/src/api/clock.rs index 8184eda..b7ccf18 100644 --- a/rosrust/src/api/clock.rs +++ b/rosrust/src/api/clock.rs @@ -29,7 +29,7 @@ impl Rate { } } -pub trait Clock { +pub trait Clock: Send + Sync { fn now(&self) -> Time; fn sleep(&self, d: Duration); fn wait_until(&self, t: Time); diff --git a/rosrust/src/api/logger.rs b/rosrust/src/api/logger.rs new file mode 100644 index 0000000..dda7bab --- /dev/null +++ b/rosrust/src/api/logger.rs @@ -0,0 +1,41 @@ +#[macro_export] +macro_rules! ros_log { + ($ros:ident, $level:expr, $msg:expr) => { + $ros.log($level, &$msg, file!(), line!()); + } +} + +#[macro_export] +macro_rules! ros_debug { + ($ros:ident, $msg:expr) => { + ros_log!($ros, $crate::msg::rosgraph_msgs::Log::DEBUG, $msg); + } +} + +#[macro_export] +macro_rules! ros_info { + ($ros:ident, $msg:expr) => { + ros_log!($ros, $crate::msg::rosgraph_msgs::Log::INFO, $msg); + } +} + +#[macro_export] +macro_rules! ros_warn { + ($ros:ident, $msg:expr) => { + ros_log!($ros, $crate::msg::rosgraph_msgs::Log::WARN, $msg); + } +} + +#[macro_export] +macro_rules! ros_err { + ($ros:ident, $msg:expr) => { + ros_log!($ros, $crate::msg::rosgraph_msgs::Log::ERROR, $msg); + } +} + +#[macro_export] +macro_rules! ros_fatal { + ($ros:ident, $msg:expr) => { + ros_log!($ros, $crate::msg::rosgraph_msgs::Log::FATAL, $msg); + } +} diff --git a/rosrust/src/api/mod.rs b/rosrust/src/api/mod.rs index 6bca0d1..90f3ac5 100644 --- a/rosrust/src/api/mod.rs +++ b/rosrust/src/api/mod.rs @@ -1,12 +1,13 @@ pub use self::ros::Ros; -pub use self::clock::Rate; +pub use self::clock::{Clock, Rate}; mod clock; pub mod error; +pub mod logger; mod master; -mod slave; +mod raii; +mod resolve; mod ros; +mod slave; mod slavehandler; mod naming; -mod raii; -mod resolve; diff --git a/rosrust/src/api/raii.rs b/rosrust/src/api/raii.rs index 1aa0e38..18b54bf 100644 --- a/rosrust/src/api/raii.rs +++ b/rosrust/src/api/raii.rs @@ -1,12 +1,16 @@ use super::error::Result; use super::master::Master; use super::slave::Slave; +use super::clock::Clock; use rosxmlrpc::Response; use std::sync::Arc; +use std::sync::atomic::AtomicUsize; use tcpros::{Message, PublisherStream, ServicePair, ServiceResult}; #[derive(Clone)] pub struct Publisher { + clock: Arc, + seq: Arc, stream: PublisherStream, _raii: Arc>, } @@ -15,6 +19,7 @@ impl Publisher { pub(crate) fn new( master: Arc, slave: Arc, + clock: Arc, hostname: &str, name: &str, ) -> Result { @@ -28,6 +33,8 @@ impl Publisher { match info.master.register_publisher(name, &T::msg_type()) { Ok(_) => Ok(Self { stream, + clock, + seq: Arc::new(AtomicUsize::new(0)), _raii: Arc::new(InteractorRaii::new(info)), }), Err(error) => { @@ -42,8 +49,9 @@ impl Publisher { } #[inline] - pub fn send(&mut self, message: &T) -> Result<()> { - self.stream.send(message).map_err(|v| v.into()) + pub fn send(&mut self, mut message: T) -> Result<()> { + message.set_header(&self.clock, &self.seq); + self.stream.send(&message).map_err(|v| v.into()) } } diff --git a/rosrust/src/api/ros.rs b/rosrust/src/api/ros.rs index 8e78fbd..bdbf873 100644 --- a/rosrust/src/api/ros.rs +++ b/rosrust/src/api/ros.rs @@ -1,4 +1,5 @@ -use msg::rosgraph_msgs::Clock as ClockMsg; +use msg::rosgraph_msgs::{Clock as ClockMsg, Log}; +use msg::std_msgs::Header; use time::{Duration, Time}; use serde::{Deserialize, Serialize}; use std::sync::{mpsc, Arc}; @@ -23,7 +24,8 @@ pub struct Ros { name: String, clock: Arc, static_subs: Vec, - shutdown_rx: mpsc::Receiver<()>, + logger: Option>, + shutdown_rx: Option>, } impl Ros { @@ -59,6 +61,8 @@ impl Ros { ros.clock = ros_clock; } + ros.logger = Some(ros.publish("/rosout")?); + Ok(ros) } @@ -87,7 +91,8 @@ impl Ros { name: name, clock: Arc::new(RealClock::default()), static_subs: Vec::new(), - shutdown_rx, + logger: None, + shutdown_rx: Some(shutdown_rx), }) } @@ -131,12 +136,18 @@ impl Ros { #[inline] pub fn is_ok(&self) -> bool { - self.shutdown_rx.try_recv() == Err(mpsc::TryRecvError::Empty) + if let Some(ref rx) = self.shutdown_rx { + rx.try_recv() == Err(mpsc::TryRecvError::Empty) + } else { + return false; + } } #[inline] - pub fn spin(&self) { - self.shutdown_rx.recv().is_ok(); + pub fn spin(&mut self) -> Spinner { + Spinner { + shutdown_rx: self.shutdown_rx.take(), + } } pub fn param<'a, 'b>(&'a self, name: &'b str) -> Option> { @@ -230,10 +241,31 @@ impl Ros { Publisher::new( Arc::clone(&self.master), Arc::clone(&self.slave), + Arc::clone(&self.clock), &self.hostname, &name, ) } + + pub fn log(&mut self, level: i8, msg: &str, file: &str, line: u32) { + let logger = &mut match self.logger { + Some(ref mut v) => v, + None => return, + }; + let message = Log { + header: Header::default(), + level, + msg: msg.into(), + name: self.name.clone(), + line, + file: file.into(), + function: String::default(), + topics: vec![], + }; + if let Err(err) = logger.send(message) { + error!("Logging error: {}", err); + } + } } pub struct Parameter<'a> { @@ -302,3 +334,15 @@ fn yaml_to_string(val: Yaml) -> Result { _ => bail!("Hash keys need to be strings"), }) } + +pub struct Spinner { + shutdown_rx: Option>, +} + +impl Drop for Spinner { + fn drop(&mut self) { + if let Some(ref rx) = self.shutdown_rx { + rx.recv().is_ok(); + } + } +} diff --git a/rosrust/src/lib.rs b/rosrust/src/lib.rs index fbed0f7..1379208 100644 --- a/rosrust/src/lib.rs +++ b/rosrust/src/lib.rs @@ -20,9 +20,8 @@ extern crate serde_rosmsg; extern crate xml_rpc; extern crate yaml_rust; -pub use api::Ros; +pub use api::{error, logger, Clock, Ros}; pub use tcpros::{Client, Message, PublisherStream, ServicePair as Service}; -pub use api::error; pub use time::{Duration, Time}; mod api; diff --git a/rosrust/src/rosxmlrpc/client.rs b/rosrust/src/rosxmlrpc/client.rs index 2436edf..701b95f 100644 --- a/rosrust/src/rosxmlrpc/client.rs +++ b/rosrust/src/rosxmlrpc/client.rs @@ -1,27 +1,20 @@ -use std::sync::Mutex; use serde::{Deserialize, Serialize}; use xml_rpc::{self, Params, Value}; use super::{Response, ResponseError, ERROR_CODE, FAILURE_CODE, SUCCESS_CODE}; pub struct Client { - client: Mutex, master_uri: String, } impl Client { pub fn new(master_uri: &str) -> Result { Ok(Client { - client: Mutex::new(xml_rpc::Client::new()?), master_uri: master_uri.to_owned(), }) } pub fn request_tree_with_tree(&self, name: &str, params: Params) -> Response { - let mut client = self.client.lock().map_err(|err| { - ResponseError::Client(format!("Failed to acquire lock on socket: {}", err)) - })?; - let mut response = client - .call_value(&self.master_uri.parse().unwrap(), name, params) + let mut response = xml_rpc::call_value(&self.master_uri.parse().unwrap(), name, params) .map_err(|err| { ResponseError::Client(format!("Failed to perform call to server: {}", err)) })? diff --git a/rosrust/src/tcpros/mod.rs b/rosrust/src/tcpros/mod.rs index 4dbf932..d7fbaab 100644 --- a/rosrust/src/tcpros/mod.rs +++ b/rosrust/src/tcpros/mod.rs @@ -6,6 +6,10 @@ pub use self::service::Service; pub use self::client::Client; pub use self::error::Error; +use Clock; +use std::sync::Arc; +use std::sync::atomic::AtomicUsize; + mod header; pub mod error; mod publisher; @@ -20,6 +24,7 @@ pub trait Message: Serialize + Deserialize<'static> + Send + 'static { fn msg_definition() -> String; fn md5sum() -> String; fn msg_type() -> String; + fn set_header(&mut self, _clock: &Arc, _seq: &Arc) {} } pub trait ServicePair: Message { From 22cd0bdba8ec62ab409863573f2bd0190d5f15b7 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 26 Dec 2017 00:50:05 +0100 Subject: [PATCH 46/50] Implement singleton interface * Implemented rosrust singleton * Cleaned up logging * Updated README * Finished examples (resolves #12) --- README.md | 270 +++++++++++++++++++------- examples/parameters/src/parameters.rs | 91 ++++----- examples/pubsub/src/publisher.rs | 22 ++- examples/pubsub/src/subscriber.rs | 23 +-- examples/serviceclient/src/client.rs | 23 +-- examples/serviceclient/src/service.rs | 27 ++- rosrust/Cargo.toml | 3 - rosrust/src/api/error.rs | 2 +- rosrust/src/api/logger.rs | 41 ---- rosrust/src/api/mod.rs | 6 +- rosrust/src/api/ros.rs | 23 ++- rosrust/src/api/slave.rs | 6 +- rosrust/src/lib.rs | 9 +- rosrust/src/log_macros.rs | 63 ++++++ rosrust/src/singleton.rs | 136 +++++++++++++ 15 files changed, 516 insertions(+), 229 deletions(-) delete mode 100644 rosrust/src/api/logger.rs create mode 100644 rosrust/src/log_macros.rs create mode 100644 rosrust/src/singleton.rs diff --git a/README.md b/README.md index bcfb827..96bff12 100644 --- a/README.md +++ b/README.md @@ -6,163 +6,283 @@ **rosrust** is a pure Rust implementation of a [ROS](http://www.ros.org/) client library. +## Usage + +The crate heavily uses `serde` for message generation macros, so adding it to the project is advised. +The following set of dependencies is needed for the full set of features: + +```toml +[dependencies] +rosrust = "*" +rosrust_codegen = "*" +serde = "*" +serde_derive = "*" + +[build-dependencies] +rosrust_codegen = "*" +``` + +The build dependency is used for message generation. + ## Implementation -**rosrust** is under development to implement all features of a ROS Client Library by fulfilling [ROS requirements for implementing client libraries](http://wiki.ros.org/Implementing%20Client%20Libraries) which are given in a more detailed form in the [technical overview](http://wiki.ros.org/ROS/Technical%20Overview). +**rosrust** is almost there with implementing all features of a ROS Client Library by fulfilling [ROS requirements for implementing client libraries](http://wiki.ros.org/Implementing%20Client%20Libraries) which are given in a more detailed form in the [technical overview](http://wiki.ros.org/ROS/Technical%20Overview). -Currently implemented features: -* ROS Master API, Parameter Server API and most of ROS Slave API (excluding 2 ambiguous methods) -* Interface for handling parameters -* Publisher, Subscriber, Client, Service fully working (until packet snooping for another undocumented feature is needed) -* Simple bindings for messages, but ROS integration for generating them has yet to be done, and will be part of the catkin integration -* Manual remapping is provided, and CLI parameter remapping is coming! +Mostly the missing features are related to extra tooling, like `sensor_msgs/PointCloud2` and `sensor_msgs/Image` encoding/decoding, TF tree handling, and other things that are external libraries in `roscpp` and `rospy` as well. -There are still quite some features to be implemented for this to become a stable API (for the library user's interface to stop changing). The most important ones are listed as Issues by the repository owner. +The API is very close to the desired final look. Integration with [catkin](http://www.ros.org/wiki/catkin) will be handled once a satisfying set of features has been implemented. ## Examples -The API is far from being stable, but this is the current desired functionality. +The API is close to reaching its final form. + +There are multiple examples in the [examples folder](https://github.com/adnanademovic/rosrust/tree/master/examples). The publisher/subscriber and service/client examples are designed to closely immitate the `roscpp` tutorial. -There is a demo project at [OTL/rosrust_tutorial](https://github.com/OTL/rosrust_tutorial), showing everything that needs to be done (including a tiny tiny `build.rs` script) to get `rosrust` running. +## Features -Most of the examples are followed by an infinite loop. ROS shutdown signal checking is coming! +### Message Generation -### Messages +Message generation is done at build time. If you have ROS installed and sourced in your shell session, you will not need to do any extra setup for this to work. -Message generation is done at build time. In the `build.rs` script, all you need to add is: +To generate messages, create a `build.rs` script in the same folder as your `Cargo.toml` file with the following content: ```rust #[macro_use] -extern crate rosrust; -// If you wanted std_msgs/String, sensor_msgs/Imu -// and all the message types used by them, like geometry_msgs/Vector3 -rosmsg_main!("std_msgs/String", "sensor_msgs/Imu"); +extern crate rosrust_codegen; + +// If you wanted +// * messages: std_msgs/String, sensor_msgs/Imu +// * services: roscpp_tutorials/TwoInts +// * and all the message types used by them, like geometry_msgs/Vector3 +rosmsg_main!("std_msgs/String", "sensor_msgs/Imu", "roscpp_tutorials/TwoInts"); ``` -You need to depend on `rosrust`, `serde`, and `serde_derive`, and have `rosrust` as a build dependency. After that, add this to your main file: +In your main file all you need to add at the top is: ```rust #[macro_use] extern crate rosrust; +#[macro_use] +extern crate rosrust_codegen; + rosmsg_include!(); ``` -This will include all the generated structures, and at them to the `msg` namespace. Thus, to create a new `sensor_msgs/Imu`, you call `msg::sensor_msgs::Imu::new()`. +This will include all the generated structures, and add them to the `msg` namespace. Thus, to create a new `sensor_msgs/Imu`, you call `msg::sensor_msgs::Imu::default()`. All fields are always public, so you can initialize structures as literals. All of the structures implement debug writing, so you can easily inspect their contents. -### Publishing to topic +### Publishing to Topic -If we wanted to publish a defined message (let's use `std_msgs/String`) to topic `some_topic` approximately every second, we can do it in the following way. +If we wanted to publish a defined message (let's use `std_msgs/String`) to topic `chatter` ten times a second, we can do it in the following way. ```rust +#[macro_use] extern crate rosrust; -use rosrust::Ros; -use std::{thread, time}; +#[macro_use] +extern crate rosrust_codegen; rosmsg_include!(); fn main() { - let mut ros = Ros::new("node_name").unwrap(); - let mut publisher = ros.publish("some_topic").unwrap(); - loop { - thread::sleep(time::Duration::from_secs(1)); - let msg = msg::std_msgs::String::new(); - msg.data = String::from("test"); - publisher.send(msg).unwrap(); + // Initialize node + rosrust::init("talker"); + + // Create publisher + let mut chatter_pub = rosrust::publish("chatter").unwrap(); + + let mut count = 0; + + // Create object that maintains 10Hz between sleep requests + let mut rate = rosrust::rate(10.0); + + // Breaks when a shutdown signal is sent + while rosrust::is_ok() { + // Create string message + let mut msg = msg::std_msgs::String::default(); + msg.data = format!("hello world {}", count); + + // Send string message to topic via publisher + chatter_pub.send(msg).unwrap(); + + // Sleep to maintain 10Hz rate + rate.sleep(); + + count += 1; } } ``` -### Subscribing to topic +### Subscribing to Topic If we wanted to subscribe to an `std_msgs/UInt64` topic `some_topic`, we just declare a callback. An alternative extra interface with iterators is being considered, but for now this is the only option. +The constructor creates an object, which represents the subscriber lifetime. +Upon the destruction of this object, the topic is unsubscribed as well. + ```rust +#[macro_use] extern crate rosrust; -use rosrust::Ros; -use std::{thread, time}; +#[macro_use] +extern crate rosrust_codegen; rosmsg_include!(); fn main() { - let mut ros = Ros::new("node_name").unwrap(); - ros.subscribe("some_topic", |v: msg::std_msgs::UInt64| println!("{}", v.data)).unwrap(); - loop { - thread::sleep(time::Duration::from_secs(100)); - } + // Initialize node + rosrust::init("listener"); + + // Create subscriber + // The subscriber is stopped when the returned object is destroyed + let _subscriber_raii = rosrust::subscribe("chatter", |v: msg::std_msgs::UInt64| { + // Callback for handling received messages + ros_info!("Received: {}", v.data); + }).unwrap(); + + // Block the thread until a shutdown signal is received + rosrust::spin(); } ``` -### Creating a service +### Creating a Service -Creating a service is the easiest out of all the options. Just define a callback for each request. Let's use some `AddTwoInts` service on the topic `/add_two_ints`. +Creating a service is the easiest out of all the options. Just define a callback for each request. Let's use the `roscpp_tutorials/AddTwoInts` service on the topic `/add_two_ints`. ```rust +#[macro_use] extern crate rosrust; -use rosrust::Ros; -use std::{thread, time}; +#[macro_use] +extern crate rosrust_codegen; rosmsg_include!(); fn main() { - let mut ros = Ros::new("node_name").unwrap(); - ros.service::("/add_two_ints", - |req| Ok(msg::our_msgs::AddTwoIntsRes { sum: req.a + req.b })).unwrap(); - loop { - thread::sleep(time::Duration::from_secs(100)); - } + // Initialize node + rosrust::init("add_two_ints_server"); + + // Create service + // The service is stopped when the returned object is destroyed + let _service_raii = + rosrust::service::("add_two_ints", move |req| { + // Callback for handling requests + let sum = req.a + req.b; + + // Log each request + ros_info!("{} + {} = {}", req.a, req.b, sum); + + Ok(msg::roscpp_tutorials::TwoIntsRes { sum }) + }).unwrap(); + + // Block the thread until a shutdown signal is received + rosrust::spin(); } + ``` -### Creating a client +### Creating a Client -Clients can handle requests synchronously and asynchronously. The sync method behaves like a function, while the async approach is via reading data afterwards. The async consumes the passed parameter, since we're passing the parameter between threads, and it's more common for users to pass and drop a parameter, so this being the default prevents needless cloning. Let's call requests from the `AddTwoInts` service on the topic `/add_two_ints`. +Clients can handle requests synchronously and asynchronously. The sync method behaves like a function, while the async approach is via reading data afterwards. The async consumes the passed parameter, since we're passing the parameter between threads. It's more common for users to pass and drop a parameter, so this being the default prevents needless cloning. + +Let's call requests from the `AddTwoInts` service on the topic `/add_two_ints`. +The numbers shall be provided as command line arguments. ```rust +#[macro_use] extern crate rosrust; -use rosrust::Ros; -use std::{thread, time}; +#[macro_use] +extern crate rosrust_codegen; + +use std::{env, time}; fn main() { - let ros = Ros::new("node_name").unwrap(); - let client = ros.client::("/add_two_ints").unwrap(); - loop { - // Sync approach - println!("5 + 7 = {}", - client.req(&msg::our_msgs::AddTwoIntsReq { a: 5, b: 7 }).unwrap().unwrap().sum); - // Async approach - let retval = client.req_async(msg::our_msgs::AddTwoIntsReq { a: 5, b: 7 }); - println!("12 + 4 = {}", retval.read().unwrap().unwrap().sum); - thread::sleep(time::Duration::from_secs(1)); + env_logger::init().unwrap(); + + let args: Vec<_> = env::args().collect(); + + if args.len() != 3 { + eprintln!("usage: client X Y"); + return; } + + let a = args[1].parse::().unwrap(); + let b = args[2].parse::().unwrap(); + + // Initialize node + rosrust::init("add_two_ints_client"); + + // Wait ten seconds for the service to appear + rosrust::wait_for_service("add_two_ints", Some(time::Duration::from_secs(10))).unwrap(); + + // Create client for the service + let client = rosrust::client::("add_two_ints").unwrap(); + + // Synchronous call that blocks the thread until a response is received + ros_info!( + "{} + {} = {}", + a, + b, + client + .req(&msg::roscpp_tutorials::TwoIntsReq { a, b }) + .unwrap() + .unwrap() + .sum + ); + + // Asynchronous call that can be resolved later on + let retval = client.req_async(msg::roscpp_tutorials::TwoIntsReq { a, b }); + ros_info!("{} + {} = {}", a, b, retval.read().unwrap().unwrap().sum); } + ``` ### Parameters +There are a lot of methods provided, so we'll just give a taste of all of them here. Get requests return results, so you can use `unwrap_or` to handle defaults. + ```rust +#[macro_use] extern crate rosrust; -use rosrust::Ros; +#[macro_use] +extern crate serde_derive; fn main() { - let ros = Ros::new("node_name").unwrap(); - let param = ros.param("~cow").unwrap(); // access /node_name/cow parameter - println!("Exists? {:?}", param.exists()); // false - param.set(&UInt64 { data: 42 }); - println!("Get {}", param.get::().unwrap().data); // 42 - // XmlRpcValue representing any parameter - println!("Get raw {:?}", param.get_raw().unwrap()); - println!("Search {:?}", param.search().unwrap()); // /node_name/cow - println!("Exists? {}", param.exists().unwrap()); // true - param.delete().unwrap() - println!("Get {:?}", param.get::().unwrap_err()); // fails to find - println!("Exists? {}", param.exists()); //false + env_logger::init().unwrap(); + + // Initialize node + rosrust::init("param_test"); + + // Create parameter, go through all methods, and delete it + let param = rosrust::param("~foo").unwrap(); + ros_info!("Handling ~foo:"); + ros_info!("Exists? {:?}", param.exists()); // false + param.set(&42u64).unwrap(); + ros_info!("Get: {:?}", param.get::().unwrap()); + ros_info!("Get raw: {:?}", param.get_raw().unwrap()); + ros_info!("Search: {:?}", param.search().unwrap()); + ros_info!("Exists? {}", param.exists().unwrap()); + param.delete().unwrap(); + ros_info!("Get {:?}", param.get::().unwrap_err()); + ros_info!("Get with default: {:?}", param.get::().unwrap_or(44u64)); + ros_info!("Exists? {}", param.exists().unwrap()); } ``` +### Logging + +Logging is provided through macros `log_debug!()`, `log_info!()`, `log_warn!()`, `log_error!()`, `log_fatal!()`. + +Setting verbosity levels and throttled logging have yet to come! + +### Command Line Remaps + +Similar to `rospy` and `roscpp`, you can use the command line to remap topics and private parameters. Private parameters should be provided in a YAML format. + +For more information, look at the [official wiki](http://wiki.ros.org/Remapping%20Arguments), since the attempt was to 100% immitate this interface. + +Functions for fetching leftover command line arguments have yet to be implemented! + ## License **rosrust** is distributed under the MIT license. diff --git a/examples/parameters/src/parameters.rs b/examples/parameters/src/parameters.rs index c87af87..a1a7c16 100644 --- a/examples/parameters/src/parameters.rs +++ b/examples/parameters/src/parameters.rs @@ -1,55 +1,55 @@ extern crate env_logger; +#[macro_use] extern crate rosrust; #[macro_use] extern crate serde_derive; -use rosrust::Ros; - fn main() { env_logger::init().unwrap(); - let ros = Ros::new("param_test").unwrap(); + // Initialize node + rosrust::init("param_test"); // Create parameter, go through all methods, and delete it - let param = ros.param("~foo").unwrap(); - println!("Handling ~foo:"); - println!("Exists? {:?}", param.exists()); // false + let param = rosrust::param("~foo").unwrap(); + ros_info!("Handling ~foo:"); + ros_info!("Exists? {:?}", param.exists()); // false param.set(&42u64).unwrap(); - println!("Get: {:?}", param.get::().unwrap()); - println!("Get raw: {:?}", param.get_raw().unwrap()); - println!("Search: {:?}", param.search().unwrap()); - println!("Exists? {}", param.exists().unwrap()); + ros_info!("Get: {:?}", param.get::().unwrap()); + ros_info!("Get raw: {:?}", param.get_raw().unwrap()); + ros_info!("Search: {:?}", param.search().unwrap()); + ros_info!("Exists? {}", param.exists().unwrap()); param.delete().unwrap(); - println!("Get {:?}", param.get::().unwrap_err()); - println!("Exists? {}", param.exists().unwrap()); + ros_info!("Get {:?}", param.get::().unwrap_err()); + ros_info!("Exists? {}", param.exists().unwrap()); // Same as before, but don't delete it - let param = ros.param("bar").unwrap(); - println!("Handling bar:"); + let param = rosrust::param("bar").unwrap(); + ros_info!("Handling bar:"); param.set(&"string data").unwrap(); - println!("Get: {:?}", param.get::().unwrap()); - println!("Get raw: {:?}", param.get_raw().unwrap()); - println!("Search: {:?}", param.search().unwrap()); - println!("Exists? {}", param.exists().unwrap()); + ros_info!("Get: {:?}", param.get::().unwrap()); + ros_info!("Get raw: {:?}", param.get_raw().unwrap()); + ros_info!("Search: {:?}", param.search().unwrap()); + ros_info!("Exists? {}", param.exists().unwrap()); // Access existing parameter - let param = ros.param("/baz").unwrap(); - println!("Handling /baz:"); + let param = rosrust::param("/baz").unwrap(); + ros_info!("Handling /baz:"); if param.exists().unwrap() { - println!("Get raw: {:?}", param.get_raw().unwrap()); - println!("Search: {:?}", param.search().unwrap()); + ros_info!("Get raw: {:?}", param.get_raw().unwrap()); + ros_info!("Search: {:?}", param.search().unwrap()); } else { - println!("Create own parameter /baz with 'rosparam' to observe interaction."); + ros_info!("Create own parameter /baz with 'rosparam' to observe interaction."); } // Access command line parameter - let param = ros.param("~privbaz").unwrap(); - println!("Handling ~privbaz:"); + let param = rosrust::param("~privbaz").unwrap(); + ros_info!("Handling ~privbaz:"); if param.exists().unwrap() { - println!("Get raw: {:?}", param.get_raw().unwrap()); - println!("Search: {:?}", param.search().unwrap()); + ros_info!("Get raw: {:?}", param.get_raw().unwrap()); + ros_info!("Search: {:?}", param.search().unwrap()); } else { - println!("Create ~privbaz by passing _privbaz:=value to observe interaction."); + ros_info!("Create ~privbaz by passing _privbaz:=value to observe interaction."); } #[derive(Debug, Deserialize, Serialize)] @@ -60,20 +60,23 @@ fn main() { } // Create tree and output the end result - println!("Handling /qux:"); - println!("Setting /qux/alpha"); - ros.param("/qux/alpha").unwrap().set(&"meow").unwrap(); - println!("Setting /qux/beta"); - ros.param("/qux/beta").unwrap().set(&44).unwrap(); - println!("Setting /qux/gamma/x"); - ros.param("/qux/gamma/x").unwrap().set(&3.0).unwrap(); - println!("Setting /qux/gamma/y"); - ros.param("/qux/gamma/y").unwrap().set(&2).unwrap(); - println!("Setting /qux/gamma/z"); - ros.param("/qux/gamma/z").unwrap().set(&true).unwrap(); - println!("Setting /qux/delta"); - ros.param("/qux/delta").unwrap().set(&[1, 2, 3]).unwrap(); - ros.param("/qux/epsilon") + ros_info!("Handling /qux:"); + ros_info!("Setting /qux/alpha"); + rosrust::param("/qux/alpha").unwrap().set(&"meow").unwrap(); + ros_info!("Setting /qux/beta"); + rosrust::param("/qux/beta").unwrap().set(&44).unwrap(); + ros_info!("Setting /qux/gamma/x"); + rosrust::param("/qux/gamma/x").unwrap().set(&3.0).unwrap(); + ros_info!("Setting /qux/gamma/y"); + rosrust::param("/qux/gamma/y").unwrap().set(&2).unwrap(); + ros_info!("Setting /qux/gamma/z"); + rosrust::param("/qux/gamma/z").unwrap().set(&true).unwrap(); + ros_info!("Setting /qux/delta"); + rosrust::param("/qux/delta") + .unwrap() + .set(&[1, 2, 3]) + .unwrap(); + rosrust::param("/qux/epsilon") .unwrap() .set(&TestStruct { foo: "x".into(), @@ -81,8 +84,8 @@ fn main() { baz: false, }) .unwrap(); - println!( + ros_info!( "Get raw: {:?}", - ros.param("/qux").unwrap().get_raw().unwrap() + rosrust::param("/qux").unwrap().get_raw().unwrap() ); } diff --git a/examples/pubsub/src/publisher.rs b/examples/pubsub/src/publisher.rs index 97cc399..b09b400 100644 --- a/examples/pubsub/src/publisher.rs +++ b/examples/pubsub/src/publisher.rs @@ -4,27 +4,35 @@ extern crate rosrust; #[macro_use] extern crate rosrust_codegen; -use rosrust::Ros; - rosmsg_include!(); fn main() { env_logger::init().unwrap(); - let mut ros = Ros::new("talker").unwrap(); - let mut chatter_pub = ros.publish("chatter").unwrap(); + // Initialize node + rosrust::init("talker"); + + // Create publisher + let mut chatter_pub = rosrust::publish("chatter").unwrap(); let mut count = 0; - let mut rate = ros.rate(10.0); - while ros.is_ok() { + // Create object that maintains 10Hz between sleep requests + let mut rate = rosrust::rate(10.0); + + // Breaks when a shutdown signal is sent + while rosrust::is_ok() { + // Create string message let mut msg = msg::std_msgs::String::default(); msg.data = format!("hello world {}", count); - ros_info!(ros, msg.data); + // Log event + ros_info!("Publishing: {}", msg.data); + // Send string message to topic via publisher chatter_pub.send(msg).unwrap(); + // Sleep to maintain 10Hz rate rate.sleep(); count += 1; diff --git a/examples/pubsub/src/subscriber.rs b/examples/pubsub/src/subscriber.rs index 732257e..50c86d2 100644 --- a/examples/pubsub/src/subscriber.rs +++ b/examples/pubsub/src/subscriber.rs @@ -4,26 +4,21 @@ extern crate rosrust; #[macro_use] extern crate rosrust_codegen; -use rosrust::Ros; -use std::sync::{Arc, Mutex}; - rosmsg_include!(); fn main() { env_logger::init().unwrap(); - let ros = Arc::new(Mutex::new(Ros::new("listener").unwrap())); - let ros_thread = Arc::clone(&ros); + // Initialize node + rosrust::init("listener"); + // Create subscriber // The subscriber is stopped when the returned object is destroyed - let _subscriber_raii = ros.lock() - .unwrap() - .subscribe("chatter", move |v: msg::std_msgs::String| { - let mut ros = ros_thread.lock().unwrap(); - println!("{}", v.data); - ros_info!(ros, v.data); - }) - .unwrap(); + let _subscriber_raii = rosrust::subscribe("chatter", |v: msg::std_msgs::String| { + // Callback for handling received messages + ros_info!("Received: {}", v.data); + }).unwrap(); - let spin = { ros.lock().unwrap().spin() }; + // Block the thread until a shutdown signal is received + rosrust::spin(); } diff --git a/examples/serviceclient/src/client.rs b/examples/serviceclient/src/client.rs index 36d2bfc..ffee2f2 100644 --- a/examples/serviceclient/src/client.rs +++ b/examples/serviceclient/src/client.rs @@ -1,9 +1,9 @@ extern crate env_logger; +#[macro_use] extern crate rosrust; #[macro_use] extern crate rosrust_codegen; -use rosrust::Ros; use std::{env, time}; rosmsg_include!(); @@ -14,23 +14,24 @@ fn main() { let args: Vec<_> = env::args().collect(); if args.len() != 3 { - println!("usage: client X Y"); + eprintln!("usage: client X Y"); return; } let a = args[1].parse::().unwrap(); let b = args[2].parse::().unwrap(); - let ros = Ros::new("add_two_ints_client").unwrap(); + // Initialize node + rosrust::init("add_two_ints_client"); - ros.wait_for_service("add_two_ints", Some(time::Duration::from_secs(10))) - .unwrap(); + // Wait ten seconds for the service to appear + rosrust::wait_for_service("add_two_ints", Some(time::Duration::from_secs(10))).unwrap(); - let client = ros.client::("add_two_ints") - .unwrap(); + // Create client for the service + let client = rosrust::client::("add_two_ints").unwrap(); - // Sync approach - println!( + // Synchronous call that blocks the thread until a response is received + ros_info!( "{} + {} = {}", a, b, @@ -41,7 +42,7 @@ fn main() { .sum ); - // Async approach + // Asynchronous call that can be resolved later on let retval = client.req_async(msg::roscpp_tutorials::TwoIntsReq { a, b }); - println!("{} + {} = {}", a, b, retval.read().unwrap().unwrap().sum); + ros_info!("{} + {} = {}", a, b, retval.read().unwrap().unwrap().sum); } diff --git a/examples/serviceclient/src/service.rs b/examples/serviceclient/src/service.rs index 60cc7db..dd49d64 100644 --- a/examples/serviceclient/src/service.rs +++ b/examples/serviceclient/src/service.rs @@ -4,28 +4,27 @@ extern crate rosrust; #[macro_use] extern crate rosrust_codegen; -use rosrust::Ros; -use std::sync::{Arc, Mutex}; - rosmsg_include!(); fn main() { env_logger::init().unwrap(); - let ros = Arc::new(Mutex::new(Ros::new("add_two_ints_server").unwrap())); - - let ros_thread = Arc::clone(&ros); + // Initialize node + rosrust::init("add_two_ints_server"); + // Create service // The service is stopped when the returned object is destroyed - let _service_raii = ros.lock() - .unwrap() - .service::("add_two_ints", move |req| { - let mut ros = ros_thread.lock().unwrap(); + let _service_raii = + rosrust::service::("add_two_ints", move |req| { + // Callback for handling requests let sum = req.a + req.b; - ros_info!(ros, format!("{} + {} = {}", req.a, req.b, sum)); + + // Log each request + ros_info!("{} + {} = {}", req.a, req.b, sum); + Ok(msg::roscpp_tutorials::TwoIntsRes { sum }) - }) - .unwrap(); + }).unwrap(); - let spin = { ros.lock().unwrap().spin() }; + // Block the thread until a shutdown signal is received + rosrust::spin(); } diff --git a/rosrust/Cargo.toml b/rosrust/Cargo.toml index f97c7cc..80a3cf3 100644 --- a/rosrust/Cargo.toml +++ b/rosrust/Cargo.toml @@ -11,13 +11,10 @@ version = "0.5.1" byteorder = "1.2.1" error-chain = "0.11.0" futures = "0.1.17" -itertools = "0.7.4" lazy_static = "1.0.0" log = "0.3.8" nix = "0.9.0" -regex = "0.2.3" rosrust_codegen = "0.0.1" -rust-crypto = "0.2.36" serde = "1.0.24" serde_derive = "1.0.24" serde_rosmsg = "0.2.0" diff --git a/rosrust/src/api/error.rs b/rosrust/src/api/error.rs index ee55b1f..c809b4f 100644 --- a/rosrust/src/api/error.rs +++ b/rosrust/src/api/error.rs @@ -20,7 +20,7 @@ error_chain! { description("Could not add duplicate") display("Could not add duplicate {}", t) } - TimeoutError + TimeoutError } } diff --git a/rosrust/src/api/logger.rs b/rosrust/src/api/logger.rs deleted file mode 100644 index dda7bab..0000000 --- a/rosrust/src/api/logger.rs +++ /dev/null @@ -1,41 +0,0 @@ -#[macro_export] -macro_rules! ros_log { - ($ros:ident, $level:expr, $msg:expr) => { - $ros.log($level, &$msg, file!(), line!()); - } -} - -#[macro_export] -macro_rules! ros_debug { - ($ros:ident, $msg:expr) => { - ros_log!($ros, $crate::msg::rosgraph_msgs::Log::DEBUG, $msg); - } -} - -#[macro_export] -macro_rules! ros_info { - ($ros:ident, $msg:expr) => { - ros_log!($ros, $crate::msg::rosgraph_msgs::Log::INFO, $msg); - } -} - -#[macro_export] -macro_rules! ros_warn { - ($ros:ident, $msg:expr) => { - ros_log!($ros, $crate::msg::rosgraph_msgs::Log::WARN, $msg); - } -} - -#[macro_export] -macro_rules! ros_err { - ($ros:ident, $msg:expr) => { - ros_log!($ros, $crate::msg::rosgraph_msgs::Log::ERROR, $msg); - } -} - -#[macro_export] -macro_rules! ros_fatal { - ($ros:ident, $msg:expr) => { - ros_log!($ros, $crate::msg::rosgraph_msgs::Log::FATAL, $msg); - } -} diff --git a/rosrust/src/api/mod.rs b/rosrust/src/api/mod.rs index 90f3ac5..bcde975 100644 --- a/rosrust/src/api/mod.rs +++ b/rosrust/src/api/mod.rs @@ -1,11 +1,11 @@ -pub use self::ros::Ros; +pub use self::ros::{Parameter, Ros}; pub use self::clock::{Clock, Rate}; +pub use self::master::{SystemState, Topic}; mod clock; pub mod error; -pub mod logger; mod master; -mod raii; +pub mod raii; mod resolve; mod ros; mod slave; diff --git a/rosrust/src/api/ros.rs b/rosrust/src/api/ros.rs index bdbf873..fbb6ce0 100644 --- a/rosrust/src/api/ros.rs +++ b/rosrust/src/api/ros.rs @@ -150,9 +150,9 @@ impl Ros { } } - pub fn param<'a, 'b>(&'a self, name: &'b str) -> Option> { + pub fn param(&self, name: &str) -> Option { self.resolver.translate(name).ok().map(|v| Parameter { - master: &self.master, + master: Arc::clone(&self.master), name: v, }) } @@ -247,20 +247,27 @@ impl Ros { ) } - pub fn log(&mut self, level: i8, msg: &str, file: &str, line: u32) { + pub fn log(&mut self, level: i8, msg: String, file: &str, line: u32) { let logger = &mut match self.logger { Some(ref mut v) => v, None => return, }; + let topics = self.slave + .publications + .lock() + .expect("Failed to acquire lock on mutex") + .keys() + .cloned() + .collect(); let message = Log { header: Header::default(), level, - msg: msg.into(), + msg, name: self.name.clone(), line, file: file.into(), function: String::default(), - topics: vec![], + topics, }; if let Err(err) = logger.send(message) { error!("Logging error: {}", err); @@ -268,12 +275,12 @@ impl Ros { } } -pub struct Parameter<'a> { - master: &'a Master, +pub struct Parameter { + master: Arc, name: String, } -impl<'a> Parameter<'a> { +impl Parameter { pub fn name(&self) -> &str { &self.name } diff --git a/rosrust/src/api/slave.rs b/rosrust/src/api/slave.rs index d2a7311..04c7727 100644 --- a/rosrust/src/api/slave.rs +++ b/rosrust/src/api/slave.rs @@ -10,9 +10,9 @@ use tcpros::{Message, Publisher, PublisherStream, Service, ServicePair, ServiceR pub struct Slave { name: String, uri: String, - publications: Arc>>, - subscriptions: Arc>>, - services: Arc>>, + pub publications: Arc>>, + pub subscriptions: Arc>>, + pub services: Arc>>, } type SerdeResult = Result; diff --git a/rosrust/src/lib.rs b/rosrust/src/lib.rs index 1379208..5a0c862 100644 --- a/rosrust/src/lib.rs +++ b/rosrust/src/lib.rs @@ -1,18 +1,14 @@ #![recursion_limit = "1024"] extern crate byteorder; -extern crate crypto; #[macro_use] extern crate error_chain; extern crate futures; -extern crate itertools; -#[cfg(test)] #[macro_use] extern crate lazy_static; #[macro_use] extern crate log; extern crate nix; -extern crate regex; #[macro_use] extern crate rosrust_codegen; extern crate serde; @@ -20,12 +16,15 @@ extern crate serde_rosmsg; extern crate xml_rpc; extern crate yaml_rust; -pub use api::{error, logger, Clock, Ros}; +pub use api::{error, Clock, Parameter}; +pub use singleton::*; pub use tcpros::{Client, Message, PublisherStream, ServicePair as Service}; pub use time::{Duration, Time}; mod api; +mod log_macros; mod rosxmlrpc; +pub mod singleton; mod tcpros; mod time; diff --git a/rosrust/src/log_macros.rs b/rosrust/src/log_macros.rs new file mode 100644 index 0000000..9a33dbb --- /dev/null +++ b/rosrust/src/log_macros.rs @@ -0,0 +1,63 @@ +#[macro_export] +macro_rules! ros_log { + ($level:expr, $($arg:tt)+) => { + #[allow(unknown_lints, useless_format)] + let msg = format!($($arg)*); + match $level { + $crate::msg::rosgraph_msgs::Log::DEBUG => { + println!("[DEBUG @ {}:{}]: {}", file!(), line!(), msg); + } + $crate::msg::rosgraph_msgs::Log::INFO => { + println!("[INFO @ {}:{}]: {}", file!(), line!(), msg); + } + $crate::msg::rosgraph_msgs::Log::WARN => { + eprintln!("[WARN @ {}:{}]: {}", file!(), line!(), msg); + + } + $crate::msg::rosgraph_msgs::Log::ERROR => { + eprintln!("[ERROR @ {}:{}]: {}", file!(), line!(), msg); + + } + $crate::msg::rosgraph_msgs::Log::FATAL => { + eprintln!("[FATAL @ {}:{}]: {}", file!(), line!(), msg); + } + _ => {} + } + $crate::log($level, msg, file!(), line!()); + } +} + +#[macro_export] +macro_rules! ros_debug { + ($($arg:tt)*) => { + ros_log!($crate::msg::rosgraph_msgs::Log::DEBUG, $($arg)*); + } +} + +#[macro_export] +macro_rules! ros_info { + ($($arg:tt)*) => { + ros_log!($crate::msg::rosgraph_msgs::Log::INFO, $($arg)*); + } +} + +#[macro_export] +macro_rules! ros_warn { + ($($arg:tt)*) => { + ros_log!($crate::msg::rosgraph_msgs::Log::WARN, $($arg)*); + } +} + +#[macro_export] +macro_rules! ros_err { + ($($arg:tt)*) => { + ros_log!($crate::msg::rosgraph_msgs::Log::ERROR, $($arg)*); + } +} + +#[macro_export] +macro_rules! ros_fatal { + ($($arg:tt)*) => { + ros_log!($crate::msg::rosgraph_msgs::Log::FATAL, $($arg)*); + } +} diff --git a/rosrust/src/singleton.rs b/rosrust/src/singleton.rs new file mode 100644 index 0000000..76d881c --- /dev/null +++ b/rosrust/src/singleton.rs @@ -0,0 +1,136 @@ +use api::{Parameter, Rate, Ros, SystemState, Topic}; +use api::raii::{Publisher, Service, Subscriber}; +use error::Result; +use rosxmlrpc::Response; +use tcpros::{Client, Message, ServicePair, ServiceResult}; +use time::{Duration, Time}; +use std::sync::Mutex; +use std::time; + +lazy_static! { + static ref ROS: Mutex> = Mutex::new(None); +} + +#[inline] +pub fn init(name: &str) { + try_init(name).unwrap(); +} + +pub fn try_init(name: &str) -> Result<()> { + let mut ros = ROS.lock().expect(LOCK_FAIL); + if ros.is_some() { + bail!(INITIALIZED); + } + *ros = Some(Ros::new(name)?); + Ok(()) +} + +macro_rules! ros { + () => {ROS.lock().expect(LOCK_FAIL).as_mut().expect(UNINITIALIZED)} +} + +#[inline] +pub fn uri() -> String { + ros!().uri().into() +} + +#[inline] +pub fn name() -> String { + ros!().name().into() +} + +#[inline] +pub fn hostname() -> String { + ros!().hostname().into() +} + +#[inline] +pub fn now() -> Time { + ros!().now() +} + +#[inline] +pub fn sleep(d: Duration) { + ros!().sleep(d) +} + +#[inline] +pub fn rate(rate: f64) -> Rate { + ros!().rate(rate) +} + +#[inline] +pub fn is_ok() -> bool { + ros!().is_ok() +} + +#[inline] +pub fn spin() { + // Spinner drop locks the thread until a kill request is received + let _spinner = { ros!().spin() }; +} + +#[inline] +pub fn param(name: &str) -> Option { + ros!().param(name) +} + +#[inline] +pub fn parameters() -> Response> { + ros!().parameters() +} + +#[inline] +pub fn state() -> Response { + ros!().state() +} + +#[inline] +pub fn topics() -> Response> { + ros!().topics() +} + +#[inline] +pub fn client(service: &str) -> Result> { + ros!().client::(service) +} + +#[inline] +pub fn wait_for_service(service: &str, timeout: Option) -> Result<()> { + ros!().wait_for_service(service, timeout) +} + +#[inline] +pub fn service(service: &str, handler: F) -> Result +where + T: ServicePair, + F: Fn(T::Request) -> ServiceResult + Send + Sync + 'static, +{ + ros!().service::(service, handler) +} + +#[inline] +pub fn subscribe(topic: &str, callback: F) -> Result +where + T: Message, + F: Fn(T) -> () + Send + 'static, +{ + ros!().subscribe::(topic, callback) +} + +#[inline] +pub fn publish(topic: &str) -> Result> +where + T: Message, +{ + ros!().publish::(topic) +} + +#[inline] +pub fn log(level: i8, msg: String, file: &str, line: u32) { + ros!().log(level, msg, file, line) +} + +static LOCK_FAIL: &str = "Failed to acquire lock on ROS instance."; +static UNINITIALIZED: &str = "ROS uninitialized. Please run ros::init(name) first!"; +static INITIALIZED: &str = "ROS initialized multiple times through ros::init."; From 296a881db2f4097e3f9be0c42c28ef9c73471076 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 26 Dec 2017 01:21:15 +0100 Subject: [PATCH 47/50] Handle leftover args --- README.md | 5 +-- examples/serviceclient/src/client.rs | 5 +-- rosrust/src/api/mod.rs | 2 +- rosrust/src/api/resolve.rs | 48 ++++++++++++++++++++++++++++ rosrust/src/singleton.rs | 6 ++++ 5 files changed, 61 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 96bff12..fae3c00 100644 --- a/README.md +++ b/README.md @@ -199,7 +199,8 @@ use std::{env, time}; fn main() { env_logger::init().unwrap(); - let args: Vec<_> = env::args().collect(); + // Fetch args that are not meant for rosrust + let args: Vec<_> = rosrust::args(); if args.len() != 3 { eprintln!("usage: client X Y"); @@ -281,7 +282,7 @@ Similar to `rospy` and `roscpp`, you can use the command line to remap topics an For more information, look at the [official wiki](http://wiki.ros.org/Remapping%20Arguments), since the attempt was to 100% immitate this interface. -Functions for fetching leftover command line arguments have yet to be implemented! +You can get a vector of the leftover command line argument strings with `rosrust::args()`, allowing easy argument parsing. This includes the first argument, the application name. ## License diff --git a/examples/serviceclient/src/client.rs b/examples/serviceclient/src/client.rs index ffee2f2..7029e5e 100644 --- a/examples/serviceclient/src/client.rs +++ b/examples/serviceclient/src/client.rs @@ -4,14 +4,15 @@ extern crate rosrust; #[macro_use] extern crate rosrust_codegen; -use std::{env, time}; +use std::time; rosmsg_include!(); fn main() { env_logger::init().unwrap(); - let args: Vec<_> = env::args().collect(); + // Fetch args that are not meant for rosrust + let args: Vec<_> = rosrust::args(); if args.len() != 3 { eprintln!("usage: client X Y"); diff --git a/rosrust/src/api/mod.rs b/rosrust/src/api/mod.rs index bcde975..70235c6 100644 --- a/rosrust/src/api/mod.rs +++ b/rosrust/src/api/mod.rs @@ -6,7 +6,7 @@ mod clock; pub mod error; mod master; pub mod raii; -mod resolve; +pub mod resolve; mod ros; mod slave; mod slavehandler; diff --git a/rosrust/src/api/resolve.rs b/rosrust/src/api/resolve.rs index 9a5a8ba..0eba31b 100644 --- a/rosrust/src/api/resolve.rs +++ b/rosrust/src/api/resolve.rs @@ -69,6 +69,19 @@ pub fn params() -> Vec<(String, String)> { .collect() } +pub fn get_unused_args() -> Vec { + args() + .enumerate() + .filter_map(|(idx, v)| { + if idx == 0 || !v.contains(":=") { + Some(v) + } else { + None + } + }) + .collect() +} + fn find_with_prefix(prefix: &str) -> Option { args() .skip(1) @@ -96,11 +109,13 @@ fn system_hostname() -> String { } #[cfg(not(test))] +#[inline] fn args() -> std::env::Args { env::args() } #[cfg(test)] +#[inline] fn args() -> std::vec::IntoIter { tests::args_mock() } @@ -205,6 +220,39 @@ mod tests { ); } + #[test] + #[allow(unused_variables)] + fn get_unused_args_gets_everything_without_equal_sign() { + let testcase = TESTCASE.lock().expect(FAILED_TO_LOCK); + set_args(&vec![]); + assert_eq!(vec![String::from("IGNORE")], get_unused_args()); + set_args(&vec![ + "a:=x", + "b=e", + "/c:=d", + "this", + "e:=/f_g", + "__name:=something", + "_param:=something", + "a:=b:=c", + "foo", + "~oo_e:=/ab_c", + "_foo:=123:=456", + "bar=:baz", + "/x_y:=~i", + ]); + assert_eq!( + vec![ + String::from("IGNORE"), + String::from("b=e"), + String::from("this"), + String::from("foo"), + String::from("bar=:baz"), + ], + get_unused_args() + ); + } + #[test] #[allow(unused_variables)] fn name_uses_passed_value_by_default() { diff --git a/rosrust/src/singleton.rs b/rosrust/src/singleton.rs index 76d881c..faf7a0e 100644 --- a/rosrust/src/singleton.rs +++ b/rosrust/src/singleton.rs @@ -1,5 +1,6 @@ use api::{Parameter, Rate, Ros, SystemState, Topic}; use api::raii::{Publisher, Service, Subscriber}; +use api::resolve::get_unused_args; use error::Result; use rosxmlrpc::Response; use tcpros::{Client, Message, ServicePair, ServiceResult}; @@ -29,6 +30,11 @@ macro_rules! ros { () => {ROS.lock().expect(LOCK_FAIL).as_mut().expect(UNINITIALIZED)} } +#[inline] +pub fn args() -> Vec { + get_unused_args() +} + #[inline] pub fn uri() -> String { ros!().uri().into() From f4297d8254127c71f92c8028aaa26a7a5a229014 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 26 Dec 2017 01:33:13 +0100 Subject: [PATCH 48/50] Bump versions --- README.md | 10 +++++----- examples/parameters/Cargo.toml | 10 +++------- examples/pubsub/Cargo.toml | 11 +++++------ examples/serviceclient/Cargo.toml | 11 +++++------ rosrust/Cargo.toml | 12 ++++++------ rosrust_codegen/Cargo.toml | 2 +- 6 files changed, 25 insertions(+), 31 deletions(-) diff --git a/README.md b/README.md index fae3c00..24fc763 100644 --- a/README.md +++ b/README.md @@ -13,13 +13,13 @@ The following set of dependencies is needed for the full set of features: ```toml [dependencies] -rosrust = "*" -rosrust_codegen = "*" -serde = "*" -serde_derive = "*" +rosrust = "0.6.0" +rosrust_codegen = "0.6.0" +serde = "1.0.25" +serde_derive = "1.0.25" [build-dependencies] -rosrust_codegen = "*" +rosrust_codegen = "0.6.0" ``` The build dependency is used for message generation. diff --git a/examples/parameters/Cargo.toml b/examples/parameters/Cargo.toml index 00bb5e0..02323d3 100644 --- a/examples/parameters/Cargo.toml +++ b/examples/parameters/Cargo.toml @@ -7,14 +7,10 @@ version = "0.0.1" [dependencies] env_logger = "0.4.3" -rosrust = "*" -serde = "1.0.24" -serde_derive = "1.0.24" +rosrust = "0.6.0" +serde = "1.0.25" +serde_derive = "1.0.25" [[bin]] name = "parameters" path = "src/parameters.rs" - -[build-dependencies] -env_logger = "0.4.3" -rosrust = "*" diff --git a/examples/pubsub/Cargo.toml b/examples/pubsub/Cargo.toml index 4845556..dfbc94f 100644 --- a/examples/pubsub/Cargo.toml +++ b/examples/pubsub/Cargo.toml @@ -7,10 +7,10 @@ version = "0.0.1" [dependencies] env_logger = "0.4.3" -rosrust = "*" -rosrust_codegen = "*" -serde = "1.0.24" -serde_derive = "1.0.24" +rosrust = "0.6.0" +rosrust_codegen = "0.6.0" +serde = "1.0.25" +serde_derive = "1.0.25" [[bin]] name = "publisher" @@ -21,5 +21,4 @@ name = "subscriber" path = "src/subscriber.rs" [build-dependencies] -env_logger = "0.4.3" -rosrust_codegen = "*" +rosrust_codegen = "0.6.0" diff --git a/examples/serviceclient/Cargo.toml b/examples/serviceclient/Cargo.toml index b22112b..ff29c8a 100644 --- a/examples/serviceclient/Cargo.toml +++ b/examples/serviceclient/Cargo.toml @@ -7,10 +7,10 @@ version = "0.0.1" [dependencies] env_logger = "0.4.3" -rosrust = "*" -rosrust_codegen = "*" -serde = "1.0.24" -serde_derive = "1.0.24" +rosrust = "0.6.0" +rosrust_codegen = "0.6.0" +serde = "1.0.25" +serde_derive = "1.0.25" [[bin]] name = "client" @@ -21,5 +21,4 @@ name = "service" path = "src/service.rs" [build-dependencies] -env_logger = "0.4.3" -rosrust_codegen = "*" +rosrust_codegen = "0.6.0" diff --git a/rosrust/Cargo.toml b/rosrust/Cargo.toml index 80a3cf3..eec8f34 100644 --- a/rosrust/Cargo.toml +++ b/rosrust/Cargo.toml @@ -5,21 +5,21 @@ license = "MIT" name = "rosrust" readme = "README.md" repository = "https://github.com/adnanademovic/rosrust" -version = "0.5.1" +version = "0.6.0" [dependencies] byteorder = "1.2.1" error-chain = "0.11.0" futures = "0.1.17" lazy_static = "1.0.0" -log = "0.3.8" +log = "0.4.0" nix = "0.9.0" -rosrust_codegen = "0.0.1" -serde = "1.0.24" -serde_derive = "1.0.24" +rosrust_codegen = "0.6.0" +serde = "1.0.25" +serde_derive = "1.0.25" serde_rosmsg = "0.2.0" xml-rpc = "0.0.8" yaml-rust = "0.4.0" [build-dependencies] -rosrust_codegen = "0.0.1" +rosrust_codegen = "0.6.0" diff --git a/rosrust_codegen/Cargo.toml b/rosrust_codegen/Cargo.toml index e1af7dc..d1f6ce0 100644 --- a/rosrust_codegen/Cargo.toml +++ b/rosrust_codegen/Cargo.toml @@ -4,7 +4,7 @@ description = "Message generation for rosrust" license = "MIT" name = "rosrust_codegen" repository = "https://github.com/adnanademovic/rosrust" -version = "0.0.1" +version = "0.6.0" [dependencies] error-chain = "0.11.0" From 1af1e2f43f6e15894f2e709f575c19f5c151795d Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 26 Dec 2017 01:35:54 +0100 Subject: [PATCH 49/50] Make leading namespace slash optional --- rosrust/src/api/ros.rs | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rosrust/src/api/ros.rs b/rosrust/src/api/ros.rs index fbb6ce0..10b6143 100644 --- a/rosrust/src/api/ros.rs +++ b/rosrust/src/api/ros.rs @@ -30,7 +30,10 @@ pub struct Ros { impl Ros { pub fn new(name: &str) -> Result { - let namespace = resolve::namespace(); + let mut namespace = resolve::namespace(); + if !namespace.starts_with('/') { + namespace = format!("/{}", namespace); + } let master_uri = resolve::master(); let hostname = resolve::hostname(); let name = resolve::name(name); From 1ad9cf6d91caefb8c1fb67620c24f0a5b4a2d121 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 26 Dec 2017 01:38:51 +0100 Subject: [PATCH 50/50] Update README location --- rosrust/Cargo.toml | 2 +- rosrust_codegen/Cargo.toml | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/rosrust/Cargo.toml b/rosrust/Cargo.toml index eec8f34..fd8af95 100644 --- a/rosrust/Cargo.toml +++ b/rosrust/Cargo.toml @@ -3,7 +3,7 @@ authors = ["Adnan Ademovic "] description = "Pure Rust implementation of a ROS client library" license = "MIT" name = "rosrust" -readme = "README.md" +readme = "../README.md" repository = "https://github.com/adnanademovic/rosrust" version = "0.6.0" diff --git a/rosrust_codegen/Cargo.toml b/rosrust_codegen/Cargo.toml index d1f6ce0..90f54ea 100644 --- a/rosrust_codegen/Cargo.toml +++ b/rosrust_codegen/Cargo.toml @@ -3,6 +3,7 @@ authors = ["Adnan Ademovic "] description = "Message generation for rosrust" license = "MIT" name = "rosrust_codegen" +readme = "../README.md" repository = "https://github.com/adnanademovic/rosrust" version = "0.6.0"