crosswalk-parsing fixed PR - #478
Conversation
…ario, as well as pedestrian.scenic
…rianCrossing for conflicting maneuvers, updated crosswalk scenario to reflect this
| while (distance from self to ego) > threshold: | ||
| wait |
There was a problem hiding this comment.
| while (distance from self to ego) > threshold: | |
| wait | |
| wait until distance from self to ego <= threshold |
|
|
||
| lane_traj = [startLane, connectingLane, endLane] | ||
| intersection_edge = startLane.centerline[-1] | ||
| egoStartPoint = new OrientedPoint at intersection_edge |
There was a problem hiding this comment.
I don't think an orientation is used here (and you aren't setting one), so you might as well just use intersection_edge rather than egoStartPoint. Also the name egoStartPoint is confusing since the ego doesn't actually start there. I think using following roadDirection from intersection_edge for ... below would be clearer.
| 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) |
There was a problem hiding this comment.
| local_heading = math.atan2(dv_du, 1) | |
| local_heading = math.atan(dv_du) |
| id_ = f"road{self.id_}_{name}{direction}" | ||
| return id_, union, centerline, leftEdge, rightEdge | ||
|
|
||
| def makeCrosswalk(): |
There was a problem hiding this comment.
Since this is only called once, and creates all the crosswalks for this road (not just one), I think there's no need for a helper function. I would just inline it.
| rightEdge=rightEdge, | ||
| road=None, | ||
| crossings=(), # TODO add crosswalks | ||
| crossings=(pedestrian_crossings), |
There was a problem hiding this comment.
This should be a tuple.
Also, I think it's theoretically possible that you could have a crosswalk that connects to one of the two sidewalks but not the other, or a crosswalk connecting to no sidewalks. So it would be safer here to check that at least one of the endpoints of the crosswalk is close to the sidewalk (within the tolerance) before adding it.
| sections=roadSections, | ||
| signals=tuple(roadSignals), | ||
| crossings=(), # TODO add these! | ||
| crossings=tuple(pedestrian_crossings), |
There was a problem hiding this comment.
The documentation for Road.crossings says they are ordered from the start to the end of the road. If I understand correctly they aren't in any particular order in OpenDRIVE, so I think you'll need to sort them by s-coordinate (probably good to do the sorting right after the makeCrosswalk loop, so that they'll be sorted when assigned to the sidewalks too).
| (_, _, _), road_heading = self.xyz_heading_at_s(s) | ||
| yaw = hdg + road_heading | ||
|
|
||
| r = R.from_euler('zyx', [yaw, pitch, roll], degrees=False) |
There was a problem hiding this comment.
If I'm reading the spec correctly, these are supposed to be intrinsic rotations, i.e. you apply the yaw rotation first, then the pitch rotation is around the new Y axis, and finally the roll rotation is around the new X axis (vs. extrinsic rotations where you always rotate around the global coordinate axes). So I think you need to use ZYX rather than zyx as the axis specification here (see the from_euler docs).
It would be good to add a unit test for this function to make sure the output is as expected even in the presence of nonzero heading, pitch, and roll.
| 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 |
There was a problem hiding this comment.
I don't think it will be obvious to people what R is later on in the code. It's only used one time anyway, so I would just keep Rotation.
| z = z0 + rotated_vector[2] | ||
| return (x, y, z) | ||
|
|
||
| def create_center_line(self, leftEdge, rightEdge): |
There was a problem hiding this comment.
Presumably you meant to use this in combineSections, where this code came from? Also since it doesn't use self at all, I would make it a helper function rather than a method.
| ) | ||
| points.append((x, y)) | ||
|
|
||
| crosswalk_polygon = cleanPolygon(Polygon(points), tolerance) |
There was a problem hiding this comment.
We've had issues with cleanPolygon sometimes being too aggressive, so I'd suggest only applying it if the raw polygon is not valid (see how we use it now in calc_geometry_for_type).
| 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]] |
There was a problem hiding this comment.
This might be a non-opposite edge if the MRR is a square, I think. In that case we have a 50% chance of picking the wrong direction anyway, but we should still make sure we at least pick opposite edges. If shortest_index is the index of the shortest edge (breaking ties arbitrarily), I think you can just use (shortest_index + 2) % 4 as the other index.
| centerline = PolylineRegion(cleanChain(centerPoints)) | ||
| return centerline | ||
|
|
||
| def construct_crosswalk_polys(self, cw, tolerance): |
There was a problem hiding this comment.
I think it would help to add a comment about the overall approach here, since it won't be obvious for someone reading this code for the first time. It can just be a sentence or two explaining how you pick which edges are the "top" and "bottom" edges and split the chain at those two edges to yield left and right pieces.
| self.leftEdge = None | ||
| self.rightEdge = None | ||
|
|
||
| def is_valid(self): |
There was a problem hiding this comment.
This doesn't seem to get used anywhere. Also I'm not sure it's the right check, since the spec says the length and width fields are optional, and if they're missing you use a default value of zero. I think we only need them if there's no <outline>, in which case the object is rectangular and length and width give you the dimensions (unless radius is set, but presumably that doesn't happen for crosswalks).
| maneuvers=tuple(allManeuvers), | ||
| signals=tuple(allSignals), | ||
| crossings=(), # TODO add these | ||
| crossings=tuple(junctionCrossings), |
There was a problem hiding this comment.
These are supposed to be ordered by adjacency too, so I think you need to use something like cyclicOrder(junctionCrossings, contactStart=True).
| 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] |
There was a problem hiding this comment.
I think this works, because you're keeping the crossings of connecting roads listed in their crossings attribute even though the parent of the crossings will be the intersection. But in case that changes later (as it might make sense to list crossings under individual roads only if they are not part of an intersection), it might be safer to gather crossings from intersections too (making sure there are no duplicates).
dfremont
left a comment
dfremont
left a comment
There was a problem hiding this comment.
Looks good overall, thanks Aarav! I've pointed out a bunch of mostly-minor things above. The only other issue I can see is that there are no tests, but if necessary we could leave adding those to the next PR.
|
|
||
| for outline_elem in crosswalk_elem.iter("outline"): | ||
| corners = [] | ||
| for corner_elem in outline_elem.iter("cornerRoad"): |
There was a problem hiding this comment.
Doesn't look like construct_crosswalk_polys handles these: it assumes you only have cornerLocal elements.
Codecov Report❌ Patch coverage is
Additional details and impacted files@@ Coverage Diff @@
## main #478 +/- ##
==========================================
- Coverage 89.87% 89.69% -0.19%
==========================================
Files 48 48
Lines 13346 13568 +222
==========================================
+ Hits 11995 12170 +175
- Misses 1351 1398 +47
🚀 New features to boost your workflow:
|
4 participants
crosswalk parsing changes to xodr_parser and 1 scenario for pedestrian-vehicle interactions