diff --git a/examples/driving/crosswalkStraightThrough.scenic b/examples/driving/crosswalkStraightThrough.scenic new file mode 100644 index 000000000..eac9547d2 --- /dev/null +++ b/examples/driving/crosswalkStraightThrough.scenic @@ -0,0 +1,87 @@ +""" Scenario Description +A vehicle drives toward an intersection with the intent to go straight through +it. The intersection has a pedestrian crosswalk on the side the vehicle is +entering or exiting. A pedestrian waits next to the crosswalk. As the ego +approaches, the pedestrian steps onto the crosswalk and walks across. +The ego is expected to brake before reaching the pedestrian. + +To run this file using the MetaDrive simulator: + scenic examples/driving/crosswalkStraightThrough.scenic --2d --model scenic.simulators.metadrive.model --simulate + +To run this file using the Carla simulator: + scenic examples/driving/crosswalkStraightThrough.scenic --2d --model scenic.simulators.carla.model --simulate +""" + +param map = localPath('../../assets/maps/CARLA/Town03.xodr') +param carla_map = 'Town03' +model scenic.domains.driving.model + +param time_step = 1.0/10 + +# CONSTANTS +EGO_SPEED = Range(7, 12) +EGO_OFFSET = -1 * Range(25, 40) +PED_SPEED = 1.2 +PED_THRESHOLD = Range(35, 55) + +# BEHAVIOR +behavior EgoBehavior(target_speed=10, trajectory=None): + assert trajectory is not None + brakeIntensity = 0.7 + + try: + do FollowTrajectoryBehavior(target_speed=target_speed, trajectory=trajectory) + terminate + + interrupt when withinDistanceToAnyPedestrians(self, 12): + take SetBrakeAction(brakeIntensity) + +behavior PedestrianCrossBehavior(threshold=20, walking_speed=1.2, walking_direction=0): + while (distance from self to ego) > threshold: + wait + take SetWalkingDirectionAction(walking_direction), SetWalkingSpeedAction(walking_speed) + +# GEOMETRY +def orderedEnds(cr, m): + ends = [cr.centerline[0], cr.centerline[-1]] + ends.sort(key=lambda p: distance from p to m.startLane.centerline) + return ends + +straightManeuverCrossingTriples = [] +for inter in network.intersections: + for cr in inter.crossings: + for m in cr.conflictingManeuvers: + if m.type != ManeuverType.STRAIGHT: + continue + near, far = orderedEnds(cr, m) + straightManeuverCrossingTriples.append((inter, m, cr, near, far)) +chosen = Uniform(*straightManeuverCrossingTriples) +intersection = chosen[0] +straight_maneuver = chosen[1] +crossing = chosen[2] +pedSpawn = chosen[3] +pedEnd = chosen[4] + +startLane = straight_maneuver.startLane +connectingLane = straight_maneuver.connectingLane +endLane = straight_maneuver.endLane + +lane_traj = [startLane, connectingLane, endLane] +intersection_edge = startLane.centerline[-1] +egoStartPoint = new OrientedPoint at intersection_edge + +# -- + +walkDirection = angle from pedSpawn to pedEnd +pedStartPoint = new OrientedPoint at pedSpawn, + facing walkDirection + +# PLACEMENT +ego = new Car following roadDirection from egoStartPoint for EGO_OFFSET, + with behavior EgoBehavior(target_speed=EGO_SPEED, trajectory=lane_traj) + +ped = new Pedestrian at pedStartPoint, + with regionContainedIn None, + with parentOrientation 0, + with behavior PedestrianCrossBehavior(PED_THRESHOLD, PED_SPEED, walkDirection), + facing walkDirection diff --git a/src/scenic/domains/driving/roads.py b/src/scenic/domains/driving/roads.py index f06377c5a..3c41d120c 100644 --- a/src/scenic/domains/driving/roads.py +++ b/src/scenic/domains/driving/roads.py @@ -726,6 +726,18 @@ class PedestrianCrossing(_ContainsCenterline, LinearElement): startSidewalk: Sidewalk endSidewalk: Sidewalk + @property + @utils.cached + def conflictingManeuvers(self) -> Tuple[Maneuver]: + """Maneuvers whose connecting lanes intersect this crossing.""" + if not isinstance(self.parent, Intersection): + return () + conflicts = [] + for maneuver in self.parent.maneuvers: + if maneuver.connectingLane.centerline.intersects(self.centerline): + conflicts.append(maneuver) + return tuple(conflicts) + @attr.s(auto_attribs=True, kw_only=True, repr=False, eq=False) class Shoulder(_ContainsCenterline, LinearElement): diff --git a/src/scenic/formats/opendrive/xodr_parser.py b/src/scenic/formats/opendrive/xodr_parser.py index 8175d2c83..2df792e1f 100644 --- a/src/scenic/formats/opendrive/xodr_parser.py +++ b/src/scenic/formats/opendrive/xodr_parser.py @@ -10,13 +10,23 @@ import numpy as np from scipy.integrate import quad, solve_ivp from scipy.optimize import brentq -from shapely.geometry import GeometryCollection, MultiPoint, MultiPolygon, Point, Polygon -from shapely.ops import snap, unary_union +from scipy.spatial.transform import Rotation as R +from shapely.geometry import ( + GeometryCollection, + LineString, + MultiPoint, + MultiPolygon, + Point as ShapelyPoint, + Polygon, +) +from shapely.ops import snap, unary_union, split +import shapely from scenic.core.geometry import ( averageVectors, cleanChain, cleanPolygon, + normalizeAngle, plotPolygon, polygonUnion, removeHoles, @@ -105,6 +115,11 @@ def point_at(self, s): """Get an (x, y, s) point along the curve at the given s coordinate.""" return + @abc.abstractmethod + def heading_at(self, s): + """Get the heading along the curve at the given s coordinate.""" + return + def rel_to_abs(self, point): """Convert from relative coordinates of curve to absolute coordinates. I.e. rotate counterclockwise by self.hdg and translate by (x0, x1).""" @@ -146,6 +161,14 @@ def point_at(self, s): pt = (s, self.poly.eval_at(u), s) return self.rel_to_abs(pt) + def heading_at(self, s): + root_func = lambda x: self.arclength(x) - s + u = float(brentq(root_func, 0, self.ubound)) + dv_du = self.poly.grad_at(u) + local_heading = math.atan2(dv_du, 1) + global_heading = local_heading + self.hdg + return normalizeAngle(global_heading) + class ParamCubic(Curve): """A curve defined by the parametric equations @@ -173,6 +196,16 @@ def point_at(self, s): pt = (self.u_poly.eval_at(p), self.v_poly.eval_at(p), s) return self.rel_to_abs(pt) + def heading_at(self, s): + root_func = lambda x: self.arclength(x) - s + p = float(brentq(root_func, 0, self.p_range + 1e-9)) + p = min(p, self.p_range) + du_dp = self.u_poly.grad_at(p) + dv_dp = self.v_poly.grad_at(p) + local_heading = math.atan2(dv_dp, du_dp) + global_heading = local_heading + self.hdg + return normalizeAngle(global_heading) + class Clothoid(Curve): """An Euler spiral with curvature varying linearly between CURV0 and CURV1. @@ -215,6 +248,10 @@ def clothoid_ode(s, state): x, y, hdg = sol.y[:, -1] return (x, y, s) + def heading_at(self, s): + theta = (self.curv0 * s) + (0.5 * self.curve_rate * s * s) + self.hdg + return normalizeAngle(theta) + class Line(Curve): """A line segment between (x0, y0) and (x1, y1).""" @@ -228,6 +265,10 @@ def __init__(self, x0, y0, hdg, length): def point_at(self, s): return self.rel_to_abs((s, 0, s)) + def heading_at(self, s): + heading = self.hdg + return normalizeAngle(heading) + def makeCurve(x0, y0, hdg, length, curve_elem): """Create a reference-line curve from an OpenDRIVE geometry element. @@ -397,6 +438,7 @@ def __init__(self, name, id_, length, junction, drive_on_right=True): self.junction = junction if junction != "-1" else None self.predecessor = None self.successor = None + self.crosswalks = [] self.signals = [] # List of Signal objects. self.lane_secs = [] # List of LaneSection objects. self.ref_line = [] # List of Curve objects defining reference line. @@ -451,7 +493,7 @@ def get_ref_points(self, num): def heading_at(self, point): # Convert point to shapely Point. - point = Point(point.x, point.y) + point = ShapelyPoint(point.x, point.y) for i in range(len(self.lane_secs)): ref_points = self.sec_points[i] poly = self.sec_polys[i] @@ -464,7 +506,7 @@ def heading_at(self, point): assert lane_id is not None, "Point not found in sec_lane_polys." min_dist = float("inf") for i in range(len(ref_points)): - cur_point = Point(ref_points[i][0], ref_points[i][1]) + cur_point = ShapelyPoint(ref_points[i][0], ref_points[i][1]) if point.distance(cur_point) < min_dist: closest_idx = i if closest_idx >= len(ref_points) - 1: @@ -920,6 +962,26 @@ def hasGap(edge1, edge2): id_ = f"road{self.id_}_{name}{direction}" return id_, union, centerline, leftEdge, rightEdge + def makeCrosswalk(): + pedestrian_crossings = [] + for cw in self.crosswalks: + crossing = roadDomain.PedestrianCrossing( + id=cw.id_, + polygon=cw.polygon, + centerline=cw.centerline, + leftEdge=cw.leftEdge, + rightEdge=cw.rightEdge, + parent=None, + startSidewalk=None, + endSidewalk=None, + ) + pedestrian_crossings.append(crossing) + allElements.append(crossing) + + return pedestrian_crossings + + pedestrian_crossings = makeCrosswalk() + def makeSidewalk(sections, backward=False): sections = tuple(sections) if not any(sections): @@ -934,7 +996,7 @@ def makeSidewalk(sections, backward=False): leftEdge=leftEdge, rightEdge=rightEdge, road=None, - crossings=(), # TODO add crosswalks + crossings=(pedestrian_crossings), ) allElements.append(sidewalk) return sidewalk @@ -1191,7 +1253,7 @@ def getEdges(forward): backwardLanes=backwardGroup, sections=roadSections, signals=tuple(roadSignals), - crossings=(), # TODO add these! + crossings=tuple(pedestrian_crossings), ) allElements.append(road) @@ -1226,9 +1288,214 @@ def getEdges(forward): sec.group = backwardGroup sec.road = road del sec._original_lane + for crossing in pedestrian_crossings: + crossing.parent = road return road, allElements + def xyz_heading_at_s(self, s): + cumalative_curve_length = 0.0 + + for curve in self.ref_line: + if cumalative_curve_length + curve.length >= s - 1e-9: + local_s = s - cumalative_curve_length + x, y, z = curve.point_at(local_s) + heading = curve.heading_at(local_s) + return (x, y, z), heading + cumalative_curve_length = cumalative_curve_length + curve.length + + last_curve = self.ref_line[-1] + x, y, z = last_curve.point_at(last_curve.length) + heading = last_curve.heading_at(last_curve.length) + return (x, y, z), heading + + def st_to_xyz(self, s, t, zOffset): + (x_ref, y_ref, z_ref), heading = self.xyz_heading_at_s(s) + x = x_ref - t * math.sin(heading) + y = y_ref + t * math.cos(heading) + z = z_ref + zOffset + return (x, y, 0) + + def uv_to_xyz(self, s, t, zOffset, u, v, z_local, hdg, pitch, roll): + (x0, y0, z0) = self.st_to_xyz(s, t, zOffset) + (_, _, _), road_heading = self.xyz_heading_at_s(s) + yaw = hdg + road_heading + + r = R.from_euler('zyx', [yaw, pitch, roll], degrees=False) + local_vector = np.array([u, v, z_local]) + rotated_vector = r.apply(local_vector) + + x = x0 + rotated_vector[0] + y = y0 + rotated_vector[1] + z = z0 + rotated_vector[2] + return (x, y, z) + + def create_center_line(self, leftEdge, rightEdge): + leftPoints = list(leftEdge.lineString.coords) + rightPoints = list(rightEdge.lineString.coords) + + if len(leftPoints) == len(rightPoints): + centerPoints = list( + averageVectors(l, r) for l, r in zip(leftPoints, rightPoints) + ) + else: + num = max(len(leftPoints), len(rightPoints)) + centerPoints = [] + for d in np.linspace(0, 1, num): + l = leftEdge.lineString.interpolate(d, normalized=True) + r = rightEdge.lineString.interpolate(d, normalized=True) + centerPoints.append(averageVectors(l.coords[0], r.coords[0])) + centerline = PolylineRegion(cleanChain(centerPoints)) + return centerline + + def construct_crosswalk_polys(self, cw, tolerance): + points = [] + for outline in cw.outlines: + for corner_local in outline: + u = corner_local['u'] + v = corner_local['v'] + z_local = corner_local['z'] + + x, y, z = self.uv_to_xyz( + float(cw.s), float(cw.t), float(cw.zOffset), + u, v, z_local, + float(cw.hdg), float(cw.pitch), float(cw.roll), + ) + points.append((x, y)) + + crosswalk_polygon = cleanPolygon(Polygon(points), tolerance) + crosswalk_polygon_coords = list(crosswalk_polygon.exterior.coords[:-1]) + + mrr = list(crosswalk_polygon.minimum_rotated_rectangle.exterior.coords)[:-1] + edges = [] + lengths = [] + + for i in range(4): + a = mrr[i] + b = mrr[(i + 1) % 4] + edge = (a, b) + edges.append(edge) + + for (a, b) in edges: + dx = b[0] - a[0] + dy = b[1] - a[1] + distance = math.hypot(dx, dy) + lengths.append(distance) + + get_mrr_shortest_indices = np.argsort(lengths)[:2] + + mrr_shortest_edge = edges[get_mrr_shortest_indices[0]] + mrr_shortest_opp_edge = edges[get_mrr_shortest_indices[1]] + + def get_edge_midpoint(vertex1, vertex2): + return (np.asarray(vertex1) + np.asarray(vertex2)) / 2 + + m1 = get_edge_midpoint(mrr_shortest_edge[0], mrr_shortest_edge[1]) + m2 = get_edge_midpoint(mrr_shortest_opp_edge[0], mrr_shortest_opp_edge[1]) + + mrr_shortest_edge_distances = [] + mrr_shortest_opp_edge_distances = [] + + mrr_shortest_edge = LineString(mrr_shortest_edge) + mrr_shortest_opp_edge = LineString(mrr_shortest_opp_edge) + + for x, y in crosswalk_polygon_coords: + crosswalk_polygon_vertex = ShapelyPoint(x, y) + mrr_shortest_opp_edge_distances.append( + crosswalk_polygon_vertex.distance(mrr_shortest_opp_edge) + ) + mrr_shortest_edge_distances.append( + crosswalk_polygon_vertex.distance(mrr_shortest_edge) + ) + + bottom_cut_index = 0 + top_cut_index = 0 + final_bottom_score = float("inf") + final_top_score = float("inf") + + for i in range(len(crosswalk_polygon_coords)): + j = (i + 1) % len(crosswalk_polygon_coords) + + b_score = max( + mrr_shortest_opp_edge_distances[i], + mrr_shortest_opp_edge_distances[j], + ) + if b_score < final_bottom_score: + final_bottom_score = b_score + bottom_cut_index = i + + t_score = max( + mrr_shortest_edge_distances[i], + mrr_shortest_edge_distances[j], + ) + if t_score < final_top_score: + final_top_score = t_score + top_cut_index = i + + top_cut = ( + top_cut_index, + (top_cut_index + 1) % len(crosswalk_polygon_coords), + ) + bottom_cut = ( + bottom_cut_index, + (bottom_cut_index + 1) % len(crosswalk_polygon_coords), + ) + + def walk_polygon(n, bottom_cut, top_cut): + t0, t1 = top_cut + b0, b1 = bottom_cut + + left_indices = [t0] + i = t0 + while i != b1: + i = (i - 1 + n) % n + left_indices.append(i) + + right_indices = [t1] + i = t1 + while i != b0: + i = (i + 1) % n + right_indices.append(i) + + return left_indices, right_indices + + left_indices, right_indices = walk_polygon( + len(crosswalk_polygon_coords), bottom_cut, top_cut + ) + left_edge = [crosswalk_polygon_coords[i] for i in left_indices] + right_edge = [crosswalk_polygon_coords[i] for i in right_indices] + + leftEdge = PolylineRegion(cleanChain(left_edge)) + rightEdge = PolylineRegion(cleanChain(right_edge)) + centerLine = self.create_center_line(leftEdge, rightEdge) + + return crosswalk_polygon, centerLine, leftEdge, rightEdge + + +class Crosswalk: + def __init__(self, type_, subtype, id_, s, t, zOffset, orientation, + length, width, hdg, pitch, roll, outlines): + self.type_ = type_ + self.subtype = subtype + self.id_ = id_ + self.s = s + self.t = t + self.zOffset = zOffset + self.orientation = orientation + self.length = length + self.width = width + self.hdg = hdg + self.pitch = pitch + self.roll = roll + self.outlines = outlines + self.polygon = None + self.centerline = None + self.leftEdge = None + self.rightEdge = None + + def is_valid(self): + return self.length > 0 and self.width > 0 + class Signal: """Traffic lights, stop signs, etc.""" @@ -1307,6 +1574,7 @@ def calculate_geometry(self, num, calc_gap=False, calc_intersect=True): drivable_polys = [] sidewalk_polys = [] shoulder_polys = [] + crosswalk_polys = [] for road in self.roads.values(): drivable_poly = road.drivable_region sidewalk_poly = road.sidewalk_region @@ -1317,6 +1585,9 @@ def calculate_geometry(self, num, calc_gap=False, calc_intersect=True): sidewalk_polys.append(sidewalk_poly) if not (shoulder_poly is None or shoulder_poly.is_empty): shoulder_polys.append(shoulder_poly) + for cw in road.crosswalks: + if not (cw.polygon is None or cw.polygon.is_empty): + crosswalk_polys.append(cw.polygon) for link in self.road_links: road_a = self.roads[link.id_a] @@ -1368,10 +1639,12 @@ def calculate_geometry(self, num, calc_gap=False, calc_intersect=True): drivable_polys = [road.drivable_region for road in self.roads.values()] sidewalk_polys = [road.sidewalk_region for road in self.roads.values()] shoulder_polys = [road.shoulder_region for road in self.roads.values()] + crosswalk_polys = [cw.polygon for road in self.roads.values() for cw in road.crosswalks] self.drivable_region = buffer_union(drivable_polys, tolerance=self.tolerance) self.sidewalk_region = buffer_union(sidewalk_polys, tolerance=self.tolerance) self.shoulder_region = buffer_union(shoulder_polys, tolerance=self.tolerance) + self.crossing_region = buffer_union(crosswalk_polys, tolerance=self.tolerance) if calc_intersect: self.calculate_intersections() @@ -1392,7 +1665,7 @@ def calculate_intersections(self): def heading_at(self, point): """Return the road heading at point.""" # Convert point to shapely Point. - point = Point(point.x, point.y) + point = ShapelyPoint(point.x, point.y) for road in self.roads.values(): if point.within(road.drivable_region.buffer(1)): return road.heading_at(point) @@ -1454,6 +1727,45 @@ def __parse_link(self, link_elem, road, contact): RoadLink(road_id, c.connecting_id, contact, c.connecting_contact) ) + def __parse_crosswalk(self, crosswalk_elem): + cw = Crosswalk( + type_=crosswalk_elem.get("type"), + subtype=crosswalk_elem.get("subtype"), + id_=crosswalk_elem.get("id"), + s=float(crosswalk_elem.get("s", 0.0)), + t=float(crosswalk_elem.get("t", 0.0)), + zOffset=float(crosswalk_elem.get("zOffset", 0.0)), + orientation=crosswalk_elem.get("orientation"), + length=float(crosswalk_elem.get("length", 0.0)), + width=float(crosswalk_elem.get("width", 0.0)), + hdg=float(crosswalk_elem.get("hdg", 0.0)), + pitch=float(crosswalk_elem.get("pitch", 0.0)), + roll=float(crosswalk_elem.get("roll", 0.0)), + outlines=[], + ) + + for outline_elem in crosswalk_elem.iter("outline"): + corners = [] + for corner_elem in outline_elem.iter("cornerRoad"): + corners.append({ + "dz": float(corner_elem.get("dz", 0.0)), + "height": float(corner_elem.get("height", 0.0)), + "id": int(corner_elem.get("id", 0.0)), + "s": float(corner_elem.get("s", 0.0)), + "t": float(corner_elem.get("t", 0.0)), + }) + for corner_elem in outline_elem.iter("cornerLocal"): + corners.append({ + "height": float(corner_elem.get("height", 0.0)), + "id": int(corner_elem.get("id", 0.0)), + "u": float(corner_elem.get("u", 0.0)), + "v": float(corner_elem.get("v", 0.0)), + "z": float(corner_elem.get("z", 0.0)), + }) + cw.outlines.append(corners) + + return cw + def __parse_signal_validity(self, validity_elem): if validity_elem is None: return None @@ -1608,6 +1920,22 @@ def parse(self, path): assert refLine road.ref_line = refLine + # Parse crosswalks + objects_elem = r.find("objects") + if objects_elem is not None: + for obj in objects_elem.iter("object"): + if obj.get("type") == "crosswalk": + cw = self.__parse_crosswalk(obj) + road.crosswalks.append(cw) + + crosswalk_poly, centerLine, leftEdge, rightEdge = ( + road.construct_crosswalk_polys(cw, self.tolerance) + ) + cw.polygon = crosswalk_poly + cw.centerline = centerLine + cw.leftEdge = leftEdge + cw.rightEdge = rightEdge + # Parse lanes: lanes = r.find("lanes") for offset in lanes.iter("laneOffset"): @@ -1800,6 +2128,7 @@ def registerAll(elements): allRoads, seenRoads = [], set() allSignals, seenSignals = [], set() maneuversForLane = defaultdict(list) + junctionCrossings, seenConnectingRoads = [], set() for connection in junction.connections: incomingID = connection.incoming_id incomingRoad = mainRoads.get(incomingID) @@ -1811,6 +2140,10 @@ def registerAll(elements): if not connectingRoad: continue # connecting road has no drivable lanes; skip it + if connectingID not in seenConnectingRoads: + seenConnectingRoads.add(connectingID) + junctionCrossings.extend(connectingRoad.crossings) + for signal in connectingRoad.signals: if signal.openDriveID not in seenSignals: allSignals.append(signal) @@ -1933,12 +2266,14 @@ def cyclicOrder(elements, contactStart=None): outgoingLanes=cyclicOrder(allOutgoingLanes, contactStart=True), maneuvers=tuple(allManeuvers), signals=tuple(allSignals), - crossings=(), # TODO add these + crossings=tuple(junctionCrossings), ) register(intersection) intersections[jid] = intersection for maneuver in allManeuvers: object.__setattr__(maneuver, "intersection", intersection) + for crossing in junctionCrossings: + crossing.parent = intersection # Hook up road-intersection links for rid, oldRoad in self.roads.items(): @@ -1970,7 +2305,7 @@ def cyclicOrder(elements, contactStart=None): groups.append(road.backwardLanes) lanes = [lane for road in allRoads for lane in road.lanes] intersections = tuple(intersections.values()) - crossings = () # TODO add these + crossings = [cr for road in allRoads for cr in road.crossings] sidewalks, shoulders = [], [] for group in groups: sidewalk = group._sidewalk