Open mchlswyr opened 3 years ago
One problem with the suggested solution occurs when the global path is a loop. If the reference path is on one side of the global path, and the goal point is on the opposite side of the global path (and not on the reference path), then the closest point on the reference path to the goal point may be in the middle of the reference path. In this case, the projected vector will be on the path, and the goal point will be considered on the reference path. But the goal point is actually on the other side of the global path, and shouldn't be considered on the reference path.
We may have to take lane width into consideration when transforming certain objects.
Now that the planner's vehicle's reference path is being used to get the frenet position of other objects, we need to be able to detect when a point isn't on
ref_path
insim_to_frenent_position()
.toArcCoordinates()
fromlanelet2.geometry
doesn't check for this. If the point is before the first point inref_path
, thentoArcCoordinates()
returns an s-value of 0. If the point is after the last point inref_path
, thentoArcCoordinate()
returns an s-value that is around the length ofref_path
.For now, I've added a temporary fix: if the point is within a certain distance (5.0 meters by default) to at least one point in
ref_path
, then it is considered to be on the path. In the case of stop lines for traffic lights, this distance is set to 10.0 meters. This isn't a very robust solution.One possible solution is to not use
toArcCoordinates()
in the transformation. Then, we can search for the closest point inref_path
to the point we are transforming. After finding the closest point, we create two vectors: one from the point before the closest point to the closest point, and the other from the closest point to the point after it. Next, we create a vector from the closest point to the point we're transforming, and project this vector onto the other two vectors. If the projected vector lies on either of the two vectors, then we use that projected length to find the s-value and d-value. Otherwise, the point is not onref_path
.