From af706f72e16f661600cef2b256ce371637973d6a Mon Sep 17 00:00:00 2001 From: Dustin Carlino Date: Mon, 30 Jul 2018 15:30:40 -0700 Subject: [PATCH] making the new protocol for agent/intersection interaction. it works! \o/ --- sim/src/driving.rs | 54 +++++++----- sim/src/intersections.rs | 185 +++++++++++++++++++++------------------ sim/src/sim.rs | 3 +- 3 files changed, 134 insertions(+), 108 deletions(-) diff --git a/sim/src/driving.rs b/sim/src/driving.rs index 6f674d3dbf..a9e4a1139d 100644 --- a/sim/src/driving.rs +++ b/sim/src/driving.rs @@ -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(); diff --git a/sim/src/intersections.rs b/sim/src/intersections.rs index b31a9ba158..66091f14a4 100644 --- a/sim/src/intersections.rs +++ b/sim/src/intersections.rs @@ -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, + started_waiting_at: BTreeMap, #[serde(serialize_with = "serialize_btreemap")] #[serde(deserialize_with = "deserialize_btreemap")] accepted: BTreeMap, - #[serde(serialize_with = "serialize_btreemap")] - #[serde(deserialize_with = "deserialize_btreemap")] - waiting: BTreeMap, } 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 = 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, + requests: BTreeSet, } 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 = 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) { diff --git a/sim/src/sim.rs b/sim/src/sim.rs index 764a3d7f9e..7d300846c2 100644 --- a/sim/src/sim.rs +++ b/sim/src/sim.rs @@ -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 {