diff --git a/BeltTearingApp/BeltTearingApp.pro b/BeltTearingApp/BeltTearingApp.pro index 6e1d6b00..0f93d05e 100644 --- a/BeltTearingApp/BeltTearingApp.pro +++ b/BeltTearingApp/BeltTearingApp.pro @@ -5,6 +5,7 @@ greaterThan(QT_MAJOR_VERSION, 4): QT += widgets TEMPLATE = app CONFIG += c++17 +QMAKE_CXXFLAGS += /utf-8 SUBDIRS += ../BeltTearingConfig/BeltTearingConfig.pro \ ../VrNet/VrTcpClient.pro diff --git a/BeltTearingApp/IStatusUpdate.h b/BeltTearingApp/IStatusUpdate.h index 08ef22dd..609cb686 100644 --- a/BeltTearingApp/IStatusUpdate.h +++ b/BeltTearingApp/IStatusUpdate.h @@ -44,9 +44,10 @@ inline std::string BeltTearingWorkStatusToString(BeltTearingWorkStatus status) { // 皮带撕裂检测结果结构体 struct BeltTearingResult { - QImage image; // 检测图像 + QImage image; // 检测图像 + bool bImageValid = false; QString result; // 检测结果描述 - double confidence = 0.0; // 置信度 + double confidence = 0.0; // 置信度 QString serverName; // 服务器名称 QDateTime timestamp; // 时间戳 }; @@ -59,6 +60,9 @@ public: // 状态文字回调 virtual void OnStatusUpdate(const QString& statusMessage) = 0; + + // 回调需要显示几个图像 + virtual void OnNeedShowImageCount(int count) = 0; // 撕裂检测结果回调 virtual void OnTearingResult(const BeltTearingResult& result) = 0; diff --git a/BeltTearingApp/Presenter/Inc/BeltTearingPresenter.h b/BeltTearingApp/Presenter/Inc/BeltTearingPresenter.h index d4c8c55f..c267eb89 100644 --- a/BeltTearingApp/Presenter/Inc/BeltTearingPresenter.h +++ b/BeltTearingApp/Presenter/Inc/BeltTearingPresenter.h @@ -7,6 +7,11 @@ #include #include "widgets/DeviceStatusWidget.h" +enum class ByteDataType { + Text = 0x01, + Image = 0x02 +}; + class BeltTearingPresenter : public QWidget { Q_OBJECT @@ -24,6 +29,9 @@ public: // 设置设备状态控件 void setDeviceStatusWidget(DeviceStatusWidget* widget) { m_deviceStatusWidget = widget; } + // 设置状态更新接口 + void setStatusUpdate(IStatusUpdate* statusUpdate) { m_statusUpdate = statusUpdate; } + private: // IBeltTearingPresenter interface implementation bool initializeConfig(const QString &configPath); @@ -40,7 +48,7 @@ private slots: void onTcpError(const QString &serverName, const QString &error); private: - + IStatusUpdate* m_statusUpdate; // 状态更新接口 IVrBeltTearingConfig * m_config; DeviceStatusWidget* m_deviceStatusWidget; // 设备状态控件 QMap m_tcpClients; // 服务器名称 -> TCP客户端映射 diff --git a/BeltTearingApp/Presenter/Src/BeltTearingPresenter.cpp b/BeltTearingApp/Presenter/Src/BeltTearingPresenter.cpp index 7129dc2f..fa1c0b4f 100644 --- a/BeltTearingApp/Presenter/Src/BeltTearingPresenter.cpp +++ b/BeltTearingApp/Presenter/Src/BeltTearingPresenter.cpp @@ -61,6 +61,10 @@ bool BeltTearingPresenter::initializeConfig(const QString &configPath) LOG_WARNING("No servers configured"); return false; } + + if(m_statusUpdate){ + m_statusUpdate->OnNeedShowImageCount(servers.size()); + } // 清空现有配置 m_serverInfos.clear(); @@ -238,10 +242,40 @@ void BeltTearingPresenter::onDisconnected(const QString &serverName) void BeltTearingPresenter::onDataReceived(const QString &serverName, const QByteArray &data) { - // 尝试解析图像数据 - QImage image; - if (image.loadFromData(data)) { + // 解析数据包 + quint8 dataType; + quint32 dataSize; + QDataStream stream(data); + stream.setByteOrder(QDataStream::BigEndian); + stream >> dataType >> dataSize; + QByteArray byteArray = data.mid(5, dataSize); + + BeltTearingResult tearResult; + tearResult.bImageValid = false; + + if (dataType == static_cast(ByteDataType::Text)) + { + // 处理文本数据 + QString textData = QString::fromUtf8(byteArray); + // 这里可以添加处理文本数据的逻辑 + LOG_DEBUG("Received text data from server %s: %s \n", serverName.toStdString().c_str(), textData.toStdString().c_str()); + } + else if (dataType == static_cast(ByteDataType::Image)) + { + // 处理图像数据 + QImage image; + if (tearResult.image.loadFromData(byteArray)) { + // 这里可以添加处理图像数据的逻辑 + tearResult.bImageValid = true; + LOG_DEBUG("Received image data from server %s, size: %dx%d \n", serverName.toStdString().c_str(), image.width(), image.height()); + } + } + + if(m_statusUpdate){ + tearResult.serverName = serverName; + tearResult.timestamp = QDateTime::currentDateTime(); + m_statusUpdate->OnTearingResult(tearResult); } } diff --git a/BeltTearingApp/mainwindow.cpp b/BeltTearingApp/mainwindow.cpp index 7ebc61f4..8fb0bbf1 100644 --- a/BeltTearingApp/mainwindow.cpp +++ b/BeltTearingApp/mainwindow.cpp @@ -45,6 +45,7 @@ MainWindow::MainWindow(QWidget *parent) // 将设备状态控件与Presenter关联 m_presenter->setDeviceStatusWidget(m_deviceStatusWidget); + m_presenter->setStatusUpdate(this); m_presenter->Init(); @@ -106,10 +107,7 @@ void MainWindow::resizeEvent(QResizeEvent* event) } void MainWindow::on_btn_test_clicked() -{ - // 初始化4个图片格子 - m_gridView->initImages(4); - +{ // 定义图片资源路径 QStringList imageResources = { "C:/project/QT/GrabBag/GrabBagApp/resource/config_camera.png", @@ -137,22 +135,17 @@ void MainWindow::OnStatusUpdate(const QString &statusMessage) LOG_DEBUG("Status update: %s", statusMessage.toStdString().c_str()); } -void MainWindow::OnTearingResult(const BeltTearingResult &result) +// 回调需要显示几个图像 +void MainWindow::OnNeedShowImageCount(int count) { - // 显示撕裂检测结果 - QString resultText = QString("服务器: %1, 结果: %2, 置信度: %3") - .arg(result.serverName) - .arg(result.result) - .arg(result.confidence); - - statusBar()->showMessage(resultText); - LOG_DEBUG("Tearing result: %s", resultText.toStdString().c_str()); - + m_gridView->initImages(count); +} + +void MainWindow::OnTearingResult(const BeltTearingResult &result) +{ // 如果图像有效，显示在网格控件中 - if (!result.image.isNull()) { - static int imageIndex = 0; - m_gridView->setImages(imageIndex % 4, result.image); - imageIndex++; + if (result.bImageValid && !result.image.isNull()) { + m_gridView->setImages(0, result.image); } } diff --git a/BeltTearingApp/mainwindow.h b/BeltTearingApp/mainwindow.h index 37f921a2..2197b746 100644 --- a/BeltTearingApp/mainwindow.h +++ b/BeltTearingApp/mainwindow.h @@ -29,6 +29,7 @@ public: // IStatusUpdate接口方法声明 void OnStatusUpdate(const QString& statusMessage) override; + void OnNeedShowImageCount(int count) override; void OnTearingResult(const BeltTearingResult& result) override; void OnServerConnected(const QString& serverName) override; void OnServerDisconnected(const QString& serverName) override; diff --git a/BeltTearingApp/mainwindow.ui b/BeltTearingApp/mainwindow.ui index e0c2847a..84698bbc 100644 --- a/BeltTearingApp/mainwindow.ui +++ b/BeltTearingApp/mainwindow.ui @@ -7,7 +7,7 @@ 0 0 1920 - 1080 + 1032 @@ -162,7 +162,7 @@ background-color: rgba(255, 255, 255, 0); 工作状态 - Qt::AlignmentFlag::AlignCenter + Qt::AlignCenter @@ -250,36 +250,36 @@ background-color: rgba(255, 255, 255, 0); 20 140 1311 - 891 + 850 background-color:rgb(37, 38, 42) - QFrame::Shape::StyledPanel + QFrame::StyledPanel - QFrame::Shadow::Raised + QFrame::Raised 1344 - 852 + 850 556 - 178 + 140 background-color: rgb(37, 38, 42); - QFrame::Shape::StyledPanel + QFrame::StyledPanel - QFrame::Shadow::Raised + QFrame::Raised @@ -295,10 +295,10 @@ background-color: rgba(255, 255, 255, 0); background-color: rgb(37, 38, 42); - QFrame::Shape::StyledPanel + QFrame::StyledPanel - QFrame::Shadow::Raised + QFrame::Raised groupBox diff --git a/BeltTearingApp/widgets/ImageGridWidget.cpp b/BeltTearingApp/widgets/ImageGridWidget.cpp index 6e0de5b8..b12efd56 100644 --- a/BeltTearingApp/widgets/ImageGridWidget.cpp +++ b/BeltTearingApp/widgets/ImageGridWidget.cpp @@ -8,9 +8,9 @@ ImageGridWidget::ImageGridWidget(QWidget* parent) : QWidget(parent) { m_layout = new QGridLayout(this); - m_layout->setContentsMargins(8, 8, 8, 8); - m_layout->setHorizontalSpacing(8); - m_layout->setVerticalSpacing(8); + m_layout->setContentsMargins(0, 0, 0, 0); + m_layout->setHorizontalSpacing(0); + m_layout->setVerticalSpacing(0); setLayout(m_layout); // 设置控件大小策略为可扩展 @@ -127,14 +127,14 @@ void ImageGridWidget::rebuildGrid() { } void ImageGridWidget::resizeEvent(QResizeEvent* event) { - // QWidget::resizeEvent(event); + QWidget::resizeEvent(event); - // // 防止递归调用，只在尺寸真正变化时更新 - // static QSize lastSize; - // if (lastSize != event->size()) { - // lastSize = event->size(); - // updateTileSizes(); - // } + // 防止递归调用，只在尺寸真正变化时更新 + static QSize lastSize; + if (lastSize != event->size()) { + lastSize = event->size(); + updateTileSizes(); + } } void ImageGridWidget::updateTileSizes() { @@ -146,13 +146,15 @@ void ImageGridWidget::updateTileSizes() { int availableHeight = height() - m_layout->contentsMargins().top() - m_layout->contentsMargins().bottom(); // 计算每个格子的基础尺寸（根据窗口大小动态调整） - int baseTileWidth = qMax(100, availableWidth / m_columns - m_layout->horizontalSpacing() * (m_columns - 1)); - int baseTileHeight = qMax(100, availableHeight / m_rows - m_layout->verticalSpacing() * (m_rows - 1)); + // 确保减去的间距不会导致负数 + int horizontalSpacingTotal = m_layout->horizontalSpacing() * (m_columns - 1); + int verticalSpacingTotal = m_layout->verticalSpacing() * (m_rows - 1); + + int baseTileWidth = qMax(100, (availableWidth - horizontalSpacingTotal) / m_columns); + int baseTileHeight = qMax(100, (availableHeight - verticalSpacingTotal) / m_rows); // 根据窗口大小动态调整尺寸 m_sizeNormal = QSize(baseTileWidth, baseTileHeight); - m_sizeSelected = QSize(baseTileWidth * 1.5, baseTileHeight * 1.5); - m_sizeOther = QSize(baseTileWidth * 0.75, baseTileHeight * 0.75); m_sizeExpanded = QSize(availableWidth, availableHeight); for (int i = 0; i < m_tiles.size(); ++i) { diff --git a/BeltTearingApp/widgets/ImageGridWidget.h b/BeltTearingApp/widgets/ImageGridWidget.h index de4f0c97..b31286f7 100644 --- a/BeltTearingApp/widgets/ImageGridWidget.h +++ b/BeltTearingApp/widgets/ImageGridWidget.h @@ -43,7 +43,5 @@ protected: int m_columns {0}; int m_rows {0}; QSize m_sizeNormal {160,160}; - QSize m_sizeSelected {240,240}; - QSize m_sizeOther {120,120}; QSize m_sizeExpanded {400,300}; }; diff --git a/BeltTearingApp/widgets/ImageTileWidget.cpp b/BeltTearingApp/widgets/ImageTileWidget.cpp index 0e8ac247..a6d96a09 100644 --- a/BeltTearingApp/widgets/ImageTileWidget.cpp +++ b/BeltTearingApp/widgets/ImageTileWidget.cpp @@ -79,7 +79,7 @@ void ImageTileWidget::paintEvent(QPaintEvent*) { p.setRenderHint(QPainter::Antialiasing, true); // 绘制背景 - p.fillRect(rect(), QColor(245, 245, 245)); + p.fillRect(rect(), QColor(37, 38, 42)); if (m_expanded) { // 展开模式：全屏显示图片，在左下角显示图片名称 @@ -97,13 +97,13 @@ void ImageTileWidget::paintEvent(QPaintEvent*) { // 绘制图片（完整显示，不超出区域） p.drawPixmap(x, y, scaled); } else { - p.setPen(Qt::gray); + p.setPen(Qt::white); p.drawText(imageRect, Qt::AlignCenter, "No Image"); } } else { // 正常模式：图片完全显示，不拉伸 - QRect inner = rect().adjusted(8, 8, -8, -8); + QRect inner = rect().adjusted(0, 0, 0, 0); if (!m_pix.isNull()) { // 使用KeepAspectRatio保持比例，确保整个图片可见 QPixmap scaled = m_pix.scaled(inner.size(), Qt::KeepAspectRatio, Qt::SmoothTransformation); @@ -114,7 +114,7 @@ void ImageTileWidget::paintEvent(QPaintEvent*) { p.drawPixmap(x, y, scaled); } else { - p.setPen(Qt::gray); + p.setPen(Qt::white); p.drawText(inner, Qt::AlignCenter, "No Image"); } @@ -138,10 +138,10 @@ void ImageTileWidget::paintEvent(QPaintEvent*) { } // 绘制边框 - QPen border(m_selected ? QColor(66, 133, 244) : QColor(200, 200, 200)); - border.setWidth(m_selected ? 3 : 1); + QPen border(QColor(0, 0, 0)); + border.setWidth(1); p.setPen(border); - p.drawRoundedRect(rect().adjusted(1,1,-1,-1), 6, 6); + p.drawRoundedRect(rect().adjusted(1,1,-1,-1), 1, 1); } void ImageTileWidget::mousePressEvent(QMouseEvent* ev) { diff --git a/BeltTearingServer/BeltTearingAlgo.cpp b/BeltTearingServer/BeltTearingAlgo.cpp new file mode 100644 index 00000000..ccb45e00 --- /dev/null +++ b/BeltTearingServer/BeltTearingAlgo.cpp @@ -0,0 +1,121 @@ +#include "BeltTearingAlgo.h" +#include +#include + +BeltTearingAlgo::BeltTearingAlgo() { + algoParam.differnceBinTh = 1.0; + algoParam.extractPara.gapChkWin = 7; + algoParam.extractPara.sameGapTh = 20.0; + algoParam.scanXScale = 4.0; + algoParam.scanYScale = 0.6; + algoParam.tearingMinGap = 50.0; //两个同位置的纵撕的最小间隔。大于此门限视为两个撕裂 + algoParam.tearingMinLen = 30.0; + lineIndex = 0; + + // 初始化 + ResetParameter(); +} + +void BeltTearingAlgo::ResetParameter() { + total_tearings.clear(); + beltTearings_new.clear(); + beltTearings_growing.clear(); + beltTearings_ended.clear(); + beltTearings_unknown.clear(); + lineIndex = 0; + int errCode = 0; + sg_detectBeltTearing( + NULL, //空扫描线，用于复位内部静态变量 + 0, + 0, + &errCode, + hLineWorkers, + beltTearings_new, + beltTearings_growing, + beltTearings_ended, + beltTearings_unknown, //未判明，应用无需处理。 + algoParam); +} + +void BeltTearingAlgo::Training(std::vector lines) { + // 本算法无需训练 +} + +void BeltTearingAlgo::Training(SVzNL3DLaserLine* lines) { + // 本算法无需训练 +} + +std::vector BeltTearingAlgo::Predit(SVzNL3DLaserLine* a_line, int lineId) { + total_tearings.clear(); + int errCode = 0; + sg_detectBeltTearing( + a_line, //一条扫描线 + lineIndex, + a_line->nPositionCnt, + &errCode, + hLineWorkers, + beltTearings_new, + beltTearings_growing, + beltTearings_ended, + beltTearings_unknown, //未判明，应用无需处理。 + algoParam); + if (beltTearings_ended.size() > 0) //收集撕裂点 + { + total_tearings.insert(total_tearings.end(), beltTearings_ended.begin(), beltTearings_ended.end()); + } + + return total_tearings; +} + +std::vector BeltTearingAlgo::Predit(std::vector lines, int lineIdx) { + total_tearings.clear(); + if (lines.empty()) { + return total_tearings; + } + + if (lineIndex >= INT_MAX - lines.size() ) { + lineIndex = 0; + } + + hLineWorkers.resize(lines[0].nPositionCnt); + int errCode = 0; + for(int i = 0; i < lines.size(); i ++) { + SVzNL3DLaserLine* a_line = &lines[i]; + sg_detectBeltTearing( + a_line, //一条扫描线 + lineIndex, + a_line->nPositionCnt, + &errCode, + hLineWorkers, + beltTearings_new, + beltTearings_growing, + beltTearings_ended, + beltTearings_unknown, //未判明，应用无需处理。 + algoParam); + lineIndex += 1; + // if (beltTearings_unknown.size() > 0) { + // std::cout << "未知的撕裂数量: " << beltTearings_unknown.size() << std::endl; + // } + + // if (beltTearings_growing.size() > 0) { + // std::cout << "进行中的撕裂数量: " << beltTearings_growing.size() << std::endl; + // } + + if (beltTearings_new.size() > 0) //收集撕裂点 + { + total_tearings.insert(total_tearings.end(), beltTearings_new.begin(), beltTearings_new.end()); + } + + if (beltTearings_ended.size() > 0) //收集撕裂点 + { + total_tearings.insert(total_tearings.end(), beltTearings_ended.begin(), beltTearings_ended.end()); + } + else if (i == lines.size() - 1) //测试数据最后一条扫描线。（实际中没有） + { + total_tearings.insert(total_tearings.end(), beltTearings_growing.begin(), beltTearings_growing.end()); + } + } + + // TODO: 返回值有可能为空，也有可能为当前检测的撕裂 + return total_tearings; +} diff --git a/BeltTearingServer/BeltTearingAlgo.h b/BeltTearingServer/BeltTearingAlgo.h new file mode 100644 index 00000000..705a5229 --- /dev/null +++ b/BeltTearingServer/BeltTearingAlgo.h @@ -0,0 +1,28 @@ +#ifndef BELTTEARINGALGO_H +#define BELTTEARINGALGO_H +#include "beltTearingDetection_Export.h" + +using namespace std; + +class BeltTearingAlgo +{ +public: + BeltTearingAlgo(); + void ResetParameter(); + void Training(SVzNL3DLaserLine* lines); + void Training(std::vector lines); + std::vector Predit(SVzNL3DLaserLine* lines, int lineId); + std::vector Predit(std::vector lines, int lineIdx); + +private: + SSG_beltTearingParam algoParam; + std::vector hLineWorkers; + std::vector beltTearings_new; + std::vector beltTearings_growing; + std::vector beltTearings_ended; + std::vector beltTearings_unknown; //未判明，应用无需处理。 + std::vector total_tearings; + int lineIndex = 0; +}; + +#endif // BELTTEARINGALGO_H diff --git a/BeltTearingServer/BeltTearingPresenter.cpp b/BeltTearingServer/BeltTearingPresenter.cpp new file mode 100644 index 00000000..085c1ac5 --- /dev/null +++ b/BeltTearingServer/BeltTearingPresenter.cpp @@ -0,0 +1,209 @@ +#include "BeltTearingPresenter.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +BeltTearingPresenter::BeltTearingPresenter(QObject *parent) + : QObject(parent) + , m_tcpServer(new QTcpServer(this)) + , m_dataTimer(new QTimer(this)) + , m_port(0) +{ + // 连接信号槽 + connect(m_tcpServer, &QTcpServer::newConnection, this, &BeltTearingPresenter::onNewConnection); + connect(m_dataTimer, &QTimer::timeout, this, &BeltTearingPresenter::onSendSimulatedData); +} + +BeltTearingPresenter::~BeltTearingPresenter() +{ + stopServer(); +} + +bool BeltTearingPresenter::startServer(quint16 port) +{ + if (m_tcpServer->isListening()) { + stopServer(); + } + + m_port = port; + bool result = m_tcpServer->listen(QHostAddress::Any, port); + + if (result) { + std::cout << "TCP server started on port " << port << std::endl; + } else { + std::cout << "Failed to start TCP server: " << m_tcpServer->errorString().toStdString() << std::endl; + } + + return result; +} + +void BeltTearingPresenter::stopServer() +{ + // 断开所有客户端连接 + for (auto it = m_clients.begin(); it != m_clients.end(); ++it) { + it.value()->disconnectFromHost(); + } + m_clients.clear(); + + // 停止服务器监听 + if (m_tcpServer->isListening()) { + m_tcpServer->close(); + } + + // 停止数据发送定时器 + stopSendingSimulatedData(); + + m_port = 0; +} + +void BeltTearingPresenter::startSendingSimulatedData() +{ + // 启动定时器，每2秒发送一次模拟数据 + m_dataTimer->start(1000); +} + +void BeltTearingPresenter::stopSendingSimulatedData() +{ + if (m_dataTimer->isActive()) { + m_dataTimer->stop(); + } +} + +void BeltTearingPresenter::onNewConnection() +{ + while (m_tcpServer->hasPendingConnections()) { + QTcpSocket *socket = m_tcpServer->nextPendingConnection(); + + // 生成客户端ID + QString clientId = generateClientId(socket); + + // 存储客户端连接 + m_clients[clientId] = socket; + + // 连接客户端信号 + connect(socket, &QTcpSocket::disconnected, this, &BeltTearingPresenter::onClientDisconnected); + + std::cout << "Client connected: " << clientId.toStdString() << std::endl; + + // 发送欢迎消息 + QByteArray welcomeMsg = "Welcome to BeltTearing Server!"; + socket->write(welcomeMsg); + } +} + +void BeltTearingPresenter::onClientDisconnected() +{ + QTcpSocket *socket = qobject_cast(sender()); + if (!socket) { + return; + } + + // 查找断开连接的客户端ID + QString clientId; + for (auto it = m_clients.begin(); it != m_clients.end(); ++it) { + if (it.value() == socket) { + clientId = it.key(); + break; + } + } + + if (!clientId.isEmpty()) { + m_clients.remove(clientId); + std::cout << "Client disconnected: " << clientId.toStdString() << std::endl; + } + + socket->deleteLater(); +} + +void BeltTearingPresenter::onSendSimulatedData() +{ + // 向所有连接的客户端发送模拟数据 + for (auto it = m_clients.begin(); it != m_clients.end(); ++it) { + QTcpSocket *socket = it.value(); + if (socket->state() == QAbstractSocket::ConnectedState) { + // 随机选择发送图像或日志记录 + sendSimulatedImage(socket); + sendSimulatedLogRecords(socket); + } + } +} + +void BeltTearingPresenter::sendSimulatedImage(QTcpSocket *socket) +{ + // 创建一个模拟的图像 + cv::Mat image(480, 640, CV_8UC3, cv::Scalar(128, 128, 128)); + + // 在图像上绘制一些随机内容 + cv::circle(image, cv::Point(320, 240), 50, cv::Scalar(0, 0, 255), -1); + cv::rectangle(image, cv::Rect(100, 100, 200, 150), cv::Scalar(0, 255, 0), 2); + + static unsigned int s_imageCount = 0; + // 在图像上叠加数字 + QString numberText = QString::number(++s_imageCount); // 生成一个随机数 + cv::putText(image, numberText.toStdString(), cv::Point(50, 50), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), 2); + + // 编码图像 + std::vector imgBuffer; + std::vector params = {cv::IMWRITE_PNG_COMPRESSION, 3}; + cv::imencode(".png", image, imgBuffer, params); + + QByteArray byteArray(reinterpret_cast(imgBuffer.data()), imgBuffer.size()); + + // 创建数据包 + QByteArray package; + QDataStream stream(&package, QIODevice::WriteOnly); + stream.setByteOrder(QDataStream::BigEndian); + stream << quint8(static_cast(ByteDataType::Image)) << quint32(byteArray.size()); + package.append(byteArray); + package.append("___END___\r\n"); + + // 发送数据 + socket->write(package); + + std::cout << "Sent simulated image data to client" << std::endl; +} + +void BeltTearingPresenter::sendSimulatedLogRecords(QTcpSocket *socket) +{ + // 创建模拟日志记录 + QJsonArray logRecords; + + for (int i = 0; i < 3; i++) { + QJsonObject record; + record["timestamp"] = QDateTime::currentDateTime().toString(Qt::ISODate); + record["level"] = QString("INFO"); + record["message"] = QString("Simulated log message %1").arg(i + 1); + record["source"] = QString("BeltTearingDetector"); + + logRecords.append(record); + } + + // 将日志记录转换为JSON字符串 + QJsonDocument doc(logRecords); + QByteArray message = doc.toJson(QJsonDocument::Compact); + + // 创建数据包 + QByteArray package; + QDataStream stream(&package, QIODevice::WriteOnly); + stream.setByteOrder(QDataStream::BigEndian); + stream << quint8(static_cast(ByteDataType::Text)) << quint32(message.size()); + package.append(message); + package.append("___END___\r\n"); + + // 发送数据 + socket->write(package); + + std::cout << "Sent simulated log records to client" << std::endl; +} + +QString BeltTearingPresenter::generateClientId(QTcpSocket *socket) +{ + // 使用UUID生成唯一的客户端ID + return QUuid::createUuid().toString(); +} diff --git a/BeltTearingServer/BeltTearingPresenter.h b/BeltTearingServer/BeltTearingPresenter.h new file mode 100644 index 00000000..20b272ee --- /dev/null +++ b/BeltTearingServer/BeltTearingPresenter.h @@ -0,0 +1,50 @@ +#ifndef BELTTEARINGPRESENTER_H +#define BELTTEARINGPRESENTER_H + +#include +#include +#include +#include +#include +#include + +// 假设ByteDataType枚举定义 +enum class ByteDataType : quint8 { + Text = 1, + Image = 2 +}; + +class BeltTearingPresenter : public QObject +{ + Q_OBJECT + +public: + explicit BeltTearingPresenter(QObject *parent = nullptr); + ~BeltTearingPresenter(); + + // 初始化TCP服务器 + bool startServer(quint16 port); + void stopServer(); + + // 模拟数据发送 + void startSendingSimulatedData(); + void stopSendingSimulatedData(); + +private slots: + void onNewConnection(); + void onClientDisconnected(); + void onSendSimulatedData(); + +private: + QTcpServer *m_tcpServer; + QMap m_clients; + QTimer *m_dataTimer; + quint16 m_port; + + // 模拟数据发送方法 + void sendSimulatedImage(QTcpSocket *socket); + void sendSimulatedLogRecords(QTcpSocket *socket); + QString generateClientId(QTcpSocket *socket); +}; + +#endif // BELTTEARINGPRESENTER_H diff --git a/BeltTearingServer/BeltTearingServer.pro b/BeltTearingServer/BeltTearingServer.pro new file mode 100644 index 00000000..98166bee --- /dev/null +++ b/BeltTearingServer/BeltTearingServer.pro @@ -0,0 +1,104 @@ +QT += core gui +QT += serialport +QT += network + +greaterThan(QT_MAJOR_VERSION, 4): QT += widgets + +TEMPLATE = app + +CONFIG += c++11 console + +# You can make your code fail to compile if it uses deprecated APIs. +# In order to do so, uncomment the following line. +#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0 + +INCLUDEPATH += $$PWD/../VrCommon/Inc +INCLUDEPATH += $$PWD/../VrUtils/Inc +INCLUDEPATH += $$PWD/Presenter/Inc +INCLUDEPATH += $$PWD/Utils/Inc + +INCLUDEPATH += $$PWD/../VrConfig/Inc +INCLUDEPATH += $$PWD/../VrEyeDevice/Inc + +SOURCES += \ + BeltTearingAlgo.cpp \ + beltTearingDetection.cpp \ + main.cpp \ + BeltTearingPresenter.cpp + +HEADERS += \ + SG_baseAlgo_Export.h \ + SG_baseDataType.h \ + SG_errCode.h \ + beltTearingDetection_Export.h \ + BeltTearingAlgo.h \ + BeltTearingPresenter.h + +FORMS += + +win32:CONFIG(debug, debug|release) { + LIBS += -L../VrUtils/debug -lVrUtils + LIBS += -L../VrEyeDevice/debug -lVrEyeDevice +}else:win32:CONFIG(release, debug|release){ + LIBS += -L../VrUtils/release -lVrUtils + LIBS += -L../VrEyeDevice/release -lVrEyeDevice +}else:unix:!macx { + # Unix/Linux平台库链接（包括交叉编译） + # 注意链接顺序：依赖关系从高到低 + LIBS += -L../VrUtils -lVrUtils + LIBS += -L../VrEyeDevice -lVrEyeDevice + + # 添加系统库依赖 + LIBS += -lpthread +} + +#linux下的为unix ，windows下用的win32 + +INCLUDEPATH += ../SDK/VzNLSDK/_Inc +INCLUDEPATH += ../SDK/VzNLSDK/Inc + +win32:CONFIG(release, debug|release): { + LIBS += -L$$PWD/../SDK/VzNLSDK/Windows/x64/Release + LIBS += -lVzKernel -lVzNLDetect -lVzNLGraphics +} +else:win32:CONFIG(debug, debug|release): { + LIBS += -L$$PWD/../SDK/VzNLSDK/Windows/x64/Debug + LIBS += -lVzKerneld -lVzNLDetectd -lVzNLGraphicsd +} +else:unix:!macx: { + LIBS += -L$$PWD/../SDK/VzNLSDK/Arm/aarch64 + LIBS += -lVzEyeSecurityLoader-shared -lVzKernel -lVzNLDetect -lVzNLGraphics +} + + +# 算法 +INCLUDEPATH += ../SDK/OpenCV320/include +win32:CONFIG(release, debug|release): { + LIBS += -L$$PWD/../SDK/OpenCV320/Windows/vc14/Release -lopencv_world320 +} +else:win32:CONFIG(debug, debug|release): { + LIBS += -L$$PWD/../SDK/OpenCV320/Windows/vc14/Release -lopencv_world320 +} +else:unix:!macx: { + LIBS += -L$$PWD/../SDK/OpenCV320/Arm/aarch64 -lopencv_core -lopencv_imgproc -lopencv_highgui +} + +# 添加libmodbus依赖 +win32 { + LIBS += -lws2_32 + LIBS += Advapi32.lib +} + +# Default rules for deployment. +unix { + target.path = /usr/lib + # Link real-time library for POSIX shared memory functions + LIBS += -lrt +} +!isEmpty(target.path): INSTALLS += target + + +# Default rules for deployment. +qnx: target.path = /tmp/$${TARGET}/bin +else: unix:!android: target.path = /opt/$${TARGET}/bin +!isEmpty(target.path): INSTALLS += target diff --git a/BeltTearingServer/SG_baseAlgo_Export.h b/BeltTearingServer/SG_baseAlgo_Export.h new file mode 100644 index 00000000..d95c4619 --- /dev/null +++ b/BeltTearingServer/SG_baseAlgo_Export.h @@ -0,0 +1,232 @@ +#pragma once + +#if defined(SG_API_LIBRARY) +# define SG_APISHARED_EXPORT __declspec(dllexport) +#else +# define SG_APISHARED_EXPORT __declspec(dllimport) +#endif + +#include "SG_baseDataType.h" +#include +#include + +//滤除离群点:z跳变门限方法（大于门限视为不连续，根据连续段点数量判断噪声） +void sg_lineDataRemoveOutlier( + SVzNL3DPosition* lineData, + int dataSize, + SSG_outlierFilterParam filterParam, + std::vector& filerData, + std::vector& noisePts); +//滤除离群点：z跳变门限方法,改变原始数据 +void sg_lineDataRemoveOutlier_changeOriginData( + SVzNL3DPosition* lineData, + int dataSize, + SSG_outlierFilterParam filterParam); + +//滤除离群点：点距离门限方法（大于门限视为不连续，根据连续段点数量判断噪声） +void sg_lineDataRemoveOutlier_ptDistMethod( + SVzNL3DPosition* lineData, + int dataSize, + SSG_outlierFilterParam filterParam, + std::vector& filerData, + std::vector& noisePts); +//平滑 +void sg_lineDataSmoothing( + std::vector& input, + int smoothWin, + std::vector& output); + +//VZ_APISHARED_EXPORT void sg_getLineMeanVar(); +void sg_getLineLVFeature( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const SSG_slopeParam slopeParam, + const SSG_VFeatureParam valleyPara, + SSG_lineFeature* line_features); + +//获取扫描线拐点特征 +void sg_getLineCornerFeature( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const SSG_cornerParam cornerPara, //scale通常取bagH的1/4 + SSG_lineFeature* line_features); + +/// +/// 提取激光线上的极值点（极大值点和极小值点） +/// +/// +void sg_getLineLocalPeaks( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const double scaleWin, + std::vector< SSG_basicFeature1D>& localMax, + std::vector< SSG_basicFeature1D>& localMin); + +void sg_getLineDownJumps( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const double jumpTh, + std::vector< SSG_basicFeature1D>& downJumps); + +//对feature进行生长 +void sg_getFeatureGrowingTrees( + std::vector& lineFeatures, + std::vector& trees, + SSG_treeGrowParam growParam); + +//对ending进行生长 +void sg_getEndingGrowingTrees( + std::vector& lineEndings, + SVzNL3DLaserLine* laser3DPoints, + bool isVScan, + int featureType, + std::vector& trees, + SSG_treeGrowParam growParam); + +//对扫描线上的 +void sg_LVFeatureGrowing( + std::vector& lineFeatures, + std::vector& trees, + SSG_bagParam bagParam, + SSG_treeGrowParam growParam, + std::vector& edgePts_0, + std::vector& edgePts_1); + +void sg_peakFeatureGrowing( + std::vector>& lineFeatures, + std::vector& trees, + SSG_treeGrowParam growParam); + +SSG_meanVar _computeMeanVar(double* data, int size); + +///搜索局部最高点（z最小点）。 +///搜索方法：每次步进搜索窗口长度的一半。对局部最高点进行标记，防止被重复记录。 +void sg_getLocalPeaks( + SVzNL3DLaserLine* scanLines, + int lineNum, + std::vector& peaks, + SSG_localPkParam searchWin); + +/// +/// 区域生长法：以局部最高点作为生长种子进行生长 +/// 生长方法与一般的区域生长不同：以种子点为圆心作圆周扫描，记录扫描到的边界 +/// +//void sg_peakPolarScan(cv::Mat& edgeMask, SVzNL2DPoint a_peak, SSG_polarScanParam polarScanParam, std::vector< SSG_2DValueI>& rgnContour); +//从Peak点进行水平垂直扫描得到区域边界 +void sg_peakXYScan( + SVzNL3DLaserLine* laser3DPoints, + int lineNum, + cv::Mat& featureEdgeMask, + SSG_2DValueI a_peak, + SSG_treeGrowParam growParam, + SSG_bagParam bagParam, + bool rgnPtAsEdge, + std::vector< SSG_lineConotours>& topContour, + std::vector< SSG_lineConotours>& bottomContour, + std::vector< SSG_lineConotours>& leftContour, + std::vector< SSG_lineConotours>& rightContour, + int* maxEdgeId_top, + int* maxEdgeId_btm, + int* maxEdgeId_left, + int* maxEdgeId_right); + +//取出与给定edgeId相同的边界点 +void sg_getContourPts( + std::vector< SSG_lineConotours>& contour_all, + int vldEdgeId, + std::vector< SSG_2DValueI>& contourFilter, + int* lowLevelFlag); + +//从边界点对中取出与给定edgeId相同的边界点 +void sg_getPairingContourPts( + std::vector& contourPairs, + std::vector& idPairs, + std::vector< SSG_conotourPair>& contourFilter, + SVzNLRangeD range, + bool isTBDir, + int* lowLevelFlag_0, + int* lowLevelFlag_1); + +//取出最短距离的边界点 +void sg_contourPostProc( + std::vector< SSG_contourPtInfo>& contour, + int maxEdgeIdx, + double sameConturDistTh, + std::vector< SSG_2DValueI>& contourFilter, + int sideID, + int* blockFlag); + +//距离变换 +//input, output均为float型 +void sg_distanceTrans(const cv::Mat input, cv::Mat& output, int distType); + +/// +/// 以5x5方式寻找localPeaks +/// +/// +/// +void sg_getLocalPeaks_distTransform( + cv::Mat& input, + std::vector& peaks, + SSG_localPkParam searchWin); + +/// +/// 使用模板法提取直角特征 +/// 水平向下直角特征：拐点左侧deltaZ在一个很小的范围内；拐点右侧deltaY在一个很小的范围内 +/// +void sg_getLineRigthAngleFeature( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const SSG_lineRightAngleParam templatePara_HF, + const SSG_lineRightAngleParam templatePara_FH, + const SSG_lineRightAngleParam templatePara_HR, + const SSG_lineRightAngleParam templatePara_RH, + SSG_lineFeature* line_features); + +//计算扫描ROI +SVzNL3DRangeD sg_getScanDataROI( + SVzNL3DLaserLine* laser3DPoints, + int lineNum); + +//XY平面直线拟合 +void lineFitting( + std::vector< SVzNL3DPoint>& inliers, + double* _k, + double* _b); + +//Bresenham算法 +void drawLine( + int x0, + int y0, + int x1, + int y1, + std::vector& pts); + +/// +/// 两步法标注 +/// +/// 目标点为“1”， 空白点为“0” +/// 标注结果。每个点为rgnID, ID从2开始 +/// +void SG_TwoPassLabel( + const cv::Mat& bwImg, + cv::Mat& labImg, + std::vector& labelRgns, + int connectivity = 4); + +//计算一个平面调平参数。 +//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平 +//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数 +SSG_planeCalibPara sg_getPlaneCalibPara( + SVzNL3DLaserLine* laser3DPoints, + int lineNum); + +// 从旋转矩阵计算欧拉角（Z-Y-X顺序） +SSG_EulerAngles rotationMatrixToEulerZYX(const double R[3][3]); +// 从欧拉角计算旋转矩阵（Z-Y-X顺序） +void eulerToRotationMatrixZYX(const SSG_EulerAngles& angles, double R[3][3]); \ No newline at end of file diff --git a/BeltTearingServer/SG_baseDataType.h b/BeltTearingServer/SG_baseDataType.h new file mode 100644 index 00000000..417c5c56 --- /dev/null +++ b/BeltTearingServer/SG_baseDataType.h @@ -0,0 +1,337 @@ +#pragma once + +#include +#include + +#define PI 3.141592654 + +// 定义欧拉角结构体（单位：度） +typedef struct{ + double roll; // 绕X轴旋转（横滚） + double pitch; // 绕Y轴旋转（俯仰） + double yaw; // 绕Z轴旋转（偏航） +}SSG_EulerAngles; + +//以工作轴为X轴(扫描方向），平行地面与Z轴垂直的轴为Y轴，垂直地面的轴为Z轴 +typedef struct +{ + bool validFlag; //指示结果是否有效 + double x; + double y; + double z; + double pitchAngle; //俯仰角：绕Y轴的偏转, 弧度 + double rollAngle; //滚转角：绕X轴的偏转, 弧度 + double yawAngle; //偏转角：绕Z轴的偏转, 弧度 +}SSG_6AxisAttitude; + +typedef struct +{ + bool validFlag; //指示结果是否有效 + double x; + double y; + double z; +}SSG_3AxisAttitude; + +typedef enum +{ + keSG_PoseSorting_Uknown = 0, + keSG_PoseSorting_ZMin2Max, ///按Z排序 + keSG_PoseSorting_L2R_T2B, ///从左到右，从上到下排序 + keSG_PoseSorting_T2B_L2R, ///从上到下，从左到右排序 +} ESG_poseSortingMode; + +typedef struct +{ + int data_0; + int data_1; + int idx; +}SSG_intPair; + +typedef struct +{ + double left; + double right; + double top; + double bottom; +}SSG_ROIRectD; + +#define LINE_FEATURE_NUM 16 +#define LINE_FEATURE_UNDEF 0 +#define LINE_FEATURE_L_JUMP_H2L 1 +#define LINE_FEATURE_L_JUMP_L2H 2 +#define LINE_FEATURE_L_SLOPE_H2L 3 +#define LINE_FEATURE_L_SLOPE_L2H 4 +#define LINE_FEATURE_V_SLOPE 5 +#define LINE_FEATURE_LINE_ENDING_0 6 //ending起点 +#define LINE_FEATURE_LINE_ENDING_1 7 //ending终点 +#define LINE_FEATURE_RGN_EDGE 8 //迭代处理时已经得到的目标的边缘点标记 +#define LINE_FEATURE_RIGHT_ANGLE_HR 9 //直角特征:水平-上升 +#define LINE_FEATURE_RIGHT_ANGLE_HF 10 //直角特征:水平-下降 +#define LINE_FEATURE_RIGHT_ANGLE_RH 11 //直角特征:上升-水平 +#define LINE_FEATURE_RIGHT_ANGLE_FH 12 //直角特征:下降-水平 +#define LINE_FEATURE_PEAK_TOP 13 +#define LINE_FEATURE_PEAK_BOTTOM 14 +#define LINE_FEATURE_CORNER_V 15 + + +typedef struct +{ + int featureType; + SVzNL2DPoint jumpPos2D; + SVzNL3DPoint jumpPos; +}SSG_basicFeature1D; + +typedef struct +{ + int lineIdx; + double gap_start; + double gap_width; + double gap_depth; + SVzNL3DPosition gapPt_0; //gap的端点 + SVzNL3DPosition gapPt_1; //gap的端点 + SSG_ROIRectD roi; +}SSG_basicFeatureGap; + +typedef struct +{ + int lineIdx; + std::vector features; + std::vector endings; +}SSG_lineFeature; + +typedef struct +{ + /* 点是否连续门限，小于此门限，视为点连续 */ + double continuityTh; //点连续门限。当使用z跳变判断连续性时，为z跳变门限；当使用点距离判断时，为点距离门限 + /* 噪声判断门限，当连续段的点数据小于此门限，视为噪声 */ + double outlierTh; +}SSG_outlierFilterParam; + +typedef struct +{ + double LSlopeZWin; //对L型Slope进行坡度计算的窗口长度 + double validSlopeH; + double minLJumpH; + double minEndingGap;//连续段门限。大于此门限，为不连续 +}SSG_slopeParam; + +typedef struct +{ + double minEndingGap; //连续段门限。大于此门限，为不连续 + double scale; //计算方向角的窗口比例尺 + double cornerTh; //拐角门限，大于此门限，为有效拐点 + double jumpCornerTh_1; //判断拐角是否为跳变的两个门限。当一个门限大于jumpCornerTh_1且另一个小于jumpCornerTh_2时跳变 + double jumpCornerTh_2; +}SSG_cornerParam; + +typedef struct +{ + double H_len; //直角特征水平段的长度 + double V_len; //直角特征垂直段的长度 + double maxDelta; +}SSG_lineRightAngleParam; + +typedef struct +{ + double valleyMinH; + double valleyMaxW; +}SSG_VFeatureParam; + +typedef struct +{ + double sameGapTh; + int gapChkWin; +}SSG_tearFeatureExtactPara; +typedef struct +{ + double yDeviation_max;//生长时相邻特征最大的Y偏差 + double zDeviation_max; //生长时相邻特征最大的Z偏差 + int maxLineSkipNum; //生长时相邻特征的最大线间隔， -1时使用maxDkipDistance + double maxSkipDistance; //若maxLineSkipNum为-1， 使用此参数.设为-1时，此参数无效 + double minLTypeTreeLen; //生长树最少的节点数目。小于此数目的生长树被移除 + double minVTypeTreeLen; //生长树最少的节点数目。小于此数目的生长树被移除 +}SSG_treeGrowParam; + +typedef struct +{ + double bagL; //长 + double bagW; //宽 + double bagH; //高 +}SSG_bagParam; + +typedef struct +{ + ESG_poseSortingMode sortMode; +}SSG_objSortParam; + +typedef struct +{ + double angleStep; + double radiusStep; +}SSG_polarScanParam; + +typedef struct +{ + int treeState; + int treeType; + int sLineIdx; + int eLineIdx; + SSG_ROIRectD roi; + std::vector< SSG_basicFeature1D> treeNodes; +}SSG_featureTree; + +typedef struct +{ + int vTreeFlag; + int treeIdx; + int treeType; + int sLineIdx; + int eLineIdx; + SSG_ROIRectD roi; +}SSG_treeInfo; + +typedef struct +{ + int seachW_lines; + int searchW_pts; +}SSG_localPkParam; + +typedef struct +{ + int x; + int y; + int value; + double valueD; + int sideID; //1-T, 2-B, 3-L, 4-R +}SSG_2DValueI; + +typedef struct +{ + int start; + int len; + int value; +}SSG_RUN; + +typedef struct +{ + double mean; + double var; +}SSG_meanVar; + +typedef struct +{ + int x; + int y; + int type; + int edgeId; + int blockFlag;//遮挡标志 + double scanDist; + SVzNL3DPoint edgePt; +}SSG_contourPtInfo; + +typedef struct +{ + int lineIdx; + std::vector contourPts; +}SSG_lineConotours; + +typedef struct +{ + int lineIdx; + int edgeId_0; + int edgeId_1; + double ptPairDist; + SSG_contourPtInfo contourPt_0; + SSG_contourPtInfo contourPt_1; +}SSG_conotourPair; + +typedef struct +{ + double x; + double y; + double z; + double x_roll; + double y_pitch; + double z_yaw; +}SSG_6DOF; + +typedef struct +{ + double L; + double W; + double H; + double angle; + SVzNL3DPoint endings[4]; +} SSG_boxCarDimension; + +typedef struct +{ + SVzNL3DPoint opCenter; //定子中心位置 + int neighbourNum; //相邻目标数量 + double opAngle; //定子外部相隔120度操作点的操作角度。以Y形为基准角度（0度) + double obstacleDist; //距离周边障碍的距离 +}SSG_motorStatorPosition; + +typedef struct +{ + int pkRgnIdx; + SVzNLSizeD objSize; + SVzNL2DPoint pos2D; + SSG_6DOF centerPos; +}SSG_peakRgnInfo; + +typedef struct +{ + SVzNL3DPoint sPt; + SVzNL3DPoint ePt; + double scanDist; + int ptNum; +}SSG_contourEdgeInfo; + +typedef struct +{ + double meanDist; + double varDist; + double minDist; + double maxDist; + int matchNum; + int level2_num; //level2为高可信度(两端为Jump或Ending; + int level1_num; //level1-为中可信度（一端为Jump或Ending） + SSG_ROIRectD roi; +}SSG_edgeMatchInfo; + +typedef struct +{ + int id1; + int id2; + int matchPts; + int matchType; //可信度， 2为高可信度(两端为Jump或Ending; 1-为中可信度（一端为Jump或Ending）；0-为低可信度 + double matchValue; + double varValue; + double minMatchValue; + double maxMatchValue; + SSG_ROIRectD roi; +}SSG_matchPair; + +typedef struct +{ + int objIdx; + bool isValid; //是否为合格目标。顶层目标，但编织袋横放（扫描到长边）为不合格目标 + SSG_ROIRectD objROI; + SVzNL3DPoint objPos; + SSG_6DOF graspPos; +}SSG_sideBagInfo; + +typedef struct +{ + SVzNLRect roi; + int ptCounter; + int labelID; +}SSG_Region; + +typedef struct +{ + double planeCalib[9]; //旋转矩阵，将点云地面调平 + double planeHeight;//地面调平后的高度，用于去除地面数据 + double invRMatrix[9]; //旋转矩阵，回到原坐标系 +}SSG_planeCalibPara; diff --git a/BeltTearingServer/SG_errCode.h b/BeltTearingServer/SG_errCode.h new file mode 100644 index 00000000..d978050b --- /dev/null +++ b/BeltTearingServer/SG_errCode.h @@ -0,0 +1,7 @@ +#pragma once + +#define SG_ERR_3D_DATA_INVLD -1000 +#define SG_ERR_FOUND_NO_TOP_PLANE -1001 +#define SG_ERR_NOT_GRID_FORMAT -1002 +#define SG_ERR_LABEL_INFO_ERROR -1003 +#define SG_ERR_INVLD_SORTING_MODE -1004 diff --git a/BeltTearingServer/beltTearingDetection.cpp b/BeltTearingServer/beltTearingDetection.cpp new file mode 100644 index 00000000..def77ce0 --- /dev/null +++ b/BeltTearingServer/beltTearingDetection.cpp @@ -0,0 +1,1283 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "beltTearingDetection_Export.h" +#include +#include "SG_errCode.h" + + +typedef struct +{ + int tearID; + int vGap_num; + double vGap_depth_max; + double vGap_width_max; + double vGap_width_sum; //用于计算均值 + int hGap_num; + double hGap_depth_max; + double hGap_width_max; //最大值为撕裂最宽处 + double hGap_width_sum; //用于计算均值 + +}SSG_tearProcessInfo; //撕裂处理过程数据 + +double _getGapDepth(std::vector& vldPts, int start, int end) +{ + double start_z = vldPts[start].pt3D.z; + double end_z = vldPts[end].pt3D.z; + double max_z = start_z; + for (int i = start+1; i <= end; i++) + { + if (max_z < vldPts[i].pt3D.z) + max_z = vldPts[i].pt3D.z; + } + double depth = max_z - (start_z + end_z) / 2; + return depth; +} + +void _combineROI(SSG_ROIRectD* roi, SSG_ROIRectD* sub_roi) +{ + if (roi->left > sub_roi->left) + roi->left = sub_roi->left; + if (roi->right < sub_roi->right) + roi->right = sub_roi->right; + if (roi->top > sub_roi->top) + roi->top = sub_roi->top; + if (roi->bottom < sub_roi->bottom) + roi->bottom = sub_roi->bottom; + return; +} + +bool compareByStartY(const SSG_basicFeatureGap& a, const SSG_basicFeatureGap& b) { + return a.gap_start < b.gap_start; +} +//垂直扫描线处理，进行垂直方向的特征提取 +//垂直扫描线：点间距小于1mm，使用差分公式。 +void sg_bearTearing_vLineProc( + SVzNL3DLaserLine* a_line, //当前扫描线 + int lineIdx, //当前扫描线序号 + const SSG_beltTearingParam measureParam, + std::vector& vLineFeatures) +{ + //double minGapW = 1.0; + + //从栅格数据生成非零序列 + //统计Gap + std::vector vldPts; + std::vector gaps; + int preIdx = -1; + int vldPtSize = 0; + int gapSize = 0; + for (int i = 0; i < a_line->nPositionCnt; i++) + { + SVzNL3DPosition a_pt = a_line->p3DPosition[i]; + if (a_pt.pt3D.z > 1e-4) + { + a_pt.nPointIdx = -1; + if (preIdx >= 0) + { + int gapPtNum = i - preIdx - 1; + double gapW = abs(a_pt.pt3D.y - vldPts[vldPtSize - 1].pt3D.y); + if ((gapPtNum >= 2) && (gapW > 1.0)) + { + SSG_RUN a_gap = { vldPtSize-1, 2, 0};//vldGap + gaps.push_back(a_gap); + vldPts[vldPtSize - 1].nPointIdx = gapSize; + a_pt.nPointIdx = gapSize; + gapSize++; + } + } + vldPts.push_back(a_pt); + vldPtSize++; + preIdx = i; + } + } + + //计算一阶差分值 + std::vector difference; + std::vector diffBinValue; + difference.resize(vldPts.size()); + diffBinValue.resize(vldPts.size()); + for (int i = 1, i_max = vldPts.size(); i < i_max; i++) + { + double a_difference = (vldPts[i].pt3D.z - vldPts[i-1].pt3D.z) / (vldPts[i].pt3D.y - vldPts[i - 1].pt3D.y); + difference[i] = a_difference; + int a_binValue; + a_binValue = a_difference > measureParam.differnceBinTh ? 1 : (a_difference < -measureParam.differnceBinTh ? -1 : 0); + diffBinValue[i] = a_binValue; + if (1 == i) //第一个 + { + difference[0] = a_difference; + diffBinValue[0] = a_binValue; + } + } + + //检测+1序列和-1序列 + std::vector< SSG_RUN> runs; + SSG_RUN a_run; + memset(&a_run, 0, sizeof(SSG_RUN)); + for (int i = 1, i_max = vldPts.size(); i < i_max; i++) + { + if (0 != diffBinValue[i]) + { + if (0 == a_run.value) + { + a_run.start = i; + a_run.len = 1; + a_run.value = diffBinValue[i]; + } + else + { + if (a_run.value != diffBinValue[i]) + { + runs.push_back(a_run); + a_run.start = i; + a_run.len = 1; + a_run.value = diffBinValue[i]; + } + else + { + a_run.len++; + } + } + } + else + { + if (0 != a_run.value) + { + runs.push_back(a_run); + memset(&a_run, 0, sizeof(SSG_RUN)); + } + } + } + if (0 != a_run.value) //最后一个 + { + runs.push_back(a_run); + } + + //配对生成特征 + for (int i = 0, i_max = runs.size(); i < i_max; i++) + { + SSG_RUN a_run = runs[i]; + if (0 == a_run.value) + continue; + + if (i < i_max-1) + { + SSG_RUN* nxt_run = &runs[i + 1]; + //检查是否可以配对 + if (a_run.value != nxt_run->value) + { + //检查距离 + double pos_end = vldPts[a_run.start + a_run.len - 1].pt3D.y; + double nxt_pos_start = vldPts[nxt_run->start].pt3D.y; + double dist = nxt_pos_start - pos_end; + if (dist < measureParam.extractPara.sameGapTh) + { + a_run.len = nxt_run->start + nxt_run->len - a_run.start; + nxt_run->value = 0; //invalid + } + } + } + //检查前后是否有Gap，精确确定起始点 + //在前后检查10个点，看是否有Gap + int start = a_run.start; + int end = a_run.start + a_run.len - 1; + bool hasGap = false; + for (int m = 0; m < measureParam.extractPara.gapChkWin; m++) + { + int ptIdx = a_run.start - m; + if (ptIdx > 0) + { + double gapLen = vldPts[ptIdx].pt3D.y - vldPts[ptIdx-1].pt3D.y; + if (gapLen > measureParam.scanYScale * 2) //找到Gap + { + start = ptIdx - 1; + hasGap = true; + if (vldPts[ptIdx].nPointIdx >= 0) + gaps[vldPts[ptIdx].nPointIdx].value = 1; //invalid + if(vldPts[ptIdx - 1].nPointIdx >= 0) + gaps[vldPts[ptIdx - 1].nPointIdx].value = 1; //invalid + break; + } + } + } + int vldPtSize = vldPts.size(); + for (int m = 0; m < measureParam.extractPara.gapChkWin; m++) + { + int ptIdx = a_run.start + a_run.len - 1 + m; + if (ptIdx < vldPtSize-1) + { + double gapLen = vldPts[ptIdx+1].pt3D.y - vldPts[ptIdx ].pt3D.y; + if (gapLen > measureParam.scanYScale * 2) //找到Gap + { + end = ptIdx + 1; + hasGap = true; + if (vldPts[ptIdx].nPointIdx >= 0) + gaps[vldPts[ptIdx].nPointIdx].value = 1; //invalid + if (vldPts[ptIdx + 1].nPointIdx >= 0) + gaps[vldPts[ptIdx + 1].nPointIdx].value = 1; //invalid + break; + } + } + } + if (end > start) //gap必须大于0 + { + //生成特征 + SSG_basicFeatureGap a_feature; + memset(&a_feature, 0, sizeof(SSG_basicFeatureGap)); + a_feature.lineIdx = lineIdx; + a_feature.gap_start = vldPts[start].pt3D.y; + a_feature.gap_width = vldPts[end].pt3D.y - vldPts[start].pt3D.y; + a_feature.gapPt_0 = vldPts[start]; + a_feature.gapPt_1 = vldPts[end]; + double x1, x2; + if (vldPts[end].pt3D.x > vldPts[start].pt3D.x) + { + x1 = vldPts[start].pt3D.x; + x2 = vldPts[end].pt3D.x; + } + else + { + x1 = vldPts[end].pt3D.x; + x2 = vldPts[start].pt3D.x; + } + a_feature.roi = { x1, x2, vldPts[start].pt3D.y, vldPts[end].pt3D.y }; + if (true == hasGap) + a_feature.gap_depth = -1; //穿透型撕裂 + else + a_feature.gap_depth = _getGapDepth(vldPts, start, end); + //检查与前一个feature是否交叉或相连 + if (vLineFeatures.size() > 0) + { + SSG_basicFeatureGap* pre_feature = &vLineFeatures[vLineFeatures.size() - 1]; + if ((a_feature.roi.bottom > pre_feature->roi.top) && (pre_feature->roi.bottom > a_feature.roi.top)) //合并 + { + //合并 + pre_feature->gapPt_1 = a_feature.gapPt_1; + _combineROI(&pre_feature->roi, &a_feature.roi); + if ((a_feature.gap_depth < 0) || (pre_feature->gap_depth < 0)) + pre_feature->gap_depth = -1; + else + pre_feature->gap_depth = pre_feature->gap_depth < a_feature.gap_depth ? a_feature.gap_depth : pre_feature->gap_depth; + pre_feature->gap_width = pre_feature->roi.bottom - pre_feature->roi.top; + } + else + { + double dist_y = a_feature.roi.top - pre_feature->roi.bottom; + if (dist_y < measureParam.scanYScale * 3) + { + //合并 + pre_feature->gapPt_1 = a_feature.gapPt_1; + _combineROI(&pre_feature->roi, &a_feature.roi); + if ((a_feature.gap_depth < 0) || (pre_feature->gap_depth < 0)) + pre_feature->gap_depth = -1; + else + pre_feature->gap_depth = pre_feature->gap_depth < a_feature.gap_depth ? a_feature.gap_depth : pre_feature->gap_depth; + pre_feature->gap_width = pre_feature->roi.bottom - pre_feature->roi.top; + } + else + vLineFeatures.push_back(a_feature); + } + } + else //if(a_gap.gap_width > minGapW) //小于minGapW的视为噪声 + vLineFeatures.push_back(a_feature); + } + } +#if KEEP_GAP_AS_FEATURE + //将Gap生成feature + int gapFeatureNum = 0; + for (int i = 0, i_max = gaps.size(); i < i_max; i++) + { + SSG_RUN* a_run = &gaps[i]; + if (0 == a_run->value) //valid + { + int start = a_run->start; + int end = a_run->start + a_run->len - 1; + SSG_basicFeatureGap a_gap; + memset(&a_gap, 0, sizeof(SSG_basicFeatureGap)); + a_gap.lineIdx = lineIdx; + a_gap.gap_start = vldPts[start].pt3D.y; + a_gap.gap_width = vldPts[end].pt3D.y - vldPts[start].pt3D.y; + a_gap.gapPt_0 = vldPts[start]; + a_gap.gapPt_1 = vldPts[end]; + double x1, x2; + if (vldPts[end].pt3D.x > vldPts[start].pt3D.x) + { + x1 = vldPts[start].pt3D.x; + x2 = vldPts[end].pt3D.x; + } + else + { + x1 = vldPts[end].pt3D.x; + x2 = vldPts[start].pt3D.x; + } + a_gap.roi = { x1, x2, vldPts[start].pt3D.y, vldPts[end].pt3D.y }; + a_gap.gap_depth = -1; //穿透型撕裂 + vLineFeatures.push_back(a_gap); + gapFeatureNum++; + } + } + //排序 + if (gapFeatureNum > 0) + { + std::sort(vLineFeatures.begin(), vLineFeatures.end(), compareByStartY); + } +#endif + return; +} + +//水平扫描线处理，进行垂直方向的特征提取和生长 +//水平扫描线：若点间距小于1mm，使用差分公式；反之直接使用前后点差值计算。 +void sg_bearTearing_hLineProc( + SSG_hLineProInfo* hLineProc, + int workerIdx, + int hLineIdx, //水平扫描点序号，对应垂直扫描线的线序号 + SVzNL3DLaserLine** buffLines, + const SSG_beltTearingParam measureParam, + std::vector& hLineFeatures ) +{ + //注意：进行处理时，buffer已经满，不需要考虑buffer不满的情况 + //处理中间扫描线数据，这样能够检测后面的跳变。整体处理延后（SCAN_BUFF_SIZE / 2）条扫描线 + int midPos = SCAN_BUFF_SIZE / 2; + SVzNL3DPosition* procData = &(buffLines[midPos]->p3DPosition[workerIdx]); + + if (procData->pt3D.z < 1e-4) + return; + + if (hLineProc->preVldData.pt3D.z < 1e-4) + { + hLineProc->preVldData = *procData; + return; + } + + //计算差值 + double x_diff = procData->pt3D.x - hLineProc->preVldData.pt3D.x; + double a_difference; + int a_binValue; + if (x_diff > measureParam.tearingMinGap) + { + hLineProc->gapPos.nPointIdx = -1; + hLineProc->hLineSM = keSG_HLineProc_Init; + memset(&hLineProc->firstHalf, 0, sizeof(SSG_RUN)); + memset(&hLineProc->firstHalf_startPt, 0, sizeof(SVzNL3DPosition)); + memset(&hLineProc->firstHalf_endPt, 0, sizeof(SVzNL3DPosition)); + a_difference = 0.0; + a_binValue = 0; + } + else + { + //检查Gap + if (x_diff > measureParam.scanXScale * 2) //找到Gap + { + hLineProc->gapPos = hLineProc->preVldData; + } + a_difference = procData->pt3D.z - hLineProc->preVldData.pt3D.z; + if (measureParam.scanXScale < 1.0) + a_difference = a_difference / x_diff; + a_binValue = a_difference > measureParam.differnceBinTh ? 1 : (a_difference < -measureParam.differnceBinTh ? -1 : 0); + } + switch (hLineProc->hLineSM) + { + case keSG_HLineProc_Init: + if (a_binValue != 0) + { + hLineProc->gapMaxZ = procData->pt3D.z; + hLineProc->firstHalf.start = procData->nPointIdx; + hLineProc->firstHalf.len = 1; + hLineProc->firstHalf.value = a_binValue; + hLineProc->firstHalf_startPt = *procData; + hLineProc->firstHalf_endPt = *procData; + hLineProc->hLineSM = keSG_HLineProc_First_Half; + } + break; + case keSG_HLineProc_First_Half: + if(hLineProc->gapMaxZ < procData->pt3D.z) + hLineProc->gapMaxZ = procData->pt3D.z; + + if (0 == a_binValue) + { + //进行wait状态 + hLineProc->hLineSM = keSG_HLineProc_Wait_Second; + } + else if (hLineProc->firstHalf.value != a_binValue) + { + hLineProc->secondHalf.start = procData->nPointIdx; + hLineProc->secondHalf.len = 1; + hLineProc->secondHalf.value = a_binValue; + hLineProc->secondHalf_startPt = *procData; + hLineProc->secondHalf_endPt = *procData; + hLineProc->hLineSM = keSG_HLineProc_Second_Half; + } + else + { + hLineProc->firstHalf.len++; + hLineProc->firstHalf_endPt = *procData; + } + break; + case keSG_HLineProc_Wait_Second: + if (hLineProc->gapMaxZ < procData->pt3D.z) + hLineProc->gapMaxZ = procData->pt3D.z; + + if (0 == a_binValue) + { + double x_diff = abs(procData->pt3D.x - hLineProc->firstHalf_endPt.pt3D.x); + if (x_diff > measureParam.extractPara.sameGapTh) + { + memset(&hLineProc->firstHalf, 0, sizeof(SSG_RUN)); + memset(&hLineProc->firstHalf_startPt, 0, sizeof(SVzNL3DPosition)); + memset(&hLineProc->firstHalf_endPt, 0, sizeof(SVzNL3DPosition)); + hLineProc->hLineSM = keSG_HLineProc_Init; + } + } + else if (hLineProc->firstHalf.value != a_binValue) + { + hLineProc->secondHalf.start = procData->nPointIdx; + hLineProc->secondHalf.len = 1; + hLineProc->secondHalf.value = a_binValue; + hLineProc->secondHalf_startPt = *procData; + hLineProc->secondHalf_endPt = *procData; + hLineProc->hLineSM = keSG_HLineProc_Second_Half; + } + else + { + if (x_diff > measureParam.extractPara.sameGapTh) + { + //firstHalf重新开始 + hLineProc->firstHalf.start = procData->nPointIdx; + hLineProc->firstHalf.len = 1; + hLineProc->firstHalf.value = a_binValue; + hLineProc->firstHalf_startPt = *procData; + hLineProc->firstHalf_endPt = *procData; + } + else + { + //firstHalf的延续 + hLineProc->firstHalf.len = procData->nPointIdx - hLineProc->firstHalf_startPt.nPointIdx + 1; + hLineProc->firstHalf_endPt = *procData; + } + hLineProc->hLineSM = keSG_HLineProc_First_Half; + } + break; + case keSG_HLineProc_Second_Half: + if (0 == a_binValue) + { + //匹配 + //生成特征 + SSG_basicFeatureGap a_gap; + memset(&a_gap, 0, sizeof(SSG_basicFeatureGap)); + a_gap.lineIdx = hLineIdx; + //向前检查Gap + SVzNL3DPosition start; + int x_lineDiff = hLineProc->firstHalf_startPt.nPointIdx - hLineProc->gapPos.nPointIdx; + if ((x_lineDiff > 0) && (x_lineDiff < measureParam.extractPara.gapChkWin)) + { + a_gap.gap_start = hLineProc->gapPos.pt3D.x; + start = hLineProc->gapPos; + } + else + { + a_gap.gap_start = hLineProc->firstHalf_startPt.pt3D.x; + start = hLineProc->firstHalf_startPt; + } + //向后检查Gap + bool hasPostGap = false; + SVzNL3DPosition postGap; + memset(&postGap, 0, sizeof(SVzNL3DPosition)); + int chkLen = measureParam.extractPara.gapChkWin; + if (chkLen > SCAN_BUFF_SIZE / 2) + chkLen = SCAN_BUFF_SIZE / 2; + + SVzNL3DPosition preData = *procData; + for (int m = 1; m <= chkLen; m++) + { + int lineIdx = midPos + m; + if (buffLines[lineIdx]->p3DPosition[workerIdx].pt3D.z < 1e-4) + continue; + + double x_dff_2 = buffLines[lineIdx]->p3DPosition[workerIdx].pt3D.x - preData.pt3D.x; + if (x_dff_2 > measureParam.scanXScale * 2) //找到Gap + { + hasPostGap = true; + postGap = buffLines[lineIdx]->p3DPosition[workerIdx]; + break; + } + preData = buffLines[lineIdx]->p3DPosition[workerIdx]; + } + + SVzNL3DPosition end; + if (true == hasPostGap) + { + end = postGap; + a_gap.gap_width = postGap.pt3D.x - a_gap.gap_start; + a_gap.gap_depth = -1;//穿透型撕裂 + double y1, y2; + if (start.pt3D.y < end.pt3D.y) + { + y1 = start.pt3D.y; + y2 = end.pt3D.y; + } + else + { + y1 = end.pt3D.y; + y2 = start.pt3D.y; + } + a_gap.roi = { a_gap.gap_start, postGap.pt3D.x, y1, y2}; + a_gap.gapPt_0 = start; + a_gap.gapPt_1 = end; + } + else + { + end = hLineProc->secondHalf_endPt; + a_gap.gap_width = hLineProc->secondHalf_endPt.pt3D.x - a_gap.gap_start; + a_gap.gap_depth = hLineProc->gapMaxZ - (start.pt3D.z + end.pt3D.z) / 2; + double y1, y2; + if (start.pt3D.y < end.pt3D.y) + { + y1 = start.pt3D.y; + y2 = end.pt3D.y; + } + else + { + y1 = end.pt3D.y; + y2 = start.pt3D.y; + } + a_gap.roi = { a_gap.gap_start, hLineProc->secondHalf_endPt.pt3D.x, y1, y2 }; + a_gap.gapPt_0 = start; + a_gap.gapPt_1 = end; + } + hLineFeatures.push_back(a_gap); + + memset(&hLineProc->firstHalf, 0, sizeof(SSG_RUN)); + memset(&hLineProc->firstHalf_startPt, 0, sizeof(SVzNL3DPosition)); + memset(&hLineProc->firstHalf_endPt, 0, sizeof(SVzNL3DPosition)); + memset(&hLineProc->secondHalf, 0, sizeof(SSG_RUN)); + memset(&hLineProc->secondHalf_startPt, 0, sizeof(SVzNL3DPosition)); + memset(&hLineProc->secondHalf_endPt, 0, sizeof(SVzNL3DPosition)); + hLineProc->hLineSM = keSG_HLineProc_Init; + } + else if (hLineProc->firstHalf.value != a_binValue) + { + //firstHalf延续 + hLineProc->firstHalf.len = procData->nPointIdx - hLineProc->firstHalf_startPt.nPointIdx + 1; + hLineProc->firstHalf_endPt = *procData; + memset(&hLineProc->secondHalf, 0, sizeof(SSG_RUN)); + memset(&hLineProc->secondHalf_startPt, 0, sizeof(SVzNL3DPosition)); + memset(&hLineProc->secondHalf_endPt, 0, sizeof(SVzNL3DPosition)); + hLineProc->hLineSM = keSG_HLineProc_First_Half; + } + else + { + //secondHalf继续 + hLineProc->secondHalf.len++; + hLineProc->secondHalf_endPt = *procData; + } + break; + default: + break; + } + hLineProc->preVldData = *procData; + return; +} + +void sg_beltTearingInit( + int linePtNum, + std::vector& hLineWorkers ) +{ + hLineWorkers.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + memset(&hLineWorkers[i], 0, sizeof(SSG_hLineProInfo)); +} + +bool _chkTreeMatching( + SSG_basicFeatureGap* a_feature, + SSG_beltTearingInfo* a_tree, + const SSG_beltTearingParam measureParam, + double* dist_x, + double* dist_y +) +{ + //此处只使用ROI来进行匹配 + //计算水平方向距离 + double x_dist; + if ((a_feature->roi.right >= a_tree->roi.left) && (a_tree->roi.right >= a_feature->roi.left)) + x_dist = 0; //有重合区 + else if (a_tree->roi.left > a_feature->roi.right) + x_dist = a_tree->roi.left - a_feature->roi.right; + else + x_dist = a_feature->roi.left - a_tree->roi.right; + + //计算垂直方向距离 + double y_dist; + if ((a_feature->roi.bottom >= a_tree->roi.top) && (a_tree->roi.bottom >= a_feature->roi.top)) + y_dist = 0; //有重合区 + else if (a_tree->roi.top > a_feature->roi.bottom) + y_dist = a_tree->roi.top - a_feature->roi.bottom; + else + y_dist = a_feature->roi.top - a_tree->roi.bottom; + + *dist_x = x_dist; + *dist_y = y_dist; + if (((y_dist < measureParam.scanYScale * 3) && (x_dist < measureParam.tearingMinGap))) + //||((x_dist < measureParam.scanXScale * 3) && (y_dist < measureParam.tearingMinGap))) + return true; + + return false; +} + + +SSG_beltTearingInfo* _findMatchTre( + SSG_basicFeatureGap* a_feature, + std::vector& existTrees, + const SSG_beltTearingParam measureParam) +{ + for (int i = 0, i_max = existTrees.size(); i < i_max; i++) + { + SSG_beltTearingInfo* a_tree = &existTrees[i]; + double x_dist, y_dist; + bool isMatching = _chkTreeMatching( + a_feature, + a_tree, + measureParam, + &x_dist, + &y_dist); + if(true == isMatching) + return a_tree; + } + return NULL; +} + +void _mergeTwoTree( + SSG_beltTearingInfo* mergeTree, + SSG_beltTearingInfo* subTree +) +{ + mergeTree->endLineIdx = mergeTree->endLineIdx < subTree->endLineIdx ? subTree->endLineIdx : mergeTree->endLineIdx; +#if OUTPUT_TEARING_POINTS + mergeTree->pts.insert(mergeTree->pts.end(), subTree->pts.begin(), subTree->pts.end()); +#endif + _combineROI(&mergeTree->roi, &subTree->roi); + mergeTree->statLineIdx = mergeTree->statLineIdx > subTree->statLineIdx ? subTree->statLineIdx : mergeTree->statLineIdx; + mergeTree->tearDepth = mergeTree->tearDepth < subTree->tearDepth ? subTree->tearDepth : mergeTree->tearDepth; + subTree->tearStatus = keSG_tearStatus_Invalid; + return; +} + +SSG_tearProcessInfo* _getProcInfo( + int tearProcInfo_IdOffset, + int tearID, + std::vector& tearProcInfo) +{ + int index = tearID; + if (tearProcInfo_IdOffset > index) + index = INT_MAX - tearProcInfo_IdOffset + index; + else + index = index - tearProcInfo_IdOffset; + SSG_tearProcessInfo* a_procInfo = &tearProcInfo[index]; + if (a_procInfo->tearID >= 0) + { + try { + if (a_procInfo->tearID != tearID) + throw std::runtime_error("撕裂的ID与撕裂处理数据的ID不匹配！"); + } + catch (const std::runtime_error& e) { + std::cerr << "Caught an exception: " << e.what() << std::endl; + } + } + return a_procInfo; +} + +///数据输入必须是grid格式，以进行水平方向和垂直方向的处理 +//每条扫描线调用一次处理程序 +//每条扫描线完成一次垂直扫描线处理，同时N个水平状态机同时处理 +//生长时，通过ROI判断是否有匹配的生长树。水平Feature的生长不记录开始和结束扫描线号 +void sg_detectBeltTearing( + SVzNL3DLaserLine* laser3DPoints, //一条扫描线 + int inputLineIdx, + int nPointCount, //每条扫描线的点数量。当采用Grid数据格式时，每条扫描线的点的数量是相同的。必须使用Grid格式 + int* errCode, + std::vector& hLineWorkers, + std::vector& beltTearings_new, + std::vector& beltTearings_growing, + std::vector& beltTearings_ended, + std::vector& beltTearings_unknown, //未判明，应用无需处理。 + const SSG_beltTearingParam measureParam) +{ + //算法需要建立Buff，同时延时处理。 + static SVzNL3DLaserLine** buffLines = (SVzNL3DLaserLine**)malloc(sizeof(SVzNL3DLaserLine*) * SCAN_BUFF_SIZE); + static int buffSize = 0; + static int tearID = 1; //因为皮带是循环运行,需要考虑无限增长 + //撕裂处理过程数据。因为皮带是循环运行，因此容器不能无限增长，需要将结束的撕裂过程数据清除 + static int tearProcInfo_IdOffset = 1; //id从1开始 + static std::vector tearProcInfo; + + if (NULL == laser3DPoints) //复位内部静态变量 + { + buffSize = 0; + tearID = 1; + tearProcInfo_IdOffset = 1; + tearProcInfo.resize(0); + return; + } + + *errCode = 0; + if (0 == laser3DPoints->nPositionCnt) + return; + + if (hLineWorkers.size() != laser3DPoints->nPositionCnt) //空行 + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return; + } + + /* 数据缓冲FIFO。只有缓冲区满后才进行处理。 */ + if (buffSize < SCAN_BUFF_SIZE) //需要申请buff空间 + { + SVzNL3DLaserLine* a_buffLine = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine)); + a_buffLine->nPositionCnt = laser3DPoints->nPositionCnt; + a_buffLine->nTimeStamp = inputLineIdx; + a_buffLine->p3DPosition = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * laser3DPoints->nPositionCnt); + memcpy(a_buffLine->p3DPosition, laser3DPoints->p3DPosition, sizeof(SVzNL3DPosition) * laser3DPoints->nPositionCnt); + for (int i = 0; i < laser3DPoints->nPositionCnt; i++) + a_buffLine->p3DPosition[i].nPointIdx = inputLineIdx; //用于水平处理时得到点对应的LineIdx + buffLines[buffSize] = a_buffLine; + buffSize++; + return; //buffer未满，不处理 + } + else //当Buffer已满，申请的空间复用，不需要再申请空间 + { + SVzNL3DLaserLine* tmpLine = buffLines[0]; + for (int i = 0; i < SCAN_BUFF_SIZE - 1; i++) + buffLines[i] = buffLines[i + 1]; + tmpLine->nPositionCnt = laser3DPoints->nPositionCnt; + tmpLine->nTimeStamp = inputLineIdx; + memcpy(tmpLine->p3DPosition, laser3DPoints->p3DPosition, sizeof(SVzNL3DPosition) * laser3DPoints->nPositionCnt); + for (int i = 0; i < laser3DPoints->nPositionCnt; i++) + tmpLine->p3DPosition[i].nPointIdx = inputLineIdx; //用于水平处理时得到点对应的LineIdx + buffLines[SCAN_BUFF_SIZE - 1] = tmpLine; + } + + /* 将已经生成的beltTearings_new合并到beltTearings_growing中，并清空beltTearings_new */ + if (beltTearings_new.size() > 0) + { + for (int i = 0; i < beltTearings_new.size(); i++) + beltTearings_new[i].tearStatus = keSG_tearStatus_Growing; + beltTearings_growing.insert(beltTearings_growing.end(), beltTearings_new.begin(), beltTearings_new.end()); + beltTearings_new.clear(); + } + + /* 将beltTearings_ended清空 */ + if (beltTearings_ended.size() > 0) + { + for (int i = 0, i_max = beltTearings_ended.size(); i < i_max; i++) + { + SSG_tearProcessInfo* match_procInfo = _getProcInfo( + tearProcInfo_IdOffset, + beltTearings_ended[i].tearID, + tearProcInfo); + match_procInfo->tearID = -1; + } + beltTearings_ended.clear(); + //调整tearProcInfo + int vldStart = -1; + for (int i = 0, i_max = tearProcInfo.size(); i < i_max; i++) + { + if (tearProcInfo[i].tearID > 0) + { + vldStart = i; + break; + } + } + if (vldStart > 0) + { + tearProcInfo.erase(tearProcInfo.begin(), tearProcInfo.begin() + vldStart); + tearProcInfo_IdOffset = tearProcInfo[0].tearID; + } + } + + /****************************************************************/ + /**** 对BuffLines的中间进行处理（延时了BuffLines的一半行数） ****/ + /****************************************************************/ + int buffMidPos = SCAN_BUFF_SIZE / 2; + if ( (buffLines[buffMidPos]->nTimeStamp == 422) ||(buffLines[buffMidPos]->nTimeStamp == 182)) + int kkk = 1; + + int procLineIdx = buffLines[buffMidPos]->nTimeStamp; + //对当前扫描线数据进行处理 + std::vector vLineFeatures; + sg_bearTearing_vLineProc( + buffLines[buffMidPos], //当前扫描线 + buffLines[buffMidPos]->nTimeStamp, //当前扫描线序号 + measureParam, + vLineFeatures); + + //缓冲,水平处理 + std::vector hLineFeatures; + //hLineFeatures.resize(hLineWorkers.size()); + for (int i = 0; i < laser3DPoints->nPositionCnt; i++) + { + //if ((buffLines[buffMidPos]->nTimeStamp == 295) && (i == 725)) + // int kkk = 1; + + //水平扫描线处理，进行垂直方向的特征提取和生长 + std::vector a_hLineFeatures; + sg_bearTearing_hLineProc( + &hLineWorkers[i], + i, + buffLines[buffMidPos]->nTimeStamp, //水平扫描点序号，对应垂直扫描线的线序号 + buffLines, + measureParam, + a_hLineFeatures); + //if (0 == a_hLineFeatures.size()) + // hLineFeatures[i].lineIdx = -1; + //else + if(a_hLineFeatures.size() > 0) + { + hLineFeatures.push_back(a_hLineFeatures[0]); + //hLineFeatures[i] = a_hLineFeatures[0]; + //if ((a_hLineFeatures[0].gapPt_0.pt3D.x < 10871) &&(a_hLineFeatures[0].gapPt_0.pt3D.x > 10870)) + // int kkk = 1; + } + } + //垂直特征生长 + //对于垂直特征生长，不能同一棵生长树对应多个特征，除非是包含关系 + //当一个特征属于两个以上的生长树时，为了简化处理逻辑，不进行生长树的合并，选择最合适的生长树 + if (vLineFeatures.size() > 0) + { + std::vector feature_tree_id_pair; //注意：vector取地址时在push_back操作后单元地址可能会变化，使用序号 + feature_tree_id_pair.resize(vLineFeatures.size()); //每个featurec对应一个matchTree + std::vector feature_tree_pair_dist; + feature_tree_pair_dist.resize(vLineFeatures.size()); + for (int i = 0, i_max = vLineFeatures.size(); i < i_max; i++) + { + feature_tree_id_pair[i].data_0 = -1; //指示tree_id + feature_tree_id_pair[i].data_1 = -1; //指示group_id + for (int j = 0, j_max = beltTearings_growing.size(); j < j_max; j++) + { + double dist_x = -1, dist_y = -1; + bool isMatching = _chkTreeMatching( + &vLineFeatures[i], + &beltTearings_growing[j], + measureParam, + &dist_x, + &dist_y); + if (true == isMatching) + { + if (feature_tree_id_pair[i].data_0 < 0) + { + feature_tree_id_pair[i].data_0 = j; + feature_tree_id_pair[i].data_1 = 0; + feature_tree_pair_dist[i].x = dist_x; + feature_tree_pair_dist[i].y = dist_y; + } + else //取最近的tree。垂直特征生长，使用dist_y来计算 + { + if (dist_y < feature_tree_pair_dist[i].y) + { + feature_tree_id_pair[i].data_0 = j; + feature_tree_id_pair[i].data_1 = 0; + feature_tree_pair_dist[i].x = dist_x; + feature_tree_pair_dist[i].y = dist_y; + } + } + } + } + for (int j = 0, j_max = beltTearings_unknown.size(); j < j_max; j++) + { + if (keSG_tearStatus_Invalid == beltTearings_unknown[j].tearStatus) + continue; + + double dist_x = -1, dist_y = -1; + bool isMatching = _chkTreeMatching( + &vLineFeatures[i], + &beltTearings_unknown[j], + measureParam, + &dist_x, + &dist_y); + if (true == isMatching) + { + if (feature_tree_id_pair[i].data_0 < 0) + { + feature_tree_id_pair[i].data_0 = j; + feature_tree_id_pair[i].data_1 = 1; + feature_tree_pair_dist[i].x = dist_x; + feature_tree_pair_dist[i].y = dist_y; + } + else //取最近的tree。垂直特征生长，使用dist_y来计算 + { + if (dist_y < feature_tree_pair_dist[i].y) + { + feature_tree_id_pair[i].data_0 = j; + feature_tree_id_pair[i].data_1 = 1; + feature_tree_pair_dist[i].x = dist_x; + feature_tree_pair_dist[i].y = dist_y; + } + } + } + } + } + //检查是否有同一个feature属于两个生长树的情况 + for (int i = 0, i_max = vLineFeatures.size(); i < i_max; i++) + { + if (feature_tree_id_pair[i].data_0 < 0) + continue; + for (int j = i + 1; j < i_max; j++) + { + if ( (feature_tree_id_pair[i].data_0 == feature_tree_id_pair[j].data_0) && + (feature_tree_id_pair[i].data_1 == feature_tree_id_pair[j].data_1)) + { + //比较y距离 + if ( (feature_tree_pair_dist[i].y < feature_tree_pair_dist[j].y) && + (feature_tree_pair_dist[j].y > 1e-4)) //不为0 + feature_tree_id_pair[j].data_0 = -1; + else if ((feature_tree_pair_dist[j].y < feature_tree_pair_dist[i].y) && + (feature_tree_pair_dist[i].y > 1e-4)) //不为0 + feature_tree_id_pair[i].data_0 = -1; + break; + } + } + } + for (int i = 0, i_max = vLineFeatures.size(); i < i_max; i++) + { + //对于垂直特征生长，不能同一棵生长树对应多个特征，除非是包含关系 + SSG_basicFeatureGap* a_vFeature = &vLineFeatures[i]; + if (feature_tree_id_pair[i].data_0 < 0) + { + SSG_beltTearingInfo a_newTree; + memset(&a_newTree, 0, sizeof(SSG_beltTearingInfo)); + a_newTree.tearID = tearID; // + tearID++; + if (tearID == 0) //循环一圈 + tearID = 1; + a_newTree.statLineIdx = procLineIdx; + a_newTree.endLineIdx = procLineIdx; + a_newTree.roi = a_vFeature->roi; + a_newTree.tearDepth = a_vFeature->gap_depth; + a_newTree.tearType = keSG_tearType_MachineDir; //纵撕 + a_newTree.tearStatus = keSG_tearStatus_Uknown; //unknown +#if OUTPUT_TEARING_POINTS + a_vFeature->gapPt_0.nPointIdx = a_newTree.tearID; + a_vFeature->gapPt_1.nPointIdx = a_newTree.tearID; + a_newTree.pts.push_back(a_vFeature->gapPt_0); + a_newTree.pts.push_back(a_vFeature->gapPt_1); +#endif + beltTearings_unknown.push_back(a_newTree); + SSG_tearProcessInfo a_procInfo; + memset(&a_procInfo, 0, sizeof(SSG_tearProcessInfo)); + a_procInfo.tearID = a_newTree.tearID; + a_procInfo.vGap_depth_max = a_vFeature->gap_depth; + a_procInfo.vGap_width_max = a_vFeature->gap_width; + a_procInfo.vGap_width_sum = a_vFeature->gap_width; + a_procInfo.vGap_num = 1; + tearProcInfo.push_back(a_procInfo); + } + else + { + SSG_beltTearingInfo* a_matchTree; + int tree_id = feature_tree_id_pair[i].data_0; + if(0 == feature_tree_id_pair[i].data_1) + a_matchTree = &beltTearings_growing[tree_id]; + else + a_matchTree = &beltTearings_unknown[tree_id]; + //取相应的procInfo + SSG_tearProcessInfo* match_procInfo = _getProcInfo( + tearProcInfo_IdOffset, + a_matchTree->tearID, + tearProcInfo); + if ((a_vFeature->gap_depth < 0) || (match_procInfo->vGap_depth_max < 0)) + match_procInfo->vGap_depth_max = -1; //贯穿撕裂 + else + match_procInfo->vGap_depth_max = match_procInfo->vGap_depth_max < a_vFeature->gap_depth ? a_vFeature->gap_depth : match_procInfo->vGap_depth_max; + match_procInfo->vGap_width_max = match_procInfo->vGap_width_max < a_vFeature->gap_width ? a_vFeature->gap_width : match_procInfo->vGap_width_max; + match_procInfo->vGap_width_sum += a_vFeature->gap_width; + match_procInfo->vGap_num++; +#if OUTPUT_TEARING_POINTS + a_vFeature->gapPt_0.nPointIdx = a_matchTree->tearID; + a_vFeature->gapPt_1.nPointIdx = a_matchTree->tearID; + a_matchTree->pts.push_back(a_vFeature->gapPt_0); + a_matchTree->pts.push_back(a_vFeature->gapPt_1); +#endif + // + a_matchTree->endLineIdx = procLineIdx; + _combineROI(&a_matchTree->roi, &a_vFeature->roi); + if (keSG_tearType_MachineDir == a_matchTree->tearType) + { + a_matchTree->tearWidth = match_procInfo->vGap_width_max; + a_matchTree->tearDepth = match_procInfo->vGap_depth_max; + } + } + } + } +#if ENABLE_CROSS_WISE_TEAR + //水平特征生长 + if (hLineFeatures.size() > 0) + { + std::vector hFeature_tree_id_pair; //注意：vector取地址时在push_back操作后单元地址可能会变化，使用序号 + hFeature_tree_id_pair.resize(hLineFeatures.size()); //每个featurec对应一个matchTree + std::vector hFeature_tree_pair_dist; + hFeature_tree_pair_dist.resize(hLineFeatures.size()); + for(int i = 0, i_max = hLineFeatures.size(); i < i_max; i++) + { + hFeature_tree_id_pair[i].data_0 = -1; //指示tree_id + hFeature_tree_id_pair[i].data_1 = -1; //指示group_id + //对于beltTearings_unknown：当纵撕不允许， 横撕允许，且合并 + for (int j = 0, j_max = beltTearings_unknown.size(); j < j_max; j++) + { + if (keSG_tearStatus_Invalid == beltTearings_unknown[j].tearStatus) + continue; + + double dist_x = -1, dist_y = -1; + bool isMatching = _chkTreeMatching( + &hLineFeatures[i], + &beltTearings_unknown[j], + measureParam, + &dist_x, + &dist_y); + if (true == isMatching) + { + if (hFeature_tree_id_pair[i].data_0 < 0) + { + hFeature_tree_id_pair[i].data_0 = j; + hFeature_tree_id_pair[i].data_1 = 1; //1:beltTearings_unknown + hFeature_tree_pair_dist[i].x = dist_x; + hFeature_tree_pair_dist[i].y = dist_y; + } + else //检查合并。 + { + int mergeTreeIdx = hFeature_tree_id_pair[i].data_0; + _mergeTwoTree(&beltTearings_unknown[mergeTreeIdx],&beltTearings_unknown[j]); + SSG_tearProcessInfo* match_procInfo = _getProcInfo( + tearProcInfo_IdOffset, + beltTearings_unknown[j].tearID, + tearProcInfo); + match_procInfo->tearID = -1; //被合并的处理块置无效 + } + } + } + + for (int j = 0, j_max = beltTearings_growing.size(); j < j_max; j++) + { + double dist_x = -1, dist_y = -1; + bool isMatching = _chkTreeMatching( + &hLineFeatures[i], + &beltTearings_growing[j], + measureParam, + &dist_x, + &dist_y); + if (true == isMatching) + { + if (hFeature_tree_id_pair[i].data_0 < 0) + { + hFeature_tree_id_pair[i].data_0 = j; + hFeature_tree_id_pair[i].data_1 = 0; //0 : beltTearings_growing + hFeature_tree_pair_dist[i].x = dist_x; + hFeature_tree_pair_dist[i].y = dist_y; + } + else + { +#if 0 + //取最近的tree。水平特征生长，也使用dist_y来计算。 + if (dist_y < hFeature_tree_pair_dist[i].y) + { + hFeature_tree_id_pair[i].data_0 = j; + hFeature_tree_id_pair[i].data_1 = 0; + hFeature_tree_pair_dist[i].x = dist_x; + hFeature_tree_pair_dist[i].y = dist_y; + } +#else + //beltTearings_growing为正在生长的树，优先。 + hFeature_tree_id_pair[i].data_0 = j; + hFeature_tree_id_pair[i].data_1 = 0; + hFeature_tree_pair_dist[i].x = dist_x; + hFeature_tree_pair_dist[i].y = dist_y; +#endif + } + } + } + } + + + for (int i = 0, i_max = hLineFeatures.size(); i < i_max; i++) + { + SSG_basicFeatureGap* a_hFeature = &hLineFeatures[i]; + if (a_hFeature->lineIdx < 0) + continue; + if (hFeature_tree_id_pair[i].data_0 < 0) + { + SSG_beltTearingInfo a_newTree; + memset(&a_newTree, 0, sizeof(SSG_beltTearingInfo)); + a_newTree.tearID = tearID; // + tearID++; + if (tearID == 0) //循环一圈 + tearID = 1; + a_newTree.statLineIdx = procLineIdx; + a_newTree.endLineIdx = procLineIdx; + a_newTree.roi = a_hFeature->roi; + a_newTree.tearDepth = a_hFeature->gap_depth; + a_newTree.tearType = keSG_tearType_CrossWise; //横撕 + a_newTree.tearStatus = keSG_tearStatus_Uknown; //unknown +#if OUTPUT_TEARING_POINTS + a_hFeature->gapPt_0.nPointIdx = a_newTree.tearID; + a_hFeature->gapPt_1.nPointIdx = a_newTree.tearID; + a_newTree.pts.push_back(a_hFeature->gapPt_0); + a_newTree.pts.push_back(a_hFeature->gapPt_1); +#endif + beltTearings_unknown.push_back(a_newTree); + SSG_tearProcessInfo a_procInfo; + memset(&a_procInfo, 0, sizeof(SSG_tearProcessInfo)); + a_procInfo.tearID = a_newTree.tearID; + a_procInfo.hGap_depth_max = a_hFeature->gap_depth; + a_procInfo.hGap_width_max = a_hFeature->gap_width; + a_procInfo.hGap_width_sum = a_hFeature->gap_width; + a_procInfo.hGap_num = 1; + tearProcInfo.push_back(a_procInfo); + } + else + { + SSG_beltTearingInfo* a_matchTree; + int tree_id = hFeature_tree_id_pair[i].data_0; + if (0 == hFeature_tree_id_pair[i].data_1) + a_matchTree = &beltTearings_growing[tree_id]; + else + a_matchTree = &beltTearings_unknown[tree_id]; + //取相应的procInfo + SSG_tearProcessInfo* match_procInfo = _getProcInfo( + tearProcInfo_IdOffset, + a_matchTree->tearID, + tearProcInfo); + match_procInfo->hGap_depth_max = match_procInfo->hGap_depth_max < a_hFeature->gap_depth ? a_hFeature->gap_depth : match_procInfo->hGap_depth_max; + match_procInfo->hGap_width_max = match_procInfo->hGap_width_max < a_hFeature->gap_width ? a_hFeature->gap_width : match_procInfo->hGap_width_max; + match_procInfo->hGap_width_sum += a_hFeature->gap_width; + match_procInfo->hGap_num++; +#if OUTPUT_TEARING_POINTS + a_hFeature->gapPt_0.nPointIdx = a_matchTree->tearID; + a_hFeature->gapPt_1.nPointIdx = a_matchTree->tearID; + a_matchTree->pts.push_back(a_hFeature->gapPt_0); + a_matchTree->pts.push_back(a_hFeature->gapPt_1); +#endif + // + a_matchTree->endLineIdx = procLineIdx; + _combineROI(&a_matchTree->roi, &a_hFeature->roi); + if (keSG_tearType_CrossWise == a_matchTree->tearType) + { + a_matchTree->tearWidth = match_procInfo->hGap_width_max; + a_matchTree->tearDepth = match_procInfo->hGap_depth_max; + } + } + } + } +#endif + //判断生长树是否结果，生成beltTearings_ended目标 + if (beltTearings_growing.size() > 0) + { + int i_max = beltTearings_growing.size(); + for (int i = i_max-1; i >= 0; i--) //从后往前处理，这样在删除时不会影响后续检查 + { + if (beltTearings_growing[i].endLineIdx != procLineIdx) + { + int idx_diff; + if (procLineIdx > beltTearings_growing[i].endLineIdx) + idx_diff = procLineIdx - beltTearings_growing[i].endLineIdx; + else + idx_diff = INT_MAX - beltTearings_growing[i].endLineIdx + 1 + procLineIdx; + double dist = idx_diff * measureParam.scanXScale; + if (dist > measureParam.tearingMinGap) + { + beltTearings_growing[i].tearStatus = keSG_tearStatus_Ended; //结束 + beltTearings_ended.push_back(beltTearings_growing[i]); + beltTearings_growing.erase(beltTearings_growing.begin() + i); + } + } + } + } + + //处理unknown的撕裂，生成beltTearings_new + if (beltTearings_unknown.size() > 0) + { + int i_max = beltTearings_unknown.size(); + for (int i = i_max - 1; i >= 0; i--)//从后往前处理，这样在删除时不会影响后续检查 + { + if (keSG_tearStatus_Invalid == beltTearings_unknown[i].tearStatus) + { + SSG_tearProcessInfo* match_procInfo = _getProcInfo( + tearProcInfo_IdOffset, + beltTearings_unknown[i].tearID, + tearProcInfo); + match_procInfo->tearID = -1; + beltTearings_unknown.erase(beltTearings_unknown.begin() + i); + } + else + { + if (beltTearings_unknown[i].endLineIdx == procLineIdx) //检查是否需要报警 + { + //计算ROI的长宽 + double w = beltTearings_unknown[i].roi.right - beltTearings_unknown[i].roi.left; + double h = beltTearings_unknown[i].roi.bottom - beltTearings_unknown[i].roi.top; + if ((w >= measureParam.tearingMinLen) || (h >= measureParam.tearingMinLen)) + { + int lines = beltTearings_unknown[i].endLineIdx - beltTearings_unknown[i].statLineIdx + 1; + SSG_tearProcessInfo* match_procInfo = _getProcInfo( + tearProcInfo_IdOffset, + beltTearings_unknown[i].tearID, + tearProcInfo); + int vldLines = match_procInfo->hGap_num > match_procInfo->vGap_num ? match_procInfo->hGap_num : match_procInfo->vGap_num; + if (vldLines < lines / 2) + { + match_procInfo->tearID = -1; + beltTearings_unknown.erase(beltTearings_unknown.begin() + i); + } + else + { + bool isVTearing = true; + if ((match_procInfo->hGap_num > 0) && (match_procInfo->vGap_num == 0)) + isVTearing = false; + else if (((match_procInfo->hGap_num > 0) && (match_procInfo->vGap_num > 0)) && + (match_procInfo->hGap_width_max < match_procInfo->vGap_width_max)) + isVTearing = false; + + if (false == isVTearing) + { + beltTearings_unknown[i].tearType = keSG_tearType_CrossWise; + beltTearings_unknown[i].tearStatus = keSG_tearStatus_New; + beltTearings_unknown[i].tearDepth = match_procInfo->hGap_depth_max; + beltTearings_unknown[i].tearWidth = match_procInfo->hGap_width_max; + beltTearings_new.push_back(beltTearings_unknown[i]); + beltTearings_unknown.erase(beltTearings_unknown.begin() + i); + } + else + { + beltTearings_unknown[i].tearType = keSG_tearType_MachineDir; + beltTearings_unknown[i].tearStatus = keSG_tearStatus_New; + beltTearings_unknown[i].tearDepth = match_procInfo->vGap_depth_max; + beltTearings_unknown[i].tearWidth = match_procInfo->vGap_width_max; + beltTearings_new.push_back(beltTearings_unknown[i]); + beltTearings_unknown.erase(beltTearings_unknown.begin() + i); + } + } + } + + } + else//检查是否是噪声 + { + int idx_diff; + if (procLineIdx > beltTearings_unknown[i].endLineIdx) + idx_diff = procLineIdx - beltTearings_unknown[i].endLineIdx; + else + idx_diff = INT_MAX - beltTearings_unknown[i].endLineIdx + 1 + procLineIdx; + + double dist = idx_diff * measureParam.scanXScale; + if (dist > measureParam.tearingMinGap) + { + SSG_tearProcessInfo* match_procInfo = _getProcInfo( + tearProcInfo_IdOffset, + beltTearings_unknown[i].tearID, + tearProcInfo); + match_procInfo->tearID = -1; + beltTearings_unknown.erase(beltTearings_unknown.begin() + i); + } + } + } + } + } + return; +} + diff --git a/BeltTearingServer/beltTearingDetection_Export.h b/BeltTearingServer/beltTearingDetection_Export.h new file mode 100644 index 00000000..52a84db1 --- /dev/null +++ b/BeltTearingServer/beltTearingDetection_Export.h @@ -0,0 +1,94 @@ +#pragma once + +#if defined(SG_API_LIBRARY) +# define SG_APISHARED_EXPORT __declspec(dllexport) +#else +# define SG_APISHARED_EXPORT __declspec(dllimport) +#endif + +#include "SG_baseDataType.h" +#include + +#define ENABLE_CROSS_WISE_TEAR 0 +#define OUTPUT_TEARING_POINTS 1 +#define KEEP_GAP_AS_FEATURE 1 +#define SCAN_BUFF_SIZE 21 //缓存区大小，缓存21条扫描线，处理中间扫描线 + +typedef struct +{ + double scanXScale; //3D扫描的X精度，取决于两条扫描线的间距。 + double scanYScale; //3D扫描的Y精度，取决于点间距。scanXScale和scanYScale可以在现场安装后扫描数据得到 + double differnceBinTh; //一阶差分二值化门限。其物理意义为撕裂斜面的斜率值。默认为1.0 + SSG_tearFeatureExtactPara extractPara; + double tearingMinLen; //最小撕裂长度，达到此长度时输出撕裂 + double tearingMinGap; //两个同位置的撕裂的间隔。小于此间隔时，视为同一个撕裂。 +}SSG_beltTearingParam; + +typedef enum +{ + keSG_tearType_Uknown = 0, + keSG_tearType_MachineDir, //纵撕 + keSG_tearType_CrossWise,//横撕 +}ESG_tearType; +typedef enum +{ + keSG_tearStatus_Uknown = 0, + keSG_tearStatus_New, //新发现的撕裂 + keSG_tearStatus_Growing, //持续的撕裂 + keSG_tearStatus_Ended, //撕裂结束 + keSG_tearStatus_Invalid, //无效撕裂（可能被合并） +}ESG_tearStatus; +typedef struct +{ + int tearID; //每个撕裂有个唯一的序号 + ESG_tearStatus tearStatus; + ESG_tearType tearType; + int statLineIdx; //撕裂开始时的扫描线序号 + int endLineIdx; //撕裂结束时的扫描线序号，当撕裂持续时，为最新的扫描线序号 + double tearDepth;//撕裂深度。-1表示是贯穿型撕裂 + double tearWidth;//撕裂宽度 + SSG_ROIRectD roi; //撕裂的ROI范围，为{左，右，上，下} +#if OUTPUT_TEARING_POINTS + std::vector pts; //撕裂的边界点，debug或显示目的 +#endif +}SSG_beltTearingInfo; + +//处理状态机 +typedef enum +{ + keSG_HLineProc_Init = 0, //初态 + keSG_HLineProc_First_Half, + keSG_HLineProc_Wait_Second, + keSG_HLineProc_Second_Half, +}ESG_HLineProcSM; + +typedef struct +{ + int hLineIdx; + //SVzNL3DPosition scanBuff[SCAN_BUFF_SIZE]; //循环buff + //int buffHead;//循环buff的头位置 + //int buffSize;//循环buff + SVzNL3DPosition preVldData; + SVzNL3DPosition gapPos; + ESG_HLineProcSM hLineSM; //水平扫描线处理状态机 + SSG_RUN firstHalf; + SVzNL3DPosition firstHalf_startPt; + SVzNL3DPosition firstHalf_endPt; + SSG_RUN secondHalf; + SVzNL3DPosition secondHalf_startPt; + SVzNL3DPosition secondHalf_endPt; + double gapMaxZ; +}SSG_hLineProInfo; + + +void sg_detectBeltTearing( + SVzNL3DLaserLine* laser3DPoints, //一条扫描线 + int lineIdx, + int nPointCount, //每条扫描线的点数量。当采用Grid数据格式时，每条扫描线的点的数量是相同的。必须使用Grid格式 + int* errCode, + std::vector& hLineWorkers, + std::vector& beltTearings_new, + std::vector& beltTearings_growing, + std::vector& beltTearings_ended, + std::vector& beltTearings_unknown, //未判明，应用无需处理。 + const SSG_beltTearingParam measureParam); \ No newline at end of file diff --git a/BeltTearingServer/main.cpp b/BeltTearingServer/main.cpp new file mode 100644 index 00000000..bebdb6ec --- /dev/null +++ b/BeltTearingServer/main.cpp @@ -0,0 +1,25 @@ + +#include +#include +#include "BeltTearingPresenter.h" + +int main(int argc, char *argv[]) +{ + QCoreApplication app(argc, argv); + + // 创建并初始化Presenter + BeltTearingPresenter presenter; + + // 启动TCP服务器，监听端口12345 + if (!presenter.startServer(12345)) { + std::cout << "Failed to start server" << std::endl; + return -1; + } + + // 开始发送模拟数据 + presenter.startSendingSimulatedData(); + + std::cout << "BeltTearing Server started. Press Ctrl+C to exit." << std::endl; + + return app.exec(); +} \ No newline at end of file diff --git a/GrabBagPrj/GrabBagPrj.pro b/GrabBagPrj/GrabBagPrj.pro index e1345b3c..25dccfd7 100644 --- a/GrabBagPrj/GrabBagPrj.pro +++ b/GrabBagPrj/GrabBagPrj.pro @@ -10,13 +10,15 @@ SUBDIRS += ../Module/ModbusTCPServer/ModbusTCPServer.pro SUBDIRS += ../VrEyeDevice/VrEyeDevice.pro -# 撕裂项目 -SUBDIRS += ../GrabBagConfig/GrabBagConfig.pro -SUBDIRS += ../GrabBagApp/GrabBagApp.pro +# 拆包项目 +# SUBDIRS += ../GrabBagConfig/GrabBagConfig.pro +# SUBDIRS += ../GrabBagApp/GrabBagApp.pro # SUBDIRS += ../GrabBagConfigCmd - +# 撕裂项目 SUBDIRS += ../VrNets/VrTcpClient.pro SUBDIRS += ../BeltTearingConfig/BeltTearingConfig.pro SUBDIRS += ../BeltTearingApp/BeltTearingApp.pro -# SUBDIRS += ../BeltTearingServer/BeltTearingServer.pro + +SUBDIRS += ../VrNets/VrTcpServer.pro +SUBDIRS += ../BeltTearingServer/BeltTearingServer.pro diff --git a/Module/ModbusTCPServer/Inc/IYModbusTCPServer.h b/Module/ModbusTCPServer/Inc/IYModbusTCPServer.h index 4969ef4d..609aaf85 100644 --- a/Module/ModbusTCPServer/Inc/IYModbusTCPServer.h +++ b/Module/ModbusTCPServer/Inc/IYModbusTCPServer.h @@ -1,65 +1,65 @@ -#ifndef IY_MODBUS_TCP_SERVER_H -#define IY_MODBUS_TCP_SERVER_H - -#include -#include -#include -#include -#include -#include -#include - -class IYModbusTCPServer { -public: - // 错误代码枚举 - enum class ErrorCode { - SUCCESS = 0, - ILLEGAL_FUNCTION = 1, - ILLEGAL_DATA_ADDRESS = 2, - ILLEGAL_DATA_VALUE = 3, - SERVER_FAILURE = 4, - ACKNOWLEDGE = 5, - SERVER_BUSY = 6, - NEGATIVE_ACKNOWLEDGE = 7, - MEMORY_PARITY = 8, - GATEWAY_PATH_UNAVAILABLE = 10, - GATEWAY_TARGET_FAILED = 11 - }; - - // 回调函数类型定义 - // 写线圈回调函数：unitId, startAddress, quantity, values -> ErrorCode - using WriteCoilsCallback = std::function; - - // 写保持寄存器回调函数：unitId, startAddress, quantity, values -> ErrorCode - using WriteRegistersCallback = std::function; - - // 连接状态回调函数：isConnected -> void - using ConnectionStatusCallback = std::function; - -public: - virtual ~IYModbusTCPServer() = default; - - static bool CreateInstance(IYModbusTCPServer** ppModbusTCPServer); - - // 启动/停止服务器 - virtual int start(int port = 502, int maxConnections = 10) = 0; - virtual void stop() = 0; - - // 设置回调函数 - virtual void setWriteCoilsCallback(WriteCoilsCallback callback) = 0; - virtual void setWriteRegistersCallback(WriteRegistersCallback callback) = 0; - virtual void setConnectionStatusCallback(ConnectionStatusCallback callback) = 0; - - // 更新数据接口 - 线程安全 - virtual void updateCoil(uint16_t address, bool value) = 0; - virtual void updateCoils(uint16_t startAddress, const std::vector& values) = 0; - virtual void updateDiscreteInput(uint16_t address, bool value) = 0; - virtual void updateDiscreteInputs(uint16_t startAddress, const std::vector& values) = 0; - virtual void updateHoldingRegister(uint16_t address, uint16_t value) = 0; - virtual void updateHoldingRegisters(uint16_t startAddress, const std::vector& values) = 0; - virtual void updateInputRegister(uint16_t address, uint16_t value) = 0; - virtual void updateInputRegisters(uint16_t startAddress, const std::vector& values) = 0; - -}; - -#endif // MODBUS_TCP_SERVER_H \ No newline at end of file +#ifndef IY_MODBUS_TCP_SERVER_H +#define IY_MODBUS_TCP_SERVER_H + +#include +#include +#include +#include +#include +#include +#include + +class IYModbusTCPServer { +public: + // 错误代码枚举 + enum class ErrorCode { + SUCCESS = 0, + ILLEGAL_FUNCTION = 1, + ILLEGAL_DATA_ADDRESS = 2, + ILLEGAL_DATA_VALUE = 3, + SERVER_FAILURE = 4, + ACKNOWLEDGE = 5, + SERVER_BUSY = 6, + NEGATIVE_ACKNOWLEDGE = 7, + MEMORY_PARITY = 8, + GATEWAY_PATH_UNAVAILABLE = 10, + GATEWAY_TARGET_FAILED = 11 + }; + + // 回调函数类型定义 + // 写线圈回调函数：unitId, startAddress, quantity, values -> ErrorCode + using WriteCoilsCallback = std::function; + + // 写保持寄存器回调函数：unitId, startAddress, quantity, values -> ErrorCode + using WriteRegistersCallback = std::function; + + // 连接状态回调函数：isConnected -> void + using ConnectionStatusCallback = std::function; + +public: + virtual ~IYModbusTCPServer() = default; + + static bool CreateInstance(IYModbusTCPServer** ppModbusTCPServer); + + // 启动/停止服务器 + virtual int start(int port = 502, int maxConnections = 10) = 0; + virtual void stop() = 0; + + // 设置回调函数 + virtual void setWriteCoilsCallback(WriteCoilsCallback callback) = 0; + virtual void setWriteRegistersCallback(WriteRegistersCallback callback) = 0; + virtual void setConnectionStatusCallback(ConnectionStatusCallback callback) = 0; + + // 更新数据接口 - 线程安全 + virtual void updateCoil(uint16_t address, bool value) = 0; + virtual void updateCoils(uint16_t startAddress, const std::vector& values) = 0; + virtual void updateDiscreteInput(uint16_t address, bool value) = 0; + virtual void updateDiscreteInputs(uint16_t startAddress, const std::vector& values) = 0; + virtual void updateHoldingRegister(uint16_t address, uint16_t value) = 0; + virtual void updateHoldingRegisters(uint16_t startAddress, const std::vector& values) = 0; + virtual void updateInputRegister(uint16_t address, uint16_t value) = 0; + virtual void updateInputRegisters(uint16_t startAddress, const std::vector& values) = 0; + +}; + +#endif // MODBUS_TCP_SERVER_H diff --git a/Module/ModbusTCPServer/ModbusTCPServer.pro b/Module/ModbusTCPServer/ModbusTCPServer.pro index 1424876d..d867966b 100644 --- a/Module/ModbusTCPServer/ModbusTCPServer.pro +++ b/Module/ModbusTCPServer/ModbusTCPServer.pro @@ -1,6 +1,7 @@ QT -= gui CONFIG += c++17 console +QMAKE_CXXFLAGS += /utf-8 # The following define makes your compiler emit warnings if you use # any Qt feature that has been marked deprecated (the exact warnings diff --git a/Module/ModbusTCPServer/Src/ModbusTCPServer.cpp b/Module/ModbusTCPServer/Src/ModbusTCPServer.cpp index d7a79d39..c3a829f4 100644 --- a/Module/ModbusTCPServer/Src/ModbusTCPServer.cpp +++ b/Module/ModbusTCPServer/Src/ModbusTCPServer.cpp @@ -1,536 +1,536 @@ -#include "ModbusTCPServer.h" -#include -#include -#include - -#include "VrError.h" -#include "VrLog.h" - -#ifdef _WIN32 -#include -#include -#pragma comment(lib, "ws2_32.lib") -#else -#include -#include -#include -#include -#include -#include -#endif - -ModbusTCPServer::ModbusTCPServer() - : m_modbusCtx(nullptr) - , m_mapping(nullptr) - , m_isRunning(false) - , m_shouldStop(false) - , m_serverSocket(-1) - , m_port(502) - , m_maxConnections(10) -{ -} - -ModbusTCPServer::~ModbusTCPServer() -{ - stop(); -} - -int ModbusTCPServer::start(int port, int maxConnections) -{ - if (m_isRunning.load()) { - setLastError("服务器已在运行"); - return SUCCESS; - } - - m_port = port; - m_maxConnections = maxConnections; - - LOG_DEBUG("ModbusTCPServer::start: port=%d, maxConnections=%d\n", port, maxConnections); - - // 创建modbus TCP context - m_modbusCtx = modbus_new_tcp("0.0.0.0", port); - if (m_modbusCtx == nullptr) { - setLastError("创建Modbus TCP上下文失败"); - return ERR_CODE(NET_ERR_CREAT_INIT); - } - - // 创建数据映射 - 分配足够的空间 - // 线圈: 0-9999, 离散输入: 0-9999, 保持寄存器: 0-9999, 输入寄存器: 0-9999 - m_mapping = modbus_mapping_new(10000, 10000, 10000, 10000); - if (m_mapping == nullptr) { - setLastError("创建数据映射失败"); - modbus_free(m_modbusCtx); - m_modbusCtx = nullptr; - return ERR_CODE(NET_ERR_CONFIG); - } - - // 监听连接 - m_serverSocket = modbus_tcp_listen(m_modbusCtx, maxConnections); - if (m_serverSocket == -1) { - setLastError(std::string("监听失败: ") + modbus_strerror(errno)); - modbus_mapping_free(m_mapping); - m_mapping = nullptr; - modbus_free(m_modbusCtx); - m_modbusCtx = nullptr; - return ERR_CODE(NET_ERR_CREAT_LISTEN); - } - - m_isRunning.store(true); - m_shouldStop.store(false); - - // 启动服务器线程 - m_serverThread = std::make_unique(&ModbusTCPServer::serverLoop, this); - - // 初始状态：服务器启动但没有客户端连接 - if (m_connectionStatusCallback) { - m_connectionStatusCallback(false); - } - - return SUCCESS; -} - -void ModbusTCPServer::stop() -{ - if (!m_isRunning.load()) { - return; - } - - m_shouldStop.store(true); - - // 关闭服务器socket以中断accept调用 - if (m_serverSocket != -1) { -#ifdef _WIN32 - closesocket(m_serverSocket); -#else - close(m_serverSocket); -#endif - m_serverSocket = -1; - } - - // 等待服务器线程退出 - if (m_serverThread && m_serverThread->joinable()) { - m_serverThread->join(); - } - - // 清理所有客户端连接 - { - std::lock_guard lock(m_clientsMutex); - m_clients.clear(); - } - - // 清理资源 - if (m_mapping) { - modbus_mapping_free(m_mapping); - m_mapping = nullptr; - } - - if (m_modbusCtx) { - modbus_free(m_modbusCtx); - m_modbusCtx = nullptr; - } - - m_isRunning.store(false); -} - -void ModbusTCPServer::serverLoop() -{ - fd_set readfds; - int maxfd = m_serverSocket; - - while (!m_shouldStop.load()) { - FD_ZERO(&readfds); - FD_SET(m_serverSocket, &readfds); - - maxfd = m_serverSocket; - - // 添加所有客户端socket到监听集合 - { - std::lock_guard lock(m_clientsMutex); - for (const auto& pair : m_clients) { - int clientSocket = pair.first; - FD_SET(clientSocket, &readfds); - if (clientSocket > maxfd) { - maxfd = clientSocket; - } - } - } - - // 设置超时时间（1秒），避免阻塞太久 - struct timeval timeout; - timeout.tv_sec = 1; - timeout.tv_usec = 0; - - int selectResult = select(maxfd + 1, &readfds, nullptr, nullptr, &timeout); - - if (selectResult == -1) { - if (!m_shouldStop.load()) { - LOG_WARNING("select error: %s\n", strerror(errno)); - } - break; - } - - if (selectResult == 0) { - // 超时，继续循环 - continue; - } - - // 检查服务器socket是否有新连接 - if (FD_ISSET(m_serverSocket, &readfds)) { - handleNewConnection(); - } - - // 检查客户端socket是否有数据 - std::vector socketsToRemove; - { - std::lock_guard lock(m_clientsMutex); - for (const auto& pair : m_clients) { - int clientSocket = pair.first; - if (FD_ISSET(clientSocket, &readfds)) { - try { - handleClientData(pair.second); - } catch (...) { - // 客户端处理出错，标记删除 - socketsToRemove.push_back(clientSocket); - } - } - } - } - - // 删除有问题的客户端连接 - for (int socket : socketsToRemove) { - removeClient(socket); - } - } -} - -void ModbusTCPServer::handleNewConnection() -{ - int clientSocket = modbus_tcp_accept(m_modbusCtx, &m_serverSocket); - if (clientSocket == -1) { - if (!m_shouldStop.load()) { - LOG_ERRO("Accept connection failed: %s\n", modbus_strerror(errno)); - } - return; - } - - // 创建新的modbus context用于此客户端 - modbus_t* clientCtx = modbus_new_tcp("0.0.0.0", m_port); - if (clientCtx == nullptr) { - LOG_ERRO("Create Modbus context for client failed\n"); -#ifdef _WIN32 - closesocket(clientSocket); -#else - close(clientSocket); -#endif - return; - } - - // 设置socket为非阻塞模式 - modbus_set_socket(clientCtx, clientSocket); - - // 创建客户端连接对象 - auto client = std::make_shared(clientSocket, clientCtx); - - // 添加到客户端列表 - { - std::lock_guard lock(m_clientsMutex); - m_clients[clientSocket] = client; - } - - LOG_INFO("New client connected: socket=%d\n", clientSocket); - - // 触发连接状态回调 - if (m_connectionStatusCallback) { - m_connectionStatusCallback(true); - } -} - -void ModbusTCPServer::handleClientData(std::shared_ptr client) -{ - uint8_t buffer[MODBUS_TCP_MAX_ADU_LENGTH]; - - // 接收数据 - int bytesReceived = recv(client->socket, reinterpret_cast(buffer), sizeof(buffer), 0); - - if (bytesReceived <= 0) { - // 连接断开或错误 - if (bytesReceived == 0) { - LOG_VERBOSE("Client disconnected: socket=%d\n", client->socket); - } else { - LOG_ERRO("Receive data error: %s\n", strerror(errno)); - } - throw std::runtime_error("客户端连接错误"); - } - - // 处理Modbus请求 - processModbusRequest(client, buffer, bytesReceived); -} - -void ModbusTCPServer::removeClient(int socket) -{ - std::lock_guard lock(m_clientsMutex); - auto it = m_clients.find(socket); - if (it != m_clients.end()) { - LOG_INFO("Remove client connection: socket=%d\n", socket); - m_clients.erase(it); - - // 如果没有客户端连接了，触发断开回调 - if (m_clients.empty() && m_connectionStatusCallback) { - m_connectionStatusCallback(false); - } - } -} - -void ModbusTCPServer::processModbusRequest(std::shared_ptr client, const uint8_t* query, int queryLength) -{ - // 使用modbus标准异常码，0表示成功 - uint8_t exceptionCode = 0; - - // 解析ModbusTCP ADU的基本字段 - uint16_t transactionId = 0; - uint16_t protocolId = 0; - uint16_t length = 0; - uint8_t unitId = 0; - uint8_t function = 0; - uint16_t address = 0; - uint16_t value = 0; - uint16_t quantity = 0; - uint8_t byteCount = 0; - std::vector registerValues; - std::vector coilValues; - - // 基本长度检查 - if (queryLength < 8) { - exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; - modbus_reply_exception(client->modbusCtx, query, exceptionCode); - return; - } - - - // 解析ADU头部 - transactionId = (query[0] << 8) | query[1]; - protocolId = (query[2] << 8) | query[3]; - length = (query[4] << 8) | query[5]; - unitId = query[6]; - function = query[7]; - - // 验证协议ID - if (protocolId != 0) { - exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; - } - // 验证长度字段 - else if (length != (queryLength - 6)) { - exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; - } - - if(exceptionCode != 0) { - modbus_reply_exception(client->modbusCtx, query, exceptionCode); - return; - } - - // 根据功能码验证数据格式并执行回调 - LOG_DEBUG("Modbus %02X - Trans:%d Unit:%d\n", function, transactionId, unitId); - - switch (function) { - case MODBUS_FC_WRITE_SINGLE_COIL: - { - if (queryLength < 12) { - exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; - } else { - address = (query[8] << 8) | query[9]; - value = (query[10] << 8) | query[11]; - - if (m_writeCoilsCallback) { - uint8_t coilValue = (value == 0xFF00) ? 1 : 0; - IYModbusTCPServer::ErrorCode result = m_writeCoilsCallback(unitId, address, 1, &coilValue); - exceptionCode = (uint8_t)result; - } - } - } - break; - - case MODBUS_FC_WRITE_MULTIPLE_COILS: - { - if (queryLength < 13) { - exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; - } else { - address = (query[8] << 8) | query[9]; - quantity = (query[10] << 8) | query[11]; - byteCount = query[12]; - - if (queryLength < (13 + byteCount)) { - exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; - } else { - coilValues.assign(&query[13], &query[13 + byteCount]); - - if (m_writeCoilsCallback) { - IYModbusTCPServer::ErrorCode result = m_writeCoilsCallback(unitId, address, quantity, coilValues.data()); - exceptionCode = (uint8_t)result; - } - } - } - } - break; - - case MODBUS_FC_WRITE_SINGLE_REGISTER: - { - if (queryLength < 12) { - exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; - } else { - address = (query[8] << 8) | query[9]; - value = (query[10] << 8) | query[11]; - - if (m_writeRegistersCallback) { - IYModbusTCPServer::ErrorCode result = m_writeRegistersCallback(unitId, address, 1, &value); - exceptionCode = (uint8_t)result; - } - } - } - break; - - case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: - { - if (queryLength < 13) { - exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; - } else { - address = (query[8] << 8) | query[9]; - quantity = (query[10] << 8) | query[11]; - byteCount = query[12]; - - if (queryLength < (13 + byteCount) || byteCount != (quantity * 2)) { - exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; - } else { - registerValues.reserve(quantity); - for (int i = 0; i < quantity; i++) { - uint16_t regValue = (query[13 + i*2] << 8) | query[13 + i*2 + 1]; - registerValues.push_back(regValue); - } - - if (m_writeRegistersCallback) { - IYModbusTCPServer::ErrorCode result = m_writeRegistersCallback(unitId, address, quantity, registerValues.data()); - exceptionCode = (uint8_t)result; - } - } - } - } - break; - - default: - // exceptionCode = MODBUS_EXCEPTION_ILLEGAL_FUNCTION; - break; - } - - LOG_DEBUG("exceptionCode: %d\n", exceptionCode); - - // 统一处理响应 - if (exceptionCode != 0) { - modbus_reply_exception(client->modbusCtx, query, exceptionCode); - return; - } - - // 发送正常响应 - { - std::lock_guard lock(m_dataMutex); - int responseRc = modbus_reply(client->modbusCtx, query, queryLength, m_mapping); - - if (responseRc == -1) { - modbus_reply_exception(client->modbusCtx, query, MODBUS_EXCEPTION_SLAVE_OR_SERVER_FAILURE); - return; - } - } -} - -void ModbusTCPServer::updateCoil(uint16_t address, bool value) -{ - std::lock_guard lock(m_dataMutex); - if (m_mapping && address < m_mapping->nb_bits) { - m_mapping->tab_bits[address] = value ? 1 : 0; - } -} - -void ModbusTCPServer::updateCoils(uint16_t startAddress, const std::vector& values) -{ - std::lock_guard lock(m_dataMutex); - if (!m_mapping) return; - - for (size_t i = 0; i < values.size() && (startAddress + i) < m_mapping->nb_bits; ++i) { - m_mapping->tab_bits[startAddress + i] = values[i] ? 1 : 0; - } -} - -void ModbusTCPServer::updateDiscreteInput(uint16_t address, bool value) -{ - std::lock_guard lock(m_dataMutex); - if (m_mapping && address < m_mapping->nb_input_bits) { - m_mapping->tab_input_bits[address] = value ? 1 : 0; - } -} - -void ModbusTCPServer::updateDiscreteInputs(uint16_t startAddress, const std::vector& values) -{ - std::lock_guard lock(m_dataMutex); - if (!m_mapping) return; - - for (size_t i = 0; i < values.size() && (startAddress + i) < m_mapping->nb_input_bits; ++i) { - m_mapping->tab_input_bits[startAddress + i] = values[i] ? 1 : 0; - } -} - -void ModbusTCPServer::updateHoldingRegister(uint16_t address, uint16_t value) -{ - std::lock_guard lock(m_dataMutex); - if (m_mapping && address < m_mapping->nb_registers) { - m_mapping->tab_registers[address] = value; - } -} - -void ModbusTCPServer::updateHoldingRegisters(uint16_t startAddress, const std::vector& values) -{ - LOG_DEBUG("updateHoldingRegisters: startAddress=%d, values.size()=%d\n", startAddress, values.size()); - std::lock_guard lock(m_dataMutex); - if (!m_mapping) return; - - for (size_t i = 0; i < values.size() && (startAddress + i) < m_mapping->nb_registers; ++i) { - m_mapping->tab_registers[startAddress + i] = values[i]; - } -} - -void ModbusTCPServer::updateInputRegister(uint16_t address, uint16_t value) -{ - std::lock_guard lock(m_dataMutex); - if (m_mapping && address < m_mapping->nb_input_registers) { - m_mapping->tab_input_registers[address] = value; - } -} - -void ModbusTCPServer::updateInputRegisters(uint16_t startAddress, const std::vector& values) -{ - std::lock_guard lock(m_dataMutex); - if (!m_mapping) return; - - for (size_t i = 0; i < values.size() && (startAddress + i) < m_mapping->nb_input_registers; ++i) { - m_mapping->tab_input_registers[startAddress + i] = values[i]; - } -} - -std::string ModbusTCPServer::getLastError() const -{ - std::lock_guard lock(m_errorMutex); - return m_lastError; -} - -void ModbusTCPServer::setLastError(const std::string& error) -{ - std::lock_guard lock(m_errorMutex); - m_lastError = error; -} - -bool IYModbusTCPServer::CreateInstance(IYModbusTCPServer** ppModbusTCPServer) -{ - if (ppModbusTCPServer == nullptr) { - return false; - } - - *ppModbusTCPServer = new ModbusTCPServer(); - return true; -} +#include "ModbusTCPServer.h" +#include +#include +#include + +#include "VrError.h" +#include "VrLog.h" + +#ifdef _WIN32 +#include +#include +#pragma comment(lib, "ws2_32.lib") +#else +#include +#include +#include +#include +#include +#include +#endif + +ModbusTCPServer::ModbusTCPServer() + : m_modbusCtx(nullptr) + , m_mapping(nullptr) + , m_isRunning(false) + , m_shouldStop(false) + , m_serverSocket(-1) + , m_port(502) + , m_maxConnections(10) +{ +} + +ModbusTCPServer::~ModbusTCPServer() +{ + stop(); +} + +int ModbusTCPServer::start(int port, int maxConnections) +{ + if (m_isRunning.load()) { + setLastError("服务器已在运行"); + return SUCCESS; + } + + m_port = port; + m_maxConnections = maxConnections; + + LOG_DEBUG("ModbusTCPServer::start: port=%d, maxConnections=%d\n", port, maxConnections); + + // 创建modbus TCP context + m_modbusCtx = modbus_new_tcp("0.0.0.0", port); + if (m_modbusCtx == nullptr) { + setLastError("创建Modbus TCP上下文失败"); + return ERR_CODE(NET_ERR_CREAT_INIT); + } + + // 创建数据映射 - 分配足够的空间 + // 线圈: 0-9999, 离散输入: 0-9999, 保持寄存器: 0-9999, 输入寄存器: 0-9999 + m_mapping = modbus_mapping_new(10000, 10000, 10000, 10000); + if (m_mapping == nullptr) { + setLastError("创建数据映射失败"); + modbus_free(m_modbusCtx); + m_modbusCtx = nullptr; + return ERR_CODE(NET_ERR_CONFIG); + } + + // 监听连接 + m_serverSocket = modbus_tcp_listen(m_modbusCtx, maxConnections); + if (m_serverSocket == -1) { + setLastError(std::string("监听失败: ") + modbus_strerror(errno)); + modbus_mapping_free(m_mapping); + m_mapping = nullptr; + modbus_free(m_modbusCtx); + m_modbusCtx = nullptr; + return ERR_CODE(NET_ERR_CREAT_LISTEN); + } + + m_isRunning.store(true); + m_shouldStop.store(false); + + // 启动服务器线程 + m_serverThread = std::make_unique(&ModbusTCPServer::serverLoop, this); + + // 初始状态：服务器启动但没有客户端连接 + if (m_connectionStatusCallback) { + m_connectionStatusCallback(false); + } + + return SUCCESS; +} + +void ModbusTCPServer::stop() +{ + if (!m_isRunning.load()) { + return; + } + + m_shouldStop.store(true); + + // 关闭服务器socket以中断accept调用 + if (m_serverSocket != -1) { +#ifdef _WIN32 + closesocket(m_serverSocket); +#else + close(m_serverSocket); +#endif + m_serverSocket = -1; + } + + // 等待服务器线程退出 + if (m_serverThread && m_serverThread->joinable()) { + m_serverThread->join(); + } + + // 清理所有客户端连接 + { + std::lock_guard lock(m_clientsMutex); + m_clients.clear(); + } + + // 清理资源 + if (m_mapping) { + modbus_mapping_free(m_mapping); + m_mapping = nullptr; + } + + if (m_modbusCtx) { + modbus_free(m_modbusCtx); + m_modbusCtx = nullptr; + } + + m_isRunning.store(false); +} + +void ModbusTCPServer::serverLoop() +{ + fd_set readfds; + int maxfd = m_serverSocket; + + while (!m_shouldStop.load()) { + FD_ZERO(&readfds); + FD_SET(m_serverSocket, &readfds); + + maxfd = m_serverSocket; + + // 添加所有客户端socket到监听集合 + { + std::lock_guard lock(m_clientsMutex); + for (const auto& pair : m_clients) { + int clientSocket = pair.first; + FD_SET(clientSocket, &readfds); + if (clientSocket > maxfd) { + maxfd = clientSocket; + } + } + } + + // 设置超时时间（1秒），避免阻塞太久 + struct timeval timeout; + timeout.tv_sec = 1; + timeout.tv_usec = 0; + + int selectResult = select(maxfd + 1, &readfds, nullptr, nullptr, &timeout); + + if (selectResult == -1) { + if (!m_shouldStop.load()) { + LOG_WARNING("select error: %s\n", strerror(errno)); + } + break; + } + + if (selectResult == 0) { + // 超时，继续循环 + continue; + } + + // 检查服务器socket是否有新连接 + if (FD_ISSET(m_serverSocket, &readfds)) { + handleNewConnection(); + } + + // 检查客户端socket是否有数据 + std::vector socketsToRemove; + { + std::lock_guard lock(m_clientsMutex); + for (const auto& pair : m_clients) { + int clientSocket = pair.first; + if (FD_ISSET(clientSocket, &readfds)) { + try { + handleClientData(pair.second); + } catch (...) { + // 客户端处理出错，标记删除 + socketsToRemove.push_back(clientSocket); + } + } + } + } + + // 删除有问题的客户端连接 + for (int socket : socketsToRemove) { + removeClient(socket); + } + } +} + +void ModbusTCPServer::handleNewConnection() +{ + int clientSocket = modbus_tcp_accept(m_modbusCtx, &m_serverSocket); + if (clientSocket == -1) { + if (!m_shouldStop.load()) { + LOG_ERRO("Accept connection failed: %s\n", modbus_strerror(errno)); + } + return; + } + + // 创建新的modbus context用于此客户端 + modbus_t* clientCtx = modbus_new_tcp("0.0.0.0", m_port); + if (clientCtx == nullptr) { + LOG_ERRO("Create Modbus context for client failed\n"); +#ifdef _WIN32 + closesocket(clientSocket); +#else + close(clientSocket); +#endif + return; + } + + // 设置socket为非阻塞模式 + modbus_set_socket(clientCtx, clientSocket); + + // 创建客户端连接对象 + auto client = std::make_shared(clientSocket, clientCtx); + + // 添加到客户端列表 + { + std::lock_guard lock(m_clientsMutex); + m_clients[clientSocket] = client; + } + + LOG_INFO("New client connected: socket=%d\n", clientSocket); + + // 触发连接状态回调 + if (m_connectionStatusCallback) { + m_connectionStatusCallback(true); + } +} + +void ModbusTCPServer::handleClientData(std::shared_ptr client) +{ + uint8_t buffer[MODBUS_TCP_MAX_ADU_LENGTH]; + + // 接收数据 + int bytesReceived = recv(client->socket, reinterpret_cast(buffer), sizeof(buffer), 0); + + if (bytesReceived <= 0) { + // 连接断开或错误 + if (bytesReceived == 0) { + LOG_VERBOSE("Client disconnected: socket=%d\n", client->socket); + } else { + LOG_ERRO("Receive data error: %s\n", strerror(errno)); + } + throw std::runtime_error("客户端连接错误"); + } + + // 处理Modbus请求 + processModbusRequest(client, buffer, bytesReceived); +} + +void ModbusTCPServer::removeClient(int socket) +{ + std::lock_guard lock(m_clientsMutex); + auto it = m_clients.find(socket); + if (it != m_clients.end()) { + LOG_INFO("Remove client connection: socket=%d\n", socket); + m_clients.erase(it); + + // 如果没有客户端连接了，触发断开回调 + if (m_clients.empty() && m_connectionStatusCallback) { + m_connectionStatusCallback(false); + } + } +} + +void ModbusTCPServer::processModbusRequest(std::shared_ptr client, const uint8_t* query, int queryLength) +{ + // 使用modbus标准异常码，0表示成功 + uint8_t exceptionCode = 0; + + // 解析ModbusTCP ADU的基本字段 + uint16_t transactionId = 0; + uint16_t protocolId = 0; + uint16_t length = 0; + uint8_t unitId = 0; + uint8_t function = 0; + uint16_t address = 0; + uint16_t value = 0; + uint16_t quantity = 0; + uint8_t byteCount = 0; + std::vector registerValues; + std::vector coilValues; + + // 基本长度检查 + if (queryLength < 8) { + exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; + modbus_reply_exception(client->modbusCtx, query, exceptionCode); + return; + } + + + // 解析ADU头部 + transactionId = (query[0] << 8) | query[1]; + protocolId = (query[2] << 8) | query[3]; + length = (query[4] << 8) | query[5]; + unitId = query[6]; + function = query[7]; + + // 验证协议ID + if (protocolId != 0) { + exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; + } + // 验证长度字段 + else if (length != (queryLength - 6)) { + exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; + } + + if(exceptionCode != 0) { + modbus_reply_exception(client->modbusCtx, query, exceptionCode); + return; + } + + // 根据功能码验证数据格式并执行回调 + LOG_DEBUG("Modbus %02X - Trans:%d Unit:%d\n", function, transactionId, unitId); + + switch (function) { + case MODBUS_FC_WRITE_SINGLE_COIL: + { + if (queryLength < 12) { + exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; + } else { + address = (query[8] << 8) | query[9]; + value = (query[10] << 8) | query[11]; + + if (m_writeCoilsCallback) { + uint8_t coilValue = (value == 0xFF00) ? 1 : 0; + IYModbusTCPServer::ErrorCode result = m_writeCoilsCallback(unitId, address, 1, &coilValue); + exceptionCode = (uint8_t)result; + } + } + } + break; + + case MODBUS_FC_WRITE_MULTIPLE_COILS: + { + if (queryLength < 13) { + exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; + } else { + address = (query[8] << 8) | query[9]; + quantity = (query[10] << 8) | query[11]; + byteCount = query[12]; + + if (queryLength < (13 + byteCount)) { + exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; + } else { + coilValues.assign(&query[13], &query[13 + byteCount]); + + if (m_writeCoilsCallback) { + IYModbusTCPServer::ErrorCode result = m_writeCoilsCallback(unitId, address, quantity, coilValues.data()); + exceptionCode = (uint8_t)result; + } + } + } + } + break; + + case MODBUS_FC_WRITE_SINGLE_REGISTER: + { + if (queryLength < 12) { + exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; + } else { + address = (query[8] << 8) | query[9]; + value = (query[10] << 8) | query[11]; + + if (m_writeRegistersCallback) { + IYModbusTCPServer::ErrorCode result = m_writeRegistersCallback(unitId, address, 1, &value); + exceptionCode = (uint8_t)result; + } + } + } + break; + + case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: + { + if (queryLength < 13) { + exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; + } else { + address = (query[8] << 8) | query[9]; + quantity = (query[10] << 8) | query[11]; + byteCount = query[12]; + + if (queryLength < (13 + byteCount) || byteCount != (quantity * 2)) { + exceptionCode = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; + } else { + registerValues.reserve(quantity); + for (int i = 0; i < quantity; i++) { + uint16_t regValue = (query[13 + i*2] << 8) | query[13 + i*2 + 1]; + registerValues.push_back(regValue); + } + + if (m_writeRegistersCallback) { + IYModbusTCPServer::ErrorCode result = m_writeRegistersCallback(unitId, address, quantity, registerValues.data()); + exceptionCode = (uint8_t)result; + } + } + } + } + break; + + default: + // exceptionCode = MODBUS_EXCEPTION_ILLEGAL_FUNCTION; + break; + } + + LOG_DEBUG("exceptionCode: %d\n", exceptionCode); + + // 统一处理响应 + if (exceptionCode != 0) { + modbus_reply_exception(client->modbusCtx, query, exceptionCode); + return; + } + + // 发送正常响应 + { + std::lock_guard lock(m_dataMutex); + int responseRc = modbus_reply(client->modbusCtx, query, queryLength, m_mapping); + + if (responseRc == -1) { + modbus_reply_exception(client->modbusCtx, query, MODBUS_EXCEPTION_SLAVE_OR_SERVER_FAILURE); + return; + } + } +} + +void ModbusTCPServer::updateCoil(uint16_t address, bool value) +{ + std::lock_guard lock(m_dataMutex); + if (m_mapping && address < m_mapping->nb_bits) { + m_mapping->tab_bits[address] = value ? 1 : 0; + } +} + +void ModbusTCPServer::updateCoils(uint16_t startAddress, const std::vector& values) +{ + std::lock_guard lock(m_dataMutex); + if (!m_mapping) return; + + for (size_t i = 0; i < values.size() && (startAddress + i) < m_mapping->nb_bits; ++i) { + m_mapping->tab_bits[startAddress + i] = values[i] ? 1 : 0; + } +} + +void ModbusTCPServer::updateDiscreteInput(uint16_t address, bool value) +{ + std::lock_guard lock(m_dataMutex); + if (m_mapping && address < m_mapping->nb_input_bits) { + m_mapping->tab_input_bits[address] = value ? 1 : 0; + } +} + +void ModbusTCPServer::updateDiscreteInputs(uint16_t startAddress, const std::vector& values) +{ + std::lock_guard lock(m_dataMutex); + if (!m_mapping) return; + + for (size_t i = 0; i < values.size() && (startAddress + i) < m_mapping->nb_input_bits; ++i) { + m_mapping->tab_input_bits[startAddress + i] = values[i] ? 1 : 0; + } +} + +void ModbusTCPServer::updateHoldingRegister(uint16_t address, uint16_t value) +{ + std::lock_guard lock(m_dataMutex); + if (m_mapping && address < m_mapping->nb_registers) { + m_mapping->tab_registers[address] = value; + } +} + +void ModbusTCPServer::updateHoldingRegisters(uint16_t startAddress, const std::vector& values) +{ + LOG_DEBUG("updateHoldingRegisters: startAddress=%d, values.size()=%d\n", startAddress, values.size()); + std::lock_guard lock(m_dataMutex); + if (!m_mapping) return; + + for (size_t i = 0; i < values.size() && (startAddress + i) < m_mapping->nb_registers; ++i) { + m_mapping->tab_registers[startAddress + i] = values[i]; + } +} + +void ModbusTCPServer::updateInputRegister(uint16_t address, uint16_t value) +{ + std::lock_guard lock(m_dataMutex); + if (m_mapping && address < m_mapping->nb_input_registers) { + m_mapping->tab_input_registers[address] = value; + } +} + +void ModbusTCPServer::updateInputRegisters(uint16_t startAddress, const std::vector& values) +{ + std::lock_guard lock(m_dataMutex); + if (!m_mapping) return; + + for (size_t i = 0; i < values.size() && (startAddress + i) < m_mapping->nb_input_registers; ++i) { + m_mapping->tab_input_registers[startAddress + i] = values[i]; + } +} + +std::string ModbusTCPServer::getLastError() const +{ + std::lock_guard lock(m_errorMutex); + return m_lastError; +} + +void ModbusTCPServer::setLastError(const std::string& error) +{ + std::lock_guard lock(m_errorMutex); + m_lastError = error; +} + +bool IYModbusTCPServer::CreateInstance(IYModbusTCPServer** ppModbusTCPServer) +{ + if (ppModbusTCPServer == nullptr) { + return false; + } + + *ppModbusTCPServer = new ModbusTCPServer(); + return true; +} diff --git a/Module/ModbusTCPServer/_Inc/ModbusTCPServer.h b/Module/ModbusTCPServer/_Inc/ModbusTCPServer.h index b6bab586..ad6f8aa0 100644 --- a/Module/ModbusTCPServer/_Inc/ModbusTCPServer.h +++ b/Module/ModbusTCPServer/_Inc/ModbusTCPServer.h @@ -1,139 +1,139 @@ -#ifndef MODBUS_TCP_SERVER_H -#define MODBUS_TCP_SERVER_H - -#include "IYModbusTCPServer.h" -#include -#include -#include -#include -#include -#include -#include - -// Platform-specific includes for socket operations -#ifdef _WIN32 -#include -#include -#pragma comment(lib, "ws2_32.lib") -#else -#include -#include -#include -#include -#include -#include -#endif - -// libmodbus headers -extern "C" { -#include "modbus.h" -#include "modbus-tcp.h" -} - -class ModbusTCPServer : public IYModbusTCPServer { -private: - // 客户端连接结构体 - struct ClientConnection { - int socket; - modbus_t* modbusCtx; - - ClientConnection(int sock, modbus_t* ctx) : socket(sock), modbusCtx(ctx) {} - - ~ClientConnection() { - if (modbusCtx) { - modbus_free(modbusCtx); - } -#ifdef _WIN32 - if (socket != -1) { - closesocket(socket); - } -#else - if (socket != -1) { - close(socket); - } -#endif - } - }; - -public: - ModbusTCPServer(); - virtual ~ModbusTCPServer() override; - - // 实现IYModbusTCPServer接口 - // 启动/停止服务器 - virtual int start(int port = 502, int maxConnections = 10) override; - virtual void stop() override; - bool isRunning() const { return m_isRunning.load(); } - - // 设置回调函数 - virtual void setWriteCoilsCallback(WriteCoilsCallback callback) override { m_writeCoilsCallback = callback; } - virtual void setWriteRegistersCallback(WriteRegistersCallback callback) override { m_writeRegistersCallback = callback; } - virtual void setConnectionStatusCallback(ConnectionStatusCallback callback) override { m_connectionStatusCallback = callback; } - - // 数据更新接口 - 线程安全 - virtual void updateCoil(uint16_t address, bool value) override; - virtual void updateCoils(uint16_t startAddress, const std::vector& values) override; - virtual void updateDiscreteInput(uint16_t address, bool value) override; - virtual void updateDiscreteInputs(uint16_t startAddress, const std::vector& values) override; - virtual void updateHoldingRegister(uint16_t address, uint16_t value) override; - virtual void updateHoldingRegisters(uint16_t startAddress, const std::vector& values) override; - virtual void updateInputRegister(uint16_t address, uint16_t value) override; - virtual void updateInputRegisters(uint16_t startAddress, const std::vector& values) override; - - // 获取错误信息 - std::string getLastError() const; - -private: - // 服务器主循环 - void serverLoop(); - - // 客户端连接管理 - void handleNewConnection(); - void handleClientData(std::shared_ptr client); - void removeClient(int socket); - void processModbusRequest(std::shared_ptr client, const uint8_t* query, int queryLength); - - // 数据读取处理器（libmodbus内部调用） - static int readCoilsHandler(modbus_t *ctx, int function_code, int addr, int nb, uint8_t *rsp, int *rsp_length, void *user_data); - static int readDiscreteInputsHandler(modbus_t *ctx, int function_code, int addr, int nb, uint8_t *rsp, int *rsp_length, void *user_data); - static int readHoldingRegistersHandler(modbus_t *ctx, int function_code, int addr, int nb, uint8_t *rsp, int *rsp_length, void *user_data); - static int readInputRegistersHandler(modbus_t *ctx, int function_code, int addr, int nb, uint8_t *rsp, int *rsp_length, void *user_data); - - // 数据写入函数（libmodbus内部调用） - static int writeCoilsHandler(modbus_t *ctx, int function_code, int addr, int nb, const uint8_t *req, int req_length, void *user_data); - static int writeRegistersHandler(modbus_t *ctx, int function_code, int addr, int nb, const uint8_t *req, int req_length, void *user_data); - -private: - // libmodbus context - modbus_t* m_modbusCtx; - modbus_mapping_t* m_mapping; - - // 服务器状态 - std::atomic m_isRunning; - std::atomic m_shouldStop; - std::unique_ptr m_serverThread; - int m_serverSocket; - int m_port; - int m_maxConnections; - - // 客户端连接管理 - std::unordered_map> m_clients; - std::mutex m_clientsMutex; - - // 数据存储 - 使用libmodbus的mapping结构 - std::mutex m_dataMutex; - - // 回调函数 - WriteCoilsCallback m_writeCoilsCallback; - WriteRegistersCallback m_writeRegistersCallback; - ConnectionStatusCallback m_connectionStatusCallback; - - // 错误信息 - mutable std::mutex m_errorMutex; - std::string m_lastError; - - // 设置错误信息 - void setLastError(const std::string& error); -}; - -#endif // MODBUS_TCP_SERVER_H +#ifndef MODBUS_TCP_SERVER_H +#define MODBUS_TCP_SERVER_H + +#include "IYModbusTCPServer.h" +#include +#include +#include +#include +#include +#include +#include + +// Platform-specific includes for socket operations +#ifdef _WIN32 +#include +#include +#pragma comment(lib, "ws2_32.lib") +#else +#include +#include +#include +#include +#include +#include +#endif + +// libmodbus headers +extern "C" { +#include "modbus.h" +#include "modbus-tcp.h" +} + +class ModbusTCPServer : public IYModbusTCPServer { +private: + // 客户端连接结构体 + struct ClientConnection { + int socket; + modbus_t* modbusCtx; + + ClientConnection(int sock, modbus_t* ctx) : socket(sock), modbusCtx(ctx) {} + + ~ClientConnection() { + if (modbusCtx) { + modbus_free(modbusCtx); + } +#ifdef _WIN32 + if (socket != -1) { + closesocket(socket); + } +#else + if (socket != -1) { + close(socket); + } +#endif + } + }; + +public: + ModbusTCPServer(); + virtual ~ModbusTCPServer() override; + + // 实现IYModbusTCPServer接口 + // 启动/停止服务器 + virtual int start(int port = 502, int maxConnections = 10) override; + virtual void stop() override; + bool isRunning() const { return m_isRunning.load(); } + + // 设置回调函数 + virtual void setWriteCoilsCallback(WriteCoilsCallback callback) override { m_writeCoilsCallback = callback; } + virtual void setWriteRegistersCallback(WriteRegistersCallback callback) override { m_writeRegistersCallback = callback; } + virtual void setConnectionStatusCallback(ConnectionStatusCallback callback) override { m_connectionStatusCallback = callback; } + + // 数据更新接口 - 线程安全 + virtual void updateCoil(uint16_t address, bool value) override; + virtual void updateCoils(uint16_t startAddress, const std::vector& values) override; + virtual void updateDiscreteInput(uint16_t address, bool value) override; + virtual void updateDiscreteInputs(uint16_t startAddress, const std::vector& values) override; + virtual void updateHoldingRegister(uint16_t address, uint16_t value) override; + virtual void updateHoldingRegisters(uint16_t startAddress, const std::vector& values) override; + virtual void updateInputRegister(uint16_t address, uint16_t value) override; + virtual void updateInputRegisters(uint16_t startAddress, const std::vector& values) override; + + // 获取错误信息 + std::string getLastError() const; + +private: + // 服务器主循环 + void serverLoop(); + + // 客户端连接管理 + void handleNewConnection(); + void handleClientData(std::shared_ptr client); + void removeClient(int socket); + void processModbusRequest(std::shared_ptr client, const uint8_t* query, int queryLength); + + // 数据读取处理器（libmodbus内部调用） + static int readCoilsHandler(modbus_t *ctx, int function_code, int addr, int nb, uint8_t *rsp, int *rsp_length, void *user_data); + static int readDiscreteInputsHandler(modbus_t *ctx, int function_code, int addr, int nb, uint8_t *rsp, int *rsp_length, void *user_data); + static int readHoldingRegistersHandler(modbus_t *ctx, int function_code, int addr, int nb, uint8_t *rsp, int *rsp_length, void *user_data); + static int readInputRegistersHandler(modbus_t *ctx, int function_code, int addr, int nb, uint8_t *rsp, int *rsp_length, void *user_data); + + // 数据写入函数（libmodbus内部调用） + static int writeCoilsHandler(modbus_t *ctx, int function_code, int addr, int nb, const uint8_t *req, int req_length, void *user_data); + static int writeRegistersHandler(modbus_t *ctx, int function_code, int addr, int nb, const uint8_t *req, int req_length, void *user_data); + +private: + // libmodbus context + modbus_t* m_modbusCtx; + modbus_mapping_t* m_mapping; + + // 服务器状态 + std::atomic m_isRunning; + std::atomic m_shouldStop; + std::unique_ptr m_serverThread; + int m_serverSocket; + int m_port; + int m_maxConnections; + + // 客户端连接管理 + std::unordered_map> m_clients; + std::mutex m_clientsMutex; + + // 数据存储 - 使用libmodbus的mapping结构 + std::mutex m_dataMutex; + + // 回调函数 + WriteCoilsCallback m_writeCoilsCallback; + WriteRegistersCallback m_writeRegistersCallback; + ConnectionStatusCallback m_connectionStatusCallback; + + // 错误信息 + mutable std::mutex m_errorMutex; + std::string m_lastError; + + // 设置错误信息 + void setLastError(const std::string& error); +}; + +#endif // MODBUS_TCP_SERVER_H diff --git a/VrNets/VrTcpServer.pro b/VrNets/VrTcpServer.pro new file mode 100644 index 00000000..ec1b209b --- /dev/null +++ b/VrNets/VrTcpServer.pro @@ -0,0 +1,27 @@ +QT += core network + +TARGET = VrTcpServer +TEMPLATE = lib +CONFIG += c++11 staticlib + +# 源文件 +SOURCES += \ + tcpServer/Src/VrTcpServer.cpp + +# 头文件 +HEADERS += \ + tcpServer/Inc/IVrTcpServer.h \ + tcpServer/Inc/VrTcpServer.h + +# 包含路径 +INCLUDEPATH += \ + tcpServer/Inc + +# 安装路径 +target.path = $$[QT_INSTALL_LIBS] +INSTALLS += target + +# 头文件安装路径 +headers.files = $$HEADERS +headers.path = $$[QT_INSTALL_HEADERS]/VrTcpServer +INSTALLS += headers \ No newline at end of file diff --git a/VrNets/tcpClient/Inc/IVrTcpClient.h b/VrNets/tcpClient/Inc/IVrTcpClient.h index cdcef3b6..4ce0175d 100644 --- a/VrNets/tcpClient/Inc/IVrTcpClient.h +++ b/VrNets/tcpClient/Inc/IVrTcpClient.h @@ -1,55 +1,55 @@ -#ifndef IVRTCPCLIENT_H -#define IVRTCPCLIENT_H - -#include -#include -#include - -class IVrTcpClient : public QObject -{ - Q_OBJECT - -public: - explicit IVrTcpClient(QObject *parent = nullptr) : QObject(parent) {} - virtual ~IVrTcpClient() {} - - // 连接服务器 - virtual bool connectToServer(const QString &host, quint16 port) = 0; - virtual bool connectToServer(const QHostAddress &address, quint16 port) = 0; - - // 断开连接 - virtual void disconnectFromServer() = 0; - - // 发送数据 - virtual qint64 sendData(const QByteArray &data) = 0; - virtual qint64 sendData(const char *data, qint64 size) = 0; - - // 获取连接状态 - virtual bool isConnected() const = 0; - - // 获取服务器地址和端口 - virtual QHostAddress serverAddress() const = 0; - virtual quint16 serverPort() const = 0; - -signals: - // 连接状态变化信号 - void connected(); - void disconnected(); - void connectionError(const QString &errorString); - - // 数据接收信号 - void dataReceived(const QByteArray &data); - - // 数据发送信号 - void dataSent(qint64 bytesSent); - -protected: - // 设置连接状态（供子类使用） - void setConnected(bool connected) { m_connected = connected; } - - bool m_connected = false; - QHostAddress m_serverAddress; - quint16 m_serverPort = 0; -}; - -#endif // IVRTCPCLIENT_H \ No newline at end of file +#ifndef IVRTCPCLIENT_H +#define IVRTCPCLIENT_H + +#include +#include +#include + +class IVrTcpClient : public QObject +{ + Q_OBJECT + +public: + explicit IVrTcpClient(QObject *parent = nullptr) : QObject(parent) {} + virtual ~IVrTcpClient() {} + + // 连接服务器 + virtual bool connectToServer(const QString &host, quint16 port) = 0; + virtual bool connectToServer(const QHostAddress &address, quint16 port) = 0; + + // 断开连接 + virtual void disconnectFromServer() = 0; + + // 发送数据 + virtual qint64 sendData(const QByteArray &data) = 0; + virtual qint64 sendData(const char *data, qint64 size) = 0; + + // 获取连接状态 + virtual bool isConnected() const = 0; + + // 获取服务器地址和端口 + virtual QHostAddress serverAddress() const = 0; + virtual quint16 serverPort() const = 0; + +signals: + // 连接状态变化信号 + void connected(); + void disconnected(); + void connectionError(const QString &errorString); + + // 数据接收信号 + void dataReceived(const QByteArray &data); + + // 数据发送信号 + void dataSent(qint64 bytesSent); + +protected: + // 设置连接状态（供子类使用） + void setConnected(bool connected) { m_connected = connected; } + + bool m_connected = false; + QHostAddress m_serverAddress; + quint16 m_serverPort = 0; +}; + +#endif // IVRTCPCLIENT_H diff --git a/VrNets/tcpClient/Inc/VrTcpClient.h b/VrNets/tcpClient/Inc/VrTcpClient.h index da203f77..185ef773 100644 --- a/VrNets/tcpClient/Inc/VrTcpClient.h +++ b/VrNets/tcpClient/Inc/VrTcpClient.h @@ -1,39 +1,39 @@ -#ifndef VRTCPCLIENT_H -#define VRTCPCLIENT_H - -#include "IVrTcpClient.h" -#include -#include - -class VrTcpClient : public IVrTcpClient -{ - Q_OBJECT - -public: - explicit VrTcpClient(QObject *parent = nullptr); - ~VrTcpClient() override; - - // 接口实现 - bool connectToServer(const QString &host, quint16 port) override; - bool connectToServer(const QHostAddress &address, quint16 port) override; - void disconnectFromServer() override; - qint64 sendData(const QByteArray &data) override; - qint64 sendData(const char *data, qint64 size) override; - bool isConnected() const override; - QHostAddress serverAddress() const override; - quint16 serverPort() const override; - - // 额外功能：自动重连 - void setAutoReconnect(bool autoReconnect); - bool autoReconnect() const; - - void setReconnectInterval(int msec); - int reconnectInterval() const; - -private: - QTcpSocket *m_socket; - QTimer *m_reconnectTimer; - bool m_autoReconnect = false; -}; - -#endif // VRTCPCLIENT_H \ No newline at end of file +#ifndef VRTCPCLIENT_H +#define VRTCPCLIENT_H + +#include "IVrTcpClient.h" +#include +#include + +class VrTcpClient : public IVrTcpClient +{ + Q_OBJECT + +public: + explicit VrTcpClient(QObject *parent = nullptr); + ~VrTcpClient() override; + + // 接口实现 + bool connectToServer(const QString &host, quint16 port) override; + bool connectToServer(const QHostAddress &address, quint16 port) override; + void disconnectFromServer() override; + qint64 sendData(const QByteArray &data) override; + qint64 sendData(const char *data, qint64 size) override; + bool isConnected() const override; + QHostAddress serverAddress() const override; + quint16 serverPort() const override; + + // 额外功能：自动重连 + void setAutoReconnect(bool autoReconnect); + bool autoReconnect() const; + + void setReconnectInterval(int msec); + int reconnectInterval() const; + +private: + QTcpSocket *m_socket; + QTimer *m_reconnectTimer; + bool m_autoReconnect = false; +}; + +#endif // VRTCPCLIENT_H diff --git a/VrNets/tcpClient/Src/VrTcpClient.cpp b/VrNets/tcpClient/Src/VrTcpClient.cpp index f3333071..a01f4e50 100644 --- a/VrNets/tcpClient/Src/VrTcpClient.cpp +++ b/VrNets/tcpClient/Src/VrTcpClient.cpp @@ -1,150 +1,150 @@ -#include "VrTcpClient.h" -#include -#include -#include - -VrTcpClient::VrTcpClient(QObject *parent) - : IVrTcpClient(parent) - , m_socket(new QTcpSocket(this)) - , m_reconnectTimer(new QTimer(this)) -{ - // 设置重连定时器 - m_reconnectTimer->setInterval(5000); // 5秒重连间隔 - m_reconnectTimer->setSingleShot(true); - - // 连接信号槽 - connect(m_socket, &QTcpSocket::connected, this, [this]() { - setConnected(true); - m_reconnectTimer->stop(); - emit connected(); - }); - - connect(m_socket, &QTcpSocket::disconnected, this, [this]() { - setConnected(false); - emit disconnected(); - - // 如果设置了自动重连，则启动重连定时器 - if (m_autoReconnect) { - m_reconnectTimer->start(); - } - }); - - connect(m_socket, QOverload::of(&QTcpSocket::errorOccurred), - this, [this](QAbstractSocket::SocketError error) { - setConnected(false); - emit connectionError(m_socket->errorString()); - }); - - connect(m_socket, &QTcpSocket::readyRead, this, [this]() { - QByteArray data = m_socket->readAll(); - if (!data.isEmpty()) { - emit dataReceived(data); - } - }); - - connect(m_socket, &QTcpSocket::bytesWritten, this, &IVrTcpClient::dataSent); - - connect(m_reconnectTimer, &QTimer::timeout, this, [this]() { - if (m_autoReconnect && !m_serverAddress.isNull() && m_serverPort > 0) { - connectToServer(m_serverAddress, m_serverPort); - } - }); -} - -VrTcpClient::~VrTcpClient() -{ - disconnectFromServer(); -} - -bool VrTcpClient::connectToServer(const QString &host, quint16 port) -{ - m_serverAddress = QHostAddress(host); - m_serverPort = port; - - if (m_serverAddress.isNull()) { - // 如果是主机名，直接连接 - m_socket->connectToHost(host, port); - return true; - } else { - return connectToServer(m_serverAddress, port); - } -} - -bool VrTcpClient::connectToServer(const QHostAddress &address, quint16 port) -{ - m_serverAddress = address; - m_serverPort = port; - - if (isConnected()) { - disconnectFromServer(); - } - - m_socket->connectToHost(address, port); - return true; -} - -void VrTcpClient::disconnectFromServer() -{ - m_reconnectTimer->stop(); - if (m_socket->state() != QAbstractSocket::UnconnectedState) { - m_socket->disconnectFromHost(); - if (m_socket->state() == QAbstractSocket::ConnectedState) { - m_socket->waitForDisconnected(3000); - } - } - setConnected(false); -} - -qint64 VrTcpClient::sendData(const QByteArray &data) -{ - if (!isConnected()) { - return -1; - } - return m_socket->write(data); -} - -qint64 VrTcpClient::sendData(const char *data, qint64 size) -{ - if (!isConnected()) { - return -1; - } - return m_socket->write(data, size); -} - -bool VrTcpClient::isConnected() const -{ - return m_connected && m_socket->state() == QAbstractSocket::ConnectedState; -} - -QHostAddress VrTcpClient::serverAddress() const -{ - return m_serverAddress; -} - -quint16 VrTcpClient::serverPort() const -{ - return m_serverPort; -} - -void VrTcpClient::setAutoReconnect(bool autoReconnect) -{ - m_autoReconnect = autoReconnect; - if (!autoReconnect) { - m_reconnectTimer->stop(); - } -} - -bool VrTcpClient::autoReconnect() const -{ - return m_autoReconnect; -} - -void VrTcpClient::setReconnectInterval(int msec) -{ - m_reconnectTimer->setInterval(msec); -} - -int VrTcpClient::reconnectInterval() const -{ - return m_reconnectTimer->interval(); -} \ No newline at end of file +#include "VrTcpClient.h" +#include +#include +#include + +VrTcpClient::VrTcpClient(QObject *parent) + : IVrTcpClient(parent) + , m_socket(new QTcpSocket(this)) + , m_reconnectTimer(new QTimer(this)) +{ + // 设置重连定时器 + m_reconnectTimer->setInterval(5000); // 5秒重连间隔 + m_reconnectTimer->setSingleShot(true); + + // 连接信号槽 + connect(m_socket, &QTcpSocket::connected, this, [this]() { + setConnected(true); + m_reconnectTimer->stop(); + emit connected(); + }); + + connect(m_socket, &QTcpSocket::disconnected, this, [this]() { + setConnected(false); + emit disconnected(); + + // 如果设置了自动重连，则启动重连定时器 + if (m_autoReconnect) { + m_reconnectTimer->start(); + } + }); + + connect(m_socket, QOverload::of(&QTcpSocket::errorOccurred), + this, [this](QAbstractSocket::SocketError error) { + setConnected(false); + emit connectionError(m_socket->errorString()); + }); + + connect(m_socket, &QTcpSocket::readyRead, this, [this]() { + QByteArray data = m_socket->readAll(); + if (!data.isEmpty()) { + emit dataReceived(data); + } + }); + + connect(m_socket, &QTcpSocket::bytesWritten, this, &IVrTcpClient::dataSent); + + connect(m_reconnectTimer, &QTimer::timeout, this, [this]() { + if (m_autoReconnect && !m_serverAddress.isNull() && m_serverPort > 0) { + connectToServer(m_serverAddress, m_serverPort); + } + }); +} + +VrTcpClient::~VrTcpClient() +{ + disconnectFromServer(); +} + +bool VrTcpClient::connectToServer(const QString &host, quint16 port) +{ + m_serverAddress = QHostAddress(host); + m_serverPort = port; + + if (m_serverAddress.isNull()) { + // 如果是主机名，直接连接 + m_socket->connectToHost(host, port); + return true; + } else { + return connectToServer(m_serverAddress, port); + } +} + +bool VrTcpClient::connectToServer(const QHostAddress &address, quint16 port) +{ + m_serverAddress = address; + m_serverPort = port; + + if (isConnected()) { + disconnectFromServer(); + } + + m_socket->connectToHost(address, port); + return true; +} + +void VrTcpClient::disconnectFromServer() +{ + m_reconnectTimer->stop(); + if (m_socket->state() != QAbstractSocket::UnconnectedState) { + m_socket->disconnectFromHost(); + if (m_socket->state() == QAbstractSocket::ConnectedState) { + m_socket->waitForDisconnected(3000); + } + } + setConnected(false); +} + +qint64 VrTcpClient::sendData(const QByteArray &data) +{ + if (!isConnected()) { + return -1; + } + return m_socket->write(data); +} + +qint64 VrTcpClient::sendData(const char *data, qint64 size) +{ + if (!isConnected()) { + return -1; + } + return m_socket->write(data, size); +} + +bool VrTcpClient::isConnected() const +{ + return m_connected && m_socket->state() == QAbstractSocket::ConnectedState; +} + +QHostAddress VrTcpClient::serverAddress() const +{ + return m_serverAddress; +} + +quint16 VrTcpClient::serverPort() const +{ + return m_serverPort; +} + +void VrTcpClient::setAutoReconnect(bool autoReconnect) +{ + m_autoReconnect = autoReconnect; + if (!autoReconnect) { + m_reconnectTimer->stop(); + } +} + +bool VrTcpClient::autoReconnect() const +{ + return m_autoReconnect; +} + +void VrTcpClient::setReconnectInterval(int msec) +{ + m_reconnectTimer->setInterval(msec); +} + +int VrTcpClient::reconnectInterval() const +{ + return m_reconnectTimer->interval(); +} diff --git a/VrNets/tcpServer/Inc/IVrTcpServer.h b/VrNets/tcpServer/Inc/IVrTcpServer.h new file mode 100644 index 00000000..d09f877d --- /dev/null +++ b/VrNets/tcpServer/Inc/IVrTcpServer.h @@ -0,0 +1,38 @@ +#ifndef IVRTCPSERVER_H +#define IVRTCPSERVER_H + +#include +#include + +class IVrTcpServer : public QObject +{ + Q_OBJECT + +public: + explicit IVrTcpServer(QObject *parent = nullptr); + ~IVrTcpServer() override; + + // 服务器接口 + virtual bool startServer(quint16 port) = 0; + virtual void stopServer() = 0; + virtual bool isListening() const = 0; + virtual quint16 serverPort() const = 0; + + // 数据发送接口 + virtual qint64 broadcastData(const QByteArray &data) = 0; + virtual qint64 broadcastData(const char *data, qint64 size) = 0; + +signals: + // 服务器状态信号 + void serverStarted(); + void serverStopped(); + void serverError(const QString &error); + + // 客户端连接信号 + void clientConnected(const QString &clientId); + void clientDisconnected(const QString &clientId); + void dataReceived(const QString &clientId, const QByteArray &data); + void dataSent(const QString &clientId, qint64 bytes); +}; + +#endif // IVRTCPSERVER_H \ No newline at end of file diff --git a/VrNets/tcpServer/Inc/VrTcpServer.h b/VrNets/tcpServer/Inc/VrTcpServer.h new file mode 100644 index 00000000..a1452acc --- /dev/null +++ b/VrNets/tcpServer/Inc/VrTcpServer.h @@ -0,0 +1,48 @@ +#ifndef VRTCPSERVER_H +#define VRTCPSERVER_H + +#include "IVrTcpServer.h" +#include +#include +#include +#include + +class VrTcpServer : public IVrTcpServer +{ + Q_OBJECT + +public: + explicit VrTcpServer(QObject *parent = nullptr); + ~VrTcpServer() override; + + // 接口实现 + bool startServer(quint16 port) override; + void stopServer() override; + bool isListening() const override; + quint16 serverPort() const override; + + // 客户端管理 + QStringList getClientIds() const; + bool disconnectClient(const QString &clientId); + qint64 sendDataToClient(const QString &clientId, const QByteArray &data); + qint64 sendDataToClient(const QString &clientId, const char *data, qint64 size); + qint64 broadcastData(const QByteArray &data); + qint64 broadcastData(const char *data, qint64 size); + QHostAddress getClientAddress(const QString &clientId) const; + quint16 getClientPort(const QString &clientId) const; + +private slots: + void onNewConnection(); + void onClientDisconnected(); + void onClientReadyRead(); + void onClientBytesWritten(qint64 bytes); + void onServerError(QAbstractSocket::SocketError error); + +private: + QTcpServer *m_server; + QMap m_clients; + quint16 m_port; + QString generateClientId(QTcpSocket *socket); +}; + +#endif // VRTCPSERVER_H \ No newline at end of file diff --git a/VrNets/tcpServer/Src/VrTcpServer.cpp b/VrNets/tcpServer/Src/VrTcpServer.cpp new file mode 100644 index 00000000..a10f4f69 --- /dev/null +++ b/VrNets/tcpServer/Src/VrTcpServer.cpp @@ -0,0 +1,281 @@ +#include "VrTcpServer.h" +#include +#include +#include +#include + +VrTcpServer::VrTcpServer(QObject *parent) + : IVrTcpServer(parent) + , m_server(new QTcpServer(this)) + , m_port(0) +{ + // 连接服务器信号 + connect(m_server, &QTcpServer::newConnection, this, &VrTcpServer::onNewConnection); + connect(m_server, &QTcpServer::acceptError, this, [this](QAbstractSocket::SocketError error) { + emit serverError(m_server->errorString()); + }); +} + +VrTcpServer::~VrTcpServer() +{ + stopServer(); +} + +bool VrTcpServer::startServer(quint16 port) +{ + if (m_server->isListening()) { + stopServer(); + } + + m_port = port; + bool result = m_server->listen(QHostAddress::Any, port); + + if (result) { + emit serverStarted(); + } else { + emit serverError(m_server->errorString()); + } + + return result; +} + +void VrTcpServer::stopServer() +{ + // 断开所有客户端连接 + for (auto it = m_clients.begin(); it != m_clients.end(); ++it) { + it.value()->disconnectFromHost(); + } + m_clients.clear(); + + // 停止服务器监听 + if (m_server->isListening()) { + m_server->close(); + emit serverStopped(); + } + + m_port = 0; +} + +bool VrTcpServer::isListening() const +{ + return m_server->isListening(); +} + +quint16 VrTcpServer::serverPort() const +{ + return m_port; +} + +QStringList VrTcpServer::getClientIds() const +{ + return m_clients.keys(); +} + +bool VrTcpServer::disconnectClient(const QString &clientId) +{ + if (!m_clients.contains(clientId)) { + return false; + } + + QTcpSocket *socket = m_clients[clientId]; + socket->disconnectFromHost(); + return true; +} + +qint64 VrTcpServer::sendDataToClient(const QString &clientId, const QByteArray &data) +{ + if (!m_clients.contains(clientId)) { + return -1; + } + + QTcpSocket *socket = m_clients[clientId]; + if (socket->state() != QAbstractSocket::ConnectedState) { + return -1; + } + + return socket->write(data); +} + +qint64 VrTcpServer::sendDataToClient(const QString &clientId, const char *data, qint64 size) +{ + if (!m_clients.contains(clientId)) { + return -1; + } + + QTcpSocket *socket = m_clients[clientId]; + if (socket->state() != QAbstractSocket::ConnectedState) { + return -1; + } + + return socket->write(data, size); +} + +QHostAddress VrTcpServer::getClientAddress(const QString &clientId) const +{ + if (!m_clients.contains(clientId)) { + return QHostAddress(); + } + + return m_clients[clientId]->peerAddress(); +} + +quint16 VrTcpServer::getClientPort(const QString &clientId) const +{ + if (!m_clients.contains(clientId)) { + return 0; + } + + return m_clients[clientId]->peerPort(); +} + +void VrTcpServer::onNewConnection() +{ + while (m_server->hasPendingConnections()) { + QTcpSocket *socket = m_server->nextPendingConnection(); + + // 生成客户端ID + QString clientId = generateClientId(socket); + + // 存储客户端连接 + m_clients[clientId] = socket; + + // 连接客户端信号 + connect(socket, &QTcpSocket::disconnected, this, &VrTcpServer::onClientDisconnected); + connect(socket, &QTcpSocket::readyRead, this, &VrTcpServer::onClientReadyRead); + connect(socket, &QTcpSocket::bytesWritten, this, &VrTcpServer::onClientBytesWritten); + + // 发送客户端连接信号 + emit clientConnected(clientId); + } +} + +void VrTcpServer::onClientDisconnected() +{ + QTcpSocket *socket = qobject_cast(sender()); + if (!socket) { + return; + } + + // 查找断开连接的客户端ID + QString clientId; + for (auto it = m_clients.begin(); it != m_clients.end(); ++it) { + if (it.value() == socket) { + clientId = it.key(); + break; + } + } + + if (!clientId.isEmpty()) { + m_clients.remove(clientId); + emit clientDisconnected(clientId); + } + + socket->deleteLater(); +} + +void VrTcpServer::onClientReadyRead() +{ + QTcpSocket *socket = qobject_cast(sender()); + if (!socket) { + return; + } + + // 查找客户端ID + QString clientId; + for (auto it = m_clients.begin(); it != m_clients.end(); ++it) { + if (it.value() == socket) { + clientId = it.key(); + break; + } + } + + if (!clientId.isEmpty()) { + QByteArray data = socket->readAll(); + if (!data.isEmpty()) { + emit dataReceived(clientId, data); + } + } +} + +void VrTcpServer::onClientBytesWritten(qint64 bytes) +{ + QTcpSocket *socket = qobject_cast(sender()); + if (!socket) { + return; + } + + // 查找客户端ID + QString clientId; + for (auto it = m_clients.begin(); it != m_clients.end(); ++it) { + if (it.value() == socket) { + clientId = it.key(); + break; + } + } + + if (!clientId.isEmpty()) { + emit dataSent(clientId, bytes); + } +} + +void VrTcpServer::onServerError(QAbstractSocket::SocketError error) +{ + Q_UNUSED(error) + QTcpSocket *socket = qobject_cast(sender()); + if (!socket) { + return; + } + + // 查找客户端ID + QString clientId; + for (auto it = m_clients.begin(); it != m_clients.end(); ++it) { + if (it.value() == socket) { + clientId = it.key(); + break; + } + } + + if (!clientId.isEmpty()) { + emit serverError(QString("Client %1 error: %2").arg(clientId, socket->errorString())); + } +} + +QString VrTcpServer::generateClientId(QTcpSocket *socket) +{ + // 使用UUID生成唯一的客户端ID + return QUuid::createUuid().toString(); +} + +qint64 VrTcpServer::broadcastData(const QByteArray &data) +{ + qint64 totalBytesWritten = 0; + + for (auto it = m_clients.begin(); it != m_clients.end(); ++it) { + QTcpSocket *socket = it.value(); + if (socket->state() == QAbstractSocket::ConnectedState) { + qint64 bytesWritten = socket->write(data); + if (bytesWritten > 0) { + totalBytesWritten += bytesWritten; + } + } + } + + return totalBytesWritten; +} + +qint64 VrTcpServer::broadcastData(const char *data, qint64 size) +{ + qint64 totalBytesWritten = 0; + + for (auto it = m_clients.begin(); it != m_clients.end(); ++it) { + QTcpSocket *socket = it.value(); + if (socket->state() == QAbstractSocket::ConnectedState) { + qint64 bytesWritten = socket->write(data, size); + if (bytesWritten > 0) { + totalBytesWritten += bytesWritten; + } + } + } + + return totalBytesWritten; +} diff --git a/VrNets/tcpServer/Test/test_tcp_server.cpp b/VrNets/tcpServer/Test/test_tcp_server.cpp new file mode 100644 index 00000000..a7d586ee --- /dev/null +++ b/VrNets/tcpServer/Test/test_tcp_server.cpp @@ -0,0 +1,60 @@ +#include "../Inc/VrTcpServer.h" +#include +#include +#include + +int main(int argc, char *argv[]) +{ + QCoreApplication app(argc, argv); + + VrTcpServer server; + + // 连接信号槽 + QObject::connect(&server, &IVrTcpServer::serverStarted, []() { + qDebug() << "Server started!"; + }); + + QObject::connect(&server, &IVrTcpServer::serverStopped, []() { + qDebug() << "Server stopped!"; + }); + + QObject::connect(&server, &IVrTcpServer::serverError, [](const QString &error) { + qDebug() << "Server error:" << error; + }); + + QObject::connect(&server, &IVrTcpServer::clientConnected, [&server](const QString &clientId) { + qDebug() << "Client connected:" << clientId; + + // 发送欢迎消息给新连接的客户端 + QByteArray welcomeMessage = "Welcome to the server!"; + server.sendDataToClient(clientId, welcomeMessage); + }); + + QObject::connect(&server, &IVrTcpServer::clientDisconnected, [](const QString &clientId) { + qDebug() << "Client disconnected:" << clientId; + }); + + QObject::connect(&server, &IVrTcpServer::dataReceived, [&server](const QString &clientId, const QByteArray &data) { + qDebug() << "Received data from" << clientId << ":" << data.toHex(); + + // 将收到的数据广播给所有客户端 + QByteArray message = "Broadcast from " + clientId.toUtf8() + ": " + data; + server.broadcastData(message); + }); + + // 启动服务器 + if (!server.startServer(12345)) { + qDebug() << "Failed to start server"; + return -1; + } + + qDebug() << "Server listening on port 12345"; + + // 10秒后停止服务器并退出 + QTimer::singleShot(10000, [&server, &app]() { + server.stopServer(); + app.quit(); + }); + + return app.exec(); +} \ No newline at end of file