技术架构¶
3.1 技术架构图¶
3.2 自动驾驶算法联调的基本流程¶
以 Apollo 为例，可以通过设计一个 bridge 程序 CybertronBridgeApollo，控制 Apollo 算法的启停，自动设置部分 Apollo 参数，结合 SimOne 案例集，达到自动化测试的目的。
CybertronBridgeApollo 桥接程序的主要工作包括两部分：准备加载的信息和实时传递的数据。
（1）控制流：
-
在案例运行之前，必须加载某些信息，例如车辆的具体参数、相机配置参数、地图等。这类信息随着案例的不同或主车设置的变化而变化，但在案例运行过程中则保持不变。
-
这类信息被称作“控制流”，它们需要在案例启动之前预先设置好并传递给 Apollo。
（2）数据流：
-
案例运行时会实时产生另一些数据，例如 GPS 信息、主车底盘信息、图像及点云数据。
-
这类信息被称作“数据流”，需要不断实时地传递给 Apollo 算法进行处理。
通过这种方式，CybertronBridgeApollo 能够确保 Apollo 自动驾驶平台在测试案例运行时接收到所需的所有必要信息和实时数据，以确保正确地执行自动化测试。具体实现流程如下：
3.3 SimOne 支持的算法联调场景¶
3.3.1 只接入决策规划¶
- 示例伪代码: C++
// 主车Id
const char* mv_id = "0";
// 是否加入帧同步
bool isJoinTimeLoop = false;
// BridgeIO 服务 Ip
const char* serverIP = "127.0.0.1";
// 初始化 SimOneAPI
InitSimOneAPI("0", isJoinTimeLoop, serverIP);
// 回调方式获取底盘等gps数据
if (IsCallBackMode)
{
auto function = [](const char* mainVehicleId, SimOne_Data_Gps *pGps){
output_gps(pGps);
};
SetGpsUpdateCB(function);
}
// 非帧同步方式获取底盘等gps数据
else
{
std::unique_ptr<SimOne_Data_Gps> pGps = std::make_unique<SimOne_Data_Gps>();
int lastFrame = 0;
while(true)
{
bool flag = GetGps(mv_id, pGps.get());
if (flag && pGps->frame != lastFrame)
{
lastFrame = pGps->frame;
output_gps(pGps.get());
}
if (!flag)
{
std::cout<<"Get GPS Fail"<< std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
// 获取案例结束状态
if (SimOneAPI::GetCaseRunStatus() == ESimOne_Case_Status::ESimOne_Case_Status_Stop)
// 关闭 SimOneAPI
if (!SimOneAPI::TerminateSimOneAPI())
{
std::cout << "TerminateSimOneAPI Failed!" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
}
// GPS 数据：
void output_gps(SimOne_Data_Gps* pData)
{
std::cout<<"frame:"<< pData->frame << std::endl;
std::cout << "posX/Y/Z: [" << pData->posX << ", " << pData->posY << ", " << pData->posZ << "]" << std::endl;
std::cout << "oriX/Y/Z: [" << pData->oriX << ", " << pData->oriY << ", " << pData->oriZ << "]" << std::endl;
std::cout << "velX/Y/Z: [" << pData->velX << ", " << pData->velY << ", " << pData->velZ << "]" << std::endl;
std::cout << "throttle: " << pData->throttle << std::endl;
std::cout << "brake: " << pData->brake << std::endl;
std::cout << "steering: " << pData->steering << std::endl;
std::cout << "gear: " << pData->gear << std::endl;
std::cout << "accelX/Y/Z: [" << pData->accelX << ", " << pData->accelY << ", " << pData->accelZ << "]" << std::endl;
std::cout << "angVelX/Y/Z: [" << pData->angVelX << ", " << pData->angVelY << ", " << pData->angVelZ << "]" << std::endl;
std::cout << "wheelSpeedFL: " << pData->wheelSpeedFL << std::endl;
std::cout << "wheelSpeedFR: " << pData->wheelSpeedFR << std::endl;
std::cout << "wheelSpeedRL: " << pData->wheelSpeedRL << std::endl;
std::cout << "wheelSpeedRR: " << pData->wheelSpeedRR << std::endl;
std::cout << "engineRpm: " << pData->engineRpm << std::endl;
std::cout << "odometer: " << pData->odometer << std::endl;
}
// 调用第三方决策控制算法，通过获取到的数据去计算生成control，pose
{
// 方式一：通过设置位置点移动主车(无动力学)
std::unique_ptr<SimOne_Data_Pose_Control> pPose = std::make_unique<SimOne_Data_Pose_Control>();
// 以下为伪代码示意，不保证可编译
callback(pPose.get())
{
// 生成主车轨迹点，Eg:
// Position X on Opendrive (by meter)
pPose->posX = m_gps.posX + cos(m_gps.oriZ); // 基于主车当前位置 X 方向移动一米
// Position Y on Opendrive (by meter)
pPose->posY = m_gps.posY + sin(m_gps.oriZ);
// Position Z on Opendrive (by meter)
pPose->posZ = m_gps.posZ;
// Rotation X on Opendrive (by radian)
pPose->oriX = m_gps.oriX;
// Rotation Y on Opendrive (by radian)
pPose->oriY = m_gps.oriY;
// Rotation Z on Opendrive (by radian)
pPose->oriZ = m_gps.oriZ;
// Automatically set Z according to scene
pPose->autoZ = false;
};
// 方式二：通过油门、刹车、方向消息驱动主车(有动力学)
std::unique_ptr<SimOne_Data_Control> pCtrl = std::make_unique<SimOne_Data_Control>();
// 以下为伪代码示意，不保证可编译
[](pCtrl.get())
{
// 生成控制消息，Eg:
pCtrl->timestamp = getCurrentTime(); // uint64 时间戳，单位us
pCtrl->throttleMode = ESimOne_Throttle_Mode::ESimOne_Throttle_Mode_Speed; // vehicle speed, m/s, in this mode, brake input is ignored
pCtrl->throttle = 10; // m/s double 油门开度 0-100。100表示最大油门驱动
pCtrl->steeringMode = ESimOne_Steering_Mode::ESimOne_Steering_Mode_SteeringWheelAngle; // steering wheel angle, degree
pCtrl->steering = 0;
pCtrl->isManualGear = false;
pCtrl->gear = ESimOne_Gear_Mode::ESimOne_Gear_Mode_Drive; // forward gear for automatic gear
}
// 方式三：通过规划轨迹点驱动主车(有动力学)
std::unique_ptr<SimOne_Data_Control_Trajectory> pTraj = std::make_unique<SimOne_Data_Control_Trajectory>();
// 以下为伪代码示意，不保证可编译
[](pTraj.get())
{
// 生成规划轨迹，Eg:
float posx = m_gps.posX; // position x
float posy = m_gps.posY; // position y
float speed = 10; // m/s
float accel = 1; // accelelation m/s^2
float theta = m_gps.oriZ; // yaw rad
float kappa = 0; // curvature
float relative_time = 0; // time relative to the first trajectory point
float s = 0; // distance from the first trajectory point
pTraj->point_num = 10;
for (int i = 0; i < pTraj->point_num; i++)
{
pTraj->points[i].posx = posx;
pTraj->points[i].posy = posy;
pTraj->points[i].speed = speed;
pTraj->points[i].accel = accel;
pTraj->points[i].theta = theta;
pTraj->points[i].kappa = kappa;
pTraj->points[i].relative_time = relative_time;
pTraj->points[i].s = s;
posx = posx + cos(theta);
posy = posy + sin(theta);
relative_time += 1;
s += 1;
}
pTraj->isReverse = false;
}
}
/* 控制方式一：设置主车位置 API
* input param:
* mainVehicleId: Vehilcle index, configure order of web UI, starts from 0
* pPose: Pose to set
* return: Success or not
*/
if (!SimOneAPI::SetPose(0, &pose_ctl))
{
std::cout << "Set Pose failed!" << std::endl;
}
/* 控制方式二：主车控制 (通过油门、刹车、方向等消息驱动主车(有动力学)，控制参数由算法端提供)
* input param:
* mainVehicleId: Vehilcle index, configure order of web UI, starts from 0
* pControl: vehicle control data
* return: Success or not
*/
if (!SetDrive(0, pCtrl.get()))
{
std::cout << "SetDrive Failed!" << std::endl;
}
/* 控制方式三：主车控制 (通过规划轨迹点驱动主车(有动力学)，不可同时使用SetDrive)
* input param:
* mainVehicleId: Vehilcle index, configure order of web UI, starts from 0
* pControlTrajectory: vehicle planning trajectory
* return: Success or not
*/
if (!SetDriveTrajectory("0", pTraj.get()))
{
std::cout << "SetDrive Failed!" << std::endl;
}
3.3.2 感知算法训练¶
- 示例伪代码: C++
// 主车Id
const char* mv_id = "0";
// 是否加入帧同步
bool isJoinTimeLoop = false;
// BridgeIO 服务 Ip
const char* serverIP = "YOUR_SERVER_IP"; // 请替换为实际服务器地址
// 初始化 SimOneAPI
InitSimOneAPI("0", isJoinTimeLoop, serverIP);
// 回调方式获取真值数据
if (IsCallBackMode)
{
auto function = [](const char* mainVehicleId, SimOne_Data_Obstacle *pObstacle) {
output_ground_truth(pObstacle);
};
SetGroundTruthUpdateCB(function);
}
// 非帧同步方式获取真值数据
else {
std::unique_ptr<SimOne_Data_Obstacle> pObstacle = std::make_unique<SimOne_Data_Obstacle>();
int lastFrame = 0;
while (true) {
bool flag = GetGroundTruth(mv_id, pObstacle.get());
if (flag && pObstacle->frame != lastFrame)
{
lastFrame = pObstacle->frame;
output_ground_truth(pObstacle.get());
}
if (!flag)
{
std::cout << "GetGroundTruth Failed!" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}
// 真值数据：
void output_ground_truth(SimOne_Data_Obstacle* pObstacle)
{
std::cout <<"frame:"<< pObstacle->frame << std::endl;
std::cout << "obstacleSize: " << pObstacle->obstacleSize << std::endl;
for (int i=0; i<pObstacle->obstacleSize ; i++)
{
std::cout << "obstacle[" << i << "].id: " << pObstacle->obstacle[i].id << std::endl;
std::cout << "obstacle[" << i << "].viewId: " << pObstacle->obstacle[i].viewId << std::endl;
std::cout << "obstacle[" << i << "].type: " << pObstacle->obstacle[i].type << std::endl;
std::cout << "obstacle[" << i << "].theta: " << pObstacle->obstacle[i].theta << std::endl;
std::cout << "obstacle[" << i << "].posX/Y/Z: [" << pObstacle->obstacle[i].posX << ", " << pObstacle->obstacle[i].posY << ", " << pObstacle->obstacle[i].posZ << "]" << std::endl;
std::cout << "obstacle[" << i << "].oriX/Y/Z: [" << pObstacle->obstacle[i].oriX << ", " << pObstacle->obstacle[i].oriY << ", " << pObstacle->obstacle[i].oriZ << "]" << std::endl;
std::cout << "obstacle[" << i << "].velX/Y/Z: [" << pObstacle->obstacle[i].velX << ", " << pObstacle->obstacle[i].velY << ", " << pObstacle->obstacle[i].velZ << "]" << std::endl;
std::cout << "obstacle[" << i << "].length: " << pObstacle->obstacle[i].length << std::endl;
std::cout << "obstacle[" << i << "].width: " << pObstacle->obstacle[i].width << std::endl;
std::cout << "obstacle[" << i << "].height: " << pObstacle->obstacle[i].height << std::endl;
std::cout << "obstacle[" << i << "].accelX/Y/Z: [" << pObstacle->obstacle[i].accelX << ", " << pObstacle->obstacle[i].accelY << ", " << pObstacle->obstacle[i].accelZ << "]" << std::endl;
}
}
// 获取案例结束状态
if (SimOneAPI::GetCaseRunStatus() == ESimOne_Case_Status::ESimOne_Case_Status_Stop)
// 关闭 SimOneAPI
if (!SimOneAPI::TerminateSimOneAPI())
{
std::cout << "TerminateSimOneAPI Failed!" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
}
// 摄像头节点 udp server ip
const char* img_ip = "127.0.0.1";
// 摄像头节点 udp server port
unsigned short img_port = 123456;
// 回调方式获取摄像头传感器物理级数据
if (IsCallBackMode)
{
auto function = [](SimOne_Streaming_Image* pImage) {
output_image(pImage);
};
SetStreamingImageUpdateCB(img_ip, img_port, function);
}
// 非帧同步方式获取摄像头传感器物理级数据
else
{
std::unique_ptr<SimOne_Streaming_Image> pImage= std::make_unique<SimOne_Streaming_Image>();
int lastFrame = 0;
while (true)
{
bool flag = GetStreamingImage(img_ip, img_port, pImage.get());
if (flag && pImage->frame != lastFrame)
{
lastFrame = pImage->frame;
output_image(pImage.get());
}
if (!flag)
{
std::cout << "GetStreamingImage Failed!" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}
// 图像数据：
void output_image(SimOne_Streaming_Image* pImage)
{
if (pImage->width != 0 && pImage->height != 0)
{
std::cout << "frame: " << pImage->frame << std::endl;
std::cout << "width: " << pImage->width << std::endl; // int Image resolution width 1920 max
std::cout << "height: " << pImage->height << std::endl; // int Image resolution height 1080 max
std::cout << "imageDataSize: " << pImage->imageDataSize << std::endl; // 1920 x 1080 x 3 max
std::cout << "imageData: " << pImage->imageData << std::endl;
}
}
// 激光雷达节点 udp server ip
const char* pcd_ip = "127.0.0.1";
// 激光雷达节点 udp server port
unsigned short pcd_port = 123456;
// 激光雷达节点 udp server info_port
unsigned short pcd_info_port = 654321;
// 回调方式获取激光雷达传感器物理级数据
if (IsCallBackMode)
{
auto function = [](SimOne_Streaming_Point_Cloud* pPointcloud) {
output_point_cloud(pPointcloud);
};
SetStreamingPointCloudUpdateCB(pcd_ip, pcd_port, pcd_info_port, function);
}
// 非帧同步方式获取激光雷达传感器物理级数据
else
{
std::unique_ptr<SimOne_Streaming_Point_Cloud> pPointCloud = std::make_unique<SimOne_Streaming_Point_Cloud>();
int lastFrame = 0;
while (true) {
bool flag = GetStreamingPointCloud(pcd_ip, pcd_port, pcd_info_port, pPointCloud.get());
if (flag && pPointCloud->frame != lastFrame)
{
lastFrame = pPointCloud->frame;
output_point_cloud(pPointCloud.get());
}
if (!flag)
{
std::cout << "GetStreamingPointCloud Failed!" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}
// 点云数据：
static std::string sNames[] = {"x", "y", "z", "intensity"};
static int sOffsets[] = {0, 4, 8, 12};
static int sDatatypes[] = {7, 7, 7, 7};
void output_point_cloud(SimOne_Streaming_Point_Cloud* pPointcloud)
{
std::cout << "frame: " << pPointcloud->frame << std::endl; // sequence number long long
std::cout << "pointStep: " << pPointcloud->pointStep << std::endl; // int
std::cout << "height: " << pPointcloud->height << std::endl; // int
std::cout << "width: " << pPointcloud->width << std::endl; // int
}
// 调用第三方感知算法函数, 对真值数据、物理集数据 进行感知训练
// 以下为伪代码示意，不保证可编译
[](pObstacle, pImage, pPointCloud)
{
// 感知训练
}
3.3.3 融合算法训练¶
- 示例伪代码: C++
// 主车Id
const char* mv_id = "0";
// 是否加入帧同步
bool isJoinTimeLoop = false;
// BridgeIO 服务 Ip
const char* serverIP = "YOUR_SERVER_IP"; // 请替换为实际服务器地址
// 初始化 SimOneAPI
InitSimOneAPI("0", isJoinTimeLoop, serverIP);
// 回调方式获得所有超声波雷达信息
if (IsCallBackMode)
{
auto function = [](const char* mainVehicleId, SimOne_Data_UltrasonicRadars* pUltrasonics)
{
output_ultrasonic_radars(pUltrasonics);
};
SetUltrasonicRadarsCB(function);
}
// 非帧同步方式获得所有超声波雷达信息
else
{
std::unique_ptr<SimOne_Data_UltrasonicRadars> pDetections = std::make_unique<SimOne_Data_UltrasonicRadars>();
int lastFrame = 0;
while (true)
{
bool flag = GetUltrasonicRadars(mv_id, pDetections.get());
if (flag && pDetections->frame != lastFrame)
{
lastFrame = pDetections->frame;
output_ultrasonic_radars(pDetections.get());
}
if (!flag)
{
std::cout << "GetUltrasonicRadars Failed!" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
// 超声波雷达数据
void output_ultrasonic_radars(SimOne_Data_UltrasonicRadars* pUltrasonicData)
{
std::cout <<"ultrasonicRadarNum: " << pUltrasonicData->ultrasonicRadarNum << std::endl;
for (int i = 0; i < pUltrasonicData->ultrasonicRadarNum; i++)
{
std::cout <<"frame:"<< pUltrasonicData->ultrasonicRadars[i].frame << std::endl;
std::cout <<"sensorId:"<< pUltrasonicData->ultrasonicRadars[i].sensorId << std::endl;
std::cout <<"obstacleNum:"<< pUltrasonicData->ultrasonicRadars[i].obstacleNum << std::endl;
for (int j = 0; j < pUltrasonicData->ultrasonicRadars[i].obstacleNum; j++)
{
std::cout << "obstacleDetections[" << j << "].obstacleRanges: "<< pUltrasonicData->ultrasonicRadars[i].obstacleDetections[j].obstacleRanges << std::endl;
std::cout << "obstacleDetections[" << j << "].x: "<< pUltrasonicData->ultrasonicRadars[i].obstacleDetections[j].x << std::endl;
std::cout << "obstacleDetections[" << j << "].y: "<< pUltrasonicData->ultrasonicRadars[i].obstacleDetections[j].y << std::endl;
std::cout << "obstacleDetections[" << j << "].z: "<< pUltrasonicData->ultrasonicRadars[i].obstacleDetections[j].z << std::endl;
}
}
}
// 回调方式获得所有毫米波雷达信息
if (IsCallBackMode)
{
auto function = [](const char* mainVehicleId, const char* sensorId, SimOne_Data_RadarDetection* pRadarData)
{
output_radar_detection(pRadarData);
};
SetRadarDetectionsUpdateCB(function);
}
// 非帧同步方式获得所有毫米波雷达信息
else
{
std::unique_ptr<SimOne_Data_RadarDetection> pRadarData = std::make_unique<SimOne_Data_RadarDetection>();
int lastFrame = 0;
while (true)
{
bool flag = GetRadarDetections(mv_id, "objectBasedRadar1", pRadarData.get());
if (flag && pRadarData->frame != lastFrame)
{
lastFrame = pRadarData->frame;
output_radar_detection(pRadarData.get());
}
if (!flag)
{
std::cout << "GetRadarDetections Failed!" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(30));
}
}
// 毫米波雷达数据
void output_radar_detection(SimOne_Data_RadarDetection* pRadarData)
{
std::cout<< "frame:" << pRadarData->frame << std::endl;
std::cout<< "detectNum :" << pRadarData->detectNum << std::endl;
for (int i = 0; i < pRadarData->detectNum; i++)
{
std::cout << "detections[" << i << "].ip: " << pRadarData->detections[i].id << std::endl;
std::cout << "detections[" << i << "].subId: " << pRadarData->detections[i].subId << std::endl;
std::cout << "detections[" << i << "].type: " << pRadarData->detections[i].type << std::endl;
std::cout << "detections[" << i << "].posX/Y/Z: [" << pRadarData->detections[i].posX << ", " << pRadarData->detections[i].posY << ", " << pRadarData->detections[i].posZ << "]"<< std::endl;
std::cout << "detections[" << i << "].velX/Y/Z: [" << pRadarData->detections[i].velX << ", " << pRadarData->detections[i].velY << ", " << pRadarData->detections[i].velZ << "]"<< std::endl;
std::cout << "detections[" << i << "].accelX/Y/Z: [" << pRadarData->detections[i].accelX << ", " << pRadarData->detections[i].accelY << ", " << pRadarData->detections[i].accelZ << "]"<< std::endl;
std::cout << "detections[" << i << "].oriX/Y/Z: [" << pRadarData->detections[i].oriX << ", " << pRadarData->detections[i].oriY << ", " << pRadarData->detections[i].oriZ << "]"<< std::endl;
std::cout << "detections[" << i << "].length: " << pRadarData->detections[i].length << std::endl;
std::cout << "detections[" << i << "].width: " << pRadarData->detections[i].width << std::endl;
std::cout << "detections[" << i << "].height: " << pRadarData->detections[i].height << std::endl;
std::cout << "detections[" << i << "].range: " << pRadarData->detections[i].range << std::endl;
std::cout << "detections[" << i << "].rangeRate: " << pRadarData->detections[i].rangeRate << std::endl;
std::cout << "detections[" << i << "].azimuth: " << pRadarData->detections[i].azimuth << std::endl;
std::cout << "detections[" << i << "].vertical: " << pRadarData->detections[i].vertical << std::endl;
std::cout << "detections[" << i << "].snrdb: " << pRadarData->detections[i].snrdb << std::endl;
std::cout << "detections[" << i << "].rcsdb: " << pRadarData->detections[i].rcsdb << std::endl;
std::cout << "detections[" << i << "].probability: " << pRadarData->detections[i].probability << std::endl;
}
}
// 回调方式获取目标级传感器检测到的感知真值
if (IsCallBackMode)
{
auto function = [](const char* MainVehicleID, const char* sensorId, SimOne_Data_SensorDetections* pGroundtruth)
{
output_sensor_detections(pGroundtruth);
};
SetSensorDetectionsUpdateCB(function);
}
// 非帧同步方式获取目标级传感器检测到的感知真值
else
{
std::unique_ptr<SimOne_Data_SensorDetections> pGroundtruth = std::make_unique<SimOne_Data_SensorDetections>();
int lastFrame = 0;
while (true)
{
// "sensorFusion1" "objectBasedCamera1" "objectBasedLidar1" "perfectPerception1"
bool flag = GetSensorDetections(mainVehicleId.c_str(), "perfectPerception1", pGroundtruth.get());
if (flag && pGroundtruth->frame != lastFrame)
{
lastFrame = pGroundtruth->frame;
output_sensor_detections(pGroundtruth.get());
}
if (!flag)
{
std::cout << "GetSensorDetections Failed!" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
// 目标极传感器真值数据
void output_sensor_detections(SimOne_Data_SensorDetections* pGroundtruth)
{
std::cout << "frame:"<< pGroundtruth->frame << std::endl;
std::cout << "objectSize: "<< pGroundtruth->objectSize << std::endl;
for (int i = 0; i < pGroundtruth->objectSize; i++)
{
std::cout << "objects[" << i << "].id" << pGroundtruth->objects[i].id << std::endl;
std::cout << "obstacles[" << i << "].type: " << pGroundtruth->objects[i].type << std::endl;
std::cout << "objects[" << i << "].posX/Y/Z: " << "[" << pGroundtruth->objects[i].posX << ", " << pGroundtruth->objects[i].posY << ", " << pGroundtruth->objects[i].posZ << "]" << std::endl;
std::cout << "objects[" << i << "].oriX/Y/Z: " << "[" << pGroundtruth->objects[i].oriX << ", " << pGroundtruth->objects[i].oriY << ", " << pGroundtruth->objects[i].oriZ << "]" << std::endl;
std::cout << "objects[" << i << "].length: " << pGroundtruth->objects[i].length << std::endl;
std::cout << "objects[" << i << "].width: " << pGroundtruth->objects[i].width << std::endl;
std::cout << "objects[" << i << "].height: " << pGroundtruth->objects[i].height << std::endl;
std::cout << "objects[" << i << "].range: " << pGroundtruth->objects[i].range << std::endl;
std::cout << "objects[" << i << "].velX/Y/Z: " << "[" << pGroundtruth->objects[i].velX << ", " << pGroundtruth->objects[i].velY << ", " << pGroundtruth->objects[i].velZ << "]" << std::endl;
std::cout << "objects[" << i << "].accelX/Y/Z: " << "[" << pGroundtruth->objects[i].accelX << ", " << pGroundtruth->objects[i].accelY << ", " << pGroundtruth->objects[i].accelZ << "]" << std::endl;
std::cout << "objects[" << i << "].probability: " << pGroundtruth->objects[i].probability << std::endl;
std::cout << "objects[" << i << "].relativePosX/Y/Z: " << "[" << pGroundtruth->objects[i].relativePosX << ", " << pGroundtruth->objects[i].relativePosY << ", " << pGroundtruth->objects[i].relativePosZ << "]" << std::endl;
std::cout << "objects[" << i << "].relativeRotX/Y/Z: " << "[" << pGroundtruth->objects[i].relativeRotX << ", " << pGroundtruth->objects[i].relativeRotY << ", " << pGroundtruth->objects[i].relativeRotZ << "]" << std::endl;
std::cout << "objects[" << i << "].relativeVelX/Y/Z: " << "[" << pGroundtruth->objects[i].relativeVelX << ", " << pGroundtruth->objects[i].relativeVelY << ", " << pGroundtruth->objects[i].relativeVelZ << "]" << std::endl;
std::cout << "objects[" << i << "].bbox2dMinX: " << pGroundtruth->objects[i].bbox2dMinX << std::endl;
std::cout << "objects[" << i << "].bbox2dMinY: " << pGroundtruth->objects[i].bbox2dMinY << std::endl;
std::cout << "objects[" << i << "].bbox2dMaxX: " << pGroundtruth->objects[i].bbox2dMaxX << std::endl;
std::cout << "objects[" << i << "].bbox2dMaxY: " << pGroundtruth->objects[i].bbox2dMaxY << std::endl;
}
}
// 调用第三方融合算法函数，对目标集数据进行融合处理
[](pUltrasonicData, pRadarData, pSensorData)
{
// 融合处理
}
// 获取案例结束状态
if (SimOneAPI::GetCaseRunStatus() == ESimOne_Case_Status::ESimOne_Case_Status_Stop)
// 关闭 SimOneAPI
if (!SimOneAPI::TerminateSimOneAPI())
{
std::cout << "TerminateSimOneAPI Failed!" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
}
3.3.4 感知决策规划闭环¶
- 示例伪代码: C++
// 主车Id
const char* mv_id = "0";
// 是否加入帧同步
bool isJoinTimeLoop = false;
// BridgeIO 服务 Ip
const char* serverIP = "10.66.9.194";
// 初始化 SimOneAPI
InitSimOneAPI("0", isJoinTimeLoop, serverIP);
// 调用第三方感知算法函数, 对真值数据、物理集数据 进行感知训练
perception_result = [](pObstacle, pImage, pPointCloud)
{
// 感知训练
}
// 调用第三方融合算法函数，对目标集数据进行融合处理
fusion_result = [](perception_result)
{
// 融合处理
}
// 调用第三方决策控制算法，通过获取到的数据去计算生成control，pose
{
// 方式一：通过设置位置点移动主车(无动力学)
[](fusion_result)
{
SimOne_Data_Pose_Control pose_ctl;
...
SimOneAPI::SetPose(0, &pose_ctl))
};
// 方式二：通过油门、刹车、方向等消息驱动主车(有动力学)
[](fusion_result)
{
SimOne_Data_Control ctrl;
...
SetDrive(0, &ctrl);
}
// 方式三：通过规划轨迹点驱动主车(有动力学)
[](pTraj.get())
{
SimOne_Data_Control_Trajectory traj;
...
SetDriveTrajectory("0", &traj);
}
}
// 获取案例结束状态
if (SimOneAPI::GetCaseRunStatus() == ESimOne_Case_Status::ESimOne_Case_Status_Stop)
// 关闭 SimOneAPI
if (!SimOneAPI::TerminateSimOneAPI())
{
std::cout << "TerminateSimOneAPI Failed!" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
}