AuseftDDSPlugins/plugins/AQTSampleMachinePlug/AQTSampleMachinePlug.cxx

#include "AQTSampleMachinePlug.hpp"
#include <QDebug>
#include <fastdds/dds/domain/DomainParticipantFactory.hpp>

/**
 * @brief 构造函数
 * @param parent 父对象指针
 * 
 * 初始化插件对象，包括：
 * - 初始化所有指针为空
 * - 创建DDS类型支持对象
 * - 初始化数据点信息
 */
AQTSampleMachinePlug::AQTSampleMachinePlug(QObject *parent)
    : QObject(parent)
    , participant_(nullptr)
    , publisher_(nullptr)
    , subscriber_(nullptr)
    , commandTopic_(nullptr)
    , commandWriter_(nullptr)
    , statusTopic_(nullptr)
    , statusReader_(nullptr)
    , configTopic_(nullptr)
    , configWriter_(nullptr)
    , pointsTopic_(nullptr)
    , pointsWriter_(nullptr)
    , pointsReader_(nullptr)
{
    // 初始化类型支持
    commandType_ = TypeSupport(new SampleCommandPubSubType());
    statusType_ = TypeSupport(new SampleMachineInfoPubSubType());
    configType_ = TypeSupport(new SampleConfigPubSubType());
    pointsType_ = TypeSupport(new SamplePointsPubSubType());
}

/**
 * @brief 析构函数
 * 
 * 清理插件对象，主要是清理所有DDS相关的资源。
 */
AQTSampleMachinePlug::~AQTSampleMachinePlug()
{
    // 清理DDS资源
    cleanupDDSEntities();
}

/**
 * @brief 初始化插件
 * @param domainId DDS域ID
 * @param domainName DDS域名称
 * @return 初始化是否成功
 * 
 * 初始化DDS通信环境，包括：
 * - 创建DDS参与者
 * - 初始化所有DDS实体
 */
bool AQTSampleMachinePlug::init(uint32_t domainId, const QString& domainName)
{
    // 创建DDS参与者
    DomainParticipantQos participantQos;
    participantQos.name(domainName.toStdString());
    participant_ = DomainParticipantFactory::get_instance()->create_participant(domainId, participantQos);

    if (!participant_) {
        qDebug() << "Failed to create participant";
        return false;
    }

    // 初始化其他DDS实体
    return initDDSEntities();
}

/**
 * @brief 获取数据点信息列表
 * @return 所有支持的数据点信息列表
 */
QList<DataPointInfo> AQTSampleMachinePlug::getDataPoints()
{
    // 在函数调用时创建并返回数据点列表
    return {
        // ===== SampleConfigData 相关数据点 (1-9) =====
        {1, "UnifiedDepth", "uint16"},      // 统一深度（全段面采样时使用）
        {2, "SampleOrder", "uint8"},        // 大车行进顺序（1:原点往外，2:往原点采）

        // ===== SampleCommand 相关数据点 (10-29) =====
        {10, "CommandMaskBits", "uint32"},    // 命令掩码，表示哪些字段被修改
        {11, "ControlType", "uint8"},         // 控制类型（1:就地，2:远程）
        {12, "StartSample", "int32"},         // 启动采样命令 (CommandState: STOP=0, START=1)
        {13, "StopSample", "int32"},          // 停止采样命令 (CommandState: STOP=0, START=1)
        {14, "Reset", "int32"},               // 系统复位命令 (CommandState: STOP=0, START=1)
        {15, "PositionCheck", "int32"},       // 位置确认命令 (CommandState: STOP=0, START=1)
        {16, "EmergencyUp", "int32"},         // 紧急提升命令 (CommandState: STOP=0, START=1)
        {17, "EmergencyStop", "int32"},       // 系统急停命令 (CommandState: STOP=0, START=1)
        {18, "ControlBelt1", "int32"},        // 启停皮带1 (CommandState: STOP=0, START=1)
        {19, "ControlBelt2", "int32"},        // 启停皮带2 (CommandState: STOP=0, START=1)
        {20, "ControlCrush1", "int32"},       // 启停破碎机1 (CommandState: STOP=0, START=1)
        {21, "ControlCrush2", "int32"},       // 启停破碎机2 (CommandState: STOP=0, START=1)
        {22, "ControlDivider", "int32"},      // 启停缩分器 (CommandState: STOP=0, START=1)
        {23, "ControlCoalDistributor", "int32"}, // 启停布煤器 (CommandState: STOP=0, START=1)
        {24, "ReductionRatio", "uint16"},     // 缩分比
        {25, "CommandVersion", "uint8"},       // 命令结构体版本号

        // ===== SampleInfo 相关数据点 (30-49) =====
        {30, "StatusMaskBits", "uint32"},    // 状态掩码，表示哪些字段被修改
        {31, "BigPoint", "uint16"},          // 大车位置
        {32, "SmallPoint", "uint16"},        // 小车位置
        {33, "DepthPoint", "uint16"},        // 深度位置
        {34, "HasToXY", "uint8"},            // 到达坐标点（0:未到达，1:已到达）
        {35, "HasSamplePoints", "uint8"},    // 已采样点数
        {36, "GameOver", "uint8"},           // 采样完成（0:未完成，1:完成）
        {37, "BackState", "uint16"},         // 采样状态返回
        {38, "SampleState", "int32"},        // 采样机状态 (DeviceState: OFF=0, ON=1, ERROR=2)
        {39, "Belt1State", "int32"},         // 皮带1状态 (DeviceState: OFF=0, ON=1, ERROR=2)
        {40, "Belt2State", "int32"},         // 皮带2状态 (DeviceState: OFF=0, ON=1, ERROR=2)
        {41, "Crush1State", "int32"},        // 破碎机1状态 (DeviceState: OFF=0, ON=1, ERROR=2)
        {42, "Crush2State", "int32"},        // 破碎机2状态 (DeviceState: OFF=0, ON=1, ERROR=2)
        {43, "DividerState", "int32"},       // 缩分器状态 (DeviceState: OFF=0, ON=1, ERROR=2)
        {44, "BigLimit", "uint8"},           // 大车限位（0:未限位，1:已限位）
        {45, "BigZeroLimit", "uint8"},       // 大车零点限位（0:未限位，1:已限位）
        {46, "SmallLimit", "uint8"},         // 小车限位（0:未限位，1:已限位）
        {47, "SmallZeroLimit", "uint8"},     // 小车零点限位（0:未限位，1:已限位）
        {48, "UpLimit", "uint8"},            // 升高限位（0:未限位，1:已限位）
        {49, "DownLimit", "uint8"},          // 降低限位（0:未限位，1:已限位）
        {50, "StatusVersion", "uint8"},      // 状态结构体版本号

        // ===== SamplePoints 相关数据点 (51-99) =====
        {51, "TotalPoints", "uint8"},        // 采样点数量
        // 点1
        {52, "Point1X", "uint16"},           // 采样点1 X坐标
        {53, "Point1Y", "uint16"},           // 采样点1 Y坐标
        {54, "Point1Z", "uint16"},           // 采样点1 Z坐标
        {55, "Point1Drop", "int32"},         // 采样点1 落料选择 (DropChoice: BYPASS=0, CRUSHER=1)
        // 点2
        {56, "Point2X", "uint16"},           // 采样点2 X坐标
        {57, "Point2Y", "uint16"},           // 采样点2 Y坐标
        {58, "Point2Z", "uint16"},           // 采样点2 Z坐标
        {59, "Point2Drop", "int32"},         // 采样点2 落料选择 (DropChoice: BYPASS=0, CRUSHER=1)
        // 点3
        {60, "Point3X", "uint16"},           // 采样点3 X坐标
        {61, "Point3Y", "uint16"},           // 采样点3 Y坐标
        {62, "Point3Z", "uint16"},           // 采样点3 Z坐标
        {63, "Point3Drop", "int32"},         // 采样点3 落料选择 (DropChoice: BYPASS=0, CRUSHER=1)
        // 点4
        {64, "Point4X", "uint16"},           // 采样点4 X坐标
        {65, "Point4Y", "uint16"},           // 采样点4 Y坐标
        {66, "Point4Z", "uint16"},           // 采样点4 Z坐标
        {67, "Point4Drop", "int32"},         // 采样点4 落料选择 (DropChoice: BYPASS=0, CRUSHER=1)
        // 点5
        {68, "Point5X", "uint16"},           // 采样点5 X坐标
        {69, "Point5Y", "uint16"},           // 采样点5 Y坐标
        {70, "Point5Z", "uint16"},           // 采样点5 Z坐标
        {71, "Point5Drop", "int32"},         // 采样点5 落料选择 (DropChoice: BYPASS=0, CRUSHER=1)
        // 点6
        {72, "Point6X", "uint16"},           // 采样点6 X坐标
        {73, "Point6Y", "uint16"},           // 采样点6 Y坐标
        {74, "Point6Z", "uint16"},           // 采样点6 Z坐标
        {75, "Point6Drop", "int32"}          // 采样点6 落料选择 (DropChoice: BYPASS=0, CRUSHER=1)
    };
}

/**
 * @brief 发布数据到DDS网络
 * @param dataList 要发布的数据项列表
 * @return 发布是否成功
 * 
 * 将数据项列表转换为相应的DDS消息并发布：
 * - 检查写入器是否可用
 * - 转换数据格式
 * - 发送数据
 */
bool AQTSampleMachinePlug::publishData(const QList<DataItem>& dataList)
{
    if (dataList.isEmpty()) {
        qDebug() << "Empty data list received";
        return false;
    }

    try {
        // 根据第一个数据项的ID判断主题类型
        int firstId = dataList.first().ID;
        
        if (firstId >= 1 && firstId <= 9) {  // 配置数据
            if (!configWriter_) {
                qDebug() << "Config writer not initialized";
                return false;
            }
            auto config = dataItemsToConfig(dataList);
            return configWriter_->write(&config);
        }
        else if (firstId >= 10 && firstId <= 29) {  // 命令数据
            if (!commandWriter_) {
                qDebug() << "Command writer not initialized";
                return false;
            }
            auto command = dataItemsToCommand(dataList);
            return commandWriter_->write(&command);
        }
        else if (firstId >= 51 && firstId <= 99) {  // 采样点数据
            if (!pointsWriter_) {
                qDebug() << "Points writer not initialized";
                return false;
            }
            auto points = dataItemsToPoints(dataList);
            return pointsWriter_->write(&points);
        }
        else {
            qDebug() << "Invalid data ID range:" << firstId;
            return false;
        }
    }
    catch (const std::exception& e) {
        qDebug() << "Error publishing data:" << e.what();
        return false;
    }
}

/**
 * @brief 初始化DDS实体
 * @return 初始化是否成功
 * 
 * 创建并初始化所有DDS通信所需的实体，包括：
 * - 注册数据类型
 * - 创建发布者和订阅者
 * - 创建主题
 * - 创建数据写入器和读取器
 * - 设置数据监听器
 */
bool AQTSampleMachinePlug::initDDSEntities()
{
    try {
        // 注册所有类型
        commandType_.register_type(participant_);
        statusType_.register_type(participant_);
        configType_.register_type(participant_);
        pointsType_.register_type(participant_);

        // 创建发布者和订阅者
        publisher_ = participant_->create_publisher(PUBLISHER_QOS_DEFAULT);
        subscriber_ = participant_->create_subscriber(SUBSCRIBER_QOS_DEFAULT);

        if (!publisher_ || !subscriber_) {
            qDebug() << "Failed to create publisher or subscriber";
            return false;
        }

        // 创建所有主题
        commandTopic_ = participant_->create_topic("SampleMachine/Command", 
            commandType_.get_type_name(), TOPIC_QOS_DEFAULT);
        statusTopic_ = participant_->create_topic("SampleMachine/Status",
            statusType_.get_type_name(), TOPIC_QOS_DEFAULT);
        configTopic_ = participant_->create_topic("SampleMachine/Config",
            configType_.get_type_name(), TOPIC_QOS_DEFAULT);
        pointsTopic_ = participant_->create_topic("SampleMachine/Points",
            pointsType_.get_type_name(), TOPIC_QOS_DEFAULT);

        if (!commandTopic_ || !statusTopic_ || !configTopic_ || !pointsTopic_) {
            qDebug() << "Failed to create topics";
            return false;
        }

        // 创建写入器和读取器
        commandWriter_ = publisher_->create_datawriter(commandTopic_, DATAWRITER_QOS_DEFAULT);
        statusReader_ = subscriber_->create_datareader(statusTopic_, DATAREADER_QOS_DEFAULT);
        configWriter_ = publisher_->create_datawriter(configTopic_, DATAWRITER_QOS_DEFAULT);
        pointsWriter_ = publisher_->create_datawriter(pointsTopic_, DATAWRITER_QOS_DEFAULT);
        pointsReader_ = subscriber_->create_datareader(pointsTopic_, DATAREADER_QOS_DEFAULT);

        if (!commandWriter_ || !statusReader_ || !configWriter_ || 
            !pointsWriter_ || !pointsReader_) {
            qDebug() << "Failed to create writers or readers";
            return false;
        }

        // 设置状态和采样点的监听器
        statusReader_->set_listener(this, StatusMask::data_available());
        pointsReader_->set_listener(this, StatusMask::data_available());

        return true;
    }
    catch (const std::exception& e) {
        qDebug() << "Error initializing DDS entities:" << e.what();
        return false;
    }
}

/**
 * @brief 清理DDS实体
 * 
 * 清理所有已创建的DDS资源，包括：
 * - 删除所有包含的实体（主题、读写器等）
 * - 删除参与者
 */
void AQTSampleMachinePlug::cleanupDDSEntities()
{
    // 清理所有DDS资源
    if (participant_) {
        participant_->delete_contained_entities();
        DomainParticipantFactory::get_instance()->delete_participant(participant_);
    }
}

/**
 * @brief 数据可用性回调函数
 * @param reader 触发回调的数据读取器
 * 
 * 当订阅的主题收到新数据时被调用。根据读取器类型分别处理状态数据和采样点数据，
 * 将DDS数据转换为DataItem列表并通过信号和回调函数通知外部。
 */
void AQTSampleMachinePlug::on_data_available(DataReader* reader)
{
    if (reader == statusReader_) {
        SampleStatus status;
        SampleInfo info;  // 添加正确的SampleInfo声明
        
        // 问题3: while循环中的info变量未声明
        while (reader->take_next_sample(&status, &info) == ReturnCode_t::RETCODE_OK) {
            if (info.valid_data) {
                auto dataItems = convertStatusToDataItems(status);
                emit onDataUpdated(dataItems);
            }
        }
    }
    else if (reader == pointsReader_) {
        // 这部分代码正确
        SamplePoints points;
        SampleInfo info;
        while (reader->take_next_sample(&points, &info) == ReturnCode_t::RETCODE_OK) {
            if (info.valid_data) {
                auto dataItems = convertPointsToDataItems(points);
                emit onDataUpdated(dataItems);
            }
        }
    }
}

/**
 * @brief 将采样机状态转换为数据项列表
 * @param infos 采样机状态数据
 * @return 转换后的数据项列表
 * 
 * 按照预定义的数据点ID，将SampleInfo结构体中的状态信息转换为DataItem列表。
 * 包括设备状态、位置信息、限位信息等。
 */
QList<DataItem> AQTSampleMachinePlug::infoToDataItems(const SampleMachineInfo& infos)
{
    QList<DataItem> items;
    
    // 按照数据点定义的顺序转换状态数据
    items.append(DataItem(30, infos.maskBits()));
    items.append(DataItem(31, infos.bigPoint()));
    items.append(DataItem(32, infos.smallPoint()));
    items.append(DataItem(33, infos.depthPoint()));
    items.append(DataItem(34, infos.hasToXY()));
    items.append(DataItem(35, infos.hasSamplePoints()));
    items.append(DataItem(36, infos.gameOver()));
    items.append(DataItem(37, infos.backState()));
    items.append(DataItem(38, static_cast<int32_t>(infos.sampleState())));
    items.append(DataItem(39, static_cast<int32_t>(infos.belt1State())));
    items.append(DataItem(40, static_cast<int32_t>(infos.belt2State())));
    items.append(DataItem(41, static_cast<int32_t>(infos.crush1State())));
    items.append(DataItem(42, static_cast<int32_t>(infos.crush2State())));
    items.append(DataItem(43, static_cast<int32_t>(infos.dividerState())));
    items.append(DataItem(44, infos.bigLimit()));
    items.append(DataItem(45, infos.bigZeroLimit()));
    items.append(DataItem(46, infos.smallLimit()));
    items.append(DataItem(47, infos.smallZeroLimit()));
    items.append(DataItem(48, infos.upLimit()));
    items.append(DataItem(49, infos.downLimit()));
    items.append(DataItem(50, infos.version()));
    
    return items;
}



SampleCommand AQTSampleMachinePlug::dataItemsToCommand(const QList<DataItem>& items)
{
    // 初始化命令对象
    SampleCommand command;
    command.maskBits(0);  // 初始化掩码为0

    // 遍历输入的数据项列表
    for (const auto& item : items) {
        // 根据数据项的ID进行不同的处理
        switch (item.ID) {
            case 11:  // 控制类型
                command.controlType(item.value.toUInt());
                command.maskBits(command.maskBits() | 0x00000002);  // 设置对应的掩码位
                break;
                
            case 12:  // 开始采样
                command.startSample(static_cast<SampleModule::CommandState>(item.value.toInt())); 
                command.maskBits(command.maskBits() | 0x00000004); 
                break;
                
            case 13:  // 停止采样
                command.stopSample(static_cast<SampleModule::CommandState>(item.value.toInt())); 
                command.maskBits(command.maskBits() | 0x00000008); 
                break;
                
            case 14:  // 复位
                command.reset(static_cast<SampleModule::CommandState>(item.value.toInt())); 
                command.maskBits(command.maskBits() | 0x00000010); 
                break;
                
            case 15:  // 位置检查
                command.positionCheck(static_cast<SampleModule::CommandState>(item.value.toInt())); 
                command.maskBits(command.maskBits() | 0x00000020); 
                break;
                
            case 16:  // 紧急上升
                command.emergencyUp(static_cast<SampleModule::CommandState>(item.value.toInt())); 
                command.maskBits(command.maskBits() | 0x00000040); 
                break;
                
            case 17:  // 紧急停止
                command.emergencyStop(static_cast<SampleModule::CommandState>(item.value.toInt())); 
                command.maskBits(command.maskBits() | 0x00000080); 
                break;
                
            case 18:  // 控制传送带1
                command.controlBelt1(static_cast<SampleModule::CommandState>(item.value.toInt())); 
                command.maskBits(command.maskBits() | 0x00000100); 
                break;
                
            case 19:  // 控制传送带2
                command.controlBelt2(static_cast<SampleModule::CommandState>(item.value.toInt())); 
                command.maskBits(command.maskBits() | 0x00000200); 
                break;
                
            case 20:  // 控制破碎机1
                command.controlCrush1(static_cast<SampleModule::CommandState>(item.value.toInt())); 
                command.maskBits(command.maskBits() | 0x00000400); 
                break;
                
            case 21:  // 控制破碎机2
                command.controlCrush2(static_cast<SampleModule::CommandState>(item.value.toInt())); 
                command.maskBits(command.maskBits() | 0x00000800); 
                break;
                
            case 22:  // 控制分流器
                command.controlDivider(static_cast<SampleModule::CommandState>(item.value.toInt())); 
                command.maskBits(command.maskBits() | 0x00001000); 
                break;
                
            case 23:  // 控制分煤器
                command.controlCoalDistributor(static_cast<SampleModule::CommandState>(item.value.toInt())); 
                command.maskBits(command.maskBits() | 0x00002000); 
                break;
                
            case 24:  // 减速比
                command.reductionRatio(item.value.toUInt()); 
                command.maskBits(command.maskBits() | 0x00004000); 
                break;
                
            case 25:  // 版本号
                command.version(item.value.toUInt()); 
                command.maskBits(command.maskBits() | 0x00008000); 
                break;
                
            default:  // 未知ID
                qWarning() << "Unknown command ID:" << item.ID;
                break;
        }
    }

    // 返回构建好的命令对象
    return command;
}
SampleConfig AQTSampleMachinePlug::dataItemsToConfig(const QList<DataItem>& items)
{
    SampleConfig config;
    for (const auto& item : items) {
        switch (item.ID) {
            case 1: config.unifiedDepth(item.value.toUInt()); break;
            case 2: config.sampleOrder(item.value.toUInt()); break;
            // 添加其他配置项的转换
            default: break;
        }
    }
    return config;
}

SamplePoints AQTSampleMachinePlug::dataItemsToPoints(const QList<DataItem>& items)
{
    SamplePoints points;
    for (const auto& item : items) {
        switch (item.ID) {
            // 从51开始添加新的数据项
            case 51: points.totalPoints(item.value.toUInt()); break;
            case 52: points.x1(item.value.toUInt()); break;
            case 53: points.y1(item.value.toUInt()); break;
            case 54: points.z1(item.value.toUInt()); break;
            case 55: points.o1(static_cast<SampleModule::DropChoice>(item.value.toInt())); break;
            case 56: points.x2(item.value.toUInt()); break;
            case 57: points.y2(item.value.toUInt()); break;
            case 58: points.z2(item.value.toUInt()); break;
            case 59: points.o2(static_cast<SampleModule::DropChoice>(item.value.toInt())); break;
            case 60: points.x3(item.value.toUInt()); break;
            case 61: points.y3(item.value.toUInt()); break;
            case 62: points.z3(item.value.toUInt()); break;
            case 63: points.o3(static_cast<SampleModule::DropChoice>(item.value.toInt())); break;
            case 64: points.x4(item.value.toUInt()); break;
            case 65: points.y4(item.value.toUInt()); break;
            case 66: points.z4(item.value.toUInt()); break;
            case 67: points.o4(static_cast<SampleModule::DropChoice>(item.value.toInt())); break;
            case 68: points.x5(item.value.toUInt()); break;
            case 69: points.y5(item.value.toUInt()); break;
            case 70: points.z5(item.value.toUInt()); break;
            case 71: points.o5(static_cast<SampleModule::DropChoice>(item.value.toInt())); break;
            case 72: points.x6(item.value.toUInt()); break;
            case 73: points.y6(item.value.toUInt()); break;
            case 74: points.z6(item.value.toUInt()); break;
            case 75: points.o6(static_cast<SampleModule::DropChoice>(item.value.toInt())); break;

            default:
                qWarning() << "Unknown command ID:" << item.ID;
                break;
        }
    }

    return points;
}