Lane Planner module to compute which lanes to follow based on HD maps.
The Lane Planner takes a list of gps points as input, and computes a path along HD maps lanes from the first to the last of the gps points, minimizing time by using distance and speed limit, minimizing the number of lane changes, and following the input gps points. The module does a dijkstra search through the lane graph to compute the optimal lane plan. A custom cost function can be provided to run the dijkstra search with a different optimum criterion.
The lane plan will start at the closest lane to the first of the input gps points. The target is any lane that is on the road segment closest to the last of the input gps points.
Data Structures | |
struct | dwLanePlan |
A lane plan consist of a list of segments that describe how to get from a source lane to a target lane, following a squence of connected lanes, including information about lane changes, lane splits and lane merges. More... | |
struct | dwLanePlanActions |
Structure that packages lane change, lane split, and lane merge info together. More... | |
struct | dwLanePlanIndex |
Index defining a point in the lane plan. More... | |
struct | dwLanePlanLane |
A lane plan lane is a sequence of dwMapsLane's, with additional information per dwMapsLane about moving direction and estimated time of arrival. More... | |
struct | dwLanePlanLaneChange |
Lane change state on the lane plan. More... | |
struct | dwLanePlanSegment |
A lane plan segment is a segment along the lane plan with a consistent lane change situation. More... | |
Typedefs | |
typedef struct dwLanePlanObject const * | dwConstLanePlanHandle_t |
typedef struct dwLanePlannerObject const * | dwConstLanePlannerHandle_t |
typedef struct dwLanePlanObject * | dwLanePlanHandle_t |
typedef float32_t(* | dwLanePlannerCostFunction_t) (const dwMapsLane *lane, bool forward, float32_t length, uint32_t laneChangeCount, dwMapsSide laneChangeSide, const dwMapsGeoPoint *gpsPathPoint, uint32_t gpsPathPointCount, bool ignoreHeight, const dwMapsLane *neighborLane, void *context) |
Cost function that allows to implement a custom cost calculation in the dijkstra algorithm that calculates the optimal lane plan. More... | |
typedef struct dwLanePlannerObject * | dwLanePlannerHandle_t |
Functions | |
DW_API_PUBLIC dwStatus | dwLanePlan_computeDistance (float32_t *distance, const dwLanePlanIndex *index1, const dwLanePlanIndex *index2, dwConstLanePlanHandle_t lanePlan) |
DW_API_PUBLIC dwStatus | dwLanePlan_containsLane (bool *isOnLanePlan, const dwMapsLane *lane, dwConstLanePlanHandle_t lanePlan) |
Check whether a given dwMapsLane is part of the lane plan. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_decreaseIndex (dwLanePlanIndex *lanePlanIndex, uint32_t decreaseCount, dwConstLanePlanHandle_t lanePlan) |
Helper function to compute the previous lane plan point on a lane plan. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_findClosestPoint (dwMapsGeoPoint *closestPoint, dwLanePlanIndex *lanePlanIndex, const dwMapsGeoPoint *gps, bool ignoreHeight, dwLanePlanHandle_t lanePlan) |
Helper function to find the closest line segment on the lane plan to a given gps location, together with the corresponding closest lane plan point (the closer of the two lane plan points on the line segment that contains the closest geometric point). More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_get (dwLanePlan *lanePlan, dwConstLanePlanHandle_t lanePlanHandle) |
Retrieve the result of a previous 'dwLanePlanner_run' call. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getComplexity (uint32_t *totalLaneChangeCount, uint32_t *totalLaneSplitCount, uint32_t *totalLaneMergeCount, dwLanePlanHandle_t lanePlan) |
Returns some numbers that indicate how complex it is to autonomously drive a given lane plan. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getCurrentGeoPoint (dwMapsGeoPoint *pointOnLanePlan, dwConstLanePlanHandle_t lanePlan) |
Get current point on the lane plan in gps coordinates. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getCurrentIndex (dwLanePlanIndex *lanePlanIndex, dwConstLanePlanHandle_t lanePlan) |
Get the index to the current point on the lane plan. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getCurrentPoint (const dwMapsRoadSegment **referenceRoadSegment, dwVector3f *pointOnLanePlan, dwConstLanePlanHandle_t lanePlan) |
Get current point on the lane plan in coordinates relative to a reference road segment. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getEstimatedArrivalInfo (float32_t *distToDestination, float32_t *timeToDestination, dwConstLanePlanHandle_t lanePlanHandle) |
Get estimated time and distance to reach the end of the lane plan from current position. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getLanePlanActions (dwLanePlanIndex *laneChangePointIndex, dwLanePlanIndex *laneSplitPointIndex, dwLanePlanActions *lanePlanActionInfo, dwLanePlanHandle_t lanePlan) |
Get information about the next lane change and lane split. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getLanePlanActionsNew (dwLanePlanIndex *laneChangePointIndex, dwLanePlanIndex *laneSplitPointIndex, dwLanePlanIndex *laneMergePointIndex, dwLanePlanActions *lanePlanActionInfo, dwLanePlanHandle_t lanePlan) |
Get information about the next lane change, lane split, and lane merge. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getLanePlanSplitMergeDetails (dwMapsLaneSplitDetails *laneSplit, dwMapsLaneMergeDetails *laneMerge, dwLanePlanHandle_t lanePlan) |
Get information about the next lane change, lane split, and lane merge. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getNextLaneChange (dwLanePlanIndex *laneChangePointIndex, dwLanePlanLaneChange *laneChange, float32_t *distance, dwLanePlanHandle_t lanePlan) |
Get next lane change. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getNextLaneChangeAt (dwLanePlanIndex *laneChangePointIndex, dwLanePlanLaneChange *laneChange, const dwLanePlanIndex *queryPointIndex, dwConstLanePlanHandle_t lanePlan) |
Get the next lane change happening on the lane plan at or after given lane plan index. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getNextLaneMerge (dwLanePlanIndex *laneMergePointIndex, dwMapsLaneMergeDetails *laneMerge, float32_t *distance, dwLanePlanHandle_t lanePlan) |
Get next lane merge. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getNextLaneMergeAt (dwLanePlanIndex *laneMergePointIndex, dwMapsLaneMergeDetails *laneMerge, const dwLanePlanIndex *queryPointIndex, dwLanePlanHandle_t lanePlan) |
Get the next lane merge happening on the lane plan at or after given lane plan index. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getNextLaneSplit (dwLanePlanIndex *laneSplitPointIndex, dwMapsLaneSplitDetails *laneSplit, float32_t *distance, dwLanePlanHandle_t lanePlan) |
Get next lane split. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getNextLaneSplitAt (dwLanePlanIndex *laneSplitPointIndex, dwMapsLaneSplitDetails *laneSplit, const dwLanePlanIndex *queryPointIndex, dwLanePlanHandle_t lanePlan) |
Get the next lane split happening on the lane plan at or after given lane plan index. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getPoint (dwMapsGeoPoint *pointWGS84, dwVector3f *pointLocal, const dwMapsLane **lane, dwTime_t *time, const dwLanePlanIndex *index, dwConstLanePlanHandle_t lanePlan) |
Get point data from lane plan at given index. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_getTargetLanes (dwMapsLanePtrBuffer *lanes, uint32_t *laneChangeIndex, float32_t distance, dwLanePlanHandle_t lanePlan) |
Get target lane ptrs e.g. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_increaseIndex (dwLanePlanIndex *lanePlanIndex, uint32_t increaseCount, dwConstLanePlanHandle_t lanePlan) |
Helper function to compute the next lane plan point along a lane plan. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_initialize (dwLanePlanHandle_t *lanePlan, dwConstLanePlannerHandle_t lanePlanner) |
Creates a lane plan object and returns a handle to it. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_initializeLanePlanActions (dwLanePlanActions *lpActions) |
Default initialize a dwLanePlanActions object. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_release (dwLanePlanHandle_t lanePlan) |
Release a lane plan object. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_updateCurrentPointWithCurrentRoadSegment (const dwMapsRoadSegment *currentRoadSegment, const dwVector3f *position, const dwMatrix3d *localToENURotation33, dwTime_t timestamp, dwLanePlanHandle_t lanePlan) |
Update the current point on the lane plan based on a pose defined in relative coordinates. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_updateCurrentPointWithGlobalPose (const dwMapsGeoPoint *gps, const dwMatrix3d *localToENURotation33, dwTime_t timestamp, bool ignoreHeight, dwLanePlanHandle_t lanePlan) |
Update the current point on the lane plan based on a pose defined in absolute coordinates. More... | |
DW_API_PUBLIC dwStatus | dwLanePlan_updateCurrentPointWithLocalPose (const dwMapsRoadSegment *referenceRoadSegment, const dwVector3f *position, const dwMatrix3d *localToENURotation33, dwTime_t timestamp, dwLanePlanHandle_t lanePlan) |
Update the current point on the lane plan based on a pose defined in relative coordinates. More... | |
DW_API_PUBLIC dwStatus | dwLanePlanner_initialize (dwLanePlannerHandle_t *lanePlanner, float32_t maxPlanLengthMeters, dwConstMapHandle_t map) |
Creates a lane planner object and returns a handle to it. More... | |
DW_API_PUBLIC dwStatus | dwLanePlanner_release (dwLanePlannerHandle_t lanePlanner) |
Release a lane planner object. More... | |
DW_API_PUBLIC dwStatus | dwLanePlanner_run (const dwMapsLane *startLane, const dwMapsLane *const *targetLanes, uint32_t targetLaneCount, const dwMapsGeoPoint *gpsPathPoints, uint32_t gpsPathPointCount, bool ignoreHeight, dwLanePlannerCostFunction_t costFn, void *costFnContext, dwLanePlanHandle_t lanePlan, dwLanePlannerHandle_t lanePlanner) |
Create a lane plan along the provided gps path points, based on the map that is given at initialization. More... | |
struct dwLanePlan |
Data Fields | ||
---|---|---|
uint32_t | segmentCount | Number segments in lane plan. |
const dwLanePlanSegment * | segments | List of segments along the lane plan. |
struct dwLanePlanActions |
Data Fields | ||
---|---|---|
float32_t | distToDestination | distance to reach destination on the lane plan |
bool | hasLaneChange | true only if a valid lane change ahead is detected |
bool | hasLaneMerge | true only if a valid lane merge ahead is detected |
bool | hasLaneSplit | true only if a valid lane split ahead is detected |
dwLanePlanLaneChange | laneChange | |
float32_t | laneChangeDist | distance to where the next lane change action takes place (start of the next lane change, or 0.0f if the lane change should happen now) |
dwMapsLaneMerge | laneMerge | |
float32_t | laneMergeDist | distance to the next lane merge point |
dwMapsLaneSplit | laneSplit | |
float32_t | laneSplitDist | distance to the next lane split point |
float32_t | timeToDestination | estimated time to reach destination on the lane plan |
struct dwLanePlanIndex |
Data Fields | ||
---|---|---|
uint32_t | lanePlanLaneIndex | index of a dwLanePlanLane in the dwLanePlanSegment |
uint32_t | mapsLaneIndex | index of a dwMapsLane in the dwLanePlanLane |
uint32_t | pointIndex | index of a point in the dwMapsLane geometry |
uint32_t | segmentIndex | index of a dwLanePlanSegment in the dwLanePlan |
struct dwLanePlanLane |
Data Fields | ||
---|---|---|
const float32_t * | distances |
Distance in [m] to beginning of each dwMapsLane. Relative to beginning of the first lane of the lane plan. |
uint32_t | laneCount | Number of dwMapsLanes. |
const bool * | laneForward | Whether to move forward on lane geometry, per dwMapsLane. |
const dwMapsLane ** | lanes | Pointers to the sequential dwMapsLane's. |
const dwTime_t * | times |
Estimated time of arrival in [us] per dwMapsLane. Relative to beginning of the first lane of the lane plan. Computed based on lane speed limits. 50 km/h assumed if no speedLimit is defined for a lane. |
struct dwLanePlanLaneChange |
Data Fields | ||
---|---|---|
uint32_t | laneChangeCount | number of lanes that need to be changed |
float32_t | laneChangeLength |
available distance to do the lane change. so this is the distance to the end of the lane change, starting at the current position if it is on the lane change, or starting at the lane change start if current position is not on the lane change yet. |
dwMapsSide | laneChangeSide | side towards the lane change is going |
struct dwLanePlanSegment |
Data Fields | ||
---|---|---|
dwMapsSide | laneChangeSide | left, right, none |
uint32_t | laneCount |
Number of lanes in this segment. If there is more than one it is a lane change segment. |
const dwLanePlanLane * | lanes |
List of parallel lanes. The first lane in the list is the lane where the lane plan segment is entered, the last one is where the lane plan segment is exited. If there is more than one lane, lane changes need to be done from the first to the last lane, traversing in all in-between lanes in sequential order. |
typedef struct dwLanePlanObject const* dwConstLanePlanHandle_t |
Definition at line 74 of file LanePlanner.h.
typedef struct dwLanePlannerObject const* dwConstLanePlannerHandle_t |
Definition at line 71 of file LanePlanner.h.
typedef struct dwLanePlanObject* dwLanePlanHandle_t |
Definition at line 73 of file LanePlanner.h.
typedef float32_t(* dwLanePlannerCostFunction_t) (const dwMapsLane *lane, bool forward, float32_t length, uint32_t laneChangeCount, dwMapsSide laneChangeSide, const dwMapsGeoPoint *gpsPathPoint, uint32_t gpsPathPointCount, bool ignoreHeight, const dwMapsLane *neighborLane, void *context) |
Cost function that allows to implement a custom cost calculation in the dijkstra algorithm that calculates the optimal lane plan.
It has to return a value >= 0.
[in] | lane | Source lane of the search node. |
[in] | forward | True if moving direction is along the lane polyline direction. False if moving direction is opposite to the lane polyline direction. |
Note: Check this direction when looking at the 'laneChangeSide' argument, as the side here is relative to the polyline direction.
[in] | length | Length of the source lane. |
[in] | laneChangeCount | Number of lane changes on this node. |
[in] | laneChangeSide | The side towards which the lane change happens. Note that this is relative to the polyline direction, not the driving direction. |
[in] | gpsPathPoint | Pointer to list of dwMapsGeoPoint gps waypoints |
[in] | gpsPathPointCount | Number of gps waypoints |
[in] | ignoreHeight | Flag to assume gps points have height of 0 |
[in] | neighborLane | Next lane after current lane plan segment. That is the lane that connects to the on one after the lane change (or to 'lane' is there is no lane change). |
[in] | context | Pointer provided by the user in the call where the cost function is provided. |
Definition at line 216 of file LanePlanner.h.
typedef struct dwLanePlannerObject* dwLanePlannerHandle_t |
Definition at line 70 of file LanePlanner.h.
DW_API_PUBLIC dwStatus dwLanePlan_computeDistance | ( | float32_t * | distance, |
const dwLanePlanIndex * | index1, | ||
const dwLanePlanIndex * | index2, | ||
dwConstLanePlanHandle_t | lanePlan | ||
) |
DW_API_PUBLIC dwStatus dwLanePlan_containsLane | ( | bool * | isOnLanePlan, |
const dwMapsLane * | lane, | ||
dwConstLanePlanHandle_t | lanePlan | ||
) |
Check whether a given dwMapsLane is part of the lane plan.
[out] | isOnLanePlan | Resulting boolean, indicating whether given dwMapsLane is part of the lane plan. |
[in] | lane | dwMapsLane to be checked. |
[in] | lanePlan | Lane plan to be checked. |
DW_API_PUBLIC dwStatus dwLanePlan_decreaseIndex | ( | dwLanePlanIndex * | lanePlanIndex, |
uint32_t | decreaseCount, | ||
dwConstLanePlanHandle_t | lanePlan | ||
) |
Helper function to compute the previous lane plan point on a lane plan.
At the beginning of a lane plan lane, the index will jump onto the connected lane of the previous lane plan segment.
[in,out] | lanePlanIndex | Lane plan index to be decreased. |
[in] | decreaseCount | Number of times the index is to be decreased. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_findClosestPoint | ( | dwMapsGeoPoint * | closestPoint, |
dwLanePlanIndex * | lanePlanIndex, | ||
const dwMapsGeoPoint * | gps, | ||
bool | ignoreHeight, | ||
dwLanePlanHandle_t | lanePlan | ||
) |
Helper function to find the closest line segment on the lane plan to a given gps location, together with the corresponding closest lane plan point (the closer of the two lane plan points on the line segment that contains the closest geometric point).
[out] | closestPoint | Closest point on lane plan (it can be between two lane plan points). nullptr allowed if this output is not needed. |
[out] | lanePlanIndex | Lower index of line segment that contains the closest point. nullptr allowed if this output is not needed. |
[in] | gps | Query point. |
[in] | ignoreHeight | Defines whether distances are computed based on lat and lon only. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_get | ( | dwLanePlan * | lanePlan, |
dwConstLanePlanHandle_t | lanePlanHandle | ||
) |
Retrieve the result of a previous 'dwLanePlanner_run' call.
[out] | lanePlan | Lane plan contained in the lane plan object. |
[in] | lanePlanHandle | Lane plan object handle. |
DW_API_PUBLIC dwStatus dwLanePlan_getComplexity | ( | uint32_t * | totalLaneChangeCount, |
uint32_t * | totalLaneSplitCount, | ||
uint32_t * | totalLaneMergeCount, | ||
dwLanePlanHandle_t | lanePlan | ||
) |
Returns some numbers that indicate how complex it is to autonomously drive a given lane plan.
[out] | totalLaneChangeCount | Number of lane changes needed when following the lane plan by always changing to target lane immediately at each lane plan segment. |
[out] | totalLaneSplitCount | Number of lane splits on all lanes of the lane plan. ( |
[out] | totalLaneMergeCount | Number of lane merges on all lanes of the lane plan. ( |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_getCurrentGeoPoint | ( | dwMapsGeoPoint * | pointOnLanePlan, |
dwConstLanePlanHandle_t | lanePlan | ||
) |
Get current point on the lane plan in gps coordinates.
The current lane plan point is set in dwLanePlan_updateCurrentPoint.
[out] | pointOnLanePlan | Current point the lane plan. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_getCurrentIndex | ( | dwLanePlanIndex * | lanePlanIndex, |
dwConstLanePlanHandle_t | lanePlan | ||
) |
Get the index to the current point on the lane plan.
The current lane plan index is set in dwLanePlan_updateCurrentPoint.
[out] | lanePlanIndex | Current lane plan point index. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_getCurrentPoint | ( | const dwMapsRoadSegment ** | referenceRoadSegment, |
dwVector3f * | pointOnLanePlan, | ||
dwConstLanePlanHandle_t | lanePlan | ||
) |
Get current point on the lane plan in coordinates relative to a reference road segment.
The current lane plan point is set in dwLanePlan_updateCurrentPoint.
[out] | referenceRoadSegment | Reference road segment that defines the coordinate system of the returned point. |
[out] | pointOnLanePlan | Current point the lane plan. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_getEstimatedArrivalInfo | ( | float32_t * | distToDestination, |
float32_t * | timeToDestination, | ||
dwConstLanePlanHandle_t | lanePlanHandle | ||
) |
Get estimated time and distance to reach the end of the lane plan from current position.
Time of arrival is estimated with speed limits on map, instead of current vehicle speed.
[out] | distToDestination | Distance to destination on current lane plan in meters. |
[out] | timeToDestination | Estimated time of arrival on current lane plan in seconds. |
[in] | lanePlanHandle | Current lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_getLanePlanActions | ( | dwLanePlanIndex * | laneChangePointIndex, |
dwLanePlanIndex * | laneSplitPointIndex, | ||
dwLanePlanActions * | lanePlanActionInfo, | ||
dwLanePlanHandle_t | lanePlan | ||
) |
Get information about the next lane change and lane split.
Returns the next lane change and the next lane split on current lane. Current point is set by dwLanePlan_updateCurrentPoint.
[out] | laneChangePointIndex | Index of point where the next lane change happens. nullptr allowed if this output is not needed. |
[out] | laneSplitPointIndex | Index of point where the next lane split happens. nullptr allowed if this output is not needed. |
[out] | lanePlanActionInfo | Description of possible actions (lane change and lane split) to take in the current lane, and distances to where the corresponding actions will take place. nullptr allowed if info of certain action is not needed. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_getLanePlanActionsNew | ( | dwLanePlanIndex * | laneChangePointIndex, |
dwLanePlanIndex * | laneSplitPointIndex, | ||
dwLanePlanIndex * | laneMergePointIndex, | ||
dwLanePlanActions * | lanePlanActionInfo, | ||
dwLanePlanHandle_t | lanePlan | ||
) |
Get information about the next lane change, lane split, and lane merge.
Returns the next lane change, next lane split and lane merge on current lane. Current point is set by dwLanePlan_updateCurrentPoint.
[out] | laneChangePointIndex | Index of point where the next lane change happens. nullptr allowed if this output is not needed. |
[out] | laneSplitPointIndex | Index of point where the next lane split happens. nullptr allowed if this output is not needed. |
[out] | laneMergePointIndex | Index of point where the next lane merge ends. nullptr allowed if this output is not needed. |
[out] | lanePlanActionInfo | Description of upcoming action (lane change, lane split, lane merge) on the current lane, and distances to where the corresponding actions should take place. nullptr allowed if this output is not needed. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_getLanePlanSplitMergeDetails | ( | dwMapsLaneSplitDetails * | laneSplit, |
dwMapsLaneMergeDetails * | laneMerge, | ||
dwLanePlanHandle_t | lanePlan | ||
) |
Get information about the next lane change, lane split, and lane merge.
Returns the details of lane split and merge from previous dwLanePlan_getLanePlanActionsNew call
[out] | laneSplit | Index of point where the next lane merge ends. nullptr allowed if this output is not needed. |
[out] | laneMerge | Description of upcoming action (lane change, lane split, lane merge) on the current lane, and distances to where the corresponding actions should take place. nullptr allowed if this output is not needed. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_getNextLaneChange | ( | dwLanePlanIndex * | laneChangePointIndex, |
dwLanePlanLaneChange * | laneChange, | ||
float32_t * | distance, | ||
dwLanePlanHandle_t | lanePlan | ||
) |
Get next lane change.
Returns the next lane change on current lane. Current point is set by dwLanePlan_updateCurrentPoint.
[out] | laneChangePointIndex | Index of point where the next lane change happens. nullptr allowed if this output is not needed. |
[out] | laneChange | Description of the lane change. nullptr allowed if this output is not needed. |
[out] | distance | Distance of the lane change start from current position. nullptr allowed if this output is not needed. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_getNextLaneChangeAt | ( | dwLanePlanIndex * | laneChangePointIndex, |
dwLanePlanLaneChange * | laneChange, | ||
const dwLanePlanIndex * | queryPointIndex, | ||
dwConstLanePlanHandle_t | lanePlan | ||
) |
Get the next lane change happening on the lane plan at or after given lane plan index.
To get the lane plan index of the point corresponding to a current vehicle pose, use dwLanePlan_updateCurrentPoint and dwLanePlan_getCurrentPoint.
[out] | laneChangePointIndex | Lane plan index of the point where the next lane change happens. nullptr allowed if this output is not needed. |
[out] | laneChange | Description of the next lane change. nullptr allowed if this output is not needed. |
[in] | queryPointIndex | Lane plan index defining the query position on the lane plan. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_getNextLaneMerge | ( | dwLanePlanIndex * | laneMergePointIndex, |
dwMapsLaneMergeDetails * | laneMerge, | ||
float32_t * | distance, | ||
dwLanePlanHandle_t | lanePlan | ||
) |
Get next lane merge.
Returns the next merge split on current lane. Current point is set by dwLanePlan_updateCurrentPoint.
[out] | laneMergePointIndex | Index of point where the next lane merge ends. nullptr allowed if this output is not needed. |
[out] | laneMerge | Description of the lane merge. nullptr allowed if this output is not needed. |
[out] | distance | Distance of the lane merge from current position. nullptr allowed if this output is not needed. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_getNextLaneMergeAt | ( | dwLanePlanIndex * | laneMergePointIndex, |
dwMapsLaneMergeDetails * | laneMerge, | ||
const dwLanePlanIndex * | queryPointIndex, | ||
dwLanePlanHandle_t | lanePlan | ||
) |
Get the next lane merge happening on the lane plan at or after given lane plan index.
To get the lane plan index of the point corresponding to a current vehicle pose, use dwLanePlan_updateCurrentPoint and dwLanePlan_getCurrentPoint.
[out] | laneMergePointIndex | Lane plan index of the point where the next lane merge ends. nullptr allowed if this output is not needed. |
[out] | laneMerge | Description of the next lane merge. nullptr allowed if this output is not needed. |
[in] | queryPointIndex | Lane plan point index defining the query position on the lane plan. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_getNextLaneSplit | ( | dwLanePlanIndex * | laneSplitPointIndex, |
dwMapsLaneSplitDetails * | laneSplit, | ||
float32_t * | distance, | ||
dwLanePlanHandle_t | lanePlan | ||
) |
Get next lane split.
Returns the next lane split on current lane. Current point is set by dwLanePlan_updateCurrentPoint.
[out] | laneSplitPointIndex | Index of point where the next lane split happens. nullptr allowed if this output is not needed. |
[out] | laneSplit | Description of the lane split. nullptr allowed if this output is not needed. |
[out] | distance | Distance of the lane split from current position. nullptr allowed if this output is not needed. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_getNextLaneSplitAt | ( | dwLanePlanIndex * | laneSplitPointIndex, |
dwMapsLaneSplitDetails * | laneSplit, | ||
const dwLanePlanIndex * | queryPointIndex, | ||
dwLanePlanHandle_t | lanePlan | ||
) |
Get the next lane split happening on the lane plan at or after given lane plan index.
To get the lane plan index of the point corresponding to a current vehicle pose, use dwLanePlan_updateCurrentPoint and dwLanePlan_getCurrentPoint.
[out] | laneSplitPointIndex | Lane plan index of the point where the next lane split starts. nullptr allowed if this output is not needed. |
[out] | laneSplit | Description of the next lane split. nullptr allowed if this output is not needed. |
[in] | queryPointIndex | Lane plan point index defining the query position on the lane plan. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_getPoint | ( | dwMapsGeoPoint * | pointWGS84, |
dwVector3f * | pointLocal, | ||
const dwMapsLane ** | lane, | ||
dwTime_t * | time, | ||
const dwLanePlanIndex * | index, | ||
dwConstLanePlanHandle_t | lanePlan | ||
) |
Get point data from lane plan at given index.
[out] | pointWGS84 | WGS84 coordinates at index. nullptr allowed if this output is not needed. |
[out] | pointLocal | local coordinates at index. nullptr allowed if this output is not needed. |
[out] | lane | Maps lane at index. nullptr allowed if this output is not needed. |
[out] | time | Arrival time at index. nullptr allowed if this output is not needed. |
[in] | index | Index pointing into the provided lane plan. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_getTargetLanes | ( | dwMapsLanePtrBuffer * | lanes, |
uint32_t * | laneChangeIndex, | ||
float32_t | distance, | ||
dwLanePlanHandle_t | lanePlan | ||
) |
Get target lane ptrs e.g.
for an upcoming split, return lanes before the split before (no ambiguity) and desired lanes after split (again no ambiguity) e.g. for an upcoming lane change, after "change now", return the immediate target adjacent lanes and its successors Use distance as the stopping criterion for successors
[out] | lanes | output maps lane array for targt lanes |
[out] | laneChangeIndex | output index in lane array indicating the LaneCount of the ego lane (lanes[laneChangeIndex] is the first index in lane change target lane) |
[in] | distance | length ahead of ego for stitching map lanes ahead |
[in] | lanePlan | current lane plan |
DW_API_PUBLIC dwStatus dwLanePlan_increaseIndex | ( | dwLanePlanIndex * | lanePlanIndex, |
uint32_t | increaseCount, | ||
dwConstLanePlanHandle_t | lanePlan | ||
) |
Helper function to compute the next lane plan point along a lane plan.
At the end of a lane plan lane, the index will jump onto the connected lane of the subsequent lane plan segment.
[in,out] | lanePlanIndex | Lane plan index to be increased. |
[in] | increaseCount | Number of times the index is to be increased. |
[in] | lanePlan | Lane plan. |
DW_API_PUBLIC dwStatus dwLanePlan_initialize | ( | dwLanePlanHandle_t * | lanePlan, |
dwConstLanePlannerHandle_t | lanePlanner | ||
) |
Creates a lane plan object and returns a handle to it.
Lane Plan objects store the result of the lane planner.
[out] | lanePlan | Resulting lane planner handle. Preallocated buffer sizes are defined in the lane planner. |
[in] | lanePlanner | Lane Planner object handle. |
DW_API_PUBLIC dwStatus dwLanePlan_initializeLanePlanActions | ( | dwLanePlanActions * | lpActions | ) |
Default initialize a dwLanePlanActions object.
[out] | lpActions | obj to be default initialized |
DW_API_PUBLIC dwStatus dwLanePlan_release | ( | dwLanePlanHandle_t | lanePlan | ) |
Release a lane plan object.
[in,out] | lanePlan | Lane plan to release. |
DW_API_PUBLIC dwStatus dwLanePlan_updateCurrentPointWithCurrentRoadSegment | ( | const dwMapsRoadSegment * | currentRoadSegment, |
const dwVector3f * | position, | ||
const dwMatrix3d * | localToENURotation33, | ||
dwTime_t | timestamp, | ||
dwLanePlanHandle_t | lanePlan | ||
) |
Update the current point on the lane plan based on a pose defined in relative coordinates.
The resulting current point can be accessed by calling dwLanePlan_getCurrentPoint.
Use this function when it is known that the current lane on is the provided road segment. This reduces the search space for the current point on the lane plan, and thus improves performance.
[in] | currentRoadSegment | ID of road segment that defines the position's coordinate system |
[in] | position | Current position |
[in] | localToENURotation33 | 3x3 Rotation matrix that rotates from local coordinate system (x: forward, y: left, z: up) into ENU local cartesian coordinate system with origin at the origin of the reference road segment (x: east, y: north, z: up). z coordinate of ENU is the normal of the earth spheroid at origin of the reference road segment. Orientation is not used if localToENURotation33 is nullptr. |
[in] | timestamp | Timestamp of the passed pose |
[in] | lanePlan | Lane plan on which the current point is being tracked. |
DW_API_PUBLIC dwStatus dwLanePlan_updateCurrentPointWithGlobalPose | ( | const dwMapsGeoPoint * | gps, |
const dwMatrix3d * | localToENURotation33, | ||
dwTime_t | timestamp, | ||
bool | ignoreHeight, | ||
dwLanePlanHandle_t | lanePlan | ||
) |
Update the current point on the lane plan based on a pose defined in absolute coordinates.
The resulting current point can be accessed by calling dwLanePlan_getCurrentPoint.
[in] | gps | Current position |
[in] | localToENURotation33 | 3x3 Rotation matrix that rotates from local coordinate system (x: forward, y: left, z: up) into ENU local cartesian coordinate system with origin at the gps point (x: east, y: north, z: up). z coordinate of ENU is the normal of the earth spheroid at the gps point. Orientation is not used if localToENURotation33 is nullptr. |
[in] | timestamp | Timestamp of the passed pose |
[in] | ignoreHeight | Defines whether distances are computed based on lat and lon only. |
[in] | lanePlan | Lane plan on which the current point is being tracked. |
DW_API_PUBLIC dwStatus dwLanePlan_updateCurrentPointWithLocalPose | ( | const dwMapsRoadSegment * | referenceRoadSegment, |
const dwVector3f * | position, | ||
const dwMatrix3d * | localToENURotation33, | ||
dwTime_t | timestamp, | ||
dwLanePlanHandle_t | lanePlan | ||
) |
Update the current point on the lane plan based on a pose defined in relative coordinates.
The resulting current point can be accessed by calling dwLanePlan_getCurrentPoint.
[in] | referenceRoadSegment | ID of road segment that defines the position's coordinate system |
[in] | position | Current position |
[in] | localToENURotation33 | 3x3 Rotation matrix that rotates from local coordinate system (x: forward, y: left, z: up) into ENU local cartesian coordinate system with origin at the origin of the reference road segment (x: east, y: north, z: up). z coordinate of ENU is the normal of the earth spheroid at origin of the reference road segment. Orientation is not used if localToENURotation33 is nullptr. |
[in] | timestamp | Timestamp of the passed pose |
[in] | lanePlan | Lane plan on which the current point is being tracked. |
DW_API_PUBLIC dwStatus dwLanePlanner_initialize | ( | dwLanePlannerHandle_t * | lanePlanner, |
float32_t | maxPlanLengthMeters, | ||
dwConstMapHandle_t | map | ||
) |
Creates a lane planner object and returns a handle to it.
[out] | lanePlanner | Resulting lane planner handle. |
[in] | maxPlanLengthMeters | Maximum length of result. This defines the module's buffer sizes that are preallocated for the runtime algorithm. |
[in] | map | Map object handle from the Maps module. |
DW_API_PUBLIC dwStatus dwLanePlanner_release | ( | dwLanePlannerHandle_t | lanePlanner | ) |
Release a lane planner object.
[in,out] | lanePlanner | Lane planner to release. |
DW_API_PUBLIC dwStatus dwLanePlanner_run | ( | const dwMapsLane * | startLane, |
const dwMapsLane *const * | targetLanes, | ||
uint32_t | targetLaneCount, | ||
const dwMapsGeoPoint * | gpsPathPoints, | ||
uint32_t | gpsPathPointCount, | ||
bool | ignoreHeight, | ||
dwLanePlannerCostFunction_t | costFn, | ||
void * | costFnContext, | ||
dwLanePlanHandle_t | lanePlan, | ||
dwLanePlannerHandle_t | lanePlanner | ||
) |
Create a lane plan along the provided gps path points, based on the map that is given at initialization.
A dijkstra search is applied to the lane graph of the HD map. The search starts at the closest lane to the first point of the input gps path points. The target is any lane on the road segment closest to the last input gps path point.
[in] | startLane | Lane where the search is started. If nullptr, the lane assigned to the first gps path point is used. |
[in] | targetLanes | Target of the search. If nullptr, the lane group assigned to the last gps path point is used. |
[in] | targetLaneCount | Number of lanes in the targetLanes array. If 0, the lane group assigned to the last gps path point is used. |
[in] | gpsPathPoints | Path points to follow. If startLane and targetRoadSegment are omitted, at least 2 points are required to define the start and end point of the lane plan. |
[in] | gpsPathPointCount | Number of provided gps path points. |
[in] | ignoreHeight | Defines whether the height of the path points should be considered when looking for the closest lanes. |
[in] | costFn | Optional. Cost function that allows to implement a custom cost calculation for a search node. |
[in] | costFnContext | Pointer that is forwarded to costFn. |
[in,out] | lanePlan | Resulting lane plan. |
[in] | lanePlanner | The lane planner object that comptues the lane plan. |