agv/vision
8
0
Форк 0
Код Задачі Запити на злиття Actions Packages Проєкти Релізи Вікі Активність

更新 YOLOPv2.cpp

Переглянути джерело
This commit is contained in:
TY
2025-06-25 13:18:25 +08:00
джерело d15db87b34
коміт bfe08d2a4e
1 змінених файлів з 183 додано та 4 видалено
Завантажити патч Завантажити файл різниці Expand all files Collapse all files

187
YOLOPv2.cpp
Переглянути файл

@@ -1,5 +1,21 @@
#include "YOLOPv2.h"
#include <QLoggingCategory>
#include "tcp_utility.hpp"
#include <iostream>
#include <vector>
#include <map>
#include <string>
#include <cstring>
#include<QJsonObject>
#include<QJsonDocument>
#include<QThread>
/*#include <winsock2.h>
#include <ws2tcpip.h>
#include <nlohmann/json.hpp> // 需要安装nlohmann/json库
#include<json/json.h>
#pragma comment(lib, "ws2_32.lib")
using json = nlohmann::json;*/
YOLOPv2::YOLOPv2(Net_config config)
{
@@ -67,6 +83,37 @@ YOLOPv2::YOLOPv2(Net_config config)
while (getline(ifs, line)) this->class_names.push_back(line);
this->num_class = class_names.size();
//initial tcp 开始
//tcp::TcpClient client;
// 设置消息回调
this->tcp_client.setMessageCallback([](tcp::TcpSocket& socket, const std::vector<uint8_t>& data) {
std::string message(data.begin(), data.end());
std::cout << "Received from server: " << message << std::endl;
});
// 设置错误回调
this->tcp_client.setErrorCallback([](tcp::TcpSocket& socket, tcp::ErrorCode error) {
std::cout << "Error: " << tcp::errorCodeToString(error) << std::endl;
});
// 连接服务器
auto result = this->tcp_client.connect("192.168.124.135", 6000);
if (result == tcp::ErrorCode::Success) {
std::cout << "Connected to server" << std::endl;
// 发送消息
//this->tcp_client.send("Hello, server!");
// 让客户端运行一段时间
std::cout << "Press Enter to disconnect..." << std::endl;
std::cin.get();
} else {
std::cout << "Failed to connect to server: "
<< tcp::errorCodeToString(result) << std::endl;
}
}
void YOLOPv2::normalize_(Mat img)
@@ -130,6 +177,106 @@ inline float sigmoid(float x)
{
return 1.0 / (1 + exp(-x));
}
/*
void SendMessage(const char* ip, int port, map<string,double> data) {
// 初始化Winsock
WSADATA wsaData;
if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
std::cerr << "WSAStartup failed: " << WSAGetLastError() << std::endl;
return;
}
// 创建套接字
SOCKET hSocket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
if (hSocket == INVALID_SOCKET) {
std::cerr << "Socket creation failed: " << WSAGetLastError() << std::endl;
WSACleanup();
return;
}
// 设置服务器地址
sockaddr_in servAddr;
servAddr.sin_family = AF_INET;
servAddr.sin_port = htons(port);
if (inet_pton(AF_INET, ip, &servAddr.sin_addr) <= 0) {
std::cerr << "Invalid address/Address not supported" << std::endl;
closesocket(hSocket);
WSACleanup();
return;
}
// 连接服务器
if (connect(hSocket, (SOCKADDR*)&servAddr, sizeof(servAddr)) == SOCKET_ERROR) {
std::cerr << "Connection failed: " << WSAGetLastError() << std::endl;
closesocket(hSocket);
WSACleanup();
return;
}
// 构造JSON消息
json message;
message["receiver"] = "ANGLE";
message["sender"] = "ANGLE";
message["type"] = 5;
json params;
for (const auto& pair: data) {
params[pair.first] = pair.second;
}
params["status"] = "OK";
message["params"] = params;
std::string jsonStr = message.dump();
// 发送消息
int bytesSent = send(hSocket, jsonStr.c_str(), (int)jsonStr.size(), 0);
if (bytesSent == SOCKET_ERROR) {
std::cerr << "Send failed: " << WSAGetLastError() << std::endl;
} else {
std::cout << "Sent " << bytesSent << " bytes: " << jsonStr << std::endl;
// 接收响应（可选）
char buffer[1024];
int bytesRecv = recv(hSocket, buffer, sizeof(buffer), 0);
if (bytesRecv > 0) {
std::cout << "Received response: " << std::string(buffer, bytesRecv) << std::endl;
}
}
// 清理资源
closesocket(hSocket);
WSACleanup();
}*/
std::vector<unsigned char> YOLOPv2::mapToByteArray(const std::map<std::string, double>& param) {
std::vector<unsigned char> byteArray;
// 写入map大小
size_t mapSize = param.size();
byteArray.insert(byteArray.end(),
reinterpret_cast<const unsigned char*>(&mapSize),
reinterpret_cast<const unsigned char*>(&mapSize) + sizeof(mapSize));
// 遍历map中的每个元素
for (const auto& pair : param) {
// 写入键的长度
size_t keyLength = pair.first.size();
byteArray.insert(byteArray.end(),
reinterpret_cast<const unsigned char*>(&keyLength),
reinterpret_cast<const unsigned char*>(&keyLength) + sizeof(keyLength));
// 写入键的内容
byteArray.insert(byteArray.end(),
pair.first.begin(),
pair.first.end());
// 写入值（double类型）
byteArray.insert(byteArray.end(),
reinterpret_cast<const unsigned char*>(&pair.second),
reinterpret_cast<const unsigned char*>(&pair.second) + sizeof(double));
}
return byteArray;
}
Mat YOLOPv2::detect(Mat& frame)
{
@@ -251,12 +398,12 @@ Mat YOLOPv2::detect(Mat& frame)
outimg.at<Vec3b>(i, j)[0] = 255;
outimg.at<Vec3b>(i, j)[1] = 0;
outimg.at<Vec3b>(i, j)[2] = 0;
if (!flg && j >= frame.cols / 2 && right == -1) { // 记录图像右半部分最靠左的车道线的左边缘坐标
if (!flg && j >= center && right == -1) { // 记录图像右半部分最靠左的车道线的左边缘坐标
right = j;
}
flg = true;
} else {
if (flg && j - 1 < frame.cols / 2) { //记录图像左半部分最靠右的车道线的右边缘坐标
if (flg && j - 1 < center) { //记录图像左半部分最靠右的车道线的右边缘坐标
left = j - 1;
}
flg = false;
@@ -280,9 +427,10 @@ Mat YOLOPv2::detect(Mat& frame)
outimg.at<Vec3b>(i, mid+k)[2] = 255;
}
//提取偏移量
for (int k = 0; k < N; k++) {
for (int k = 1; k < N-1; k++) {
if (ypos[k] == i) {
deviation[dis[k]] = (mid - center) * rate[k];
cerr<<mid<<' '<<center<<' '<<rate[k]<<' '<<deviation[dis[k]]<<endl;
}
}
}
@@ -334,9 +482,40 @@ Mat YOLOPv2::detect(Mat& frame)
cerr << d.first << ": " << d.second <<endl; //前方xx 距离处：道路中心线相对于小车位置横向偏移距离（右偏为正）(单位mm）
}
double slope = (siz * sumXY - sumX * sumY) / (siz * sumXX - sumX * sumX); //道路中心线相对小车正前方向偏移角度正切值（右偏为正）
dev_angle = atan(slope); //道路中心线相对小车正前方向偏移角弧度值（右偏为正）
dev_angle = atan(slope)*180/3.1415926; //道路中心线相对小车正前方向偏移角弧度值（右偏为正）
cerr << slope <<endl << dev_angle <<endl;
}
//map<string, double> params;
//params["dev_1m"] = deviation[dis[0]];
//params["dev_2m"] = deviation[dis[1]];
//params["dev_3m"] = deviation[dis[2]];
//params["dev_4m"] = deviation[dis[3]];
//params["dev_5m"] = deviation[dis[4]];
//params["dev_6m"] = deviation[dis[5]];
//params["angle"] = dev_angle;
//SendMessage("127.0.0.1", 6000, params);
//std::vector<unsigned char> byteVector = mapToByteArray(params);
// 发送消息
QJsonObject json;
json["receiver"]="DRIVER";
json["sender"]="FAST";
json["type"]=22;
QJsonObject params;
params["angle"]=dev_angle;
json["params"]=params;
QJsonDocument doc(json);
QByteArray data=QJsonDocument(json).toJson();
std::string jsonstr=data.toStdString();
std::cout << jsonstr<< std::endl;
this->tcp_client.send(jsonstr);
return outimg;
}