add RTF monitor

Author: aquintan4

exercises/global_navigation/cpp_lib/src/WebGUI.cpp

@@ -1,6 +1,7 @@
 #include "WebGUI.hpp"
 #include "Map.hpp"
 #include "common_interfaces_cpp/webgui/WebGUIBridge.hpp"
+#include "common_interfaces_cpp/webgui/RTFMonitor.hpp"
 #include "common_interfaces_cpp/hal/odometry.hpp"
 #include "geometry_msgs/msg/point.hpp"
 #include "nav_msgs/msg/path.hpp"
@@ -19,6 +20,8 @@ class WebGUINode : public BaseWebGUI
         : BaseWebGUI("webgui_node", "127.0.0.1", "2303", 30.0)
         , image_updated_(false)
         , last_image_payload_("{\"image\":\"\",\"shape\":[]}")
+        , gui_iterations_(0)
+        , rtf_monitor_("/stats", std::chrono::milliseconds(500))
         , map_util_([this](){ return odom_node_->getPose3d(); })
     {
         odom_node_ = std::make_shared<OdometryNode>("/odom", "webgui_odom");
@@ -48,6 +51,11 @@ class WebGUINode : public BaseWebGUI
                 }
                 show_path(path);
             });
+
+        last_stat_time_ = std::chrono::steady_clock::now();
+        stats_timer_ = this->create_wall_timer(
+            std::chrono::milliseconds(500),
+            std::bind(&WebGUINode::send_stats, this));
     }
 
     std::vector<rclcpp::Node::SharedPtr> get_internal_nodes() {
@@ -87,6 +95,8 @@ class WebGUINode : public BaseWebGUI
 
 protected:
     json update_gui() override {
+        gui_iterations_++;
+
         json inner;
         inner["image"] = encode_image();
 
@@ -133,6 +143,26 @@ class WebGUINode : public BaseWebGUI
     }
 
 private:
+    void send_stats() {
+        auto now = std::chrono::steady_clock::now();
+        std::chrono::duration<double> elapsed = now - last_stat_time_;
+
+        double current_freq = 0.0;
+        if (elapsed.count() > 0.0) {
+            current_freq = gui_iterations_.exchange(0) / elapsed.count();
+        }
+        last_stat_time_ = now;
+
+        json stats;
+        stats["brain"] = std::round(current_freq * 10.0) / 10.0;
+        stats["gui"]   = 30.0;
+        stats["rtf"]   = rtf_monitor_.get();
+        stats["fps"]   = -1.0;
+        stats["lat"]   = -1.0;
+
+        send_to_frontend(stats);
+    }
+
     std::string encode_image() {
         cv::Mat local;
         {
@@ -182,6 +212,11 @@ class WebGUINode : public BaseWebGUI
     std::mutex target_mtx_;
 
     Map map_util_;
+    
+    RTFMonitor rtf_monitor_;
+    rclcpp::TimerBase::SharedPtr stats_timer_;
+    std::atomic<int> gui_iterations_;
+    std::chrono::time_point<std::chrono::steady_clock> last_stat_time_;
 };
 
 std::shared_ptr<WebGUINode> WebGUI::gui_node_  = nullptr;