Support for OpenDRIVE spiral (clothoid) curves

[vectorzeroinc]

CARLA currently crashes if the OpenDRIVE data contains spiral curves. In issue #1335, it is mentioned that these are unsupported because RoadRunner does not generate spirals.

The latest version of RoadRunner now supports all OpenDRIVE curve types (including spirals), so it would be awesome if support could be added to CARLA.

Here's a couple options for evaluating clothoid curves that have worked well for us:

• Sample code on the OpenDRIVE website
• G1fitting library

[marcgpuig]

Thanks for the samples @vectorzeroinc Do you guys know any efficient "nearest point to clothoid" function around?

[vectorzeroinc]

We don't have a specific library/algorithm for "nearest point to clothoid". Internally, we implement our point-to-curve functionality in a curve-independent manner.

Roughly, we use the following approach:

1. Adaptively tessellate the curve into a polyline based on a minimum error threshold. This is typically performed once per curve and cached (so it can be re-used for visualization, etc.)
2. Find the nearest location on the polyline. Each vertex on our polyline also stores the parameter (s-value) of the corresponding point on the curve, so we can linearly interpolate those parameters to get an initial guess of the s-value for the closest point on the curve.
3. If needed, we locally refine the guess (limiting our search range to the s-values of the nearby polyline points).

[vectorzeroinc]

It looks like CARLA also doesn't support the "paramPoly3" geometry type, which RoadRunner can now create as well. These are fairly typical cubic curves, and we're happy to answer any questions about our support for that type.

We have found that both the 'spiral' type and the 'paramPoly3' type are quite widely-used in existing industry data, so it would be quite beneficial if CARLA could support both.

Note that RoadRunner does not export the 'poly3' type (and likely never will). That geometry type is quite odd, and I have always considered it as obsolete with the introduction of paramPoly3.

By the way: You can test both curve types in RoadRunner using the new "explicit" road curve representation. Here's some documentation on that feature.

The OpenDRIVE samples also include examples of these geometry types. Eg:

• The Crossing8Course example (and others) includes spirals.
• The Atlatec example includes paramPoly3.

[MikeTroppmann]

Hey there,

I tried to implement such support for the last couple of days in the context of my bachelor-thesis. As i far as I understand the existing code, the DistanceTo() method in Geometry.cpp has to be implemented. What I don't get is why it returns a pair<float,float> and what distance is beeing calculated. My approach was kind of close to what @vectorzeroinc wrote. I filled a vector with points on the spiral ( using PosFromDist() ), then calculated the Distance and returned the shortest one.

A problem I had was, that the missing of laneoffsets in the .xodr-file lead to prohibited memory access. So i couldn't test my solution properly.

The result is me getting routePlanner-objects that contain NaN-values, which i haven't been able to trace until now.

I would appreciateit, if you could red through and maybe help me find the "error's root".

Kind regards, Mike

My code for DistanceTo() :

std::pair<float, float> GeometrySpiral::DistanceTo(const geom::Location &location) const
{
  const double step_size = 1;//stepsize in meter

  std::cout << "=====================DEBUG====================>Geometry.cpp Spiral::DistanceTo  I WAS HERE " << std::endl;

  //number of vertices by stepsize (s)
  u_long vertex_count =  static_cast<u_long>(_length/step_size);

  std::vector<std::pair<int, double>> verts;
  std::pair<int, double> shortest_distance;

  //port to poly-line (one vertex per meter)
  for (u_long s = 0; s <= vertex_count; s++)
  {
    //compute vertex array
    DirectedPoint point_on_spiral = PosFromDist(s);
    std::pair<u_long, double> push_back_pair;

    //compute distanceToPoint
    double Xs = point_on_spiral.location.x;
    double Ys = point_on_spiral.location.y;
    double Xp = location.x;
    double Yp = location.y;

    push_back_pair.first = s;
    push_back_pair.second = std::sqrt(std::pow(Xs - Xp, 2) + std::pow(Ys - Yp, 2));
    verts.push_back(push_back_pair);

    //store shortest;
    if (s==0)
    {
      shortest_distance.first = verts.at(s).first;
      shortest_distance.second = verts.at(s).second;
    }
    else
    {
      if(verts[s].second < shortest_distance.second)
      {
        shortest_distance.first = verts.at(s).first;
        shortest_distance.second = verts.at(s).second;
      }
    }

  }

  std::cout << "=====================DEBUG====================>Geometry.cpp Spiral::DistanceTo  shortest_distance  -> s: "<<shortest_distance.first<< " | dist: "<<shortest_distance.second << std::endl;

  return shortest_distance;
}

[marcgpuig]

Hi, @vectorzeroinc Awesome! Thanks for your valuable explanation and your references. We will focus on this in some time.

Also, hi @MikeTroppmann, Thanks for your interest in solving this :) It's true that we missed the docs on that one! You can find more about that std::pair<float, float> in Geometry.h on the equivalent DistanceTo(const geom::Location &p) functions for the other geometries. It returns a pair containing:

• First: distance to the nearest point in this line from the beginning of the shape (s OpenDRIVE value).
• Second: Euclidean distance from the nearest point in this geometry to the actual point.

Also, you could consider using our geom::Math::DistanceSegmentToPoint() to compute even a more accurate value!

I'm not sure what is your problem regarding the lane offsets, but I guess that is that we store all of them regarding the start of the road, not the lane segment (if I remember correctly). This is an optimization where we precompute the addition of all the cubic polynomials (see LibCarla/source/carla/geom/CubicPolynomial.h) using all the offsets that are affecting the road, so we can evaluate a single polynomial instead of the addition of all of them.

Hope this helps! Cheers!

[MikeTroppmann]

Hey there @marcgpuig and @vectorzeroinc !

After finding myself confronted with more and more problems on my way to implementing the features and compatibility i need, i had to suspend my work until geometry support is implemented in a standardized way. That leads me to this question: Are there plans or deadlines when ParamPoly3- and Clothoidsupport is going to be implemented?

Greetings, Mike

[Andrea2202]

Any update on the possibility to have ParamPoly3 or Spiral supported by Carla? Dates?

[kino3]

Are there any list of supported OpenDRIVE tags in Carla? I can only see that of OpenSCENARIO.

[ThrashAbaddon]

@kino3 Yes, OpenDrive manual explains all of them.

[vectorzeroinc]

Awesome! Our customers will be thrilled by this, and we're looking forward to trying it out!