airsim_client/types/
pose.rs

1use msgpack_rpc::{message::Response, Utf8String, Value};
2
3use crate::Vector3;
4
5#[derive(Debug, Clone, Copy)]
6pub struct Position3 {
7    pub x: f32,
8    pub y: f32,
9    pub z: f32,
10}
11
12impl Position3 {
13    pub fn new(x: f32, y: f32, z: f32) -> Self {
14        Position3 { x, y, z }
15    }
16}
17
18impl From<Value> for Position3 {
19    fn from(msgpack: Value) -> Self {
20        let payload: &Vec<(Value, Value)> = msgpack.as_map().unwrap();
21
22        // position
23        let mut points = vec![];
24        for (_, v) in payload {
25            let p = v.as_f64().unwrap() as f32;
26            points.push(p);
27        }
28        Position3::new(points[0], points[1], points[2])
29    }
30}
31
32#[derive(Debug, Clone, Copy)]
33pub struct Orientation3 {
34    /// roll angle, in radians
35    pub roll: f32,
36    /// pitch angle, in radians
37    pub pitch: f32,
38    /// yaw angle, in radians
39    pub yaw: f32,
40}
41
42impl Orientation3 {
43    pub fn new(roll: f32, pitch: f32, yaw: f32) -> Self {
44        Orientation3 { roll, pitch, yaw }
45    }
46}
47
48impl From<Value> for Quaternion {
49    fn from(msgpack: Value) -> Self {
50        let payload: &Vec<(Value, Value)> = msgpack.as_map().unwrap();
51
52        // quaternion
53        let mut quats = vec![];
54        for (_, q_i) in payload {
55            let q = q_i.as_f64().unwrap() as f32;
56            quats.push(q);
57        }
58        Quaternion::new(quats[0], quats[1], quats[2], quats[3])
59    }
60}
61
62#[derive(Debug, Clone, Copy)]
63pub struct Quaternion {
64    pub w: f32,
65    pub x: f32,
66    pub y: f32,
67    pub z: f32,
68}
69
70impl Quaternion {
71    pub fn new(w: f32, x: f32, y: f32, z: f32) -> Self {
72        Self { w, x, y, z }
73    }
74}
75
76#[derive(Debug, Clone, Copy)]
77pub struct Pose3 {
78    pub position: Position3,
79    pub orientation: Quaternion,
80}
81
82impl Pose3 {
83    pub fn new(position: Position3, orientation: Quaternion) -> Self {
84        Self { position, orientation }
85    }
86
87    pub(crate) fn as_msgpack(&self) -> Value {
88        // position
89        let x_val: Utf8String = "x_val".into();
90        let y_val: Utf8String = "y_val".into();
91        let z_val: Utf8String = "z_val".into();
92
93        let position = Value::Map(vec![
94            (Value::String(x_val.to_owned()), Value::F32(self.position.x)),
95            (Value::String(y_val.to_owned()), Value::F32(self.position.y)),
96            (Value::String(z_val.to_owned()), Value::F32(self.position.z)),
97        ]);
98
99        let pos_msg: Vec<(msgpack_rpc::Value, msgpack_rpc::Value)> = position.as_map().map(|x| x.to_owned()).unwrap();
100        let position_msg = Value::Map(pos_msg);
101
102        // orientation
103        let w_val: Utf8String = "w_val".into();
104
105        let orientation = Value::Map(vec![
106            (Value::String(w_val), Value::F32(self.orientation.w)),
107            (Value::String(x_val), Value::F32(self.orientation.x)),
108            (Value::String(y_val), Value::F32(self.orientation.y)),
109            (Value::String(z_val), Value::F32(self.orientation.z)),
110        ]);
111
112        let orr_msg: Vec<(msgpack_rpc::Value, msgpack_rpc::Value)> =
113            orientation.as_map().map(|x| x.to_owned()).unwrap();
114        let orientation_msg = Value::Map(orr_msg);
115
116        // pose
117        let position_key: Utf8String = "position".into();
118        let orientation_key: Utf8String = "orientation".into();
119
120        let pose = Value::Map(vec![
121            (Value::String(position_key), position_msg),
122            (Value::String(orientation_key), orientation_msg),
123        ]);
124
125        let pose_msg: Vec<(msgpack_rpc::Value, msgpack_rpc::Value)> = pose.as_map().map(|x| x.to_owned()).unwrap();
126        Value::Map(pose_msg)
127    }
128}
129
130impl From<Response> for Pose3 {
131    fn from(msgpack: Response) -> Self {
132        println!("\n received pose: {msgpack:?} \n \n");
133        match msgpack.result {
134            Ok(res) => {
135                let payload: &Vec<(Value, Value)> = res.as_map().unwrap();
136
137                // position
138                let position: Position3 = payload[0].1.to_owned().into();
139                // println!("pose3 position: {position:?}");
140
141                // orientation
142                let orientation: Quaternion = payload[1].1.to_owned().into();
143                // println!("pose3 orientation: {orientation:?}");
144
145                Self { position, orientation }
146            }
147            Err(_) => panic!("Could not decode result from Pose3 msgpack"),
148        }
149    }
150}
151
152#[derive(Debug, Clone, Copy)]
153pub struct Orientation2 {
154    /// roll angle, in radians
155    pub roll: f32,
156    /// pitch angle, in radians
157    pub pitch: f32,
158}
159
160impl Orientation2 {
161    pub fn new(roll: f32, pitch: f32) -> Self {
162        Orientation2 { roll, pitch }
163    }
164}
165
166#[derive(Debug, Clone, Copy)]
167pub struct Velocity3 {
168    pub vx: f32,
169    pub vy: f32,
170    pub vz: f32,
171}
172
173impl Velocity3 {
174    pub fn new(vx: f32, vy: f32, vz: f32) -> Self {
175        Velocity3 { vx, vy, vz }
176    }
177}
178
179#[derive(Debug, Clone, Copy)]
180pub struct Velocity2 {
181    pub vx: f32,
182    pub vy: f32,
183}
184
185impl Velocity2 {
186    pub fn new(vx: f32, vy: f32) -> Self {
187        Velocity2 { vx, vy }
188    }
189}
190
191/// The kinematic state of the vehicle
192#[derive(Debug, Clone, Copy)]
193pub struct KinematicsState {
194    /// position in the frame of the vehicle's starting point
195    pub position: Position3,
196    /// orientation in the frame of the vehicle's starting point
197    pub orientation: Orientation3,
198    /// linear velocity in ENU body frame
199    pub linear_velocity: Vector3,
200    /// angular velocity in ENU body frame
201    pub angular_velocity: Vector3,
202    /// linear acceleration in ENU body frame
203    pub linear_acceleration: Vector3,
204    /// angular acceleration in ENU body frame
205    pub angular_acceleration: Vector3,
206}
207
208impl KinematicsState {
209    pub fn new(
210        position: Position3,
211        orientation: Orientation3,
212        linear_velocity: Vector3,
213        angular_velocity: Vector3,
214        linear_acceleration: Vector3,
215        angular_acceleration: Vector3,
216    ) -> Self {
217        KinematicsState {
218            position,
219            orientation,
220            linear_velocity,
221            angular_velocity,
222            linear_acceleration,
223            angular_acceleration,
224        }
225    }
226}
227
228impl From<Value> for KinematicsState {
229    fn from(msgpack: Value) -> Self {
230        let payload: &Vec<(Value, Value)> = msgpack.as_map().unwrap();
231
232        // position
233        let mut points = vec![];
234        let position_msgpack: &Vec<(Value, Value)> = payload[0].1.as_map().unwrap();
235        for (_, v) in position_msgpack {
236            let p = v.as_f64().unwrap() as f32;
237            points.push(p);
238        }
239        let position = Position3::new(points[0], points[1], points[2]);
240
241        // orientation
242        let mut points = vec![];
243        let orientation_msgpack: &Vec<(Value, Value)> = payload[1].1.as_map().unwrap();
244        for (_, v) in orientation_msgpack {
245            let p = v.as_f64().unwrap() as f32;
246            points.push(p);
247        }
248        let orientation = Orientation3::new(points[0], points[1], points[2]);
249
250        // linear velocity
251        let mut points = vec![];
252        let linear_velocity_msgpack: &Vec<(Value, Value)> = payload[2].1.as_map().unwrap();
253        for (_, v) in linear_velocity_msgpack {
254            let p = v.as_f64().unwrap() as f32;
255            points.push(p);
256        }
257        let linear_velocity = Vector3::new(points[0], points[1], points[2]);
258
259        // angular velocity
260        let mut points = vec![];
261        let angular_velocity_msgpack: &Vec<(Value, Value)> = payload[3].1.as_map().unwrap();
262        for (_, v) in angular_velocity_msgpack {
263            let p = v.as_f64().unwrap() as f32;
264            points.push(p);
265        }
266        let angular_velocity = Vector3::new(points[0], points[1], points[2]);
267
268        // linear acceleration
269        let mut points = vec![];
270        let linear_acceleration_msgpack: &Vec<(Value, Value)> = payload[4].1.as_map().unwrap();
271        for (_, v) in linear_acceleration_msgpack {
272            let p = v.as_f64().unwrap() as f32;
273            points.push(p);
274        }
275        let linear_acceleration = Vector3::new(points[0], points[1], points[2]);
276
277        // linear acceleration
278        let mut points = vec![];
279        let angular_acceleration_msgpack: &Vec<(Value, Value)> = payload[5].1.as_map().unwrap();
280        for (_, v) in angular_acceleration_msgpack {
281            let p = v.as_f64().unwrap() as f32;
282            points.push(p);
283        }
284        let angular_acceleration = Vector3::new(points[0], points[1], points[2]);
285
286        Self {
287            position,
288            orientation,
289            linear_velocity,
290            angular_velocity,
291            linear_acceleration,
292            angular_acceleration,
293        }
294    }
295}
airsim_client/types/pose.rs

airsim_client/types/
pose.rs
