Several errors when loading some maps.

[francocipollone]

I decided to try some "real maps" without superelevation or variables lane offset or width, in order to verify the current state of the parser/builder system.

I found the following when running delphyne-gui's integration test:

• Town02: Failing in the contiguity checker when parsing geometries.
• Town03: Failing when checking Connections wihtin a Junciton(the xodr seems to be ok)
• Town04: Idem 03.
• Town06: Failing when using a method that get the connections out of a junction.(The xodr seems to be ok)

EDIT: 8/31/2020

In addition, the following maps should be analyzed:

• Town05
• Town07
• curved_intersection
• kpit_map
• Figure8
• RRFigure8
• RRLongRoad
• Highway
• onramp
• cloverleaf
• Leghorn

[agalbachicar]

I suspect some errors will be solved with ToyotaResearchInstitute/malidrive#519

[agalbachicar]

Town02: Failing in the contiguity checker when parsing geometries.

After some time debugging this map I could find the following.

• This map has laneOffsets different from zero which we don't currently support. Still, I disabled the check and move forward.
• I found that there is a gap in the plainView definition of Road 255. Below there is small piece of the map:

<road name="Road 255" length="1.3699796163349601e+1" id="255" junction="230">
    <link>
        <predecessor elementType="road" elementId="1" contactPoint="start"/>
        <successor elementType="road" elementId="4" contactPoint="start"/>
    </link>
    <planView>
        <geometry s="0.0000000000000000e+0" x="-5.4554205170595491e+0" y="-1.8232271194471792e+2" hdg="4.7126147647623515e+0" length="2.6462725233713513e-1">
            <line/>
        </geometry>
        <geometry s="2.6462725233713513e-1" x="-5.4553607683605749e+0" y="-1.8258733919030993e+2" hdg="4.7126147647623515e+0" length="3.7568376650554569e-2">
            <line/>
        </geometry>
        <geometry s="3.0219562898768970e-1" x="-5.4553522860081065e+0" y="-1.8262490756600286e+2" hdg="4.7126147647623515e+0" length="3.7626586383125016e-1">
            <line/>

which shows a gap of:

>>> 2.6462725233713513e-1 + 3.7568376650554569e-2 - 3.7568376650554569e-2
0.26462725233713513

• Trying the map with 0.3 of linear tolerance leads to:

[TRACE] Starting to build malidrive::RoadGeometry.
[TRACE] Visiting XODR Roads...
[TRACE] Visiting XODR Road ID: 262.
[TRACE] Visiting XODR LaneSection: 0 of Road: 262...
[TRACE] Built Junction ID: 230.
[TRACE] Built Segment ID: 262_0.
[TRACE] Visiting XODR Lanes from XODR LaneSection: 0 in XODR Road: 262...
[TRACE] Built Lane ID: 262_0_1.
[TRACE] Visiting XODR LaneSection: 1 of Road: 262...
[TRACE] Using Junction ID: 230.
terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  piecewise_ground_curve.cc:GetPiecewiseP:90: value: 13.275733 is greater than max_value: 13.163157

The exception is thrown in the ground curve and it is because the following situation:

• linear tolerance is bigger that one of the pieces in road 262
• piecewise ground curve stores all the pieces with a tolerance, which is the same as the RoadGeometry.
• to search the ground curve piece from a parameter coordinate it is necessary to find the ground curve mapping that coordinate but account for the possible case of non-continuous pieces. In this case, it is throwing because a previous piece is matched due to its short length + large linear tolerance value.

This points to an inconsistency in the map in general terms:

It is hard to both handle large gaps of non-contiguous piecewise definitions and account for very specific geometries below that tolerance values

[agalbachicar]

Town03: Failing when checking Connections wihtin a Junciton(the xodr seems to be ok)

Followed the suggestion in ToyotaResearchInstitute/malidrive#554 solved the connectivity issue. Then, I got another problem related to tolerances.

• Linear tolerance: 0.05
• Error while parsing Road 797 because at least one of its pieces is shorter than 0.05.

Moved linear_tolerance to 1e-3:

• Error in road 691 because one of its pieces is ~4.53e-6 long.

Moved linear tolerance to 1e-6:

• Error in road 847 because the third and the fourth piece are not G1 contiguous by 0.00010225422702234150511. Below piece of the XODR:

    ...
    <road name="Road 847" length="2.7137343386939342e+1" id="847" junction="831">
        <link>
            <predecessor elementType="road" elementId="40" contactPoint="end"/>
            <successor elementType="road" elementId="27" contactPoint="start"/>
        </link>
        <planView>
            <geometry s="0.0000000000000000e+0" x="-2.9495060439023684e+0" y="2.1172058868596395e+1" hdg="-3.0244101684689153e+0" length="1.1837467941303236e+1">
                <arc curvature="4.7729820065755491e-2"/>
            </geometry>
            <geometry s="1.1837467941303236e+1" x="-1.3709535085814219e+1" y="1.6626816830781628e+1" hdg="-2.4594099535963632e+0" length="2.1624942910285831e+0">
                <arc curvature="4.7729820065755491e-2"/>
            </geometry>
            <geometry s="1.3999962232331820e+1" x="-1.5314779947051854e+1" y="1.5179260150651565e+1" hdg="-2.3561944901923448e+0" length="3.3596505700909063e-1">
                <line/>
            </geometry>
            <geometry s="1.4335927289340910e+1" x="-1.5552416128868799e+1" y="1.4941623968834620e+1" hdg="-2.3561944901923448e+0" length="1.2801416097598434e+1">
                <arc curvature="-1.1012724258080094e-2"/>
            </geometry>
        </planView>
        ...

I suspect we will have more issues like this even if we improve the initial coordinate positions. Moving forward with the other maps.

[agalbachicar]

Town04: Idem 03.

I did not notice any issue when loading the map related to logical connectivity. However, using linear tolerance = 0.05 the build process failed with:

• Road 973, LaneSection 2

• Error in road_curve_offset.cc:255

Moved linear tolerance to be 0.001 --> same issue as before.

Moved linear tolerance to be 0.0001:

• Error while building the RoadCurve due to G1 constraint violation in PiecewiseGroundCurve --> Road 973, first and second piece, difference is: 0.00024559037831964434118 above 0.0001.

Similar error to the last one in Town03.xodr.

[agalbachicar]

Town05

Using linear_tolerance=0.05

[TRACE] Visiting XODR LaneSection: 1 of Road: 1275...
[TRACE] Using Junction ID: 1260.
terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  piecewise_ground_curve.cc:GetPiecewiseP:105: value: 24.350192 is greater than max_value: 24.330766
Aborted (core dumped)

Using linear_tolerance=0.001

[TRACE] Visiting XODR LaneSection: 1 of Road: 1244...
[TRACE] Using Junction ID: 1175.
terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  piecewise_ground_curve.cc:GetPiecewiseP:105: value: 0.001737 is greater than max_value: 0.000876
Aborted (core dumped)

Using linear_tolerance=0.0001

[TRACE] Visiting XODR Road ID: 1216.

LHS p: [0.0060446313727240408298; 0.012088787772441378365]
RHS p: [0.012088787772441378365; 0.21574994315689366431]
Linear: 0.00019775271949702005487
terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  Failure at /home/agalbachicar/Localwork/tri_ws/src/maliput_ws/src/malidrive/src/malidrive/road_curve/piecewise_ground_curve.cc:72 in PiecewiseGroundCurve(): condition 'contiguity_checker(previous_ground_curve, ground_curve.get())' failed.
Aborted (core dumped)

Using linear tolerance=0.0003

[TRACE] Built Lane ID: 1147_0_-1.
[TRACE] Visiting XODR Road ID: 1134.

LHS p: [14.884719275165604557; 15.006670576892492619]
RHS p: [15.006670576892492619; 15.083654651244433609]
Linear: 9.0990383848192537674e-05
terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  Failure at /home/agalbachicar/Localwork/tri_ws/src/maliput_ws/src/malidrive/src/malidrive/road_curve/piecewise_ground_curve.cc:72 in PiecewiseGroundCurve(): condition 'contiguity_checker(previous_ground_curve, ground_curve.get())' failed.
Aborted (core dumped)

Which means that Road 1134 is built with pieces that differ by more than 0.0003 + ~1e-4 ~ 0.0004 which leads to another trial with 0.0004:

[TRACE] Visiting XODR LaneSection: 4 of Road: 1127...
[TRACE] Using Junction ID: 1070.
terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  piecewise_ground_curve.cc:GetPiecewiseP:105: value: 24.023135 is greater than max_value: 24.023010
Aborted (core dumped)

So, the investigation leads to having two roads that have inconsistent tolerances.

[agalbachicar]

Town06: Failing when using a method that get the connections out of a junction.(The xodr seems to be ok)

The map is not loaded because of the following error:

terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  db_manager.cc:VerifyLinkingIsReciprocal:235: JunctionId: 208 is not equal to JunctionId: -1 from RoadHeaderId: 35
Aborted (core dumped)

By inspecting in detail the Road and Junction information:

• Road
  • ID: 3 --> connects to Road 210, does not belong to any Junction.
  • ID: 34 --> connects to Road 213, does not belong to any Junction.
  • ID: 210 --> is connected by Road 3 and connects Road 35. It belongs to Junction 208. Holds the connection information to connect with Road 35 and 3.
  • ID: 213 --> is connected by Road 35 and connects Road 35. It belongs to Junction 208. Holds the connection information to connect with Road 35 and 34.
  • ID: 35 --> is connected by Junction 208. Does not hold any information about the predecessor.
• Junction
  • ID: 208 --> describes connections for Road 3 with Road 210 and Road 34 with Road 213 only.

It might happen that the map is incomplete at the Junction level but it is not based on OpenDRIVE specification (see section 3.6.2.1 Basics):

Junctions consist of a connection matrix which indicates all possibilities to enter a connecting road
from a given in-coming road. These connections are listed on a per-lane basis in order to facilitate
navigation. Once a connecting road is entered, the following connection to the corresponding outgoing road can be retrieved from the general successor/predecessor information that is stored with
each road. Within the junctions, priorities of roads relative to each other may be stored but they may
also be retrieved by evaluating the signs/signals and geometry

There is a bug in the code now. Because we are throwing in DBManager::Impl::VerifyLinkingIsReciprocal() but the provided information is enough.

@francocipollone would you review this case to assert my thoughts?

RE: after debugging with @francocipollone , we found two fixes to the map that can be solved by applying the following patch:

git diff resources/Town06.xodr
diff --git a/resources/Town06.xodr b/resources/Town06.xodr
index e48d487..a5199d6 100644
--- a/resources/Town06.xodr
+++ b/resources/Town06.xodr
@@ -4103,7 +4103,7 @@
     </road>
     <road name="Road 36" length="1.0414560960727385e+2" id="36" junction="-1">
         <link>
-            <predecessor elementType="junction" elementId="208"/>
+            <predecessor elementType="road" elementId="35" contactPoint="end" />
             <successor elementType="junction" elementId="196"/>
         </link>
         <type s="0.0000000000000000e+0" type="town">
@@ -6166,7 +6166,7 @@
     </road>
     <road name="Road 55" length="1.9142457879714328e+2" id="55" junction="-1">
         <link>
-            <predecessor elementType="junction" elementId="704"/>
+            <predecessor elementType="road" elementId="54" contactPoint="end" />
             <successor elementType="road" elementId="56" contactPoint="start"/>
         </link>
         <type s="0.0000000000000000e+0" type="town">

Note this will lead to errors in tolerances like the others.

[agalbachicar]

Town07.xodr

Similarly to the previous maps, Road 788 is copied below

...
    <road name="Road 788" length="1.2000191935776883e+1" id="788" junction="780">
        <link>
            <predecessor elementType="road" elementId="29" contactPoint="start"/>
            <successor elementType="road" elementId="28" contactPoint="end"/>
        </link>
        <planView>
            <geometry s="0.0000000000000000e+0" x="-5.1894463125456937e+1" y="1.0863601930174571e+0" hdg="9.7477415163771397e-3" length="6.0000958530241348e+0">
                <line/>
            </geometry>
            <geometry s="6.0000958530241348e+0" x="-4.5894652330123556e+1" y="1.1448466502403942e+0" hdg="9.7477415163771397e-3" length="7.0737371515861014e-8">
                <line/>
            </geometry>
            <geometry s="6.0000959237615064e+0" x="-4.5894652259389559e+1" y="1.1448466509299129e+0" hdg="9.7477415163771397e-3" length="3.5368685757930507e-8">
                <line/>
            </geometry>
            <geometry s="6.0000959591301921e+0" x="-4.5894652224022543e+1" y="1.1448466512746722e+0" hdg="9.7477415163771397e-3" length="1.2008642897853861e-6">
                <line/>
            </geometry>
            <geometry s="6.0000971599944819e+0" x="-4.5894651023215310e+1" y="1.1448466629802014e+0" hdg="9.7477415163771397e-3" length="6.0000947757824008e+0">
                <line/>
            </geometry>
        </planView>
...

As you can see, pieces in the middle are very short with respect to the others which makes this map to require very small tolerance to be loaded (~1e-8) but then it cannot follow that tolerance:

[TRACE] Visiting XODR Road ID: 742.

LHS p: [2.4551639748452913636; 2.5040923420464764604]
RHS p: [2.5040923420464764604; 2.587223947777554045]
Linear: 0.00027601101370801253265
terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  Failure at /home/agalbachicar/Localwork/tri_ws/src/maliput_ws/src/malidrive/src/malidrive/road_curve/piecewise_ground_curve.cc:72 in PiecewiseGroundCurve(): condition 'contiguity_checker(previous_ground_curve, ground_curve.get())' failed.
Aborted (core dumped)

[agalbachicar]

Figure8.xodr

• Loaded with linear_tolerance=0.05

[TRACE] Visiting XODR LaneSection: 1 of Road: 21...
[TRACE] Using Junction ID: 10.
terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  piecewise_ground_curve.cc:GetPiecewiseP:105: value: 10.766533 is greater than max_value: 10.766533
Aborted (core dumped)

This is generally because the tolerance is too loose for some internal pieces that are part of a road (the first piece is 4.5996124512612369e-2 long).

• Loaded with decreasing tolerances until I got a different error, linear tolerance = 0.0001

[TRACE] Visiting XODR Road ID: 21.

LHS p: [0; 0.045996124512612368562]
RHS p: [0.045996124512612368562; 0.1724825213342011665]
Linear: 0.00016235658100554494669
terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  Failure at /home/agalbachicar/Localwork/tri_ws/src/maliput_ws/src/malidrive/src/malidrive/road_curve/piecewise_ground_curve.cc:72 in PiecewiseGroundCurve(): condition 'contiguity_checker(previous_ground_curve, ground_curve.get())' failed.
Aborted (core dumped)

Same error as before regarding tolerances in internal connections of Road 21.

[francocipollone]

curved_intersection: Fail during range verification within CubicPolynomial::GetPiecewiseP().

Error: malidrive/road_curve/piecewise_ground_curve.cc:90 in GetPiecewiseP(): condition 'p_i >= ground_curve->p0() && p_i <= ground_curve->p1()' failed.

• Linear Tolerance: 5e-2 Apparently the p_i value is greater than p1.

[DEBUG] [PiecewiseGroundCurve]GetPiecewiseP: 
     p0:12.5582  -- p_i:22.1172 -- p1:22.1064

It happens on Road 29 and Road 36, where the geometry is piecewise-defined. And the length of the geometries is very small in comparison with the tolerances.

• Decreasing tolerance to 1e-2 solves the issue.

• Decreasing tolerance under 1e-4 leads to Contiguity error:

  
  what():  Failure at /home/franco/Worskpace/RoadNetwork/tri_ws/src/malidrive/src/malidrive/road_curve/piecewise_ground_curve.cc:57 in PiecewiseGroundCurve(): condition 'contiguity_checker(previous_ground_curve, ground_curve.get())' failed.
  Aborted (core dumped)

[francocipollone]

kpit_map: Fail during range verification within CubicPolynomial::GetPiecewiseP().

Error:

• Linear Tolerance: 5e-2

  [TRACE] Visiting XODR LaneSection: 0 of Road: 71...
  [TRACE] Built Junction ID: 68.
  terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  Failure at /home/franco/Worskpace/RoadNetwork/tri_ws/src/malidrive/src/malidrive/road_curve/piecewise_ground_curve.cc:94 in GetPiecewiseP(): condition 'p_i >= ground_curve->p0() && p_i <= ground_curve->p1()' failed.

• Decreasing Tolerance to 1e-4 sorted out previous error but it happened to throw with a contiguity error

TRACE] Visiting XODR Road ID: 62.
ArcGroundCurve constructor
ArcGroundCurve constructor
terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  Failure at /home/franco/Worskpace/RoadNetwork/tri_ws/src/malidrive/src/malidrive/road_curve/piecewise_ground_curve.cc:57 in PiecewiseGroundCurve(): condition 'contiguity_checker(previous_ground_curve, ground_curve.get())' failed.
Aborted (core dumped)

Check the planView of the XODR description (RoadId 62): The first 4 Geometries are split without any apparent reason. They are lines. And their length is in the order of 1e-2

        <planView>
            <geometry s="0.0000000000000000e+0" x="-1.6462989741564112e+1" y="1.0377680159820743e+2" hdg="1.2417116900980130e-3" length="5.5483106189598175e-2">
                <line/>
            </geometry>
            <geometry s="5.5483106189598175e-2" x="-1.6407506678147765e+1" y="1.0377687049221129e+2" hdg="1.2417116900980130e-3" length="1.1096420836398124e-1">
                <line/>
            </geometry>
            <geometry s="1.6644731455357942e-1" x="-1.6296542555328742e+1" y="1.0377700827773059e+2" hdg="1.2417116900980130e-3" length="5.5483097818068927e-2">
                <line/>
            </geometry>
            <geometry s="2.2193041237164834e-1" x="-1.6240557288794754e+1" y="1.0377707779532624e+2" hdg="1.2417116900671488e-3" length="1.4254674836662495e-1">
                <line/>
            </geometry>
            <geometry s="3.6447716073827330e-1" x="-1.6098010650320820e+1" y="1.0377725479724458e+2" hdg="1.2417116901000114e-3" length="6.3975016681169343e+0">
                <arc curvature="1.2740169111964933e-1"/>
            </geometry>
            <geometry s="6.7619786293967206e+0" x="-1.0388738257122654e+1" y="1.0625032451001613e+2" hdg="8.1629424314897392e-1" length="6.4517142005797767e+0">
                <arc curvature="1.1705070439275739e-1"/>
            </geometry>
            <geometry s="1.3213692653344436e+1" x="-8.0701951619947092e+0" y="1.1210762863296321e+2" hdg="1.5714719348675894e+0" length="1.4286901218112505e-1">
                <line/>
            </geometry>
        </planView>

[agalbachicar]

RRFigure8.xodr

Using linear tolerances in the range [1e-13, 1e-2] will all fail with:

[TRACE] Built Lane ID: 95_2_1.
[TRACE] Visiting XODR LaneSection: 3 of Road: 95...
[TRACE] Using Junction ID: 10.
terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  piecewise_ground_curve.cc:GetPiecewiseP:105: value: 15.856232 is greater than max_value: 15.856232
Aborted (core dumped)

This in an interesting case because Road 95 is composed of a list of lines that share the same heading.

[agalbachicar]

RRLongRoad.xodr

After disabling the constraint to have only one width description per lane I was able to load the map. It worked with tolerances in the range of 1e-2 to 1e-4.

[francocipollone]

Highway

• Works fine (After disabling the constraint to have only one width description and a constant lane offset per lane )

• If LinearTolerance is 1e-4 or lower then:

  terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  Failure at /home/franco/Worskpace/RoadNetwork/tri_ws/src/malidrive/src/malidrive/road_curve/piecewise_ground_curve.cc:57 in PiecewiseGroundCurve(): condition 'contiguity_checker(previous_ground_curve, ground_curve.get())' failed.
  Aborted (core dumped)

[francocipollone]

onramp

• 5e-2 > linear tolerance > 1e-5 Error:

  [TRACE] Visiting XODR LaneSection: 2 of Road: 165...
  [TRACE] Using Junction ID: 164.
  terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  Failure at /home/franco/Worskpace/RoadNetwork/tri_ws/src/malidrive/src/malidrive/road_curve/piecewise_ground_curve.cc:94 in GetPiecewiseP(): condition 'p_i >= ground_curve->p0() && p_i <= ground_curve->p1()' failed.
  Aborted (core dumped)

Changing tolerance doesn't vary the outcome. (After 1e-5 a Contiguity checker error at Parser Level arises):

• linear tolerance < 1e-5

  terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  Failure at /home/franco/Worskpace/RoadNetwork/tri_ws/src/malidrive/src/malidrive/xodr/parser.cc:674 in As<malidrive::xodr::PlanView>(): condition '"Geometries doesn't meet contiguity constraint."' failed.
  Aborted (core dumped)

[agalbachicar]

cloverleaf.xodr

There is a problem when loading because of the VerifyRoadLinks condition:

terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  db_manager.cc:VerifyLinkingIsReciprocal:235: JunctionId: 365 is not equal to JunctionId: -1 from RoadHeaderId: 29
Aborted (core dumped)

I manually inspected the Junction 365 and the involved roads. It seems to be a similar case to https://github.com/ToyotaResearchInstitute/malidrive/issues/517#issuecomment-683940687 but it has more information in this case the Junction.

[agalbachicar]

Up to know, I can identify the following follow up actions:

• Convert the multiple width and multiple lane offset into a warn log instead of an assertion. The build would do a a best try at loading the map.
• Create a method in malidrive::xodr::DBManager to retrieve the smallest and biggest plainView piece length in all XODR Roads.
• Create application for XODR validation and introspection.
• Split tolerance used within xodr parsing and RoadGeometry construction process.

[francocipollone]

Leghorn

Leghorn should be analyzed after updating Leghorn to the new version. Because there are some junctions wrongly described. ToyotaResearchInstitute/malidrive#515

[francocipollone]

Just to confirm what we supposed. Here there is the outcome of the last queries added to the xodr_query app applied to Town03 map: Geometry Length:

Shortest Geometry in the XODR: 1.93096e-07
Located at RoadHaderId: 160, Geometry Index: 5

Gap between Geometries:

Largest Gap between Geometries in the XODR: 0.00110894
Located at RoadHaderId: 183, Geometry Indexes: [3,4]

Clearly, it supports the goal: Split tolerance used within xodr parsing and RoadGeometry construction process.

[agalbachicar]

Update coming from ToyotaResearchInstitute/maliput_malidrive#9

All Town maps have been integrated. The following maps are the next targets:

• onramp
• cloverleaf
• Leghorn

[agalbachicar]

77 improves the automatic loading process.

There are still some TODOs that will improve code quality and consistency but those would not block an integration because are implementation details and don't affect the API.

[agalbachicar]

Closing this ticket because all maps loaded now. See #82 for the TODOs described in https://github.com/ToyotaResearchInstitute/maliput_malidrive/issues/12#issuecomment-852289713