#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
169



被折叠的 条评论
为什么被折叠?



