【FastDDS】DDS-XTypes动态类型使用：编写类似 ROS2 的topic pub命令行工具

最新推荐文章于 2025-12-05 17:28:17 发布

原创最新推荐文章于 2025-12-05 17:28:17 发布 · 839 阅读

10 ·

CC 4.0 BY-SA版权

文章标签：

#FastDDS #ROS2 #DDS-XTypes #中间件

ROS2 从入门到进阶专栏收录该内容

20 篇文章

订阅专栏

一、动态类型讲解

FastDDS的动态类型数据发现是基于DDS-XTypes规范实现的核心功能，允许节点在没有预编译类型定义的情况下，动态发现、理解和交换数据。其实现机制可以分为以下几个关键部分：

1. 动态类型的表示与标识

FastDDS通过以下结构实现类型的标准化描述：

TypeIdentifier：类型的唯一标识，由类型名称、命名空间和哈希值组成，用于快速类型匹配
TypeObject：包含完整的类型元数据，包括字段定义、数据类型、约束条件等详细信息
TypeInformation：关联TypeIdentifier和TypeObject，作为类型发现的入口点

这些结构采用二进制格式编码，确保高效传输和跨平台兼容性。

2. 动态类型发现的完整流程

（1）类型注册与发布

当发布者使用动态类型时，会先将类型注册到本地TypeRegistry
创建DataWriter时，FastDDS自动将TypeInformation和TypeObject通过元发现通道广播
元数据通过DDS规范定义的内置话题（如DCPSPublication）传播，所有节点都能接收

（2）类型发现与请求

订阅者收到发布者的元数据后，首先检查本地TypeRegistry中是否存在匹配的TypeIdentifier
若不存在，订阅者会通过类型请求机制向发布者请求完整的TypeObject
发布者收到请求后，通过专用响应通道发送完整的TypeObject

（3）类型验证与兼容检查

订阅者接收TypeObject后，进行完整性验证
根据DDS-XTypes规则检查类型兼容性：
- 基础类型必须完全匹配
- 结构化类型支持有限扩展（可添加新字段，但不能修改或删除已有字段）
- 集合类型的元素类型和约束需兼容

（4）动态数据处理

验证通过后，订阅者基于TypeObject构建内存中的类型描述
创建DynamicData对象用于数据的接收、存储和访问
通过反射接口（如get_int32_value()、set_string_value()）动态操作数据字段

3. 关键组件与API

DynamicTypeBuilder：用于动态构建类型定义，支持添加成员、设置类型属性等
TypeRegistry：本地类型缓存，管理已发现和注册的所有类型
DynamicData：动态数据容器，提供运行时字段访问接口
TypeObjectFactory：负责TypeObject与具体类型之间的转换

以下是一个简单示例，展示动态类型的创建和发布过程：

// 创建动态类型构建器
auto builder = DynamicTypeBuilderFactory::get_instance()->create_struct_builder();
builder->set_name("DynamicMessage");

// 添加字段
builder->add_member(0, "timestamp", DynamicTypeBuilderFactory::get_instance()->create_uint64_type());
builder->add_member(1, "value", DynamicTypeBuilderFactory::get_instance()->create_float_type());

// 构建动态类型
DynamicType_ptr dynamic_type = builder->build();

// 注册类型到参与者
TypeSupport type_support(dynamic_type);
type_support.register_type(participant, "DynamicMessage");

// 创建话题和DataWriter
Topic* topic = participant->create_topic("dynamic_topic", "DynamicMessage", TOPIC_QOS_DEFAULT);
DataWriter* writer = publisher->create_datawriter(topic, DATAWRITER_QOS_DEFAULT);

// 创建并填充动态数据
DynamicData_ptr data = dynamic_type->create_data();
data->set_uint64_value(123456789, 0);  // 设置timestamp字段
data->set_float_value(3.14f, 1);       // 设置value字段

// 发布数据
writer->write(data);

4. 动态发现的优势与应用场景

跨系统集成：不同开发团队或平台可动态交换数据，无需统一预编译类型
类型演化：系统可在不中断通信的情况下逐步扩展数据类型
脚本化交互：支持Python等脚本语言动态处理数据，无需代码生成
调试与监控：工具可动态解析任意话题数据，无需了解类型细节

FastDDS的动态类型发现机制在保持DDS原有可靠性和性能的同时，极大提升了系统的灵活性和互操作性，特别适合复杂分布式系统的动态集成场景。

5、利用动态类型如何写一个类似ros2 topic pub的命令行工具

二、利用动态类型写一个类似ros2 topic pub的命令行工具

#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
#include <fastdds/dds/domain/DomainParticipant.hpp>
#include <fastdds/dds/publisher/Publisher.hpp>
#include <fastdds/dds/publisher/DataWriter.hpp>
#include <fastdds/dds/publisher/DataWriterListener.hpp>
#include <fastdds/dds/topic/Topic.hpp>
#include <fastdds/dds/core/status/StatusMask.hpp>
#include <fastrtps/types/DynamicTypeBuilderFactory.h>
#include <fastrtps/types/DynamicTypeBuilder.h>
#include <fastrtps/types/DynamicType.h>
#include <fastrtps/types/DynamicData.h>
#include <fastrtps/types/TypeIdentifier.h>
#include <fastrtps/types/TypeObjectFactory.h>
#include <fastrtps/types/TypeRegistry.h>

#include <fstream>
#include <sstream>
#include <vector>
#include <map>
#include <string>
#include <cstring>

using namespace eprosima::fastdds::dds;
using namespace eprosima::fastrtps::types;

// 解析消息类型文件，返回动态类型构建器
DynamicTypeBuilder_ptr parse_type_file(const std::string& filename)
{
    std::ifstream file(filename);
    if (!file.is_open())
    {
        std::cerr << "无法打开类型文件: " << filename << std::endl;
        return nullptr;
    }

    std::stringstream buffer;
    buffer << file.rdbuf();
    std::string content = buffer.str();

    // 简单解析示例：假设文件格式为:
    // struct <类型名> {
    //   <类型> <字段名>;
    //   ...
    // }
    // 实际应用中可能需要更复杂的解析器
    
    size_t struct_pos = content.find("struct");
    if (struct_pos == std::string::npos)
    {
        std::cerr << "类型文件格式错误，未找到struct关键字" << std::endl;
        return nullptr;
    }
    
    size_t name_start = content.find_first_not_of(" \t", struct_pos + 6);
    size_t name_end = content.find_first_of(" \t{\n", name_start);
    std::string type_name = content.substr(name_start, name_end - name_start);
    
    auto builder = DynamicTypeBuilderFactory::get_instance()->create_struct_builder();
    builder->set_name(type_name);
    
    size_t brace_start = content.find("{", name_end);
    size_t brace_end = content.find("}", brace_start);
    std::string fields_content = content.substr(brace_start + 1, brace_end - brace_start - 1);
    
    std::istringstream fields_stream(fields_content);
    std::string line;
    uint32_t member_id = 0;
    
    while (std::getline(fields_stream, line))
    {
        // 跳过空行和注释
        size_t comment_pos = line.find("//");
        if (comment_pos != std::string::npos)
        {
            line = line.substr(0, comment_pos);
        }
        if (line.empty())
            continue;
            
        // 解析字段类型和名称
        size_t type_end = line.find_first_of(" \t");
        if (type_end == std::string::npos)
            continue;
            
        std::string field_type = line.substr(0, type_end);
        size_t name_start = line.find_first_not_of(" \t", type_end);
        size_t name_end = line.find_first_of(" ;\t", name_start);
        if (name_end == std::string::npos)
            continue;
            
        std::string field_name = line.substr(name_start, name_end - name_start);
        
        // 根据类型创建对应的DynamicType
        DynamicTypeBuilder_ptr field_builder;
        
        if (field_type == "int32")
        {
            field_builder = DynamicTypeBuilderFactory::get_instance()->create_int32_type();
        }
        else if (field_type == "uint32")
        {
            field_builder = DynamicTypeBuilderFactory::get_instance()->create_uint32_type();
        }
        else if (field_type == "float")
        {
            field_builder = DynamicTypeBuilderFactory::get_instance()->create_float_type();
        }
        else if (field_type == "double")
        {
            field_builder = DynamicTypeBuilderFactory::get_instance()->create_double_type();
        }
        else if (field_type == "string")
        {
            field_builder = DynamicTypeBuilderFactory::get_instance()->create_string_type();
        }
        else
        {
            std::cerr << "不支持的字段类型: " << field_type << std::endl;
            continue;
        }
        
        // 添加成员到结构体
        builder->add_member(member_id++, field_name, field_builder);
    }
    
    return builder;
}

// 解析数据字符串到DynamicData
bool parse_data_string(DynamicData_ptr data, const std::string& data_str)
{
    // 数据格式示例: "field1:value1,field2:value2,..."
    std::map<std::string, std::string> field_values;
    std::stringstream ss(data_str);
    std::string pair;
    
    while (std::getline(ss, pair, ','))
    {
        size_t colon_pos = pair.find(':');
        if (colon_pos == std::string::npos)
        {
            std::cerr << "数据格式错误: " << pair << std::endl;
            return false;
        }
        
        std::string field = pair.substr(0, colon_pos);
        std::string value = pair.substr(colon_pos + 1);
        field_values[field] = value;
    }
    
    // 获取类型信息
    const DynamicType* type = data->get_type();
    std::vector<MemberId> member_ids;
    type->get_all_members(member_ids);
    
    for (MemberId id : member_ids)
    {
        std::string member_name;
        type->get_member_name(id, member_name);
        
        auto it = field_values.find(member_name);
        if (it == field_values.end())
        {
            std::cerr << "警告: 未提供字段 " << member_name << " 的值" << std::endl;
            continue;
        }
        
        std::string value = it->second;
        TypeKind kind = type->get_member_type(id)->get_kind();
        
        // 根据字段类型设置值
        switch (kind)
        {
            case TK_INT32:
                data->set_int32_value(std::stoi(value), id);
                break;
            case TK_UINT32:
                data->set_uint32_value(std::stoul(value), id);
                break;
            case TK_FLOAT:
                data->set_float_value(std::stof(value), id);
                break;
            case TK_DOUBLE:
                data->set_double_value(std::stod(value), id);
                break;
            case TK_STRING:
                data->set_string_value(value, id);
                break;
            default:
                std::cerr << "不支持的字段类型，字段名: " << member_name << std::endl;
                return false;
        }
    }
    
    return true;
}

class PubListener : public DataWriterListener
{
public:
    PubListener() : matched_(0) {}

    ~PubListener() override {}

    void on_publication_matched(
            DataWriter* writer,
            const PublicationMatchedStatus& info) override
    {
        if (info.current_count_change == 1)
        {
            matched_ = info.total_count;
            std::cout << "发布者与订阅者匹配成功" << std::endl;
        }
        else if (info.current_count_change == -1)
        {
            matched_ = info.total_count;
            std::cout << "发布者与订阅者匹配断开" << std::endl;
        }
        else
        {
            std::cout << "on_publication_matched: 未知状态变化 " << info.current_count_change << std::endl;
        }
    }

    int matched_;
};

int main(int argc, char* argv[])
{
    // 检查命令行参数
    if (argc != 4)
    {
        std::cerr << "用法: " << argv[0] << " <话题名称> <类型文件> <数据>" << std::endl;
        std::cerr << "示例: " << argv[0] << " /chatter message_type.txt \"id:1,content:hello\"" << std::endl;
        return 1;
    }

    std::string topic_name = argv[1];
    std::string type_file = argv[2];
    std::string data_str = argv[3];

    // 解析类型文件
    DynamicTypeBuilder_ptr type_builder = parse_type_file(type_file);
    if (!type_builder)
    {
        return 1;
    }

    // 构建动态类型
    DynamicType_ptr dynamic_type = type_builder->build();
    if (!dynamic_type)
    {
        std::cerr << "构建动态类型失败" << std::endl;
        return 1;
    }

    // 创建DomainParticipant
    DomainParticipant* participant = DomainParticipantFactory::get_instance()->create_participant(0, PARTICIPANT_QOS_DEFAULT);
    if (!participant)
    {
        std::cerr << "创建DomainParticipant失败" << std::endl;
        return 1;
    }

    // 注册类型
    TypeSupport type_support(dynamic_type);
    std::string type_name = dynamic_type->get_name();
    if (type_support.register_type(participant, type_name) != ReturnCode_t::RETCODE_OK)
    {
        std::cerr << "注册类型失败" << std::endl;
        DomainParticipantFactory::get_instance()->delete_participant(participant);
        return 1;
    }

    // 创建Publisher
    Publisher* publisher = participant->create_publisher(PUBLISHER_QOS_DEFAULT, nullptr);
    if (!publisher)
    {
        std::cerr << "创建Publisher失败" << std::endl;
        participant->delete_contained_entities();
        DomainParticipantFactory::get_instance()->delete_participant(participant);
        return 1;
    }

    // 创建Topic
    Topic* topic = participant->create_topic(
        topic_name,
        type_name,
        TOPIC_QOS_DEFAULT);
    if (!topic)
    {
        std::cerr << "创建Topic失败" << std::endl;
        participant->delete_contained_entities();
        DomainParticipantFactory::get_instance()->delete_participant(participant);
        return 1;
    }

    // 创建DataWriter
    PubListener listener;
    DataWriter* writer = publisher->create_datawriter(
        topic,
        DATAWRITER_QOS_DEFAULT,
        &listener,
        StatusMask::publication_matched());
    if (!writer)
    {
        std::cerr << "创建DataWriter失败" << std::endl;
        participant->delete_contained_entities();
        DomainParticipantFactory::get_instance()->delete_participant(participant);
        return 1;
    }

    // 创建动态数据并填充
    DynamicData_ptr data = dynamic_type->create_data();
    if (!parse_data_string(data, data_str))
    {
        std::cerr << "解析数据失败" << std::endl;
        participant->delete_contained_entities();
        DomainParticipantFactory::get_instance()->delete_participant(participant);
        return 1;
    }

    // 等待匹配
    std::cout << "等待订阅者连接..." << std::endl;
    while (listener.matched_ == 0)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    // 发布数据
    std::cout << "发布数据到话题 " << topic_name << std::endl;
    if (writer->write(data) != ReturnCode_t::RETCODE_OK)
    {
        std::cerr << "发布数据失败" << std::endl;
        participant->delete_contained_entities();
        DomainParticipantFactory::get_instance()->delete_participant(participant);
        return 1;
    }

    // 短暂等待确保数据发送完成
    std::this_thread::sleep_for(std::chrono::seconds(1));

    // 清理资源
    participant->delete_contained_entities();
    DomainParticipantFactory::get_instance()->delete_participant(participant);

    std::cout << "数据发布完成" << std::endl;
    return 0;
}

创建一个类似于ros2 topic pub的FastDDS动态类型发布工具，该工具可以指定话题名称、消息结构体类型文件和具体数据。

这个实现将包括：

从文件加载消息类型定义
解析命令行参数（话题名、类型文件、数据）
使用FastDDS动态类型API创建类型和数据
发布数据到指定话题

代码说明

这个FastDDS动态发布工具的工作流程如下：

命令行参数解析：接收三个参数（话题名称、类型文件路径、数据字符串）
类型文件解析：
- 从指定文件加载消息类型定义
- 支持基本数据类型（int32, uint32, float, double, string）
- 构建DynamicTypeBuilder用于创建动态类型
数据解析：
- 解析类似"field1:value1,field2:value2"格式的字符串
- 根据类型信息将值设置到对应的字段
DDS实体创建：
- 创建DomainParticipant、Publisher、Topic和DataWriter
- 注册动态类型
- 设置监听器以检测与订阅者的匹配状态
数据发布：
- 等待至少一个订阅者连接
- 发布数据并清理资源

使用示例

创建一个消息类型文件（例如message_type.txt）：

struct Message {
    int32 id;       // 消息ID
    string content; // 消息内容
    float value;    // 数值
};

编译程序后运行：

./fastdds_dynamic_pub /chatter message_type.txt "id:1,content:hello,value:3.14"

与ROS2 topic pub的区别

直接使用FastDDS API，没有ROS2的中间层（rmw等）
类型定义使用自定义格式文件，而非ROS2的.msg文件
数据格式更简洁，直接使用键值对
缺少ROS2的一些高级特性（如QoS配置的命令行参数）

这个实现展示了FastDDS动态类型的核心功能，你可以根据需要扩展它，比如添加QoS配置、重复发布等功能。