jtbx/esp32-c_its-companion
0
0
Fork 0
Code Activity

Evaluate remaining red/green time for upcoming intersection

Browse source
This commit is contained in:
Jannik Beyerstedt 2026-05-30 23:34:06 +02:00
commit 7463adb038
12 changed files with 2979 additions and 103 deletions
Download patch file Download diff file Expand all files Collapse all files

543
src/applogic/mod.rs
View file

@ -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));
}
}

126
src/applogic/tlc.rs
View file

@ -1,52 +1,108 @@
//! C-ITS Traffic Light Data (MAPEM/ SPATEM) Evaluation
//! C-ITS Traffic Light Data (MAPEM/ SPATEM) Evaluation/ Cache
use c_its_parser::standards::mapem_2_2_1::mapem_pdu_descriptions;
use c_its_parser::standards::spatem_2_2_1::spatem_pdu_descriptions;
use alloc::rc::Rc;
use alloc::vec::Vec;
pub use c_its_parser::standards::mapem_2_2_1::mapem_pdu_descriptions::MAPEM;
pub use c_its_parser::standards::spatem_2_2_1::spatem_pdu_descriptions::SPATEM;
use chrono::Datelike;
#[cfg(target_arch = "riscv32")]
#[cfg(all(target_arch = "riscv32", feature = "spat_debug"))]
use esp_println::println;
pub fn handle_mapem(etsi: &mapem_pdu_descriptions::MAPEM) {
// TODO: proof of concept only -- replace with actual evaluation
if let Some(map) = &etsi.map.intersections {
for int in &map.0 {
println!("got MAPEM for intersection {}", int.id.id.0);
}
}
use super::v2x::map;
use crate::cache::Cache;
pub struct Data {
intersections: Cache<u16, map::Intersection>,
}
pub fn handle_spatem(etsi: &spatem_pdu_descriptions::SPATEM, now: &chrono::NaiveDateTime) {
// TODO: proof of concept only -- replace with actual evaluation
for int in &etsi.spat.intersections.0 {
println!("got SPATEM for intersection {}", int.id.id.0);
impl Data {
pub fn new(cache_lifetime: chrono::Duration) -> Self {
Self {
intersections: Cache::<u16, map::Intersection>::new(cache_lifetime),
}
}
if let (Some(moy), Some(second)) = (&int.moy, &int.time_stamp) {
let year = now.year();
pub fn known_intersections(&self) -> Vec<u16> {
self.intersections.keys()
}
let ref_time = c_its_parser::time_utils::time_from_moy_and_dsecond(moy, second, year);
println!("- ref time: {ref_time:?}");
pub fn intersections_matching<P>(
&self,
predicate: P,
) -> Vec<Rc<core::cell::RefCell<map::Intersection>>>
where
P: Fn(&map::Intersection) -> bool,
{
self.intersections.get_filtered(predicate)
}
for state in &int.states.0 {
let sig_grp = state.signal_group.0;
pub fn handle_mapem(&mut self, time_now: chrono::NaiveDateTime, etsi: &MAPEM) {
let layer_id = etsi.map.layer_id.as_ref().map(|v| v.0);
if let Some(map) = &etsi.map.intersections {
for geom in &map.0 {
let int_id = geom.id.id.0;
if let Some(event) = state.state_time_speed.0.first() {
if let Some(timing) = &event.timing {
let min_end_time = timing.min_end_time.to_datetime_from_moy(moy, year);
if self.intersections.contains(&int_id) {
// update with new map layers
println!(
"- sig grp. {sig_grp:03}: until {min_end_time:?} at {:?}",
event.event_state
);
} else {
println!(
"- sig grp. {sig_grp:03}: {:?} until unknown",
event.event_state
);
}
// we can ignore the result, since we're sure the item exists
let _ = self.intersections.update(time_now, int_id, |i| {
if !i.is_complete() {
i.add_layer(layer_id, geom);
#[cfg(feature = "spat_debug")]
if i.is_complete() {
Self::pretty_print_intersection(&i);
}
}
});
} else {
println!("- sig grp. {sig_grp:03}: no events");
// create new intersection
let intersection = map::Intersection::new(layer_id, geom);
#[cfg(feature = "spat_debug")]
if intersection.is_complete() {
Self::pretty_print_intersection(&intersection);
}
// we can ignore the result, since we're sure the item does not exist
let _ = self.intersections.insert(time_now, intersection);
}
}
}
}
#[cfg(feature = "spat_debug")]
fn pretty_print_intersection(intersection: &map::Intersection) {
println!("TLC: {intersection}");
println!("TLC: Signal Groups");
for grp in intersection.signal_groups() {
println!(" {grp}");
}
}
pub fn prune(&mut self, time_now: chrono::NaiveDateTime) {
self.intersections.prune(time_now);
}
pub fn handle_spatem(&mut self, time_now: chrono::NaiveDateTime, etsi: &SPATEM) {
for int in &etsi.spat.intersections.0 {
let int_id = int.id.id.0;
if self.intersections.contains(&int_id) {
// update with SPAT data
// we can ignore the result, since we're sure the item exists
let _ = self.intersections.update(time_now, int_id, |i| {
if i.is_complete() {
// add SPAT information to intersection
i.add_spat(int, time_now.year());
}
});
} else {
// drop message since we don't have (full) MAP yet
}
}
}
}

1019
src/applogic/v2x/map.rs Normal file
View file

File diff suppressed because it is too large Load diff

4
src/applogic/v2x/mod.rs Normal file
View file

@ -0,0 +1,4 @@
//! Types for V2X data handling
#[cfg(feature = "spat")]
pub mod map;