工程创新大赛智能网联汽车赛项AEB项目满分开源

#include "SimOneServiceAPI.h"
#include "SimOnePNCAPI.h"
#include "SimOneHDMapAPI.h"
#include "SimOneSensorAPI.h"
#include "SSD/SimPoint3D.h"
#include "UtilMath.h"
#include "hdmap/SampleGetNearMostLane.h"
#include "hdmap/SampleGetLaneST.h"
#include <memory>
#include <limits>
#include <iostream>
#include "SimOneEvaluationAPI.h"
#include <iostream>

//Main function
//
using namespace std;
int main()
{
    bool inAEBState = false;
    bool Forword_people = false;
    bool isSimOneInitialized = false;
    const char* MainVehicleId = "0";
    bool isJoinTimeLoop = true;
    SimOneAPI::InitSimOneAPI(MainVehicleId, isJoinTimeLoop);
    SimOneAPI::SetDriverName(MainVehicleId, "AEB");
    SimOneAPI::SetDriveMode(MainVehicleId, ESimOne_Drive_Mode_API);
    SimOneAPI::InitEvaluationServiceWithLocalData(MainVehicleId);

    int timeout = 20;
    while (true) {
        if (SimOneAPI::LoadHDMap(timeout)) {
            SimOneAPI::SetLogOut(ESimOne_LogLevel_Type::ESimOne_LogLevel_Type_Information, "HDMap Information Loaded");
            break;
        }
        SimOneAPI::SetLogOut(ESimOne_LogLevel_Type::ESimOne_LogLevel_Type_Information, "HDMap Information Loading...");
    }

    while (true) {
        int frame = SimOneAPI::Wait();

        if (SimOneAPI::GetCaseRunStatus() == ESimOne_Case_Status::ESimOne_Case_Status_Stop) {
            SimOneAPI::SaveEvaluationRecord();
            break;
        }

        std::unique_ptr<SimOne_Data_Gps> pGps = std::make_unique<SimOne_Data_Gps>();
        if (!SimOneAPI::GetGps(MainVehicleId, pGps.get())) {
            SimOneAPI::SetLogOut(ESimOne_LogLevel_Type::ESimOne_LogLevel_Type_Warning, "Fetch GPS failed");
        }

        std::unique_ptr<SimOne_Data_Obstacle> pObstacle = std::make_unique<SimOne_Data_Obstacle>();
        if (!SimOneAPI::GetGroundTruth(MainVehicleId, pObstacle.get())) {
            SimOneAPI::SetLogOut(ESimOne_LogLevel_Type::ESimOne_LogLevel_Type_Warning, "Fetch obstacle failed");
        }

        if (SimOneAPI::GetCaseRunStatus() == ESimOne_Case_Status::ESimOne_Case_Status_Running) {
            if (!isSimOneInitialized) {
                SimOneAPI::SetLogOut(ESimOne_LogLevel_Type::ESimOne_LogLevel_Type_Information, "SimOne Initialized!");
                isSimOneInitialized = true;
            }

            SSD::SimPoint3D mainVehiclePos(pGps->posX, pGps->posY, pGps->posZ);
            double mainVehicleSpeed = UtilMath::calculateSpeed(pGps->velX, pGps->velY, pGps->velZ);

            double minDistance = std::numeric_limits<double>::max();
            int potentialObstacleIndex = pObstacle->obstacleSize;
            SSD::SimString mainVehicleLaneId = SampleGetNearMostLane(mainVehiclePos);
            SSD::SimString potentialObstacleLaneId = "";
            for (size_t i = 0; i < pObstacle->obstacleSize; ++i) {
                SSD::SimPoint3D obstaclePos(pObstacle->obstacle[i].posX, pObstacle->obstacle[i].posY, pObstacle->obstacle[i].posZ);
                SSD::SimString obstacleLaneId = SampleGetNearMostLane(obstaclePos);
                if (mainVehicleLaneId == obstacleLaneId) {
                    double obstacleDistance = UtilMath::planarDistance(mainVehiclePos, obstaclePos);

                    if (obstacleDistance < minDistance) {
                        minDistance = obstacleDistance;
                        potentialObstacleIndex = (int)i;
                        potentialObstacleLaneId = obstacleLaneId;
                    }
                }
            }

            auto& potentialObstacle = pObstacle->obstacle[potentialObstacleIndex];
            double obstacleSpeed = UtilMath::calculateSpeed(potentialObstacle.velX, potentialObstacle.velY, potentialObstacle.velZ);

            SSD::SimPoint3D potentialObstaclePos(potentialObstacle.posX, potentialObstacle.posY, potentialObstacle.posZ);
            double sObstalce = 0;
            double tObstacle = 0;

            double sMainVehicle = 0;
            double tMainVehicle = 0;

            bool isObstalceBehind = false;
            bool ObstalceisGoing = false;
            if (!potentialObstacleLaneId.Empty()) {

                SampleGetLaneST(potentialObstacleLaneId, potentialObstaclePos, sObstalce, tObstacle);
                SampleGetLaneST(mainVehicleLaneId, mainVehiclePos, sMainVehicle, tMainVehicle);

                isObstalceBehind = !(sMainVehicle >= sObstalce);
                if (tMainVehicle - tObstacle > 1) {
                    ObstalceisGoing = true;
                }
            }

            std::unique_ptr<SimOne_Data_Control> pControl = std::make_unique<SimOne_Data_Control>();

            // Control mainVehicle with SimOneDriver
            SimOneAPI::GetDriverControl(MainVehicleId, pControl.get());

            // Control mainVehicle without SimOneDriver
            /*pControl->throttle = 0.5;
            pControl->brake = 0;
            pControl->steering = 0;
            pControl->handbrake = 0;
            pControl->isManualGear = 0;
            pControl->gear = static_cast<ESimOne_Gear_Mode>(1);*/

            if (isObstalceBehind && obstacleSpeed != 0) {
                double defaultDistance = 0.1f;
                double timeToCollision = std::abs((minDistance - defaultDistance)) / (obstacleSpeed - mainVehicleSpeed);
                double defautlTimeToCollision = 3.4f;//越大停的越远
                if (-timeToCollision < defautlTimeToCollision && timeToCollision < 0) {
                    inAEBState = true;

                }
                if (inAEBState) {
                    pControl->brake = 1.f;
                    pControl->throttle = 0.;

                    if (mainVehicleSpeed <= 0.5f || std::abs((minDistance - defaultDistance) - defaultDistance) <= 0) {
                        pControl->throttle = 0;
                        pControl->brake = 1;
                    }

                    if (ObstalceisGoing) {
                        pControl->throttle = 0.1f;
                        pControl->brake = 0;
                    }

                }

            }
            if (obstacleSpeed == 0) {
                if (isObstalceBehind) {
                    double defaultDistance = 0.1f;
                    double timeToCollision = std::abs((minDistance - defaultDistance)) / (obstacleSpeed - mainVehicleSpeed);
                    double defautlTimeToCollision = 1.3f;//越大停的越远
                    if (-timeToCollision < defautlTimeToCollision && timeToCollision < 0) {
                        inAEBState = true;

                    }
                    if (inAEBState) {
                        pControl->brake = 1.f;
                        pControl->throttle = 0.;

                        if (mainVehicleSpeed <= 0.5f || std::abs((minDistance - defaultDistance) - defaultDistance) <= 0) {
                            pControl->throttle = 0;
                            pControl->brake = 1;
                        }

                        if (ObstalceisGoing) {
                            pControl->throttle = 0.1f;
                            pControl->brake = 0;
                        }

                    }

                }
            }

            //if (45 < pGps->posX < -45) {
                //double defaultDistance = 2.f;
                //double timeToCollision = std::abs((minDistance - defaultDistance)) / (obstacleSpeed - mainVehicleSpeed);
                //double defautlTimeToCollision = 3.4f;
                //if (-timeToCollision < defautlTimeToCollision && timeToCollision < 0) {
                    //inAEBState = true;
                    //pControl->brake = 1;
                //}
                //if (mainVehicleSpeed <= 1.f) {
                    //pControl->throttle = 0;
                    //pControl->brake = 1;
                //}

            //}

        //}

            SimOneAPI::SetDrive(MainVehicleId, pControl.get());
        }
        else {
            SimOneAPI::SetLogOut(ESimOne_LogLevel_Type::ESimOne_LogLevel_Type_Information, "SimOne Initializing...");
        }

        SimOneAPI::NextFrame(frame);
    }
    return 0;
}