diff --git a/CMakeLists.txt b/CMakeLists.txt index 8692f3f..527df32 100644 --- a/CMakeLists.txt +++ b/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 + ) diff --git a/common/SerialMsgHandler.cxx b/common/SerialMsgHandler.cxx new file mode 100644 index 0000000..0c698cd --- /dev/null +++ b/common/SerialMsgHandler.cxx @@ -0,0 +1,118 @@ +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/common/SerialMsgHandler.hpp b/common/SerialMsgHandler.hpp new file mode 100644 index 0000000..9e485c4 --- /dev/null +++ b/common/SerialMsgHandler.hpp @@ -0,0 +1,24 @@ +#ifndef _SERIALMSGHANDLER_HPP_ +#define _SERIALMSGHANDLER_HPP_ + +#include +#include + +#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_ diff --git a/weigh/MsgHandler.cxx b/weigh/MsgHandler.cxx new file mode 100644 index 0000000..8de4a2a --- /dev/null +++ b/weigh/MsgHandler.cxx @@ -0,0 +1,528 @@ +#include +#include +#include +#include +#include +#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& 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& 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& 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& 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& msg) +{ + return; +} + +bool MsgHandler::get_frame(std::vector& 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& 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(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& 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(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& 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(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& 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(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 +bool MsgHandler::HandleDeviceMsg(T& msg) +{ + std::vector m_Data; + std::vector 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&); +template bool MsgHandler::HandleDeviceMsg(float&); \ No newline at end of file diff --git a/weigh/MsgHandler.hpp b/weigh/MsgHandler.hpp new file mode 100644 index 0000000..4165132 --- /dev/null +++ b/weigh/MsgHandler.hpp @@ -0,0 +1,41 @@ +#ifndef _MSGHANDLER_HPP_ +#define _MSGHANDLER_HPP_ + +#include +#include +#include +#include + +class MsgHandler { +public: + int fd; + std::string device; + std::vector m_WeightData; + + MsgHandler(); + ~MsgHandler() = default; + + void HandleDdsMsg(const std::map& msg); + template + 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& data); + virtual int RecvDeviceMsg(std::vector& data, int timeoutMs); + int SendMsg(const std::vector& data); + int RecvMsg(std::vector& data, int timeoutMs); + bool isOpen() const { + return fd != -1; + } + void ClosePort(); + +private: + bool get_frame(std::vector& frame, uint8_t frame_size, uint8_t start, uint8_t end); + bool get_weight_1(std::string& weight, std::vector& frame); + bool get_weight_1(float& weight, std::vector& frame); + bool get_weight_2(std::string& weight, std::vector& frame); + bool get_weight_2(float& weight, std::vector& frame); +}; + +#endif \ No newline at end of file diff --git a/weigh/Publisher.cxx b/weigh/Publisher.cxx new file mode 100644 index 0000000..c8ba67f --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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 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 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 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(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 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(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 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 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(); +} \ No newline at end of file diff --git a/weigh/Publisher.hpp b/weigh/Publisher.hpp new file mode 100644 index 0000000..3b86061 --- /dev/null +++ b/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 + +#include +#include +#include +#include + +#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 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 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 stop_; +}; + +#endif // FAST_DDS_GENERATED__PUBLISHERAPP_HPP \ No newline at end of file diff --git a/weigh/Subscriber.cxx b/weigh/Subscriber.cxx new file mode 100644 index 0000000..ba53473 --- /dev/null +++ b/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 +#include + +#include +#include +#include +#include +#include +#include +#include + +#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 lock(MsgData::queue_cv_mtx_); + MsgData::WeighReq_queue_.push(std::move(sample_)); + lock.unlock(); + } + } + } + } +} + +void SubscriberApp::run() +{ + std::unique_lock 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(); +} \ No newline at end of file diff --git a/weigh/Subscriber.hpp b/weigh/Subscriber.hpp new file mode 100644 index 0000000..81132dc --- /dev/null +++ b/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 + +#include +#include +#include +#include + +#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 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 stop_; + mutable std::mutex terminate_cv_mtx_; + std::condition_variable terminate_cv_; +}; + +#endif // FAST_DDS_GENERATED__SUBSCRIBERAPP_HPP \ No newline at end of file diff --git a/weigh/WeightStabilityDetector.cxx b/weigh/WeightStabilityDetector.cxx new file mode 100644 index 0000000..7d96faf --- /dev/null +++ b/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(config.window_size)), + vehicle_window_size_(static_cast(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(config.window_size); + vehicle_window_size_ = static_cast(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(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(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(n); + stats_.std_dev = std::sqrt(stats_.variance); + + // 更新计数器 + stats_.sample_count = static_cast(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( + 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); + } +} \ No newline at end of file diff --git a/weigh/WeightStabilityDetector.hpp b/weigh/WeightStabilityDetector.hpp new file mode 100644 index 0000000..5d04fe5 --- /dev/null +++ b/weigh/WeightStabilityDetector.hpp @@ -0,0 +1,207 @@ +#ifndef WEIGHT_STABILITY_DETECTOR_HPP +#define WEIGHT_STABILITY_DETECTOR_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 StateCallback; + typedef std::function 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& 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 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 weight_window_; // 全局重量窗口 + std::deque 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 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 debug_output_; + + // 状态标志 + bool is_initialized_; + bool has_vehicle_on_; // 是否有车在磅上 + bool is_leaving_state_; // 是否处于离开状态 +}; + +#endif // WEIGHT_STABILITY_DETECTOR_HPP \ No newline at end of file diff --git a/weigh/main.cxx b/weigh/main.cxx new file mode 100644 index 0000000..0b7329a --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +#include + +#include "Subscriber.hpp" +#include "Publisher.hpp" +#include "SerialMsgHandler.hpp" +#include "msg.hpp" + +#define VERSION "v1.0" + +using eprosima::fastdds::dds::Log; + +std::function 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 MsgData::WeighReq_queue_; +std::mutex MsgData::queue_cv_mtx_; + +int main(int argc, char** argv) +{ + auto ret = EXIT_SUCCESS; + std::shared_ptr sub; + std::shared_ptr pub; + std::shared_ptr 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(domain_id); + pub = std::make_shared(domain_id); + dev = std::make_shared(); + + 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; +} diff --git a/weigh/msg.hpp b/weigh/msg.hpp new file mode 100644 index 0000000..f533444 --- /dev/null +++ b/weigh/msg.hpp @@ -0,0 +1,14 @@ +#ifndef _MSG_HPP_ +#define _MSG_HPP_ + +#include +#include +#include "System.hpp" + +class MsgData { +public: + static std::queue WeighReq_queue_; + static std::mutex queue_cv_mtx_; +}; + +#endif \ No newline at end of file