1. 新建称重程序。

CMakeLists.txt

@@ -41,5 +41,18 @@ target_link_libraries(Demo fastcdr fastdds
         System_lib
         )
 
+# Weigh Application.
+add_executable(Weigh
+                weigh/main.cxx
+                weigh/Publisher.cxx
+                weigh/Subscriber.cxx
+                weigh/MsgHandler.cxx
+                weigh/WeightStabilityDetector.cxx
+                common/SerialMsgHandler.cxx
+              )
+target_include_directories(Weigh PRIVATE weigh)
+target_link_libraries(Weigh fastcdr fastdds
+        System_lib
+        )
 
 

common/SerialMsgHandler.cxx

@@ -0,0 +1,118 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <errno.h>
+
+#include "SerialMsgHandler.hpp"
+
+SerialMsgHandler::SerialMsgHandler() {}
+
+SerialMsgHandler::~SerialMsgHandler()
+{
+    ClosePort();
+}
+
+// 打开串口
+bool SerialMsgHandler::OpenPort(const std::string &port, const std::string &baudrate)
+{
+    std::string tty = "/dev/" + port;
+    int baund = B9600;
+
+    ClosePort();
+
+    fd = open(tty.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
+    if (fd == -1)
+    {
+        perror("打开串口失败");
+        return false;
+    }
+
+    // 恢复串口为阻塞状态
+    if (fcntl(fd, F_SETFL, 0) < 0)
+    {
+        perror("设置阻塞模式失败");
+        close(fd);
+        fd = -1;
+        return false;
+    }
+
+    // 检查是否是终端设备
+    if (!isatty(fd))
+    {
+        fprintf(stderr, "%s 不是终端设备\n", tty.c_str());
+        close(fd);
+        fd = -1;
+        return false;
+    }
+
+    if (baudrate == "115200")
+    {
+        baund = B115200;
+    }
+    else if (baudrate == "9600")
+    {
+        baund = B9600;
+    }
+    else
+    {
+        baund = B9600;
+    }
+    
+    return setPortAttributes(baund);
+}
+
+// 设置串口参数
+bool SerialMsgHandler::setPortAttributes(int baudrate)
+{
+    struct termios options;
+
+    // 获取当前串口设置
+    if (tcgetattr(fd, &options) != 0)
+    {
+        perror("获取串口属性失败");
+        return false;
+    }
+
+    // 设置波特率
+    cfsetispeed(&options, baudrate);
+    cfsetospeed(&options, baudrate);
+
+    // 设置数据位：8位数据位
+    options.c_cflag &= ~CSIZE;
+    options.c_cflag |= CS8;
+
+    // 设置校验位：无校验
+    options.c_cflag &= ~PARENB;
+    options.c_iflag &= ~(INPCK | INLCR | ICRNL | IGNCR);
+
+    // 设置停止位：1位停止位
+    options.c_cflag &= ~CSTOPB;
+
+    // 设置流控制：无流控制
+    options.c_cflag &= ~CRTSCTS;
+
+    // 设置原始输入模式
+    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+
+    // 设置原始输出模式
+    options.c_oflag &= ~OPOST;
+
+    // 设置最小字符数和等待时间
+    options.c_cc[VMIN] = 0;   // 读取的最小字符数
+    options.c_cc[VTIME] = 10; // 读取超时时间（单位：0.1秒）
+
+    // 清空输入输出缓冲区
+    tcflush(fd, TCIOFLUSH);
+
+    // 应用设置
+    if (tcsetattr(fd, TCSANOW, &options) != 0)
+    {
+        perror("设置串口属性失败");
+        return false;
+    }
+
+    return true;
+}

common/SerialMsgHandler.hpp

@@ -0,0 +1,24 @@
+#ifndef _SERIALMSGHANDLER_HPP_
+#define _SERIALMSGHANDLER_HPP_
+
+#include <string>
+#include <cstdint>
+
+#include "MsgHandler.hpp"
+
+class SerialMsgHandler : public MsgHandler
+{
+public:
+    SerialMsgHandler();
+    ~SerialMsgHandler();
+
+    // 打开串口
+    bool OpenPort(const std::string& port, const std::string& baudrate);
+    
+    // 设置串口参数
+    bool setPortAttributes(int baudrate);
+
+private:
+};
+
+#endif // _SERIALMSGHANDLER_HPP_

weigh/MsgHandler.cxx

@@ -0,0 +1,528 @@
+#include <iostream>
+#include <iomanip>
+#include <algorithm>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include "MsgHandler.hpp"
+
+constexpr uint8_t STX = 0x02;
+constexpr uint8_t ETX = 0x03;
+constexpr uint8_t CR = 0x0D;
+constexpr uint8_t LF = 0x0A;
+
+MsgHandler::MsgHandler() : fd(-1) {}
+
+bool MsgHandler::OpenPort(const std::string& port, const std::string& baudrate)
+{
+    return true;
+}
+
+void MsgHandler::ClosePort()
+{
+    if (fd != -1)
+    {
+        close(fd);
+        fd = -1;
+    }
+}
+
+bool MsgHandler::SetDevice(const std::string& device)
+{
+    this->device = device;
+
+    return true;
+}
+
+int MsgHandler::SendDeviceMsg(const std::vector<uint8_t>& data)
+{
+    std::cout << "write to " << this->device << std::endl;
+    for (int num : data) {
+        std::cout << std::hex << std::setw(2) << std::setfill('0')
+                  << num << " ";
+    }
+    std::cout << std::endl;
+
+    int bytesWritten =  SendMsg(data);
+
+    return bytesWritten;
+}
+
+int MsgHandler::RecvDeviceMsg(std::vector<uint8_t>& data, int timeoutMs)
+{
+    int bytesRead = RecvMsg(data, timeoutMs);
+
+    if (bytesRead > 0)
+    {
+        std::cout << "read from " << this->device << std::endl;
+        for (int num : data)
+        {
+            std::cout << std::hex << std::setw(2) << std::setfill('0')
+                      << num << " ";
+        }
+        std::cout << std::endl;
+    }
+
+    return bytesRead;
+}
+
+int MsgHandler::SendMsg(const std::vector<uint8_t>& data)
+{
+    if (fd == -1)
+    {
+        perror("设备未打开");
+        return -1;
+    }
+
+    int bytesWritten = write(fd, data.data(), data.size());
+    if (bytesWritten < 0)
+    {
+        perror("发送数据失败");
+    }
+
+    return bytesWritten;
+}
+
+int MsgHandler::RecvMsg(std::vector<uint8_t>& data, int timeoutMs)
+{
+    if (fd == -1)
+    {
+        perror("设备未打开\n");
+        return -1;
+    }
+
+    // 使用select实现超时
+    fd_set readfds;
+    struct timeval tv;
+
+    FD_ZERO(&readfds);
+    FD_SET(fd, &readfds);
+
+    tv.tv_sec = timeoutMs / 1000;
+    tv.tv_usec = (timeoutMs % 1000) * 1000;
+
+    int ret = select(fd + 1, &readfds, NULL, NULL, &tv);
+    if (ret == -1)
+    {
+        perror("select错误");
+        return -1;
+    }
+    else if (ret == 0)
+    {
+        //无数据可读
+        return 0;
+    }
+
+    // 有数据可读
+    uint8_t bytes;
+    if (ioctl(fd, FIONREAD, &bytes) < 0)
+    {
+        perror("ioctl FIONREAD失败");
+        return -1;
+    }
+    data.resize(bytes);
+    int bytesRead = read(fd, data.data(), bytes);
+    if (bytesRead < 0)
+    {
+        perror("读取数据失败");
+        return -1;
+    }
+
+    return bytesRead;
+}
+
+void MsgHandler::HandleDdsMsg(const std::map<std::string, std::string>& msg)
+{
+    return;
+}
+
+bool MsgHandler::get_frame(std::vector<uint8_t>& frame, uint8_t frame_size, uint8_t start, uint8_t end)
+{
+    bool frame_found = false;
+
+    do
+    {
+        auto start_it = std::find(m_WeightData.begin(), m_WeightData.end(), start);
+
+        if (start_it == m_WeightData.end())
+        {
+            // 没有找到起始字节，清空缓冲区
+            m_WeightData.clear();
+            break;
+        }
+
+        // 移除起始字节前的无用数据
+        if (start_it != m_WeightData.begin())
+        {
+            m_WeightData.erase(m_WeightData.begin(), start_it);
+        }
+
+        // 检查是否有足够的数据（至少一个完整帧）
+        if (m_WeightData.size() >= 12)
+        {
+            // 检查结束字节
+            if (m_WeightData[frame_size - 1] == end)
+            {
+                // 提取完整帧
+                frame.clear();
+                frame.insert(frame.begin(), m_WeightData.begin(), m_WeightData.begin() + frame_size);
+
+                // 从缓冲区移除已处理的数据
+                m_WeightData.erase(m_WeightData.begin(), m_WeightData.begin() + frame_size);
+                frame_found = true;
+                break;
+            }
+            else
+            {
+                // 结束字节不匹配，可能是数据错误或粘包
+                // 查找下一个起始字节
+                auto next_start = std::find(m_WeightData.begin() + 1, m_WeightData.end(), start);
+                if (next_start != m_WeightData.end())
+                {
+                    // 移除到下一个起始字节之前的数据
+                    m_WeightData.erase(m_WeightData.begin(), next_start);
+                }
+                else
+                {
+                    // 没有找到下一个起始字节，清空缓冲区
+                    m_WeightData.clear();
+                    break;
+                }
+            }
+        }
+        else
+        {
+            // 数据不完整，等待更多数据
+            break;
+        }
+    } while (true);
+
+    return frame_found;
+}
+
+bool MsgHandler::get_weight_1(std::string& weight, std::vector<uint8_t>& frame)
+{
+    // 1. 解析符号
+    char sign = '+';
+    if (frame[1] == 0x2D)
+    {
+        sign = '-';
+    }
+    else if (frame[1] != 0x2B)
+    {
+        return false;
+    }
+
+    // 2. 解析称量数据（6位数字）
+    std::string weightDigits;
+    for (int i = 2; i <= 7; i++)
+    {
+        if (frame[i] < 0x30 || frame[i] > 0x39)
+        {
+            return false;
+        }
+        weightDigits += static_cast<char>(frame[i]);
+    }
+
+    // 3. 解析小数点位置
+    if (frame[8] < 0x30 || frame[8] > 0x34)
+    {
+        return false;
+    }
+    int decimalPos = frame[8] - 0x30;
+
+    // 4. 构建结果字符串
+    std::string weightStr;
+
+    if (decimalPos == 0)
+    {
+        // 情况1：没有小数点，去除所有前导零，但保留至少一位数字
+        std::string integerPart = weightDigits;
+
+        // 去除前导零
+        size_t firstNonZero = 0;
+        while (firstNonZero < integerPart.length() && integerPart[firstNonZero] == '0')
+        {
+            firstNonZero++;
+        }
+
+        if (firstNonZero == integerPart.length())
+        {
+            // 如果全部是零，保留一个零
+            weightStr = "0";
+        }
+        else
+        {
+            weightStr = integerPart.substr(firstNonZero);
+        }
+    }
+    else
+    {
+        // 情况2：有小数点
+        if (decimalPos > weightDigits.length())
+        {
+            return false;
+        }
+
+        // 从右向左插入小数点
+        int insertPos = weightDigits.length() - decimalPos;
+
+        // 分离整数部分和小数部分
+        std::string integerPart = weightDigits.substr(0, insertPos);
+        std::string decimalPart = weightDigits.substr(insertPos, decimalPos);
+
+        // 处理整数部分：去除前导零，但如果全为零则保留一个0
+        bool allZero = true;
+        for (char c : integerPart)
+        {
+            if (c != '0')
+            {
+                allZero = false;
+                break;
+            }
+        }
+
+        if (allZero && !integerPart.empty())
+        {
+            integerPart = "0";
+        }
+        else
+        {
+            // 去除整数部分的前导零
+            size_t firstNonZero = 0;
+            while (firstNonZero < integerPart.length() && integerPart[firstNonZero] == '0')
+            {
+                firstNonZero++;
+            }
+
+            if (firstNonZero > 0 && firstNonZero < integerPart.length())
+            {
+                integerPart = integerPart.substr(firstNonZero);
+            }
+        }
+
+        // 构建最终字符串，小数部分保留完整位数
+        weightStr = integerPart + "." + decimalPart;
+    }
+
+    // 5. 添加符号
+    weight.clear();
+    weight += sign;
+    weight += weightStr;
+
+    return true;
+}
+
+bool MsgHandler::get_weight_1(float& weight, std::vector<uint8_t>& frame)
+{
+    float raw_value;
+    float divisors[] = {1.0f, 10.0f, 100.0f, 1000.0f, 10000.0f};
+
+    std::string weightDigits;
+    for (int i = 2; i <= 7; i++)
+    {
+        if (frame[i] < 0x30 || frame[i] > 0x39)
+        {
+            return false;
+        }
+        weightDigits += static_cast<char>(frame[i]);
+    }
+
+    int decimalPos = frame[8] - 0x30;
+    if (decimalPos < 0)
+    {
+        decimalPos = 0;
+    }
+
+    raw_value = std::stof(weightDigits);
+    weight = raw_value / divisors[decimalPos];
+
+    if (frame[1] == 0x2D)
+    {
+        weight = 0 - weight;
+    }
+
+    return true;
+}
+
+bool MsgHandler::get_weight_2(std::string& weight, std::vector<uint8_t>& frame)
+{
+    // 1. 解析符号
+    char sign = '+';
+    if (frame[2] & 0x02)
+    {
+        sign = '-';
+    }
+
+    // 2. 解析称量数据（6位数字）
+    std::string weightDigits;
+    for (int i = 4; i <= 9; i++)
+    {
+        if ((frame[i] >= 0x30) && (frame[i] <= 0x39))
+        {
+            weightDigits += static_cast<char>(frame[i]);
+        }
+        else if (frame[i] == 0x20)
+        {
+            weightDigits += '0';
+        }
+    }
+
+    // 3. 解析小数点位置
+    int decimalPos = frame[1]&0x07 - 2;
+    if (decimalPos < 0)
+    {
+        decimalPos = 0;
+    }
+
+    // 4. 构建结果字符串
+    std::string weightStr;
+
+    if (decimalPos == 0)
+    {
+        // 情况1：没有小数点，去除所有前导零，但保留至少一位数字
+        std::string integerPart = weightDigits;
+
+        // 去除前导零
+        size_t firstNonZero = 0;
+        while (firstNonZero < integerPart.length() && integerPart[firstNonZero] == '0')
+        {
+            firstNonZero++;
+        }
+
+        if (firstNonZero == integerPart.length())
+        {
+            // 如果全部是零，保留一个零
+            weightStr = "0";
+        }
+        else
+        {
+            weightStr = integerPart.substr(firstNonZero);
+        }
+    }
+    else
+    {
+        // 情况2：有小数点
+        if (decimalPos > weightDigits.length())
+        {
+            return false;
+        }
+
+        // 从右向左插入小数点
+        int insertPos = weightDigits.length() - decimalPos;
+
+        // 分离整数部分和小数部分
+        std::string integerPart = weightDigits.substr(0, insertPos);
+        std::string decimalPart = weightDigits.substr(insertPos, decimalPos);
+
+        // 处理整数部分：去除前导零，但如果全为零则保留一个0
+        bool allZero = true;
+        for (char c : integerPart)
+        {
+            if (c != '0')
+            {
+                allZero = false;
+                break;
+            }
+        }
+
+        if (allZero && !integerPart.empty())
+        {
+            integerPart = "0";
+        }
+        else
+        {
+            // 去除整数部分的前导零
+            size_t firstNonZero = 0;
+            while (firstNonZero < integerPart.length() && integerPart[firstNonZero] == '0')
+            {
+                firstNonZero++;
+            }
+
+            if (firstNonZero > 0 && firstNonZero < integerPart.length())
+            {
+                integerPart = integerPart.substr(firstNonZero);
+            }
+        }
+
+        // 构建最终字符串，小数部分保留完整位数
+        weightStr = integerPart + "." + decimalPart;
+    }
+
+    // 5. 添加符号
+    weight.clear();
+    weight += sign;
+    weight += weightStr;
+
+    std::cout << weight << std::endl;
+
+    return true;
+}
+
+bool MsgHandler::get_weight_2(float& weight, std::vector<uint8_t>& frame)
+{
+    float raw_value;
+    float divisors[] = {1.0f, 10.0f, 100.0f, 1000.0f, 10000.0f, 100000.0f};
+
+    std::string weightDigits;
+    for (int i = 4; i <= 9; i++)
+    {
+        if ((frame[i] >= 0x30) && (frame[i] <= 0x39))
+        {
+            weightDigits += static_cast<char>(frame[i]);
+        }
+        else if (frame[i] == 0x20)
+        {
+            weightDigits += '0';
+        }
+    }
+
+    int decimalPos = frame[1]&0x07 - 2;
+    if (decimalPos < 0)
+    {
+        decimalPos = 0;
+    }
+
+    raw_value = std::stof(weightDigits);
+    weight = raw_value / divisors[decimalPos];
+
+    if (frame[2] & 0x02)
+    {
+        weight = 0 - weight;
+    }
+
+    return true;
+}
+
+template<typename T>
+bool MsgHandler::HandleDeviceMsg(T& msg)
+{
+    std::vector<uint8_t> m_Data;
+    std::vector<uint8_t> frame;
+
+    if (RecvDeviceMsg(m_Data, 100) > 0)
+    {
+        m_WeightData.insert(m_WeightData.end(), m_Data.begin(), m_Data.end());
+    }
+
+    if (m_WeightData.empty() != true)
+    {
+        if (device == "Keli")
+        {
+            if (get_frame(frame, 12, STX, ETX) == true)
+            {
+                return get_weight_1(msg, frame);
+            }
+        }
+        else if (device == "Toledo")
+        {
+            if (get_frame(frame, 17, STX, CR) == true)
+            {
+                return get_weight_2(msg, frame);
+            }
+        }
+    }
+
+    return false;
+}
+template bool MsgHandler::HandleDeviceMsg<std::string>(std::string&);
+template bool MsgHandler::HandleDeviceMsg<float>(float&);

weigh/MsgHandler.hpp

@@ -0,0 +1,41 @@
+#ifndef _MSGHANDLER_HPP_
+#define _MSGHANDLER_HPP_
+
+#include <queue>
+#include <mutex>
+#include <vector>
+#include <map>
+
+class MsgHandler {
+public:
+    int fd;
+    std::string device;
+    std::vector<uint8_t> m_WeightData;
+
+    MsgHandler();
+    ~MsgHandler() = default;
+
+    void HandleDdsMsg(const std::map<std::string, std::string>& msg);
+    template<typename T>
+    bool HandleDeviceMsg(T& msg);
+    bool SetDevice(const std::string& device);
+
+    virtual bool OpenPort(const std::string& port, const std::string& baudrate);
+    virtual int SendDeviceMsg(const std::vector<uint8_t>& data);
+    virtual int RecvDeviceMsg(std::vector<uint8_t>& data, int timeoutMs);
+    int SendMsg(const std::vector<uint8_t>& data);
+    int RecvMsg(std::vector<uint8_t>& data, int timeoutMs);
+    bool isOpen() const {
+        return fd != -1;
+    }
+    void ClosePort();
+
+private:
+    bool get_frame(std::vector<uint8_t>& frame, uint8_t frame_size, uint8_t start, uint8_t end);
+    bool get_weight_1(std::string& weight, std::vector<uint8_t>& frame);
+    bool get_weight_1(float& weight, std::vector<uint8_t>& frame);
+    bool get_weight_2(std::string& weight, std::vector<uint8_t>& frame);
+    bool get_weight_2(float& weight, std::vector<uint8_t>& frame);
+};
+
+#endif

weigh/Publisher.cxx

@@ -0,0 +1,297 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file Publisher.cxx
+ * This file contains the implementation of the publisher functions.
+ *
+ * This file was generated by the tool fastddsgen.
+ */
+
+#include "Publisher.hpp"
+
+#include <condition_variable>
+#include <csignal>
+#include <stdexcept>
+#include <thread>
+
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/log/Log.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+
+#include "SystemPubSubTypes.hpp"
+
+#include "msg.hpp"
+#include "MsgHandler.hpp"
+#include "WeightStabilityDetector.hpp"
+
+using namespace eprosima::fastdds::dds;
+
+PublisherApp::PublisherApp(
+        const int& domain_id)
+    : factory_(nullptr)
+    , participant_(nullptr)
+    , publisher_(nullptr)
+    , topic_(nullptr)
+    , writer_(nullptr)
+    , type_(new WeighRspPubSubType())
+    , matched_(0)
+    , samples_sent_(0)
+    , stop_(false)
+{
+    //
+
+    // Create the participant
+    DomainParticipantQos pqos = PARTICIPANT_QOS_DEFAULT;
+    pqos.name("Weigh_pub_participant");
+    pqos.wire_protocol().builtin.discovery_config.leaseDuration = Duration_t(60, 0);
+    pqos.wire_protocol().builtin.discovery_config.leaseDuration_announcementperiod = Duration_t(30, 0);
+    factory_ = DomainParticipantFactory::get_shared_instance();
+    participant_ = factory_->create_participant(domain_id, pqos, nullptr, StatusMask::none());
+    if (participant_ == nullptr)
+    {
+        throw std::runtime_error("WeighRsp Participant initialization failed");
+    }
+
+    // Register the type
+    type_.register_type(participant_);
+
+    // Create the publisher
+    PublisherQos pub_qos = PUBLISHER_QOS_DEFAULT;
+    participant_->get_default_publisher_qos(pub_qos);
+    publisher_ = participant_->create_publisher(pub_qos, nullptr, StatusMask::none());
+    if (publisher_ == nullptr)
+    {
+        throw std::runtime_error("WeighRsp Publisher initialization failed");
+    }
+
+    // Create the topic
+    TopicQos topic_qos = TOPIC_QOS_DEFAULT;
+    participant_->get_default_topic_qos(topic_qos);
+    topic_ = participant_->create_topic("WeighRspTopic", type_.get_type_name(), topic_qos);
+    if (topic_ == nullptr)
+    {
+        throw std::runtime_error("WeighRsp Topic initialization failed");
+    }
+
+    // Create the data writer
+    DataWriterQos writer_qos = DATAWRITER_QOS_DEFAULT;
+    publisher_->get_default_datawriter_qos(writer_qos);
+    writer_qos.reliability().kind = ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
+    writer_qos.durability().kind = DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS;
+    writer_qos.history().kind = HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
+    writer_ = publisher_->create_datawriter(topic_, writer_qos, this, StatusMask::all());
+    if (writer_ == nullptr)
+    {
+        throw std::runtime_error("WeighRsp DataWriter initialization failed");
+    }
+}
+
+PublisherApp::~PublisherApp()
+{
+    if (nullptr != participant_)
+    {
+        // Delete DDS entities contained within the DomainParticipant
+        participant_->delete_contained_entities();
+
+        // Delete DomainParticipant
+        factory_->delete_participant(participant_);
+    }
+}
+
+void PublisherApp::on_publication_matched(
+        DataWriter* writer,
+        const PublicationMatchedStatus& info)
+{
+    if (info.current_count_change == 1)
+    {
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            matched_ = info.current_count;
+        }
+        std::cout << writer->get_topic()->get_name() << " Publisher matched." << std::endl;
+        cv_.notify_one();
+    }
+    else if (info.current_count_change == -1)
+    {
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            matched_ = info.current_count;
+        }
+        std::cout << writer->get_topic()->get_name() << " Publisher unmatched." << std::endl;
+    }
+    else
+    {
+        std::cout << info.current_count_change
+                  << " is not a valid value for PublicationMatchedStatus current count change" << std::endl;
+    }
+}
+
+void PublisherApp::run(std::shared_ptr<MsgHandler> handler)
+{
+    uint8_t send_stable = 0;
+
+    // 配置检测器 - 使用更合理的参数
+    WeightStabilityDetector::Config config;
+    config.jitter_threshold = 30.0f;           // 30kg抖动阈值
+    config.min_weight_threshold = 5000.0f;     // 5吨开始检测
+    config.empty_car_threshold = 300.0f;       // 300kg为空车
+    config.fast_drop_threshold = 1000.0f;      // 快速下降1吨认为车辆离开
+    config.required_stable_count = 6;          // 6次稳定即可
+    config.min_jitter_count = 2;               // 至少2次非增长
+    config.timeout_extra = 3;                  // 超时额外次数
+    config.window_size = 8;                    // 窗口大小8
+    config.max_std_dev = 30.0f;                // 最大标准差30kg
+    config.enable_debug_log = true;
+    config.reset_window_on_vehicle_on = true;
+    config.leave_reset_delay_ms = 500;         // 0.5秒后重置
+    
+    // 创建检测器
+    WeightStabilityDetector detector(config);
+    
+    // 设置状态回调
+    detector.setStateCallback([](WeightStabilityDetector::VehicleState state,
+                                float weight,
+                                [[maybe_unused]] void* user_data) {
+        const char* state_names[] = {
+            "NO_VEHICLE",
+            "VEHICLE_ON",
+            "VEHICLE_STABLE",
+            "VEHICLE_LEAVING"
+        };
+        
+        std::cout << "\n=== STATE CHANGE: " << state_names[static_cast<int>(state)]
+                  << ", Weight: " << weight << " kg ===" << std::endl;
+    });
+    
+    // 设置重量回调
+    detector.setWeightCallback([](float weight, bool is_stable, [[maybe_unused]] void* user_data) {
+        if (is_stable) {
+            std::cout << "Weight stabilized: " << weight << " kg" << std::endl;
+        }
+    });
+    
+    while (!is_stopped())
+    {
+        std::unique_lock<std::mutex> lock(MsgData::queue_cv_mtx_, std::try_to_lock);
+        if (lock.owns_lock())
+        {
+            if(!MsgData::WeighReq_queue_.empty())
+            {
+                MsgData::WeighReq_queue_.pop();
+                lock.unlock();
+            }
+            else
+            {
+                lock.unlock();
+            }
+        }
+
+        if (handler->isOpen() == true)
+        {
+            float weight;
+            if (handler->HandleDeviceMsg(weight) == true)
+            {
+                WeighRsp rsp;
+                rsp.instantaneous_weight() = weight;
+
+                bool success = detector.processWeight(weight);
+                if (!success)
+                {
+                    std::cout << "Failed to process weight!" << std::endl;
+                    continue;
+                }
+
+                // 获取统计信息
+                auto stats = detector.getStatistics();
+                auto state = detector.getState();
+
+                const char *state_names[] = {
+                    "NO_VEHICLE", "VEHICLE_ON", "VEHICLE_STABLE", "VEHICLE_LEAVING"};
+
+                std::cout << "State: " << state_names[static_cast<int>(state)]
+                          << ", Stable: " << (detector.isStable() ? "Yes" : "No")
+                          << ", HasVehicle: " << (detector.hasVehicle() ? "Yes" : "No") << std::endl;
+
+                if (stats.sample_count > 0)
+                {
+                    std::cout << "Stats: Mean=" << std::setprecision(2) << stats.mean
+                              << "kg, StdDev=" << stats.std_dev
+                              << "kg, Range=" << (stats.max - stats.min)
+                              << "kg, Samples=" << stats.sample_count << std::endl;
+                }
+
+                // 如果稳定，显示稳定重量
+                if (detector.isStable())
+                {
+                    if(send_stable == 0)
+                    {
+                        rsp.stable_weight() = rsp.instantaneous_weight();
+                        send_stable = 1;
+                    }
+                    std::cout << "STABLE WEIGHT: " << detector.getStableWeight() << " kg" << std::endl;
+                }
+                else
+                {
+                    rsp.stable_weight() = 0;
+                    send_stable = 0;
+                }
+
+                writer_->write(&rsp);
+            }
+        }
+
+        // Wait for period or stop event
+        std::unique_lock<std::mutex> period_lock(mutex_);
+        cv_.wait_for(period_lock, std::chrono::milliseconds(period_ms_), [this]()
+                {
+                    return is_stopped();
+                });
+    }
+}
+
+
+bool PublisherApp::publish()
+{
+    bool ret = false;
+    // Wait for the data endpoints discovery
+    std::unique_lock<std::mutex> matched_lock(mutex_);
+    cv_.wait(matched_lock, [&]()
+            {
+                // at least one has been discovered
+                return ((matched_ > 0) || is_stopped());
+            });
+
+    if (!is_stopped())
+    {
+        /* Initialize your structure here */
+        WeighRsp sample_;
+        ret = (RETCODE_OK == writer_->write(&sample_));
+    }
+    return ret;
+}
+
+bool PublisherApp::is_stopped()
+{
+    return stop_.load();
+}
+
+void PublisherApp::stop()
+{
+    stop_.store(true);
+    cv_.notify_one();
+}

weigh/Publisher.hpp

@@ -0,0 +1,76 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file PublisherApp.hpp
+ * This header file contains the declaration of the publisher functions.
+ *
+ * This file was generated by the tool fastddsgen.
+ */
+
+#ifndef FAST_DDS_GENERATED__PUBLISHERAPP_HPP
+#define FAST_DDS_GENERATED__PUBLISHERAPP_HPP
+
+#include <condition_variable>
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include "MsgHandler.hpp"
+
+class PublisherApp : public eprosima::fastdds::dds::DataWriterListener
+{
+public:
+
+    PublisherApp(
+            const int& domain_id);
+
+    ~PublisherApp();
+
+    //! Publisher matched method
+    void on_publication_matched(
+            eprosima::fastdds::dds::DataWriter* writer,
+            const eprosima::fastdds::dds::PublicationMatchedStatus& info) override;
+
+    //! Run publisher
+    void run(std::shared_ptr<MsgHandler> handler);
+
+    //! Trigger the end of execution
+    void stop();
+
+private:
+
+    //! Return the current state of execution
+    bool is_stopped();
+
+    //! Publish a sample
+    bool publish();
+
+    std::shared_ptr<eprosima::fastdds::dds::DomainParticipantFactory> factory_;
+    eprosima::fastdds::dds::DomainParticipant* participant_;
+    eprosima::fastdds::dds::Publisher* publisher_;
+    eprosima::fastdds::dds::Topic* topic_;
+    eprosima::fastdds::dds::DataWriter* writer_;
+    eprosima::fastdds::dds::TypeSupport type_;
+    std::condition_variable cv_;
+    int32_t matched_;
+    std::mutex mutex_;
+    const uint32_t period_ms_ = 100; // in ms
+    uint16_t samples_sent_;
+    std::atomic<bool> stop_;
+};
+
+#endif // FAST_DDS_GENERATED__PUBLISHERAPP_HPP

weigh/Subscriber.cxx

@@ -0,0 +1,171 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file Subscriber.cxx
+ * This file contains the implementation of the subscriber functions.
+ *
+ * This file was generated by the tool fastddsgen.
+ */
+
+#include "Subscriber.hpp"
+
+#include <condition_variable>
+#include <stdexcept>
+
+#include <fastdds/dds/core/status/SubscriptionMatchedStatus.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/subscriber/DataReader.hpp>
+#include <fastdds/dds/subscriber/qos/DataReaderQos.hpp>
+#include <fastdds/dds/subscriber/qos/SubscriberQos.hpp>
+#include <fastdds/dds/subscriber/SampleInfo.hpp>
+#include <fastdds/dds/subscriber/Subscriber.hpp>
+
+#include "SystemPubSubTypes.hpp"
+
+#include "msg.hpp"
+
+using namespace eprosima::fastdds::dds;
+
+SubscriberApp::SubscriberApp(
+        const int& domain_id)
+    : factory_(nullptr)
+    , participant_(nullptr)
+    , subscriber_(nullptr)
+    , topic_(nullptr)
+    , reader_(nullptr)
+    , type_(new WeighReqPubSubType())
+    , samples_received_(0)
+    , stop_(false)
+{
+    // Create the participant
+    DomainParticipantQos pqos = PARTICIPANT_QOS_DEFAULT;
+    pqos.name("Weigh_sub_participant");
+    pqos.wire_protocol().builtin.discovery_config.leaseDuration = Duration_t(60, 0);
+    pqos.wire_protocol().builtin.discovery_config.leaseDuration_announcementperiod = Duration_t(30, 0);
+    factory_ = DomainParticipantFactory::get_shared_instance();
+    participant_ = factory_->create_participant(domain_id, pqos, nullptr, StatusMask::none());
+    if (participant_ == nullptr)
+    {
+        throw std::runtime_error("WeighReq Participant initialization failed");
+    }
+
+    // Register the type
+    type_.register_type(participant_);
+
+    // Create the subscriber
+    SubscriberQos sub_qos = SUBSCRIBER_QOS_DEFAULT;
+    participant_->get_default_subscriber_qos(sub_qos);
+    subscriber_ = participant_->create_subscriber(sub_qos, nullptr, StatusMask::none());
+    if (subscriber_ == nullptr)
+    {
+        throw std::runtime_error("WeighReq Subscriber initialization failed");
+    }
+
+    // Create the topic
+    TopicQos topic_qos = TOPIC_QOS_DEFAULT;
+    participant_->get_default_topic_qos(topic_qos);
+    topic_ = participant_->create_topic("WeighReqTopic", type_.get_type_name(), topic_qos);
+    if (topic_ == nullptr)
+    {
+        throw std::runtime_error("WeighReq Topic initialization failed");
+    }
+
+    // Create the reader
+    DataReaderQos reader_qos = DATAREADER_QOS_DEFAULT;
+    subscriber_->get_default_datareader_qos(reader_qos);
+    reader_qos.reliability().kind = ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
+    reader_qos.durability().kind = DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS;
+    reader_qos.history().kind = HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
+    reader_ = subscriber_->create_datareader(topic_, reader_qos, this, StatusMask::all());
+    if (reader_ == nullptr)
+    {
+        throw std::runtime_error("WeighReq DataReader initialization failed");
+    }
+}
+
+SubscriberApp::~SubscriberApp()
+{
+    if (nullptr != participant_)
+    {
+        // Delete DDS entities contained within the DomainParticipant
+        participant_->delete_contained_entities();
+
+        // Delete DomainParticipant
+        factory_->delete_participant(participant_);
+    }
+}
+
+void SubscriberApp::on_subscription_matched(
+        DataReader* reader,
+        const SubscriptionMatchedStatus& info)
+{
+    if (info.current_count_change == 1)
+    {
+        std::cout << reader->get_topicdescription()->get_name() << " Subscriber matched." << std::endl;
+    }
+    else if (info.current_count_change == -1)
+    {
+        std::cout << reader->get_topicdescription()->get_name() << " Subscriber unmatched." << std::endl;
+    }
+    else
+    {
+        std::cout << info.current_count_change
+                  << " is not a valid value for SubscriptionMatchedStatus current count change" << std::endl;
+    }
+}
+
+void SubscriberApp::on_data_available(
+        DataReader* reader)
+{
+    SampleInfo info;
+    std::string topic_name = reader->get_topicdescription()->get_name();
+    std::cout << topic_name << std::endl;
+
+    if (topic_name == "WeighReqTopic")
+    {
+        WeighReq sample_;
+        while ((!is_stopped()) && (RETCODE_OK == reader->take_next_sample(&sample_, &info)))
+        {
+            if ((info.instance_state == ALIVE_INSTANCE_STATE) && info.valid_data)
+            {
+                {
+                    std::unique_lock<std::mutex> lock(MsgData::queue_cv_mtx_);
+                    MsgData::WeighReq_queue_.push(std::move(sample_));
+                    lock.unlock();
+                }
+            }
+        }
+    }
+}
+
+void SubscriberApp::run()
+{
+    std::unique_lock<std::mutex> lck(terminate_cv_mtx_);
+    terminate_cv_.wait(lck, [this]
+            {
+                return is_stopped();
+            });
+}
+
+bool SubscriberApp::is_stopped()
+{
+    return stop_.load();
+}
+
+void SubscriberApp::stop()
+{
+    stop_.store(true);
+    terminate_cv_.notify_all();
+}

weigh/Subscriber.hpp

@@ -0,0 +1,75 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file SubscriberApp.hpp
+ * This header file contains the declaration of the subscriber functions.
+ *
+ * This file was generated by the tool fastddsgen.
+ */
+
+#ifndef FAST_DDS_GENERATED__SUBSCRIBERAPP_HPP
+#define FAST_DDS_GENERATED__SUBSCRIBERAPP_HPP
+
+#include <condition_variable>
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/subscriber/DataReaderListener.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include "System.hpp"
+
+class SubscriberApp : public eprosima::fastdds::dds::DataReaderListener
+{
+public:
+
+    SubscriberApp(
+            const int& domain_id);
+
+    virtual ~SubscriberApp();
+
+    //! Subscription callback
+    void on_data_available(
+            eprosima::fastdds::dds::DataReader* reader) override;
+
+    //! Subscriber matched method
+    void on_subscription_matched(
+            eprosima::fastdds::dds::DataReader* reader,
+            const eprosima::fastdds::dds::SubscriptionMatchedStatus& info) override;
+
+    //! Run subscriber
+    void run();
+
+    //! Trigger the end of execution
+    void stop();
+
+private:
+
+    //! Return the current state of execution
+    bool is_stopped();
+
+    std::shared_ptr<eprosima::fastdds::dds::DomainParticipantFactory> factory_;
+    eprosima::fastdds::dds::DomainParticipant* participant_;
+    eprosima::fastdds::dds::Subscriber* subscriber_;
+    eprosima::fastdds::dds::Topic* topic_;
+    eprosima::fastdds::dds::DataReader* reader_;
+    eprosima::fastdds::dds::TypeSupport type_;
+    uint16_t samples_received_;
+    std::atomic<bool> stop_;
+    mutable std::mutex terminate_cv_mtx_;
+    std::condition_variable terminate_cv_;
+};
+
+#endif // FAST_DDS_GENERATED__SUBSCRIBERAPP_HPP

weigh/WeightStabilityDetector.cxx

@@ -0,0 +1,422 @@
+#include "WeightStabilityDetector.hpp"
+
+WeightStabilityDetector::WeightStabilityDetector(const Config& config) :
+    config_(config),
+    current_state_(VehicleState::NO_VEHICLE),
+    current_weight_(0.0f),
+    previous_weight_(0.0f),
+    stable_weight_(0.0f),
+    max_weight_(0.0f),
+    current_window_size_(static_cast<size_t>(config.window_size)),
+    vehicle_window_size_(static_cast<size_t>(config.window_size)),
+    stable_count_(0),
+    continuous_increase_count_(0),
+    total_samples_(0),
+    state_callback_(nullptr),
+    weight_callback_(nullptr),
+    state_callback_data_(nullptr),
+    weight_callback_data_(nullptr),
+    debug_output_(nullptr),
+    is_initialized_(false),
+    has_vehicle_on_(false),
+    is_leaving_state_(false) {
+    
+    reset(false); // 初始化时不通知回调
+    
+    // 默认调试输出到标准输出
+    debug_output_ = [](const std::string& msg) {
+        std::cout << "[WeightDetector] " << msg << std::endl;
+    };
+}
+
+WeightStabilityDetector::~WeightStabilityDetector() {
+    // 清理资源
+}
+
+void WeightStabilityDetector::reset(bool notify_callback) {
+    VehicleState old_state = current_state_;
+    current_state_ = VehicleState::NO_VEHICLE;
+    
+    weight_window_.clear();
+    vehicle_weight_window_.clear();
+    weight_history_.clear();
+    
+    current_weight_ = 0.0f;
+    previous_weight_ = 0.0f;
+    stable_weight_ = 0.0f;
+    max_weight_ = 0.0f;
+    
+    stable_count_ = 0;
+    continuous_increase_count_ = 0;
+    total_samples_ = 0;
+    
+    stats_ = Statistics{};
+    has_vehicle_on_ = false;
+    is_leaving_state_ = false;
+    
+    last_state_change_time_ = std::chrono::steady_clock::now();
+    last_weight_time_ = std::chrono::steady_clock::now();
+    
+    is_initialized_ = true;
+    
+    // 通知状态变化
+    if (notify_callback && state_callback_ && old_state != current_state_) {
+        state_callback_(current_state_, 0.0f, state_callback_data_);
+    }
+    
+    debugLog("Detector reset");
+}
+
+void WeightStabilityDetector::setConfig(const Config& config) {
+    config_ = config;
+    current_window_size_ = static_cast<size_t>(config.window_size);
+    vehicle_window_size_ = static_cast<size_t>(config.window_size);
+    
+    // 如果窗口大小改变，需要调整窗口
+    if (weight_window_.size() > current_window_size_) {
+        while (weight_window_.size() > current_window_size_) {
+            weight_window_.pop_front();
+        }
+    }
+    
+    if (vehicle_weight_window_.size() > vehicle_window_size_) {
+        while (vehicle_weight_window_.size() > vehicle_window_size_) {
+            vehicle_weight_window_.pop_front();
+        }
+    }
+    
+    // 重新计算统计信息
+    calculateVehicleStatistics();
+}
+
+bool WeightStabilityDetector::processWeight(float weight_kg) {
+    return processWeightData(WeightData(weight_kg));
+}
+
+bool WeightStabilityDetector::processWeightData(const WeightData& data) {
+    if (!data.is_valid) {
+        debugLog("Invalid weight data received");
+        return false;
+    }
+    
+    // 保存旧状态用于比较
+    VehicleState old_state = current_state_;
+    
+    // 更新时间戳
+    last_weight_time_ = data.timestamp;
+    
+    // 更新重量值
+    previous_weight_ = current_weight_;
+    current_weight_ = data.value_kg;
+    
+    // 记录最大重量（非离开状态时）
+    if (!is_leaving_state_ && current_weight_ > max_weight_) {
+        max_weight_ = current_weight_;
+    }
+    
+    // 添加到历史记录
+    addWeightToHistory(current_weight_);
+    
+    // 更新全局滑动窗口
+    updateWindow(current_weight_);
+    
+    // 如果不是离开状态，检查车辆状态
+    if (!is_leaving_state_) {
+        if (isVehicleOn(current_weight_) && !has_vehicle_on_) {
+            has_vehicle_on_ = true;
+            if (config_.reset_window_on_vehicle_on) {
+                vehicle_weight_window_.clear();  // 车辆上磅时清空车辆重量窗口
+                max_weight_ = current_weight_;   // 重置最大重量
+            }
+            debugLog("Vehicle on scale detected, clearing vehicle weight window");
+        }
+        
+        // 如果有车在磅上，更新车辆重量窗口
+        if (has_vehicle_on_ && current_weight_ > config_.empty_car_threshold) {
+            updateVehicleWindow(current_weight_);
+        } else if (current_weight_ < config_.empty_car_threshold) {
+            // 如果重量低于空车阈值，重置车辆状态
+            has_vehicle_on_ = false;
+            vehicle_weight_window_.clear();
+        }
+    }
+    
+    // 计算统计信息（基于车辆重量窗口）
+    // if (!vehicle_weight_window_.empty()) {
+        calculateVehicleStatistics();
+    // }
+    
+    // 检查状态转换
+    checkStateTransition(current_weight_);
+    
+    // 触发回调
+    bool state_changed = (old_state != current_state_);
+    
+    if (state_callback_ && state_changed) {
+        state_callback_(current_state_, current_weight_, state_callback_data_);
+    }
+    
+    if (weight_callback_) {
+        weight_callback_(current_weight_, isStable(), weight_callback_data_);
+    }
+    
+    // 调试输出
+    if (config_.enable_debug_log) {
+        std::ostringstream oss;
+        oss << std::fixed << std::setprecision(2)
+            << "Weight: " << current_weight_ << "kg, "
+            << "State: " << static_cast<int>(current_state_) << ", "
+            << "StableCount: " << stable_count_ << ", "
+            << "VehicleWindow: " << vehicle_weight_window_.size() << ", "
+            << "StdDev: " << stats_.std_dev << "kg, "
+            << "MaxWeight: " << max_weight_ << "kg";
+        debugLog(oss.str());
+    }
+    
+    return true;
+}
+
+void WeightStabilityDetector::updateWindow(float weight_kg) {
+    weight_window_.push_back(weight_kg);
+    
+    // 保持窗口大小
+    if (weight_window_.size() > current_window_size_) {
+        weight_window_.pop_front();
+    }
+}
+
+void WeightStabilityDetector::updateVehicleWindow(float weight_kg) {
+    vehicle_weight_window_.push_back(weight_kg);
+    
+    // 保持窗口大小
+    if (vehicle_weight_window_.size() > vehicle_window_size_) {
+        vehicle_weight_window_.pop_front();
+    }
+}
+
+void WeightStabilityDetector::calculateVehicleStatistics() {
+    // 使用车辆重量窗口计算统计信息
+    if (vehicle_weight_window_.empty()) {
+        stats_ = Statistics{};
+        return;
+    }
+    
+    // 计算基本统计量
+    size_t n = vehicle_weight_window_.size();
+    
+    // 计算最小值和最大值
+    stats_.min = *std::min_element(vehicle_weight_window_.begin(), vehicle_weight_window_.end());
+    stats_.max = *std::max_element(vehicle_weight_window_.begin(), vehicle_weight_window_.end());
+    
+    // 计算平均值
+    float sum = std::accumulate(vehicle_weight_window_.begin(), vehicle_weight_window_.end(), 0.0f);
+    stats_.mean = sum / static_cast<float>(n);
+    
+    // 计算方差和标准差
+    float variance_sum = 0.0f;
+    for (float w : vehicle_weight_window_) {
+        float diff = w - stats_.mean;
+        variance_sum += diff * diff;
+    }
+    stats_.variance = variance_sum / static_cast<float>(n);
+    stats_.std_dev = std::sqrt(stats_.variance);
+    
+    // 更新计数器
+    stats_.sample_count = static_cast<int>(n);
+    stats_.stable_count = stable_count_;
+    stats_.increase_count = continuous_increase_count_;
+}
+
+void WeightStabilityDetector::checkStateTransition(float weight_kg) {
+    switch (current_state_) {
+        case VehicleState::NO_VEHICLE:
+            if (isVehicleOn(weight_kg)) {
+                transitionToState(VehicleState::VEHICLE_ON);
+            }
+            break;
+            
+        case VehicleState::VEHICLE_ON: {
+            // 检查重量抖动
+            float weight_diff = std::abs(weight_kg - previous_weight_);
+            
+            if (isJitterExceeded(weight_diff)) {
+                // 抖动过大，重置计数
+                stable_count_ = 0;
+                continuous_increase_count_ = 0;
+                
+                debugLog("Jitter exceeded, resetting stability counters");
+            } else if (weight_kg > config_.min_weight_threshold) {
+                // 增加稳定计数
+                stable_count_++;
+                
+                // 检查是否连续增长
+                if (weight_kg > previous_weight_) {
+                    continuous_increase_count_++;
+                } else {
+                    continuous_increase_count_ = 0;
+                }
+                
+                // 检查是否达到稳定条件
+                if (checkStableCondition()) {
+                    transitionToState(VehicleState::VEHICLE_STABLE);
+                }
+            }
+            
+            // 检查是否离开（包括快速下降）
+            if (isVehicleLeaving(weight_kg) || checkFastDrop(weight_kg)) {
+                transitionToState(VehicleState::VEHICLE_LEAVING);
+            }
+            break;
+        }
+            
+        case VehicleState::VEHICLE_STABLE: {
+            // 稳定状态下继续监测是否离开
+            if (isVehicleLeaving(weight_kg) || checkFastDrop(weight_kg)) {
+                transitionToState(VehicleState::VEHICLE_LEAVING);
+            }
+            break;
+        }
+            
+        case VehicleState::VEHICLE_LEAVING: {
+            // 离开状态持续一段时间后自动回到无车状态
+            auto now = std::chrono::steady_clock::now();
+            auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
+                now - last_state_change_time_).count();
+            
+            if (elapsed > config_.leave_reset_delay_ms) {
+                // 重置但不通知回调，由processWeight统一处理
+                reset(false);
+                // 状态已改变，需要在processWeight中触发回调
+                // 这里不需要额外处理，因为reset(false)不会触发回调
+                // 状态变化会在processWeight返回前被检测到并触发回调
+            }
+            break;
+        }
+            
+        default:
+            // 处理未知状态
+            break;
+    }
+}
+
+void WeightStabilityDetector::transitionToState(VehicleState new_state) {
+    current_state_ = new_state;
+    last_state_change_time_ = std::chrono::steady_clock::now();
+    
+    // 状态特定处理
+    switch (new_state) {
+        case VehicleState::VEHICLE_STABLE:
+            stable_weight_ = stats_.mean; // 使用均值作为稳定重量
+            debugLog("Vehicle stable, weight: " + std::to_string(stable_weight_) + "kg");
+            break;
+            
+        case VehicleState::VEHICLE_LEAVING:
+            vehicle_weight_window_.clear(); // 清空车辆重量窗口
+            is_leaving_state_ = true;      // 标记为离开状态
+            debugLog("Vehicle leaving detected");
+            break;
+            
+        case VehicleState::NO_VEHICLE:
+            is_leaving_state_ = false;     // 清除离开状态标记
+            debugLog("No vehicle on scale");
+            break;
+            
+        case VehicleState::VEHICLE_ON:
+            is_leaving_state_ = false;     // 清除离开状态标记
+            debugLog("Vehicle detected on scale");
+            break;
+            
+        default:
+            break;
+    }
+}
+
+bool WeightStabilityDetector::isJitterExceeded(float weight_diff) const {
+    return weight_diff > config_.jitter_threshold;
+}
+
+bool WeightStabilityDetector::isVehicleOn(float weight_kg) const {
+    return weight_kg > config_.min_weight_threshold;
+}
+
+bool WeightStabilityDetector::isVehicleLeaving(float weight_kg) const {
+    return weight_kg < config_.empty_car_threshold;
+}
+
+bool WeightStabilityDetector::checkFastDrop(float weight_kg) const {
+    // 检查快速下降：当前重量远低于历史最高重量
+    if (max_weight_ > config_.min_weight_threshold) {
+        float drop_amount = max_weight_ - weight_kg;
+        if (drop_amount > config_.fast_drop_threshold) {
+            if (config_.enable_debug_log && debug_output_) {
+                std::ostringstream oss;
+                oss << "Fast drop detected: max=" << max_weight_ 
+                    << "kg, current=" << weight_kg << "kg, drop=" << drop_amount << "kg";
+                debug_output_(oss.str());
+            }
+            return true;
+        }
+    }
+    return false;
+}
+
+bool WeightStabilityDetector::checkStableCondition() const {
+    // 条件1：稳定次数达到要求
+    if (stable_count_ < config_.required_stable_count) {
+        return false;
+    }
+    
+    // 条件2：车辆重量窗口至少有3个样本
+    if (vehicle_weight_window_.size() < 3) {
+        return false;
+    }
+    
+    // 条件3：基于车辆重量的稳定性判断
+    // 计算极差（最大值-最小值）
+    float weight_range = stats_.max - stats_.min;
+    
+    // 条件3.1：标准差小于阈值
+    bool std_dev_stable = (stats_.std_dev < config_.max_std_dev);
+    
+    // 条件3.2：极差小于2倍抖动阈值
+    bool range_stable = (weight_range < (2.0f * config_.jitter_threshold));
+    
+    // 条件4：抖动次数要求
+    int non_increase_count = stable_count_ - continuous_increase_count_;
+    bool jitter_met = (non_increase_count >= config_.min_jitter_count);
+    
+    // 条件5：超时稳定
+    bool timeout_stable = (stable_count_ >= (config_.required_stable_count + config_.timeout_extra));
+    
+    // 综合判断：统计稳定且（满足抖动要求或超时）
+    bool is_stable = (std_dev_stable || range_stable) && (jitter_met || timeout_stable);
+    
+    if (config_.enable_debug_log && debug_output_) {
+        std::ostringstream oss;
+        oss << "Stable Check: std_dev=" << stats_.std_dev 
+            << ", range=" << weight_range
+            << ", non_increase=" << non_increase_count
+            << ", stable_count=" << stable_count_
+            << ", vehicle_window=" << vehicle_weight_window_.size()
+            << ", result=" << (is_stable ? "STABLE" : "NOT_STABLE");
+        debug_output_(oss.str());
+    }
+    
+    return is_stable;
+}
+
+void WeightStabilityDetector::addWeightToHistory(float weight_kg) {
+    weight_history_.push_back(weight_kg);
+    total_samples_++;
+    
+    // 限制历史记录大小
+    if (weight_history_.size() > MAX_HISTORY_SIZE) {
+        weight_history_.erase(weight_history_.begin());
+    }
+}
+
+void WeightStabilityDetector::debugLog(const std::string& message) const {
+    if (config_.enable_debug_log && debug_output_) {
+        debug_output_(message);
+    }
+}

weigh/WeightStabilityDetector.hpp

@@ -0,0 +1,207 @@
+#ifndef WEIGHT_STABILITY_DETECTOR_HPP
+#define WEIGHT_STABILITY_DETECTOR_HPP
+
+#include <queue>
+#include <vector>
+#include <cmath>
+#include <functional>
+#include <memory>
+#include <chrono>
+#include <algorithm>
+#include <numeric>
+#include <iostream>
+#include <sstream>
+#include <iomanip>
+#include <string>
+
+class WeightStabilityDetector {
+public:
+    // 配置结构体
+    struct Config {
+        float jitter_threshold;        // 抖动阈值(kg)
+        float min_weight_threshold;    // 最小车辆重量阈值(kg)
+        float empty_car_threshold;     // 空车阈值(kg)
+        float fast_drop_threshold;     // 快速下降阈值(kg)
+        int required_stable_count;        // 要求稳定次数
+        int min_jitter_count;             // 最小抖动次数
+        int timeout_extra;                // 超时额外次数
+        int window_size;                  // 滑动窗口大小
+        float max_std_dev;                // 最大标准差(kg)
+        bool enable_debug_log;            // 是否启用调试日志
+        bool reset_window_on_vehicle_on;  // 车辆上磅时重置窗口
+        int leave_reset_delay_ms;         // 离开状态重置延迟(ms)
+    };
+    
+    // 车辆状态枚举
+    enum class VehicleState {
+        NO_VEHICLE,      // 无车
+        VEHICLE_ON,      // 车辆上磅
+        VEHICLE_STABLE,  // 车辆稳定
+        VEHICLE_LEAVING  // 车辆离开
+    };
+    
+    // 回调函数类型定义
+    typedef std::function<void(VehicleState, float, void*)> StateCallback;
+    typedef std::function<void(float, bool, void*)> WeightCallback;
+    
+    // 统计信息结构体
+    struct Statistics {
+        float mean;      // 平均值
+        float std_dev;   // 标准差
+        float min;       // 最小值
+        float max;       // 最大值
+        float variance;  // 方差
+        int sample_count;   // 样本数量
+        int stable_count;   // 稳定次数
+        int increase_count; // 连续增长次数
+        
+        Statistics() :
+            mean(0.0f),
+            std_dev(0.0f),
+            min(0.0f),
+            max(0.0f),
+            variance(0.0f),
+            sample_count(0),
+            stable_count(0),
+            increase_count(0) {}
+    };
+    
+    // 重量数据类型定义
+    struct WeightData {
+        float value_kg;                           // 重量值(kg)
+        std::chrono::steady_clock::time_point timestamp; // 时间戳
+        bool is_valid;                            // 是否有效
+        bool is_stable;                           // 是否稳定
+        
+        WeightData(float val = 0.0f) :
+            value_kg(val),
+            timestamp(std::chrono::steady_clock::now()),
+            is_valid(true),
+            is_stable(false) {}
+    };
+    
+    // 构造函数和析构函数
+    explicit WeightStabilityDetector(const Config& config = Config());
+    ~WeightStabilityDetector();
+    
+    // 禁用拷贝构造和赋值
+    WeightStabilityDetector(const WeightStabilityDetector&) = delete;
+    WeightStabilityDetector& operator=(const WeightStabilityDetector&) = delete;
+    
+    // 主要接口
+    bool processWeight(float weight_kg);
+    bool processWeightData(const WeightData& data);
+    
+    // 状态获取
+    VehicleState getState() const { return current_state_; }
+    bool isStable() const { return current_state_ == VehicleState::VEHICLE_STABLE; }
+    bool hasVehicle() const { 
+        return current_state_ == VehicleState::VEHICLE_ON || 
+               current_state_ == VehicleState::VEHICLE_STABLE; 
+    }
+    
+    // 重量获取
+    float getCurrentWeight() const { return current_weight_; }
+    float getStableWeight() const { return stable_weight_; }
+    float getLastWeight() const { return previous_weight_; }
+    
+    // 统计信息获取
+    Statistics getStatistics() const { return stats_; }
+    const std::vector<float>& getWeightHistory() const { return weight_history_; }
+    
+    // 配置管理
+    void setConfig(const Config& config);
+    const Config& getConfig() const { return config_; }
+    
+    // 重置检测器
+    void reset(bool notify_callback = true);
+    
+    // 强制设置状态（用于测试）
+    void forceState(VehicleState state) { current_state_ = state; }
+    
+    // 回调设置
+    void setStateCallback(StateCallback callback, void* user_data = nullptr) {
+        state_callback_ = callback;
+        state_callback_data_ = user_data;
+    }
+    
+    void setWeightCallback(WeightCallback callback, void* user_data = nullptr) {
+        weight_callback_ = callback;
+        weight_callback_data_ = user_data;
+    }
+    
+    // 调试功能
+    void enableDebugLog(bool enable) { config_.enable_debug_log = enable; }
+    void setDebugOutput(std::function<void(const std::string&)> debug_func) {
+        debug_output_ = debug_func;
+    }
+    
+private:
+    // 内部辅助函数
+    void updateWindow(float weight_kg);
+    void updateVehicleWindow(float weight_kg);
+    void calculateVehicleStatistics();
+    void checkStateTransition(float weight_kg);
+    void transitionToState(VehicleState new_state);
+    
+    // 状态判断函数 - 添加const修饰符
+    bool isJitterExceeded(float weight_diff) const;
+    bool isVehicleOn(float weight_kg) const;
+    bool isVehicleLeaving(float weight_kg) const;
+    bool checkFastDrop(float weight_kg) const;
+    bool checkStableCondition() const;
+    
+    // 内部数据处理
+    void addWeightToHistory(float weight_kg);
+    
+    // 调试输出 - 添加const修饰符
+    void debugLog(const std::string& message) const;
+    
+private:
+    Config config_;
+    VehicleState current_state_;
+    
+    // 重量数据
+    float current_weight_;
+    float previous_weight_;
+    float stable_weight_;
+    float max_weight_;  // 记录最大重量
+    
+    // 滑动窗口
+    std::deque<float> weight_window_;  // 全局重量窗口
+    std::deque<float> vehicle_weight_window_;  // 车辆上磅后的重量窗口
+    size_t current_window_size_;
+    size_t vehicle_window_size_;
+    
+    // 计数器
+    int stable_count_;
+    int continuous_increase_count_;
+    int total_samples_;
+    
+    // 历史数据
+    std::vector<float> weight_history_;
+    static const size_t MAX_HISTORY_SIZE = 1000;
+    
+    // 时间管理
+    std::chrono::steady_clock::time_point last_state_change_time_;
+    std::chrono::steady_clock::time_point last_weight_time_;
+    
+    // 统计信息
+    mutable Statistics stats_;  // 标记为mutable，因为calculateVehicleStatistics需要修改它
+    
+    // 回调函数
+    StateCallback state_callback_;
+    WeightCallback weight_callback_;
+    void* state_callback_data_;
+    void* weight_callback_data_;
+    
+    // 调试输出
+    std::function<void(const std::string&)> debug_output_;
+    
+    // 状态标志
+    bool is_initialized_;
+    bool has_vehicle_on_;  // 是否有车在磅上
+    bool is_leaving_state_; // 是否处于离开状态
+};
+
+#endif // WEIGHT_STABILITY_DETECTOR_HPP

weigh/main.cxx

@@ -0,0 +1,168 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file Systemmain.cxx
+ * This file acts as a main entry point to the application.
+ *
+ * This file was generated by the tool fastddsgen.
+ */
+
+#include <csignal>
+#include <cstring>
+#include <functional>
+#include <iostream>
+#include <stdexcept>
+#include <thread>
+
+#include <fastdds/dds/log/Log.hpp>
+
+#include "Subscriber.hpp"
+#include "Publisher.hpp"
+#include "SerialMsgHandler.hpp"
+#include "msg.hpp"
+
+#define VERSION "v1.0"
+
+using eprosima::fastdds::dds::Log;
+
+std::function<void(int)> stop_handler;
+void signal_handler(
+        int signum)
+{
+    stop_handler(signum);
+}
+
+std::string parse_signal(
+        const int& signum)
+{
+    switch (signum)
+    {
+        case SIGINT:
+            return "SIGINT";
+        case SIGTERM:
+            return "SIGTERM";
+#ifndef _WIN32
+        case SIGQUIT:
+            return "SIGQUIT";
+        case SIGHUP:
+            return "SIGHUP";
+#endif // _WIN32
+        default:
+            return "UNKNOWN SIGNAL";
+    }
+}
+
+std::queue<WeighReq> MsgData::WeighReq_queue_;
+std::mutex MsgData::queue_cv_mtx_;
+
+int main(int argc, char** argv)
+{
+    auto ret = EXIT_SUCCESS;
+    std::shared_ptr<SubscriberApp> sub;
+    std::shared_ptr<PublisherApp> pub;
+    std::shared_ptr<MsgHandler> dev;
+
+    int domain_id = 0;
+    const char* interface = "serial";
+    const char* port = "ttyS1";
+    const char* baudrate = "9600";
+    const char* device = "Toledo";
+
+    for (int i = 1; i < argc; i++)
+    {
+        if (strcmp(argv[i], "--domain") == 0 && i + 1 < argc)
+        {
+            domain_id = atoi(argv[++i]);
+        }
+        else if (strcmp(argv[i], "--interface") == 0 && i + 1 < argc)
+        {
+            interface = argv[++i];
+        }
+        else if (strcmp(argv[i], "--port") == 0 && i + 1 < argc)
+        {
+            port = argv[++i];
+        }
+        else if (strcmp(argv[i], "--baudrate") == 0 && i + 1 < argc)
+        {
+            baudrate = argv[++i];
+        }
+        else if (strcmp(argv[i], "--device") == 0 && i + 1 < argc)
+        {
+            device = argv[++i];
+        }
+        else if (strcmp(argv[i], "--version") == 0)
+        {
+            std::cout << "Vesrion: " << VERSION << "\n";
+            return EXIT_SUCCESS;
+        }
+        else if (strcmp(argv[i], "--help") == 0)
+        {
+            std::cout << "Usage: [options]\n"
+                      << "Options:\n"
+                      << "  --domain        Set domain ID\n"
+                      << "  --interface     Set interface (e.g., serial)\n"
+                      << "  --baudrate      Set baudrate (e.g., 9600)\n"
+                      << "  --port          Set port name (e.g., ttyS1)\n"
+                      << "  --device        Set device name (e.g., Toledo)\n"
+                      << "  --version       Show software Version\n"
+                      << "  --help          Show this help message\n";
+            return EXIT_SUCCESS;
+        }
+        else
+        {
+            std::cerr << "Unknown option: " << argv[i] << "\n";
+            std::cout << "Usage: [options]\n"
+                      << "Options:\n"
+                      << "  --domain        Set domain ID\n"
+                      << "  --interface     Set interface (e.g., serial)\n"
+                      << "  --baudrate      Set baudrate (e.g., 9600)\n"
+                      << "  --port          Set port name (e.g., ttyS1)\n"
+                      << "  --device        Set device name (e.g., Toledo)\n"
+                      << "  --version       Show software Version\n"
+                      << "  --help          Show this help message\n";
+            return EXIT_FAILURE;
+        }
+    }
+
+    sub = std::make_shared<SubscriberApp>(domain_id);
+    pub = std::make_shared<PublisherApp>(domain_id);
+    dev = std::make_shared<SerialMsgHandler>();
+
+    dev->OpenPort(port, baudrate);
+    dev->SetDevice(device);
+
+    std::thread pub_thread(&PublisherApp::run, pub, dev);
+
+    std::cout << "Program is running. Please press Ctrl+C to stop at any time." << std::endl;
+
+    stop_handler = [&](int signum)
+            {
+                std::cout << "\n" << parse_signal(signum) << " received, stopping " << argv[1]
+                          << " execution." << std::endl;
+                pub->stop();
+            };
+
+    signal(SIGINT, signal_handler);
+    signal(SIGTERM, signal_handler);
+#ifndef _WIN32
+    signal(SIGQUIT, signal_handler);
+    signal(SIGHUP, signal_handler);
+#endif // _WIN32
+
+    pub_thread.join();
+
+    Log::Reset();
+    return ret;
+}

weigh/msg.hpp

@@ -0,0 +1,14 @@
+#ifndef _MSG_HPP_
+#define _MSG_HPP_
+
+#include <queue>
+#include <mutex>
+#include "System.hpp"
+
+class MsgData {
+public:
+    static std::queue<WeighReq> WeighReq_queue_;
+    static std::mutex queue_cv_mtx_;
+};
+
+#endif