Plane work

src/Bringup/launch/core.launch.py

@@ -44,6 +44,12 @@ def generate_launch_description():
         name="node_status_publisher",
     )
 
+    system_telemetry_node = Node(
+        package="system-telemetry-cpp",
+        executable="system_telemetry_publisher",
+        name="system_telemetry_node",
+    )
+
     return LaunchDescription(
         get_included_launch_descriptions(launch_files) + [snmp_node, status_node]
     )

src/Cameras/video_streaming/config/presets.yaml

@@ -5,10 +5,13 @@ preset_node:
     - "EEFPreset"
     - "BellyPreset"
     - "MastPreset"
+    - "SciencePreset"
+    - "MicroscopePreset"
     - "DriveEEFPreset"
     - "EEFDrivePreset"
     - "EEFDriveMast"
     - "DriveMastPreset"
+    - "BellyDrivePreset"
     DrivePreset:
       name: "Drive"
       sources:
@@ -45,6 +48,24 @@ preset_node:
         height: 100
         origin_x: 0
         origin_y: 0
+    SciencePreset:
+      name: "Science"
+      sources:
+      - "Science"
+      Science:
+        width: 100
+        height: 100
+        origin_x: 0
+        origin_y: 0
+    MicroscopePreset:
+      name: "Microscope"
+      sources:
+      - "Microscope"
+      Microscope:
+        width: 100
+        height: 100
+        origin_x: 0
+        origin_y: 0
     DriveEEFPreset:
       name: "Drive + EEF"
       sources:
@@ -107,6 +128,21 @@ preset_node:
         origin_x: 0
         origin_y: 0
       Mast:
+        width: 30
+        height: 30
+        origin_x: 70
+        origin_y: 0
+    BellyDrivePreset:
+      name: "Belly + Drive"
+      sources:
+      - "Belly"
+      - "Drive"
+      Belly:
+        width: 100
+        height: 100
+        origin_x: 0
+        origin_y: 0
+      Drive:
         width: 30
         height: 30
         origin_x: 70

src/Cameras/video_streaming/include/video_streaming/preset_node.hpp

@@ -5,6 +5,7 @@
 
 #include "interfaces/msg/video_preset.hpp"
 #include "interfaces/msg/video_presets.hpp"
+#include "interfaces/srv/get_cameras.hpp"
 
 class PresetNode : public rclcpp::Node {
 public:
@@ -14,9 +15,11 @@ class PresetNode : public rclcpp::Node {
 private:
   void declare_parameters();
   void load_presets();
+  void sort_presets();
 
   std::vector<interfaces::msg::VideoPreset> presets_;
   rclcpp::Publisher<interfaces::msg::VideoPresets>::SharedPtr presets_pub_;
+  rclcpp::Client<interfaces::srv::GetCameras>::SharedPtr cameras_client_;
 };
 
 #endif // PRESET_NODE_HPP

src/Cameras/video_streaming/src/preset_node.cpp

@@ -12,10 +12,14 @@ PresetNode::PresetNode(const rclcpp::NodeOptions &options)
   presets_pub_ = this->create_publisher<interfaces::msg::VideoPresets>(
       "/video_presets", presets_qos);
 
+  cameras_client_ = this->create_client<interfaces::srv::GetCameras>(
+      "/input_node/get_cameras");
+
   interfaces::msg::VideoPresets presets_msg;
   presets_msg.presets = presets_;
   presets_pub_->publish(presets_msg);
 
+  sort_presets();
   RCLCPP_INFO(this->get_logger(), "PresetNode started");
 }
 
@@ -66,4 +70,47 @@ void PresetNode::load_presets() {
   RCLCPP_INFO(this->get_logger(), "Loaded %ld presets", presets_.size());
 }
 
+void PresetNode::sort_presets() {
+  if (!cameras_client_->wait_for_service(std::chrono::seconds(5))) {
+    RCLCPP_ERROR(this->get_logger(), "Get cameras service is unavailable!!!!");
+    return;
+  }
+
+  auto request = std::make_shared<interfaces::srv::GetCameras::Request>();
+  cameras_client_->async_send_request(
+      request, [this](const std::shared_future<
+                      interfaces::srv::GetCameras::Response::SharedPtr>
+                          future) {
+        auto response = future.get();
+
+        std::vector<interfaces::msg::VideoPreset> new_presets;
+        int i = 0;
+        for (const auto &preset : presets_) {
+          int found = 0;
+          for (const auto &source : preset.sources) {
+            for (const auto &camera : response->sources) {
+              if (camera == source.name) {
+                found++;
+                break;
+              }
+            }
+          }
+          if (found == preset.sources.size()) {
+            new_presets.push_back(preset);
+          }
+          i++;
+        }
+
+        presets_ = std::move(new_presets);
+
+        RCLCPP_INFO(this->get_logger(),
+                    "Sorted to %ld available presets using %ld cameras",
+                    presets_.size(), response->sources.size());
+
+        interfaces::msg::VideoPresets presets_msg;
+        presets_msg.presets = presets_;
+        presets_pub_->publish(presets_msg);
+      });
+}
+
 RCLCPP_COMPONENTS_REGISTER_NODE(PresetNode)

src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py

@@ -13,10 +13,7 @@ def convert_from_radians(angle, servo_min, servo_max, servo_rom):
 class MastESP(Node):
     def __init__(self):
         super().__init__("mast_esp")
-        self.ser = serial.Serial(
-            "/dev/serial/by-id/usb-Espressif_USB_JTAG_serial_debug_unit_20:6E:F1:69:EE:E0-if00",
-            115200,
-        )
+
         self.pub = self.create_publisher(Distance, "eef_distance", 10)
         self.us_sub = self.create_subscription(
             Float32, "/mast_angle", self.mast_callback, 3
@@ -28,48 +25,119 @@ def __init__(self):
         self.declare_parameter("min", 500.0)
         self.declare_parameter("max", 2500.0)
         self.declare_parameter("rom", 6.2832)
+        self.declare_parameter("reconnect_period_s", 2.0)
+        self.declare_parameter(
+            "port",
+            "/dev/serial/by-id/usb-Espressif_USB_JTAG_serial_debug_unit_20:6E:F1:69:EE:E0-if00",
+        )
+        self.declare_parameter("baudrate", 115200)
+
+        self._port = self.get_parameter("port").get_parameter_value().string_value
+        self._baudrate = (
+            self.get_parameter("baudrate").get_parameter_value().integer_value
+        )
+        self._reconnect_period = (
+            self.get_parameter("reconnect_period_s").get_parameter_value().double_value
+        )
+
+        self._serial = None
+        self._last_reconnect_attempt_ns = 0
 
         self.servo_min = self.get_parameter("min").get_parameter_value().double_value
         self.servo_max = self.get_parameter("max").get_parameter_value().double_value
         self.servo_rom = self.get_parameter("rom").get_parameter_value().double_value
 
         self.create_timer(0.001, self.loop)
-        self.get_logger().info("Mast ESP node started")
+        self._ensure_serial_connected(force=True)
+        self.get_logger().info(
+            f"Mast ESP node started, port={self._port}, baud={self._baudrate}"
+        )
+
+    def _ensure_serial_connected(self, force: bool = False) -> bool:
+        if self._serial is not None and self._serial.is_open:
+            return True
+
+        now_ns = self.get_clock().now().nanoseconds
+        if not force and (now_ns - self._last_connect_attempt_ns) < int(
+            self._reconnect_period * 1e9
+        ):
+            return False
+        self._last_connect_attempt_ns = now_ns
+
+        try:
+            self._serial = serial.Serial(
+                port=self._port, baudrate=self._baudrate, timeout=0.0
+            )
+            self.get_logger().info(f"Connected to serial port {self._port}")
+            return True
+        except serial.SerialException as exc:
+            self._serial = None
+            self.get_logger().warn(
+                f"Failed to open serial port {self._port}: {exc}. Retrying..."
+            )
+            return False
+
+    def _close_serial(self):
+        if self._serial is not None:
+            try:
+                self._serial.close()
+            except Exception:
+                pass
+        self._serial = None
 
     def mast_callback(self, msg: Float32):
         us = convert_from_radians(
             msg.data, self.servo_min, self.servo_max, self.servo_rom
         )
         self.get_logger().debug(f"Received angle: {msg.data:.3f} rad -> PWM: {us} us")
         data = bytes("S" + str(us) + "\n", "utf-8")
-        self.ser.write(data)
+        try:
+            self._serial.write(data)
+        except (serial.SerialException, OSError) as exc:
+            self.get_logger().warn(f"Serial write failed: {exc}")
+            self._close_serial()
 
     def morse_callback(self, msg: String):
         self.get_logger().info(f"Transmitting morse: {msg.data}")
         data = bytes("M" + msg.data + "\n", "utf-8")
-        self.ser.write(data)
+        try:
+            self._serial.write(data)
+        except (serial.SerialException, OSError) as exc:
+            self.get_logger().warn(f"Serial write failed: {exc}")
+            self._close_serial()
 
     def loop(self):
-        if self.ser.in_waiting:
-            line = self.ser.readline().decode().strip()
-            try:
-                reading = int(line)
-                msg = Distance()
-                msg.header.stamp = self.get_clock().now().to_msg()
-                if reading == -2:
-                    mm = 0
-                    msg.status = Distance.STATUS_ERROR
-                elif reading == -1:
-                    mm = 0
-                    msg.status = Distance.STATUS_INVALID
-                else:
-                    msg.status = Distance.STATUS_OK
-                    mm = reading
-                    msg.distance = mm / 1000.0
-                    self.get_logger().debug(f"Distance: {msg.distance:.3f} m")
-                self.pub.publish(msg)
-            except ValueError:
-                pass
+        if not self._ensure_serial_connected():
+            return
+        try:
+            if self._serial.in_waiting:
+                line = self._serial.readline().decode().strip()
+                try:
+                    reading = int(line)
+                    msg = Distance()
+                    msg.header.stamp = self.get_clock().now().to_msg()
+                    if reading == -2:
+                        mm = 0
+                        msg.status = Distance.STATUS_ERROR
+                    elif reading == -1:
+                        mm = 0
+                        msg.status = Distance.STATUS_INVALID
+                    else:
+                        msg.status = Distance.STATUS_OK
+                        mm = reading
+                        msg.distance = mm / 1000.0
+                        self.get_logger().debug(f"Distance: {msg.distance:.3f} m")
+                    self.pub.publish(msg)
+                except ValueError:
+                    pass
+        except (serial.SerialException, OSError) as exc:
+            self.get_logger().warn(f"Serial read failed: {exc}")
+            self._close_serial()
+            return
+
+    def destroy_node(self):
+        self._close_serial()
+        super().destroy_node()
 
 
 def main(args=None):

src/Teleop-Control/system-telemetry-cpp/CMakeLists.txt

@@ -34,6 +34,7 @@ add_executable(system_telemetry_publisher
   src/cpu_collector.cpp
   src/memory_collector.cpp
   src/gpu_collector.cpp
+  src/temp_collector.cpp
 )
 
 add_executable(node_status_publisher

src/Teleop-Control/system-telemetry-cpp/include/system-telemetry-cpp/temp_collector.hpp

@@ -0,0 +1,20 @@
+#ifndef TEMP_COLLECTOR_HPP_
+#define TEMP_COLLECTOR_HPP_
+
+#include <fstream>
+
+#include "telemetry_collector.hpp"
+
+class TempCollector : public TelemetryCollector {
+public:
+  TempCollector();
+  void collect(interfaces::msg::SystemTelemetry &msg) override;
+
+private:
+  float read_temp(std::ifstream &file);
+
+  std::ifstream core_file_;
+  std::ifstream case_file_;
+};
+
+#endif // TEMP_COLLECTOR_HPP_

src/Teleop-Control/system-telemetry-cpp/src/system_telemetry_publisher.cpp

@@ -10,6 +10,7 @@
 #include "interfaces/msg/system_telemetry.hpp"
 #include "memory_collector.hpp"
 #include "telemetry_collector.hpp"
+#include "temp_collector.hpp"
 
 SystemTelemetryPublisher::SystemTelemetryPublisher()
     : Node("system_telemetry_publisher") {
@@ -23,11 +24,13 @@ SystemTelemetryPublisher::SystemTelemetryPublisher()
   this->declare_parameter("cpu", true);
   this->declare_parameter("memory", true);
   this->declare_parameter("gpu", true);
+  this->declare_parameter("temp", true);
   const bool cpu = this->get_parameter("cpu").as_bool();
   const bool memory = this->get_parameter("memory").as_bool();
   const bool gpu = this->get_parameter("gpu").as_bool();
+  const bool temp = this->get_parameter("temp").as_bool();
 
-  if (!cpu && !memory && !gpu) {
+  if (!cpu && !memory && !gpu && !temp) {
     RCLCPP_ERROR(this->get_logger(), "No collectors enabled.");
     return;
   }
@@ -40,6 +43,9 @@ SystemTelemetryPublisher::SystemTelemetryPublisher()
   if (gpu) {
     collectors_.emplace_back(std::make_unique<GPUCollector>());
   }
+  if (temp) {
+    collectors_.emplace_back(std::make_unique<TempCollector>());
+  }
 
   timer_ = this->create_wall_timer(
       period, std::bind(&SystemTelemetryPublisher::publish_telemetry, this));
@@ -53,8 +59,10 @@ void SystemTelemetryPublisher::publish_telemetry() {
   }
   publisher_->publish(msg);
   RCLCPP_INFO(this->get_logger(),
-              "Published Telemetry: CPU: %.1f%%, Mem: %.1f%%, GPU: %.1f%%",
-              msg.cpu_usage, msg.mem_usage, msg.gpu_usage);
+              "Published Telemetry: CPU: %.1f%%, Mem: %.1f%%, GPU: %.1f%%, "
+              "Core Temp: %.1f C, Case Temp: %.1f C",
+              msg.cpu_usage, msg.mem_usage, msg.gpu_usage, msg.core_temp,
+              msg.case_temp);
 }
 
 int main(int argc, char *argv[]) {