urdf_rs/
deserialize.rs

1use serde::de::Visitor;
2use serde::{Deserialize, Deserializer, Serialize};
3
4use std::ops::{Deref, DerefMut};
5
6#[derive(Debug, Deserialize, Serialize, Default, Clone)]
7pub struct Mass {
8    #[serde(rename(serialize = "@value"))]
9    #[serde(deserialize_with = "de_f64")]
10    pub value: f64,
11}
12
13#[derive(Debug, Deserialize, Serialize, Default, Clone)]
14pub struct Inertia {
15    #[serde(rename(serialize = "@ixx"))]
16    #[serde(deserialize_with = "de_f64")]
17    pub ixx: f64,
18    #[serde(rename(serialize = "@ixy"))]
19    #[serde(deserialize_with = "de_f64")]
20    pub ixy: f64,
21    #[serde(rename(serialize = "@ixz"))]
22    #[serde(deserialize_with = "de_f64")]
23    pub ixz: f64,
24    #[serde(rename(serialize = "@iyy"))]
25    #[serde(deserialize_with = "de_f64")]
26    pub iyy: f64,
27    #[serde(rename(serialize = "@iyz"))]
28    #[serde(deserialize_with = "de_f64")]
29    pub iyz: f64,
30    #[serde(rename(serialize = "@izz"))]
31    #[serde(deserialize_with = "de_f64")]
32    pub izz: f64,
33}
34
35#[derive(Debug, Deserialize, Serialize, Default, Clone)]
36pub struct Inertial {
37    #[serde(default)]
38    pub origin: Pose,
39    pub mass: Mass,
40    pub inertia: Inertia,
41}
42
43#[derive(Debug, Deserialize, Clone)]
44#[serde(rename_all = "snake_case")]
45pub enum Geometry {
46    Box {
47        size: Vec3,
48    },
49    Cylinder {
50        #[serde(deserialize_with = "de_f64")]
51        radius: f64,
52        #[serde(deserialize_with = "de_f64")]
53        length: f64,
54    },
55    Capsule {
56        #[serde(deserialize_with = "de_f64")]
57        radius: f64,
58        #[serde(deserialize_with = "de_f64")]
59        length: f64,
60    },
61    Sphere {
62        #[serde(deserialize_with = "de_f64")]
63        radius: f64,
64    },
65    Mesh {
66        filename: String,
67        scale: Option<Vec3>,
68    },
69}
70
71impl Default for Geometry {
72    fn default() -> Self {
73        Self::Box {
74            size: Vec3::default(),
75        }
76    }
77}
78
79impl Serialize for Geometry {
80    fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
81    where
82        S: serde::Serializer,
83    {
84        // Workaround for https://github.com/openrr/urdf-rs/pull/107#discussion_r1741875356.
85        #[derive(Serialize)]
86        #[serde(rename_all = "snake_case")]
87        enum GeometryRepr<'a> {
88            Box {
89                #[serde(rename(serialize = "@size"))]
90                size: Vec3,
91            },
92            Cylinder {
93                #[serde(rename(serialize = "@radius"))]
94                radius: f64,
95                #[serde(rename(serialize = "@length"))]
96                length: f64,
97            },
98            Capsule {
99                #[serde(rename(serialize = "@radius"))]
100                radius: f64,
101                #[serde(rename(serialize = "@length"))]
102                length: f64,
103            },
104            Sphere {
105                #[serde(rename(serialize = "@radius"))]
106                radius: f64,
107            },
108            Mesh {
109                #[serde(rename(serialize = "@filename"))]
110                filename: &'a str,
111                #[serde(rename(serialize = "@scale"), skip_serializing_if = "Option::is_none")]
112                scale: Option<Vec3>,
113            },
114        }
115        #[derive(Serialize)]
116        struct GeometryTag<'a> {
117            #[serde(rename(serialize = "$value"))]
118            value: GeometryRepr<'a>,
119        }
120        let value = match *self {
121            Self::Box { size } => GeometryRepr::Box { size },
122            Self::Cylinder { radius, length } => GeometryRepr::Cylinder { radius, length },
123            Self::Capsule { radius, length } => GeometryRepr::Capsule { radius, length },
124            Self::Sphere { radius } => GeometryRepr::Sphere { radius },
125            Self::Mesh {
126                ref filename,
127                scale,
128            } => GeometryRepr::Mesh { filename, scale },
129        };
130        GeometryTag::serialize(&GeometryTag { value }, serializer)
131    }
132}
133
134#[derive(Debug, Deserialize, Serialize, Default, Clone)]
135pub struct Color {
136    #[serde(rename(serialize = "@rgba"))]
137    pub rgba: Vec4,
138}
139
140#[derive(Debug, Deserialize, Serialize, Default, Clone)]
141pub struct Texture {
142    #[serde(rename(serialize = "@filename"))]
143    pub filename: String,
144}
145
146#[derive(Debug, Deserialize, Serialize, Default, Clone)]
147pub struct Material {
148    #[serde(rename(serialize = "@name"))]
149    pub name: String,
150    #[serde(skip_serializing_if = "Option::is_none")]
151    pub color: Option<Color>,
152    #[serde(
153        rename(serialize = "@texture"),
154        skip_serializing_if = "Option::is_none"
155    )]
156    pub texture: Option<Texture>,
157}
158
159#[derive(Debug, Deserialize, Serialize, Default, Clone)]
160pub struct Visual {
161    #[serde(rename(serialize = "@name"), skip_serializing_if = "Option::is_none")]
162    pub name: Option<String>,
163    #[serde(default)]
164    pub origin: Pose,
165    pub geometry: Geometry,
166    #[serde(skip_serializing_if = "Option::is_none")]
167    pub material: Option<Material>,
168}
169
170#[derive(Debug, Deserialize, Serialize, Default, Clone)]
171pub struct Collision {
172    #[serde(rename(serialize = "@name"), skip_serializing_if = "Option::is_none")]
173    pub name: Option<String>,
174    #[serde(default)]
175    pub origin: Pose,
176    pub geometry: Geometry,
177}
178
179/// Urdf Link element
180/// See <http://wiki.ros.org/urdf/XML/link> for more detail.
181#[derive(Debug, Deserialize, Serialize, Clone)]
182pub struct Link {
183    #[serde(rename(serialize = "@name"))]
184    pub name: String,
185    #[serde(default)]
186    pub inertial: Inertial,
187    #[serde(default)]
188    pub visual: Vec<Visual>,
189    #[serde(default)]
190    pub collision: Vec<Collision>,
191}
192
193#[derive(Debug, Serialize, Default, Clone, Copy, PartialEq)]
194pub struct Vec3(#[serde(rename(serialize = "$text"))] pub [f64; 3]);
195
196impl Deref for Vec3 {
197    type Target = [f64; 3];
198
199    fn deref(&self) -> &Self::Target {
200        &self.0
201    }
202}
203
204impl DerefMut for Vec3 {
205    fn deref_mut(&mut self) -> &mut Self::Target {
206        &mut self.0
207    }
208}
209
210impl<'de> Deserialize<'de> for Vec3 {
211    fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
212    where
213        D: serde::Deserializer<'de>,
214    {
215        deserializer.deserialize_str(Vec3Visitor)
216    }
217}
218
219struct Vec3Visitor;
220impl Visitor<'_> for Vec3Visitor {
221    type Value = Vec3;
222
223    fn expecting(&self, formatter: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
224        formatter.write_str("a string containing three floating point values separated by spaces")
225    }
226
227    fn visit_str<E>(self, v: &str) -> Result<Self::Value, E>
228    where
229        E: serde::de::Error,
230    {
231        let split_results: Vec<_> = v
232            .split_whitespace()
233            .filter_map(|s| s.parse::<f64>().ok())
234            .collect();
235        if split_results.len() != 3 {
236            return Err(E::custom(format!(
237                "Wrong vector element count, expected 3 found {} for [{}]",
238                split_results.len(),
239                v
240            )));
241        }
242        let mut res = [0.0f64; 3];
243        res.copy_from_slice(&split_results);
244        Ok(Vec3(res))
245    }
246}
247
248#[derive(Debug, Serialize, Default, Clone, Copy, PartialEq)]
249pub struct Vec4(#[serde(rename(serialize = "$text"))] pub [f64; 4]);
250
251impl Deref for Vec4 {
252    type Target = [f64; 4];
253
254    fn deref(&self) -> &Self::Target {
255        &self.0
256    }
257}
258
259impl DerefMut for Vec4 {
260    fn deref_mut(&mut self) -> &mut Self::Target {
261        &mut self.0
262    }
263}
264
265impl<'de> Deserialize<'de> for Vec4 {
266    fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
267    where
268        D: serde::Deserializer<'de>,
269    {
270        deserializer.deserialize_str(Vec4Visitor)
271    }
272}
273
274struct Vec4Visitor;
275impl Visitor<'_> for Vec4Visitor {
276    type Value = Vec4;
277
278    fn expecting(&self, formatter: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
279        formatter.write_str("a string containing four floating point values separated by spaces")
280    }
281
282    fn visit_str<E>(self, v: &str) -> Result<Self::Value, E>
283    where
284        E: serde::de::Error,
285    {
286        let split_results: Vec<_> = v
287            .split_whitespace()
288            .filter_map(|s| s.parse::<f64>().ok())
289            .collect();
290        if split_results.len() != 4 {
291            return Err(E::custom(format!(
292                "Wrong vector element count, expected 4 found {} for [{}]",
293                split_results.len(),
294                v
295            )));
296        }
297        let mut res = [0.0f64; 4];
298        res.copy_from_slice(&split_results);
299        Ok(Vec4(res))
300    }
301}
302
303#[derive(Debug, Deserialize, Serialize, Clone)]
304pub struct Axis {
305    #[serde(rename(serialize = "@xyz"))]
306    pub xyz: Vec3,
307}
308
309impl Default for Axis {
310    fn default() -> Axis {
311        Axis {
312            xyz: Vec3([1.0, 0.0, 0.0]),
313        }
314    }
315}
316
317#[derive(Debug, Default, Deserialize, Serialize, Clone)]
318pub struct Pose {
319    #[serde(rename(serialize = "@xyz"), default)]
320    pub xyz: Vec3,
321    #[serde(rename(serialize = "@rpy"), default)]
322    pub rpy: Vec3,
323}
324
325#[derive(Debug, Default, Deserialize, Serialize, Clone)]
326pub struct LinkName {
327    #[serde(rename(serialize = "@link"))]
328    pub link: String,
329}
330
331#[derive(Debug, Default, Deserialize, Serialize, Clone, PartialEq, Eq)]
332#[serde(rename_all = "snake_case")]
333pub enum JointType {
334    Revolute,
335    Continuous,
336    Prismatic,
337    #[default]
338    Fixed,
339    Floating,
340    Planar,
341    Spherical,
342}
343
344#[derive(Debug, Deserialize, Serialize, Default, Clone)]
345pub struct JointLimit {
346    #[serde(rename(serialize = "@lower"), default)]
347    #[serde(deserialize_with = "de_f64")]
348    pub lower: f64,
349    #[serde(rename(serialize = "@upper"), default)]
350    #[serde(deserialize_with = "de_f64")]
351    pub upper: f64,
352    #[serde(rename(serialize = "@effort"), default)]
353    #[serde(deserialize_with = "de_f64")]
354    pub effort: f64,
355    #[serde(rename(serialize = "@velocity"))]
356    #[serde(deserialize_with = "de_f64")]
357    pub velocity: f64,
358}
359
360#[derive(Debug, Deserialize, Serialize, Default, Clone)]
361pub struct Mimic {
362    #[serde(rename(serialize = "@joint"))]
363    pub joint: String,
364    // `default` is needed when using `deserialize_with`: https://github.com/serde-rs/serde/issues/723#issuecomment-368135287
365    #[serde(
366        rename(serialize = "@multiplier"),
367        default,
368        skip_serializing_if = "Option::is_none"
369    )]
370    #[serde(deserialize_with = "de_opt_f64")]
371    pub multiplier: Option<f64>,
372    // `default` is needed when using `deserialize_with`: https://github.com/serde-rs/serde/issues/723#issuecomment-368135287
373    #[serde(
374        rename(serialize = "@offset"),
375        default,
376        skip_serializing_if = "Option::is_none"
377    )]
378    #[serde(deserialize_with = "de_opt_f64")]
379    pub offset: Option<f64>,
380}
381
382#[derive(Debug, Deserialize, Serialize, Default, Clone)]
383pub struct SafetyController {
384    #[serde(rename(serialize = "@soft_lower_limit"), default)]
385    #[serde(deserialize_with = "de_f64")]
386    pub soft_lower_limit: f64,
387    #[serde(rename(serialize = "@soft_upper_limit"), default)]
388    #[serde(deserialize_with = "de_f64")]
389    pub soft_upper_limit: f64,
390    #[serde(rename(serialize = "@k_position"), default)]
391    #[serde(deserialize_with = "de_f64")]
392    pub k_position: f64,
393    #[serde(rename(serialize = "@k_velocity"))]
394    #[serde(deserialize_with = "de_f64")]
395    pub k_velocity: f64,
396}
397
398/// Urdf Joint element
399/// See <http://wiki.ros.org/urdf/XML/joint> for more detail.
400#[derive(Debug, Deserialize, Serialize, Clone)]
401pub struct Joint {
402    #[serde(rename(serialize = "@name"))]
403    pub name: String,
404    #[serde(rename(deserialize = "type", serialize = "@type"))]
405    pub joint_type: JointType,
406    #[serde(default)]
407    pub origin: Pose,
408    pub parent: LinkName,
409    pub child: LinkName,
410    #[serde(default)]
411    pub axis: Axis,
412    #[serde(default)]
413    pub limit: JointLimit,
414    #[serde(skip_serializing_if = "Option::is_none")]
415    pub calibration: Option<Calibration>,
416    #[serde(skip_serializing_if = "Option::is_none")]
417    pub dynamics: Option<Dynamics>,
418    #[serde(skip_serializing_if = "Option::is_none")]
419    pub mimic: Option<Mimic>,
420    #[serde(skip_serializing_if = "Option::is_none")]
421    pub safety_controller: Option<SafetyController>,
422}
423
424#[derive(Debug, Deserialize, Serialize, Default, Clone)]
425pub struct Calibration {
426    // `default` is needed when using `deserialize_with`: https://github.com/serde-rs/serde/issues/723#issuecomment-368135287
427    #[serde(
428        rename(serialize = "@rising"),
429        default,
430        skip_serializing_if = "Option::is_none"
431    )]
432    #[serde(deserialize_with = "de_opt_f64")]
433    pub rising: Option<f64>,
434    // `default` is needed when using `deserialize_with`: https://github.com/serde-rs/serde/issues/723#issuecomment-368135287
435    #[serde(
436        rename(serialize = "@falling"),
437        default,
438        skip_serializing_if = "Option::is_none"
439    )]
440    #[serde(deserialize_with = "de_opt_f64")]
441    pub falling: Option<f64>,
442}
443
444#[derive(Debug, Deserialize, Serialize, Default, Clone)]
445pub struct Dynamics {
446    #[serde(rename(serialize = "@damping"), default)]
447    #[serde(deserialize_with = "de_f64")]
448    pub damping: f64,
449    #[serde(rename(serialize = "@friction"), default)]
450    #[serde(deserialize_with = "de_f64")]
451    pub friction: f64,
452}
453
454/// Top level struct to access urdf.
455///
456/// # Compatibility Note
457///
458/// This type and its descendant types implement `serde::Serialize` and
459/// `serde::Deserialize`. However, because XML serialization and deserialization
460/// using `serde` require embedding special markers to distinguish between
461/// attributes and elements, there is no stability guarantee as to exactly which
462/// `serde` representation these types will be serialized with.
463///
464/// In other words, serialization and deserialization without going through
465/// functions provided by this crate such as [`read_from_string`](crate::read_from_string)
466/// or [`write_to_string`](crate::write_to_string) may not work as expected at all
467/// or in the future.
468///
469/// This is intentional to remove the need to create semver-incompatible
470/// release of this crate every time an XML-related crate, which frequently
471/// creates semver-incompatible releases is updated. And it also allows
472/// improving our situation of depending on multiple XML-related crates without
473/// a change considered breaking.
474#[derive(Debug, Deserialize, Serialize, Clone)]
475#[serde(rename = "robot")]
476pub struct Robot {
477    #[serde(rename(serialize = "@name"), default)]
478    pub name: String,
479
480    #[serde(rename = "link", default, skip_serializing_if = "Vec::is_empty")]
481    pub links: Vec<Link>,
482
483    #[serde(rename = "joint", default, skip_serializing_if = "Vec::is_empty")]
484    pub joints: Vec<Joint>,
485
486    #[serde(rename = "material", default, skip_serializing_if = "Vec::is_empty")]
487    pub materials: Vec<Material>,
488}
489
490fn de_f64<'de, D>(deserializer: D) -> Result<f64, D::Error>
491where
492    D: Deserializer<'de>,
493{
494    deserializer.deserialize_str(F64Visitor)
495}
496fn de_opt_f64<'de, D>(deserializer: D) -> Result<Option<f64>, D::Error>
497where
498    D: Deserializer<'de>,
499{
500    deserializer.deserialize_option(OptF64Visitor)
501}
502
503struct F64Visitor;
504impl Visitor<'_> for F64Visitor {
505    type Value = f64;
506
507    fn expecting(&self, formatter: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
508        formatter.write_str("a string containing one floating point value")
509    }
510
511    fn visit_str<E>(self, v: &str) -> Result<Self::Value, E>
512    where
513        E: serde::de::Error,
514    {
515        let res = v.trim().parse::<f64>().map_err(E::custom)?;
516        Ok(res)
517    }
518}
519struct OptF64Visitor;
520impl<'de> Visitor<'de> for OptF64Visitor {
521    type Value = Option<f64>;
522
523    fn expecting(&self, formatter: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
524        formatter.write_str("a string containing one floating point value")
525    }
526
527    fn visit_some<D>(self, deserializer: D) -> Result<Self::Value, D::Error>
528    where
529        D: Deserializer<'de>,
530    {
531        deserializer.deserialize_str(F64Visitor).map(Some)
532    }
533
534    fn visit_none<E>(self) -> Result<Self::Value, E>
535    where
536        E: serde::de::Error,
537    {
538        Ok(None)
539    }
540}