SafetyForceField.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) 2019-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_SAFETYFORCEFIELD_H_
#define DW_PLANNING_SAFETYFORCEFIELD_H_

#include <dw/core/Types.h>
#include <dw/core/Status.h>
#include <dw/world/ObjectArray.h>
#include <dw/image/Image.h>
#include <dw/egomotion/Egomotion.h>
#include <dw/control/vehicleio/VehicleIO.h>
#include <dw/worldmodel/LaneGraph.h>
#include <dw/planning/safetyforcefield/ControlConstraints.h>

#ifdef __cplusplus
extern "C" {
#endif

#define DW_SAFETY_FORCE_FIELD_MAX_NUM_ACTORS_LIMIT 150
#define DW_SAFETY_FORCE_FIELD_NUM_SLICES 16
#define DW_SAFETY_FORCE_FIELD_METRICS_MAX_NUM_COLLIDING_OBJECTS 200
#define DW_SAFETY_FORCE_FIELD_NUM_VERTICES_EXPANDING_CLAIMED_SETS 6
#define DW_SAFETY_FORCE_FIELD_MAX_NUM_CLOSE_ACTORS 10 

//#######################################################################################
// Handles
//#######################################################################################
typedef struct dwSafetyForceFieldObject* dwSafetyForceFieldHandle_t;
typedef struct dwSafetyForceFieldObject const* dwConstSafetyForceFieldHandle_t;

//#######################################################################################
// Structs
//#######################################################################################

typedef struct dwSafetyForceFieldRequest
{
    bool activationRequest; 
    dwTime_t timeStamp;     
} dwSafetyForceFieldRequest;

typedef struct dwSafetyForceFieldParams
{
    uint32_t maxNumActors;   
    bool useGPU;             
    bool useRoadStructure;   
    bool expandClaimedSets;  
    bool forceStraight;      
    bool filterActors;       
    float32_t minConfidence; 
    dwVehicle vehicleProps;  
    float32_t margin;        
    float32_t maxClearance;  
    bool restrictToMoving;   
} dwSafetyForceFieldParams;

typedef struct dwSafetyForceFieldMetrics
{
    uint32_t totalNumFrames;                 
    uint32_t totalNumFramesWithCollision;    
    uint32_t totalNumSequencesWithCollision; 
    uint32_t totalNumFramesWithActuation;    
    uint32_t totalNumSequencesWithActuation; 
    float32_t collisionEMA;                  
    float32_t actuationEMA;                  
    // current frame information
    uint32_t collisionSequenceLength; 
    uint32_t actuationSequenceLength; 
    uint32_t numCollidingObjects;     

    uint64_t collidingObjectIds[DW_SAFETY_FORCE_FIELD_METRICS_MAX_NUM_COLLIDING_OBJECTS];       
    uint32_t collidingObjectSensorIds[DW_SAFETY_FORCE_FIELD_METRICS_MAX_NUM_COLLIDING_OBJECTS]; 
} dwSafetyForceFieldMetrics;

typedef struct dwSafetyForceFieldPerActor
{
    uint64_t objectId;                 
    uint32_t objectSensorId;           
    float32_t collisionTime;           
    dwVector2f collisionPoint;         
    float32_t longitudinalSafetyForce; 
    float32_t lateralSafetyForce;      

    dwVector2f positions[DW_SAFETY_FORCE_FIELD_NUM_SLICES]; 
    float32_t headings[DW_SAFETY_FORCE_FIELD_NUM_SLICES];   
} dwSafetyForceFieldPerActor;

typedef struct dwSafetyForceFieldState
{
    float32_t desiredLongitudinalAccel;              
    float32_t desiredLateralAccel;                   
    float32_t maxStopTime;                           
    float32_t controlImageStart;                     
    float32_t controlImageEnd;                       
    dwSafetyForceFieldConeType controlImageConeType; 

    uint32_t numActors; 
    dwSafetyForceFieldPerActor actors[DW_SAFETY_FORCE_FIELD_MAX_NUM_ACTORS_LIMIT];

    uint32_t numClaimedSets;                                                      
    uint32_t claimedSetsActorIndices[DW_SAFETY_FORCE_FIELD_MAX_NUM_CLOSE_ACTORS]; 
    dwVector2f claimedSets[DW_SAFETY_FORCE_FIELD_MAX_NUM_CLOSE_ACTORS]
                          [DW_SAFETY_FORCE_FIELD_NUM_VERTICES_EXPANDING_CLAIMED_SETS]
                          [DW_SAFETY_FORCE_FIELD_NUM_SLICES]; 
} dwSafetyForceFieldState;

//#######################################################################################
// Functions
//#######################################################################################

DW_API_PUBLIC dwStatus dwSafetyForceField_initDefaultParams(dwSafetyForceFieldParams* params);

DW_API_PUBLIC dwStatus dwSafetyForceField_initialize(dwSafetyForceFieldHandle_t* handle,
                                                     const dwSafetyForceFieldParams* params,
                                                     dwContextHandle_t context);

DW_API_PUBLIC dwStatus dwSafetyForceField_release(dwSafetyForceFieldHandle_t handle);

DW_API_PUBLIC dwStatus dwSafetyForceField_reset(dwSafetyForceFieldHandle_t handle);

DW_API_PUBLIC dwStatus dwSafetyForceField_setCUDAStream(cudaStream_t stream, dwSafetyForceFieldHandle_t handle);

DW_API_PUBLIC dwStatus dwSafetyForceField_getCUDAStream(cudaStream_t* stream, dwSafetyForceFieldHandle_t handle);

DW_API_PUBLIC dwStatus dwSafetyForceField_setEgomotion(dwEgomotionConstHandle_t egomotion,
                                                       dwSafetyForceFieldHandle_t handle);

DW_API_PUBLIC dwStatus dwSafetyForceField_process(dwObjectArray** objectsList,
                                                  uint32_t numObjectArrays,
                                                  const dwVehicleIOState* vioState,
                                                  const dwLaneGraph* lg,
                                                  dwSafetyForceFieldHandle_t handle);

DW_API_PUBLIC dwStatus dwSafetyForceField_getControlImage(dwImageHandle_t* img,
                                                          const dwSafetyForceFieldControl* beforeActuation,
                                                          const dwSafetyForceFieldControl* afterActuation,
                                                          float32_t steeringLimit,
                                                          float32_t minAccel,
                                                          float32_t maxAccel,
                                                          float32_t steerRange,
                                                          float32_t accelRange,
                                                          dwSafetyForceFieldHandle_t handle);

DW_API_PUBLIC dwStatus dwSafetyForceField_getControlConstraints(dwSafetyForceFieldControlConstraints* constraints,
                                                                dwSafetyForceFieldHandle_t handle);

DW_API_PUBLIC dwStatus dwSafetyForceField_combineControlConstraints(dwSafetyForceFieldMultiControlConstraints* out,
                                                                    const dwSafetyForceFieldControlConstraints* ins,
                                                                    uint32_t inCount,
                                                                    dwSafetyForceFieldCombinationType type,
                                                                    uint32_t constraintBits);

DW_API_PUBLIC dwStatus dwSafetyForceField_initializeControlConstraints(dwSafetyForceFieldControlConstraints* c);

DW_API_PUBLIC dwStatus dwSafetyForceField_initializeMultiControlConstraints(dwSafetyForceFieldMultiControlConstraints* c);

DW_API_PUBLIC dwStatus dwSafetyForceField_getMetrics(dwSafetyForceFieldMetrics* metrics,
                                                     dwSafetyForceFieldHandle_t handle);

DW_API_PUBLIC dwStatus dwSafetyForceField_getState(dwSafetyForceFieldState* state,
                                                   const dwSafetyForceFieldControl* beforeActuation,
                                                   const dwSafetyForceFieldControl* afterActuation,
                                                   bool relative,
                                                   dwSafetyForceFieldHandle_t handle);

#ifdef __cplusplus
}
#endif

#endif // DW_PLANNING_SAFETYFORCEFIELD_H_