search

maliput / maliput_malidrive

Open-source ready OpenDrive backend for Maliput
BSD 3-Clause "New" or "Revised" License
1 stars 2 forks source link

Error when building piece-wise defined Road #88

Closed francocipollone closed 3 years ago

francocipollone commented 3 years ago

Based on this slack thread

This Road map (minimum xodr to reproduce the error):

<?xml version="1.0" encoding="UTF-8"?>
<OpenDRIVE>
    <header revMajor="1" revMinor="4" name="" version="1" >
    </header>
    <road name="Road 10" length="1.2647156278890400e+3" id="10" junction="-1">
        <link>
        </link>
        <type s="0.0000000000000000e+0" type="town">
            <speed max="40" unit="mph"/>
        </type>
        <planView>
            <geometry s="0.0000000000000000e+0" x="7.6309958458413951e+2" y="4.9557367776578508e+2" hdg="-2.2651204334664277e-2" length="1.2540883253939796e+0">
                <line/>
            </geometry>
            <geometry s="1.2540883253939796e+0" x="7.6435335120131481e+2" y="4.9554527358394057e+2" hdg="-2.2651204334664055e-2" length="3.8039251433908720e+2">
                <arc curvature="-4.0764360310436955e-3"/>
            </geometry>
            <geometry s="3.8164660266448112e+2" x="1.0041087646484375e+3" y="2.4968244361877441e+2" hdg="-1.5732969557258247e+0" length="2.3932158826954756e+2">
                <line/>
            </geometry>
            <geometry s="6.2096819093402883e+2" x="1.0035103107847186e+3" y="1.0361603605139493e+1" hdg="-1.5732969557258247e+0" length="2.1933872474175646e+0">
                <arc curvature="-2.0000000000000000e-3"/>
            </geometry>
            <geometry s="6.2316157818144620e+2" x="1.0035000150355584e+3" y="8.1682422806842965e+0" hdg="-1.5776837302206941e+0" length="2.5354179997683974e+2">
                <line/>
            </geometry>
            <geometry s="8.7670337815828611e+2" x="1.0017537841796875e+3" y="-2.4536754417419434e+2" hdg="-1.5776837302206939e+0" length="3.8801224973075387e+2">
                <arc curvature="-3.8937986821118678e-3"/>
            </geometry>
        </planView>
        <elevationProfile>
            <elevation s="0.0000000000000000e+0" a="0.0000000000000000e+0" b="0.0000000000000000e+0" c="0.0000000000000000e+0" d="0.0000000000000000e+0"/>
            <elevation s="3.2484315715881229e+2" a="0.0000000000000000e+0" b="0.0000000000000000e+0" c="0.0000000000000000e+0" d="0.0000000000000000e+0"/>
            <elevation s="3.5484315715881229e+2" a="0.0000000000000000e+0" b="0.0000000000000000e+0" c="0.0000000000000000e+0" d="0.0000000000000000e+0"/>
            <elevation s="9.1691315715881228e+2" a="0.0000000000000000e+0" b="0.0000000000000000e+0" c="0.0000000000000000e+0" d="0.0000000000000000e+0"/>
            <elevation s="9.4691315715881228e+2" a="0.0000000000000000e+0" b="0.0000000000000000e+0" c="0.0000000000000000e+0" d="0.0000000000000000e+0"/>
        </elevationProfile>
        <lateralProfile>
            <superelevation s="0.0000000000000000e+0" a="0.0000000000000000e+0" b="0.0000000000000000e+0" c="0.0000000000000000e+0" d="0.0000000000000000e+0"/>
        </lateralProfile>
        <lanes>
            <laneOffset s="0.0000000000000000e+0" a="0.0000000000000000e+0" b="0.0000000000000000e+0" c="0.0000000000000000e+0" d="0.0000000000000000e+0"/>
            <laneSection s="0.0000000000000000e+0">
                <left>
                    <lane id="1" type="driving" level="false">
                        <width sOffset="0.0000000000000000e+0" a="3.5000000000000000e+0" b="0.0000000000000000e+0" c="0.0000000000000000e+0" d="0.0000000000000000e+0"/>
                        <roadMark sOffset="0.0000000000000000e+0" type="none" material="standard" color="white" laneChange="none"/>
                        <userData>
                            <vectorLane sOffset="0.0000000000000000e+0" laneId="{3806bb58-1ce2-497f-a4eb-f8bc1d539fdf}" travelDir="backward"/>
                        </userData>
                    </lane>
                </left>
                <center>
                    <lane id="0" type="none" level="false">
                        <roadMark sOffset="0.0000000000000000e+0" type="none" material="standard" color="white" laneChange="none"/>
                    </lane>
                </center>
                <right>
                    <lane id="-1" type="driving" level="false">
                        <width sOffset="0.0000000000000000e+0" a="3.5000000000000000e+0" b="0.0000000000000000e+0" c="0.0000000000000000e+0" d="0.0000000000000000e+0"/>
                        <roadMark sOffset="0.0000000000000000e+0" type="none" material="standard" color="white" laneChange="none"/>
                        <userData>
                            <vectorLane sOffset="0.0000000000000000e+0" laneId="{8f48d9f7-c419-4b90-8bf1-d8a6da3ac878}" travelDir="forward"/>
                        </userData>
                    </lane>
                </right>
            </laneSection>
        </lanes>
    </road>
</OpenDRIVE>

is throwing when building lane -1(id) with:

[TRACE] Building Lane ID: 10_0_-1.
terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  piecewise_ground_curve.cc:GetGroundCurveFromP:87: p = 1264.715628 doesn't match with any GroundCurve interval.
Aborted (core dumped)

The query piecewise_ground_curve.cc:GetGroundCurveFromP:87: seems to be falling out of the length of the road (length="1.2647156278890400e+3" )

francocipollone commented 3 years ago

GDB backtrace:

(gdb) backtrace
#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:51
#1  0x00007ffff6748921 in __GI_abort () at abort.c:79
#2  0x00007ffff6d9d957 in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#3  0x00007ffff6da3ae6 in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#4  0x00007ffff6da3b21 in std::terminate() () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#5  0x00007ffff6da3d54 in __cxa_throw () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007ffff2f625c6 in malidrive::road_curve::PiecewiseGroundCurve::GetGroundCurveFromP(double) const ()
   from /home/franco/maliput_ws/install/maliput_malidrive/lib/libmaliput_malidrive_road_curve.so
#7  0x00007ffff2f62839 in malidrive::road_curve::PiecewiseGroundCurve::DoGDot(double) const ()
   from /home/franco/maliput_ws/install/maliput_malidrive/lib/libmaliput_malidrive_road_curve.so
#8  0x00007ffff2f67611 in malidrive::road_curve::RoadCurve::WDot(maliput::math::Vector3 const&, malidrive::road_curve::Function const*) const () from /home/franco/maliput_ws/install/maliput_malidrive/lib/libmaliput_malidrive_road_curve.so
#9  0x00007ffff2f6fec6 in std::_Function_handler<double (double const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&), malidrive::road_curve::(anonymous namespace)::ArcLengthDerivativeFunction>::_M_invoke(std::_Any_data const&, double const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&) () from /home/franco/maliput_ws/install/maliput_malidrive/lib/libmaliput_malidrive_road_curve.so
#10 0x00007ffff50fae0a in std::_Function_handler<Eigen::Matrix<double, -1, 1, 0, -1, 1> (double const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&), drake::systems::ScalarInitialValueProblem<double>::ScalarInitialValueProblem(std::function<double (double const&, double const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)> const&, drake::systems::ScalarInitialValueProblem<double>::ScalarOdeContext const&)::{lambda(double const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)#1}>::_M_invoke(std::_Any_data const&, double const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&) () from /opt/drake/0.27.0/lib/libdrake.so
#11 0x00007ffff50f866f in ?? () from /opt/drake/0.27.0/lib/libdrake.so
#12 0x00007ffff4665436 in drake::systems::CacheEntry::Calc(drake::systems::ContextBase const&, drake::AbstractValue*) const ()
   from /opt/drake/0.27.0/lib/libdrake.so
#13 0x00007ffff46a1b46 in drake::systems::CacheEntry::UpdateValue(drake::systems::ContextBase const&) const ()
   from /opt/drake/0.27.0/lib/libdrake.so
#14 0x00007ffff50789e9 in drake::systems::RungeKutta3Integrator<double>::DoStep(double const&) () from /opt/drake/0.27.0/lib/libdrake.so
#15 0x00007ffff475bf09 in drake::systems::IntegratorBase<double>::StepOnceErrorControlledAtMost(double const&) ()
   from /opt/drake/0.27.0/lib/libdrake.so
#16 0x00007ffff475c3dc in drake::systems::IntegratorBase<double>::IntegrateNoFurtherThanTime(double const&, double const&, double const&)
    () from /opt/drake/0.27.0/lib/libdrake.so
#17 0x00007ffff50f94e8 in drake::systems::InitialValueProblem<double>::Solve(double const&, drake::systems::InitialValueProblem<double>::OdeContext const&) const () from /opt/drake/0.27.0/lib/libdrake.so
#18 0x00007ffff2f70e71 in malidrive::road_curve::RoadCurveOffset::CalcSFromP(double) const ()
   from /home/franco/maliput_ws/install/maliput_malidrive/lib/libmaliput_malidrive_road_curve.so
#19 0x00007ffff2f715d5 in malidrive::road_curve::RoadCurveOffset::PFromS() const ()
   from /home/franco/maliput_ws/install/maliput_malidrive/lib/libmaliput_malidrive_road_curve.so
#20 0x00007ffff31a1bc5 in malidrive::Lane::Lane(maliput::api::TypeSpecificIdentifier<maliput::api::Lane> const&, int, int, maliput::api::HBounds const&, malidrive::road_curve::RoadCurve const*, std::unique_ptr<malidrive::road_curve::Function, std::default_delete<malidrive::road_curve::Function> >, std::unique_ptr<malidrive::road_curve::Function, std::default_delete<malidrive::road_curve::Function> >, double, double) () from /home/franco/maliput_ws/install/maliput_malidrive/lib/libmaliput_malidrive_base.so
#21 0x00007ffff62a80cb in malidrive::builder::RoadGeometryBuilder::BuildLane(malidrive::xodr::Lane const*, malidrive::xodr::RoadHeader const*, malidrive::xodr::LaneSection const*, int, malidrive::builder::RoadCurveFactoryBase const*, malidrive::Segment*, malidrive::road_curve::LaneOffset::AdjacentLaneFunctions*) () from /home/franco/maliput_ws/install/maliput_malidrive/lib/libmaliput_malidrive_builder.so
#22 0x00007ffff62a8529 in malidrive::builder::RoadGeometryBuilder::BuildLanesForSegment(malidrive::xodr::RoadHeader const*, malidrive::xodr::LaneSection const*, int, malidrive::builder::RoadCurveFactoryBase const*, bool, malidrive::RoadGeometry*, malidrive::Segment*) ()
   from /home/franco/maliput_ws/install/maliput_malidrive/lib/libmaliput_malidrive_builder.so
#23 0x00007ffff62a8a7f in malidrive::builder::RoadGeometryBuilder::LanesBuilderSequentialPolicy(malidrive::RoadGeometry*) ()
   from /home/franco/maliput_ws/install/maliput_malidrive/lib/libmaliput_malidrive_builder.so
#24 0x00007ffff62b0262 in malidrive::builder::RoadGeometryBuilder::FillSegmentsWithLanes(malidrive::RoadGeometry*) ()
   from /home/franco/maliput_ws/install/maliput_malidrive/lib/libmaliput_malidrive_builder.so
#25 0x00007ffff62b17c6 in malidrive::builder::RoadGeometryBuilder::DoBuild() ()
   from /home/franco/maliput_ws/install/maliput_malidrive/lib/libmaliput_malidrive_builder.so
#26 0x00007ffff62b2394 in malidrive::builder::RoadGeometryBuilder::operator()() ()
   from /home/franco/maliput_ws/install/maliput_malidrive/lib/libmaliput_malidrive_builder.so
#27 0x00007ffff62c2ab3 in malidrive::builder::RoadNetworkBuilder::operator()() const ()
   from /home/franco/maliput_ws/install/maliput_malidrive/lib/libmaliput_malidrive_builder.so
#28 0x00007ffff79a46f2 in std::unique_ptr<maliput::api::RoadNetwork, std::default_delete<maliput::api::RoadNetwork> > malidrive::loader::Load<malidrive::builder::RoadNetworkBuilder>(malidrive::builder::RoadNetworkConfiguration const&) ()
   from /home/franco/maliput_ws/install/maliput_integration/lib/libintegration.so
#29 0x00007ffff79a6363 in maliput::integration::CreateMalidriveRoadNetwork(maliput::integration::MalidriveBuildProperties const&) ()
francocipollone commented 3 years ago

It can't find the correct interval however from what I've checked the ranges are ok:

https://github.com/ToyotaResearchInstitute/maliput_malidrive/blob/31625b99c101ce2b09a4f0c60dc2a97b387efa62/maliput_malidrive/src/maliput_malidrive/road_curve/piecewise_ground_curve.cc#L84

Probably it is related to the operator< of RoadCurveInterval implementation: https://github.com/ToyotaResearchInstitute/maliput_malidrive/blob/31625b99c101ce2b09a4f0c60dc2a97b387efa62/maliput_malidrive/src/maliput_malidrive/road_curve/piecewise_ground_curve.cc#L159

I will continue debugging.

francocipollone commented 3 years ago 
---->>>>> CREATING THE GROUND CURVES.
[DEBUG] ground_curve_0: p0: 0                     | p1: 1.2540883253939796305 | delta_p: 1.2540883253939796305 | cumulativeArcLength:   1.2540883253939796305
[DEBUG] ground_curve_1: p0: 1.2540883253939796305 | p1: 381.64660266448117909 | delta_p: 380.39251433908719946 | cumulativeArcLength: 381.64660266448117909
[DEBUG] ground_curve_2: p0: 381.64660266448112225 | p1: 620.96819093402871204 | delta_p: 239.32158826954758979 | cumulativeArcLength: 620.96819093402871204
[DEBUG] ground_curve_3: p0: 620.96819093402882572 | p1: 623.16157818144642988 | delta_p: 2.1933872474176041578 | cumulativeArcLength: 623.16157818144631619
[DEBUG] ground_curve_4: p0: 623.16157818144620251 | p1: 876.70337815828588646 | delta_p: 253.54179997683968395 | cumulativeArcLength: 876.70337815828611383
[DEBUG] ground_curve_5: p0: 876.70337815828611383 | p1: 1264.715627889039979  | delta_p: 388.01224973075386515 | cumulativeArcLength: 1264.715627889039979

[DEBUG] --- Piecewise geo created ---
[DEBUG] --->p0: 0 | p1: 1264.715627889039979

---->>>>> ITERATING THE ROAD CURVE INTERVAL / GROUND CURVE MAP
[DEBUG] ---> interval_ground_curve_.size(): 6
[TRACE] -----> RoadCurveInterval min: 0                          | max: 1.254088325393979630462127. ground_curve p0: 0                          | p1: 1.254088325393979630462127
[TRACE] -----> RoadCurveInterval min: 1.254088325393979630462127 | max: 381.6466026644811790902168. ground_curve p0: 1.254088325393979630462127 | p1: 381.6466026644811790902168
[TRACE] -----> RoadCurveInterval min: 381.6466026644811790902168 | max: 620.9681909340288257226348. ground_curve p0: 381.6466026644811222467979 | p1: 620.9681909340287120357971
[TRACE] -----> RoadCurveInterval min: 620.9681909340288257226348 | max: 623.1615781814464298804523. ground_curve p0: 620.9681909340288257226348 | p1: 623.1615781814464298804523
[TRACE] -----> RoadCurveInterval min: 623.1615781814464298804523 | max: 876.7033781582861138303997. ground_curve p0: 623.1615781814462025067769 | p1: 876.7033781582858864567243
[TRACE] -----> RoadCurveInterval min: 876.7033781582861138303997 | max: 1264.7156278890399789816.   ground_curve p0: 876.7033781582861138303997 | p1: 1264.7156278890399789816

--->>>> GetGroundCurveFromP with p: 1264.7156278890399789816

[TRACE] [RoadCurve::WDot] p: 1264.7156278890399789816
[TRACE]     [DoGDot][GetGroundCurveFromP] p: 1264.7156278890399789816
[TRACE] [RoadCurveInterval::operator<] min: 1.254088325393979630462127 | max: 381.6466026644811790902168 |rhs.min: 1264.7156278890399789816 | rhs.max: 1264.7156278890399789816 
[TRACE]     [RoadCurveInterval::operator<] min < rhs.min
[TRACE]         [RoadCurveInterval::operator<] max <= rhs.max
                        Return true
[TRACE] [RoadCurveInterval::operator<] min: 620.9681909340288257226348 | max: 623.1615781814464298804523 |rhs.min: 1264.7156278890399789816 | rhs.max: 1264.7156278890399789816 
[TRACE]     [RoadCurveInterval::operator<] min < rhs.min
[TRACE]         [RoadCurveInterval::operator<] max <= rhs.max
                        Return true
[TRACE] [RoadCurveInterval::operator<] min: 623.1615781814464298804523 | max: 876.7033781582861138303997 |rhs.min: 1264.7156278890399789816 | rhs.max: 1264.7156278890399789816 
[TRACE]     [RoadCurveInterval::operator<] min < rhs.min
[TRACE]         [RoadCurveInterval::operator<] max <= rhs.max
                        Return true
[TRACE] [RoadCurveInterval::operator<] min: 876.7033781582861138303997 | max: 1264.7156278890399789816 |rhs.min: 1264.7156278890399789816 | rhs.max: 1264.7156278890399789816 
[TRACE]     [RoadCurveInterval::operator<] min < rhs.min
[TRACE]         [RoadCurveInterval::operator<] max <= rhs.max
                        Return true

There is numerical errors when doing the comparison in the RoadCurveInterval::operator< as the std::map tries to order it. Probably the implementation isn't completely correct or we should re-think if this collection suits this use.

francocipollone commented 3 years ago

There is numerical errors when doing the comparison in the RoadCurveInterval::operator< as the std::map tries to order it. Probably the implementation isn't completely correct or we should re-think if this collection suits this use.

FTR: @agalbachicar have reviewed this part and seems to be ok.

More updates

The p value used to find the the RoadCurveInterval that matches with a particular GroundCurve is filtered using an OpenRangeValidator instance.

validate_p_ = OpenRangeValidator(p0_, p1_, linear_tolerance_, kEpsilon);
...
p = validate_p_(p);
auto search_it = interval_ground_curve_.find(RoadCurveInterval(p));

Values used to create the OpenRangeValidator instance:

[DEBUG] --->p0: 0.00000000000000000000 | p1: 1264.71562788903997898160
[DEBUG] --->linear_tolerance: 0.05000000000000000278 | kEpsilon: 0.00000000000010000000

I printed both p values:

[TRACE]         [GetGroundCurveFromP]       raw_p: 1264.71562788903997898160
[TRACE]         [GetGroundCurveFromP] validated_p: 1264.71562788903997898160

So raw_p and validated_p seems to be equal to p1

Which is also interesting because:

double OpenRangeValidator::operator()(double s) const {
  MALIDRIVE_IS_IN_RANGE(s, min_ - tolerance_, max_ + tolerance_);
  return std::clamp(s, min_ + epsilon_, max_ - epsilon_);
}

so validated_p should be p1 - epsilon.

What's happening

This numerical error can be reproduced here: https://coliru.stacked-crooked.com/a/7ea0d3b075a007e0

As the number gets bigger (~1200 in this case) we can't continue to enforce calculation demanding 1-e13 precision. There is a limitation even for double's types. As the number increases, I will have fewer bits for the decimal part.

Info here Let's say that we have 16 significant digits (typically between 15 and 18) when using doubles, so the kEpsilon = 1e-13 makes sense only when the integer part is less than 1000, that's why it fails for this particular long roads.

Even more updates

The value of GroundCurve::kEpsilon is 1e-13.

Relaxing it to 1e-12 is enough to avoid this error. But what happens if we had a road with a length of 10000. Having a fixed epsilon probably isn't the best.

francocipollone commented 3 years ago

Relaxing it to 1e-12 is enough to avoid this error. But what happens if we had a road with a length of 10000. Having a fixed epsilon probably isn't the best.

Clearly, the epsilon used to validate the value of p cant be absolute and probably must be relative to the road's length.

We've discussed F2F with @agalbachicar that could be benefited of having the OpenRangeValidator::operator()(double s) reimplemented with something like:

double OpenRangeValidator::operator()(double s) const {
  MALIDRIVE_IS_IN_RANGE(s, min_ - tolerance_, max_ + tolerance_);
  const double range = max_ - min_;
  return std::clamp(s, min_  + range * epsilon_, max_ - range * epsilon_);
}

That way the epsilon value will be weighted by the value of the range.

Given that this is narrowed down to the correct selection of the epsilon value then I don't see completely accurate to change the behavior of OpenRangeValidator (which is "deterministic") but passing the correct epsilon to the OpenRangeValidator's constructor.

Therefore I've implemented here (check https://github.com/ToyotaResearchInstitute/maliput_malidrive/commit/590cc83bc63e0600f0d6f3e9714b2b1ab1282708) a possible solution to this: 1 - The GroundCurve::kEpsilon (1e-13) is weighted by the maximum value of the range. (e.g. Having a 10000m road will lead to a relative_epsilon of 1e-9. 2 - It doesn't affect the behavior of OpenRangeValidator class.

This solves the issue faced with this map.

francocipollone commented 3 years ago

wdyt @agalbachicar ?

agalbachicar commented 3 years ago

This solves the issue faced with this map.

I think it is equivalent to just reduce the kEpsilon value and stay the same.

Given that this is narrowed down to the correct selection of the epsilon value then I don't see completely accurate to change the behavior of OpenRangeValidator (which is "deterministic") but passing the correct epsilon to the OpenRangeValidator's constructor.

I disagree with this statement keeping pragmatic reasons aside. I suggest we give it a try. The change is relatively small and we have all the machinery in place to quickly try the results it yields with all the maps. Would you consider making a test with the proposed implementation?

francocipollone commented 3 years ago

Closing issue as #92 is merged.