WIP: Towards adding an SDFormat parser to this repo.

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;

samples/minimal.sdf

@@ -0,0 +1,6 @@
+<?xml version='1.0'?>
+<sdf version='1.9'>
+<model name='my_model'>
+  <link name='link'/>
+</model>
+</sdf>

src/common.rs

@@ -0,0 +1,5 @@
+mod elements;
+pub use elements::*;
+
+mod space_seperated_vectors;
+pub use space_seperated_vectors::*;

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(),
+        }
+    }
+}

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::<f64>().ok())
+            .collect::<Vec<_>>();
+        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<Option<[f64; 3]>, D::Error>
+    where
+        D: Deserializer<'a>,
+    {
+        let s = String::deserialize(deserializer)?;
+        let vec = s
+            .split(' ')
+            .filter_map(|x| x.parse::<f64>().ok())
+            .collect::<Vec<_>>();
+        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::<f64>().ok())
+            .collect::<Vec<_>>();
+        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)
+    }
+}

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;

src/utils.rs → 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());
 }

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;

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<ColorRGBA>,
+//     pub texture: Option<Texture>,
+// }
+
+// #[derive(Debug, Deserialize, Default, Clone)]
+// pub struct Visual {
+//     pub name: Option<String>,
+//     #[serde(default)]
+//     pub origin: Pose,
+//     pub geometry: Geometry,
+//     pub material: Option<Material>,
+// }
+
+// /// Urdf Link element
+// /// See <http://wiki.ros.org/urdf/XML/link> for more detail.
+// #[derive(Debug, Deserialize, Clone)]
+// pub struct Link {
+//     pub name: String,
+//     #[serde(default)]
+//     pub pose: Pose,
+//     #[serde(default)]
+//     pub visual: Vec<Visual>,
+// }
+
+// #[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 <http://wiki.ros.org/urdf/XML/joint> 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 <model> 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<Link>,
+
+    // #[serde(rename = "joint", default)]
+    // pub joints: Vec<Joint>,
+
+    // #[serde(rename = "material", default)]
+    // pub materials: Vec<Material>,
+}
+
+/// Top level struct representing an SDFormat <world> 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<Frame>,
+    #[serde(rename = "model", default)]
+    pub models: Vec<Model>,
+}
+
+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<World>),
+    // 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<World>,
+    #[serde(rename = "model", default)]
+    pub models: Vec<Model>,
+}