From c3d7595fc3278eb59870f4b8819cc055528ac37b Mon Sep 17 00:00:00 2001 From: Dustin Carlino Date: Fri, 24 Aug 2018 10:51:29 -0700 Subject: [PATCH] (slightly) smarter stop sign assignment --- control/src/stop_signs.rs | 102 ++++++++++++++++++++++++++++-- docs/design.md | 4 ++ editor/src/plugins/road_editor.rs | 5 +- editor/src/plugins/selection.rs | 2 +- editor/src/plugins/warp.rs | 14 +++- editor/src/render/lane.rs | 16 +++-- editor/src/render/map.rs | 13 ++-- editor/src/ui.rs | 19 +++++- geom/src/angle.rs | 9 +++ map_model/src/turn.rs | 30 ++++++++- 10 files changed, 194 insertions(+), 20 deletions(-) diff --git a/control/src/stop_signs.rs b/control/src/stop_signs.rs index ef5dc6f579..3e89c5c3df 100644 --- a/control/src/stop_signs.rs +++ b/control/src/stop_signs.rs @@ -2,8 +2,8 @@ use ModifiedStopSign; -use map_model::{IntersectionID, Map, TurnID}; -use std::collections::HashMap; +use map_model::{IntersectionID, LaneID, Map, TurnID}; +use std::collections::{HashMap, HashSet}; #[derive(Serialize, Deserialize, Debug, PartialEq, Clone, Copy, PartialOrd)] pub enum TurnPriority { @@ -31,7 +31,65 @@ pub struct ControlStopSign { impl ControlStopSign { pub fn new(map: &Map, intersection: IntersectionID) -> ControlStopSign { assert!(!map.get_i(intersection).has_traffic_signal); - ControlStopSign::all_way_stop(map, intersection) + let ss = ControlStopSign::smart_assignment(map, intersection); + ss.validate(map, intersection); + ss + } + + fn smart_assignment(map: &Map, intersection: IntersectionID) -> ControlStopSign { + // Higher numbers are higher rank roads + let mut rank_per_incoming_lane: HashMap = HashMap::new(); + let mut ranks: HashSet = HashSet::new(); + let mut highest_rank = 0; + // TODO should just be incoming, but because of weirdness with sidewalks... + for l in map.get_i(intersection) + .incoming_lanes + .iter() + .chain(map.get_i(intersection).outgoing_lanes.iter()) + { + let r = map.get_parent(*l); + let rank = if let Some(highway) = r.osm_tags.get("highway") { + match highway.as_ref() { + "tertiary" => 10, + "tertiary_link" => 9, + "secondary" => 8, + "residential" => 5, + _ => panic!("Unknown OSM highway {}", highway), + } + } else { + 0 + }; + rank_per_incoming_lane.insert(*l, rank); + highest_rank = highest_rank.max(rank); + ranks.insert(rank); + } + if ranks.len() == 1 { + return ControlStopSign::all_way_stop(map, intersection); + } + + let mut ss = ControlStopSign { + intersection, + turns: HashMap::new(), + changed: false, + }; + for t in &map.get_i(intersection).turns { + if rank_per_incoming_lane[&t.src] == highest_rank { + // If it's the highest rank road, make the straight and right turns priority (if + // possible) and other turns yield. + let turn = map.get_t(*t); + if (turn.is_right_turn(map) || turn.is_straight_turn(map)) + && ss.could_be_priority_turn(*t, map) + { + ss.turns.insert(*t, TurnPriority::Priority); + } else { + ss.turns.insert(*t, TurnPriority::Yield); + } + } else { + // Lower rank roads have to stop. + ss.turns.insert(*t, TurnPriority::Stop); + } + } + ss } fn all_way_stop(map: &Map, intersection: IntersectionID) -> ControlStopSign { @@ -72,6 +130,13 @@ impl ControlStopSign { self.changed } + pub fn is_priority_lane(&self, lane: LaneID) -> bool { + self.turns + .iter() + .find(|(turn, pri)| turn.src == lane && **pri > TurnPriority::Stop) + .is_some() + } + pub fn get_savestate(&self) -> Option { if !self.changed() { return None; @@ -87,7 +152,36 @@ impl ControlStopSign { self.turns = state.turns.clone(); } - // TODO need to color turn icons + fn validate(&self, map: &Map, intersection: IntersectionID) { + // Does the assignment cover the correct set of turns? + let all_turns = &map.get_i(intersection).turns; + assert_eq!(self.turns.len(), all_turns.len()); + for t in all_turns { + assert!(self.turns.contains_key(t)); + } + + // Do any of the priority turns conflict? + let priority_turns: Vec = self.turns + .iter() + .filter_map(|(turn, pri)| { + if *pri == TurnPriority::Priority { + Some(*turn) + } else { + None + } + }) + .collect(); + for t1 in &priority_turns { + for t2 in &priority_turns { + if map.get_t(*t1).conflicts_with(map.get_t(*t2)) { + panic!( + "Stop sign has conflicting priority turns {:?} and {:?}", + t1, t2 + ); + } + } + } + } } #[cfg(test)] diff --git a/docs/design.md b/docs/design.md index 667dbb6c57..17682ed11f 100644 --- a/docs/design.md +++ b/docs/design.md @@ -600,3 +600,7 @@ SidewalkSpot - TODO: https://data-seattlecitygis.opendata.arcgis.com/datasets/channelization - https://data-seattlecitygis.opendata.arcgis.com/datasets/street-signs + +## Stop sign priority + +Use OSM highway tags to rank. For all the turns on the higher priority road, detect priority/yield based on turn angle, I guess. diff --git a/editor/src/plugins/road_editor.rs b/editor/src/plugins/road_editor.rs index 22c2d31ec6..27afd28212 100644 --- a/editor/src/plugins/road_editor.rs +++ b/editor/src/plugins/road_editor.rs @@ -1,3 +1,4 @@ +use control::ControlMap; use ezgui::input::UserInput; use map_model::{EditReason, Edits, LaneID, LaneType, Map}; use piston::input::Key; @@ -21,6 +22,7 @@ impl RoadEditor { current_selection: &SelectionState, map: &mut Map, draw_map: &mut DrawMap, + control_map: &ControlMap, sim: &mut Sim, ) -> bool { let mut new_state: Option = None; @@ -97,9 +99,10 @@ impl RoadEditor { } } + // TODO Pretty sure control map needs to recalculate based on the new turns let old_type = map.get_l(id).lane_type; map.edit_lane_type(id, new_type); - draw_map.edit_lane_type(id, map); + draw_map.edit_lane_type(id, map, control_map); sim.edit_lane_type(id, old_type, map); // Add turns back diff --git a/editor/src/plugins/selection.rs b/editor/src/plugins/selection.rs index b669dff500..77902c2317 100644 --- a/editor/src/plugins/selection.rs +++ b/editor/src/plugins/selection.rs @@ -216,7 +216,7 @@ impl SelectionState { ID::Car(id) => sim.car_tooltip(id), ID::Pedestrian(id) => sim.ped_tooltip(id), ID::Intersection(id) => vec![format!("{}", id)], - ID::Turn(id) => vec![format!("{}", id)], + ID::Turn(id) => map.get_t(id).tooltip_lines(map), ID::ExtraShape(id) => draw_map.get_es(id).tooltip_lines(), }; canvas.draw_mouse_tooltip(g, &lines); diff --git a/editor/src/plugins/warp.rs b/editor/src/plugins/warp.rs index b8ea1d304b..4555c81fac 100644 --- a/editor/src/plugins/warp.rs +++ b/editor/src/plugins/warp.rs @@ -1,7 +1,7 @@ use ezgui::canvas::Canvas; use ezgui::input::UserInput; use ezgui::text_box::TextBox; -use map_model::{geometry, BuildingID, IntersectionID, LaneID, Map, ParcelID}; +use map_model::{geometry, BuildingID, IntersectionID, LaneID, Map, ParcelID, RoadID}; use piston::input::Key; use plugins::selection::SelectionState; use sim::{CarID, PedestrianID, Sim}; @@ -70,6 +70,18 @@ fn warp( let pt = match usize::from_str_radix(&line[1..line.len()], 10) { // TODO express this more succinctly Ok(idx) => match line.chars().next().unwrap() { + 'r' => { + let id = RoadID(idx); + if let Some(r) = map.maybe_get_r(id) { + let l = map.get_l(r.children_forwards[0].0); + println!("Warping to {}, which belongs to {}", l.id, id); + *selection_state = SelectionState::SelectedLane(l.id, None); + l.first_pt() + } else { + println!("{} doesn't exist", id); + return; + } + } 'l' => { let id = LaneID(idx); if let Some(l) = map.maybe_get_l(id) { diff --git a/editor/src/render/lane.rs b/editor/src/render/lane.rs index 0ce61b3c89..6be84e1b23 100644 --- a/editor/src/render/lane.rs +++ b/editor/src/render/lane.rs @@ -2,6 +2,7 @@ use aabb_quadtree::geom::Rect; use colors::{ColorScheme, Colors}; +use control::ControlMap; use dimensioned::si; use ezgui::canvas::Canvas; use ezgui::GfxCtx; @@ -33,7 +34,7 @@ pub struct DrawLane { } impl DrawLane { - pub fn new(lane: &map_model::Lane, map: &map_model::Map) -> DrawLane { + pub fn new(lane: &map_model::Lane, map: &map_model::Map, control_map: &ControlMap) -> DrawLane { let road = map.get_r(lane.parent); let start = new_perp_line(lane.first_line(), geometry::LANE_THICKNESS); let end = new_perp_line(lane.last_line().reverse(), geometry::LANE_THICKNESS); @@ -61,9 +62,8 @@ impl DrawLane { } { markings.push(m); } - // TODO not all sides of the lane have to stop if lane.is_driving() && !map.get_i(lane.dst_i).has_traffic_signal { - if let Some(m) = calculate_stop_sign_line(lane) { + if let Some(m) = calculate_stop_sign_line(lane, control_map) { markings.push(m); } } @@ -264,7 +264,13 @@ fn calculate_driving_lines(lane: &map_model::Lane, parent: &map_model::Road) -> }) } -fn calculate_stop_sign_line(lane: &map_model::Lane) -> Option { +fn calculate_stop_sign_line(lane: &map_model::Lane, control_map: &ControlMap) -> Option { + if control_map.stop_signs[&lane.dst_i].is_priority_lane(lane.id) { + return None; + } + + // TODO maybe draw the stop sign octagon on each lane? + let (pt1, angle) = lane.safe_dist_along(lane.length() - (2.0 * geometry::LANE_THICKNESS * si::M))?; // Reuse perp_line. Project away an arbitrary amount @@ -272,7 +278,7 @@ fn calculate_stop_sign_line(lane: &map_model::Lane) -> Option { Some(Marking { lines: vec![perp_line(Line::new(pt1, pt2), geometry::LANE_THICKNESS)], color: Colors::StopSignMarking, - thickness: 0.25, + thickness: 0.45, round: true, }) } diff --git a/editor/src/render/map.rs b/editor/src/render/map.rs index df9a2ef452..8542dff51c 100644 --- a/editor/src/render/map.rs +++ b/editor/src/render/map.rs @@ -2,6 +2,7 @@ use aabb_quadtree::geom::{Point, Rect}; use aabb_quadtree::QuadTree; +use control::ControlMap; use geom::{LonLat, Pt2D}; use kml::{ExtraShape, ExtraShapeID}; use map_model::{BuildingID, IntersectionID, Lane, LaneID, Map, ParcelID, Turn, TurnID}; @@ -31,10 +32,14 @@ pub struct DrawMap { impl DrawMap { // Also returns the center of the map in map-space - pub fn new(map: &Map, raw_extra_shapes: Vec) -> (DrawMap, Pt2D) { + pub fn new( + map: &Map, + control_map: &ControlMap, + raw_extra_shapes: Vec, + ) -> (DrawMap, Pt2D) { let mut lanes: Vec = Vec::new(); for l in map.all_lanes() { - lanes.push(DrawLane::new(l, map)); + lanes.push(DrawLane::new(l, map, control_map)); } let mut turn_to_lane_offset: HashMap = HashMap::new(); @@ -137,9 +142,9 @@ impl DrawMap { } } - pub fn edit_lane_type(&mut self, id: LaneID, map: &Map) { + pub fn edit_lane_type(&mut self, id: LaneID, map: &Map, control_map: &ControlMap) { // No need to edit the quadtree; the bbox shouldn't depend on lane type. - self.lanes[id.0] = DrawLane::new(map.get_l(id), map); + self.lanes[id.0] = DrawLane::new(map.get_l(id), map, control_map); } pub fn edit_remove_turn(&mut self, id: TurnID) { diff --git a/editor/src/ui.rs b/editor/src/ui.rs index 972beebf89..12ca981f85 100644 --- a/editor/src/ui.rs +++ b/editor/src/ui.rs @@ -52,6 +52,7 @@ pub struct UI { show_intersections: ToggleableLayer, show_parcels: ToggleableLayer, show_extra_shapes: ToggleableLayer, + show_all_turn_icons: ToggleableLayer, debug_mode: ToggleableLayer, // This is a particularly special plugin, since it's always kind of active and other things @@ -96,8 +97,8 @@ impl UI { Vec::new() }; - let (draw_map, center_pt) = render::DrawMap::new(&map, extra_shapes); let control_map = ControlMap::new(&map); + let (draw_map, center_pt) = render::DrawMap::new(&map, &control_map, extra_shapes); let steepness_viz = SteepnessVisualizer::new(&map); let turn_colors = TurnColors::new(&control_map); @@ -128,6 +129,7 @@ impl UI { Key::D7, Some(MIN_ZOOM_FOR_LANES), ), + show_all_turn_icons: ToggleableLayer::new("turn icons", Key::D9, None), debug_mode: ToggleableLayer::new("debug mode", Key::G, None), current_selection_state: SelectionState::Empty, @@ -174,6 +176,7 @@ impl UI { self.show_intersections.handle_zoom(old_zoom, new_zoom); self.show_parcels.handle_zoom(old_zoom, new_zoom); self.show_extra_shapes.handle_zoom(old_zoom, new_zoom); + self.show_all_turn_icons.handle_zoom(old_zoom, new_zoom); self.debug_mode.handle_zoom(old_zoom, new_zoom); } @@ -385,7 +388,9 @@ impl UI { } fn show_icons_for(&self, id: IntersectionID) -> bool { - self.stop_sign_editor.show_turn_icons(id) || self.traffic_signal_editor.show_turn_icons(id) + self.show_all_turn_icons.is_enabled() + || self.stop_sign_editor.show_turn_icons(id) + || self.traffic_signal_editor.show_turn_icons(id) } } @@ -435,6 +440,7 @@ impl gui::GUI for UI { &self.current_selection_state, &mut self.map, &mut self.draw_map, + &self.control_map, &mut self.sim_ctrl.sim )); stop_if_done!(self.current_search_state.event(input)); @@ -486,6 +492,15 @@ impl gui::GUI for UI { } return gui::EventLoopMode::InputOnly; } + if self.show_all_turn_icons.handle_event(input) { + if let SelectionState::SelectedTurn(_) = self.current_selection_state { + self.current_selection_state = SelectionState::Empty; + } + if let SelectionState::Tooltip(ID::Turn(_)) = self.current_selection_state { + self.current_selection_state = SelectionState::Empty; + } + return gui::EventLoopMode::InputOnly; + } stop_if_done!(self.show_parcels.handle_event(input)); stop_if_done!(self.debug_mode.handle_event(input)); diff --git a/geom/src/angle.rs b/geom/src/angle.rs index b210e6d49c..bcb99390d9 100644 --- a/geom/src/angle.rs +++ b/geom/src/angle.rs @@ -1,3 +1,4 @@ +use std; use std::f64; use std::fmt; @@ -36,3 +37,11 @@ impl fmt::Display for Angle { write!(f, "Angle({} degrees)", self.normalized_degrees()) } } + +impl std::ops::Sub for Angle { + type Output = Angle; + + fn sub(self, other: Angle) -> Angle { + Angle(self.0 - other.0) + } +} diff --git a/map_model/src/turn.rs b/map_model/src/turn.rs index 897ef4a5e3..a3e3e6aa78 100644 --- a/map_model/src/turn.rs +++ b/map_model/src/turn.rs @@ -4,8 +4,7 @@ use dimensioned::si; use geom::{Angle, Line, Pt2D}; use std::f64; use std::fmt; -use IntersectionID; -use LaneID; +use {IntersectionID, LaneID, Map}; // Turns are uniquely identified by their (src, dst) lanes and their parent intersection. // Intersection is needed to distinguish crosswalks that exist at two ends of a sidewalk. @@ -45,6 +44,9 @@ impl PartialEq for Turn { impl Turn { pub fn conflicts_with(&self, other: &Turn) -> bool { + if self == other { + return false; + } if self.between_sidewalks && other.between_sidewalks { return false; } @@ -66,4 +68,28 @@ impl Turn { pub fn length(&self) -> si::Meter { self.line.length() } + + // TODO all the stuff based on turn angle is a bit... wrong, especially for sidewalks. :\ + // also, make sure right/left/straight are disjoint... and maybe cover all turns. return an enum from one method. + fn turn_angle(&self, map: &Map) -> Angle { + let lane_angle = map.get_l(self.src).end_line(self.parent).angle(); + self.line.angle() - lane_angle + } + + pub fn is_right_turn(&self, map: &Map) -> bool { + let a = self.turn_angle(map).normalized_degrees(); + a < 95.0 && a > 20.0 + } + + pub fn is_straight_turn(&self, map: &Map) -> bool { + let a = self.turn_angle(map).normalized_degrees(); + a <= 20.0 || a >= 320.0 + } + + pub fn tooltip_lines(&self, map: &Map) -> Vec { + vec![ + format!("{}", self.id), + format!("Angle {}", self.turn_angle(map)), + ] + } }