工程创新训练大赛智能网联车辆赛道LKA+自动驾驶9000+分数分案例

「文智助手」全场景办公AI助理AI+OPC全流程实践方案(大赛作品) 阅读详情
AI摘要

该博客展示了工程创新训练大赛智能网联车辆赛道LKA+自动驾驶案例的代码。代码包含多个头文件,实现了延时、获取障碍物车道ID、导航路径等功能,还涉及根据起点和终点获取途径路径、计算转向等操作,用于自动驾驶的控制。

#include "SimOneServiceAPI.h"
#include "SimOneSensorAPI.h"
#include "SimOneHDMapAPI.h"
#include "SSD/SimPoint3D.h"
#include "UtilDriver.h"
#include "UtilMath.h"
#include "SampleGetNearMostLane.h"
#include "SampleGetLaneST.h"
#include "SimOneEvaluationAPI.h"  //评价函数
#include <iostream>
#include <memory>
#include "SimOnePNCAPI.h"
#include <chrono>
#include <thread>
#include <fstream>
#include <vector>

using SSD::SimString;
using SSD::SimPoint3D;

using namespace HDMapStandalone;


SimOne_Data_Gps Gps = SimOne_Data_Gps();
SimOne_Data_CaseInfo pCaseInfoTest = SimOne_Data_CaseInfo();
SimOne_Data_WayPoints WayPoints;
SSD::SimPoint3DVector targetPath;
using SSD::SimPoint3D;
using SSD::SimString;


void delay_ms(int ms) {
    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}//延时函数
SSD::SimString GetObstacleLaneId(const SSD::SimPoint3D& obstaclePos) {

    return SampleGetNearMostLane(obstaclePos);
}

SSD::SimVector<long> GetNavigateRoadIdList(const SSD::SimPoint3D& startPt, const SSD::SimPoint3D& endPt)
{
    SSD::SimVector<long> naviRoadIdList;
    SSD::SimPoint3DVector ptList;
    ptList.push_back(startPt);
    ptList.push_back(endPt);
    SSD::SimVector<int> indexOfValidPoints;
    SimOneAPI::Navigate(ptList, indexOfValidPoints, naviRoadIdList);
    return std::move(naviRoadIdList);
}//输入起点和终点,返回所有途径的路径

bool GetValidSuccessor(const MLaneId& laneId, const long& currentRoadId, const long& nextRoadId, MLaneId& successor)
{
    if (nextRoadId == -1)//如果没有下一车道
    {
        return false;
    }
    MLaneLink laneLink;

    //Get connected road information
    bool valid = SimOneAPI::GetLaneLink(laneId.ToString(), laneLink);//获取车道链接信息
    //assert(valid);如果为假,则会终结程序

    //Check whether the current road has a successor road
    if (laneLink.successorLaneNameList.size() == 0)
    {
        return false;
    }

    //Traverse all the successor roads, find the RoadId of the successor road is the target road of the current road RoadId

    for (auto& successorLane : laneLink.successorLaneNameList)
    {

        MLaneId successorId(successorLane);//

        if (successorId.roadId != currentRoadId)
        {
            if (successorId.roadId == nextRoadId)
            {
                successor = successorLane;
                return true;
            }
        }
        else
        {
            //Recursive
            MLaneId successorOfSuccessor;
            if (GetValidSuccessor(successorId, currentRoadId, nextRoadId, successorOfSuccessor))
            {
                if (successorOfSuccessor.roadId == nextRoadId)
                {
                    successor = successorId;
                    return true;
                }

            }
        }
    }
    return false;
}
void AddSamples(const MLaneId& laneId, SSD::SimPoint3DVector& path)
{
    MLaneInfo laneInfo;
    //Get the centerline of the lane according to the road Id
    if (SimOneAPI::GetLaneSample(laneId.ToString(), laneInfo))
    {
        path.reserve(path.size() + laneInfo.centerLine.size());
        for (auto& pt : laneInfo.centerLine)
        {
            //Put the center point in the target path
            path.push_back(pt);
        }
    }
}


SSD::SimPoint3DVector GetReferencePath(const SSD::SimPoint3D& startPt, const SSD::SimVector<long>& naviRoadIdList)
{
    SSD::SimString obstacleLaneId = "";
    SSD::SimPoint3DVector path;
    double s, t, s_toCenterLine, t_toCenterLine;
    HDMapStandalone::MLaneInfo info;

    SSD::SimString laneName;

    //Get the lane information of the current main vehicle
    bool ret = SimOneAPI::GetNearMostLane(startPt, laneName, s, t, s_toCenterLine, t_toCenterLine);
    //assert(ret);
    MLaneId id(laneName);
    if (id.roadId != naviRoadIdList[0])
    {
        return std::move(path);
    }

    AddSamples(id, path);

    int index = 0;

    std::unique_ptr<SimOne_Data_Obstacle> pObstacle = std::make_unique<SimOne_Data_Obstacle>();
    for (size_t i = 0; i < pObstacle->obstacleSize; ++i) {
        SSD::SimPoint3D obstaclePos(pObstacle->obstacle[i].posX, pObstacle->obstacle[i].posY, pObstacle->obstacle[i].posZ);
        obstacleLaneId = SampleGetNearMostLane(obstaclePos);
    }


    //Get the centerline of all roads
    double sObstalce = 0;
    double tObstacle = 0;

    double sMainVehicle = 0;
    double tMainVehicle = 0;
    while (index < (int)naviRoadIdList.size())
    {
        long roadId = naviRoadIdList[index];
        long nextRoadId = -1, next_nextRoadId = -1;
        if (index + 1 < (int)naviRoadIdList.size()) {
            nextRoadId = naviRoadIdList[index + 1];
        }
        if (index + 2 < (int)naviRoadIdList.size()) {
            next_nextRoadId = naviRoadIdList[index + 2];
        }

        MLaneId successorId;
        bool foundValidSuccessor = false;
        while (!foundValidSuccessor)
        {
            foundValidSuccessor = GetValidSuccessor(id, roadId, nextRoadId, successorId);
            if (foundValidSuccessor)
            {
                SSD::SimString obstacleLaneId = "";
                std::unique_ptr<SimOne_Data_Obstacle> pObstacle = std::make_unique<SimOne_Data_Obstacle>();
                for (size_t j = 0; j < pObstacle->obstacleSize; ++j)
                {
                    SSD::SimPoint3D obstaclePos(pObstacle->obstacle[j].posX, pObstacle->obstacle[j].posY, pObstacle->obstacle[j].posZ);


                    obstacleLaneId = GetObstacleLaneId(obstaclePos);
                    if (obstacleLaneId == successorId.ToString())
                    {
                        SampleGetLaneST(obstacleLaneId, obstaclePos, sObstalce, tObstacle);
                        if (sObstalce < 1) {
                            foundValidSuccessor = false;
                            break;
                        }
                    }
                }
                if (!foundValidSuccessor)
                {
                    id = successorId;
                }
            }
            else
            {
                break;
            }
        }

        AddSamples(id, path);
        if (successorId.roadId != roadId)
        {
            index++;
        }
        id = successorId;
    }

    return std::move(path);
}

enum AVPStatus
{
    eApproaching,
    eForwarding,
    eBrakingPrep,
    eReversing,
    eFinishing,
    eWaiting,
    eWaiting2,
    eLeaving,
    eEnded,
    eUnknown,
    efinal,
    eshring,
    estright
};
static double PlanarDistance(const SSD::SimPoint3D& pt1, const SSD::SimPoint3D& pt2)
{
    return std::sqrt(std::pow(pt1.x - pt2.x, 2) + std::pow(pt1.y - pt2.y, 2));
}
//Main function
//
static void SetControl(const long long timestamp, const double throttle, const double brake,
    const double steering, const ESimOne_Gear_Mode gear = ESimOne_Gear_Mode::ESimOne_Gear_Mode_Drive,
    const char* mainVehicleId = "0")
{
    std::unique_ptr<SimOne_Data_Control> pControl = std::make_unique <SimOne_Data_Control>();

    pControl->timestamp = timestamp;
    pControl->throttle = (float)throttle;
    pControl->brake = (float)brake;
    pControl->steering = (float)steering;
    pControl->handbrake = false;
    pControl->isManualGear = false;
    pControl->gear = gear;
    SimOneAPI::SetDrive(mainVehicleId, pControl.get());
}


int main(int argc, char* argv[])
{
    const char* MainVehicleId = "0";// 初始车辆相关参数
    std::unique_ptr<SimOne_Data_Control> pControl = std::make_unique<SimOne_Data_Control>();

    std::unique_ptr<SimOne_Data_Gps> gpsPtr = std::make_unique<SimOne_Data_Gps>();// 初始化车辆GPS数据、障碍物数据和航点数据
    std::unique_ptr<SimOne_Data_Obstacle> obstaclesPtr = std::make_unique<SimOne_Data_Obstacle>();
    std::unique_ptr<SimOne_Data_WayPoints> wayPointsPtr = std::make_unique<SimOne_Data_WayPoints>();

    int a = 1;
    bool hasobstacle = false;
    bool injunction = false;
    bool inAEBState = false;
    int timeout = 20;
    bool isSimOneInitialized = false;
    SimOne_Data_Signal_Lights Lightdata;
    SSD::SimVector<long> GetJunctionList();
    bool leaveAfterParked = true;
    bool isJoinTimeLoop = true;

    SimOneAPI::InitSimOneAPI(MainVehicleId, isJoinTimeLoop);
    SimOneAPI::InitEvaluationServiceWithLocalData(MainVehicleId);
    SimOneAPI::SetDriverName(0, "LKA");
    SSD::SimPoint3D pointen;//场景27点
    pointen.x = -286.15;
    pointen.y = -13.81;
    pointen.z = 0;
    SSD::SimPoint3D dian2;//场景27二号点
    dian2.x = -275.0;
    dian2.y = -14;
    dian2.z = 0;
    SSD::SimPoint3D dian3;//场景30 33 34 35 36 
    dian3.x = 54.0;
    dian3.y = -11.0;
    dian3.z = 0;
    SSD::SimPoint3D dian4;//38
    dian4.x = -18.0;
    dian4.y = 2.;
    dian4.z = 0;
    SSD::SimPoint3D dian5;//12
    dian5.x = 23;
    dian5.y = -13;
    dian5.z = 0;
    SSD::SimPoint3D dian6;//17
    dian6.x = -475;
    dian6.y = -14;
    dian6.z = 0;
    SSD::SimPoint3D dian7;//18 20
    dian7.x = -23;
    dian7.y = 2;
    dian7.z = 0;
    SSD::SimPoint3D dian8;// 25
    dian8.x = -23.5;
    dian8.y = -4.75;
    dian8.z = 0;
    SSD::SimPoint3D dian9;//31
    dian9.x = -39;
    dian9.y = 2.3;
    dian9.z = 0;
    SSD::SimPoint3D dian10;//32
    dian10.x = -19.7;
    dian10.y = 2;
    dian10.z = 0;
    SSD::SimPoint3D dian11;//37
    dian11.x = 52;
    dian11.y = -11;
    dian11.z = 0;
    SSD::SimPoint3D dian12;//场景37
    dian12.x = -24;
    dian12.y = 2;
    dian12.z = 0;
    SSD::SimPoint3D dian13;//场景21
    dian13.x = -220;
    dian13.y = -17;
    dian13.z = 0;
    SSD::SimPoint3D dian14;//场景11
    dian14.x = 3;
    dian14.y = -4.5;
    dian14.z = 0;
    SSD::SimPoint3D dian39;//场景39
    dian39.x = -42;
    dian39.y = 1.8;
    dian39.z = 0;
    SSD::SimPoint3D dian40;//场景39
    dian40.x = 26.8;
    dian40.y = -4.2;
    dian40.z = 0;

    SSD::SimPoint3D change_point;//场景21
    change_point.x = -201.125;
    change_point.y = -13.31;
    change_point.z = 0;
    SSD::SimPoint3D lianxu;//场景21
    lianxu.x = 18.5;
    lianxu.y = 303;
    lianxu.z = 0;
    SSD::SimPoint3D lianxu2;//场景21
    lianxu2.x = 46;
    lianxu2.y = 167;
    lianxu2.z = 0;
    ////21车道id
    //SSD::SimString ID_21;
    //21 id信息

    SSD::SimPoint3DVector inputPoints_21;
    SSD::SimPoint3D point21_1 = { -218.78, -13.75, 0.0 };
    inputPoints_21.push_back(point21_1);

    SSD::SimPoint3D point21_2 = { -210.25, -13.75, 0.0 };
    inputPoints_21.push_back(point21_2);

    SSD::SimPoint3D point21_3 = { -204.75, -13.75, 0.0 };
    inputPoints_21.push_back(point21_3);

    SSD::SimPoint3D point21_4 = { -201., -13.31, 0.0 };
    inputPoints_21.push_back(point21_4);
    SSD::SimPoint3DVector targetPath_21;
    SSD::SimVector<int> indexOfValidPoints_21;
    //连续场景
    SSD::SimPoint3DVector inputPoints_2;
    SSD::SimPoint3D point2_1 = { -3.92, 420.04, 0.0 };
    inputPoints_2.push_back(point2_1);
    SSD::SimPoint3D point2_1_1 = { -23,45.8125, 0.0 };
    inputPoints_2.push_back(point2_1_1);
    SSD::SimPoint3D point2_2 = { -3.92, -30.78, 0.0 };
    inputPoints_2.push_back(point2_2);
    SSD::SimPoint3D point2_3 = { 1.93, -110.9, 0.0 };
    inputPoints_2.push_back(point2_3);
    SSD::SimPoint3D point2_4 = { -38.5, -151.60, 0.0 };
    inputPoints_2.push_back(point2_4);
    SSD::SimPoint3D point2_5 = { -51.4, -159.16, 0.0 };
    inputPoints_2.push_back(point2_5);
    SSD::SimPoint3D point2_6 = { -66.15, -196.17, 0.0 };
    inputPoints_2.push_back(point2_6);
    SSD::SimPoint3D point2_7 = { -51.35, -262.29, 0.0 };
    inputPoints_2.push_back(point2_7);
    SSD::SimPoint3D point2_8 = { 6.69, -282.16, 0.0 };
    inputPoints_2.push_back(point2_8);
    SSD::SimPoint3D point2_9 = { 81.89, -293.28, 0.0 };
    inputPoints_2.push_back(point2_9);
    SSD::SimPoint3D point2_10 = { 176.83, -307.35, 0.0 };
    inputPoints_2.push_back(point2_10);
    SSD::SimPoint3D point2_11 = { 290.09, -306.58, 0.0 };
    inputPoints_2.push_back(point2_11);
    SSD::SimPoint3D point2_12 = { 325.93, -269.28, 0.0 };
    inputPoints_2.push_back(point2_12);
    SSD::SimPoint3D point2_13 = { 304.63,-119.85, 0.0 };
    inputPoints_2.push_back(point2_13);
    SSD::SimPoint3D point2_14 = { 246.01, 40.29, 0.0 };
    inputPoints_2.push_back(point2_14);
    SSD::SimPoint3D point2_15 = { 232.75, 72.625, 0.0 };
    inputPoints_2.push_back(point2_15);
    SSD::SimPoint3D point2_16 = { 213, 172.25, 0.0 };
    inputPoints_2.push_back(point2_16);
    SSD::SimPoint3D point2_17 = { 213.05, 190.3, 0.0 };
    inputPoints_2.push_back(point2_17);
    SSD::SimPoint3D point2_18 = { 184.91, 330.03, 0.0 };
    inputPoints_2.push_back(point2_18);
    SSD::SimPoint3D point2_19 = { 177.28, 389.83, 0.0 };
    inputPoints_2.push_back(point2_19);
    SSD::SimPoint3D point2_20 = { 103.76, 433.42, 0.0 };
    inputPoints_2.push_back(point2_20);
    SSD::SimPoint3D point2_21 = { 45.59, 382.29, 0.0 };
    inputPoints_2.push_back(point2_21);
    SSD::SimPoint3D point2_22 = { 49.68, 313.25, 0.0 };
    inputPoints_2.push_back(point2_22);
    SSD::SimPoint3D point2_23 = { 69.95, 256.14, 0.0 };
    inputPoints_2.push_back(point2_23);
    /*/
    SSD::SimPoint3D point2_24 = { 74.24, 210.2, 0.0 };
    inputPoints_2.push_back(point2_24);
    SSD::SimPoint3D point2_25 = { 93.61, 114.02, 0.0 };
    inputPoints_2.push_back(point2_25);
    SSD::SimPoint3D point2_26 = { 106.72, 48.79, 0.0 };
    inputPoints_2.push_back(point2_26);
    /*/
    SSD::SimPoint3D point2_27 = { 112.87, -0.87, 0.0 };
    inputPoints_2.push_back(point2_27);
    SSD::SimPoint3D point2_28 = { 133.81, -104.12, 0.0 };
    inputPoints_2.push_back(point2_28);
    SSD::SimPoint3D point2_29 = { 145.71, -146.09, 0.0 };
    inputPoints_2.push_back(point2_29);
    SSD::SimPoint3D point2_30 = { 156.54, -199.96, 0.0 };
    inputPoints_2.push_back(point2_30);
    SSD::SimPoint3D point2_31 = { 189.27, -294.95, 0.0 };
    inputPoints_2.push_back(point2_31);
    SSD::SimPoint3D point2_32 = { 257.43, -309.15, 0.0 };
    inputPoints_2.push_back(point2_32);
    SSD::SimPoint3D point2_33 = { 304.48, -239.84, 0.0 };
    inputPoints_2.push_back(point2_33);
    SSD::SimPoint3D point2_34 = { 252.47, -180.36, 0.0 };
    inputPoints_2.push_back(point2_34);
    SSD::SimPoint3D point2_35 = { 220.58, -113.8, 0.0 };
    inputPoints_2.push_back(point2_35);
    SSD::SimPoint3D point2_36 = { 183.7, -62.44, 0.0 };
    inputPoints_2.push_back(point2_36);
    SSD::SimPoint3D point2_37 = { 189.51, 24.68, 0.0 };
    inputPoints_2.push_back(point2_37);
    SSD::SimPoint3D point2_38 = { 120.55, 86.29, 0.0 };
    inputPoints_2.push_back(point2_38);
    SSD::SimPoint3D point2_39 = { 198.09, 169.35, 0.0 };
    inputPoints_2.push_back(point2_39);
    SSD::SimPoint3D point2_40 = { 99.1, 185.71, 0.0 };
    inputPoints_2.push_back(point2_40);
    /*
    SSD::SimPoint3D point2_25 = { 189.27, -294.95, 0.0 };
    inputPoints_2.push_back(point2_25);
    SSD::SimPoint3D point2_26 = { 304.48, -239.84, 0.0 };
    inputPoints_2.push_back(point2_26);
    SSD::SimPoint3D point2_27 = { 252.47, -180.36, 0.0 };
    inputPoints_2.push_back(point2_27);
    SSD::SimPoint3D point2_28 = { 220.58, -113.8, 0.0 };
    inputPoints_2.push_back(point2_28);
    SSD::SimPoint3D point2_29 = { 183.7, -62.44, 0.0 };
    inputPoints_2.push_back(point2_29);
    SSD::SimPoint3D point2_30 = { 189.51, 24.68, 0.0 };
    inputPoints_2.push_back(point2_30);
    SSD::SimPoint3D point2_31 = { 120.55, 86.29, 0.0 };
    inputPoints_2.push_back(point2_31);
    SSD::SimPoint3D point2_32 = { 198.09, 169.35, 0.0 };
    inputPoints_2.push_back(point2_32);
    SSD::SimPoint3D point2_f = { 99.1, 185.71, 0.0 };
    inputPoints_2.push_back(point2_f);*/
    SSD::SimPoint3DVector targetPath_2;
    SSD::SimVector<int> indexOfValidPoints_2;
    //----
    int timeoutSecond = 20;
    if (!SimOneAPI::LoadHDMap(timeoutSecond))
    {
        std::cout << "Failed to load map!" << std::endl;
        return 0;
    }

    SSD::SimPoint3D startPoint;
    if (SimOneAPI::GetGps(0, gpsPtr.get()))
    {
        startPoint.x = gpsPtr->posX;
        startPoint.y = gpsPtr->posY;
        startPoint.z = gpsPtr->posZ;
    }
    else
    {
        std::cout << "Fetch GPS failed" << std::endl;
    }
    /* 5. Get terminal point */
    /* 5. Get all waypoints */
    while (!SimOneAPI::GetWayPoints(MainVehicleId, wayPointsPtr.get()) || wayPointsPtr->wayPointsSize < 2)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
    // 确保至少有2个航点时才继续
    SSD::SimPoint3D terminalPoint{ lastWayPoint.posX, lastWayPoint.posY, 0. };
    lastWayPoint.posX = abs(lastWayPoint.posX);
    std::cout << "终点路径:" << std::endl;
    std::cout << "X: " << lastWayPoint.posX << std::endl;
    std::cout << "Y: " << lastWayPoint.posY << std::endl;
    /* 7. Control */
    AVPStatus status = AVPStatus::eForwarding;

    bool brakingPrepStarted = false;// 是否已开始制动准备
    bool reversingStarted = false;// 是否已开始倒车
    bool finishingStarted = false;// 是否已开始完成停车
    bool waitingStarted = false;// 是否已开始等待
    double brakingPrepTime = 1.;// 制动准备时间
    double waitingTimeSecond = 23.;// 等待时间
    double waitingTimeSecond38 = 2.5;// 等待时间12
    double waitingTimeSecond10 = 10;
    double waitingTimeSecond11 = 17;//场景37 17s
    double waitingTimeSecond12 = 4;//场景25 4s
    double waitingTimeSecond13 = 2;//场景21 2s
    auto startTime = std::chrono::system_clock::now();// 开始时间
    double headingErrorThresholdDegree = 0.5;// 方向偏差阈值
    double parkingEndDistanceThreshold = 1;// 停车结束距离阈值
    double leavingEndDistanceThreshold = 1;// 离开结束距离阈值
    std::unique_ptr<SimOne_Data_Gps> pGps = std::make_unique<SimOne_Data_Gps>();
    SSD::SimPoint3D mainVehiclePos(pGps->posX, pGps->posY, pGps->posZ);
    //输入点的容器
    std::cout << "gpsX" << startPoint.x << std::endl;
    SSD::SimPoint3DVector inputPoints;
    std::unique_ptr<SimOne_Data_WayPoints> pWayPoints = std::make_unique<SimOne_Data_WayPoints>();
    if (SimOneAPI::GetWayPoints(MainVehicleId, pWayPoints.get()) && pWayPoints->wayPointsSize >= 2)
    {
        inputPoints.reserve(pWayPoints->wayPointsSize);
        for (size_t i = 0; i < pWayPoints->wayPointsSize; ++i) {
            // 添加z坐标处理
            SSD::SimPoint3D inputWayPoints(
                pWayPoints->wayPoints[i].posX,
                pWayPoints->wayPoints[i].posY,
                pWayPoints->wayPoints[i].posZ  // 添加z坐标
            );
            inputPoints.push_back(inputWayPoints);
        }
    }
    else {
        SimOneAPI::SetLogOut(ESimOne_LogLevel_Type::ESimOne_LogLevel_Type_Error, "Get mainVehicle wayPoints failed");
        return -1;
    }

    SSD::SimPoint3DVector targetPath;
    SSD::SimPoint3DVector ChangeLanetargetPath;
    SSD::SimPoint3DVector LinktargetPath;
    SSD::SimPoint3DVector MidtargetPath;//中间变量
    if (inputPoints.size() >= 2) {  // 使用实际获取到的点数判断
        SSD::SimVector<int> indexOfValidPoints;
        if (!SimOneAPI::GenerateRoute(inputPoints, indexOfValidPoints, targetPath)) {
            SimOneAPI::SetLogOut(ESimOne_LogLevel_Type::ESimOne_LogLevel_Type_Error, "Generate mainVehicle route failed");
            return -1;
        }

    }
    else if (pWayPoints->wayPointsSize == 1) {
        SSD::SimString laneIdInit = SampleGetNearMostLane(inputPoints[0]);
        HDMapStandalone::MLaneInfo laneInfoInit;
        if (!SimOneAPI::GetLaneSample(laneIdInit, laneInfoInit)) {
            SimOneAPI::SetLogOut(ESimOne_LogLevel_Type::ESimOne_LogLevel_Type_Error, "Generate mainVehicle initial route failed");
            return -1;
        }
        else {
            targetPath = laneInfoInit.centerLine;

        }


    }

    else {
        SimOneAPI::SetLogOut(ESimOne_LogLevel_Type::ESimOne_LogLevel_Type_Error, "Number of wayPoints is zero");
        return -1;
    }
    while (1)
    {
        int frame = SimOneAPI::Wait();

        if (SimOneAPI::GetCaseRunStatus() == ESimOne_Case_Status::ESimOne_Case_Status_Stop) {
            SimOneAPI::SaveEvaluationRecord();//保存评价函数的信息
            break;
        }

        if (!SimOneAPI::GetGps(MainVehicleId, gpsPtr.get()))
        {
            std::cout << "Fetch GPS failed" << std::endl;;
        }
        //获取GPS信息
        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");
        }
#if AVP_LOG
        AVPLog::getInstance().addData(gpsPtr->posX);
        AVPLog::getInstance().addData(gpsPtr->posY);
#endif // AVP_LOG
        //场景11
        if (lastWayPoint.posX >= 50 && lastWayPoint.posX < 51) {
            SimOne_Data_Signal_Lights signal_lights;
            std::cout << "场景11" << std::endl;
            if (status == AVPStatus::eForwarding) // approaching the parking space接近阶段
            {
                if (PlanarDistance({ gpsPtr->posX, gpsPtr->posY, gpsPtr->posZ }, dian14) < 7)
                {
                    SetControl(gpsPtr->timestamp, 0., 1., 0., ESimOne_Gear_Mode_Neutral);
                    status = AVPStatus::eBrakingPrep;
                    std::cout << "到达指定点!" << std::endl;
                }
                else {
                    signal_lights.signalLights = ESimOne_Signal_Light_DoubleFlash;
                    SimOneAPI::SetSignalLights(MainVehicleId, &signal_lights);
                    SetControl(gpsPtr->timestamp, 0.01, 0., 0);
                }
            }

        }
        //场景27
        else if (lastWayPoint.posX >= 223 && lastWayPoint.posX < 224) {
            std::cout << "场景27" << std::endl;
            if (status == AVPStatus::eForwarding) // approaching the parking space接近阶段
            {
                if (PlanarDistance({ gpsPtr->posX, gpsPtr->posY, gpsPtr->posZ }, pointen) < 7)
                {
                    SetControl(gpsPtr->timestamp, 0., 1., 0., ESimOne_Gear_Mode_Neutral);
                    status = AVPStatus::eBrakingPrep;
                    std::cout << "到达指定点!" << std::endl;
                }
                else {
                    SetControl(gpsPtr->timestamp, 0.01, 0., -0.05);
                }
            }
            else if (status == AVPStatus::eBrakingPrep) // coming to a full stop
            {
                if (PlanarDistance({ gpsPtr->posX, gpsPtr->posY, gpsPtr->posZ }, dian2) < 7)
                {
                    SetControl(gpsPtr->timestamp, 0., 1., 0., ESimOne_Gear_Mode_Neutral);
                    status = AVPStatus::efinal;
                    std::cout << "到达2号指定点!" << std::endl;
                }
                else {
                    SetControl(gpsPtr->timestamp, 0.01, 0., 0.2);
                }
            }

            else if (status == AVPStatus::efinal)
            {
                //double endDistance = UtilMath::PlanarDistance({ gpsPtr->posX, gpsPtr->posY, 0. }, terminalPoint);
                if (PlanarDistance({ gpsPtr->posX, gpsPtr->posY, gpsPtr->posZ }, terminalPoint) < 3)
                {
                    status = AVPStatus::efinal;
                    SetControl(gpsPtr->timestamp, 0, 1, 0., ESimOne_Gear_Mode_Neutral);
                }
                else
                {
                    SetControl(gpsPtr->timestamp, 1, 0., -0.04);
                }
            }
        }
        //场景30 33 35 36 34
        else if (lastWayPoint.posX >= 70 && lastWayPoint.posX < 71) {
            std::cout << "场景30 33 35 36" << std::endl;

            if (status == AVPStatus::eForwarding) // approaching the parking space接近阶段
            {
                if (PlanarDistance({ gpsPtr->posX, gpsPtr->posY, gpsPtr->posZ }, dian3) < 7)
                {
                    SetControl(gpsPtr->timestamp, 0., 1., 0., ESimOne_Gear_Mode_Neutral);
                    status = AVPStatus::eWaiting;
                    std::cout << "到达指定点!" << std::endl;
                }
                else {
                    SetControl(gpsPtr->timestamp, 0.01, 0., 0);
                }
            }

            else if (status == AVPStatus::eWaiting)
            {
                if (!waitingStarted)
                {
                    startTime = std::chrono::system_clock::now();
                    waitingStarted = true;
                }
                else
                {
                    // wait for a total of waitingTimeSecond
                    std::chrono::duration<double> currentWaitingTimeSecond = std::chrono::system_clock::now() - startTime;
                    if (currentWaitingTimeSecond.count() > waitingTimeSecond)
                    {
                        status = AVPStatus::eLeaving;
                        std::cout << "Finished waiting for " << waitingTimeSecond << " second!" << std::endl;
                    }
                }
                SetControl(gpsPtr->timestamp, 0., 1., 0., ESimOne_Gear_Mode_Neutral);
            }
            else if (status == AVPStatus::eLeaving)
            {
                ////double endDistance = UtilMath::PlanarDistance({ gpsPtr->posX, gpsPtr->posY, 0. }, planner.LeavingTrajectory().back());
                //if (PlanarDistance({ gpsPtr->posX, gpsPtr->posY, gpsPtr->posZ }, terminalPoint) < 1)
                //{
                //    status = AVPStatus::eEnded;
                //    std::cout << "Finished leaving!" << std::endl;
                //    SetControl(gpsPtr->timestamp, 0., 1., 0., ESimOne_Gear_Mode_Neutral);
                //}
                /*else*/

                SSD::SimPoint3DVector inputPoints;
                //存储车道信息的指针
                std::unique_ptr<SimOne_Data_WayPoints> pWayPoints = std::make_unique<SimOne_Data_WayPoints>();

                //主车路点数据
                if (SimOneAPI::GetWayPoints(MainVehicleId, pWayPoints.get()))
                {
                    // 提取主车道点的坐标并添加到输入点容器中
                    for (size_t i = 0; i < pWayPoints->wayPointsSize; ++i) {
                        SSD::SimPoint3D inputWayPoints(pWayPoints->wayPoints[i].posX, pWayPoints->wayPoints[i].posY, 0);
                        inputPoints.push_back(inputWayPoints);
                    }
                }
                else {
                    SimOneAPI::SetLogOut(ESimOne_LogLevel_Type::ESimOne_LogLevel_Type_Error, "Get mainVehicle wayPoints failed");
                    return -1;
                }

                //目标路径的容器
                SSD::SimPoint3DVector targetPath;
                if (pWayPoints->wayPointsSize >= 2) {
                    SSD::SimVector<int> indexOfValidPoints;

                    //生成主车路线
                    if (!SimOneAPI::GenerateRoute(inputPoints, indexOfValidPoints, targetPath)) {
                        SimOneAPI::SetLogOut(ESimOne_LogLevel_Type::ESimOne_LogLevel_Type_Error, "Generate mainVehicle route failed");
                        return -1;
                    }
                }
                else if (pWayPoints->wayPointsSize == 1) {
                    //只有一个路点,获取最近的车道并初始化路线
                    //inputPoints[0] = SSD::SimPoint3D(gpsPtr->posX, gpsPtr->posY, gpsPtr->posZ);
                    SSD::SimString laneIdInit = SampleGetNearMostLane(inputPoints[0]);
                    HDMapStandalone::MLaneInfo laneInfoInit;
                    if (!SimOneAPI::GetLaneSample(laneIdInit, laneInfoInit)) {
                        SimOneAPI::SetLogOut(ESimOne_LogLevel_Type::ESimOne_LogLevel_Type_Error, "Generate mainVehicle ini

SRAM vs RRAM:存算一体芯片选型指南(含最新技术对比) 本文深入对比了SRAM与RRAM在存算一体芯片设计中的核心差异,为硬件工程师和架构师提供实战选型指南。文章从技术本质、性能、功耗、成本及设计风险等多维度剖析,指出SRAM基于成熟CMOS工艺,适合高精度、低延迟场景;而RRAM凭借非易失性和高密度潜力,在超低功耗、固定模型应用中优势明显。决策需综合项目预算、上市时间及技术储备。 阅读详情

相关推荐

别再让CPU干GPU的活了:CST Studio Suite GPU加速深度调优指南(从驱动版本到任务管理器监控)

本文深入探讨了CST Studio Suite的GPU加速深度调优方法,从NVIDIA驱动版本选择到系统级资源管理,再到实时监控与瓶颈诊断技术。通过科学的驱动优化、GPU资源管理策略和高级调优技巧,显著提升仿真效率,特别适合电磁仿真工程师和硬件加速需求的专业用户。

weixin_28740299的博客 169

逆向Shopee签名机制:从参数分析到稳定采集方案

本文深入解析了Shopee签名机制的逆向过程,从参数分析到稳定采集方案的实现。通过抓包定位关键参数`sap-ri`和`x-sap-sec`,还原了签名生成算法,并提供了Python实现代码。文章还分享了反反爬策略、性能优化技巧及长期维护建议,帮助开发者构建稳定的Shopee数据采集方案。

weixin_27722377的博客 679

工业视觉降本300%实战:C# + 开源YOLO如何干掉Halcon/VisionPro?

在当前制造业普遍追求“降本增效”的大背景下,工业视觉检测系统作为产线的眼睛,其成本问题日益突出。通过该方案,我们成功帮助一家电子厂将单条产线的视觉检测成本从15万降低至不足5千(主要为工控机硬件成本),降本幅度超过95%,且系统稳定性经受住了6个月连续生产的考验。:C#负责“工程化”和“业务逻辑”,YOLO负责“智能算法”,二者结合,完美覆盖了工业视觉从算法到落地的全链路。的组合,构建一套高性能、低成本、易维护的工业视觉解决方案,并彻底替代昂贵的付费库。” 这是一个典型的误区。工业相机/USB相机。

2601_94871597的博客 431

【速通RAG实战:索引】4.RAG文档解析技术

通过本文技术方案,可构建高精准、高效率的文档解析流水线,为 RAG 应用提供坚实的知识底座。核心在于**格式解析的鲁棒性**和**语义分块的准确性**,后续通过持续的性能优化和质量监控,逐步提升索引质量和检索效率。

2025博客之星Top81。专注AI工程化与架构实战。从分布式思维到模型部署,用工程化视角为你厘清AI落地的真实路径。 1548

YOLOv11改进 | Conv改进篇 | 含2种改进,引入DynamicConv替换普通Conv卷积下采样和C3k2_DynamicConv替换C3k2(来自CVPR2024)

本文提出了一种基于DynamicConv动态卷积优化的YOLOv11轻量化模型改进方案。通过引入多专家机制的动态卷积模块,在增加模型参数量的同时保持较低的计算复杂度(FLOPs),有效解决了低FLOPs模型在大规模预训练中的性能瓶颈问题。文章详细介绍了动态卷积的工作原理、核心代码实现,并提供了在YOLOv11中集成DynamicConv和C3k2_GhostModule_DynamicConv模块的完整教程,包括模块添加、配置文件修改和模型训练验证过程。实验结果表明,该方案在保持高效推理速度的同时显著提升了

Ai缝合怪、助力小伙伴高效发论文 540

基于Java+SpringBoot+Vue的热门网游推荐系统设计与实现

本文详细介绍了基于Java+SpringBoot+Vue技术栈的热门网游推荐系统的设计与实现过程。系统采用前后端分离架构,后端使用SpringBoot构建RESTful API,前端使用Vue.js框架实现用户界面,结合MySQL数据库进行数据存储。系统实现了用户管理、游戏信息管理、个性化推荐、评论互动等核心功能,并采用了协同过滤推荐算法为用户提供个性化的游戏推荐。本文将从需求分析、系统设计、技术选型、核心功能实现以及系统测试等方面进行全面阐述。关键词:Java;SpringBoot;Vue.js;

2201_76125393的博客 76

OpenClaw实战系列02:企业微信接入完全指南

本文详细介绍了如何将OpenClaw与企业微信对接,实现智能机器人功能。主要内容包括:企业微信侧创建智能机器人或自建应用,获取必要凭证和权限;OpenClaw侧安装配置企业微信插件,设置通信通道;完成双向验证并进行功能测试。文章提供了详细的配置步骤、常见问题排查方法和进阶玩法,帮助用户在30分钟内完成对接。关键点包括:确保URL可访问、Token一致、网关正常运行,以及针对不同部署环境(云服务器/内网)的配置差异。

PeterPan的博客 3817

AD22等长布线规则

本文介绍了PCB布线中实现信号线等长的详细操作步骤:1.创建网络类(NetClass),将需要等长的信号线归入同一组;2.设置高速布线规则(High-SpeedRule),定义等长容差等参数;3.进行交互式等长布线(InteractiveLengthTuning),使用蛇形线调整线长;4.实时调整与最终检查,确保所有网络满足等长要求。文中还提供了实用技巧,如使用快捷键调整蛇形线参数、为关键信号设置更严格规则等。操作需在PCB设计软件中完成,通过统一管理、规则设置和精确调整来实现信号同步传输。

whm128的博客 1564

迈瑞BS系列全自动生化分析仪LIS协议接口手册(内含HL7和ASTM)

迈瑞BS系列全自动生化分析仪LIS协议接口手册,内含HL7和ASTM)

复分析 阿尔福斯(Ahlfors) 第三版答案 英文

答案分为十章,有部分习题没有答案,全英文复分析阿尔福斯(Ahlfors) 第三版答案

IMSL CNL 7.0.0 (x86-64)Crack

IMSL CNL 7.0.0 (x86-64)Crack intel的算法库破解文件,这是C++版本的破解文件

数学建模模型总结

这是本人在两年内准备参加数学建模竞赛搜集到的模型资料,已经经过了本人的筛选和整理,希望能够对准备参加数学建模竞赛的学弟学妹们提供一些帮助!

上一篇: 工程创新大赛智能网联汽车设计赛项经验帖——debug篇
下一篇: 工程创新训练大赛智能网联车辆赛道AVP将近完美的案例AVPlaner部分
静水深流deepl
博客等级 码龄3年 230粉丝 6原创
评论 5
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付:10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值