XODR: wrong p value from integrators

[francocipollone]

The following XODR description forces the bug.

Click to see XODR

```xml

```

When loading this xodr using a linear tolerance equal to 0.1 the following error is thrown:

terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  open_range_validator.cc:operator():40: -0.122301 is less than -0.100000
Aborted (core dumped)

This happens in a range validation (OpenRangeValidator::operator()) when the value to be evaluated isn't within the range.

Where is this validation failure coming from? This failing validation comes from the call to LaneOffset::do_f(double p) method. In that particular case, a "invalid" p of -0.122301 is passed to that function and then the OpenRangeValidator throws.

Where is p value coming from? When the lane whose id is 5 is being built. The Lane's constructor creates the RoadCurveOffset object. During the RoadCurveOffset's constructor the RoadCurveOffset defines some functions to convert p_from_s and s_from_p

  p_from_s_ivp_ = std::make_unique<drake::systems::ScalarInitialValueProblem<double>>(
      InverseArcLengthODEFunction(road_curve_, lane_offset_, p0_, p1_), p_from_s_ivp_values);

The InverseArcLengthODEFunction functor is passed and the call to the LaneOffset::do_f() with the odd p is made in the operator() method of InverseArcLengthODEFunction.

Here printed are the p values that the functor receives. This p value is generated progressively across the lane by the drake integrators. For some reason at some point uses -0.122301 and of course the validator throws.

[TRACE] Building lane id 647_0_5
[InverseArcLengthODEFunction] p: 8.87015e-13
[InverseArcLengthODEFunction] p: 0.140559
[InverseArcLengthODEFunction] p: 0.280619
[InverseArcLengthODEFunction] p: 0.280868
[InverseArcLengthODEFunction] p: 0.280868
[InverseArcLengthODEFunction] p: 0.98242
[InverseArcLengthODEFunction] p: -0.122301
terminate called after throwing an instance of 'maliput::common::assertion_error'
  what():  open_range_validator.cc:operator():40: -0.122301 is less than -0.100000
Aborted (core dumped)

[francocipollone]

Just to be clearer.

The InverseArcLengthODEFunction functor is passed and the call to the LaneOffset::do_f() with the odd p is made in the operator() method of InverseArcLengthODEFunction. This p value is generated progressively across the lane by the drake integrators.

This is performed at PFromS() method is called when the DenseSolve is called. https://github.com/ToyotaResearchInstitute/maliput_malidrive/blob/105163ab6138709858c0ef08700ef62fc0948dbb/maliput_malidrive/src/maliput_malidrive/road_curve/road_curve_offset.cc#L226

[francocipollone]

The aforementioned XODR that reproduces this error describes the following Road composed by two geometries. The first one is a very short arc and the second one is a line.

After analyzing the setup of the integrators we decided(@agalbachicar ) to just clamp the negative value from p to p0 when this happens. These are very corner cases and the idea is to log a warning in those cases.