FastDDS基础教程 - DDS标准实现与ROS2集成

FastDDS基础教程 - DDS标准实现与ROS2集成

前言

FastDDS是eProsima开发的DDS（Data Distribution Service）标准的高性能实现，也是ROS2的默认中间件。本文将深入介绍FastDDS的基本概念、架构设计、配置方法以及与ROS2的集成使用。

FastDDS架构概述

整体架构

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
┌─────────────────────────────────────────────────────────────┐
│                    FastDDS Architecture                     │
├─────────────────────────────────────────────────────────────┤
│  Application Layer                                          │
│  ┌─────────────┐  ┌─────────────┐  ┌─────────────┐         │
│  │   Publisher │  │  Subscriber │  │   Service   │         │
│  └─────────────┘  └─────────────┘  └─────────────┘         │
├─────────────────────────────────────────────────────────────┤
│  DDS Layer                                                  │
│  ┌─────────────┐  ┌─────────────┐  ┌─────────────┐         │
│  │ Domain      │  │ Participant │  │ QoS Policy  │         │
│  └─────────────┘  └─────────────┘  └─────────────┘         │
├─────────────────────────────────────────────────────────────┤
│  RTPS Layer                                                 │
│  ┌─────────────┐  ┌─────────────┐  ┌─────────────┐         │
│  │ Discovery   │  │ Transport   │  │ Security    │         │
│  └─────────────┘  └─────────────┘  └─────────────┘         │
├─────────────────────────────────────────────────────────────┤
│  Transport Layer                                            │
│  ┌─────────────┐  ┌─────────────┐  ┌─────────────┐         │
│  │   UDPv4     │  │   TCPv4     │  │ Shared Mem  │         │
│  └─────────────┘  └─────────────┘  └─────────────┘         │
└─────────────────────────────────────────────────────────────┘

FastDDS核心概念

1. 域（Domain）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
#include <fastdds/dds/domain/DomainParticipant.hpp>
#include <fastdds/dds/domain/DomainParticipantFactory.hpp>

class FastDDSDomainDemo {
public:
    FastDDSDomainDemo() {
        // 创建域参与者工厂
        factory_ = eprosima::fastdds::dds::DomainParticipantFactory::get_instance();
        
        // 创建域参与者
        participant_ = factory_->create_participant(
            DOMAIN_ID, 
            eprosima::fastdds::dds::PARTICIPANT_QOS_DEFAULT);
        
        if (participant_ == nullptr) {
            throw std::runtime_error("Failed to create domain participant");
        }
        
        std::cout << "Domain participant created in domain " << DOMAIN_ID << std::endl;
    }
    
    ~FastDDSDomainDemo() {
        if (participant_) {
            factory_->delete_participant(participant_);
        }
    }
    
    eprosima::fastdds::dds::DomainParticipant* get_participant() {
        return participant_;
    }

private:
    static constexpr uint32_t DOMAIN_ID = 0;
    
    eprosima::fastdds::dds::DomainParticipantFactory* factory_;
    eprosima::fastdds::dds::DomainParticipant* participant_;
};

2. 发布者（Publisher）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
#include <fastdds/dds/publisher/Publisher.hpp>
#include <fastdds/dds/publisher/DataWriter.hpp>
#include <fastdds/dds/topic/TypeSupport.hpp>

template<typename T>
class FastDDSPublisher {
public:
    FastDDSPublisher(eprosima::fastdds::dds::DomainParticipant* participant,
                     const std::string& topic_name) 
        : participant_(participant), topic_name_(topic_name) {
        
        // 注册数据类型
        type_support_ = new eprosima::fastdds::dds::TypeSupport(new T());
        participant_->register_type(type_support_);
        
        // 创建主题
        topic_ = participant_->create_topic(
            topic_name_, 
            type_support_->get_type_name(),
            eprosima::fastdds::dds::TOPIC_QOS_DEFAULT);
        
        if (topic_ == nullptr) {
            throw std::runtime_error("Failed to create topic");
        }
        
        // 创建发布者
        publisher_ = participant_->create_publisher(
            eprosima::fastdds::dds::PUBLISHER_QOS_DEFAULT);
        
        if (publisher_ == nullptr) {
            throw std::runtime_error("Failed to create publisher");
        }
        
        // 创建数据写入器
        writer_ = publisher_->create_datawriter(
            topic_, 
            eprosima::fastdds::dds::DATAWRITER_QOS_DEFAULT);
        
        if (writer_ == nullptr) {
            throw std::runtime_error("Failed to create data writer");
        }
        
        std::cout << "Publisher created for topic: " << topic_name_ << std::endl;
    }
    
    ~FastDDSPublisher() {
        if (publisher_) {
            participant_->delete_publisher(publisher_);
        }
        if (topic_) {
            participant_->delete_topic(topic_);
        }
        delete type_support_;
    }
    
    bool publish(const T& data) {
        return writer_->write(&data) == eprosima::fastrtps::types::ReturnCode_t::RETCODE_OK;
    }
    
    eprosima::fastdds::dds::DataWriter* get_writer() {
        return writer_;
    }

private:
    eprosima::fastdds::dds::DomainParticipant* participant_;
    std::string topic_name_;
    eprosima::fastdds::dds::TypeSupport* type_support_;
    eprosima::fastdds::dds::Topic* topic_;
    eprosima::fastdds::dds::Publisher* publisher_;
    eprosima::fastdds::dds::DataWriter* writer_;
};

3. 订阅者（Subscriber）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
#include <fastdds/dds/subscriber/Subscriber.hpp>
#include <fastdds/dds/subscriber/DataReader.hpp>
#include <fastdds/dds/subscriber/DataReaderListener.hpp>

template<typename T>
class FastDDSSubscriber : public eprosima::fastdds::dds::DataReaderListener {
public:
    FastDDSSubscriber(eprosima::fastdds::dds::DomainParticipant* participant,
                      const std::string& topic_name) 
        : participant_(participant), topic_name_(topic_name) {
        
        // 注册数据类型
        type_support_ = new eprosima::fastdds::dds::TypeSupport(new T());
        participant_->register_type(type_support_);
        
        // 创建主题
        topic_ = participant_->create_topic(
            topic_name_, 
            type_support_->get_type_name(),
            eprosima::fastdds::dds::TOPIC_QOS_DEFAULT);
        
        if (topic_ == nullptr) {
            throw std::runtime_error("Failed to create topic");
        }
        
        // 创建订阅者
        subscriber_ = participant_->create_subscriber(
            eprosima::fastdds::dds::SUBSCRIBER_QOS_DEFAULT);
        
        if (subscriber_ == nullptr) {
            throw std::runtime_error("Failed to create subscriber");
        }
        
        // 创建数据读取器
        reader_ = subscriber_->create_datareader(
            topic_, 
            eprosima::fastdds::dds::DATAREADER_QOS_DEFAULT,
            this);
        
        if (reader_ == nullptr) {
            throw std::runtime_error("Failed to create data reader");
        }
        
        std::cout << "Subscriber created for topic: " << topic_name_ << std::endl;
    }
    
    ~FastDDSSubscriber() {
        if (subscriber_) {
            participant_->delete_subscriber(subscriber_);
        }
        if (topic_) {
            participant_->delete_topic(topic_);
        }
        delete type_support_;
    }
    
    // DataReaderListener回调
    void on_data_available(eprosima::fastdds::dds::DataReader* reader) override {
        eprosima::fastdds::dds::SampleInfo info;
        T sample;
        
        while (reader->take_next_sample(&sample, &info) == 
               eprosima::fastrtps::types::ReturnCode_t::RETCODE_OK) {
            if (info.valid_data) {
                on_data_received(sample);
            }
        }
    }
    
    virtual void on_data_received(const T& data) = 0;

private:
    eprosima::fastdds::dds::DomainParticipant* participant_;
    std::string topic_name_;
    eprosima::fastdds::dds::TypeSupport* type_support_;
    eprosima::fastdds::dds::Topic* topic_;
    eprosima::fastdds::dds::Subscriber* subscriber_;
    eprosima::fastdds::dds::DataReader* reader_;
};

自定义数据类型

IDL定义和代码生成

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
// HelloWorld.idl
module HelloWorld {
    struct HelloWorld {
        unsigned long index;
        string message;
    };
};

// RobotStatus.idl
module RobotControl {
    struct Pose {
        double x;
        double y;
        double z;
        double yaw;
        double pitch;
        double roll;
    };
    
    struct RobotStatus {
        unsigned long robot_id;
        Pose current_pose;
        double battery_level;
        boolean emergency_stop;
        sequence<double> sensor_data;
    };
};

使用生成的类型

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
#include "HelloWorld.h"
#include "RobotStatus.h"

class CustomDataTypeDemo {
public:
    CustomDataTypeDemo() {
        // 创建域参与者
        participant_ = factory_->create_participant(
            DOMAIN_ID, 
            eprosima::fastdds::dds::PARTICIPANT_QOS_DEFAULT);
        
        // 创建发布者和订阅者
        setup_publisher();
        setup_subscriber();
    }
    
    void setup_publisher() {
        // 注册HelloWorld类型
        hello_type_support_ = new eprosima::fastdds::dds::TypeSupport(
            new HelloWorld::HelloWorldPubSubType());
        participant_->register_type(hello_type_support_);
        
        // 创建主题
        hello_topic_ = participant_->create_topic(
            "HelloWorldTopic", 
            hello_type_support_->get_type_name(),
            eprosima::fastdds::dds::TOPIC_QOS_DEFAULT);
        
        // 创建发布者
        publisher_ = participant_->create_publisher(
            eprosima::fastdds::dds::PUBLISHER_QOS_DEFAULT);
        
        // 创建数据写入器
        hello_writer_ = publisher_->create_datawriter(
            hello_topic_, 
            eprosima::fastdds::dds::DATAWRITER_QOS_DEFAULT);
    }
    
    void setup_subscriber() {
        // 注册RobotStatus类型
        robot_type_support_ = new eprosima::fastdds::dds::TypeSupport(
            new RobotControl::RobotStatusPubSubType());
        participant_->register_type(robot_type_support_);
        
        // 创建主题
        robot_topic_ = participant_->create_topic(
            "RobotStatusTopic", 
            robot_type_support_->get_type_name(),
            eprosima::fastdds::dds::TOPIC_QOS_DEFAULT);
        
        // 创建订阅者
        subscriber_ = participant_->create_subscriber(
            eprosima::fastdds::dds::SUBSCRIBER_QOS_DEFAULT);
        
        // 创建数据读取器
        robot_reader_ = subscriber_->create_datareader(
            robot_topic_, 
            eprosima::fastdds::dds::DATAREADER_QOS_DEFAULT,
            &robot_listener_);
    }
    
    void publish_hello_world() {
        HelloWorld::HelloWorld sample;
        sample.index(hello_count_++);
        sample.message("Hello FastDDS!");
        
        hello_writer_->write(&sample);
        std::cout << "Published HelloWorld: " << sample.index() 
                  << " - " << sample.message() << std::endl;
    }
    
    void publish_robot_status() {
        RobotControl::RobotStatus sample;
        sample.robot_id(1);
        sample.current_pose().x(1.0);
        sample.current_pose().y(2.0);
        sample.current_pose().z(0.0);
        sample.current_pose().yaw(0.5);
        sample.current_pose().pitch(0.0);
        sample.current_pose().roll(0.0);
        sample.battery_level(85.5);
        sample.emergency_stop(false);
        
        // 填充传感器数据
        sample.sensor_data().resize(10);
        for (int i = 0; i < 10; ++i) {
            sample.sensor_data()[i] = 1.0 + i * 0.1;
        }
        
        robot_writer_->write(&sample);
        std::cout << "Published RobotStatus: ID=" << sample.robot_id() 
                  << ", Battery=" << sample.battery_level() << "%" << std::endl;
    }

private:
    static constexpr uint32_t DOMAIN_ID = 0;
    
    eprosima::fastdds::dds::DomainParticipantFactory* factory_;
    eprosima::fastdds::dds::DomainParticipant* participant_;
    
    // HelloWorld相关
    eprosima::fastdds::dds::TypeSupport* hello_type_support_;
    eprosima::fastdds::dds::Topic* hello_topic_;
    eprosima::fastdds::dds::DataWriter* hello_writer_;
    
    // RobotStatus相关
    eprosima::fastdds::dds::TypeSupport* robot_type_support_;
    eprosima::fastdds::dds::Topic* robot_topic_;
    eprosima::fastdds::dds::DataReader* robot_reader_;
    
    // 监听器
    class RobotStatusListener : public eprosima::fastdds::dds::DataReaderListener {
    public:
        void on_data_available(eprosima::fastdds::dds::DataReader* reader) override {
            eprosima::fastdds::dds::SampleInfo info;
            RobotControl::RobotStatus sample;
            
            while (reader->take_next_sample(&sample, &info) == 
                   eprosima::fastrtps::types::ReturnCode_t::RETCODE_OK) {
                if (info.valid_data) {
                    std::cout << "Received RobotStatus: ID=" << sample.robot_id() 
                              << ", Pose=(" << sample.current_pose().x() 
                              << ", " << sample.current_pose().y() << ")" << std::endl;
                }
            }
        }
    } robot_listener_;
    
    uint32_t hello_count_ = 0;
};

QoS配置详解

服务质量策略配置

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
#include <fastdds/dds/core/policy/QosPolicies.hpp>

class QoSConfigurationDemo {
public:
    QoSConfigurationDemo() {
        // 配置可靠性QoS
        configure_reliability_qos();
        
        // 配置持久性QoS
        configure_durability_qos();
        
        // 配置实时性QoS
        configure_deadline_qos();
        
        // 配置生命周期QoS
        configure_liveliness_qos();
    }
    
    void configure_reliability_qos() {
        // 可靠传输配置
        reliable_qos_.reliability().kind = eprosima::fastdds::dds::RELIABLE_RELIABILITY_QOS;
        reliable_qos_.reliability().max_blocking_time = {1, 0}; // 1秒
        
        // 尽力而为传输配置
        best_effort_qos_.reliability().kind = eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS;
        
        std::cout << "Reliability QoS configured" << std::endl;
    }
    
    void configure_durability_qos() {
        // 易失性持久性
        volatile_qos_.durability().kind = eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS;
        
        // 瞬态本地持久性
        transient_local_qos_.durability().kind = eprosima::fastdds::dds::TRANSIENT_LOCAL_DURABILITY_QOS;
        
        // 瞬态持久性
        transient_qos_.durability().kind = eprosima::fastdds::dds::TRANSIENT_DURABILITY_QOS;
        
        // 持久性持久性
        persistent_qos_.durability().kind = eprosima::fastdds::dds::PERSISTENT_DURABILITY_QOS;
        
        std::cout << "Durability QoS configured" << std::endl;
    }
    
    void configure_deadline_qos() {
        // 设置截止时间
        deadline_qos_.deadline().period = {0, 50000000}; // 50ms
        
        std::cout << "Deadline QoS configured: 50ms" << std::endl;
    }
    
    void configure_liveliness_qos() {
        // 自动生命周期
        auto_liveliness_qos_.liveliness().kind = eprosima::fastdds::dds::AUTOMATIC_LIVELINESS_QOS;
        auto_liveliness_qos_.liveliness().lease_duration = {1, 0}; // 1秒
        
        // 手动生命周期
        manual_liveliness_qos_.liveliness().kind = eprosima::fastdds::dds::MANUAL_BY_PARTICIPANT_LIVELINESS_QOS;
        manual_liveliness_qos_.liveliness().lease_duration = {2, 0}; // 2秒
        
        std::cout << "Liveliness QoS configured" << std::endl;
    }
    
    void create_optimized_publisher() {
        // 创建优化的发布者QoS
        eprosima::fastdds::dds::PublisherQos pub_qos;
        
        // 设置分区
        pub_qos.partition().push_back("robot_control");
        pub_qos.partition().push_back("sensors");
        
        // 设置用户数据
        pub_qos.user_data().push_back(0x01);
        pub_qos.user_data().push_back(0x02);
        pub_qos.user_data().push_back(0x03);
        
        std::cout << "Optimized publisher QoS created" << std::endl;
    }
    
    void create_optimized_subscriber() {
        // 创建优化的订阅者QoS
        eprosima::fastdds::dds::SubscriberQos sub_qos;
        
        // 设置分区
        sub_qos.partition().push_back("robot_control");
        
        // 设置组数据
        sub_qos.group_data().push_back(0x10);
        sub_qos.group_data().push_back(0x20);
        
        std::cout << "Optimized subscriber QoS created" << std::endl;
    }

private:
    eprosima::fastdds::dds::DataWriterQos reliable_qos_;
    eprosima::fastdds::dds::DataWriterQos best_effort_qos_;
    eprosima::fastdds::dds::DataWriterQos volatile_qos_;
    eprosima::fastdds::dds::DataWriterQos transient_local_qos_;
    eprosima::fastdds::dds::DataWriterQos transient_qos_;
    eprosima::fastdds::dds::DataWriterQos persistent_qos_;
    eprosima::fastdds::dds::DataWriterQos deadline_qos_;
    eprosima::fastdds::dds::DataWriterQos auto_liveliness_qos_;
    eprosima::fastdds::dds::DataWriterQos manual_liveliness_qos_;
};

与ROS2集成

ROS2中间件接口

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

class FastDDSROS2Integration : public rclcpp::Node {
public:
    FastDDSROS2Integration() : Node("fastdds_ros2_integration") {
        // 设置FastDDS作为ROS2中间件
        setup_fastdds_middleware();
        
        // 创建发布者和订阅者
        setup_publishers_subscribers();
        
        // 创建定时器
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&FastDDSROS2Integration::publish_message, this));
        
        RCLCPP_INFO(this->get_logger(), "FastDDS ROS2 integration started");
    }

private:
    void setup_fastdds_middleware() {
        // FastDDS配置
        rclcpp::QoS qos(10);
        qos.reliability(RMW_QOS_RELIABILITY_RELIABLE);
        qos.durability(RMW_QOS_DURABILITY_VOLATILE);
        
        qos_profile_ = qos;
    }
    
    void setup_publishers_subscribers() {
        // 创建发布者
        publisher_ = this->create_publisher<std_msgs::msg::String>(
            "/fastdds/demo", qos_profile_);
        
        // 创建订阅者
        subscription_ = this->create_subscription<std_msgs::msg::String>(
            "/fastdds/demo", qos_profile_,
            std::bind(&FastDDSROS2Integration::message_callback, this, std::placeholders::_1));
    }
    
    void publish_message() {
        auto msg = std_msgs::msg::String();
        msg.data = "FastDDS ROS2 message " + std::to_string(message_count_++);
        publisher_->publish(msg);
        
        RCLCPP_INFO(this->get_logger(), "Published: %s", msg.data.c_str());
    }
    
    void message_callback(const std_msgs::msg::String::SharedPtr msg) {
        RCLCPP_INFO(this->get_logger(), "Received: %s", msg->data.c_str());
    }
    
    rclcpp::QoS qos_profile_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
    rclcpp::TimerBase::SharedPtr timer_;
    
    int message_count_ = 0;
};

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<FastDDSROS2Integration>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

总结

FastDDS作为DDS标准的高性能实现，提供了：

  1. 完整的DDS实现：支持所有DDS核心功能
  2. 高性能通信：优化的传输层和序列化
  3. 灵活的QoS配置：支持多种服务质量策略
  4. ROS2集成：作为ROS2的默认中间件
  5. 跨平台支持：支持多种操作系统和架构

通过掌握FastDDS的基本概念和使用方法，可以更好地理解和优化ROS2应用程序的性能。