453 lines
14 KiB
C++
453 lines
14 KiB
C++
#include "AutomaticService.h"
|
|
|
|
#include <algorithm>
|
|
#include <codecvt>
|
|
#include <exception>
|
|
#include <fstream>
|
|
#include <sstream>
|
|
|
|
#include "AutomaticServiceDataManager.h"
|
|
#include "AutomaticServiceLogManager.h"
|
|
#include "GraphicTools.h"
|
|
|
|
using namespace automatic_service;
|
|
|
|
constexpr auto kDefaultRealRange = 50; // 绘图实际边长（单位：米）
|
|
|
|
AutomaticService::AutomaticService() { pGraphicTool_ = __NEW__(GraphicImage); }
|
|
|
|
int AutomaticService::autoServiceInit()
|
|
{
|
|
fieldModelPtrList_.emplace_back(new FieldModelDcrk());
|
|
fieldModelPtrList_.emplace_back(new FieldModelZjzw());
|
|
fieldModelPtrList_.emplace_back(new FieldModelCftc());
|
|
fieldModelPtrList_.emplace_back(new FieldModelPdqb());
|
|
fieldModelPtrList_.emplace_back(new FieldModelQxxs());
|
|
|
|
if (!pTrackInfo_)
|
|
{
|
|
pTrackInfo_ = new TrackInfo();
|
|
}
|
|
if (!pTrackModeManager_)
|
|
{
|
|
pTrackModeManager_ = new TrackModeManager();
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int AutomaticService::autoServiceSetBasePoint(const char *data, int length)
|
|
{
|
|
try
|
|
{
|
|
std::wstring_convert<std::codecvt_utf8<wchar_t>> convert;
|
|
std::wstring str = convert.from_bytes(data);
|
|
nlohmann::json basePointJson = nlohmann::json::parse(str);
|
|
AutomaticServiceDataManager::GetInstance()->basePoint() = basePointJson.get<BasePoint>();
|
|
}
|
|
catch (std::exception &e)
|
|
{
|
|
std::string errorInfo = e.what();
|
|
AutomaticServiceLogManager::GetInstance()->log(errorInfo);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int AutomaticService::autoServiceSetFeildModel(const char *data, int length)
|
|
{
|
|
try
|
|
{
|
|
std::wstring_convert<std::codecvt_utf8<wchar_t>> convert;
|
|
std::wstring str = convert.from_bytes(data);
|
|
nlohmann::json fieldModelListJson = nlohmann::json::parse(str);
|
|
AutomaticServiceDataManager::GetInstance()->fieldModelList() =
|
|
fieldModelListJson.get<FieldModelList>();
|
|
}
|
|
catch (std::exception &e)
|
|
{
|
|
std::string errorInfo = e.what();
|
|
AutomaticServiceLogManager::GetInstance()->log(errorInfo);
|
|
}
|
|
|
|
for (size_t i = 0; i < fieldModelPtrList_.size(); i++)
|
|
{
|
|
FieldModelAbstractClass *pCurrModel = fieldModelPtrList_[i];
|
|
pCurrModel->initModelPoint();
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int AutomaticService::autoServiceSetTeachPointInfo(const char *data, int length)
|
|
{
|
|
try
|
|
{
|
|
std::wstring_convert<std::codecvt_utf8<wchar_t>> convert;
|
|
std::wstring str = convert.from_bytes(data);
|
|
nlohmann::json teachPointInfoJson = nlohmann::json::parse(str);
|
|
AutomaticServiceDataManager::GetInstance()->teachPointInfo() =
|
|
teachPointInfoJson.get<TeachItemList>();
|
|
}
|
|
catch (std::exception &e)
|
|
{
|
|
std::string errorInfo = e.what();
|
|
AutomaticServiceLogManager::GetInstance()->log(errorInfo);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int AutomaticService::autoServiceSetTeachPointParam(const char *data, int length)
|
|
{
|
|
try
|
|
{
|
|
std::wstring_convert<std::codecvt_utf8<wchar_t>> convert;
|
|
std::wstring str = convert.from_bytes(data);
|
|
nlohmann::json teachPointParamJson = nlohmann::json::parse(str);
|
|
AutomaticServiceDataManager::GetInstance()->teachPointParam() =
|
|
teachPointParamJson.get<TeachPointList>();
|
|
}
|
|
catch (std::exception &e)
|
|
{
|
|
std::string errorInfo = e.what();
|
|
AutomaticServiceLogManager::GetInstance()->log(errorInfo);
|
|
}
|
|
|
|
for (size_t i = 0; i < fieldModelPtrList_.size(); i++)
|
|
{
|
|
FieldModelAbstractClass *pCurrModel = fieldModelPtrList_[i];
|
|
pCurrModel->initTeachPoint();
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int AutomaticService::autoServiceSetCarModel(const char *data, int length)
|
|
{
|
|
try
|
|
{
|
|
std::wstring_convert<std::codecvt_utf8<wchar_t>> convert;
|
|
std::wstring str = convert.from_bytes(data);
|
|
nlohmann::json carModelJson = nlohmann::json::parse(str);
|
|
AutomaticServiceDataManager::GetInstance()->carModel() = carModelJson.get<CarModelList>();
|
|
}
|
|
catch (std::exception &e)
|
|
{
|
|
std::string errorInfo = e.what();
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int AutomaticService::autoServiceSetLineInfo(const char *data, int length)
|
|
{
|
|
try
|
|
{
|
|
std::string temp = AutomaticServiceDataManager::handleJson(data, length);
|
|
std::wstring_convert<std::codecvt_utf8<wchar_t>> convert;
|
|
std::wstring str = convert.from_bytes(temp.c_str());
|
|
nlohmann::json lineInfoJson = nlohmann::json::parse(str);
|
|
auto temp1 = lineInfoJson.get<Track>();
|
|
AutomaticServiceDataManager::GetInstance()->lineInfo() = lineInfoJson.get<Track>();
|
|
}
|
|
catch (std::exception &e)
|
|
{
|
|
std::string errorInfo = e.what();
|
|
return -1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int AutomaticService::autoServiceSetLibInfo(const char *data, int length)
|
|
{
|
|
try
|
|
{
|
|
std::string temp = AutomaticServiceDataManager::handleJson(data, length);
|
|
std::wstring_convert<std::codecvt_utf8<wchar_t>> convert;
|
|
std::wstring str = convert.from_bytes(temp.c_str());
|
|
nlohmann::json libInfiJson = nlohmann::json::parse(str);
|
|
auto temp1 = libInfiJson.get<Track>();
|
|
AutomaticServiceDataManager::GetInstance()->libInfo() = libInfiJson.get<Track>();
|
|
}
|
|
catch (std::exception &e)
|
|
{
|
|
std::string errorInfo = e.what();
|
|
return -1;
|
|
}
|
|
|
|
pTrackInfo_->initLibInfo();
|
|
|
|
return 0;
|
|
}
|
|
|
|
int AutomaticService::autoServiceSelectLineAndLib(int lineIndex, int libIndex)
|
|
{
|
|
if (!pTrackInfo_)
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
selectedLineIndex_ = lineIndex;
|
|
selectedLibIndex_ = libIndex;
|
|
pTrackInfo_->selectLineAndLib(lineIndex, libIndex);
|
|
return 0;
|
|
}
|
|
|
|
int AutomaticService::autoServiceTrack(double x, double y, double yaw, double v)
|
|
{
|
|
double tempX = 0.0;
|
|
double tempY = 0.0;
|
|
auto basePoint = AutomaticServiceDataManager::GetInstance()->basePoint();
|
|
GpsMath::getDimensionalCoordinate(std::stod(basePoint.basegpse), std::stod(basePoint.basegpsn),
|
|
x, y, 5.652, tempX, tempY);
|
|
x = tempX;
|
|
y = tempY;
|
|
|
|
std::vector<double> currPos = {x, y};
|
|
historyPosList_.emplace_back(currPos);
|
|
|
|
// 绘图
|
|
if (!pGraphicTool_)
|
|
{
|
|
AutomaticServiceLogManager::GetInstance()->log(
|
|
"[AutomaticService] pGraphicTool_ == nullptr");
|
|
return -1;
|
|
}
|
|
pGraphicTool_->clean();
|
|
|
|
// 计算每个像素对应的实际距离
|
|
double minPixel = std::min(imageWidth_, imageHeight_);
|
|
unit_ = minPixel / kDefaultRealRange;
|
|
AutomaticServiceLogManager::GetInstance()->log(
|
|
"[AutomaticService] unit_ = " + std::to_string(unit_) + " pixel/m");
|
|
|
|
// 以当前位置点为中心，得到绘图实际距离范围（单位：米）
|
|
Pointi currCarPosInImage(imageWidth_ / 2, imageHeight_ / 2);
|
|
AutoPoint topLeft(x - currCarPosInImage.x / unit_, y - currCarPosInImage.y / unit_);
|
|
AutoPoint bottomRight(x + currCarPosInImage.x / unit_, y + currCarPosInImage.y / unit_);
|
|
mapRect_ = AutoRect(topLeft, bottomRight);
|
|
std::string logText = "[AutomaticService] topLeft = (" + std::to_string(topLeft.x) + ", " +
|
|
std::to_string(topLeft.y) + "); bottomRight = (" +
|
|
std::to_string(bottomRight.x) + ", " + std::to_string(bottomRight.y) +
|
|
")";
|
|
AutomaticServiceLogManager::GetInstance()->log(logText);
|
|
|
|
drawFieldModel(x, y); // 绘制场地模型
|
|
// drawCarModel(yaw); // 绘制车模
|
|
drawSelectedLine(x, y); // 绘制路径
|
|
drawSelectedLib(x, y); // 绘制库位
|
|
drawCarHistoryPos(x, y); // 绘制车辆行驶轨迹
|
|
|
|
if (mapImageCallback_)
|
|
{
|
|
mapImageCallback_(pGraphicTool_->image(), pGraphicTool_->size());
|
|
}
|
|
|
|
// 教学点判断
|
|
std::string currItem;
|
|
int currTeachPointIndex = -1;
|
|
for (size_t i = 0; i < fieldModelPtrList_.size(); i++)
|
|
{
|
|
FieldModelAbstractClass *pCurrProject = fieldModelPtrList_[i];
|
|
auto itemStatus = pCurrProject->updateItemStatus(x, y);
|
|
if (itemStatus == Enter)
|
|
{
|
|
currItem = pCurrProject->getModelName();
|
|
currTeachPointIndex = pCurrProject->calculateTeachPoint(x, y);
|
|
break;
|
|
}
|
|
}
|
|
|
|
AutomaticServiceLogManager::GetInstance()->log("[AutomaticService] currItem = " + currItem);
|
|
AutomaticServiceLogManager::GetInstance()->log("[AutomaticService] currTeachPointIndex = " +
|
|
std::to_string(currTeachPointIndex));
|
|
|
|
if (soundCallback_ && !currItem.empty() && currTeachPointIndex >= 0)
|
|
{
|
|
soundCallback_(itemIndex2SoundMap[currItem], currTeachPointIndex);
|
|
}
|
|
|
|
// 初始化写入教学配置信息
|
|
pTrackInfo_->init(x, y, yaw, v, currItem, currTeachPointIndex);
|
|
|
|
// 开始跟踪算法计算
|
|
pTrackModeManager_->init(*pTrackInfo_);
|
|
std::string order = pTrackModeManager_->run(*pTrackInfo_);
|
|
|
|
if (controlCallback_)
|
|
{
|
|
controlCallback_(order.c_str(), order.size());
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int AutomaticService::autoServiceSetImageSize(int width, int height)
|
|
{
|
|
if (!pGraphicTool_)
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
imageWidth_ = width;
|
|
imageHeight_ = height;
|
|
pGraphicTool_->resize(width, height);
|
|
|
|
return 0;
|
|
}
|
|
|
|
TrackInfo AutomaticService::getTrack() { return *pTrackInfo_; }
|
|
|
|
int AutomaticService::registerMapImageCallback(examJudgeCallbackMapImage callback)
|
|
{
|
|
if (!callback)
|
|
{
|
|
return -1;
|
|
}
|
|
mapImageCallback_ = callback;
|
|
|
|
return 0;
|
|
}
|
|
|
|
int AutomaticService::registerAutoServiceControlCallback(autoServerControlCallback callback)
|
|
{
|
|
if (!callback)
|
|
{
|
|
return -1;
|
|
}
|
|
controlCallback_ = callback;
|
|
|
|
return 0;
|
|
}
|
|
|
|
int AutomaticService::registerAutoServiceSoundCallback(autoServerSoundCallback callback)
|
|
{
|
|
if (!callback)
|
|
{
|
|
return -1;
|
|
}
|
|
soundCallback_ = callback;
|
|
|
|
return 0;
|
|
}
|
|
|
|
int AutomaticService::registerAutoServiceLogCallback(autoServerLogCallback callback)
|
|
{
|
|
if (!callback)
|
|
{
|
|
return -1;
|
|
}
|
|
AutomaticServiceLogManager::GetInstance()->setCallback(callback);
|
|
AutomaticServiceLogManager::GetInstance()->log("registerAutoServiceLogCallback success");
|
|
|
|
return 0;
|
|
}
|
|
|
|
void AutomaticService::drawFieldModel(int x, int y)
|
|
{
|
|
Pointi currCarPosInImage(imageWidth_ / 2, imageHeight_ / 2); // 绘图画布中心点（单位：像素）
|
|
Pointi padding(currCarPosInImage.x - x * unit_, currCarPosInImage.y - y * unit_);
|
|
for (size_t i = 0; i < fieldModelPtrList_.size(); i++)
|
|
{
|
|
FieldModelAbstractClass *pCurrModel = fieldModelPtrList_[i];
|
|
if (pCurrModel->isModelInMapArea(mapRect_))
|
|
{
|
|
// 在框定的视野范围，才绘制模型
|
|
pCurrModel->drawModel(pGraphicTool_, padding, unit_);
|
|
}
|
|
}
|
|
}
|
|
|
|
void AutomaticService::drawCarModel(double yaw)
|
|
{
|
|
Pointi currCarPosInImage(imageWidth_ / 2, imageHeight_ / 2); // 绘图画布中心点（单位：像素）
|
|
// 计算车模每个点相对基准点坐标（考虑航向角）
|
|
auto carModelList = AutomaticServiceDataManager::GetInstance()->carModel();
|
|
TGPSPoint carModelBasePoint;
|
|
carModelBasePoint.jd = carModelList.Model[0].BasePointE;
|
|
carModelBasePoint.wd = carModelList.Model[0].BasePointN;
|
|
carModelBasePoint.gc = carModelList.Model[0].BasePointHigh;
|
|
std::vector<Pointi> pointList;
|
|
for (size_t i = 0; i < carModelList.Model[0].Points.size(); i++)
|
|
{
|
|
auto currCarModelPoint = carModelList.Model[0].Points[i];
|
|
TGPSPoint currGpsPoint;
|
|
currGpsPoint.jd = currCarModelPoint.GPSE;
|
|
currGpsPoint.wd = currCarModelPoint.GPSN;
|
|
currGpsPoint.gc = currCarModelPoint.High;
|
|
|
|
// 计算相对坐标
|
|
double currX = 0.0;
|
|
double currY = 0.0;
|
|
GpsMath::getDimensionalCoordinate(carModelBasePoint.jd, carModelBasePoint.wd,
|
|
currGpsPoint.jd, currGpsPoint.wd, carModelBasePoint.gc,
|
|
currX, currY);
|
|
// 相对坐标转绘图像素点
|
|
Pointi tempPoint(currX * unit_ + currCarPosInImage.x, -currY * unit_ + currCarPosInImage.y);
|
|
// 旋转航向角角度
|
|
tempPoint = GpsMath::rotation(tempPoint, currCarPosInImage, yaw);
|
|
|
|
pointList.emplace_back(tempPoint);
|
|
}
|
|
|
|
// 绘制轮廓
|
|
for (size_t i = 0; i < 23; i++)
|
|
{
|
|
pGraphicTool_->drawLine(pointList[i], pointList[i + 1]);
|
|
}
|
|
pGraphicTool_->drawLine(pointList[23], pointList[0]);
|
|
|
|
// 标注前保险杠中心点
|
|
pGraphicTool_->drawPoint(pointList[0]);
|
|
}
|
|
|
|
void AutomaticService::drawSelectedLine(int x, int y)
|
|
{
|
|
Pointi currCarPosInImage(imageWidth_ / 2, imageHeight_ / 2);
|
|
Pointi padding(currCarPosInImage.x - x * unit_, currCarPosInImage.y - y * unit_);
|
|
auto selectedLine =
|
|
AutomaticServiceDataManager::GetInstance()->lineInfo().getFeaturesById(selectedLineIndex_);
|
|
auto points = selectedLine.geometry.coordinates;
|
|
std::vector<Pointi> pointList;
|
|
FieldModelAbstractClass::transferPoint2ImagePixel(points, padding, unit_, pointList);
|
|
pGraphicTool_->drawPoint(pointList[0], RGB_GREEN, 2);
|
|
for (size_t i = 0; i < pointList.size() - 1; i++)
|
|
{
|
|
pGraphicTool_->drawLine(pointList[i], pointList[i + 1], RGB_RED);
|
|
pGraphicTool_->drawPoint(pointList[i + 1], RGB_GREEN, 2);
|
|
}
|
|
}
|
|
|
|
void AutomaticService::drawSelectedLib(int x, int y)
|
|
{
|
|
Pointi currCarPosInImage(imageWidth_ / 2, imageHeight_ / 2);
|
|
Pointi padding(currCarPosInImage.x - x * unit_, currCarPosInImage.y - y * unit_);
|
|
auto selectedLib =
|
|
AutomaticServiceDataManager::GetInstance()->libInfo().getFeaturesById(selectedLibIndex_);
|
|
auto points = selectedLib.geometry.coordinates;
|
|
std::vector<Pointi> pointList;
|
|
FieldModelAbstractClass::transferPoint2ImagePixel(points, padding, unit_, pointList);
|
|
pGraphicTool_->drawPoint(pointList[0], RGB_GREEN, 2);
|
|
for (size_t i = 0; i < pointList.size() - 1; i++)
|
|
{
|
|
pGraphicTool_->drawLine(pointList[i], pointList[i + 1], RGB_RED);
|
|
pGraphicTool_->drawPoint(pointList[i + 1], RGB_GREEN, 2);
|
|
}
|
|
}
|
|
|
|
void AutomaticService::drawCarHistoryPos(int x, int y)
|
|
{
|
|
Pointi currCarPosInImage(imageWidth_ / 2, imageHeight_ / 2); // 绘图画布中心点（单位：像素）
|
|
Pointi padding(currCarPosInImage.x - x * unit_, currCarPosInImage.y - y * unit_);
|
|
std::vector<Pointi> pixelList;
|
|
FieldModelAbstractClass::transferPoint2ImagePixel(historyPosList_, padding, unit_, pixelList);
|
|
pGraphicTool_->drawPoint(pixelList[0], RGB_BLACK, 2);
|
|
for (size_t i = 1; i < pixelList.size(); i++)
|
|
{
|
|
pGraphicTool_->drawPoint(pixelList[i], RGB_BLACK, 2);
|
|
pGraphicTool_->drawLine(pixelList[i - 1], pixelList[i], RGB_BLACK);
|
|
}
|
|
}
|