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 #[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#[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 #[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 #[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#[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 #[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 #[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#[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}