23 #include <unordered_set>
45 typedef std::unordered_set<BorderBased::Node *, BorderBased::NodeHasher>
TBorderSubSet;
104 template <
int lfg_a,
int lfg_b>
119 double preview_distance,
double adjacency_lower_limit = 0.0)
133 for (
auto it = lfg->
begin(); it != lfg->
end(); it++, i++)
135 if (i >= lfg_adjacency_i_start)
196 bool neighboringLaneExists;
199 neighboringLaneExists =
204 neighboringLaneExists =
true;
206 if (neighboringLaneExists)
250 std::max(0.0, preview_distance - s));
255 for (basFollowStraight.
getNextBorder(current, inverted); current != 0;
263 node->setPredecessor(lastNode);
270 std::vector<BorderBased::Node *> open_list;
272 std::unordered_set<Border *> closed_set;
274 while (open_list.size() > 0)
277 open_list.pop_back();
278 closed_set.emplace(parent->
m_border);
280 while (expansion.hasMore())
283 if (closed_set.find(child->
m_border) == closed_set.end() &&
292 open_list.push_back(child);
306 leftBA.
append(&basNeighbor);
310 centerBorderBA.
append(*it,
false);
315 rightBA.
append(*it,
false);
321 leftBA.
append(&basNeighbor);
324 centerBorderBA.
append(&basNeighbor2);
328 rightBA.
append(*it,
false);
335 double max_lane_change_distance = 10.0;
352 bool width_open =
false;
358 for (
int i = 0; i < data.nc(); i++)
360 double d = -(std::abs)(data(1, i)) + (std::abs)(border->fi(data(0, i), 0)) -
m_vehicle_width;
408 if(
m_direction == adore::view::ALaneChangeView::direction::LEFT)
419 if(
m_direction == adore::view::ALaneChangeView::direction::RIGHT)
This class chooses the straightest successor of a border until an upper limit on distance is reached.
Definition: borderaccumulator.h:78
virtual void getNextBorder(Border *&border, bool &inverted) override
Get the next border.
Definition: borderaccumulator.h:97
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 defineFunction(function_type &f)
Create (with new) a function, which contains all accumulated border paths.
Definition: borderaccumulator.h:395
Definition: bordergraph.h:213
Expansion getPredecessors(Node *n, bool left_neighbors=true, bool right_neighbors=true, bool allow_direction_inversion=false)
Definition: bordergraph.h:346
efficiently store borders in boost R-tree
Definition: borderset.h:99
Border * getRightNeighbor(Border *b)
get the right neighbor of a border
Definition: borderset.h:1279
Border * getLeftNeighbor(Border *b)
Get left neighbor of a border.
Definition: borderset.h:1252
bool hasLeftNeighbor(Border *b)
checks whether left border exists for a border
Definition: borderset.h:1267
bool borderTypeValid(Border *b)
check whether border type is in allowed types of set
Definition: borderset.h:204
A class with a geometry description of a lane next to the current lane.
Definition: lanechangegeometry.h:38
bool isValid() const
Check whether the LaneChangeGeometry is valid.
Definition: lanechangegeometry.h:86
double m_s_viewing_distance
Definition: lanechangegeometry.h:55
adore::view::ALaneChangeView::direction getLCDirection() const
Get the direction of the LaneChangeGeometry.
Definition: lanechangegeometry.h:439
double m_vehicle_width
Definition: lanechangegeometry.h:68
Border * m_lfg_adjacency_b0
Definition: lanechangegeometry.h:60
BorderBased::Node * m_lfg_adjacency_bn0
Definition: lanechangegeometry.h:62
bool m_view_valid
Definition: lanechangegeometry.h:69
int m_lfg_adjacency_i0
Definition: lanechangegeometry.h:56
function_type_xyz m_rightBorder_fct
Definition: lanechangegeometry.h:47
double getProgressOfWidthClosed() const
Get the s-coordinate where the lane stops to have the required width.
Definition: lanechangegeometry.h:457
function_type_xyz m_reference_fct
Definition: lanechangegeometry.h:49
double getProgressOfGateOpen() const
Get the s-coordinate where the lane starts to be in direct adjacency to the LaneFollowingGeometry whi...
Definition: lanechangegeometry.h:498
TBorderSubSet m_borderSubSet
Definition: lanechangegeometry.h:64
BorderBased::BAContainer m_borderSequence
Definition: lanechangegeometry.h:65
function_type_xyz m_centerBorder_fct
Definition: lanechangegeometry.h:48
LaneChangeGeometry()
Construct a new LaneChangeGeometry object.
Definition: lanechangegeometry.h:76
function_type_xyz m_leftBorder_fct
Definition: lanechangegeometry.h:46
double getProgressOfWidthOpen() const
Get the s-coordinate where the lane reaches the required width.
Definition: lanechangegeometry.h:448
double getOffsetOfRightBorder(double s)
Get the offset of the right border at a certain position.
Definition: lanechangegeometry.h:478
double getOffsetOfCenterBorder(double s)
Get the offset of the center border at a certain position.
Definition: lanechangegeometry.h:489
int m_lfg_adjacency_i1
Definition: lanechangegeometry.h:57
BorderSubSet * getInnerBorders()
Get the sequence of inner Borders.
Definition: lanechangegeometry.h:388
double m_adjacency_s0
Definition: lanechangegeometry.h:58
adore::view::ALaneChangeView::direction m_direction
Definition: lanechangegeometry.h:67
adore::mad::function_type_scalar function_type_scalar
Definition: lanechangegeometry.h:43
function_type_scalar m_rightBorderDistance_fct
Definition: lanechangegeometry.h:51
double getProgressOfGateClosed() const
Get the s-coordinate where the lane ends to be in direct adjacency to the LaneFollowingGeometry whith...
Definition: lanechangegeometry.h:507
BorderSubSet * getRightBorders()
Get the sequence of inner Borders.
Definition: lanechangegeometry.h:417
void update(LaneFollowingGeometry< lfg_a, lfg_b > *lfg, BorderSet *borderSet, adore::env::VehicleMotionState9d *ego, int lfg_adjacency_i_start, adore::view::ALaneChangeView::direction direction, double review_distance, double preview_distance, double adjacency_lower_limit=0.0)
update the road geometry
Definition: lanechangegeometry.h:117
void clearGeometry()
Clear the LaneChangeGeometry.
Definition: lanechangegeometry.h:94
double getViewingDistance() const
Get the viewing distance.
Definition: lanechangegeometry.h:430
double m_s_lane_width_closed
Definition: lanechangegeometry.h:54
double m_adjacency_s1
Definition: lanechangegeometry.h:59
BorderSubSet * getLeftBorders()
Get the sequence of inner Borders.
Definition: lanechangegeometry.h:406
function_type_scalar m_centerBorderDistance_fct
Definition: lanechangegeometry.h:52
BorderBased::BAContainer m_innerBorderSequence
Definition: lanechangegeometry.h:66
function_type_scalar m_leftBorderDistance_fct
Definition: lanechangegeometry.h:50
double m_s_lane_width_open
Definition: lanechangegeometry.h:53
std::unordered_set< BorderBased::Node *, BorderBased::NodeHasher > TBorderSubSet
Definition: lanechangegeometry.h:45
BorderBased::Node * m_lfg_adjacency_bn1
Definition: lanechangegeometry.h:63
Border * m_lfg_adjacency_b1
Definition: lanechangegeometry.h:61
double getOffsetOfLeftBorder(double s)
Get the offset of the left border at a certain position.
Definition: lanechangegeometry.h:467
BorderSubSet * getOuterBorders()
Get the sequence of outer Borders.
Definition: lanechangegeometry.h:397
A class with a geometry description of the current lane.
Definition: lanefollowinggeometry.h:41
void toRelativeCoordinates(double Xg, double Yg, double &s, double &n)
Transform from euclidian to relative coordinates.
Definition: lanefollowinggeometry.h:770
BAContainer::iterator begin()
Get the begin()-iterator of the BAContainer of the right borders.
Definition: lanefollowinggeometry.h:692
BAContainer::iterator end()
Get the end()-iterator of the BAContainer of the right borders.
Definition: lanefollowinggeometry.h:701
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_scalar * getOffsetOfRightBorderFct()
Get the function that holds the offset of right borders.
Definition: lanefollowinggeometry.h:752
virtual DT limitLo() const override
Definition: llinearpiecewisefunction.h:264
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
virtual DT limitHi() const override
Definition: llinearpiecewisefunction.h:259
direction
Definition: alanechangeview.h:42
@ LEFT
Definition: alanechangeview.h:43
std::vector< Border * > BorderSubSet
Definition: borderset.h:92
std::vector< Border * > BAContainer
Definition: borderaccumulator.h:35
adore::mad::LLinearPiecewiseFunctionM< double, 3 > function_type_xyz
Definition: linearfunctiontypedefs.h:22
adore::mad::LLinearPiecewiseFunctionM< double, 1 > function_type_scalar
Definition: linearfunctiontypedefs.h:24
T bound(T lb, T value, T ub)
Definition: adoremath.h:569
void defineDistanceMap2d(LLinearPiecewiseFunctionM< T, n_dfun > *dfun, int id, LLinearPiecewiseFunctionM< T, n_base > *base, LLinearPiecewiseFunctionM< T, n_normal > *normal, LLinearPiecewiseFunctionM< T, n_target > *target, T guard, LLinearPiecewiseFunctionM< T, 1 > *guardfun=0)
Definition: llinearpiecewisefunction.h:1405
adore::mad::LLinearPiecewiseFunctionM< double, 2 > function_type2d
Definition: linearfunctiontypedefs.h:23
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
Definition: areaofeffectconverter.h:20
Coordinate m_first
Definition: borderid.h:32
The border struct contains data of the smallest.
Definition: border.h:62
@ OPPOSITE_DIRECTION
Definition: border.h:507
@ SAME_DIRECTION
Definition: border.h:507
bool isPredecessorOf(const BorderID &successorID)
Check whether border is a direct predecessor of another border.
Definition: border.h:333
bool isContinuousPredecessorOf(Border *other)
Check whether the border is a continuous predecessor of another border.
Definition: border.h:363
Direction getNeighborDirection()
Get the direction of the left neighbor.
Definition: border.h:517
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
Definition: bordergraph.h:28
double g() const
Definition: bordergraph.h:71
Border * m_border
Definition: bordergraph.h:30
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
ControlErrorNode node
Definition: test_control_error_node.cpp:124