1063 lines
34 KiB
Rust
1063 lines
34 KiB
Rust
//! Types for V2X data handling
|
|
|
|
use alloc::string::{String, ToString};
|
|
use alloc::vec::Vec;
|
|
use alloc::{format, vec};
|
|
use core::ops::Div;
|
|
|
|
use c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc;
|
|
use log::warn;
|
|
|
|
use crate::cache::Cachable;
|
|
use crate::geo_alg;
|
|
|
|
#[derive(Debug, Clone)]
|
|
#[allow(unused)]
|
|
pub struct Intersection {
|
|
id: u16,
|
|
/// reference point as lon/lat
|
|
ref_point: geo_types::Point,
|
|
/// guessed center point as lon/lat
|
|
center: geo_types::Point,
|
|
lanes: Vec<Lane>,
|
|
signal_groups: Vec<SignalGroup>,
|
|
|
|
// internal state
|
|
num_layers: u8,
|
|
layers_present: Vec<u8>,
|
|
is_complete: bool,
|
|
last_spat_millis: u16,
|
|
}
|
|
|
|
impl Cachable<u16> for Intersection {
|
|
fn key(&self) -> u16 {
|
|
self.id
|
|
}
|
|
}
|
|
|
|
impl core::fmt::Display for Intersection {
|
|
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
|
let lanes = self.lanes.iter().fold(String::new(), |mut acc, i| {
|
|
use core::fmt::Write;
|
|
let _ = writeln!(acc, " {i}");
|
|
acc
|
|
});
|
|
|
|
write!(
|
|
f,
|
|
"Intersection {} {{ ref: {:?}, \n{lanes}}}",
|
|
self.id, self.ref_point
|
|
)
|
|
}
|
|
}
|
|
|
|
#[allow(unused)]
|
|
impl Intersection {
|
|
pub fn new(layer_id: Option<u8>, value: &etsi_its_dsrc::IntersectionGeometry) -> Self {
|
|
let id = value.id.id.0;
|
|
let ref_point = value.ref_point.clone().into();
|
|
let lanes = value
|
|
.lane_set
|
|
.0
|
|
.iter()
|
|
.filter_map(|v| match Lane::new(v, &ref_point) {
|
|
Ok(lane) => Some(lane),
|
|
Err(err) => {
|
|
warn!("{err}");
|
|
None
|
|
}
|
|
})
|
|
.collect::<Vec<_>>();
|
|
|
|
let (num_layers, layers_present, is_complete) = if let Some(layer_id) = layer_id {
|
|
let (num_layers, layer_num) = Self::layer_num_and_count(layer_id);
|
|
let complete = num_layers == 1; // just to be save from stupid input data
|
|
|
|
(num_layers, alloc::vec![layer_num], complete)
|
|
} else {
|
|
(1, alloc::vec![1], true)
|
|
};
|
|
|
|
let signal_groups = if is_complete {
|
|
Self::collect_signal_groups(&lanes)
|
|
} else {
|
|
Vec::default()
|
|
};
|
|
|
|
let center = if is_complete {
|
|
Self::calculate_center(&ref_point, &lanes)
|
|
} else {
|
|
geo_types::Point::default()
|
|
};
|
|
|
|
Self {
|
|
id,
|
|
ref_point,
|
|
center,
|
|
lanes,
|
|
signal_groups,
|
|
num_layers,
|
|
layers_present,
|
|
is_complete,
|
|
last_spat_millis: 0,
|
|
}
|
|
}
|
|
|
|
pub fn add_layer(&mut self, layer_id: Option<u8>, value: &etsi_its_dsrc::IntersectionGeometry) {
|
|
if self.is_complete {
|
|
return;
|
|
}
|
|
let Some(layer_id) = layer_id else {
|
|
return;
|
|
};
|
|
let (_, layer_num) = Self::layer_num_and_count(layer_id);
|
|
|
|
// determine if we already have this layer
|
|
if self.layers_present.contains(&layer_num) {
|
|
return;
|
|
}
|
|
|
|
// add lanes
|
|
for lane in &value.lane_set.0 {
|
|
let lane_id = lane.lane_id.0;
|
|
|
|
if self.lanes.iter().find(|i| i.id == lane_id).is_none() {
|
|
match Lane::new(lane, &self.ref_point) {
|
|
Ok(lane) => self.lanes.push(lane),
|
|
Err(err) => {
|
|
warn!("{err}");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
self.layers_present.push(layer_num);
|
|
|
|
// we can just count the present layers since we made sure to not add already known ones beforehand
|
|
if self.layers_present.len() == self.num_layers as usize {
|
|
self.signal_groups = Self::collect_signal_groups(&self.lanes);
|
|
self.center = Self::calculate_center(&self.ref_point, &self.lanes);
|
|
self.is_complete = true;
|
|
}
|
|
}
|
|
|
|
pub fn add_spat(&mut self, value: &etsi_its_dsrc::IntersectionState, year: i32) {
|
|
if let Some(millis) = &value.time_stamp {
|
|
if self.last_spat_millis == millis.0 {
|
|
// skip this SPATEM since we already processed one with the same timestamp
|
|
return;
|
|
}
|
|
self.last_spat_millis = millis.0;
|
|
}
|
|
|
|
if let Some(moy) = &value.moy {
|
|
for state in &value.states.0 {
|
|
let sig_grp = state.signal_group.0;
|
|
|
|
if let Some(event) = state.state_time_speed.0.first() {
|
|
// get write access to SignalGroup item
|
|
if let Some(idx) = self.signal_groups.iter().position(|i| i.id == sig_grp)
|
|
&& let Some(sig_grp) = self.signal_groups.get_mut(idx)
|
|
{
|
|
sig_grp.phase = Some(event.event_state);
|
|
if let Some(timing) = &event.timing {
|
|
if !timing.min_end_time.is_out_of_range()
|
|
&& !timing.min_end_time.is_unknown()
|
|
{
|
|
sig_grp.min_end_time =
|
|
Some(timing.min_end_time.to_datetime_from_moy(moy, year));
|
|
}
|
|
if let Some(likely) = &timing.likely_time
|
|
&& !likely.is_out_of_range()
|
|
&& !likely.is_unknown()
|
|
{
|
|
sig_grp.likely_time = Some(likely.to_datetime_from_moy(moy, year));
|
|
}
|
|
if let Some(max) = &timing.max_end_time
|
|
&& !max.is_out_of_range()
|
|
&& !max.is_unknown()
|
|
{
|
|
sig_grp.max_end_time = Some(max.to_datetime_from_moy(moy, year));
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
// warn!("SPAT: SG {sig_grp:03} has no timing in first event");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
fn collect_signal_groups(lanes: &[Lane]) -> Vec<SignalGroup> {
|
|
let mut connections = lanes
|
|
.iter()
|
|
.flat_map(|i| i.connections.clone())
|
|
.filter(|i| i.signal_group.is_some())
|
|
.collect::<Vec<_>>();
|
|
|
|
connections.sort_by_key(|i| i.signal_group.unwrap_or_default());
|
|
|
|
let chunks = connections.chunk_by(|a, b| a.signal_group == b.signal_group);
|
|
chunks
|
|
.map(|chunk| {
|
|
let (sig_grp_id, maneuvers, mut target_lanes) = chunk.iter().fold(
|
|
(None, Maneuvers::default(), vec![]),
|
|
|(sig_grp_id, maneuvers, mut lanes), i| {
|
|
let new_maneuvers = i
|
|
.maneuvers
|
|
.map(|v| core::ops::Add::add(maneuvers, v))
|
|
.unwrap_or_default();
|
|
lanes.push(i.target_lane_id);
|
|
|
|
(i.signal_group, new_maneuvers, lanes)
|
|
},
|
|
);
|
|
|
|
// unwrap is fine since we removed all connections without signal group beforehand
|
|
let id = sig_grp_id.unwrap();
|
|
let maneuvers = if maneuvers.is_emtpy() {
|
|
None
|
|
} else {
|
|
Some(maneuvers)
|
|
};
|
|
target_lanes.sort_unstable();
|
|
// TODO: dedup as well!?
|
|
|
|
SignalGroup {
|
|
id,
|
|
maneuvers,
|
|
target_lanes,
|
|
phase: None,
|
|
min_end_time: None,
|
|
max_end_time: None,
|
|
likely_time: None,
|
|
}
|
|
})
|
|
.collect()
|
|
}
|
|
|
|
fn calculate_center(ref_pos: &geo_types::Point, lanes: &[Lane]) -> geo_types::Point {
|
|
let (sum, count) = lanes.iter().fold(
|
|
(geo_types::Point::default(), 0),
|
|
|(center_sum, count), lane| {
|
|
// only use vehicle lanes
|
|
if lane.usages.contains(&UsageType::Vehicle)
|
|
&& let Some(stop_line) = lane.get_stop_line()
|
|
{
|
|
(center_sum + stop_line.into(), count + 1)
|
|
} else {
|
|
(center_sum, count)
|
|
}
|
|
},
|
|
);
|
|
|
|
// convert from X/Y to geo coordinate
|
|
let center_xy = sum.div(f64::from(count));
|
|
|
|
geo_alg::dcart_to_lonlat(ref_pos, ¢er_xy)
|
|
}
|
|
|
|
fn layer_num_and_count(layer_id: u8) -> (u8, u8) {
|
|
let layer_num = layer_id % 10;
|
|
let num_layers = (layer_id - layer_num) / 10;
|
|
|
|
(num_layers, layer_num)
|
|
}
|
|
|
|
pub fn is_complete(&self) -> bool {
|
|
self.is_complete
|
|
}
|
|
|
|
pub fn id(&self) -> u16 {
|
|
self.id
|
|
}
|
|
pub fn ref_point(&self) -> geo_types::Point {
|
|
self.ref_point
|
|
}
|
|
pub fn center(&self) -> geo_types::Point {
|
|
self.center
|
|
}
|
|
pub fn lanes(&self) -> &[Lane] {
|
|
&self.lanes
|
|
}
|
|
pub fn signal_groups(&self) -> &[SignalGroup] {
|
|
&self.signal_groups
|
|
}
|
|
|
|
pub fn lane(&self, lane_id: u8) -> Option<&Lane> {
|
|
self.lanes.iter().find(|i| i.id == lane_id)
|
|
}
|
|
|
|
pub fn ingress_lanes(&self) -> Vec<&Lane> {
|
|
self.lanes.iter().filter(|i| i.is_ingress).collect()
|
|
}
|
|
|
|
pub fn bikeable_ingress_lanes(&self) -> Vec<&Lane> {
|
|
self.lanes
|
|
.iter()
|
|
.filter(|i| i.is_ingress && i.usages.contains(&UsageType::Bike))
|
|
.collect()
|
|
}
|
|
|
|
pub fn approach_lanes(&self, approach_id: u8) -> Vec<&Lane> {
|
|
self.lanes
|
|
.iter()
|
|
.filter(|i| i.approach_id.is_some_and(|i| i == approach_id))
|
|
.collect()
|
|
}
|
|
}
|
|
|
|
#[derive(Debug, Clone)]
|
|
#[allow(unused)]
|
|
pub struct Lane {
|
|
id: u8,
|
|
approach_id: Option<u8>,
|
|
/// lane nodes as local X/Y coordinates, X to the North, Y to the East
|
|
nodes: geo_types::LineString,
|
|
is_ingress: bool,
|
|
is_egress: bool,
|
|
usages: Vec<UsageType>,
|
|
connections: Vec<Connection>,
|
|
maneuvers: Vec<Maneuvers>,
|
|
}
|
|
|
|
impl core::fmt::Display for Lane {
|
|
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
|
let type_str = match (self.is_ingress, self.is_egress) {
|
|
(true, true) => "IE",
|
|
(true, false) => "I",
|
|
(false, true) => "E",
|
|
(false, false) => "",
|
|
};
|
|
let appr_str = if let Some(appr) = self.approach_id {
|
|
format!("{appr}")
|
|
} else {
|
|
String::new()
|
|
};
|
|
let conns_str: String = self.connections.iter().fold(String::new(), |mut acc, i| {
|
|
use core::fmt::Write;
|
|
let _ = write!(acc, "{i}");
|
|
acc
|
|
});
|
|
|
|
write!(
|
|
f,
|
|
"Lane {} ({type_str}-{appr_str}, {:?}) -> [{conns_str}]",
|
|
self.id, self.usages
|
|
)
|
|
}
|
|
}
|
|
|
|
#[derive(Debug, Copy, Clone)]
|
|
#[allow(unused)]
|
|
pub struct Connection {
|
|
id: Option<u8>,
|
|
target_lane_id: u8,
|
|
maneuvers: Option<Maneuvers>,
|
|
signal_group: Option<u8>,
|
|
}
|
|
|
|
impl core::fmt::Display for Connection {
|
|
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
|
let id = self.id.unwrap_or_default();
|
|
let sig_str = match self.signal_group {
|
|
Some(sig) => format!("sig. {sig}"),
|
|
None => String::new(),
|
|
};
|
|
|
|
write!(f, "{{ conn. {id}: to {}, {sig_str} }}", self.target_lane_id)
|
|
}
|
|
}
|
|
|
|
#[allow(unused)]
|
|
impl Connection {
|
|
pub fn id(&self) -> Option<u8> {
|
|
self.id
|
|
}
|
|
pub fn maneuvers(&self) -> Option<Maneuvers> {
|
|
self.maneuvers
|
|
}
|
|
pub fn signal_group(&self) -> Option<u8> {
|
|
self.signal_group
|
|
}
|
|
}
|
|
|
|
impl From<&etsi_its_dsrc::Connection> for Connection {
|
|
fn from(value: &etsi_its_dsrc::Connection) -> Self {
|
|
let id = value.connection_id.as_ref().map(|v| v.0);
|
|
let target_lane_id = value.connecting_lane.lane.0;
|
|
let maneuvers = value
|
|
.connecting_lane
|
|
.maneuver
|
|
.as_ref()
|
|
.map(Maneuvers::from_dsrc);
|
|
let signal_group = value.signal_group.as_ref().map(|v| v.0);
|
|
|
|
Self {
|
|
id,
|
|
target_lane_id,
|
|
maneuvers,
|
|
signal_group,
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Different view on a lane connection
|
|
///
|
|
/// Technically this data is redundant with the `Connection` data of a `Lane`,
|
|
/// but it makes processing SPAT information easier, if signal group maneuvers can be accessed directly and not my collecting them from all lane connections.
|
|
#[derive(Debug, Clone)]
|
|
#[allow(unused)]
|
|
pub struct SignalGroup {
|
|
id: u8,
|
|
maneuvers: Option<Maneuvers>,
|
|
target_lanes: Vec<u8>, // mainly for debugging
|
|
|
|
// SPAT data
|
|
/// current signal phase
|
|
phase: Option<etsi_its_dsrc::MovementPhaseState>,
|
|
min_end_time: Option<chrono::DateTime<chrono::Utc>>,
|
|
max_end_time: Option<chrono::DateTime<chrono::Utc>>,
|
|
likely_time: Option<chrono::DateTime<chrono::Utc>>,
|
|
}
|
|
|
|
impl core::fmt::Display for SignalGroup {
|
|
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
|
let manv_str = self.maneuvers.map(|v| format!(" {v}")).unwrap_or_default();
|
|
let phase_str = self.phase_to_str().unwrap_or_default();
|
|
write!(
|
|
f,
|
|
"SG {:2} {{{manv_str} to {:?} {phase_str}}}",
|
|
self.id, self.target_lanes
|
|
)
|
|
}
|
|
}
|
|
|
|
#[allow(unused)]
|
|
impl SignalGroup {
|
|
pub fn id(&self) -> u8 {
|
|
self.id
|
|
}
|
|
pub fn maneuvers(&self) -> Option<Maneuvers> {
|
|
self.maneuvers
|
|
}
|
|
pub fn phase(&self) -> Option<etsi_its_dsrc::MovementPhaseState> {
|
|
self.phase
|
|
}
|
|
pub fn min_end_time(&self) -> Option<chrono::prelude::DateTime<chrono::prelude::Utc>> {
|
|
self.min_end_time
|
|
}
|
|
pub fn likely_time(&self) -> Option<chrono::prelude::DateTime<chrono::prelude::Utc>> {
|
|
self.likely_time
|
|
}
|
|
pub fn max_end_time(&self) -> Option<chrono::prelude::DateTime<chrono::prelude::Utc>> {
|
|
self.max_end_time
|
|
}
|
|
|
|
pub fn phase_to_str(&self) -> Option<String> {
|
|
self.phase.map(|v| {
|
|
match v {
|
|
etsi_its_dsrc::MovementPhaseState::unavailable => "n.a.",
|
|
etsi_its_dsrc::MovementPhaseState::dark => "dark",
|
|
etsi_its_dsrc::MovementPhaseState::stop_Then_Proceed => "stop",
|
|
etsi_its_dsrc::MovementPhaseState::stop_And_Remain => "RED",
|
|
etsi_its_dsrc::MovementPhaseState::pre_Movement => "PRE",
|
|
etsi_its_dsrc::MovementPhaseState::permissive_Movement_Allowed
|
|
| etsi_its_dsrc::MovementPhaseState::protected_Movement_Allowed => "GRN",
|
|
etsi_its_dsrc::MovementPhaseState::permissive_clearance
|
|
| etsi_its_dsrc::MovementPhaseState::protected_clearance => "YEL",
|
|
|
|
etsi_its_dsrc::MovementPhaseState::caution_Conflicting_Traffic => "blnk",
|
|
}
|
|
.to_string()
|
|
})
|
|
}
|
|
}
|
|
|
|
#[derive(Debug, Clone)]
|
|
#[allow(unused)]
|
|
pub struct SignalPhase {
|
|
state: etsi_its_dsrc::SignalStatus,
|
|
}
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
|
|
#[allow(unused)]
|
|
pub enum UsageType {
|
|
/// vehicle lanes that are not limited to Bus or Taxi use, nor restricted from public use
|
|
Vehicle,
|
|
/// vehicle lanes that are restricted to bus use or vehicle lanes shared with bus vehicle traffic
|
|
Bus,
|
|
/// tracked-vehicle lanes or vehicle lanes shared with tracked vehicle traffic
|
|
Tram,
|
|
/// bike-lanes or vehicle lanes shared with cyclist vehicle traffic
|
|
Bike,
|
|
/// crosswalks or sidewalks
|
|
Pedestrian,
|
|
}
|
|
|
|
#[derive(Debug, Default, Copy, Clone, PartialEq, Eq)]
|
|
pub struct Maneuvers {
|
|
pub left_allowed: bool,
|
|
pub staight_allowed: bool,
|
|
pub right_allowed: bool,
|
|
}
|
|
|
|
impl core::fmt::Display for Maneuvers {
|
|
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
|
write!(
|
|
f,
|
|
"Maneuvers {{ {}{}{} }}",
|
|
if self.left_allowed { "l" } else { "_" },
|
|
if self.staight_allowed { "s" } else { "_" },
|
|
if self.right_allowed { "r" } else { "_" },
|
|
)
|
|
}
|
|
}
|
|
|
|
impl PartialOrd for Maneuvers {
|
|
fn partial_cmp(&self, other: &Self) -> Option<core::cmp::Ordering> {
|
|
Some(self.cmp(other))
|
|
}
|
|
}
|
|
impl Ord for Maneuvers {
|
|
fn cmp(&self, other: &Self) -> core::cmp::Ordering {
|
|
if self.eq(other) {
|
|
core::cmp::Ordering::Equal
|
|
} else {
|
|
#[allow(clippy::unnested_or_patterns)]
|
|
match (self.left_allowed, self.staight_allowed, self.right_allowed) {
|
|
// left (or none) < everything
|
|
(true, false, false) | (false, false, false) => core::cmp::Ordering::Less,
|
|
// left and straight > (just left)
|
|
(true, true, false) => {
|
|
if other.eq(&Maneuvers::new(true, false, false)) {
|
|
core::cmp::Ordering::Greater
|
|
} else {
|
|
core::cmp::Ordering::Less
|
|
}
|
|
}
|
|
// omni > (anything without right)
|
|
(true, true, true) | (true, false, true) => {
|
|
if other.right_allowed {
|
|
core::cmp::Ordering::Less
|
|
} else {
|
|
core::cmp::Ordering::Greater
|
|
}
|
|
}
|
|
// (only straight) > omni or left or left+straight
|
|
(false, true, false) => {
|
|
if other.eq(&Maneuvers::new(true, true, true))
|
|
|| other.eq(&Maneuvers::new(true, false, false))
|
|
|| other.eq(&Maneuvers::new(true, true, false))
|
|
{
|
|
core::cmp::Ordering::Greater
|
|
} else {
|
|
core::cmp::Ordering::Less
|
|
}
|
|
}
|
|
// (straight and right) < right
|
|
(false, true, true) => {
|
|
if other.eq(&Maneuvers::new(false, false, true)) {
|
|
core::cmp::Ordering::Less
|
|
} else {
|
|
core::cmp::Ordering::Greater
|
|
}
|
|
}
|
|
// right > alles
|
|
(false, false, true) => core::cmp::Ordering::Greater,
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
impl core::ops::Add for Maneuvers {
|
|
type Output = Maneuvers;
|
|
|
|
fn add(self, rhs: Self) -> Self::Output {
|
|
Self::Output {
|
|
staight_allowed: self.staight_allowed || rhs.staight_allowed,
|
|
left_allowed: self.left_allowed || rhs.left_allowed,
|
|
right_allowed: self.right_allowed || rhs.right_allowed,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[allow(unused)]
|
|
impl Maneuvers {
|
|
pub fn new(left_allowed: bool, staight_allowed: bool, right_allowed: bool) -> Self {
|
|
Self {
|
|
left_allowed,
|
|
staight_allowed,
|
|
right_allowed,
|
|
}
|
|
}
|
|
|
|
pub fn from_dsrc(value: &etsi_its_dsrc::AllowedManeuvers) -> Self {
|
|
let left_allowed =
|
|
value.get_maneuver_left_allowed() || value.get_maneuver_left_turn_on_red_allowed();
|
|
let right_allowed =
|
|
value.get_maneuver_right_allowed() || value.get_maneuver_right_turn_on_red_allowed();
|
|
|
|
Self {
|
|
left_allowed,
|
|
staight_allowed: value.get_maneuver_straight_allowed(),
|
|
right_allowed,
|
|
}
|
|
}
|
|
|
|
pub fn is_emtpy(self) -> bool {
|
|
!self.left_allowed && !self.staight_allowed && !self.right_allowed
|
|
}
|
|
}
|
|
|
|
#[allow(unused)]
|
|
impl Lane {
|
|
/// Creates a `Lane` from C-ITS data
|
|
pub fn new(
|
|
lane: &etsi_its_dsrc::GenericLane,
|
|
ref_pos: &geo_types::Point,
|
|
) -> Result<Self, String> {
|
|
let etsi_its_dsrc::NodeListXY::nodes(node_set_xy) = &lane.node_list else {
|
|
return Err(format!(
|
|
"Lane::new(): ComputedLane not supported (used in lane {})",
|
|
lane.lane_id.0
|
|
));
|
|
};
|
|
|
|
let nodes_enu = Self::node_set_to_line_string_xy(node_set_xy, *ref_pos);
|
|
|
|
// convert to NED instead of ENU
|
|
let nodes = nodes_enu
|
|
.points()
|
|
.map(|p| geo_types::Point::new(p.y(), p.x()))
|
|
.collect();
|
|
|
|
let usages = UsageType::get_usages(&lane.lane_attributes)?;
|
|
let connections = lane
|
|
.connects_to
|
|
.as_ref()
|
|
.map(|v| v.0.iter().map(core::convert::Into::into).collect())
|
|
.unwrap_or_default();
|
|
|
|
let maneuvers = alloc::vec![];
|
|
|
|
let is_ingress = lane.lane_attributes.directional_use.get_ingress_path();
|
|
let is_egress = lane.lane_attributes.directional_use.get_egress_path();
|
|
|
|
Ok(Self {
|
|
id: lane.lane_id.0,
|
|
approach_id: lane.ingress_approach.as_ref().map(|v| v.0),
|
|
nodes,
|
|
usages,
|
|
connections,
|
|
maneuvers,
|
|
is_ingress,
|
|
is_egress,
|
|
})
|
|
}
|
|
|
|
fn node_set_to_line_string_xy(
|
|
data: &etsi_its_dsrc::NodeSetXY,
|
|
ref_pos: geo_types::Point,
|
|
) -> geo_types::LineString {
|
|
let mut prev_delta = geo_types::Point::default();
|
|
let mut prev_ref_pos = ref_pos;
|
|
data.0
|
|
.iter()
|
|
.map(|pt| {
|
|
let node = match &pt.delta {
|
|
etsi_its_dsrc::NodeOffsetPointXY::node_LatLon(node_llm_d64b) => {
|
|
let result = pt.delta.to_ddist(&prev_ref_pos);
|
|
|
|
// reference position needs to be the last `node_LatLon`
|
|
prev_ref_pos = geo_types::Point::new(
|
|
node_llm_d64b.lon.as_deg(),
|
|
node_llm_d64b.lat.as_deg(),
|
|
);
|
|
result
|
|
}
|
|
_ => prev_delta + pt.delta.to_ddist(&ref_pos),
|
|
};
|
|
prev_delta = node;
|
|
|
|
node
|
|
})
|
|
.collect()
|
|
}
|
|
|
|
pub fn id(&self) -> u8 {
|
|
self.id
|
|
}
|
|
pub fn approach_id(&self) -> Option<u8> {
|
|
self.approach_id
|
|
}
|
|
pub fn is_ingress(&self) -> bool {
|
|
self.is_ingress
|
|
}
|
|
pub fn nodes(&self) -> &geo_types::LineString {
|
|
&self.nodes
|
|
}
|
|
pub fn usages(&self) -> &[UsageType] {
|
|
&self.usages
|
|
}
|
|
pub fn connections(&self) -> &[Connection] {
|
|
&self.connections
|
|
}
|
|
pub fn maneuvers(&self) -> &[Maneuvers] {
|
|
&self.maneuvers
|
|
}
|
|
|
|
pub fn get_stop_line(&self) -> Option<geo_types::Coord> {
|
|
self.nodes.0.first().copied()
|
|
}
|
|
|
|
/// Determines average heading
|
|
pub fn get_heading(&self) -> f32 {
|
|
// unwrap is fine since a lane always contains at least 2 nodes
|
|
let stop_line = self.nodes.points().next().unwrap();
|
|
// unwrap is fine since a lane always contains at least 2 nodes
|
|
let second_point = self.nodes.points().next_back().unwrap();
|
|
|
|
#[allow(clippy::cast_possible_truncation)]
|
|
let average_heading = geo_alg::cartesian_bearing(second_point, stop_line) as f32;
|
|
|
|
average_heading
|
|
}
|
|
|
|
/// Determines heading at stop line and average
|
|
pub fn get_headings(&self) -> (f32, f32) {
|
|
// unwrap is fine since a lane always contains at least 2 nodes
|
|
let stop_line = self.nodes.points().next().unwrap();
|
|
// unwrap is fine since a lane always contains at least 2 nodes
|
|
let second_point = self.nodes.points().next().unwrap();
|
|
// unwrap is fine since a lane always contains at least 2 nodes
|
|
let entry_point = self.nodes.points().next_back().unwrap();
|
|
|
|
#[allow(clippy::cast_possible_truncation)]
|
|
let stop_line_heading = geo_alg::cartesian_bearing(second_point, stop_line) as f32;
|
|
#[allow(clippy::cast_possible_truncation)]
|
|
let average_heading = geo_alg::cartesian_bearing(entry_point, stop_line) as f32;
|
|
|
|
(stop_line_heading, average_heading)
|
|
}
|
|
}
|
|
|
|
#[allow(dead_code)]
|
|
impl UsageType {
|
|
/// Determine possible usages of the lane
|
|
///
|
|
/// Lanes may be used my multiple classes of traffic at once, so this needs to
|
|
/// return a vector of [`UsageType`].
|
|
///
|
|
/// # Errors
|
|
/// Returns display string of the [`LaneAttributes`], if no suitable lane class was found
|
|
pub fn get_usages(lane_attrs: &etsi_its_dsrc::LaneAttributes) -> Result<Vec<Self>, String> {
|
|
let mut result = alloc::vec![];
|
|
if Self::is_vehicle_useable(lane_attrs) {
|
|
result.push(Self::Vehicle);
|
|
}
|
|
if Self::is_bus_useable(lane_attrs) {
|
|
result.push(Self::Bus);
|
|
}
|
|
if Self::is_tram_useable(lane_attrs) {
|
|
result.push(Self::Tram);
|
|
}
|
|
if Self::is_bike_useable(lane_attrs) {
|
|
result.push(Self::Bike);
|
|
}
|
|
if Self::is_pedestrian_useable(lane_attrs) {
|
|
result.push(Self::Pedestrian);
|
|
}
|
|
|
|
if result.is_empty() {
|
|
Err(format!("Unsupported LaneAttributes: {lane_attrs}"))
|
|
} else {
|
|
Ok(result)
|
|
}
|
|
}
|
|
|
|
/// See [`UsageType::Vehicle`]
|
|
fn is_vehicle_useable(lane_attrs: &etsi_its_dsrc::LaneAttributes) -> bool {
|
|
match &lane_attrs.lane_type {
|
|
etsi_its_dsrc::LaneTypeAttributes::vehicle(attrs) => {
|
|
!attrs.get_restricted_to_bus_use()
|
|
&& !attrs.get_restricted_to_taxi_use()
|
|
&& !attrs.get_restricted_from_public_use()
|
|
}
|
|
_ => false,
|
|
}
|
|
}
|
|
|
|
/// See [`UsageType::Bus`]
|
|
fn is_bus_useable(lane_attrs: &etsi_its_dsrc::LaneAttributes) -> bool {
|
|
match &lane_attrs.lane_type {
|
|
etsi_its_dsrc::LaneTypeAttributes::vehicle(attrs) => {
|
|
attrs.get_restricted_to_bus_use() | lane_attrs.shared_with.get_bus_vehicle_traffic()
|
|
}
|
|
_ => false,
|
|
}
|
|
}
|
|
|
|
/// See [`UsageType::Tram`]
|
|
fn is_tram_useable(lane_attrs: &etsi_its_dsrc::LaneAttributes) -> bool {
|
|
match &lane_attrs.lane_type {
|
|
etsi_its_dsrc::LaneTypeAttributes::trackedVehicle(_) => true,
|
|
etsi_its_dsrc::LaneTypeAttributes::vehicle(_) => {
|
|
lane_attrs.shared_with.get_tracked_vehicle_traffic()
|
|
}
|
|
_ => false,
|
|
}
|
|
}
|
|
|
|
/// See [`UsageType::Bike`]
|
|
fn is_bike_useable(lane_attrs: &etsi_its_dsrc::LaneAttributes) -> bool {
|
|
match &lane_attrs.lane_type {
|
|
etsi_its_dsrc::LaneTypeAttributes::bikeLane(_) => true,
|
|
// etsi_its_dsrc::LaneTypeAttributes::sidewalk(attrs) => {
|
|
// attrs.get_bicycle_use_allowed()
|
|
// }
|
|
etsi_its_dsrc::LaneTypeAttributes::vehicle(_) => {
|
|
lane_attrs.shared_with.get_cyclist_vehicle_traffic()
|
|
}
|
|
_ => false,
|
|
}
|
|
}
|
|
|
|
/// See [`UsageType::Pedestrian`]
|
|
fn is_pedestrian_useable(lane_attrs: &etsi_its_dsrc::LaneAttributes) -> bool {
|
|
match &lane_attrs.lane_type {
|
|
etsi_its_dsrc::LaneTypeAttributes::crosswalk(_)
|
|
| etsi_its_dsrc::LaneTypeAttributes::sidewalk(_) => true,
|
|
// etsi_its_dsrc::LaneTypeAttributes::vehicle(_) => {
|
|
// lane_attrs.shared_with.get_pedestrian_traffic()
|
|
// || lane_attrs.shared_with.get_pedestrians_traffic()
|
|
// }
|
|
_ => false,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc::IntersectionGeometry;
|
|
use c_its_parser::standards::mapem_2_2_1::mapem_pdu_descriptions;
|
|
|
|
use super::*;
|
|
use crate::init_test_env_logger;
|
|
use crate::testdata::{
|
|
MAPEM_HH_560,
|
|
MAPEM_HH_1328,
|
|
MAPEM_HH_2349_L31,
|
|
MAPEM_HH_2349_L32,
|
|
MAPEM_HH_2349_L33,
|
|
};
|
|
|
|
#[test]
|
|
fn map_simple() {
|
|
let etsi = decode_mapem(MAPEM_HH_1328);
|
|
|
|
let layer_id = etsi.map.layer_id.map(|v| v.0);
|
|
if let Some(map) = &etsi.map.intersections {
|
|
for geom in &map.0 {
|
|
let intersection = Intersection::new(layer_id, geom);
|
|
|
|
assert!(intersection.is_complete());
|
|
assert_ne!(geo_types::Point::default(), intersection.center());
|
|
|
|
// validate that center position is reasonable
|
|
let ref_pos_dist =
|
|
geo_alg::haversine_dist(&intersection.center, &intersection.ref_point);
|
|
println!(
|
|
"Map ref {:?}, center {ref_pos_dist:.1} m away",
|
|
intersection.ref_point
|
|
);
|
|
assert!(ref_pos_dist < 15.); // for MAP 1328 the reference point is ~12 meters from the intersection center
|
|
}
|
|
} else {
|
|
panic!("MAPEM is missing intersection data")
|
|
}
|
|
|
|
// forge a layered mapem
|
|
let layer_id = Some(11);
|
|
if let Some(map) = &etsi.map.intersections {
|
|
for geom in &map.0 {
|
|
let intersection = Intersection::new(layer_id, geom);
|
|
|
|
assert!(intersection.is_complete());
|
|
assert_ne!(geo_types::Point::default(), intersection.center());
|
|
}
|
|
} else {
|
|
panic!("MAPEM is missing intersection data")
|
|
}
|
|
|
|
let layer_id = Some(22);
|
|
if let Some(map) = &etsi.map.intersections {
|
|
for geom in &map.0 {
|
|
let intersection = Intersection::new(layer_id, geom);
|
|
|
|
assert!(!intersection.is_complete());
|
|
assert_eq!(geo_types::Point::default(), intersection.center());
|
|
}
|
|
} else {
|
|
panic!("MAPEM is missing intersection data")
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn map_layered() {
|
|
let etsi_layer31 = decode_mapem(MAPEM_HH_2349_L31);
|
|
let etsi_layer32 = decode_mapem(MAPEM_HH_2349_L32);
|
|
let etsi_layer33 = decode_mapem(MAPEM_HH_2349_L33);
|
|
|
|
// add some layer
|
|
let (layer_id, geom) = extract_geom(&etsi_layer32);
|
|
let mut intersection = Intersection::new(layer_id, geom);
|
|
assert!(!intersection.is_complete());
|
|
assert_eq!(geo_types::Point::default(), intersection.center());
|
|
|
|
// add a second layer
|
|
{
|
|
let num_lanes_before = intersection.lanes().len();
|
|
|
|
let (layer_id, geom) = extract_geom(&etsi_layer33);
|
|
intersection.add_layer(layer_id, geom);
|
|
assert!(!intersection.is_complete());
|
|
assert_eq!(geo_types::Point::default(), intersection.center());
|
|
|
|
let num_lanes_after = intersection.lanes().len();
|
|
assert!(num_lanes_before <= num_lanes_after);
|
|
}
|
|
|
|
// add first layer again
|
|
{
|
|
let num_lanes_before = intersection.lanes().len();
|
|
|
|
let (layer_id, geom) = extract_geom(&etsi_layer32);
|
|
intersection.add_layer(layer_id, geom);
|
|
assert!(!intersection.is_complete());
|
|
assert_eq!(geo_types::Point::default(), intersection.center());
|
|
|
|
let num_lanes_after = intersection.lanes().len();
|
|
assert!(num_lanes_before == num_lanes_after);
|
|
}
|
|
|
|
// add final layer
|
|
{
|
|
let num_lanes_before = intersection.lanes().len();
|
|
|
|
let (layer_id, geom) = extract_geom(&etsi_layer31);
|
|
intersection.add_layer(layer_id, geom);
|
|
assert!(intersection.is_complete());
|
|
assert_ne!(geo_types::Point::default(), intersection.center());
|
|
|
|
let num_lanes_after = intersection.lanes().len();
|
|
assert!(num_lanes_before <= num_lanes_after);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn map_signal_groups() {
|
|
init_test_env_logger();
|
|
|
|
let etsi = decode_mapem(MAPEM_HH_1328);
|
|
let layer_id = etsi.map.layer_id.map(|v| v.0);
|
|
if let Some(map) = &etsi.map.intersections {
|
|
for geom in &map.0 {
|
|
let intersection = Intersection::new(layer_id, geom);
|
|
|
|
assert!(intersection.is_complete());
|
|
assert_eq!(2, intersection.signal_groups.len());
|
|
}
|
|
} else {
|
|
panic!("MAPEM is missing intersection data")
|
|
}
|
|
|
|
let etsi = decode_mapem(MAPEM_HH_560);
|
|
let layer_id = etsi.map.layer_id.map(|v| v.0);
|
|
if let Some(map) = &etsi.map.intersections {
|
|
for geom in &map.0 {
|
|
let intersection = Intersection::new(layer_id, geom);
|
|
|
|
assert!(intersection.is_complete());
|
|
assert_eq!(10, intersection.signal_groups.len());
|
|
}
|
|
} else {
|
|
panic!("MAPEM is missing intersection data")
|
|
}
|
|
}
|
|
|
|
fn decode_mapem(uper: &[u8]) -> Box<mapem_pdu_descriptions::MAPEM> {
|
|
if let c_its_parser::ItsMessage::Mapem {
|
|
geonetworking: _,
|
|
transport: _,
|
|
etsi,
|
|
} = c_its_parser::de::decode(uper, c_its_parser::Headers::None).unwrap()
|
|
{
|
|
etsi
|
|
} else {
|
|
panic!("Unexpected C-ITS message")
|
|
}
|
|
}
|
|
|
|
fn extract_geom(
|
|
etsi: &Box<mapem_pdu_descriptions::MAPEM>,
|
|
) -> (Option<u8>, &IntersectionGeometry) {
|
|
let layer_id = etsi.map.layer_id.as_ref().map(|v| v.0);
|
|
if let Some(map) = &etsi.map.intersections {
|
|
// just assume we only have one intersection per mapem
|
|
(layer_id, map.0.first().unwrap())
|
|
} else {
|
|
panic!("MAPEM is missing intersection data")
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn maneuver_order() {
|
|
let left = Maneuvers::new(true, false, false);
|
|
let straight_and_left = Maneuvers::new(true, true, false);
|
|
let straight = Maneuvers::new(false, true, false);
|
|
let omni = Maneuvers::new(true, true, true);
|
|
let straight_and_right = Maneuvers::new(false, true, true);
|
|
let right = Maneuvers::new(false, false, true);
|
|
|
|
let maneuvers = vec![
|
|
right,
|
|
omni,
|
|
straight,
|
|
left,
|
|
straight_and_left,
|
|
straight_and_right,
|
|
];
|
|
|
|
let mut sorted = maneuvers.clone();
|
|
sorted.sort();
|
|
|
|
println!("Sorted maneuvers:");
|
|
for manv in &sorted {
|
|
println!("- {manv}");
|
|
}
|
|
|
|
assert_eq!(left, sorted[0]);
|
|
assert_eq!(straight_and_left, sorted[1]);
|
|
assert_eq!(omni, sorted[2]);
|
|
assert_eq!(straight, sorted[3]);
|
|
assert_eq!(straight_and_right, sorted[4]);
|
|
assert_eq!(right, sorted[5]);
|
|
|
|
// was a bug at 2026-06-21
|
|
let maneuvers = vec![right, left, straight_and_left, straight];
|
|
|
|
let mut sorted = maneuvers.clone();
|
|
sorted.sort();
|
|
|
|
println!("Sorted maneuvers:");
|
|
for manv in &sorted {
|
|
println!("- {manv}");
|
|
}
|
|
|
|
assert_eq!(left, sorted[0]);
|
|
assert_eq!(straight_and_left, sorted[1]);
|
|
assert_eq!(straight, sorted[2]);
|
|
assert_eq!(right, sorted[3]);
|
|
}
|
|
}
|