ROS2 時間相關 API： Time、Rate、Duration 的 C++ 實作

前言

在機器人開發中，時間管理是一個至關重要的概念。無論是控制機器人的運動頻率、記錄感測器數據的時間戳，還是實現複雜的時序邏輯，都需要精確的時間控制。ROS2 提供了完整的時間管理 API，包括 Time、Rate、Duration 等核心類別，讓開發者能夠輕鬆處理各種時間相關的任務。

本文將深入探討這些 API 的使用方式，並提供實際的 C++ 程式碼範例，幫助您在 ROS2 專案中有效運用時間管理功能。

1. Duration（持續時間）- 時間間隔的精確控制

基本概念

Duration 表示一段時間間隔，可以是正值或負值。它在 ROS2 中廣泛用於計時、延遲和時間計算。Duration 的內部表示包含秒數和奈秒數兩個部分，提供了高精度的時間控制。

C++ 實作範例

讓我們看看如何在實際專案中使用 Duration：

#include "rclcpp/rclcpp.hpp"

class MyDuration: public rclcpp::Node {
public:
    MyDuration(): Node("my_duration_node") {
        demonstrate_duration_usage();
    }

private:
    void demonstrate_duration_usage() {
        // 方法1: 使用 chrono 創建 Duration
        rclcpp::Duration duration0(std::chrono::seconds(1));
        
        // 方法2: 使用秒和奈秒創建 Duration
        rclcpp::Duration duration1(5, 500000000);  // 5.5秒
        
        // 輸出 Duration 資訊
        RCLCPP_INFO(this->get_logger(), 
                   "duration0: %.6f seconds, %ld nanoseconds", 
                   duration0.seconds(), duration0.nanoseconds());
        
        RCLCPP_INFO(this->get_logger(), 
                   "duration1: %.6f seconds, %ld nanoseconds", 
                   duration1.seconds(), duration1.nanoseconds());
        
        // Duration 運算範例
        demonstrate_duration_arithmetic();
    }
    
    void demonstrate_duration_arithmetic() {
        rclcpp::Duration d1(2, 0);        // 2秒
        rclcpp::Duration d2(1, 500000000); // 1.5秒
        
        // Duration 相加
        rclcpp::Duration sum = d1 + d2;
        RCLCPP_INFO(this->get_logger(), 
                   "Duration sum: %.6f seconds", sum.seconds());
        
        // Duration 相減
        rclcpp::Duration diff = d1 - d2;
        RCLCPP_INFO(this->get_logger(), 
                   "Duration difference: %.6f seconds", diff.seconds());
        
        // Duration 比較
        if (d1 > d2) {
            RCLCPP_INFO(this->get_logger(), "d1 is longer than d2");
        }
    }
};

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

使用場景

• 超時控制：設定服務呼叫或動作的超時時間
• 週期性任務：定義任務執行的時間間隔
• 性能測量：計算演算法執行時間
• 延遲處理：實現精確的時間延遲

2. Time（時間點）- 絕對時間的管理

基本概念

Time 表示特定的時間點，類似於「今天下午5點」這樣的絕對時間概念。在 ROS2 中，Time 不僅支援實際時間，還能與模擬時間無縫配合。

C++ 實作範例

#include "rclcpp/rclcpp.hpp"

class TimeDemo: public rclcpp::Node {
public:
    TimeDemo(): Node("time_demo") {
        demonstrate_time_usage();
    }

private:
    void demonstrate_time_usage() {
        // 獲取當前時間
        rclcpp::Time now = this->get_clock()->now();
        
        // 創建特定時間點
        rclcpp::Time specific_time(10, 500000000);  // 10.5秒
        
        RCLCPP_INFO(this->get_logger(), 
                   "Current time: %.6f seconds", now.seconds());
        RCLCPP_INFO(this->get_logger(), 
                   "Specific time: %.6f seconds", specific_time.seconds());
        
        // 時間運算範例
        demonstrate_time_arithmetic();
        
        // 時間比較範例
        demonstrate_time_comparison();
    }
    
    void demonstrate_time_arithmetic() {
        rclcpp::Time start_time = this->get_clock()->now();
        rclcpp::Duration delay(2, 0);  // 2秒延遲
        
        // Time + Duration = Time
        rclcpp::Time future_time = start_time + delay;
        
        RCLCPP_INFO(this->get_logger(), 
                   "Start time: %.6f", start_time.seconds());
        RCLCPP_INFO(this->get_logger(), 
                   "Future time: %.6f", future_time.seconds());
        
        // Time - Time = Duration
        rclcpp::Duration time_diff = future_time - start_time;
        RCLCPP_INFO(this->get_logger(), 
                   "Time difference: %.6f seconds", time_diff.seconds());
    }
    
    void demonstrate_time_comparison() {
        rclcpp::Time t1 = this->get_clock()->now();
        
        // 模擬一些處理時間
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        
        rclcpp::Time t2 = this->get_clock()->now();
        
        if (t2 > t1) {
            RCLCPP_INFO(this->get_logger(), "t2 is later than t1");
            rclcpp::Duration elapsed = t2 - t1;
            RCLCPP_INFO(this->get_logger(), 
                       "Elapsed time: %.6f seconds", elapsed.seconds());
        }
    }
};

使用場景

• 時間戳記錄：為感測器數據添加精確的時間戳
• 事件排序：根據時間順序處理事件
• 同步控制：多個節點間的時間同步
• 數據關聯：基於時間戳關聯不同來源的數據

3. Rate（頻率控制）- 精確的執行頻率管理

基本概念

Rate 用於控制程式執行頻率，確保迴圈以固定頻率運行。這對於需要穩定控制頻率的應用場景非常重要。

C++ 實作範例

#include "rclcpp/rclcpp.hpp"

class RateDemo: public rclcpp::Node {
public:
    RateDemo(): Node("rate_demo") {
        demonstrate_rate_usage();
    }

private:
    void demonstrate_rate_usage() {
        // 創建 10Hz 的頻率控制
        rclcpp::Rate rate(10.0);
        
        RCLCPP_INFO(this->get_logger(), "Starting rate control demo at 10Hz");
        
        rclcpp::Time start_time = this->get_clock()->now();
        int count = 0;
        
        while (rclcpp::ok() && count < 50) {
            rclcpp::Time current_time = this->get_clock()->now();
            rclcpp::Duration elapsed = current_time - start_time;
            
            RCLCPP_INFO(this->get_logger(), 
                       "Loop %d: elapsed time %.3f seconds", 
                       count, elapsed.seconds());
            
            // 執行一些工作
            perform_work();
            
            count++;
            
            // 維持 10Hz 頻率
            rate.sleep();
        }
        
        rclcpp::Duration total_time = this->get_clock()->now() - start_time;
        RCLCPP_INFO(this->get_logger(), 
                   "Completed %d iterations in %.3f seconds", 
                   count, total_time.seconds());
    }
    
    void perform_work() {
        // 模擬一些工作負載
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
    }
};

進階 Rate 控制範例

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

class AdvancedRateDemo: public rclcpp::Node {
public:
    AdvancedRateDemo(): Node("advanced_rate_demo") {
        // 創建發布者
        publisher_ = this->create_publisher<std_msgs::msg::String>("heartbeat", 10);
        
        // 使用不同頻率的多個任務
        start_multiple_rate_tasks();
    }

private:
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    
    void start_multiple_rate_tasks() {
        // 高頻率任務 (50Hz)
        std::thread high_freq_thread([this]() {
            this->high_frequency_task();
        });
        
        // 低頻率任務 (1Hz)
        std::thread low_freq_thread([this]() {
            this->low_frequency_task();
        });
        
        high_freq_thread.detach();
        low_freq_thread.detach();
        
        // 主執行緒保持運行
        rclcpp::spin(this->shared_from_this());
    }
    
    void high_frequency_task() {
        rclcpp::Rate rate(50.0);  // 50Hz
        int count = 0;
        
        while (rclcpp::ok()) {
            // 高頻率的感測器數據處理
            process_sensor_data(count++);
            rate.sleep();
        }
    }
    
    void low_frequency_task() {
        rclcpp::Rate rate(1.0);  // 1Hz
        int count = 0;
        
        while (rclcpp::ok()) {
            // 低頻率的狀態報告
            publish_status_report(count++);
            rate.sleep();
        }
    }
    
    void process_sensor_data(int count) {
        // 模擬高頻率感測器數據處理
        if (count % 50 == 0) {  // 每秒輸出一次
            RCLCPP_INFO(this->get_logger(), 
                       "High freq task: processed %d sensor readings", count);
        }
    }
    
    void publish_status_report(int count) {
        auto message = std_msgs::msg::String();
        message.data = "Status report #" + std::to_string(count);
        
        publisher_->publish(message);
        RCLCPP_INFO(this->get_logger(), 
                   "Published: %s", message.data.c_str());
    }
};

使用場景

• 感測器數據發布：以固定頻率發布感測器數據
• 控制迴圈：機器人控制演算法的執行頻率
• 狀態監控：定期檢查系統狀態
• 數據採集：按固定間隔採集數據

4. 綜合應用範例：機器人巡邏系統

讓我們看一個結合所有時間 API 的實際應用範例：

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"

class PatrolRobot: public rclcpp::Node {
public:
    PatrolRobot(): Node("patrol_robot"),
                   patrol_duration_(std::chrono::minutes(5)),  // 5分鐘巡邏
                   control_rate_(20.0),                        // 20Hz 控制頻率
                   state_change_interval_(std::chrono::seconds(10)) {  // 10秒狀態切換
        
        // 創建速度命令發布者
        cmd_vel_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
            "cmd_vel", 10);
        
        // 記錄開始時間
        start_time_ = this->get_clock()->now();
        last_state_change_ = start_time_;
        
        RCLCPP_INFO(this->get_logger(), "Starting patrol mission");
        
        // 開始巡邏
        patrol_loop();
    }

private:
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_publisher_;
    
    rclcpp::Time start_time_;
    rclcpp::Time last_state_change_;
    rclcpp::Duration patrol_duration_;
    rclcpp::Duration state_change_interval_;
    rclcpp::Rate control_rate_;
    
    enum class PatrolState {
        MOVING_FORWARD,
        TURNING_LEFT,
        TURNING_RIGHT,
        STOPPED
    };
    
    PatrolState current_state_ = PatrolState::MOVING_FORWARD;
    
    void patrol_loop() {
        while (rclcpp::ok()) {
            rclcpp::Time current_time = this->get_clock()->now();
            
            // 檢查是否完成巡邏任務
            if ((current_time - start_time_) > patrol_duration_) {
                RCLCPP_INFO(this->get_logger(), "Patrol mission completed!");
                stop_robot();
                break;
            }
            
            // 檢查是否需要改變狀態
            if ((current_time - last_state_change_) > state_change_interval_) {
                change_patrol_state();
                last_state_change_ = current_time;
            }
            
            // 執行當前狀態的動作
            execute_current_state();
            
            // 輸出狀態資訊
            log_patrol_status(current_time);
            
            // 維持控制頻率
            control_rate_.sleep();
        }
    }
    
    void change_patrol_state() {
        switch (current_state_) {
            case PatrolState::MOVING_FORWARD:
                current_state_ = PatrolState::TURNING_LEFT;
                break;
            case PatrolState::TURNING_LEFT:
                current_state_ = PatrolState::MOVING_FORWARD;
                break;
            case PatrolState::TURNING_RIGHT:
                current_state_ = PatrolState::MOVING_FORWARD;
                break;
            case PatrolState::STOPPED:
                current_state_ = PatrolState::MOVING_FORWARD;
                break;
        }
        
        RCLCPP_INFO(this->get_logger(), "State changed to: %s", 
                   state_to_string(current_state_).c_str());
    }
    
    void execute_current_state() {
        auto twist_msg = geometry_msgs::msg::Twist();
        
        switch (current_state_) {
            case PatrolState::MOVING_FORWARD:
                twist_msg.linear.x = 0.5;   // 前進
                twist_msg.angular.z = 0.0;
                break;
            case PatrolState::TURNING_LEFT:
                twist_msg.linear.x = 0.0;
                twist_msg.angular.z = 0.5;  // 左轉
                break;
            case PatrolState::TURNING_RIGHT:
                twist_msg.linear.x = 0.0;
                twist_msg.angular.z = -0.5; // 右轉
                break;
            case PatrolState::STOPPED:
                twist_msg.linear.x = 0.0;
                twist_msg.angular.z = 0.0;
                break;
        }
        
        cmd_vel_publisher_->publish(twist_msg);
    }
    
    void stop_robot() {
        auto twist_msg = geometry_msgs::msg::Twist();
        twist_msg.linear.x = 0.0;
        twist_msg.angular.z = 0.0;
        cmd_vel_publisher_->publish(twist_msg);
        current_state_ = PatrolState::STOPPED;
    }
    
    void log_patrol_status(const rclcpp::Time& current_time) {
        static int log_counter = 0;
        
        // 每秒輸出一次狀態 (20Hz / 20 = 1Hz)
        if (++log_counter % 20 == 0) {
            rclcpp::Duration elapsed = current_time - start_time_;
            rclcpp::Duration remaining = patrol_duration_ - elapsed;
            
            RCLCPP_INFO(this->get_logger(), 
                       "Patrol status - State: %s, Elapsed: %.1fs, Remaining: %.1fs",
                       state_to_string(current_state_).c_str(),
                       elapsed.seconds(),
                       remaining.seconds());
        }
    }
    
    std::string state_to_string(PatrolState state) {
        switch (state) {
            case PatrolState::MOVING_FORWARD: return "MOVING_FORWARD";
            case PatrolState::TURNING_LEFT: return "TURNING_LEFT";
            case PatrolState::TURNING_RIGHT: return "TURNING_RIGHT";
            case PatrolState::STOPPED: return "STOPPED";
            default: return "UNKNOWN";
        }
    }
};

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

5. 最佳實踐與建議

性能優化建議

1. 選擇合適的頻率：根據應用需求選擇適當的控制頻率，避免不必要的高頻率操作
2. 使用 Timer 替代 Rate：對於事件驅動的應用，考慮使用 create_timer() 而非 Rate::sleep()
3. 避免阻塞操作：在高頻率迴圈中避免阻塞操作，保持系統響應性

時間同步考量

1. 使用節點時鐘：始終使用 get_clock()->now() 而非系統時間函數
2. 模擬時間支援：確保程式碼能夠正確處理模擬時間環境
3. 分散式同步：在多節點系統中考慮時間同步問題

錯誤處理

// 安全的時間運算
rclcpp::Time safe_time_add(const rclcpp::Time& time, const rclcpp::Duration& duration) {
    try {
        return time + duration;
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "Time arithmetic error: %s", e.what());
        return time;
    }
}

// 檢查時間有效性
bool is_valid_time(const rclcpp::Time& time) {
    return time.nanoseconds() > 0;
}

ROS2 時間相關 API 比較

1. 功能比較表

2. 使用場景比較表

3. 主要差異說明

1. 概念層面

   • Rate：著重於頻率控制，是一個控制工具
   • Time：代表特定時間點，類似時鐘上的刻度
   • Duration：表示時間區間，類似碼表計時

2. 操作特性

   • Rate：主要用於循環控制，具有阻塞特性
   • Time：支持比較和算術運算，不具阻塞特性
   • Duration：可進行加減運算，支持正負值

3. 使用方式

   • Rate：通常在迴圈中使用 sleep() 方法
   • Time：常用於獲取當前時間和時間戳記
   • Duration：多用於時間間隔計算和延遲設定

結論

ROS2 的時間相關 API 提供了強大而靈活的時間管理功能。通過合理使用 Time、Rate、Duration 這些工具，您可以：

• 實現精確的時間控制
• 構建穩定的週期性任務
• 處理複雜的時序邏輯
• 確保系統的可靠性和性能

掌握這些 API 的使用方法，將大大提升您在 ROS2 開發中的效率和程式品質。無論是簡單的感測器數據發布，還是複雜的機器人控制系統，這些時間管理工具都是不可或缺的基礎組件。

希望本文的範例和說明能夠幫助您更好地理解和應用 ROS2 的時間相關 API。在實際開發中，建議根據具體需求靈活運用這些工具，並持續關注 ROS2 社群的最新發展和最佳實踐。