35 template <
int PolyFitPo
ints,
int PolyEvaluatePo
ints>
76 double Sbuf[PolyEvaluatePoints];
77 double Xbuf[PolyEvaluatePoints];
78 double Ybuf[PolyEvaluatePoints];
79 double nXbuf[PolyEvaluatePoints];
80 double nYbuf[PolyEvaluatePoints];
81 double dXbuf[PolyEvaluatePoints];
82 double dYbuf[PolyEvaluatePoints];
128 double lookahead_distance,
129 double lookbehind_distance,
130 double smoothness = 0.05,
131 bool activate_navigation =
false
136 if(start==
nullptr)
return;
138 const double excess_distance_border = 10.0;
139 borderTrace->
setDistanceLimit(lookbehind_distance + excess_distance_border);
144 double walkback_distance = s0_border;
147 for (
auto it = borderTrace->
rbegin(); it != borderTrace->
rend() && !(it->first == start->
m_id); it++)
149 auto current = borderSet->
getBorder(it->first);
154 walkback_distance = s0_border;
159 walkback_distance += it->second;
164 if(activate_navigation)
171 BASFollowStraight bas(start, borderSet, lookahead_distance + s0_border + excess_distance_border);
184 adoreMatrix<double, 0, 0> center_data;
191 double review_distance =
std::min(walkback_distance, lookbehind_distance);
192 double preview_distance = lookahead_distance;
200 for (
int i = 0; i < PolyFitPoints; i++)
209 csaps::DoubleArray sdata(PolyFitPoints);
210 csaps::DoubleArray xdata(PolyFitPoints);
211 csaps::DoubleArray ydata(PolyFitPoints);
218 for (
int i = 0; i < PolyFitPoints; ++i)
225 csaps::UnivariateCubicSmoothingSpline splineX(sdata, xdata, smoothness);
226 csaps::UnivariateCubicSmoothingSpline splineY(sdata, ydata, smoothness);
230 csaps::DoubleArray xidata;
231 csaps::DoubleArray dxidata;
232 csaps::DoubleArray ddxidata;
233 csaps::DoubleArray dddxidata;
234 csaps::DoubleArray yidata;
235 csaps::DoubleArray dyidata;
236 csaps::DoubleArray ddyidata;
237 csaps::DoubleArray dddyidata;
239 std::tie(xidata,dxidata,ddxidata,dddxidata) = splineX(PolyEvaluatePoints, sdata);
240 std::tie(yidata,dyidata,ddyidata,dddyidata) = splineY(PolyEvaluatePoints, sdata);
243 for (
int i = 0; i < PolyEvaluatePoints; ++i)
246 dXbuf[i] = dxidata(i);
250 dYbuf[i] = dyidata(i);
258 for (
int i = 0; i < PolyEvaluatePoints; i++)
262 if (i + 1 < PolyEvaluatePoints)
266 Li = (std::sqrt)(dx * dx + dy * dy);
305 if (i + 1 < PolyEvaluatePoints)
318 double max_intersect_distance = 5.0;
319 bool extend_fringes =
false;
325 for (
int i = 0; i < PolyEvaluatePoints; i++)
328 distance, max_intersect_distance, extend_fringes);
336 for (
int i = 0; i < PolyEvaluatePoints; i++)
339 s_intersect, distance, max_intersect_distance, extend_fringes);
344 bool width_open =
false;
348 for (
int i = 0; i <
left.nc(); i++)
376 if(activate_navigation)
378 double d_sample = 5.0;
379 std::vector<adoreMatrix<double,3,1>> navcost_vector;
382 auto c = borderCostMap->find(rb->m_id);
383 if(c==borderCostMap->end())
return false;
384 adoreMatrix<double,3,1> value;
385 value(0) = c->second.getDistanceToGoal();
386 value(1) = rb->m_id.m_first.m_X;
387 value(2) = rb->m_id.m_first.m_Y;
388 navcost_vector.push_back(value);
389 if(rb->m_path!=
nullptr && rb->getLength()>d_sample)
391 for(
double d = d_sample;d<rb->getLength();d+=d_sample)
393 auto p = rb->m_path->f(d);
394 value(0) = c->second.getDistanceToGoal() + d;
397 navcost_vector.push_back(value);
403 auto c = borderCostMap->find(rb->
m_id);
404 if(c==borderCostMap->end())
return false;
405 adoreMatrix<double,3,1> value;
409 navcost_vector.push_back(value);
412 for(
double d = d_sample;d<rb->
getLength();d+=d_sample)
415 value(0) = c->second.getDistanceToGoal() + d;
418 navcost_vector.push_back(value);
423 std::vector<double> svalues;
424 std::vector<double> cvalues;
427 for(
auto& value:navcost_vector)
434 double dx = value(1)-xstart(0);
435 double dy = value(2)-xstart(1);
436 double d = std::sqrt(dx*dx+dy*dy);
437 if(d>1.5*d_sample)
continue;
442 double dx = value(1)-xend(0);
443 double dy = value(2)-xend(1);
444 double d = std::sqrt(dx*dx+dy*dy);
445 if(d>1.5*d_sample)
continue;
452 if( svalues.size()>0 && s<svalues[svalues.size()-1])
continue;
453 else if( svalues.size()>0 && s==svalues[svalues.size()-1])
455 if(c<cvalues[cvalues.size()-1])
457 svalues[svalues.size()-1] = s;
458 cvalues[svalues.size()-1] = c;
463 svalues.push_back(s);
464 cvalues.push_back(c);
470 for(
int i=0;i<svalues.size();i++)
509 void excludeObstaclePoint(
double X,
double Y,
double s_min,
double s_safety,
double n_safety,
double s_off = 0.0)
512 this->toLocalCoordinates(X, Y, s, n);
585 if((*it_right)->isPointInsideLane(*it_left,Xc,Yc))
678 else if(derivative<=1)
701 BAContainer::iterator
end()
This class chooses the successor with the lowest cost until an upper limit on distance is reached.
Definition: borderaccumulator.h:136
This class chooses the straightest successor of a border until an upper limit on distance is reached.
Definition: borderaccumulator.h:78
This class choses the left/right neighbors of a border sequence.
Definition: borderaccumulator.h:231
@ LEFT
Definition: borderaccumulator.h:242
This class collects a sequence of borders, according to chosen BorderAccumulationStrategy.
Definition: borderaccumulator.h:293
void append(Border *current, bool inverted)
**
Definition: borderaccumulator.h:353
void clear()
Clear the BorderAccumulator.
Definition: borderaccumulator.h:312
BAContainer * getBorders()
Get the Accumulated Borders.
Definition: borderaccumulator.h:386
void defineFunction(function_type &f)
Create (with new) a function, which contains all accumulated border paths.
Definition: borderaccumulator.h:395
Definition: bordercostmap.h:31
efficiently store borders in boost R-tree
Definition: borderset.h:99
Border * getBorder(const BorderID &id) const
retrieve a border by ID
Definition: borderset.h:628
Definition: bordertrace.h:30
CTYPE::reverse_iterator rend()
Definition: bordertrace.h:70
CTYPE::reverse_iterator rbegin()
Definition: bordertrace.h:69
void setDistanceLimit(double distance_limit)
Definition: bordertrace.h:41
A class with a geometry description of the current lane.
Definition: lanefollowinggeometry.h:41
double m_s_lane_width_open
Definition: lanefollowinggeometry.h:72
void toRelativeCoordinates(double Xg, double Yg, double &s, double &n)
Transform from euclidian to relative coordinates.
Definition: lanefollowinggeometry.h:770
function_type_scalar m_navigationCost_fct
Definition: lanefollowinggeometry.h:61
double getOffsetOfLeftBorder(double s)
Get the offset of the left border at a certain position.
Definition: lanefollowinggeometry.h:722
double getViewingDistance() const
Get the viewing distance.
Definition: lanefollowinggeometry.h:615
double getProgressOfWidthOpen() const
Get the s-coordinate where the lane reaches the required width.
Definition: lanefollowinggeometry.h:632
function_type_xyz m_leftBorder_fct
Definition: lanefollowinggeometry.h:49
void update(BorderSet *borderSet, BorderTrace *borderTrace, BorderCostMap *borderCost, Border *start, adore::env::VehicleMotionState9d *ego, double lookahead_distance, double lookbehind_distance, double smoothness=0.05, bool activate_navigation=false)
update the road geometry
Definition: lanefollowinggeometry.h:123
double dXbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:81
double getSMin() const
Definition: lanefollowinggeometry.h:623
double getHeading(double s)
Get the heading at a certain position.
Definition: lanefollowinggeometry.h:711
BorderAccumulator * getLeftBorders()
Get the left borders of the LaneFollowingGeometry.
Definition: lanefollowinggeometry.h:555
double Xbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:77
double getProgressOfWidthClosed() const
Get the s-coordinate where the lane stops to have the required width.
Definition: lanefollowinggeometry.h:641
BorderAccumulator m_rightBorders
Definition: lanefollowinggeometry.h:67
int m_egoBorderIndex
Definition: lanefollowinggeometry.h:71
Border * m_start
Definition: lanefollowinggeometry.h:70
function_type2d m_centerNormal_fct
Definition: lanefollowinggeometry.h:55
function_type_xyz m_centerRaw_fct
Definition: lanefollowinggeometry.h:51
double m_s_lane_width_closed
Definition: lanefollowinggeometry.h:73
bool m_view_valid
Definition: lanefollowinggeometry.h:65
double nXbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:79
BAContainer::iterator begin()
Get the begin()-iterator of the BAContainer of the right borders.
Definition: lanefollowinggeometry.h:692
BorderAccumulator * getRightBorders()
Get the right borders of the LaneFollowingGeometry.
Definition: lanefollowinggeometry.h:546
velocity_profile m_velocity_fct
Definition: lanefollowinggeometry.h:48
function_type_xyz * getLeftBorderFct()
Definition: lanefollowinggeometry.h:757
BorderAccumulator m_leftBorders
Definition: lanefollowinggeometry.h:68
function_type_scalar m_centerHeading_fct
Definition: lanefollowinggeometry.h:56
adore::mad::function_type_scalar function_type_scalar
Definition: lanefollowinggeometry.h:46
function_type_scalar m_centerSmoothedCurvature_fct
Definition: lanefollowinggeometry.h:57
double m_planning_time
Definition: lanefollowinggeometry.h:62
double Sbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:76
double dddYbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:86
int m_startIndexInRightBorders
Definition: lanefollowinggeometry.h:69
double ddYbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:84
double m_s_max
Definition: lanefollowinggeometry.h:64
double m_min_view_distance
Definition: lanefollowinggeometry.h:66
function_type_xyz m_centerSmoothedDerivative1_fct
Definition: lanefollowinggeometry.h:53
function_type_scalar m_leftDistance_fct
Definition: lanefollowinggeometry.h:59
BAContainer::iterator end()
Get the end()-iterator of the BAContainer of the right borders.
Definition: lanefollowinggeometry.h:701
Border * getBestMatchingBorder(double X, double Y, double max_deviation)
Get the best matching border for a given ego position.
Definition: lanefollowinggeometry.h:564
double getOffsetOfRightBorder(double s)
Get the offset of the right border at a certain position.
Definition: lanefollowinggeometry.h:742
void toEucledianCoordinates(double s, double n, double &Xg, double &Yg, double &Zg)
Transform from relative to euclidian coordinates.
Definition: lanefollowinggeometry.h:787
double Ybuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:78
double m_vehicle_width
Definition: lanefollowinggeometry.h:74
function_type_xyz m_centerSmoothedDerivative2_fct
Definition: lanefollowinggeometry.h:54
double m_s_min
Definition: lanefollowinggeometry.h:63
double ddXbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:83
bool computeNavigationCost(bool activate_navigation, BorderCostMap *borderCostMap)
Definition: lanefollowinggeometry.h:374
bool isValid() const
Check whether the LaneFollowingGeometry is valid.
Definition: lanefollowinggeometry.h:606
function_type_xyz * getRightBorderFct()
Definition: lanefollowinggeometry.h:759
double nYbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:80
double dYbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:82
function_type_scalar * getOffsetOfLeftBorderFct()
Get the function that holds the offset of left borders.
Definition: lanefollowinggeometry.h:732
function_type2d * getCenterlineNormal()
Get the Centerline Normal object.
Definition: lanefollowinggeometry.h:660
function_type_xyz * getCenterline()
Get the centerline of the lane.
Definition: lanefollowinggeometry.h:650
function_type_xyz m_centerSmoothed_fct
Definition: lanefollowinggeometry.h:52
function_type_scalar m_centerSmoothedCurvatureDerivative_fct
Definition: lanefollowinggeometry.h:58
function_type_xyz m_rightBorder_fct
Definition: lanefollowinggeometry.h:50
void excludeObstaclePoint(double X, double Y, double s_min, double s_safety, double n_safety, double s_off=0.0)
Modify a lane boundary to exclude a given point.
Definition: lanefollowinggeometry.h:509
double getCurvature(double s, int derivative)
Get the curvature of the lane at a certain position.
Definition: lanefollowinggeometry.h:671
LaneFollowingGeometry()
Construct a new LaneFollowingGeometry object.
Definition: lanefollowinggeometry.h:93
double dddXbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:85
function_type_scalar * getOffsetOfRightBorderFct()
Get the function that holds the offset of right borders.
Definition: lanefollowinggeometry.h:752
double getSMax() const
Definition: lanefollowinggeometry.h:619
function_type_scalar m_rightDistance_fct
Definition: lanefollowinggeometry.h:60
virtual CT f(DT x) const override
Definition: llinearpiecewisefunction.h:251
void setData(const adoreMatrix< T, n+1, 0 > &data)
Definition: llinearpiecewisefunction.h:580
double getClosestParameter(T px, T py, int d1, int d2, T &n_min) const
Definition: llinearpiecewisefunction.h:1014
virtual T fi(DT x, int row) const override
Definition: llinearpiecewisefunction.h:391
virtual DT limitLo() const override
Definition: llinearpiecewisefunction.h:264
unsigned int findIndex(DT x, DT precision=0.001) const
Definition: llinearpiecewisefunction.h:158
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
virtual DT limitHi() const override
Definition: llinearpiecewisefunction.h:259
bool getNextIntersectionWithVector2d(T x0, T px, T py, T vx, T vy, T &x_result, T &distance, T max_distance, bool extend_fringes=false)
Definition: llinearpiecewisefunction.h:850
@ left
Definition: indicator_hint.h:35
interval< T > atan2(interval< T > y, interval< T > x)
Definition: intervalarithmetic.h:234
void createAngularContinuity(adoreMatrix< T, N, 0 > &data, int row)
Definition: adoremath.h:200
adore::mad::LLinearPiecewiseFunctionM< double, 3 > function_type_xyz
Definition: linearfunctiontypedefs.h:22
adore::mad::LLinearPiecewiseFunctionM< double, 1 > function_type_scalar
Definition: linearfunctiontypedefs.h:24
adoreMatrix< T, 1, n > linspace(T x0, T x1)
Definition: adoremath.h:91
T bound(T lb, T value, T ub)
Definition: adoremath.h:569
adore::mad::LLinearPiecewiseFunctionM< double, 2 > function_type2d
Definition: linearfunctiontypedefs.h:23
bool toRelativeWithNormalExtrapolation(double qX, double qY, const T1 pi, const T1 pj, const T2 ni, const T2 nj, double &s, double &t)
Transformation from Euclidean coordinate system to a relative coordinate system represented by linear...
Definition: adoremath.h:321
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
int computeCenterline(const adoreMatrix< double, 0, 0 > &left, const adoreMatrix< double, 0, 0 > &right, adoreMatrix< double, 0, 0 > ¢er)
Definition: centerline.h:29
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
bool fromRelative(double s, double t, const T1 pi, const T1 pj, const T2 ni, const T2 nj, double &X, double &Y, double &Z)
Transform from relative coordinates to Euclidean coordinates.
Definition: adoremath.h:475
r
Definition: adore_suppress_lanechanges.py:209
Definition: areaofeffectconverter.h:20
Coordinate m_last
Definition: borderid.h:32
The border struct contains data of the smallest.
Definition: border.h:62
Tborderpath * m_path
Definition: border.h:70
double getLength()
Get the length of the border.
Definition: border.h:703
BorderID m_id
Definition: border.h:68
double m_Y
Definition: coordinate.h:35
double m_X
Definition: coordinate.h:35
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
double getX() const
Get the x-coordinate.
Definition: vehiclemotionstate9d.h:54
double getY() const
Get the y-coordinate.
Definition: vehiclemotionstate9d.h:60