724 lines
24 KiB
Rust
724 lines
24 KiB
Rust
//! Application Logic
|
|
|
|
#[cfg(feature = "spat")]
|
|
use alloc::rc::Rc;
|
|
#[cfg(feature = "spat")]
|
|
use alloc::vec::Vec;
|
|
|
|
use log::info;
|
|
#[cfg(feature = "spat")]
|
|
use log::warn;
|
|
#[cfg(feature = "spat")]
|
|
use log::{debug, error};
|
|
|
|
#[cfg(feature = "spat")]
|
|
use crate::{applogic::v2x::map, geo_alg};
|
|
|
|
pub mod v2x;
|
|
|
|
#[cfg(feature = "cam")]
|
|
pub mod cam;
|
|
#[cfg(any(feature = "cam_tx", test))]
|
|
pub mod cam_tx;
|
|
#[cfg(feature = "denm")]
|
|
pub mod denm;
|
|
#[cfg(feature = "spat")]
|
|
pub mod tlc;
|
|
|
|
#[cfg(feature = "spat")]
|
|
const TLC_CACHE_LIFETIME: chrono::Duration = chrono::Duration::minutes(1);
|
|
#[cfg(feature = "spat")]
|
|
const PRUNE_INTERVAL: chrono::Duration = chrono::Duration::seconds(5);
|
|
|
|
#[cfg(feature = "spat")]
|
|
const GLOSA_SEARCH_DIST_M: f32 = 400.;
|
|
#[cfg(feature = "spat")]
|
|
const GLOSA_SEARCH_ANGLE_PARAMS: [(f32, f32); 2] = [(400., 45.), (50., 180.)]; // far and near (distance, angle) tuple
|
|
#[cfg(feature = "spat")]
|
|
const GLOSA_LANE_HEADING_DIFF_DEG: f32 = 35.;
|
|
#[cfg(feature = "spat")]
|
|
const GLOSA_AT_LANE_OFFSET_M: f64 = 10.;
|
|
#[cfg(feature = "spat")]
|
|
const GLOSA_LANE_LEFT_OFFSET_M: f64 = 20.;
|
|
#[cfg(feature = "spat")]
|
|
const GLOSA_STOP_LINE_PASSED_M: f64 = 5.;
|
|
|
|
#[allow(unused)]
|
|
pub struct State {
|
|
initialized: bool,
|
|
time: chrono::NaiveDateTime,
|
|
|
|
#[allow(unused)]
|
|
pos: PositionState,
|
|
|
|
#[cfg(feature = "spat")]
|
|
last_prune: chrono::NaiveDateTime,
|
|
#[cfg(feature = "spat")]
|
|
tlc_cache: tlc::Data,
|
|
|
|
// GLOSA
|
|
#[cfg(feature = "spat")]
|
|
#[allow(clippy::struct_field_names)]
|
|
glosa_state: GlosaState,
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
#[derive(Debug)]
|
|
pub enum GlosaOutput {
|
|
Error(alloc::string::String),
|
|
Searching(GlosaSearching),
|
|
Found(GlosaFound),
|
|
Locked(GlosaSignalInfo),
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
#[derive(Debug, PartialEq, Eq)]
|
|
pub struct GlosaSearching {
|
|
pub known_intersection_ids: Vec<u16>,
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
#[derive(Debug, PartialEq, Eq)]
|
|
pub struct GlosaFound {
|
|
pub intersection_id: u16,
|
|
pub approach_id: Option<u8>,
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
impl From<&TlData> for GlosaFound {
|
|
fn from(value: &TlData) -> Self {
|
|
Self {
|
|
intersection_id: value.intersection_id,
|
|
approach_id: value.approach_id,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
#[derive(Debug, Clone, PartialEq, Eq)]
|
|
pub struct GlosaSignalInfo {
|
|
pub intersection_id: u16,
|
|
pub signal_groups: Vec<SignalGroupData>,
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
#[derive(Debug, Default, Clone, PartialEq, Eq)]
|
|
pub struct SignalGroupData {
|
|
pub phase: SignalPhase,
|
|
pub maneuver: map::Maneuvers,
|
|
pub end_sec: Option<i16>,
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
#[derive(Debug, Default, Clone, PartialEq, Eq)]
|
|
pub enum SignalPhase {
|
|
#[default]
|
|
Unknown,
|
|
Red,
|
|
RedYellow,
|
|
Green,
|
|
Yellow,
|
|
}
|
|
|
|
#[allow(unused)]
|
|
#[derive(Debug, Default, Clone, PartialEq)]
|
|
pub struct GnssFix {
|
|
pub time: Option<chrono::NaiveDateTime>,
|
|
pub latitude_deg: f64,
|
|
pub longitude_deg: f64,
|
|
pub heading_deg: Option<f32>,
|
|
pub speed_mps: Option<f32>,
|
|
}
|
|
|
|
#[allow(unused)]
|
|
#[derive(Debug, Default, Clone, PartialEq)]
|
|
pub struct PositionState {
|
|
pub position: geo_types::Point,
|
|
pub heading_deg: Option<f32>,
|
|
pub speed_mps: Option<f32>,
|
|
}
|
|
|
|
impl From<&GnssFix> for PositionState {
|
|
fn from(value: &GnssFix) -> Self {
|
|
let position = geo_types::Point::new(value.longitude_deg, value.latitude_deg);
|
|
|
|
Self {
|
|
position,
|
|
heading_deg: value.heading_deg,
|
|
speed_mps: value.speed_mps,
|
|
}
|
|
}
|
|
}
|
|
impl From<GnssFix> for PositionState {
|
|
fn from(value: GnssFix) -> Self {
|
|
(&value).into()
|
|
}
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
impl core::fmt::Display for SignalPhase {
|
|
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
|
match self {
|
|
SignalPhase::Unknown => write!(f, "__"),
|
|
SignalPhase::Red => write!(f, "RE"),
|
|
SignalPhase::RedYellow => write!(f, "RY"),
|
|
SignalPhase::Green => write!(f, "GN"),
|
|
SignalPhase::Yellow => write!(f, "YE"),
|
|
}
|
|
}
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
impl From<c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc::MovementPhaseState> for SignalPhase {
|
|
fn from(value: c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc::MovementPhaseState) -> Self {
|
|
use c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc::MovementPhaseState;
|
|
|
|
match value {
|
|
MovementPhaseState::stop_And_Remain => SignalPhase::Red,
|
|
MovementPhaseState::pre_Movement => SignalPhase::RedYellow,
|
|
MovementPhaseState::permissive_Movement_Allowed
|
|
| MovementPhaseState::protected_Movement_Allowed => SignalPhase::Green,
|
|
MovementPhaseState::permissive_clearance | MovementPhaseState::protected_clearance => {
|
|
SignalPhase::Yellow
|
|
}
|
|
_ => SignalPhase::Unknown,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
#[derive(Debug, Default)]
|
|
enum GlosaState {
|
|
#[default]
|
|
Idle,
|
|
Locked(TlData),
|
|
}
|
|
|
|
/// Where we are and pre-processed information about the current intersection
|
|
#[cfg(feature = "spat")]
|
|
#[derive(Debug)]
|
|
struct TlData {
|
|
intersection_id: u16, // mostly for debug printing
|
|
intersection: Rc<core::cell::RefCell<v2x::map::Intersection>>,
|
|
|
|
lane_id: u8,
|
|
approach_id: Option<u8>,
|
|
|
|
signal_groups: Vec<u8>,
|
|
}
|
|
|
|
impl State {
|
|
pub fn new() -> Self {
|
|
Self {
|
|
initialized: false,
|
|
time: chrono::NaiveDateTime::default(),
|
|
pos: PositionState::default(),
|
|
#[cfg(feature = "spat")]
|
|
last_prune: chrono::NaiveDateTime::default(),
|
|
#[cfg(feature = "spat")]
|
|
tlc_cache: tlc::Data::new(TLC_CACHE_LIFETIME),
|
|
#[cfg(feature = "spat")]
|
|
glosa_state: GlosaState::default(),
|
|
}
|
|
}
|
|
|
|
#[allow(unused)]
|
|
#[cfg(feature = "esp")]
|
|
pub fn update_with_gpsfix(&mut self, fix: &GnssFix) {
|
|
self.pos = fix.into();
|
|
if let Some(time) = fix.time {
|
|
self.time = time;
|
|
|
|
if !self.initialized {
|
|
info!("GNSS fix: {}", self.print_fix());
|
|
}
|
|
self.initialized = true;
|
|
}
|
|
}
|
|
|
|
#[allow(unused)]
|
|
pub fn print_fix(&self) -> alloc::string::String {
|
|
alloc::format!(
|
|
"{:?}, {:.4} N, {:.4} E, {:.0}°, {:.2} m/s",
|
|
self.time,
|
|
self.pos.position.y(),
|
|
self.pos.position.x(),
|
|
self.pos.heading_deg.unwrap_or_default(),
|
|
self.pos.speed_mps.unwrap_or_default(),
|
|
)
|
|
}
|
|
|
|
#[allow(clippy::unused_self)]
|
|
pub fn prune(&mut self) {
|
|
#[cfg(feature = "spat")]
|
|
if self.last_prune + PRUNE_INTERVAL < self.time {
|
|
self.tlc_cache.prune(self.time);
|
|
|
|
info!("Known int.: {:?}", self.tlc_cache.known_intersections());
|
|
|
|
self.last_prune = self.time;
|
|
}
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
pub fn handle_mapem(&mut self, etsi: &tlc::MAPEM) {
|
|
self.tlc_cache.handle_mapem(self.time, etsi);
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
pub fn handle_spatem(&mut self, etsi: &tlc::SPATEM) {
|
|
self.tlc_cache.handle_spatem(self.time, etsi);
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
pub fn run_glosa(&mut self) -> GlosaOutput {
|
|
let Some(own_heading) = self.pos.heading_deg else {
|
|
use alloc::string::ToString;
|
|
|
|
return GlosaOutput::Error("Heading unknown".to_string());
|
|
};
|
|
|
|
let known_intersection_ids = self.tlc_cache.known_intersections();
|
|
let (state_update, output_data) = match &self.glosa_state {
|
|
// search for upcoming intersection
|
|
GlosaState::Idle => {
|
|
debug!(
|
|
"GLOSA: Searching with {} known intersections",
|
|
known_intersection_ids.len()
|
|
);
|
|
|
|
if let Some(search_result) =
|
|
Self::find_intersection(&self.pos.position, own_heading, &self.tlc_cache)
|
|
{
|
|
info!(
|
|
"GLOSA: Approach has signal groups: {:?}",
|
|
search_result.signal_groups
|
|
);
|
|
|
|
let glosa_found_data = (&search_result).into();
|
|
(
|
|
Some(GlosaState::Locked(search_result)),
|
|
GlosaOutput::Found(glosa_found_data),
|
|
)
|
|
} else {
|
|
(
|
|
None,
|
|
GlosaOutput::Searching(GlosaSearching {
|
|
known_intersection_ids,
|
|
}),
|
|
)
|
|
}
|
|
}
|
|
GlosaState::Locked(info) => {
|
|
debug!(
|
|
"GLOSA: Re-evaluating intersection {:4} (at lane {:3})",
|
|
info.intersection_id, info.lane_id
|
|
);
|
|
|
|
// ensure that "our" intersection is kept in TLC cache
|
|
if self
|
|
.tlc_cache
|
|
.keep_intersection(info.intersection_id, self.time)
|
|
.is_err()
|
|
{
|
|
info!("GLOSA: Intersection not known any more");
|
|
(
|
|
Some(GlosaState::Idle),
|
|
GlosaOutput::Searching(GlosaSearching {
|
|
known_intersection_ids,
|
|
}),
|
|
)
|
|
} else {
|
|
// determine if intersection is left
|
|
let intersection = info.intersection.borrow();
|
|
if Self::is_intersection_left(
|
|
&intersection,
|
|
info.lane_id,
|
|
info.approach_id,
|
|
&self.pos.position,
|
|
) {
|
|
info!("GLOSA: Intersection was left -> back to idle");
|
|
(
|
|
Some(GlosaState::Idle),
|
|
GlosaOutput::Searching(GlosaSearching {
|
|
known_intersection_ids,
|
|
}),
|
|
)
|
|
} else {
|
|
// get signal groups which are in our approach
|
|
let mut approach_sig_grps = intersection
|
|
.signal_groups()
|
|
.iter()
|
|
.filter(|i| {
|
|
info.signal_groups
|
|
.iter()
|
|
.find(|sg_id| **sg_id == i.id())
|
|
.is_some()
|
|
})
|
|
.collect::<Vec<_>>();
|
|
|
|
// sort signal groups by maneuver
|
|
approach_sig_grps.sort_by_key(|v| v.maneuvers());
|
|
|
|
// display signal phases
|
|
let sig_grps = approach_sig_grps
|
|
.iter()
|
|
.filter_map(|sig| {
|
|
if let Some(manv) = sig.maneuvers() {
|
|
use core::ops::Sub;
|
|
|
|
use num_traits::Float;
|
|
|
|
let phase = sig
|
|
.phase()
|
|
.map(core::convert::Into::into)
|
|
.unwrap_or_default();
|
|
|
|
let end_sec = sig
|
|
.likely_time()
|
|
.or_else(|| sig.min_end_time())
|
|
.map(|time| {
|
|
let dur =
|
|
time.sub(self.time.and_utc()).as_seconds_f32();
|
|
|
|
#[allow(clippy::cast_possible_truncation)]
|
|
let end_sec = dur.round() as i16;
|
|
|
|
end_sec
|
|
});
|
|
|
|
Some(SignalGroupData {
|
|
phase,
|
|
maneuver: manv,
|
|
end_sec,
|
|
})
|
|
} else {
|
|
None
|
|
}
|
|
})
|
|
.collect::<Vec<_>>();
|
|
|
|
(
|
|
None,
|
|
GlosaOutput::Locked(GlosaSignalInfo {
|
|
intersection_id: intersection.id(),
|
|
signal_groups: sig_grps,
|
|
}),
|
|
) // keep state, return signal groups
|
|
}
|
|
}
|
|
}
|
|
};
|
|
|
|
// can't update the state from within the match statement, so do it here
|
|
if let Some(new) = state_update {
|
|
self.glosa_state = new;
|
|
}
|
|
|
|
output_data
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
#[allow(clippy::too_many_lines)]
|
|
fn find_intersection(
|
|
own_pos: &geo_types::Point,
|
|
own_heading: f32,
|
|
tlc_cache: &tlc::Data,
|
|
) -> Option<TlData> {
|
|
// filter known intersections by distance (and bearing)
|
|
let upcoming_intersections = tlc_cache.intersections_matching(|i| {
|
|
let int_pos = i.center();
|
|
|
|
#[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
|
|
let distance = geo_alg::haversine_dist(own_pos, &int_pos) as f32;
|
|
let bearing = geo_alg::haversine_bearing(own_pos, &int_pos);
|
|
let bearing_diff = geo_alg::deg_180(own_heading - bearing);
|
|
|
|
distance < GLOSA_SEARCH_DIST_M && bearing_diff.abs() < Self::get_search_angle(distance)
|
|
});
|
|
|
|
info!(
|
|
"GLOSA: upcoming int. count: {}",
|
|
upcoming_intersections.len()
|
|
);
|
|
|
|
// determine relevant approach
|
|
for int in upcoming_intersections {
|
|
let intersection = int.borrow();
|
|
let lanes = intersection.ingress_lanes();
|
|
|
|
let own_pos_cart = geo_alg::lonlat_to_dcart(&intersection.ref_point(), own_pos);
|
|
|
|
// find lanes with matching heading
|
|
let relevant_lanes = lanes
|
|
.iter()
|
|
.filter(|i| {
|
|
let lane_heading = i.get_heading();
|
|
let heading_diff = geo_alg::deg_180(own_heading - lane_heading);
|
|
|
|
debug!(
|
|
" - Lane {:4}/{:03} has heading {lane_heading:4.0} -> diff {heading_diff:4.0}°",
|
|
intersection.id(),
|
|
i.id()
|
|
);
|
|
|
|
heading_diff.abs() < GLOSA_LANE_HEADING_DIFF_DEG
|
|
})
|
|
.collect::<Vec<_>>();
|
|
|
|
info!(
|
|
"GLOSA: Int {:4} has {} relevant lanes",
|
|
intersection.id(),
|
|
relevant_lanes.len()
|
|
);
|
|
for lane in &relevant_lanes {
|
|
info!(
|
|
" - lane {:03} from appr. {:?}, usages {:?}",
|
|
lane.id(),
|
|
lane.approach_id(),
|
|
lane.usages()
|
|
);
|
|
}
|
|
|
|
// determine in which lane/ approach we are
|
|
let current_lane = relevant_lanes.iter().find(|lane| {
|
|
#[cfg(feature = "spat_debug")]
|
|
info!("GLOSA: Searching for current lane");
|
|
match geo_alg::LinePosition::for_ingress_lane(own_pos_cart, lane.nodes()) {
|
|
Ok(pos) => {
|
|
#[cfg(feature = "spat_debug")]
|
|
info!(
|
|
" - Lane {:4}/{:03} {:?} with {:.0} m offset",
|
|
intersection.id(),
|
|
lane.id(),
|
|
pos.semantic,
|
|
pos.perpendicular_dist,
|
|
);
|
|
|
|
if pos.perpendicular_dist < GLOSA_AT_LANE_OFFSET_M
|
|
&& pos.semantic == geo_alg::LinePositionType::InBetween
|
|
{
|
|
info!(
|
|
"GLOSA: At lane {:4}/{:03} -> locking intersection",
|
|
intersection.id(),
|
|
lane.id()
|
|
);
|
|
|
|
true
|
|
} else {
|
|
false
|
|
}
|
|
}
|
|
Err(err) => {
|
|
warn!("Lane {:04}: Failed to determine position: {err}", lane.id());
|
|
false
|
|
}
|
|
}
|
|
});
|
|
|
|
if let Some(lane) = current_lane {
|
|
// we found some lane which we are "inside" -> lane search can be aborted here
|
|
// but we should extract some information which is needed for future processing first
|
|
|
|
// collect all signal groups from the current approach
|
|
let mut signal_groups = if let Some(own_approach) = lane.approach_id() {
|
|
let approach_lanes = relevant_lanes.iter().filter(|lane| {
|
|
if let Some(approach) = lane.approach_id() {
|
|
approach == own_approach
|
|
} else {
|
|
false
|
|
}
|
|
});
|
|
|
|
approach_lanes
|
|
.flat_map(|lane| {
|
|
lane.connections()
|
|
.iter()
|
|
.filter_map(v2x::map::Connection::signal_group)
|
|
})
|
|
.collect::<Vec<_>>()
|
|
} else {
|
|
error!(
|
|
"GLOSA: Lane has no approach ID, can't determine all possibly relevant signal groups"
|
|
);
|
|
// just fall back to only the current lane's signal groups
|
|
lane.connections()
|
|
.iter()
|
|
.filter_map(v2x::map::Connection::signal_group)
|
|
.collect::<Vec<_>>()
|
|
};
|
|
|
|
signal_groups.sort_unstable();
|
|
signal_groups.dedup();
|
|
|
|
return Some(TlData {
|
|
intersection_id: intersection.id(),
|
|
intersection: int.clone(),
|
|
lane_id: lane.id(),
|
|
approach_id: lane.approach_id(),
|
|
signal_groups,
|
|
});
|
|
}
|
|
}
|
|
|
|
None
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
fn get_search_angle(distance: f32) -> f32 {
|
|
let (dist_far, angle_far) = GLOSA_SEARCH_ANGLE_PARAMS[0];
|
|
let (dist_near, angle_near) = GLOSA_SEARCH_ANGLE_PARAMS[1];
|
|
|
|
if distance > dist_far {
|
|
angle_far
|
|
} else if distance < dist_near {
|
|
angle_near
|
|
} else {
|
|
// interpolate angle from near to far
|
|
let diff_dist = dist_far - dist_near;
|
|
let diff_angle = angle_far - angle_near;
|
|
|
|
angle_near + (diff_angle / diff_dist) * (distance - dist_near)
|
|
}
|
|
}
|
|
|
|
/// Determines if intersection was left
|
|
///
|
|
/// TODO: return if left via stop line or otherwise
|
|
#[cfg(feature = "spat")]
|
|
fn is_intersection_left(
|
|
intersection: &v2x::map::Intersection,
|
|
lane_id: u8,
|
|
approach_id: Option<u8>,
|
|
own_pos: &geo_types::Point,
|
|
) -> bool {
|
|
// get all lanes from the current approach
|
|
let lanes = if let Some(approach) = approach_id {
|
|
intersection.approach_lanes(approach)
|
|
} else if let Some(lane) = intersection.lane(lane_id) {
|
|
alloc::vec![lane]
|
|
} else {
|
|
alloc::vec![]
|
|
};
|
|
|
|
// find minimal distance to any lane where we are "inside"
|
|
let own_pos_cart = geo_alg::lonlat_to_dcart(&intersection.ref_point(), own_pos);
|
|
let min_perp_dist = lanes.iter().fold(f64::MAX, |min_dist, i| {
|
|
match geo_alg::LinePosition::for_ingress_lane(own_pos_cart, i.nodes()) {
|
|
Ok(pos) => {
|
|
// check if we left the lane via the stop line, but only count stop line as passed after X meters
|
|
if pos.semantic == geo_alg::LinePositionType::InBetween
|
|
|| (pos.semantic == geo_alg::LinePositionType::BeforeStart
|
|
&& pos.parallel_dist.abs() < GLOSA_STOP_LINE_PASSED_M)
|
|
{
|
|
// still at the lane -> update min_dist
|
|
pos.perpendicular_dist
|
|
} else {
|
|
// not at the lane -> take old min_dist
|
|
min_dist
|
|
}
|
|
}
|
|
Err(err) => {
|
|
warn!("Lane {:04}: Failed to determine position: {err}", i.id());
|
|
min_dist
|
|
}
|
|
}
|
|
});
|
|
|
|
min_perp_dist > GLOSA_LANE_LEFT_OFFSET_M
|
|
}
|
|
|
|
// -- getters --
|
|
|
|
#[allow(unused)]
|
|
pub fn initialized(&self) -> bool {
|
|
self.initialized
|
|
}
|
|
|
|
#[allow(unused)]
|
|
pub fn time(&self) -> chrono::prelude::NaiveDateTime {
|
|
self.time
|
|
}
|
|
|
|
#[allow(unused)]
|
|
pub fn pos(&self) -> &PositionState {
|
|
&self.pos
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
|
|
use super::*;
|
|
use crate::init_test_env_logger;
|
|
#[cfg(feature = "spat")]
|
|
use crate::testdata::{MAPEM_HH_560, MAPEM_HH_1328};
|
|
|
|
#[cfg(feature = "spat")]
|
|
#[test]
|
|
fn glosa() {
|
|
init_test_env_logger();
|
|
|
|
// prime TLC cache
|
|
let mut state = State::new();
|
|
let time_now = chrono::NaiveDateTime::default();
|
|
|
|
let etsi = if let c_its_parser::ItsMessage::Mapem {
|
|
geonetworking: _,
|
|
transport: _,
|
|
etsi,
|
|
} =
|
|
c_its_parser::de::decode(MAPEM_HH_1328, c_its_parser::Headers::None).unwrap()
|
|
{
|
|
etsi
|
|
} else {
|
|
panic!("Unexpected C-ITS message")
|
|
};
|
|
state.tlc_cache.handle_mapem(time_now, &etsi);
|
|
|
|
let etsi = if let c_its_parser::ItsMessage::Mapem {
|
|
geonetworking: _,
|
|
transport: _,
|
|
etsi,
|
|
} =
|
|
c_its_parser::de::decode(MAPEM_HH_560, c_its_parser::Headers::None).unwrap()
|
|
{
|
|
etsi
|
|
} else {
|
|
panic!("Unexpected C-ITS message")
|
|
};
|
|
state.tlc_cache.handle_mapem(time_now, &etsi);
|
|
|
|
// test GLOSA
|
|
let own_pos_infront = geo_types::Point::new(9.9752864, 53.5564028);
|
|
let own_pos_inside = geo_types::Point::new(9.9760764, 53.5563621);
|
|
let own_pos_passed = geo_types::Point::new(9.977217, 53.5560602);
|
|
let own_heading = 90.;
|
|
|
|
state.pos.heading_deg = Some(own_heading);
|
|
|
|
state.pos.position = own_pos_infront;
|
|
state.run_glosa();
|
|
assert!(matches!(state.glosa_state, GlosaState::Idle));
|
|
|
|
state.pos.position = own_pos_inside;
|
|
state.run_glosa();
|
|
assert!(matches!(state.glosa_state, GlosaState::Locked(_)));
|
|
if let GlosaState::Locked(info) = &state.glosa_state {
|
|
assert_eq!(1328, info.intersection_id);
|
|
assert!(info.lane_id == 1 || info.lane_id == 2);
|
|
}
|
|
|
|
state.pos.position = own_pos_inside;
|
|
state.run_glosa();
|
|
assert!(matches!(state.glosa_state, GlosaState::Locked(_)));
|
|
if let GlosaState::Locked(info) = &state.glosa_state {
|
|
assert_eq!(1328, info.intersection_id);
|
|
assert!(info.lane_id == 1 || info.lane_id == 2);
|
|
}
|
|
|
|
state.pos.position = own_pos_passed;
|
|
state.run_glosa();
|
|
assert!(matches!(state.glosa_state, GlosaState::Idle));
|
|
}
|
|
}
|