mirror of
https://github.com/a-b-street/abstreet.git
synced 2024-12-25 15:33:44 +03:00
making the new protocol for agent/intersection interaction. it works!
\o/
This commit is contained in:
parent
fe62537ee3
commit
af706f72e1
@ -1,7 +1,6 @@
|
||||
// Copyright 2018 Google LLC, licensed under http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
use abstutil::{deserialize_btreemap, serialize_btreemap};
|
||||
use control::ControlMap;
|
||||
use dimensioned::si;
|
||||
use draw_car::DrawCar;
|
||||
use geom::{Angle, Pt2D};
|
||||
@ -52,7 +51,13 @@ impl Car {
|
||||
}
|
||||
|
||||
// Note this doesn't change the car's state, and it observes a fixed view of the world!
|
||||
fn react(&self, map: &Map, time: Tick, sim: &DrivingSimState) -> Action {
|
||||
fn react(
|
||||
&self,
|
||||
map: &Map,
|
||||
time: Tick,
|
||||
sim: &DrivingSimState,
|
||||
intersections: &IntersectionSimState,
|
||||
) -> Action {
|
||||
let desired_on: On = {
|
||||
if let Some(on) = self.waiting_for {
|
||||
on
|
||||
@ -84,7 +89,12 @@ impl Car {
|
||||
On::Lane(id) => sim.lanes[id.0].cars_queue[0] == self.id,
|
||||
On::Turn(id) => sim.turns[&id].cars_queue[0] == self.id,
|
||||
};
|
||||
if has_room_now && is_lead_vehicle {
|
||||
let intersection_req_granted = match desired_on {
|
||||
// Already doing a turn, finish it!
|
||||
On::Lane(_) => true,
|
||||
On::Turn(id) => intersections.request_granted(Request::for_car(self.id, id), map),
|
||||
};
|
||||
if has_room_now && is_lead_vehicle && intersection_req_granted {
|
||||
Action::Goto(desired_on)
|
||||
} else {
|
||||
Action::WaitFor(desired_on)
|
||||
@ -273,19 +283,16 @@ impl DrivingSimState {
|
||||
self.turns.insert(id, SimQueue::new(On::Turn(id), map));
|
||||
}
|
||||
|
||||
pub fn step(
|
||||
&mut self,
|
||||
time: Tick,
|
||||
map: &Map,
|
||||
control_map: &ControlMap,
|
||||
intersections: &mut IntersectionSimState,
|
||||
) {
|
||||
pub fn step(&mut self, time: Tick, map: &Map, intersections: &mut IntersectionSimState) {
|
||||
// Could be concurrent, since this is deterministic.
|
||||
let mut requested_moves: Vec<(CarID, Action)> = Vec::new();
|
||||
for c in self.cars.values() {
|
||||
requested_moves.push((c.id, c.react(map, time, &self)));
|
||||
requested_moves.push((c.id, c.react(map, time, &self, intersections)));
|
||||
}
|
||||
|
||||
// In AORTA, there was a split here -- react vs step phase. We're still following the same
|
||||
// thing, but it might be slightly more clear to express it differently?
|
||||
|
||||
// Apply moves, resolving conflicts. This has to happen serially.
|
||||
// It might make more sense to push the conflict resolution down to SimQueue?
|
||||
// TODO should shuffle deterministically here, to be more fair
|
||||
@ -297,19 +304,12 @@ impl DrivingSimState {
|
||||
}
|
||||
Action::Continue => {}
|
||||
Action::Goto(on) => {
|
||||
// Order matters due to can_do_turn being mutable and due to
|
||||
// new_car_entered_this_step.
|
||||
let mut ok_to_turn = true;
|
||||
if let On::Turn(t) = on {
|
||||
ok_to_turn = intersections.can_do_turn(
|
||||
Request::for_car(*id, t),
|
||||
time,
|
||||
map,
|
||||
control_map,
|
||||
);
|
||||
}
|
||||
|
||||
if new_car_entered_this_step.contains(&on) || !ok_to_turn {
|
||||
// Order matters due to new_car_entered_this_step.
|
||||
// TODO rethink this, since intersections should make this safe
|
||||
if new_car_entered_this_step.contains(&on) {
|
||||
// The car thought they could go, but have to abort last-minute. We may
|
||||
// need to set waiting_for, since the car didn't necessarily return WaitFor
|
||||
// previously.
|
||||
self.cars.get_mut(&id).unwrap().waiting_for = Some(on);
|
||||
} else {
|
||||
new_car_entered_this_step.insert(on);
|
||||
@ -331,10 +331,16 @@ impl DrivingSimState {
|
||||
}
|
||||
Action::WaitFor(on) => {
|
||||
self.cars.get_mut(&id).unwrap().waiting_for = Some(on);
|
||||
if let On::Turn(t) = on {
|
||||
// Note this is idempotent and does NOT grant the request.
|
||||
intersections.submit_request(Request::for_car(*id, t), time, map);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// TODO could simplify this by only adjusting the SimQueues we need above
|
||||
|
||||
// Group cars by lane and turn
|
||||
// TODO ideally, just hash On
|
||||
let mut cars_per_lane = MultiMap::new();
|
||||
|
@ -5,7 +5,7 @@ use control::stop_signs::{ControlStopSign, TurnPriority};
|
||||
use control::ControlMap;
|
||||
use dimensioned::si;
|
||||
use map_model::{IntersectionID, Map, TurnID};
|
||||
use std::collections::BTreeMap;
|
||||
use std::collections::{BTreeMap, BTreeSet};
|
||||
use {CarID, PedestrianID, Tick, SPEED_LIMIT};
|
||||
|
||||
use std;
|
||||
@ -20,7 +20,7 @@ pub enum AgentID {
|
||||
Pedestrian(PedestrianID),
|
||||
}
|
||||
|
||||
#[derive(Serialize, Deserialize, PartialEq, Eq, PartialOrd, Ord)]
|
||||
#[derive(Serialize, Deserialize, PartialEq, Eq, PartialOrd, Ord, Clone)]
|
||||
pub struct Request {
|
||||
pub agent: AgentID,
|
||||
pub turn: TurnID,
|
||||
@ -62,29 +62,63 @@ impl IntersectionSimState {
|
||||
IntersectionSimState { intersections }
|
||||
}
|
||||
|
||||
// This must only be called when the agent is ready to enter the intersection.
|
||||
pub fn can_do_turn(
|
||||
&mut self,
|
||||
req: Request,
|
||||
time: Tick,
|
||||
map: &Map,
|
||||
control_map: &ControlMap,
|
||||
) -> bool {
|
||||
// This is just an immutable query.
|
||||
pub fn request_granted(&self, req: Request, map: &Map) -> bool {
|
||||
let i = map.get_t(req.turn).parent;
|
||||
// TODO this operates on common state, so dont delegate to the policy
|
||||
match self.intersections[i.0] {
|
||||
IntersectionPolicy::StopSignPolicy(ref p) => {
|
||||
p.accepted.get(&req.agent) == Some(&req.turn)
|
||||
}
|
||||
IntersectionPolicy::TrafficSignalPolicy(ref p) => {
|
||||
p.accepted.get(&req.agent) == Some(&req.turn)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// This is mutable, but MUST be idempotent, because it could be called in parallel/nondet
|
||||
// orders. It does NOT grant the request, just enqueues it for later consideration. The agent
|
||||
// must be ready to enter the intersection (leader vehicle and at the end of the lane already).
|
||||
// The request may have been previously granted, but the agent might not have been able to
|
||||
// start the turn.
|
||||
pub fn submit_request(&mut self, req: Request, time: Tick, map: &Map) {
|
||||
let i = map.get_t(req.turn).parent;
|
||||
// TODO this operates on common state, so dont delegate to the policy
|
||||
match self.intersections[i.0] {
|
||||
IntersectionPolicy::StopSignPolicy(ref mut p) => {
|
||||
p.can_do_turn(req, time, map, control_map)
|
||||
if let Some(t) = p.accepted.get(&req.agent) {
|
||||
assert_eq!(*t, req.turn);
|
||||
} else {
|
||||
// TODO assert that the agent hasn't requested something different previously
|
||||
if !p.started_waiting_at.contains_key(&req) {
|
||||
p.started_waiting_at.insert(req, time);
|
||||
}
|
||||
}
|
||||
}
|
||||
IntersectionPolicy::TrafficSignalPolicy(ref mut p) => {
|
||||
p.can_do_turn(req, time, map, control_map)
|
||||
if let Some(t) = p.accepted.get(&req.agent) {
|
||||
assert_eq!(*t, req.turn);
|
||||
} else {
|
||||
// TODO assert that the agent hasn't requested something different previously
|
||||
p.requests.insert(req);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn step(&mut self, time: Tick, map: &Map, control_map: &ControlMap) {
|
||||
for i in self.intersections.iter_mut() {
|
||||
match i {
|
||||
IntersectionPolicy::StopSignPolicy(ref mut p) => p.step(time, map, control_map),
|
||||
IntersectionPolicy::TrafficSignalPolicy(ref mut p) => {
|
||||
p.step(time, map, control_map)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn on_enter(&self, req: Request, map: &Map) {
|
||||
let i = map.get_t(req.turn).parent;
|
||||
|
||||
match self.intersections[i.0] {
|
||||
IntersectionPolicy::StopSignPolicy(ref p) => p.on_enter(req),
|
||||
IntersectionPolicy::TrafficSignalPolicy(ref p) => p.on_enter(req),
|
||||
@ -93,7 +127,6 @@ impl IntersectionSimState {
|
||||
|
||||
pub fn on_exit(&mut self, req: Request, map: &Map) {
|
||||
let i = map.get_t(req.turn).parent;
|
||||
|
||||
match self.intersections[i.0] {
|
||||
IntersectionPolicy::StopSignPolicy(ref mut p) => p.on_exit(req),
|
||||
IntersectionPolicy::TrafficSignalPolicy(ref mut p) => p.on_exit(req),
|
||||
@ -116,13 +149,10 @@ struct StopSign {
|
||||
// is an alt.
|
||||
#[serde(serialize_with = "serialize_btreemap")]
|
||||
#[serde(deserialize_with = "deserialize_btreemap")]
|
||||
started_waiting_at: BTreeMap<AgentID, Tick>,
|
||||
started_waiting_at: BTreeMap<Request, Tick>,
|
||||
#[serde(serialize_with = "serialize_btreemap")]
|
||||
#[serde(deserialize_with = "deserialize_btreemap")]
|
||||
accepted: BTreeMap<AgentID, TurnID>,
|
||||
#[serde(serialize_with = "serialize_btreemap")]
|
||||
#[serde(deserialize_with = "deserialize_btreemap")]
|
||||
waiting: BTreeMap<AgentID, TurnID>,
|
||||
}
|
||||
|
||||
impl StopSign {
|
||||
@ -131,7 +161,6 @@ impl StopSign {
|
||||
id,
|
||||
started_waiting_at: BTreeMap::new(),
|
||||
accepted: BTreeMap::new(),
|
||||
waiting: BTreeMap::new(),
|
||||
}
|
||||
}
|
||||
|
||||
@ -151,51 +180,43 @@ impl StopSign {
|
||||
) -> bool {
|
||||
let base_t = map.get_t(turn);
|
||||
let base_priority = ss.get_priority(turn);
|
||||
self.waiting
|
||||
.values()
|
||||
.find(|t| base_t.conflicts_with(map.get_t(**t)) && ss.get_priority(**t) > base_priority)
|
||||
self.started_waiting_at
|
||||
.keys()
|
||||
.find(|req| {
|
||||
base_t.conflicts_with(map.get_t(req.turn))
|
||||
&& ss.get_priority(req.turn) > base_priority
|
||||
})
|
||||
.is_some()
|
||||
}
|
||||
|
||||
fn can_do_turn(
|
||||
&mut self,
|
||||
req: Request,
|
||||
time: Tick,
|
||||
map: &Map,
|
||||
control_map: &ControlMap,
|
||||
) -> bool {
|
||||
let (agent, turn) = (req.agent, req.turn);
|
||||
assert_eq!(map.get_t(turn).parent, self.id);
|
||||
fn step(&mut self, time: Tick, map: &Map, control_map: &ControlMap) {
|
||||
let mut newly_accepted: Vec<Request> = Vec::new();
|
||||
for (req, started_waiting) in self.started_waiting_at.iter() {
|
||||
let (agent, turn) = (req.agent, req.turn);
|
||||
assert_eq!(map.get_t(turn).parent, self.id);
|
||||
assert_eq!(self.accepted.contains_key(&agent), false);
|
||||
|
||||
if self.accepted.contains_key(&agent) {
|
||||
return true;
|
||||
if self.conflicts_with_accepted(turn, map) {
|
||||
continue;
|
||||
}
|
||||
|
||||
let ss = &control_map.stop_signs[&self.id];
|
||||
if self.conflicts_with_waiting_with_higher_priority(turn, map, ss) {
|
||||
continue;
|
||||
}
|
||||
if ss.get_priority(turn) == TurnPriority::Stop
|
||||
&& (time - *started_waiting).as_time() < WAIT_AT_STOP_SIGN
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
newly_accepted.push(req.clone());
|
||||
}
|
||||
|
||||
if !self.started_waiting_at.contains_key(&agent) {
|
||||
self.started_waiting_at.insert(agent, time);
|
||||
for req in newly_accepted.into_iter() {
|
||||
self.accepted.insert(req.agent, req.turn);
|
||||
self.started_waiting_at.remove(&req);
|
||||
}
|
||||
|
||||
if self.conflicts_with_accepted(turn, map) {
|
||||
self.waiting.insert(agent, turn);
|
||||
return false;
|
||||
}
|
||||
|
||||
let ss = &control_map.stop_signs[&self.id];
|
||||
if self.conflicts_with_waiting_with_higher_priority(turn, map, ss) {
|
||||
self.waiting.insert(agent, turn);
|
||||
return false;
|
||||
}
|
||||
if ss.get_priority(turn) == TurnPriority::Stop
|
||||
&& (time - self.started_waiting_at[&agent]).as_time() < WAIT_AT_STOP_SIGN
|
||||
{
|
||||
self.waiting.insert(agent, turn);
|
||||
return false;
|
||||
}
|
||||
|
||||
self.accepted.insert(agent, turn);
|
||||
self.waiting.remove(&agent);
|
||||
self.started_waiting_at.remove(&agent);
|
||||
true
|
||||
}
|
||||
|
||||
fn on_enter(&self, req: Request) {
|
||||
@ -214,6 +235,7 @@ struct TrafficSignal {
|
||||
#[serde(serialize_with = "serialize_btreemap")]
|
||||
#[serde(deserialize_with = "deserialize_btreemap")]
|
||||
accepted: BTreeMap<AgentID, TurnID>,
|
||||
requests: BTreeSet<Request>,
|
||||
}
|
||||
|
||||
impl TrafficSignal {
|
||||
@ -221,43 +243,40 @@ impl TrafficSignal {
|
||||
TrafficSignal {
|
||||
id,
|
||||
accepted: BTreeMap::new(),
|
||||
requests: BTreeSet::new(),
|
||||
}
|
||||
}
|
||||
|
||||
// TODO determine if agents are staying in the intersection past the cycle time.
|
||||
|
||||
fn can_do_turn(
|
||||
&mut self,
|
||||
req: Request,
|
||||
time: Tick,
|
||||
map: &Map,
|
||||
control_map: &ControlMap,
|
||||
) -> bool {
|
||||
let turn = map.get_t(req.turn);
|
||||
|
||||
assert_eq!(turn.parent, self.id);
|
||||
|
||||
if self.accepted.contains_key(&req.agent) {
|
||||
return true;
|
||||
}
|
||||
|
||||
fn step(&mut self, time: Tick, map: &Map, control_map: &ControlMap) {
|
||||
let signal = &control_map.traffic_signals[&self.id];
|
||||
let (cycle, remaining_cycle_time) = signal.current_cycle_and_remaining_time(time.as_time());
|
||||
|
||||
if !cycle.contains(turn.id) {
|
||||
return false;
|
||||
}
|
||||
// How long will it take the agent to cross the turn?
|
||||
// TODO different speeds
|
||||
let crossing_time = turn.length() / SPEED_LIMIT;
|
||||
// TODO account for TIMESTEP
|
||||
let mut keep_requests: BTreeSet<Request> = BTreeSet::new();
|
||||
for req in self.requests.iter() {
|
||||
let turn = map.get_t(req.turn);
|
||||
let agent = req.agent;
|
||||
assert_eq!(turn.parent, self.id);
|
||||
assert_eq!(self.accepted.contains_key(&agent), false);
|
||||
|
||||
if crossing_time < remaining_cycle_time {
|
||||
self.accepted.insert(req.agent, turn.id);
|
||||
return true;
|
||||
if !cycle.contains(turn.id) {
|
||||
keep_requests.insert(req.clone());
|
||||
continue;
|
||||
}
|
||||
// How long will it take the agent to cross the turn?
|
||||
// TODO different speeds
|
||||
let crossing_time = turn.length() / SPEED_LIMIT;
|
||||
// TODO account for TIMESTEP
|
||||
|
||||
if crossing_time < remaining_cycle_time {
|
||||
self.accepted.insert(req.agent, turn.id);
|
||||
} else {
|
||||
keep_requests.insert(req.clone());
|
||||
}
|
||||
}
|
||||
|
||||
false
|
||||
self.requests = keep_requests;
|
||||
}
|
||||
|
||||
fn on_enter(&self, req: Request) {
|
||||
|
@ -246,8 +246,9 @@ impl Sim {
|
||||
|
||||
// TODO Vanish action should become Park
|
||||
self.driving_state
|
||||
.step(self.time, map, control_map, &mut self.intersection_state);
|
||||
.step(self.time, map, &mut self.intersection_state);
|
||||
self.walking_state.step(TIMESTEP, map, control_map);
|
||||
self.intersection_state.step(self.time, map, control_map);
|
||||
}
|
||||
|
||||
pub fn get_car_state(&self, c: CarID) -> CarState {
|
||||
|
Loading…
Reference in New Issue
Block a user