Closed francocipollone closed 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&) ()
It can't find the correct interval however from what I've checked the ranges are ok:
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.
---->>>>> 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.
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.
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.
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.
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.
wdyt @agalbachicar ?
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?
Closing issue as #92 is merged.
Based on this slack thread
This Road map (minimum xodr to reproduce the error):
is throwing when building lane
-1
(id) with:The query
piecewise_ground_curve.cc:GetGroundCurveFromP:87:
seems to be falling out of the length of the road (length="1.2647156278890400e+3" )