diff --git a/README.md b/README.md index 00454ca..e4cae43 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ Only [link](http://wiki.ros.org/urdf/XML/link) and [joint](http://wiki.ros.org/u You can access urdf elements like below example. ```rust -let urdf_robo = urdf_rs::read_file("sample.urdf").unwrap(); +let urdf_robo = urdf_rs::urdf::read_file("samples/sample.urdf").unwrap(); let links = urdf_robo.links; println!("{:?}", links[0].visual[0].origin.xyz); let joints = urdf_robo.joints; diff --git a/samples/minimal.sdf b/samples/minimal.sdf new file mode 100644 index 0000000..27ee29c --- /dev/null +++ b/samples/minimal.sdf @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/sample.urdf b/samples/sample.urdf similarity index 100% rename from sample.urdf rename to samples/sample.urdf diff --git a/src/common.rs b/src/common.rs new file mode 100644 index 0000000..a4c3338 --- /dev/null +++ b/src/common.rs @@ -0,0 +1,5 @@ +mod elements; +pub use elements::*; + +mod space_seperated_vectors; +pub use space_seperated_vectors::*; diff --git a/src/common/elements.rs b/src/common/elements.rs new file mode 100644 index 0000000..e9c861a --- /dev/null +++ b/src/common/elements.rs @@ -0,0 +1,38 @@ +use crate::common::space_seperated_vectors::*; +use serde::Deserialize; + +#[derive(Debug, Deserialize, Default, Clone)] +pub struct Mass { + pub value: f64, +} + +#[derive(Deserialize, Debug, Clone)] +pub struct Vec3 { + #[serde(with = "ss_vec3")] + pub data: [f64; 3], +} + +#[derive(Debug, Deserialize, Default, Clone)] +pub struct ColorRGBA { + #[serde(with = "ss_vec4")] + pub rgba: [f64; 4], +} + +#[derive(Debug, Deserialize, Default, Clone)] +pub struct ColorRGB { + #[serde(with = "ss_vec3")] + pub rgba: [f64; 3], +} + +#[derive(Debug, Deserialize, Clone)] +pub struct Texture { + pub filename: String, +} + +impl Default for Texture { + fn default() -> Texture { + Texture { + filename: "".to_string(), + } + } +} diff --git a/src/common/space_seperated_vectors.rs b/src/common/space_seperated_vectors.rs new file mode 100644 index 0000000..b7d3f5e --- /dev/null +++ b/src/common/space_seperated_vectors.rs @@ -0,0 +1,68 @@ +pub mod ss_vec3 { + use serde::{self, Deserialize, Deserializer}; + pub fn deserialize<'a, D>(deserializer: D) -> Result<[f64; 3], D::Error> + where + D: Deserializer<'a>, + { + let s = String::deserialize(deserializer)?; + let vec = s + .split(' ') + .filter_map(|x| x.parse::().ok()) + .collect::>(); + if vec.len() != 3 { + return Err(serde::de::Error::custom(format!( + "failed to parse float array in {s}" + ))); + } + let mut arr = [0.0f64; 3]; + arr.copy_from_slice(&vec); + Ok(arr) + } +} + +pub mod ss_option_vec3 { + use serde::{self, Deserialize, Deserializer}; + pub fn deserialize<'a, D>(deserializer: D) -> Result, D::Error> + where + D: Deserializer<'a>, + { + let s = String::deserialize(deserializer)?; + let vec = s + .split(' ') + .filter_map(|x| x.parse::().ok()) + .collect::>(); + if vec.is_empty() { + Ok(None) + } else if vec.len() == 3 { + let mut arr = [0.0; 3]; + arr.copy_from_slice(&vec); + Ok(Some(arr)) + } else { + Err(serde::de::Error::custom(format!( + "failed to parse float array in {s}" + ))) + } + } +} + +pub mod ss_vec4 { + use serde::{self, Deserialize, Deserializer}; + pub fn deserialize<'a, D>(deserializer: D) -> Result<[f64; 4], D::Error> + where + D: Deserializer<'a>, + { + let s = String::deserialize(deserializer)?; + let vec = s + .split(' ') + .filter_map(|x| x.parse::().ok()) + .collect::>(); + if vec.len() != 4 { + return Err(serde::de::Error::custom(format!( + "failed to parse float array in {s}" + ))); + } + let mut arr = [0.0f64; 4]; + arr.copy_from_slice(&vec); + Ok(arr) + } +} diff --git a/src/lib.rs b/src/lib.rs index a8e2916..7064938 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,13 +1,8 @@ #![doc = include_str!("../README.md")] #![warn(missing_debug_implementations, rust_2018_idioms)] +mod common; mod errors; -pub use errors::*; - -mod deserialize; -pub use deserialize::*; - -mod funcs; -pub use funcs::*; - -pub mod utils; +pub mod ros_utils; +pub mod sdf; +pub mod urdf; diff --git a/src/utils.rs b/src/ros_utils.rs similarity index 92% rename from src/utils.rs rename to src/ros_utils.rs index 3686387..c66c15e 100644 --- a/src/utils.rs +++ b/src/ros_utils.rs @@ -1,6 +1,9 @@ -use crate::deserialize::Robot; +//! ROS-specific functions that make use of `rosrun` and `rospack` as +//! subprocesses. + use crate::errors::*; -use crate::funcs::*; +use crate::urdf::Robot; +use crate::urdf::{read_file, read_from_string}; use once_cell::sync::Lazy; use regex::Regex; @@ -91,6 +94,6 @@ fn it_works() { expand_package_path("/home/aaa.obj", Some(Path::new(""))), "/home/aaa.obj" ); - assert!(read_urdf_or_xacro("sample.urdf").is_ok()); + assert!(read_urdf_or_xacro("samples/sample.urdf").is_ok()); assert!(read_urdf_or_xacro("sample_urdf").is_err()); } diff --git a/src/sdf.rs b/src/sdf.rs new file mode 100644 index 0000000..3bf8fb5 --- /dev/null +++ b/src/sdf.rs @@ -0,0 +1,8 @@ +mod deserialize; +pub use deserialize::*; + +mod funcs; +pub use funcs::*; + +pub use crate::errors::*; +pub type SdfError = UrdfError; diff --git a/src/sdf/deserialize.rs b/src/sdf/deserialize.rs new file mode 100644 index 0000000..eabd880 --- /dev/null +++ b/src/sdf/deserialize.rs @@ -0,0 +1,214 @@ +// use crate::common::*; +use serde::Deserialize; + +// #[derive(Debug, Deserialize, Clone)] +// #[serde(rename_all = "snake_case")] +// pub enum Geometry { +// Empty, +// Box { +// #[serde(with = "ss_vec3")] +// size: [f64; 3], +// }, +// Capsule { +// radius: f64, +// length: f64, +// }, +// Cylinder { +// radius: f64, +// length: f64, +// }, +// // Ellipsoid, +// // Heightmap, +// // Image, +// Mesh { +// filename: String, +// #[serde(with = "crate::common::ss_option_vec3", default)] +// scale: Option<[f64; 3]>, +// }, +// // Plane, +// // Polyline, +// Sphere { +// radius: f64, +// }, +// } + +// impl Default for Geometry { +// fn default() -> Geometry { +// Geometry::Box { +// size: [0.0f64, 0.0, 0.0], +// } +// } +// } + +// #[derive(Debug, Deserialize, Default, Clone)] +// pub struct Material { +// pub name: String, +// pub color: Option, +// pub texture: Option, +// } + +// #[derive(Debug, Deserialize, Default, Clone)] +// pub struct Visual { +// pub name: Option, +// #[serde(default)] +// pub origin: Pose, +// pub geometry: Geometry, +// pub material: Option, +// } + +// /// Urdf Link element +// /// See for more detail. +// #[derive(Debug, Deserialize, Clone)] +// pub struct Link { +// pub name: String, +// #[serde(default)] +// pub pose: Pose, +// #[serde(default)] +// pub visual: Vec, +// } + +// #[derive(Debug, Deserialize, Clone)] +// pub struct Axis { +// #[serde(with = "crate::common::ss_vec3")] +// pub xyz: [f64; 3], +// } + +// impl Default for Axis { +// fn default() -> Axis { +// Axis { +// xyz: [0.0f64, 0.0, 1.0], +// } +// } +// } + +// #[derive(Debug, Deserialize, Clone)] +// pub struct Pose { +// #[serde(with = "crate::common::ss_vec3")] +// #[serde(default = "default_zero3")] +// pub xyz: [f64; 3], +// #[serde(with = "crate::common::ss_vec3")] +// #[serde(default = "default_zero3")] +// pub rpy: [f64; 3], +// } + +// fn default_zero3() -> [f64; 3] { +// [0.0f64, 0.0, 0.0] +// } + +// impl Default for Pose { +// fn default() -> Pose { +// Pose { +// xyz: default_zero3(), +// rpy: default_zero3(), +// } +// } +// } + +// #[derive(Debug, Deserialize, Clone)] +// pub struct LinkName { +// pub link: String, +// } + +// #[derive(Debug, Deserialize, Clone, PartialEq, Eq)] +// #[serde(rename_all = "snake_case")] +// pub enum JointType { +// Revolute, +// Continuous, +// Prismatic, +// Fixed, +// Floating, +// Planar, +// Spherical, +// } + +// #[derive(Debug, Deserialize, Default, Clone)] +// pub struct JointLimit { +// #[serde(default)] +// pub lower: f64, +// #[serde(default)] +// pub upper: f64, +// pub effort: f64, +// pub velocity: f64, +// } + +// /// Urdf Joint element +// /// See for more detail. +// #[derive(Debug, Deserialize, Clone)] +// pub struct Joint { +// pub name: String, +// #[serde(rename = "type")] +// pub joint_type: JointType, +// #[serde(default)] +// pub origin: Pose, +// pub parent: LinkName, +// pub child: LinkName, +// #[serde(default)] +// pub axis: Axis, +// #[serde(default)] +// pub limit: JointLimit, +// } + +/// Top level struct representing an SDFormat element. +/// http://sdformat.org/spec?ver=1.9&elem=model +#[derive(Debug, Deserialize, Clone, PartialEq)] +pub struct Model { + #[serde(default)] + pub name: String, + // #[serde(rename = "link", default)] + // pub links: Vec, + + // #[serde(rename = "joint", default)] + // pub joints: Vec, + + // #[serde(rename = "material", default)] + // pub materials: Vec, +} + +/// Top level struct representing an SDFormat element. +/// http://sdformat.org/spec?ver=1.9&elem=world +#[derive(Debug, Deserialize, Clone, PartialEq)] +#[serde(rename = "world")] +pub struct World { + #[serde(default)] + pub name: String, + // #[serde(rename = "frame", default)] + // pub frames: Vec, + #[serde(rename = "model", default)] + pub models: Vec, +} + +impl World { + pub fn models(&self) -> Vec<&Model> { + self.models.iter().collect() + } +} + +/// Top level enum to access root element of a parsed sdf. +/// http://sdformat.org/spec?ver=1.9&elem=sdf +#[derive(Debug, Clone, PartialEq)] +pub enum Sdf { + Model(Model), + Worlds(Vec), + // TODO: Actor + // TODO: Light +} + +impl Sdf { + pub fn models(&self) -> Vec<&Model> { + match &self { + Sdf::Model(m) => { + vec![&m] + }, + Sdf::Worlds(ws) => ws.iter().map(|w| w.models()).flatten().collect(), + } + } +} + +#[derive(Debug, Deserialize, Clone, PartialEq)] +#[serde(rename = "sdf")] +pub struct SDFormat { + #[serde(rename = "world", default)] + pub worlds: Vec, + #[serde(rename = "model", default)] + pub models: Vec, +} diff --git a/src/sdf/funcs.rs b/src/sdf/funcs.rs new file mode 100644 index 0000000..64e54c3 --- /dev/null +++ b/src/sdf/funcs.rs @@ -0,0 +1,89 @@ +use crate::errors::*; +use crate::sdf::deserialize::*; + +use std::path::Path; + +/// Read SDFormat file and create Sdf instance +/// +/// # Examples +/// +/// ``` +/// let sdf = urdf_rs::sdf::read_file("samples/minimal.sdf").unwrap(); +/// println!("{:?}", sdf); +/// ``` +pub fn read_file>(path: P) -> Result { + read_from_string(&std::fs::read_to_string(path)?) +} + +/// Read from string instead of file. +/// +/// +/// # Examples +/// +/// ``` +/// let s = r##" +/// +/// +/// +/// +/// +/// +/// "##; +/// let sdf = urdf_rs::sdf::read_from_string(s).unwrap(); +/// let model = urdf_rs::sdf::Model { name: "my_model".to_string() }; +/// assert_eq!(sdf.models(), vec![&model]); +/// ``` +pub fn read_from_string(string: &str) -> Result { + // let sdf_elem: xml::Element = string.parse().map_err(UrdfError::new)?; + // if (sdf_elem.name != "sdf") { + // return UrdfError + // } + let mut sdf: SDFormat = serde_xml_rs::from_str(&string).map_err(UrdfError::new)?; + if sdf.worlds.len() > 0 { + return Ok(Sdf::Worlds(sdf.worlds)); + } + let model = sdf.models.pop(); + match model { + Some(m) => Ok(Sdf::Model(m)), + None => panic!("TODO: Custom error types."), + } +} + +#[test] +fn models_parsed() { + let s = r##" + + + + + + +"##; + let sdf = read_from_string(s).unwrap(); + + if let Sdf::Model(root) = sdf { + assert_eq!(root.name, "my_model"); + } else { + unreachable!() + } + + let s = r##" + + + + + + + + + + + + + +"##; + + let sdf = read_from_string(s).unwrap(); + let model_names: Vec<&String> = sdf.models().iter().map(|m| &m.name).collect(); + assert_eq!(model_names, vec!["my_model", "discovery"]); +} diff --git a/src/urdf.rs b/src/urdf.rs new file mode 100644 index 0000000..73f7645 --- /dev/null +++ b/src/urdf.rs @@ -0,0 +1,7 @@ +mod deserialize; +pub use deserialize::*; + +mod funcs; +pub use funcs::*; + +pub use crate::errors::*; diff --git a/src/deserialize.rs b/src/urdf/deserialize.rs similarity index 60% rename from src/deserialize.rs rename to src/urdf/deserialize.rs index 4ce35c7..8afffda 100644 --- a/src/deserialize.rs +++ b/src/urdf/deserialize.rs @@ -1,10 +1,6 @@ +use crate::common::*; use serde::Deserialize; -#[derive(Debug, Deserialize, Default, Clone)] -pub struct Mass { - pub value: f64, -} - #[derive(Debug, Deserialize, Default, Clone)] pub struct Inertia { pub ixx: f64, @@ -27,7 +23,7 @@ pub struct Inertial { #[serde(rename_all = "snake_case")] pub enum Geometry { Box { - #[serde(with = "urdf_vec3")] + #[serde(with = "ss_vec3")] size: [f64; 3], }, Cylinder { @@ -43,7 +39,7 @@ pub enum Geometry { }, Mesh { filename: String, - #[serde(with = "urdf_option_vec3", default)] + #[serde(with = "ss_option_vec3", default)] scale: Option<[f64; 3]>, }, } @@ -56,29 +52,10 @@ impl Default for Geometry { } } -#[derive(Debug, Deserialize, Default, Clone)] -pub struct Color { - #[serde(with = "urdf_vec4")] - pub rgba: [f64; 4], -} - -#[derive(Debug, Deserialize, Clone)] -pub struct Texture { - pub filename: String, -} - -impl Default for Texture { - fn default() -> Texture { - Texture { - filename: "".to_string(), - } - } -} - #[derive(Debug, Deserialize, Default, Clone)] pub struct Material { pub name: String, - pub color: Option, + pub color: Option, pub texture: Option, } @@ -112,84 +89,9 @@ pub struct Link { pub collision: Vec, } -#[derive(Deserialize, Debug, Clone)] -pub struct Vec3 { - #[serde(with = "urdf_vec3")] - pub data: [f64; 3], -} - -mod urdf_vec3 { - use serde::{self, Deserialize, Deserializer}; - pub fn deserialize<'a, D>(deserializer: D) -> Result<[f64; 3], D::Error> - where - D: Deserializer<'a>, - { - let s = String::deserialize(deserializer)?; - let vec = s - .split(' ') - .filter_map(|x| x.parse::().ok()) - .collect::>(); - if vec.len() != 3 { - return Err(serde::de::Error::custom(format!( - "failed to parse float array in {s}" - ))); - } - let mut arr = [0.0f64; 3]; - arr.copy_from_slice(&vec); - Ok(arr) - } -} - -mod urdf_option_vec3 { - use serde::{self, Deserialize, Deserializer}; - pub fn deserialize<'a, D>(deserializer: D) -> Result, D::Error> - where - D: Deserializer<'a>, - { - let s = String::deserialize(deserializer)?; - let vec = s - .split(' ') - .filter_map(|x| x.parse::().ok()) - .collect::>(); - if vec.is_empty() { - Ok(None) - } else if vec.len() == 3 { - let mut arr = [0.0; 3]; - arr.copy_from_slice(&vec); - Ok(Some(arr)) - } else { - Err(serde::de::Error::custom(format!( - "failed to parse float array in {s}" - ))) - } - } -} - -mod urdf_vec4 { - use serde::{self, Deserialize, Deserializer}; - pub fn deserialize<'a, D>(deserializer: D) -> Result<[f64; 4], D::Error> - where - D: Deserializer<'a>, - { - let s = String::deserialize(deserializer)?; - let vec = s - .split(' ') - .filter_map(|x| x.parse::().ok()) - .collect::>(); - if vec.len() != 4 { - return Err(serde::de::Error::custom(format!( - "failed to parse float array in {s}" - ))); - } - let mut arr = [0.0f64; 4]; - arr.copy_from_slice(&vec); - Ok(arr) - } -} - #[derive(Debug, Deserialize, Clone)] pub struct Axis { - #[serde(with = "urdf_vec3")] + #[serde(with = "ss_vec3")] pub xyz: [f64; 3], } @@ -203,10 +105,10 @@ impl Default for Axis { #[derive(Debug, Deserialize, Clone)] pub struct Pose { - #[serde(with = "urdf_vec3")] + #[serde(with = "ss_vec3")] #[serde(default = "default_zero3")] pub xyz: [f64; 3], - #[serde(with = "urdf_vec3")] + #[serde(with = "ss_vec3")] #[serde(default = "default_zero3")] pub rpy: [f64; 3], } diff --git a/src/funcs.rs b/src/urdf/funcs.rs similarity index 97% rename from src/funcs.rs rename to src/urdf/funcs.rs index 9fd3a08..c314a68 100644 --- a/src/funcs.rs +++ b/src/urdf/funcs.rs @@ -1,5 +1,5 @@ -use crate::deserialize::*; use crate::errors::*; +use crate::urdf::deserialize::*; use std::path::Path; @@ -32,7 +32,7 @@ fn sort_link_joint(string: &str) -> Result { /// # Examples /// /// ``` -/// let urdf_robo = urdf_rs::read_file("sample.urdf").unwrap(); +/// let urdf_robo = urdf_rs::urdf::read_file("samples/sample.urdf").unwrap(); /// let links = urdf_robo.links; /// println!("{:?}", links[0].visual[0].origin.xyz); /// ``` @@ -88,7 +88,7 @@ pub fn read_file>(path: P) -> Result { /// /// /// "##; -/// let urdf_robo = urdf_rs::read_from_string(s).unwrap(); +/// let urdf_robo = urdf_rs::urdf::read_from_string(s).unwrap(); /// println!("{:?}", urdf_robo.links[0].visual[0].origin.xyz); /// ```