From e41792ee491d1ad7df13cf19e45c7cff8b7e83ad Mon Sep 17 00:00:00 2001 From: Nathaniel Hargrave Date: Fri, 7 Aug 2026 00:40:46 +0000 Subject: [PATCH 1/5] Add retry logic for mast esp --- .../gpio_controller/mast_esp.py | 122 ++++++++++++++---- 1 file changed, 95 insertions(+), 27 deletions(-) diff --git a/src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py b/src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py index 69dfe9ad..553a1545 100644 --- a/src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py +++ b/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,13 +25,65 @@ 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._baud}" + ) + + 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( @@ -42,34 +91,53 @@ def mast_callback(self, msg: Float32): ) 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): From 67080570796fc5ce1aedbe2e2577fa7a1a89a1c9 Mon Sep 17 00:00:00 2001 From: Nathaniel Hargrave Date: Thu, 6 Aug 2026 22:15:36 -0400 Subject: [PATCH 2/5] Add temperature reporting to system telemetry --- src/Bringup/launch/core.launch.py | 6 ++++ .../system-telemetry-cpp/CMakeLists.txt | 1 + .../system-telemetry-cpp/temp_collector.hpp | 20 +++++++++++++ .../src/system_telemetry_publisher.cpp | 14 +++++++-- .../src/temp_collector.cpp | 29 +++++++++++++++++++ src/interfaces/msg/SystemTelemetry.msg | 4 ++- 6 files changed, 70 insertions(+), 4 deletions(-) create mode 100644 src/Teleop-Control/system-telemetry-cpp/include/system-telemetry-cpp/temp_collector.hpp create mode 100644 src/Teleop-Control/system-telemetry-cpp/src/temp_collector.cpp diff --git a/src/Bringup/launch/core.launch.py b/src/Bringup/launch/core.launch.py index 8c89c1b5..500e6cf4 100644 --- a/src/Bringup/launch/core.launch.py +++ b/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] ) diff --git a/src/Teleop-Control/system-telemetry-cpp/CMakeLists.txt b/src/Teleop-Control/system-telemetry-cpp/CMakeLists.txt index 9dfca1e1..1cd6a47c 100644 --- a/src/Teleop-Control/system-telemetry-cpp/CMakeLists.txt +++ b/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 diff --git a/src/Teleop-Control/system-telemetry-cpp/include/system-telemetry-cpp/temp_collector.hpp b/src/Teleop-Control/system-telemetry-cpp/include/system-telemetry-cpp/temp_collector.hpp new file mode 100644 index 00000000..4c166ebe --- /dev/null +++ b/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 + +#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_ diff --git a/src/Teleop-Control/system-telemetry-cpp/src/system_telemetry_publisher.cpp b/src/Teleop-Control/system-telemetry-cpp/src/system_telemetry_publisher.cpp index 437436b9..599e4f0c 100644 --- a/src/Teleop-Control/system-telemetry-cpp/src/system_telemetry_publisher.cpp +++ b/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()); } + if (temp) { + collectors_.emplace_back(std::make_unique()); + } 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[]) { diff --git a/src/Teleop-Control/system-telemetry-cpp/src/temp_collector.cpp b/src/Teleop-Control/system-telemetry-cpp/src/temp_collector.cpp new file mode 100644 index 00000000..8f581956 --- /dev/null +++ b/src/Teleop-Control/system-telemetry-cpp/src/temp_collector.cpp @@ -0,0 +1,29 @@ +#include "temp_collector.hpp" + +#include +#include + +TempCollector::TempCollector() + : core_file_("/sys/class/hwmon/hwmon8/temp1_input"), + case_file_("/sys/class/hwmon/hwmon1/temp1_input") {} + +float TempCollector::read_temp(std::ifstream &file) { + float temp = 0.f; + file.clear(); + file.seekg(0, std::ios::beg); + std::string line; + if (std::getline(file, line)) { + std::istringstream iss(line); + uint64_t temp_mdeg; + iss >> temp_mdeg; + temp = (float)temp_mdeg / 1000; + } else { + RCLCPP_ERROR(logger_, "Failed to read temperature file"); + } + return temp; +} + +void TempCollector::collect(interfaces::msg::SystemTelemetry &msg) { + msg.core_temp = read_temp(core_file_); + msg.case_temp = read_temp(case_file_); +} diff --git a/src/interfaces/msg/SystemTelemetry.msg b/src/interfaces/msg/SystemTelemetry.msg index 68a5a720..19700e8d 100644 --- a/src/interfaces/msg/SystemTelemetry.msg +++ b/src/interfaces/msg/SystemTelemetry.msg @@ -1,3 +1,5 @@ float32 cpu_usage float32 mem_usage -float32 gpu_usage \ No newline at end of file +float32 gpu_usage +float32 core_temp +float32 case_temp \ No newline at end of file From 19847b62f113781d6c1a9a9bf30aef14f2077a62 Mon Sep 17 00:00:00 2001 From: Nathaniel Hargrave Date: Fri, 7 Aug 2026 00:06:25 -0400 Subject: [PATCH 3/5] PresetNode sorts presets based on available cams --- .../include/video_streaming/preset_node.hpp | 3 ++ .../video_streaming/src/preset_node.cpp | 47 +++++++++++++++++++ 2 files changed, 50 insertions(+) diff --git a/src/Cameras/video_streaming/include/video_streaming/preset_node.hpp b/src/Cameras/video_streaming/include/video_streaming/preset_node.hpp index ed2360e0..052cfc1e 100644 --- a/src/Cameras/video_streaming/include/video_streaming/preset_node.hpp +++ b/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 presets_; rclcpp::Publisher::SharedPtr presets_pub_; + rclcpp::Client::SharedPtr cameras_client_; }; #endif // PRESET_NODE_HPP \ No newline at end of file diff --git a/src/Cameras/video_streaming/src/preset_node.cpp b/src/Cameras/video_streaming/src/preset_node.cpp index 264860c2..eb32a08a 100644 --- a/src/Cameras/video_streaming/src/preset_node.cpp +++ b/src/Cameras/video_streaming/src/preset_node.cpp @@ -12,10 +12,14 @@ PresetNode::PresetNode(const rclcpp::NodeOptions &options) presets_pub_ = this->create_publisher( "/video_presets", presets_qos); + cameras_client_ = this->create_client( + "/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(); + cameras_client_->async_send_request( + request, [this](const std::shared_future< + interfaces::srv::GetCameras::Response::SharedPtr> + future) { + auto response = future.get(); + + std::vector 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) \ No newline at end of file From bbd43ba28f1b67631ca0f0479556b06a4f31e9e2 Mon Sep 17 00:00:00 2001 From: Nathaniel Hargrave Date: Fri, 7 Aug 2026 00:10:06 -0400 Subject: [PATCH 4/5] Science camera presets --- .../video_streaming/config/presets.yaml | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/Cameras/video_streaming/config/presets.yaml b/src/Cameras/video_streaming/config/presets.yaml index ad32ee4f..9eed833d 100644 --- a/src/Cameras/video_streaming/config/presets.yaml +++ b/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 From 51cf274af6b899e677767de9b6b5ab4bfc3827e7 Mon Sep 17 00:00:00 2001 From: "Software Lead (Rover)" Date: Sat, 8 Aug 2026 01:56:52 +0000 Subject: [PATCH 5/5] fix: mast esp --- src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py b/src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py index 553a1545..f57b6f6d 100644 --- a/src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py +++ b/src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py @@ -50,7 +50,7 @@ def __init__(self): self.create_timer(0.001, self.loop) self._ensure_serial_connected(force=True) self.get_logger().info( - f"Mast ESP node started, port={self._port}, baud={self._baud}" + f"Mast ESP node started, port={self._port}, baud={self._baudrate}" ) def _ensure_serial_connected(self, force: bool = False) -> bool: