工创智能网联竞赛代码

这是完整代码一部分
传感器与路径感知
#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 <vector>
#include <algorithm>
using namespace std;
// 定义一些常量和变量
const double MAX_SPEED = 10.0; // 最大速度
const double MIN_DISTANCE = 5.0; // 最小安全距离
vector<SSD::SimPoint3D> pathPoints; // 路径点
SSD::SimString currentLaneId; // 当前车道ID
// 初始化仿真平台
void initSimOne() {
    const char* MainVehicleId = "0";
    bool isJoinTimeLoop = true;
    SimOneAPI::InitSimOneAPI(MainVehicleId, isJoinTimeLoop);
    SimOneAPI::SetDriverName(MainVehicleId, "MyCar");
    SimOneAPI::InitEvaluationServiceWithLocalData(MainVehicleId);
}
// 获取路径点
void getPathPoints() {
    std::unique_ptr<SimOne_Data_WayPoints> pWayPoints = std::make_unique<SimOne_Data_WayPoints>();
    if (SimOneAPI::GetWayPoints("0", pWayPoints.get())) {
        for (size_t i = 0; i < pWayPoints->wayPointsSize; ++i) {
            SSD::SimPoint3D point(pWayPoints->wayPoints[i].posX, pWayPoints->wayPoints[i].posY, 0);
            pathPoints.push_back(point);
        }
    } else {
        SimOneAPI::SetLogOut(ESimOne_LogLevel_Type::ESimOne_LogLevel_Type_Error, "Get wayPoints failed");
    }
}
// 生成路径
void generatePath() {
    SSD::SimVector<int> validPointsIndex;
    if (!SimOneAPI::GenerateRoute(pathPoints, validPointsIndex, targetPath)) {
        SimOneAPI::SetLogOut(ESimOne_LogLevel_Type::ESimOne_LogLevel_Type_Error, "Generate route failed");
    }
}
// 获取当前车道ID
void getCurrentLaneId(SSD::SimPoint3D position) {
    currentLaneId = SampleGetNearMostLane(position);
}
// 障碍物检测
void detectObstacles(SimOne_Data_Obstacle* pObstacle, SSD::SimPoint3D egoPosition) {
    for (size_t i = 0; i < pObstacle->obstacleSize; ++i) {
        SSD::SimPoint3D obstaclePosition(pObstacle->obstacle[i].posX, pObstacle->obstacle[i].posY, pObstacle->obstacle[i].posZ);
        double distance = UtilMath::planarDistance(egoPosition, obstaclePosition);
        if (distance < MIN_DISTANCE) {
            // 障碍物太近，需要避让
            handleObstacleAvoidance(obstaclePosition);
        }
    }
}
// 障碍物避让
void handleObstacleAvoidance(SSD::SimPoint3D obstaclePosition) {
    // 计算避让路径
    vector<SSD::SimPoint3D> avoidancePath;
    // ...
    // 更新目标路径
    targetPath = avoidancePath;
}
// 路径规划
void planPath() {
    // 使用A*算法或其他路径规划算法
    // ...
}
// 车辆控制
void controlVehicle(SimOne_Data_Gps* pGps, SimOne_Data_Control* pControl) {
    SSD::SimPoint3D egoPosition(pGps->posX, pGps->posY, pGps->posZ);
    double speed = UtilMath::calculateSpeed(pGps->velX, pGps->velY, pGps->velZ);
    // 计算转向角度
    double steeringAngle = UtilDriver::calculateSteering(targetPath, pGps);
    // 计算油门和刹车
    double throttle = 0.0;
    double brake = 0.0;
    if (speed < MAX_SPEED) {
        throttle = 0.2;
    } else {
        brake = 0.1;
    }
    // 设置控制指令
    pControl->steering = (float)steeringAngle;
    pControl->throttle = (float)throttle;
    pControl->brake = (float)brake;
}
// 主循环
int main() {
    initSimOne();
    getPathPoints();
    generatePath();
    while (true) {
        int frame = SimOneAPI::
————————————————