Temp Monitoring with Fans

src/temp_monitor.cpp

@@ -10,6 +10,15 @@
 #include <filesystem>
 #include <thread>
 #include <chrono>
+#include <csignal>
+#include <atomic>
+
+std::atomic_bool running(true);
+
+void handle_sigint(int)
+{
+    running = false;  // causes main loop to exit
+}
 
 namespace fs = std::filesystem;
 
@@ -56,15 +65,24 @@ int main(int argc, char** argv) {
     auto node = std::make_shared<rclcpp::Node>("temperature_monitor");
     auto fan_publisher = std::make_shared<FanPublisher>(node);
 
+    std::signal(SIGINT, handle_sigint);
+
+    auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    executor->add_node(node);
+
+    // Thread that runs the executor manually
     std::thread spin_thread([&]() {
-    rclcpp::spin(node);
+        while (rclcpp::ok() && running) {
+            executor->spin_some();
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        }
     });
 
-    // Set the rate to 10 Hz
-    rclcpp::Rate rate(10);
+    // Set the rate to 1 Hz
+    rclcpp::Rate rate(1);
 
     RCLCPP_INFO(node->get_logger(), "Monitoring CPU temperature (press Ctrl+C to stop):");
-    while (rclcpp::ok()) {
+    while (rclcpp::ok() && running) {
         double tempC = readTemperature(tempPath);
         if (tempC < 0) {
             RCLCPP_INFO(node->get_logger(), "Failed to read temperature!");
@@ -80,15 +98,15 @@ int main(int argc, char** argv) {
                 fanSpeed.fan_percent_3 = 0;
                 fan_publisher->trigger_publish(fanSpeed);
             } else if (tempC >= 30.0 && tempC < 40.0) {
-                // Handle case for temperatures between 30°C and 50°C
+                // Handle case for temperatures between 30°C and 40°C
                 FanSpeed fanSpeed;
                 fanSpeed.fan_percent_0 = 25;
                 fanSpeed.fan_percent_1 = 25;
                 fanSpeed.fan_percent_2 = 25;
                 fanSpeed.fan_percent_3 = 25;
                 fan_publisher->trigger_publish(fanSpeed);
             } else if (tempC >= 40.0 && tempC < 50) {
-                // Handle case for temperatures above 50°C
+                // Handle case for temperatures between 40 and 50
                 FanSpeed fanSpeed;
                 fanSpeed.fan_percent_0 = 50;
                 fanSpeed.fan_percent_1 = 50;
@@ -109,5 +127,22 @@ int main(int argc, char** argv) {
         rate.sleep();
     }
 
+    FanSpeed off;
+    off.fan_percent_0 = 0;
+    off.fan_percent_1 = 0;
+    off.fan_percent_2 = 0;
+    off.fan_percent_3 = 0;
+    fan_publisher->trigger_publish(off);
+    RCLCPP_INFO(rclcpp::get_logger("shutdown"), "Shutdown: Turning off fans.");
+
+    // Give time to publish and flush
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+    // Cleanup
+    executor->cancel();  // This ensures executor thread stops
+    rclcpp::shutdown();
+    spin_thread.join();
+
+
     return 0;
 }