631 lines
21 KiB
Rust
631 lines
21 KiB
Rust
//! Application Logic
|
|
|
|
#[cfg(feature = "spat")]
|
|
use alloc::rc::Rc;
|
|
#[cfg(feature = "spat")]
|
|
use alloc::vec::Vec;
|
|
|
|
#[cfg(feature = "spat")]
|
|
use log::{debug, error};
|
|
#[cfg(feature = "gnss")]
|
|
use log::{info, warn};
|
|
|
|
#[cfg(feature = "spat")]
|
|
use crate::{applogic::v2x::map, geo_alg};
|
|
|
|
pub mod v2x;
|
|
|
|
#[cfg(feature = "cam")]
|
|
pub mod cam;
|
|
#[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)]
|
|
position: geo_types::Point,
|
|
#[allow(unused)]
|
|
heading_deg: Option<f32>,
|
|
#[allow(unused)]
|
|
speed_mps: Option<f32>,
|
|
|
|
#[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, Default)]
|
|
pub struct SignalGroupData {
|
|
pub phase: SignalPhase,
|
|
pub maneuver: map::Maneuvers,
|
|
pub end_sec: Option<i16>,
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
#[derive(Debug, Default)]
|
|
pub enum SignalPhase {
|
|
#[default]
|
|
Unknown,
|
|
Red,
|
|
RedYellow,
|
|
Green,
|
|
Yellow,
|
|
}
|
|
|
|
#[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(),
|
|
position: geo_types::Point::default(),
|
|
heading_deg: None,
|
|
speed_mps: None,
|
|
#[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(),
|
|
}
|
|
}
|
|
|
|
#[cfg(all(feature = "esp", feature = "gnss"))]
|
|
pub fn update_with_gpsfix(&mut self, fix: &embassy_gps::types::GpsFix) {
|
|
self.position = geo_types::Point::new(fix.longitude_deg, fix.latitude_deg);
|
|
|
|
if let Some(heading) = fix.true_course_deg {
|
|
self.heading_deg = Some(heading);
|
|
} else {
|
|
warn!("State: Course missing from GpsFix");
|
|
}
|
|
|
|
if let Some(speed) = fix.speed_over_ground {
|
|
self.speed_mps = Some(speed);
|
|
} else {
|
|
warn!("State: Speed missing from GpsFix");
|
|
}
|
|
|
|
if let Some(time) = fix.get_timestamp() {
|
|
self.time = time;
|
|
|
|
if !self.initialized {
|
|
info!("GNSS fix: {}", self.print_fix());
|
|
}
|
|
self.initialized = true;
|
|
}
|
|
}
|
|
|
|
#[cfg(feature = "gnss")]
|
|
pub fn print_fix(&self) -> alloc::string::String {
|
|
alloc::format!(
|
|
"{:?}, {:.4} N, {:.4} E, {:.0}°, {:.2} m/s",
|
|
self.time,
|
|
self.position.y(),
|
|
self.position.x(),
|
|
self.heading_deg.unwrap_or_default(),
|
|
self.speed_mps.unwrap_or_default(),
|
|
)
|
|
}
|
|
|
|
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) {
|
|
// only update "our" intersection
|
|
if let GlosaState::Locked(data) = &self.glosa_state
|
|
&& etsi
|
|
.spat
|
|
.intersections
|
|
.0
|
|
.iter()
|
|
.find(|i| i.id.id.0 == data.intersection_id)
|
|
.is_some()
|
|
{
|
|
self.tlc_cache.handle_spatem(self.time, etsi);
|
|
}
|
|
}
|
|
|
|
#[cfg(feature = "spat")]
|
|
pub fn run_glosa(&mut self) -> Vec<SignalGroupData> {
|
|
let Some(own_heading) = self.heading_deg else {
|
|
return alloc::vec![];
|
|
};
|
|
|
|
let (state_update, sig_grps) = match &self.glosa_state {
|
|
// search for upcoming intersection
|
|
GlosaState::Idle => {
|
|
debug!(
|
|
"GLOSA: Searching with {} known intersections",
|
|
self.tlc_cache.known_intersections().len()
|
|
);
|
|
|
|
if let Some(search_result) =
|
|
Self::find_intersection(&self.position, own_heading, &self.tlc_cache)
|
|
{
|
|
info!(
|
|
"GLOSA: Approach has signal groups: {:?}",
|
|
search_result.signal_groups
|
|
);
|
|
|
|
(Some(GlosaState::Locked(search_result)), None)
|
|
} else {
|
|
(None, None)
|
|
}
|
|
}
|
|
GlosaState::Locked(info) => {
|
|
debug!(
|
|
"GLOSA: Re-evaluating intersection {:4} (at lane {:3})",
|
|
info.intersection_id, info.lane_id
|
|
);
|
|
|
|
// determine if intersection is left
|
|
let intersection = info.intersection.borrow();
|
|
if Self::is_intersection_left(
|
|
&intersection,
|
|
info.lane_id,
|
|
info.approach_id,
|
|
&self.position,
|
|
) {
|
|
info!("GLOSA: Intersection was left -> back to idle");
|
|
(Some(GlosaState::Idle), None)
|
|
} 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, Some(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;
|
|
}
|
|
|
|
sig_grps.unwrap_or_default()
|
|
}
|
|
|
|
#[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
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
|
|
use super::*;
|
|
use crate::init_test_env_logger;
|
|
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.heading_deg = Some(own_heading);
|
|
|
|
state.position = own_pos_infront;
|
|
state.run_glosa();
|
|
assert!(matches!(state.glosa_state, GlosaState::Idle));
|
|
|
|
state.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.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.position = own_pos_passed;
|
|
state.run_glosa();
|
|
assert!(matches!(state.glosa_state, GlosaState::Idle));
|
|
}
|
|
}
|