Point Not on Ref Path in Sim to Frenet Transformation

[mchlswyr]

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 in sim_to_frenent_position(). toArcCoordinates() from lanelet2.geometry doesn't check for this. If the point is before the first point in ref_path, then toArcCoordinates() returns an s-value of 0. If the point is after the last point in ref_path, then toArcCoordinate() returns an s-value that is around the length of ref_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 in ref_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 on ref_path.

[mchlswyr]

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.