Evaluate remaining red/green time for upcoming intersection
This commit is contained in:
parent
933106b7ea
commit
7463adb038
12 changed files with 2979 additions and 103 deletions
|
|
@ -1,8 +1,551 @@
|
|||
//! Application Logic
|
||||
|
||||
#[cfg(feature = "spat")]
|
||||
use alloc::rc::Rc;
|
||||
#[cfg(feature = "spat")]
|
||||
use alloc::vec::Vec;
|
||||
|
||||
#[cfg(all(target_arch = "riscv32", feature = "spat"))]
|
||||
use esp_println::println;
|
||||
#[cfg(feature = "spat")]
|
||||
use log::{debug, error};
|
||||
use log::{info, warn};
|
||||
|
||||
#[cfg(feature = "spat")]
|
||||
use crate::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 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 = 10.;
|
||||
|
||||
pub struct State {
|
||||
initialized: bool,
|
||||
time: chrono::NaiveDateTime,
|
||||
|
||||
position: geo_types::Point,
|
||||
heading_deg: Option<f32>,
|
||||
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)]
|
||||
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(chrono::Duration::seconds(20)),
|
||||
#[cfg(feature = "spat")]
|
||||
glosa_state: GlosaState::default(),
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "esp")]
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
let Some(own_heading) = self.heading_deg else {
|
||||
return;
|
||||
};
|
||||
|
||||
let state_update = 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))
|
||||
} else {
|
||||
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)
|
||||
} 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
|
||||
for sig in approach_sig_grps {
|
||||
if let Some(phase) = sig.phase_to_str()
|
||||
&& let Some(manv) = sig.maneuvers()
|
||||
{
|
||||
use core::ops::Sub;
|
||||
|
||||
let min_end = sig.min_end_time();
|
||||
let dur = min_end.sub(self.time.and_utc()).as_seconds_f32();
|
||||
println!(
|
||||
"GLOSA: {manv} {phase} for {dur:.1} s (until {})",
|
||||
sig.min_end_time()
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
None // keep state
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// can't update the state from within the match statement, so do it here
|
||||
if let Some(new) = state_update {
|
||||
self.glosa_state = new;
|
||||
}
|
||||
}
|
||||
|
||||
#[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_near, angle_near) = GLOSA_SEARCH_ANGLE_PARAMS[0];
|
||||
let (dist_far, angle_far) = 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));
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue