a-b-street/abstreet
1
1
Fork 0
mirror of https://github.com/a-b-street/abstreet.git synced 2024-12-24 23:15:24 +03:00
Code Issues Projects Releases Wiki Activity

making the new protocol for agent/intersection interaction. it works!

Browse Source 
\o/
This commit is contained in:
Dustin Carlino 2018-07-30 15:30:40 -07:00
parent fe62537ee3
commit af706f72e1
3 changed files with 134 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
sim/src/driving.rs
View File

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

185
sim/src/intersections.rs
View File

@ -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) {

3
sim/src/sim.rs
View File

@ -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 {