libits_client/reception/exchange/
mobile_perceived_object.rs

1// Software Name: its-client
2// SPDX-FileCopyrightText: Copyright (c) 2016-2022 Orange
3// SPDX-License-Identifier: MIT License
4//
5// This software is distributed under the MIT license, see LICENSE.txt file for more details.
6//
7// Author: Frédéric GARDES <frederic.gardes@orange.com> et al.
8// Software description: This Intelligent Transportation Systems (ITS) [MQTT](https://mqtt.org/) client based on the [JSon](https://www.json.org) [ETSI](https://www.etsi.org/committee/its) specification transcription provides a ready to connect project for the mobility (connected and autonomous vehicles, road side units, vulnerable road users,...).
9use core::cmp;
10
11use crate::reception::exchange::mobile;
12use crate::reception::exchange::mobile::{speed_from_yaw_angle, Mobile};
13use crate::reception::exchange::perceived_object::PerceivedObject;
14use crate::reception::exchange::reference_position::ReferencePosition;
15use crate::reception::typed::Typed;
16use log::warn;
17use serde::{Deserialize, Serialize};
18
19#[derive(Clone, Debug, Hash, Serialize, Deserialize)]
20pub struct MobilePerceivedObject {
21    pub perceived_object: PerceivedObject,
22    pub mobile_id: u32,
23    pub reference_position: ReferencePosition,
24    pub speed: u16,
25    pub heading: u16,
26}
27
28impl MobilePerceivedObject {
29    pub(crate) fn new(
30        perceived_object: PerceivedObject,
31        cpm_station_id: u32,
32        cpm_position: &ReferencePosition,
33        cpm_heading: u16,
34    ) -> Self {
35        let compute_mobile_id = compute_id(perceived_object.object_id, cpm_station_id);
36        let computed_reference_position = compute_position(
37            perceived_object.x_distance,
38            perceived_object.y_distance,
39            cpm_position,
40            cpm_heading,
41        );
42        let computed_speed = compute_speed(perceived_object.x_speed, perceived_object.y_speed);
43        let computed_heading = compute_heading(perceived_object.y_distance, cpm_heading);
44
45        Self {
46            perceived_object,
47            mobile_id: compute_mobile_id,
48            reference_position: computed_reference_position,
49            speed: computed_speed,
50            heading: computed_heading,
51        }
52    }
53}
54
55impl Mobile for MobilePerceivedObject {
56    fn mobile_id(&self) -> u32 {
57        self.mobile_id
58    }
59
60    fn position(&self) -> &ReferencePosition {
61        &self.reference_position
62    }
63
64    fn speed(&self) -> Option<u16> {
65        Some(self.speed)
66    }
67
68    fn heading(&self) -> Option<u16> {
69        Some(self.heading)
70    }
71}
72
73impl cmp::PartialEq for MobilePerceivedObject {
74    fn eq(&self, other: &Self) -> bool {
75        self.perceived_object == other.perceived_object
76            && self.mobile_id == other.mobile_id
77            && self.reference_position == other.reference_position
78            && self.heading == other.heading
79            && self.speed == other.speed
80    }
81}
82
83impl Typed for MobilePerceivedObject {
84    fn get_type() -> String {
85        "po".to_string()
86    }
87}
88
89fn compute_id(object_id: u8, cpm_station_id: u32) -> u32 {
90    let string_id = format!("{}{}", cpm_station_id, object_id);
91    match string_id.parse() {
92        Ok(id) => id,
93        Err(_err) => {
94            warn!(
95                "unable to generate a mobile id with {}, we create a short one",
96                string_id
97            );
98            cpm_station_id + object_id as u32
99        }
100    }
101}
102
103fn compute_position(
104    x_distance: i32,
105    y_distance: i32,
106    cpm_position: &ReferencePosition,
107    cpm_heading: u16,
108) -> ReferencePosition {
109    let x_offset_meters = x_distance as f64 / 100.0;
110    let y_offset_meters = y_distance as f64 / 100.0;
111    let heading_in_degrees = mobile::heading_in_degrees(cpm_heading);
112    return cpm_position
113        .get_destination(x_offset_meters, heading_in_degrees)
114        .get_destination(y_offset_meters, (heading_in_degrees - 90.0 + 360.0) % 360.0);
115}
116
117fn compute_speed(x_speed: i16, y_speed: i16) -> u16 {
118    speed_from_yaw_angle(x_speed, y_speed)
119}
120
121fn compute_heading(y_distance: i32, cpm_heading: u16) -> u16 {
122    match y_distance {
123        y if y < 0 => (cpm_heading + 1800) % 3600,
124        _ => cpm_heading,
125    }
126}
127
128#[cfg(test)]
129mod tests {
130    use crate::reception::exchange::mobile_perceived_object::{
131        compute_heading, compute_id, compute_position, MobilePerceivedObject,
132    };
133    use crate::reception::exchange::perceived_object::PerceivedObject;
134    use crate::reception::exchange::reference_position::ReferencePosition;
135
136    #[test]
137    fn it_can_compute_a_position() {
138        //south with x
139        assert_eq!(
140            compute_position(
141                50000,
142                0,
143                &ReferencePosition {
144                    latitude: 434667520,
145                    longitude: 1205862,
146                    altitude: 220000,
147                },
148                1800,
149            ),
150            ReferencePosition {
151                latitude: 434622516,
152                longitude: 1205862,
153                altitude: 220000,
154            }
155        );
156        // east with y
157        assert_eq!(
158            compute_position(
159                0,
160                10000,
161                &ReferencePosition {
162                    latitude: 434667520,
163                    longitude: 1205862,
164                    altitude: 220000,
165                },
166                1800,
167            ),
168            ReferencePosition {
169                latitude: 434667520,
170                longitude: 1218219,
171                altitude: 220000,
172            }
173        );
174    }
175
176    #[test]
177    fn it_can_compute_an_id() {
178        //not too large, we concatenate
179        assert_eq!(compute_id(1, 100), 1001);
180        assert_eq!(compute_id(1, 400000000), 4000000001);
181        //too large, we add
182        assert_eq!(compute_id(1, 500000000), 500000001);
183    }
184
185    #[test]
186    fn it_can_compute_a_heading() {
187        //east
188        assert_eq!(compute_heading(50000, 900), 900);
189        assert_eq!(compute_heading(-50000, 2700), 900);
190        //south
191        assert_eq!(compute_heading(50000, 1800), 1800);
192        assert_eq!(compute_heading(-50000, 1800), 0);
193        //west
194        assert_eq!(compute_heading(50000, 2700), 2700);
195        assert_eq!(compute_heading(-50000, 2700), 900);
196    }
197
198    #[test]
199    fn create_a_new() {
200        //south east with x and y
201        assert_eq!(
202            MobilePerceivedObject::new(
203                PerceivedObject {
204                    object_id: 1,
205                    x_distance: 50000,
206                    y_distance: 10000,
207                    ..Default::default()
208                },
209                10,
210                &ReferencePosition {
211                    latitude: 434667520,
212                    longitude: 1205862,
213                    altitude: 220000,
214                },
215                1800,
216            ),
217            MobilePerceivedObject {
218                perceived_object: PerceivedObject {
219                    object_id: 1,
220                    x_distance: 50000,
221                    y_distance: 10000,
222                    ..Default::default()
223                },
224                mobile_id: 101,
225                reference_position: ReferencePosition {
226                    latitude: 434622516,
227                    longitude: 1218218,
228                    altitude: 220000,
229                },
230                speed: 0,
231                heading: 1800,
232            }
233        );
234    }
235}
libits_client/reception/exchange/mobile_perceived_object.rs

libits_client/reception/exchange/
mobile_perceived_object.rs
