Skip to content

Prepare to refactor routing behaviour #13

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
LdDl wants to merge 5 commits into
base: master
Choose a base branch
from
Open

Prepare to refactor routing behaviour #13

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
cc43400
log num of vehicles after step
LdDl Feb 25, 2026
6fee8c5
distinct dest and trip_dest
LdDl Feb 25, 2026
0cab15c
disable temporary depth + allow fail test
LdDl Feb 26, 2026
09e58f9
early return on red signal
LdDl Feb 26, 2026
dd93351
bearing based on maneuver
LdDl Feb 26, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions src/agents/vehicle.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,8 @@ pub struct Vehicle {
pub min_safe_distance: i32,
/// Final cell for the vehicle's trip
pub destination: CellID,
/// Final trip destination (immutable, used for completion check; destination may change for transit cells)
pub trip_destination: CellID,

/// A boolean indicating if the vehicle is a confclict participant
pub is_conflict_participant: bool,
Expand Down Expand Up @@ -162,6 +164,7 @@ impl Vehicle {
bearing: 0.0,
min_safe_distance: 0,
destination: -1,
trip_destination: -1,
is_conflict_participant: false,
trip: -1,
transits_made: 0,
Expand Down Expand Up @@ -672,6 +675,7 @@ impl VehicleBuilder {
/// ```
pub fn with_destination(mut self, cell_id: CellID) -> Self {
self.vehicle.destination = cell_id;
self.vehicle.trip_destination = cell_id;
self
}

Expand Down
264 changes: 141 additions & 123 deletions src/intentions/intention.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@ use crate::behaviour::BehaviourType;
use crate::agents::{
TailIntentionManeuver, Vehicle, VehicleError, VehicleID, VehicleIntention,
};
use crate::grid::cell::CellState;
use crate::grid::cell::{Cell, CellState};
use crate::maneuver::LaneChangeType;
use crate::grid::{cell::CellID, road_network::GridRoads};
use crate::intentions::{intention_type::IntentionType, Intentions};
Expand Down Expand Up @@ -238,6 +238,27 @@ pub fn find_intention<'a>(
return Ok(result);
}

// Шf stopped at red traffic light do early return
let forward_cell_id = source_cell.get_forward_id();
if forward_cell_id > 0 {
if let Some(forward_cell) = net.get_cell(&forward_cell_id) {
if forward_cell.get_state() == CellState::Banned {
let result = VehicleIntention {
intention_maneuver: LaneChangeType::Block,
intention_speed: 0,
destination: None,
confusion: None,
intention_cell_id: vehicle.cell_id,
tail_intention_cells: vec![],
intermediate_cells: Vec::with_capacity(0),
tail_maneuver: tail_maneuver,
should_stop: false,
};
return Ok(result);
}
}
}

// Vehicle's speed should not be greater than speed limit
let mut intention_speed = vehicle.speed.min(speed_limit);

Expand All @@ -263,14 +284,14 @@ pub fn find_intention<'a>(
}

// Considering that vehicle always wants to accelerate:
let observe_distance = speed_possible + vehicle.min_safe_distance;
let _observe_distance = speed_possible + vehicle.min_safe_distance;

// Check if maneuvers are allowed (they could be prohibeted due the vehicle's tail is not done previous maneuver yet)
let maneuvers_allowed = vehicle.timer_non_maneuvers <= 0
&& tail_maneuver.intention_maneuver != LaneChangeType::ChangeRight
&& tail_maneuver.intention_maneuver != LaneChangeType::ChangeLeft;

let mut destination: Option<CellID> = None;
let destination: Option<CellID> = None;
let mut confusion: Option<bool> = None;

// println!(
Expand All @@ -293,7 +314,8 @@ pub fn find_intention<'a>(
source_cell,
net,
maneuvers_allowed,
observe_distance + 1,
1000,
// observe_distance + 1, // depth-limited
) {
Ok(path) => path,
Err(e) => {
Expand All @@ -313,9 +335,16 @@ pub fn find_intention<'a>(
target_cell,
net,
maneuvers_allowed,
Some(observe_distance + 1),
None,
// Some(observe_distance + 1), // depth-limited
) {
Ok(path) => path,
Ok(path) => {
// Clear confusion if vehicle was previously in confusion mode and found path
if vehicle.confusion {
confusion = Some(false);
}
path
},
Err(e)
if e != shortest_path::router::AStarError::NoPathFound {
start_id: source_cell.get_id(),
Expand All @@ -329,7 +358,7 @@ pub fn find_intention<'a>(
Ok(path) => path,
Err(e) => return Err(IntentionError::NoPathForNoRoute(e)),
};
destination = Some(new_path.vertices()[new_path.vertices().len() - 1].get_id());
// Do NOT overwrite destination - keep original trip destination
intention_speed = 1;
speed_possible = intention_speed;
confusion = Some(true);
Expand Down Expand Up @@ -431,6 +460,70 @@ const UNDEFINED_MANEUVER: LaneChangeType = LaneChangeType::ChangeRight;

/// Attempts to find an alternate maneuver (lane change) for a blocked vehicle.
///
/// Helper: Creates a block intention
fn create_block_intention(cell_id: CellID, should_stop: bool) -> VehicleIntention {
VehicleIntention {
intention_maneuver: LaneChangeType::Block,
intention_speed: 0,
destination: None,
confusion: None,
intention_cell_id: cell_id,
tail_intention_cells: vec![],
intermediate_cells: Vec::with_capacity(0),
tail_maneuver: TailIntentionManeuver::default(),
should_stop,
}
}

/// Helper: Checks if alternate path (left or right) is available and calculates cost
fn check_alternate_direction(
cell_id: CellID,
source_cell: &Cell,
target_cell: &Cell,
net: &GridRoads,
current_state: &HashMap<CellID, VehicleID>,
direction: &str,
max_depth: Option<i32>,
) -> Result<(CellID, f64), IntentionError> {
if cell_id <= 0 {
return Ok((-1, INFINITY));
}

let cell = net.get_cell(&cell_id).ok_or_else(|| {
if direction == "left" {
IntentionError::NoLeftCell(source_cell.get_id())
} else {
IntentionError::NoRightCell(source_cell.get_id())
}
})?;

let is_blocked = current_state
.get(&cell_id)
.map(|&id| id > 0)
.unwrap_or(false);

if is_blocked || cell.get_state() != CellState::Free {
return Ok((-1, INFINITY));
}

match shortest_path(cell, target_cell, net, true, max_depth) {
Ok(path) => {
let cost = path.cost() + source_cell.distance_to(cell);
Ok((cell_id, cost))
}
Err(shortest_path::router::AStarError::NoPathFound { .. }) => {
Ok((-1, INFINITY))
}
Err(_) => {
if direction == "left" {
Err(IntentionError::LeftPathFind(cell_id))
} else {
Err(IntentionError::RightPathFind(cell_id))
}
}
}
}

/// If the vehicle cannot move forward, tries left or right lane changes
/// and selects the best available option.
///
Expand All @@ -447,18 +540,7 @@ pub fn find_alternate_intention<'a>(

// If maneuvers are not allowed (tail still completing previous maneuver), block immediately
if !maneuvers_allowed {
let result = VehicleIntention {
intention_maneuver: LaneChangeType::Block,
intention_speed: 0,
destination: None,
confusion: None,
intention_cell_id: source_cell_id,
tail_intention_cells: vec![],
intermediate_cells: Vec::with_capacity(0),
tail_maneuver: TailIntentionManeuver::default(),
should_stop: false,
};
return Ok(result);
return Ok(create_block_intention(source_cell_id, false));
}

let source_cell = net
Expand All @@ -469,103 +551,52 @@ pub fn find_alternate_intention<'a>(
.get_cell(&target_cell_id)
.ok_or(IntentionError::NoTargetCell(target_cell_id))?;

let mut min_left_dist = INFINITY;
let mut min_right_dist = INFINITY;

// Check left maneuver
let mut left_cell_id = source_cell.get_left_id();
if left_cell_id > 0 {
let left_cell = net
.get_cell(&left_cell_id)
.ok_or(IntentionError::NoLeftCell(vehicle.cell_id))?;

// Check if possible maneuver can't be made
let is_blocked = current_state
.get(&left_cell_id)
.map(|&id| id > 0)
.unwrap_or(false);

if !is_blocked && left_cell.get_state() == CellState::Free {
match shortest_path(left_cell, target_cell, net, true, Some(vehicle.speed)) {
Ok(path) => {
let cost = path.cost();
min_left_dist = cost + source_cell.distance_to(left_cell);
}
Err(e)
if e != shortest_path::router::AStarError::NoPathFound {
start_id: left_cell_id,
end_id: target_cell_id,
} =>
{
return Err(IntentionError::LeftPathFind(left_cell_id))
}
Err(_) => {
min_left_dist = INFINITY;
}
}
} else {
left_cell_id = -1;
}
}

// Check right maneuver
let mut right_cell_id = source_cell.get_right_id();
if right_cell_id > 0 {
let right_cell = net
.get_cell(&right_cell_id)
.ok_or(IntentionError::NoRightCell(vehicle.cell_id))?;
// Check left and right alternate paths (no depth limit to see full route)
let (left_cell_id, min_left_dist) = check_alternate_direction(
source_cell.get_left_id(),
source_cell,
target_cell,
net,
current_state,
"left",
None,
// Some(vehicle.speed), // depth-limited
)?;

let is_blocked = current_state
.get(&right_cell_id)
.map(|&id| id > 0)
.unwrap_or(false);
let (right_cell_id, min_right_dist) = check_alternate_direction(
source_cell.get_right_id(),
source_cell,
target_cell,
net,
current_state,
"right",
None,
// Some(vehicle.speed), // depth-limited
)?;

if !is_blocked && right_cell.get_state() == CellState::Free {
match shortest_path(right_cell, target_cell, net, true, Some(vehicle.speed)) {
Ok(path) => {
let cost = path.cost();
min_right_dist = cost + source_cell.distance_to(right_cell);
}
Err(e)
if e != shortest_path::router::AStarError::NoPathFound {
start_id: right_cell_id,
end_id: target_cell_id,
} =>
{
return Err(IntentionError::RightPathFind(right_cell_id))
}
Err(_) => {
min_right_dist = INFINITY;
}
}
} else {
right_cell_id = -1;
}
// If both paths are impossible (infinite distance), don't attempt a lane change - just block
if min_left_dist == INFINITY && min_right_dist == INFINITY {
return Ok(create_block_intention(source_cell_id, true));
}

// Choose best maneuver
let mut min_cell: CellID;
let mut intention_maneuver: LaneChangeType;
if UNDEFINED_MANEUVER == LaneChangeType::ChangeRight {
min_cell = right_cell_id;
intention_maneuver = LaneChangeType::ChangeRight;
if min_left_dist < min_right_dist {
min_cell = left_cell_id;
intention_maneuver = LaneChangeType::ChangeLeft;
}
// Choose best maneuver based on distance comparison
let (min_cell, intention_maneuver) = if min_left_dist < min_right_dist {
(left_cell_id, LaneChangeType::ChangeLeft)
} else if min_right_dist < min_left_dist {
(right_cell_id, LaneChangeType::ChangeRight)
} else {
min_cell = left_cell_id;
intention_maneuver = LaneChangeType::ChangeLeft;
if min_right_dist < min_left_dist {
min_cell = right_cell_id;
intention_maneuver = LaneChangeType::ChangeRight;
// Equal distances - use UNDEFINED_MANEUVER as tiebreaker
if UNDEFINED_MANEUVER == LaneChangeType::ChangeRight {
(right_cell_id, LaneChangeType::ChangeRight)
} else {
(left_cell_id, LaneChangeType::ChangeLeft)
}
}
};

// Apply the chosen maneuver
// Apply the chosen maneuver if valid
if min_cell > 0 {
let result = VehicleIntention {
intention_maneuver: intention_maneuver,
return Ok(VehicleIntention {
intention_maneuver,
intention_speed: 1,
destination: None,
confusion: None,
Expand All @@ -574,22 +605,9 @@ pub fn find_alternate_intention<'a>(
intermediate_cells: Vec::with_capacity(0),
tail_maneuver: TailIntentionManeuver::default(),
should_stop: true,
};
return Ok(result);
})
}
let result = VehicleIntention {
intention_maneuver: LaneChangeType::Block,
intention_speed: 0,
destination: None,
confusion: None,
intention_cell_id: source_cell_id,
tail_intention_cells: vec![],
intermediate_cells: Vec::with_capacity(0),
tail_maneuver: TailIntentionManeuver::default(),
should_stop: false,
};

Ok(result)
Ok(create_block_intention(source_cell_id, false))
}

#[cfg(test)]
Expand Down
Loading