Heading issue with twin waypoints

[Agustin Alba Chicar]

In RNDF implementation of Maliput, when you have two or more waypoints that have the same coordinates, you'll get an error of bad heading if you set the angular_tolerance to a small value (like 0.01 * M_PI). This is because there is a point in the same branch point which has a completely different heading to the other. This may be solved by reviewing the builder code. The original bug may be introduced there as the waypoints don't have any connection.

Here is a picture to explicitly show the case:

[Agustin Alba Chicar]

From discussion in https://github.com/RobotLocomotion/drake/issues/5804 we can infer that the BranchPoint creation should be done in terms of RNDF connection logic (entries and exits) and not in terms of geometry constraints. However, geometry constrains must be applied at a geometry level to give a valid geometry to Maliput. So, having road geometry as shown in the picture above is valid while there is no logical connection between the roads.

[Agustin Alba Chicar]

This was solved by refactoring all the drake::maliput::rndf::Builder class. Endpoints and all the other classes were removed in favor of just two wrappers: DirectedWaypoint and Connection. The first contains:

• The UniqueId that identifies a waypoint.
• The position of the waypoint.
• The tangent that should be applied to that waypoint.

In case of the Connection, it holds:

• The name of the connection like 'A_B_C-X_Y_Z'. Where A,B,C are segment, lane and waypoint of the first waypoint and X, Y, Z are the same but for the last waypoint.
• A collection of directed waypoints.

From it, right now we are just creating maliput::BranchPoints when there is connection on one waypoint. It should have more than one lane coming in or out, but the reference to the waypoint is important to keep track of it.

Given the samples that we already have, SimpleCity and Darpa, visualization and car driving is OK with the correct tolerances (in length and and angle).