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;

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/mod.rs

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

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/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<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 to access urdf.
+#[derive(Debug, Deserialize, Clone)]
+pub struct Robot {
+    #[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>,
+}

src/funcs.rs → 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<String> {
 /// # 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<P: AsRef<Path>>(path: P) -> Result<Robot> {
 ///         </joint>
 ///     </robot>
 ///    "##;
-/// 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);
 /// ```
 

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;