fsds_rs/
types.rs

1//! This module contains the definitions of the input and output types used to
2//! communicate with the simulator.
3//!
4//! Struct are serialized to `msgpack_rpc::Value::Map` and vice versa in order
5//! to be sent and received from the simulator.
6
7use fsds_rs_derive::FromIntoValue;
8use msgpack_rpc::Value;
9use std::{
10    any::Any,
11    ops::{Add, Div, DivAssign, Mul, MulAssign, Sub},
12};
13use struct_iterable::Iterable;
14
15/// Utility to convert any type to a `msgpack_rpc::Value`.
16fn any_to_value(value: &dyn Any) -> Value {
17    if let Some(value) = value.downcast_ref::<u64>() {
18        Value::from(*value)
19    } else if let Some(value) = value.downcast_ref::<f64>() {
20        Value::from(*value)
21    } else if let Some(value) = value.downcast_ref::<bool>() {
22        Value::from(*value)
23    } else if let Some(value) = value.downcast_ref::<String>() {
24        Value::String(value.clone().into())
25    } else if let Some(value) = value.downcast_ref::<Vec<u64>>() {
26        Value::Array(value.iter().map(|v| Value::from(*v)).collect())
27    } else if let Some(value) = value.downcast_ref::<Vec<f64>>() {
28        Value::Array(value.iter().map(|v| Value::from(*v)).collect())
29    } else if let Some(value) = value.downcast_ref::<Vec<bool>>() {
30        Value::Array(value.iter().map(|v| Value::from(*v)).collect())
31    } else if let Some(value) = value.downcast_ref::<Vec<String>>() {
32        Value::Array(
33            value
34                .iter()
35                .map(|v| Value::String(v.clone().into()))
36                .collect(),
37        )
38    } else {
39        Value::Nil
40    }
41}
42
43/// ---------- ///
44/// IMAGE TYPE ///
45/// ---------- ///
46#[derive(Clone, Copy)]
47pub enum ImageType {
48    Scene = 0,
49    DepthPlanner = 1,
50    DepthPerspective = 2,
51    DepthVis = 3,
52    DisparityNormalized = 4,
53    Segmentation = 5,
54    SurfaceNormals = 6,
55    Infrared = 7,
56}
57
58impl From<ImageType> for Value {
59    fn from(value: ImageType) -> Self {
60        Value::from(value as u64)
61    }
62}
63
64impl TryFrom<Value> for ImageType {
65    type Error = anyhow::Error;
66
67    fn try_from(value: Value) -> Result<Self, Self::Error> {
68        match value {
69            // TODO: removeunwrap below
70            Value::Integer(value) => Ok(match value.as_u64().unwrap() {
71                0 => ImageType::Scene,
72                1 => ImageType::DepthPlanner,
73                2 => ImageType::DepthPerspective,
74                3 => ImageType::DepthVis,
75                4 => ImageType::DisparityNormalized,
76                5 => ImageType::Segmentation,
77                6 => ImageType::SurfaceNormals,
78                7 => ImageType::Infrared,
79                _ => return Err(anyhow::anyhow!("Invalid ImageType")),
80            }),
81            _ => Err(anyhow::anyhow!("Invalid ImageType")),
82        }
83    }
84}
85
86/// --------- ///
87/// VECTOR 3R ///
88/// --------- ///
89#[derive(Copy, Clone, Default, Iterable, FromIntoValue, Debug)]
90pub struct Vector3r {
91    pub x_val: f64,
92    pub y_val: f64,
93    pub z_val: f64,
94}
95
96impl Vector3r {
97    pub fn nan_vector3r() -> Self {
98        Self {
99            x_val: f64::NAN,
100            y_val: f64::NAN,
101            z_val: f64::NAN,
102        }
103    }
104
105    pub fn dot(&self, other: &Self) -> f64 {
106        self.x_val * other.x_val + self.y_val * other.y_val + self.z_val * other.z_val
107    }
108
109    pub fn cross(&self, other: &Self) -> Self {
110        Self {
111            x_val: self.y_val * other.z_val - self.z_val * other.y_val,
112            y_val: self.z_val * other.x_val - self.x_val * other.z_val,
113            z_val: self.x_val * other.y_val - self.y_val * other.x_val,
114        }
115    }
116
117    pub fn get_length(&self) -> f64 {
118        (self.x_val.powi(2) + self.y_val.powi(2) + self.z_val.powi(2)).sqrt()
119    }
120
121    pub fn distance_to(&self, other: &Self) -> f64 {
122        (*self - *other).get_length()
123    }
124}
125
126impl Add for Vector3r {
127    type Output = Self;
128
129    fn add(self, other: Self) -> Self {
130        Self {
131            x_val: self.x_val + other.x_val,
132            y_val: self.y_val + other.y_val,
133            z_val: self.z_val + other.z_val,
134        }
135    }
136}
137
138impl Sub for Vector3r {
139    type Output = Self;
140
141    fn sub(self, other: Self) -> Self {
142        Self {
143            x_val: self.x_val - other.x_val,
144            y_val: self.y_val - other.y_val,
145            z_val: self.z_val - other.z_val,
146        }
147    }
148}
149
150impl DivAssign<f64> for Vector3r {
151    fn div_assign(&mut self, other: f64) {
152        self.x_val /= other;
153        self.y_val /= other;
154        self.z_val /= other;
155    }
156}
157
158impl MulAssign<f64> for Vector3r {
159    fn mul_assign(&mut self, other: f64) {
160        self.x_val *= other;
161        self.y_val *= other;
162        self.z_val *= other;
163    }
164}
165
166/// ----------- ///
167/// QUATERNIONR ///
168/// ----------- ///
169
170#[derive(Copy, Clone, Default, Iterable, FromIntoValue, Debug)]
171pub struct Quaternionr {
172    w_val: f64,
173    x_val: f64,
174    y_val: f64,
175    z_val: f64,
176}
177
178impl Quaternionr {
179    pub fn new(w_val: f64, x_val: f64, y_val: f64, z_val: f64) -> Self {
180        Self {
181            w_val,
182            x_val,
183            y_val,
184            z_val,
185        }
186    }
187
188    pub fn nan_quaternionr() -> Self {
189        Self {
190            w_val: f64::NAN,
191            x_val: f64::NAN,
192            y_val: f64::NAN,
193            z_val: f64::NAN,
194        }
195    }
196
197    pub fn dot(&self, other: &Self) -> f64 {
198        self.w_val * other.w_val
199            + self.x_val * other.x_val
200            + self.y_val * other.y_val
201            + self.z_val * other.z_val
202    }
203
204    pub fn cross(&self, other: &Self) -> Self {
205        let mut diff = *self * *other - *other * *self;
206        diff /= 2.0;
207        diff
208    }
209
210    pub fn outer_product(&self, other: &Self) -> Self {
211        let mut double = self.inverse() * *other - other.inverse() * *self;
212        double /= 2.0;
213        double
214    }
215
216    pub fn rotate(&self, other: &Quaternionr) -> Result<Self, anyhow::Error> {
217        if other.get_length() == 1.0 {
218            return Ok(*other * *self * other.inverse());
219        }
220
221        Err(anyhow::anyhow!("Quaternion is not normalized"))
222    }
223
224    pub fn conjugate(&self) -> Self {
225        Self {
226            w_val: self.w_val,
227            x_val: -self.x_val,
228            y_val: -self.y_val,
229            z_val: -self.z_val,
230        }
231    }
232
233    pub fn star(&self) -> Self {
234        self.conjugate()
235    }
236
237    pub fn inverse(&self) -> Self {
238        let mut star = self.star();
239        star /= self.dot(self);
240
241        star
242    }
243
244    pub fn sgn(&self) -> Quaternionr {
245        let mut self_deref = *self;
246        self_deref /= self.get_length();
247
248        self_deref
249    }
250
251    pub fn get_length(&self) -> f64 {
252        (self.w_val.powi(2) + self.x_val.powi(2) + self.y_val.powi(2) + self.z_val.powi(2)).sqrt()
253    }
254}
255
256impl Add for Quaternionr {
257    type Output = Self;
258
259    fn add(self, other: Self) -> Self {
260        Self {
261            w_val: self.w_val + other.w_val,
262            x_val: self.x_val + other.x_val,
263            y_val: self.y_val + other.y_val,
264            z_val: self.z_val + other.z_val,
265        }
266    }
267}
268
269impl Sub for Quaternionr {
270    type Output = Self;
271
272    fn sub(self, other: Self) -> Self {
273        Self {
274            w_val: self.w_val - other.w_val,
275            x_val: self.x_val - other.x_val,
276            y_val: self.y_val - other.y_val,
277            z_val: self.z_val - other.z_val,
278        }
279    }
280}
281
282impl Mul for Quaternionr {
283    type Output = Self;
284
285    fn mul(self, other: Self) -> Self {
286        let (t, x, y, z) = (self.w_val, self.x_val, self.y_val, self.z_val);
287        let (a, b, c, d) = (other.w_val, other.x_val, other.y_val, other.z_val);
288
289        Self {
290            w_val: a * t - b * x - c * y - d * z,
291            x_val: b * t + a * x + d * y - c * z,
292            y_val: c * t + a * y + b * z - d * x,
293            z_val: d * t + z * a + c * x - b * y,
294        }
295    }
296}
297
298impl Div for Quaternionr {
299    type Output = Self;
300
301    #[allow(clippy::suspicious_arithmetic_impl)]
302    fn div(self, other: Self) -> Self {
303        self * other.inverse()
304    }
305}
306
307impl DivAssign<f64> for Quaternionr {
308    fn div_assign(&mut self, other: f64) {
309        self.w_val /= other;
310        self.x_val /= other;
311        self.y_val /= other;
312        self.z_val /= other;
313    }
314}
315
316impl From<Vector3r> for Quaternionr {
317    fn from(value: Vector3r) -> Self {
318        Self {
319            w_val: value.x_val,
320            x_val: value.y_val,
321            y_val: value.z_val,
322            z_val: 0.0,
323        }
324    }
325}
326
327/// ---- ///
328/// POSE ///
329/// ---- ///
330#[derive(Copy, Clone, Default, Iterable, FromIntoValue)]
331pub struct Pose {
332    position: Vector3r,
333    orientation: Quaternionr,
334}
335
336impl Pose {
337    pub fn new(posizion_val: Option<Vector3r>, orientation_val: Option<Quaternionr>) -> Self {
338        Self {
339            position: posizion_val.unwrap_or(Vector3r::nan_vector3r()),
340            orientation: orientation_val.unwrap_or(Quaternionr::nan_quaternionr()),
341        }
342    }
343
344    pub fn nan_pose() -> Self {
345        Self {
346            position: Vector3r::nan_vector3r(),
347            orientation: Quaternionr::nan_quaternionr(),
348        }
349    }
350}
351
352/// --------- ///
353/// GEO POINT ///
354/// --------- ///
355#[derive(Copy, Clone, Default, Iterable, FromIntoValue)]
356pub struct GeoPoint {
357    latitude: f64,
358    longitude: f64,
359    altitude: f64,
360}
361
362/// ------------- ///
363/// IMAGE REQUEST ///
364/// ------------- ///
365#[derive(Iterable, Clone, FromIntoValue)]
366pub struct ImageRequest {
367    camera_name: String,
368    image_type: ImageType,
369    pixels_as_float: bool,
370    compress: bool,
371}
372
373impl Default for ImageRequest {
374    fn default() -> Self {
375        Self {
376            camera_name: "0".to_string(),
377            image_type: ImageType::Scene,
378            pixels_as_float: false,
379            compress: false,
380        }
381    }
382}
383
384/// -------------- ///
385/// IMAGE RESPONSE ///
386/// -------------- ///
387#[derive(Iterable, FromIntoValue)]
388pub struct ImageResponse {
389    image_data_uint8: u64,
390    image_data_float: f64,
391    camera_position: Vector3r,
392    camera_orientation: Quaternionr,
393    timestamp: u64, // TODO: SystemTime?
394    message: String,
395    pixels_as_float: f64,
396    compress: bool,
397    width: u64,
398    height: u64,
399    image_type: ImageType,
400}
401
402impl Default for ImageResponse {
403    fn default() -> Self {
404        Self {
405            image_data_uint8: 0,
406            image_data_float: 0.0,
407            camera_position: Default::default(),
408            camera_orientation: Default::default(),
409            timestamp: 0,
410            message: "".to_string(),
411            pixels_as_float: 0.0,
412            compress: true,
413            width: 0,
414            height: 0,
415            image_type: ImageType::Scene,
416        }
417    }
418}
419
420/// ------------ ///
421/// CAR CONTROLS ///
422/// ------------ ///
423#[derive(Iterable, FromIntoValue)]
424pub struct CarControls {
425    pub throttle: f64,
426    pub steering: f64,
427    pub brake: f64,
428    pub handbrake: bool,
429    pub is_manual_gear: bool,
430    pub manual_gear: u64,
431    pub gear_immediate: bool,
432}
433
434impl Default for CarControls {
435    fn default() -> Self {
436        Self {
437            throttle: Default::default(),
438            steering: Default::default(),
439            brake: Default::default(),
440            handbrake: false,
441            is_manual_gear: false,
442            manual_gear: Default::default(),
443            gear_immediate: true,
444        }
445    }
446}
447
448/// ---------------- ///
449/// KINEMATICS STATE ///
450/// ---------------- ///
451#[derive(Iterable, FromIntoValue, Default, Debug)]
452pub struct KinematicsState {
453    pub position: Vector3r,
454    pub orientation: Quaternionr,
455    pub linear_velocity: Vector3r,
456    pub angular_velocity: Vector3r,
457    pub linear_acceleration: Vector3r,
458    pub angular_acceleration: Vector3r,
459}
460
461/// ----------------- ///
462/// ENVIRONMENT STATE ///
463/// ----------------- ///
464#[derive(Iterable, FromIntoValue, Default)]
465pub struct EnvironmentState {
466    pub position: Vector3r,
467    pub geo_point: GeoPoint,
468    pub gravity: Vector3r,
469    pub air_pressure: f64,
470    pub temperature: f64,
471    pub air_density: f64,
472}
473
474/// -------------- ///
475/// COLLISION INFO ///
476/// -------------- ///
477#[derive(Iterable, FromIntoValue)]
478pub struct CollisionInfo {
479    pub has_collided: bool,
480    pub normal: Vector3r,
481    pub impact_point: Vector3r,
482    pub position: Vector3r,
483    pub penetration_depth: f64,
484    pub timestamp: f64, // TODO: SystemTime?
485    pub object_name: String,
486    pub object_id: u64,
487}
488
489/// --------- ///
490/// CAR STATE ///
491/// --------- ///
492#[derive(Iterable, FromIntoValue)]
493pub struct CarState {
494    pub speed: f64,
495    pub kinematics_estimated: KinematicsState,
496    pub timestamp: u64, // TODO: SystemTime?
497}
498
499/// ----------- ///
500/// POSITION 2D ///
501/// ----------- ///
502#[derive(Iterable, FromIntoValue, Default)]
503pub struct Position2D {
504    pub x_val: f64,
505    pub y_val: f64,
506}
507
508/// ------------- ///
509/// REFEREE STATE ///
510/// ------------- ///
511#[derive(Iterable, Default)]
512pub struct RefereeState {
513    pub doo_counter: u64,
514    pub laps: f64,
515    pub initial_position: Position2D,
516    pub cones: Vec<Position2D>, // TODO: Vec<Position2D> does not implement Into<Value>
517}
518
519// TODO:
520// ----------------- ///
521// PROJECTION MATRIX ///
522// ----------------- ///
523// #[derive(Iterable, FromIntoValue, Default)]
524// pub struct ProjectionMatrix {
525//     pub matrix: Vec<_>,
526// }
fsds_rs/types.rs

fsds_rs/
types.rs
