From c2fea063333aaea20bf42643274877ea701bfbc1 Mon Sep 17 00:00:00 2001 From: Will Stott Date: Mon, 11 Apr 2022 20:39:30 +0100 Subject: [PATCH 1/2] WIP Towards adding an SDFormat parser to this repo. --- README.md | 2 +- sample.urdf => samples/sample.urdf | 0 src/common/elements.rs | 38 +++++ src/common/mod.rs | 5 + src/common/space_seperated_vectors.rs | 68 ++++++++ src/lib.rs | 13 +- src/{utils.rs => ros_utils.rs} | 9 +- src/sdf/deserialize.rs | 165 +++++++++++++++++++ src/{ => sdf}/funcs.rs | 6 +- src/sdf/mod.rs | 8 + src/{ => urdf}/deserialize.rs | 112 +------------ src/urdf/funcs.rs | 218 ++++++++++++++++++++++++++ src/urdf/mod.rs | 7 + 13 files changed, 530 insertions(+), 121 deletions(-) rename sample.urdf => samples/sample.urdf (100%) create mode 100644 src/common/elements.rs create mode 100644 src/common/mod.rs create mode 100644 src/common/space_seperated_vectors.rs rename src/{utils.rs => ros_utils.rs} (92%) create mode 100644 src/sdf/deserialize.rs rename src/{ => sdf}/funcs.rs (97%) create mode 100644 src/sdf/mod.rs rename src/{ => urdf}/deserialize.rs (60%) create mode 100644 src/urdf/funcs.rs create mode 100644 src/urdf/mod.rs 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/sample.urdf b/samples/sample.urdf similarity index 100% rename from sample.urdf rename to samples/sample.urdf 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/mod.rs b/src/common/mod.rs new file mode 100644 index 0000000..a4c3338 --- /dev/null +++ b/src/common/mod.rs @@ -0,0 +1,5 @@ +mod elements; +pub use elements::*; + +mod space_seperated_vectors; +pub use space_seperated_vectors::*; 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/deserialize.rs b/src/sdf/deserialize.rs new file mode 100644 index 0000000..c58eab7 --- /dev/null +++ b/src/sdf/deserialize.rs @@ -0,0 +1,165 @@ +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 to access urdf. +#[derive(Debug, Deserialize, Clone)] +pub struct Robot { + #[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, +} diff --git a/src/funcs.rs b/src/sdf/funcs.rs similarity index 97% rename from src/funcs.rs rename to src/sdf/funcs.rs index 9fd3a08..87896f2 100644 --- a/src/funcs.rs +++ b/src/sdf/funcs.rs @@ -1,5 +1,5 @@ -use crate::deserialize::*; use crate::errors::*; +use crate::sdf::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::sdf::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::sdf::read_from_string(s).unwrap(); /// println!("{:?}", urdf_robo.links[0].visual[0].origin.xyz); /// ``` diff --git a/src/sdf/mod.rs b/src/sdf/mod.rs new file mode 100644 index 0000000..3bf8fb5 --- /dev/null +++ b/src/sdf/mod.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/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/urdf/funcs.rs b/src/urdf/funcs.rs new file mode 100644 index 0000000..c314a68 --- /dev/null +++ b/src/urdf/funcs.rs @@ -0,0 +1,218 @@ +use crate::errors::*; +use crate::urdf::deserialize::*; + +use std::path::Path; + +/// sort and to avoid the [issue](https://github.com/RReverser/serde-xml-rs/issues/5) +fn sort_link_joint(string: &str) -> Result { + let e: xml::Element = string.parse().map_err(UrdfError::new)?; + let mut links = Vec::new(); + let mut joints = Vec::new(); + let mut materials = Vec::new(); + for c in &e.children { + if let xml::Xml::ElementNode(ref xml_elm) = *c { + if xml_elm.name == "link" { + links.push(xml::Xml::ElementNode(xml_elm.clone())); + } else if xml_elm.name == "joint" { + joints.push(xml::Xml::ElementNode(xml_elm.clone())); + } else if xml_elm.name == "material" { + materials.push(xml::Xml::ElementNode(xml_elm.clone())); + } + }; + } + let mut new_elm = e; + links.extend(joints); + links.extend(materials); + new_elm.children = links; + Ok(format!("{new_elm}")) +} + +/// Read urdf file and create Robot instance +/// +/// # Examples +/// +/// ``` +/// let urdf_robo = urdf_rs::urdf::read_file("samples/sample.urdf").unwrap(); +/// let links = urdf_robo.links; +/// println!("{:?}", links[0].visual[0].origin.xyz); +/// ``` +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 urdf_robo = urdf_rs::urdf::read_from_string(s).unwrap(); +/// println!("{:?}", urdf_robo.links[0].visual[0].origin.xyz); +/// ``` + +pub fn read_from_string(string: &str) -> Result { + let sorted_string = sort_link_joint(string)?; + serde_xml_rs::from_str(&sorted_string).map_err(UrdfError::new) +} + +#[test] +fn it_works() { + use assert_approx_eq::assert_approx_eq; + + let s = r##" + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + "##; + let robo = read_from_string(s).unwrap(); + + assert_eq!(robo.name, "robo"); + assert_eq!(robo.links.len(), 3); + assert_eq!(robo.joints.len(), 2); + assert_eq!(robo.links[0].visual.len(), 3); + let xyz = robo.links[0].visual[0].origin.xyz; + assert_approx_eq!(xyz[0], 0.1); + assert_approx_eq!(xyz[1], 0.2); + assert_approx_eq!(xyz[2], 0.3); + let rpy = robo.links[0].visual[0].origin.rpy; + assert_approx_eq!(rpy[0], -0.1); + assert_approx_eq!(rpy[1], -0.2); + assert_approx_eq!(rpy[2], -0.3); + + match robo.links[0].visual[0].geometry { + Geometry::Box { size } => { + assert_approx_eq!(size[0], 1.0f64); + assert_approx_eq!(size[1], 2.0f64); + assert_approx_eq!(size[2], 3.0f64); + } + _ => panic!("geometry error"), + } + match robo.links[0].visual[1].geometry { + Geometry::Mesh { + ref filename, + scale, + } => { + assert_eq!(filename, "aa.dae"); + assert_eq!(scale, None); + } + _ => panic!("geometry error"), + } + match robo.links[0].visual[2].geometry { + Geometry::Mesh { + ref filename, + scale, + } => { + assert_eq!(filename, "bbb.dae"); + assert!(scale.is_some()); + } + _ => panic!("geometry error"), + } + + assert_eq!(robo.materials.len(), 1); + + assert_eq!(robo.joints[0].name, "shoulder_pitch"); + let xyz = robo.joints[0].axis.xyz; + assert_approx_eq!(xyz[0], 0.0f64); + assert_approx_eq!(xyz[1], 1.0f64); + assert_approx_eq!(xyz[2], -1.0f64); + let xyz = robo.joints[0].axis.xyz; + //"0 1 -1" + assert_approx_eq!(xyz[0], 0.0); + assert_approx_eq!(xyz[1], 1.0); + assert_approx_eq!(xyz[2], -1.0); +} diff --git a/src/urdf/mod.rs b/src/urdf/mod.rs new file mode 100644 index 0000000..73f7645 --- /dev/null +++ b/src/urdf/mod.rs @@ -0,0 +1,7 @@ +mod deserialize; +pub use deserialize::*; + +mod funcs; +pub use funcs::*; + +pub use crate::errors::*; From 869caba5649df0f78da4685c2af3513ce03068ef Mon Sep 17 00:00:00 2001 From: Will Stott Date: Sun, 17 Apr 2022 17:12:52 +0100 Subject: [PATCH 2/2] Refactor away from mod.rs - start to simple SDFormat parsing tests --- samples/minimal.sdf | 6 + src/{common/mod.rs => common.rs} | 0 src/{sdf/mod.rs => sdf.rs} | 0 src/sdf/deserialize.rs | 339 ++++++++++++++++++------------- src/sdf/funcs.rs | 247 ++++++---------------- src/{urdf/mod.rs => urdf.rs} | 0 6 files changed, 259 insertions(+), 333 deletions(-) create mode 100644 samples/minimal.sdf rename src/{common/mod.rs => common.rs} (100%) rename src/{sdf/mod.rs => sdf.rs} (100%) rename src/{urdf/mod.rs => urdf.rs} (100%) 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/src/common/mod.rs b/src/common.rs similarity index 100% rename from src/common/mod.rs rename to src/common.rs diff --git a/src/sdf/mod.rs b/src/sdf.rs similarity index 100% rename from src/sdf/mod.rs rename to src/sdf.rs diff --git a/src/sdf/deserialize.rs b/src/sdf/deserialize.rs index c58eab7..eabd880 100644 --- a/src/sdf/deserialize.rs +++ b/src/sdf/deserialize.rs @@ -1,165 +1,214 @@ -use crate::common::*; +// 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, - }, -} +// #[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, -impl Default for Geometry { - fn default() -> Geometry { - Geometry::Box { - size: [0.0f64, 0.0, 0.0], - } - } -} + // #[serde(rename = "joint", default)] + // pub joints: Vec, -#[derive(Debug, Deserialize, Default, Clone)] -pub struct Material { - pub name: String, - pub color: Option, - pub texture: Option, + // #[serde(rename = "material", default)] + // pub materials: Vec, } -#[derive(Debug, Deserialize, Default, Clone)] -pub struct Visual { - pub name: Option, +/// 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 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, + // #[serde(rename = "frame", default)] + // pub frames: Vec, + #[serde(rename = "model", default)] + pub models: 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], - } +impl World { + pub fn models(&self) -> Vec<&Model> { + self.models.iter().collect() } } -#[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], +/// 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 } -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(), +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)] -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 to access urdf. -#[derive(Debug, Deserialize, Clone)] -pub struct Robot { - #[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, +#[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 index 87896f2..64e54c3 100644 --- a/src/sdf/funcs.rs +++ b/src/sdf/funcs.rs @@ -3,40 +3,15 @@ use crate::sdf::deserialize::*; use std::path::Path; -/// sort and to avoid the [issue](https://github.com/RReverser/serde-xml-rs/issues/5) -fn sort_link_joint(string: &str) -> Result { - let e: xml::Element = string.parse().map_err(UrdfError::new)?; - let mut links = Vec::new(); - let mut joints = Vec::new(); - let mut materials = Vec::new(); - for c in &e.children { - if let xml::Xml::ElementNode(ref xml_elm) = *c { - if xml_elm.name == "link" { - links.push(xml::Xml::ElementNode(xml_elm.clone())); - } else if xml_elm.name == "joint" { - joints.push(xml::Xml::ElementNode(xml_elm.clone())); - } else if xml_elm.name == "material" { - materials.push(xml::Xml::ElementNode(xml_elm.clone())); - } - }; - } - let mut new_elm = e; - links.extend(joints); - links.extend(materials); - new_elm.children = links; - Ok(format!("{new_elm}")) -} - -/// Read urdf file and create Robot instance +/// Read SDFormat file and create Sdf instance /// /// # Examples /// /// ``` -/// let urdf_robo = urdf_rs::sdf::read_file("samples/sample.urdf").unwrap(); -/// let links = urdf_robo.links; -/// println!("{:?}", links[0].visual[0].origin.xyz); +/// let sdf = urdf_rs::sdf::read_file("samples/minimal.sdf").unwrap(); +/// println!("{:?}", sdf); /// ``` -pub fn read_file>(path: P) -> Result { +pub fn read_file>(path: P) -> Result { read_from_string(&std::fs::read_to_string(path)?) } @@ -47,172 +22,68 @@ pub fn read_file>(path: P) -> Result { /// /// ``` /// let s = r##" -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// "##; -/// let urdf_robo = urdf_rs::sdf::read_from_string(s).unwrap(); -/// println!("{:?}", urdf_robo.links[0].visual[0].origin.xyz); +/// +/// +/// +/// +/// +/// +/// "##; +/// 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 sorted_string = sort_link_joint(string)?; - serde_xml_rs::from_str(&sorted_string).map_err(UrdfError::new) +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 it_works() { - use assert_approx_eq::assert_approx_eq; - +fn models_parsed() { let s = r##" - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - "##; - let robo = read_from_string(s).unwrap(); - - assert_eq!(robo.name, "robo"); - assert_eq!(robo.links.len(), 3); - assert_eq!(robo.joints.len(), 2); - assert_eq!(robo.links[0].visual.len(), 3); - let xyz = robo.links[0].visual[0].origin.xyz; - assert_approx_eq!(xyz[0], 0.1); - assert_approx_eq!(xyz[1], 0.2); - assert_approx_eq!(xyz[2], 0.3); - let rpy = robo.links[0].visual[0].origin.rpy; - assert_approx_eq!(rpy[0], -0.1); - assert_approx_eq!(rpy[1], -0.2); - assert_approx_eq!(rpy[2], -0.3); + + + + + + +"##; + let sdf = read_from_string(s).unwrap(); - match robo.links[0].visual[0].geometry { - Geometry::Box { size } => { - assert_approx_eq!(size[0], 1.0f64); - assert_approx_eq!(size[1], 2.0f64); - assert_approx_eq!(size[2], 3.0f64); - } - _ => panic!("geometry error"), - } - match robo.links[0].visual[1].geometry { - Geometry::Mesh { - ref filename, - scale, - } => { - assert_eq!(filename, "aa.dae"); - assert_eq!(scale, None); - } - _ => panic!("geometry error"), - } - match robo.links[0].visual[2].geometry { - Geometry::Mesh { - ref filename, - scale, - } => { - assert_eq!(filename, "bbb.dae"); - assert!(scale.is_some()); - } - _ => panic!("geometry error"), + if let Sdf::Model(root) = sdf { + assert_eq!(root.name, "my_model"); + } else { + unreachable!() } - assert_eq!(robo.materials.len(), 1); + let s = r##" + + + + + + + + + + + + + +"##; - assert_eq!(robo.joints[0].name, "shoulder_pitch"); - let xyz = robo.joints[0].axis.xyz; - assert_approx_eq!(xyz[0], 0.0f64); - assert_approx_eq!(xyz[1], 1.0f64); - assert_approx_eq!(xyz[2], -1.0f64); - let xyz = robo.joints[0].axis.xyz; - //"0 1 -1" - assert_approx_eq!(xyz[0], 0.0); - assert_approx_eq!(xyz[1], 1.0); - assert_approx_eq!(xyz[2], -1.0); + 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/mod.rs b/src/urdf.rs similarity index 100% rename from src/urdf/mod.rs rename to src/urdf.rs