LanePlanner.h

Go to the documentation of this file.

// This code contains NVIDIA Confidential Information and is disclosed
// under the Mutual Non-Disclosure Agreement.
//
// Notice
// ALL NVIDIA DESIGN SPECIFICATIONS AND CODE ("MATERIALS") ARE PROVIDED "AS IS" NVIDIA MAKES
// NO REPRESENTATIONS, WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
// THE MATERIALS, AND EXPRESSLY DISCLAIMS ANY IMPLIED WARRANTIES OF NONINFRINGEMENT,
// MERCHANTABILITY, OR FITNESS FOR A PARTICULAR PURPOSE.
//
// NVIDIA Corporation assumes no responsibility for the consequences of use of such
// information or for any infringement of patents or other rights of third parties that may
// result from its use. No license is granted by implication or otherwise under any patent
// or patent rights of NVIDIA Corporation. No third party distribution is allowed unless
// expressly authorized by NVIDIA.  Details are subject to change without notice.
// This code supersedes and replaces all information previously supplied.
// NVIDIA Corporation products are not authorized for use as critical
// components in life support devices or systems without express written approval of
// NVIDIA Corporation.
//
// Copyright (c) 2017-2020 NVIDIA Corporation. All rights reserved.
//
// NVIDIA Corporation and its licensors retain all intellectual property and proprietary
// rights in and to this software and related documentation and any modifications thereto.
// Any use, reproduction, disclosure or distribution of this software and related
// documentation without an express license agreement from NVIDIA Corporation is
// strictly prohibited.
//

#ifndef DW_PLANNING_LANEPLANNER_H_
#define DW_PLANNING_LANEPLANNER_H_

#include <dw/maps/Maps.h>

#ifdef __cplusplus
extern "C" {
#endif

//#######################################################################################
// Handles
//#######################################################################################
typedef struct dwLanePlannerObject* dwLanePlannerHandle_t;
typedef struct dwLanePlannerObject const* dwConstLanePlannerHandle_t;

typedef struct dwLanePlanObject* dwLanePlanHandle_t;
typedef struct dwLanePlanObject const* dwConstLanePlanHandle_t;

//#######################################################################################
// Lane Planner
//#######################################################################################

typedef struct dwLanePlanLane
{
    const dwMapsLane** lanes;   
    const bool* laneForward;    
    const float32_t* distances; 
    const dwTime_t* times;      
    uint32_t laneCount;         
} dwLanePlanLane;

typedef struct dwLanePlanSegment
{
    const dwLanePlanLane* lanes; 
    uint32_t laneCount;          
    dwMapsSide laneChangeSide;   
} dwLanePlanSegment;

typedef struct dwLanePlan
{
    const dwLanePlanSegment* segments; 
    uint32_t segmentCount;             
} dwLanePlan;

DW_API_PUBLIC
dwStatus dwLanePlanner_initialize(dwLanePlannerHandle_t* lanePlanner,
                                  float32_t maxPlanLengthMeters,
                                  dwConstMapHandle_t map);

DW_API_PUBLIC
dwStatus dwLanePlanner_release(dwLanePlannerHandle_t lanePlanner);

DW_API_PUBLIC
dwStatus dwLanePlan_initialize(dwLanePlanHandle_t* lanePlan, dwConstLanePlannerHandle_t lanePlanner);

DW_API_PUBLIC
dwStatus dwLanePlan_release(dwLanePlanHandle_t lanePlan);

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);

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);

DW_API_PUBLIC
dwStatus dwLanePlan_get(dwLanePlan* lanePlan,
                        dwConstLanePlanHandle_t lanePlanHandle);

//#######################################################################################
// Lane Plan Utilities
//#######################################################################################

typedef struct dwLanePlanIndex
{
    uint32_t segmentIndex;      
    uint32_t lanePlanLaneIndex; 
    uint32_t mapsLaneIndex;     
    uint32_t pointIndex;        
} dwLanePlanIndex;

DW_API_PUBLIC
dwStatus dwLanePlan_getPoint(dwMapsGeoPoint* pointWGS84,
                             dwVector3f* pointLocal,
                             const dwMapsLane** lane,
                             dwTime_t* time,
                             const dwLanePlanIndex* index,
                             dwConstLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_findClosestPoint(dwMapsGeoPoint* closestPoint,
                                     dwLanePlanIndex* lanePlanIndex,
                                     const dwMapsGeoPoint* gps,
                                     bool ignoreHeight,
                                     dwLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_increaseIndex(dwLanePlanIndex* lanePlanIndex,
                                  uint32_t increaseCount,
                                  dwConstLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_decreaseIndex(dwLanePlanIndex* lanePlanIndex,
                                  uint32_t decreaseCount,
                                  dwConstLanePlanHandle_t lanePlan);

typedef struct dwLanePlanLaneChange
{
    dwMapsSide laneChangeSide;  
    uint32_t laneChangeCount;   
    float32_t laneChangeLength; 
} dwLanePlanLaneChange;

typedef struct dwLanePlanActions
{
    bool hasLaneChange; 
    bool hasLaneSplit;  
    bool hasLaneMerge;  
    dwLanePlanLaneChange laneChange;
    dwMapsLaneSplit laneSplit;
    dwMapsLaneMerge laneMerge;
    float32_t laneChangeDist;    
    float32_t laneSplitDist;     
    float32_t laneMergeDist;     
    float32_t timeToDestination; 
    float32_t distToDestination; 
} dwLanePlanActions;

DW_API_PUBLIC
dwStatus dwLanePlan_initializeLanePlanActions(dwLanePlanActions* lpActions);

DW_API_PUBLIC
dwStatus dwLanePlan_getNextLaneChangeAt(dwLanePlanIndex* laneChangePointIndex,
                                        dwLanePlanLaneChange* laneChange,
                                        const dwLanePlanIndex* queryPointIndex,
                                        dwConstLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_getNextLaneChange(dwLanePlanIndex* laneChangePointIndex,
                                      dwLanePlanLaneChange* laneChange,
                                      float32_t* distance,
                                      dwLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_getNextLaneSplitAt(dwLanePlanIndex* laneSplitPointIndex,
                                       dwMapsLaneSplitDetails* laneSplit,
                                       const dwLanePlanIndex* queryPointIndex,
                                       dwLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_getNextLaneSplit(dwLanePlanIndex* laneSplitPointIndex,
                                     dwMapsLaneSplitDetails* laneSplit,
                                     float32_t* distance,
                                     dwLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_getNextLaneMergeAt(dwLanePlanIndex* laneMergePointIndex,
                                       dwMapsLaneMergeDetails* laneMerge,
                                       const dwLanePlanIndex* queryPointIndex,
                                       dwLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_getNextLaneMerge(dwLanePlanIndex* laneMergePointIndex,
                                     dwMapsLaneMergeDetails* laneMerge,
                                     float32_t* distance,
                                     dwLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_getLanePlanActions(dwLanePlanIndex* laneChangePointIndex,
                                       dwLanePlanIndex* laneSplitPointIndex,
                                       dwLanePlanActions* lanePlanActionInfo,
                                       dwLanePlanHandle_t lanePlan)
    DW_DEPRECATED("WARNING: the function will be removed in the next major release, use dwLanePlan_getLanePlanActionsNew.");

DW_API_PUBLIC
dwStatus dwLanePlan_getLanePlanActionsNew(dwLanePlanIndex* laneChangePointIndex,
                                          dwLanePlanIndex* laneSplitPointIndex,
                                          dwLanePlanIndex* laneMergePointIndex,
                                          dwLanePlanActions* lanePlanActionInfo,
                                          dwLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_getLanePlanSplitMergeDetails(dwMapsLaneSplitDetails* laneSplit,
                                                 dwMapsLaneMergeDetails* laneMerge,
                                                 dwLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_computeDistance(float32_t* distance,
                                    const dwLanePlanIndex* index1, const dwLanePlanIndex* index2,
                                    dwConstLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_getComplexity(uint32_t* totalLaneChangeCount,
                                  uint32_t* totalLaneSplitCount,
                                  uint32_t* totalLaneMergeCount,
                                  dwLanePlanHandle_t lanePlan);

//#######################################################################################
// Lane Plan Tracker
//#######################################################################################

DW_API_PUBLIC
dwStatus dwLanePlan_updateCurrentPointWithGlobalPose(const dwMapsGeoPoint* gps,
                                                     const dwMatrix3d* localToENURotation33,
                                                     dwTime_t timestamp,
                                                     bool ignoreHeight,
                                                     dwLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_updateCurrentPointWithLocalPose(const dwMapsRoadSegment* referenceRoadSegment,
                                                    const dwVector3f* position,
                                                    const dwMatrix3d* localToENURotation33,
                                                    dwTime_t timestamp,
                                                    dwLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_updateCurrentPointWithCurrentRoadSegment(const dwMapsRoadSegment* currentRoadSegment,
                                                             const dwVector3f* position,
                                                             const dwMatrix3d* localToENURotation33,
                                                             dwTime_t timestamp,
                                                             dwLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_getCurrentIndex(dwLanePlanIndex* lanePlanIndex,
                                    dwConstLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_getCurrentGeoPoint(dwMapsGeoPoint* pointOnLanePlan,
                                       dwConstLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_getCurrentPoint(const dwMapsRoadSegment** referenceRoadSegment,
                                    dwVector3f* pointOnLanePlan,
                                    dwConstLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_containsLane(bool* isOnLanePlan, const dwMapsLane* lane, dwConstLanePlanHandle_t lanePlan);

DW_API_PUBLIC
dwStatus dwLanePlan_getEstimatedArrivalInfo(float32_t* distToDestination,
                                            float32_t* timeToDestination,
                                            dwConstLanePlanHandle_t lanePlanHandle);

DW_API_PUBLIC
dwStatus dwLanePlan_getTargetLanes(dwMapsLanePtrBuffer* lanes,
                                   uint32_t* laneChangeIndex,
                                   float32_t distance,
                                   dwLanePlanHandle_t lanePlan);

#ifdef __cplusplus
}
#endif

#endif // DW_PLANNING_LANEPLANNER_LANEPLANNER_H_