From f5a9c1beb43c5b8109b72449f320dd2ded6cbc06 Mon Sep 17 00:00:00 2001 From: mrichards03 Date: Mon, 27 Oct 2025 13:16:14 -0700 Subject: [PATCH 01/15] Thermal Camera documentation --- Documentation/2025/Thermal Camera.md | 129 +++++++++++++++++++++++++++ 1 file changed, 129 insertions(+) create mode 100644 Documentation/2025/Thermal Camera.md diff --git a/Documentation/2025/Thermal Camera.md b/Documentation/2025/Thermal Camera.md new file mode 100644 index 0000000..b5ac7d4 --- /dev/null +++ b/Documentation/2025/Thermal Camera.md @@ -0,0 +1,129 @@ +# **FLIR Lepton 3.1R Radiometric Camera Guide** + +This guide covers using the FLIR Lepton 3.1R camera with a PureThermal 3 (PT3) breakout board. The Lepton 3.1R is a **radiometric** camera, meaning it can output a 2D array of actual temperature values, not just a colorized image. + +This document focuses on the software stack required to access this 14-bit raw temperature data, primarily on Linux. + +## **The Core Challenge: Radiometry (14-bit) vs. Webcam Video (8-bit)** + +The PureThermal 3 board can expose the Lepton camera to your computer in two different modes over USB: + +1. **Standard Webcam Mode (8-bit AGC):** This is the default mode. The camera outputs a standard 8-bit colorized video stream (using Automatic Gain Control, or AGC). This is what applications like **OpenCV**, Zoom, or Photo Booth see by default. This mode **does not** contain temperature data. +2. **Radiometric Mode (14-bit T-Linear):** This special mode outputs the raw 14-bit sensor data (in a Y16 format). In this "T-Linear" mode, the value of each pixel maps directly to a temperature (in centiKelvin). + +To get temperature data, we **must** send a special command to the PureThermal 3 via USB to switch it from standard webcam mode to radiometric mode. Standard webcam libraries (like OpenCV's default `VideoCapture`) do not know how to send this command. + +This is why we need a software stack based on `libuvc`. + +## **Recommended Python Stack (flirpy)** + +For accessing temperature data in Python, `flirpy` is the most direct and recommended tool. It handles all the low-level complexity for you. + +### **1\. `libuvc` (The C-Library / "Driver")** + +* **What it is:** A low-level C library that enables direct communication with USB Video Class (UVC) devices like your PureThermal 3\. +* **Why you need it:** It's the foundation that allows `flirpy` (via `pyuvc`) to send the custom commands to enable radiometric mode. It is a required system-level dependency. +* **Installation:** + * **Ubuntu/Debian:** `sudo apt install libuvc-dev` + * **macOS:** `brew install libuvc` + * **Windows:** This is more complex; it's often easier to use the Windows-specific FLIR application for diagnostics. + +### **2\. `pyuvc` (The Python Wrapper)** + +* **What it is:** A Python library that "wraps" libuvc. It allows Python code to access the low-level controls of the UVC device. +* **Why you need it:** `flirpy` uses this library to do the actual work. You don't interact with it directly, but `flirpy` will install it as a dependency. + +### **3\. `flirpy` (The High-Level FLIR Library)** + +* **What it is:** A high-level Python library specifically for FLIR Lepton cameras on PureThermal boards. +* **Why you need it:** This is the tool for your job. It finds the camera, sends the command to enable radiometry, and grabs the 14-bit raw image. This image is a 2D NumPy array where values are in **centiKelvin**. You must convert this to Celsius manually. +* **Installation:** + `pip install flirpy` + +* **Example Python Usage:** +```python +import flirpy.camera.lepton +import numpy as np + +# flirpy.camera.lepton.Lepton() will auto-detect the PT3 +try: + with flirpy.camera.lepton.Lepton() as cam: + # Grab a single 2D numpy array of raw centiKelvin values + raw_image = cam.grab() + + if raw_image is None: + print("Failed to capture image.") + else: + print(f"Got image with shape {raw_image.shape}") + + # Convert the raw centiKelvin to Celsius + celsius_image = (raw_image.astype(np.float32) / 100.0) - 273.15 + + print(f"Min temp: {np.min(celsius_image):.2f} C, Max temp: {np.max(celsius_image):.2f} C") + +except Exception as e: + print(f"Error connecting to Lepton camera: {e}") + print("Make sure libuvc is installed and udev rules are set.") +``` +### **Linux `udev` Rule (Mandatory for Linux)** + +On Linux, you must give your user account permission to access the PureThermal device. + +1. Run the following command to create a `udev` rule file. This rule finds the PureThermal board (Vendor ID `1e4e`, Product ID `0100`) and gives it the correct permissions. + ``` + sudo sh -c "echo 'SUBSYSTEMS==\"usb\", ATTRS{idVendor}==\"1e4e\", ATTRS{idProduct}==\"0100\", SYMLINK+=\"pt1\", GROUP=\"usb\", MODE=\"666\"' > /etc/udev/rules.d/99-pt1.rules" + ``` + +2. Apply the new rule without rebooting: + ``` + sudo udevadm control --reload-rules + sudo udevadm trigger + ``` + +3. Unplug and replug your PureThermal 3 camera. + +## **Diagnostics and Testing** + +Before you start coding, it's wise to confirm the camera is working. + +* **`GetThermal` (Linux)** + * An excellent (though no longer actively supported) desktop app for viewing the radiometric stream. It's the best way to confirm `libuvc` and your `udev` rules are working. + * **Instructions:** + 1. Ensure `libuvc` is installed (see above). + 2. Download the `GetThermal-*.AppImage` from the [v0.1.4 release page](https://github.com/groupgets/GetThermal/releases/tag/v0.1.4). + 3. Make it executable: `chmod +x GetThermal-*.AppImage` + 4. Run it: `./GetThermal-*.AppImage` + 5. You should see a live stream with a temperature scale and a cursor that reads temperature. +* **FLIR Lepton Application (Windows)** + * The official FLIR Windows app for viewing the live stream, changing settings, and diagnosing the camera. + * **Download:** Find it on the [FLIR Lepton product page](https://oem.flir.com/en-hk/products/lepton/?vertical=microcam&segment=oem&docPage=2#Downloads) under "Software & Firmware". + +## **Other Libraries & Frameworks (Context)** + +These are other tools you might encounter, but they are generally *not* what you want for accessing temperature data from your PT3. + +* **opencv (cv2) (The General Vision Library)** + * **Limitation:** As explained in the "Core Challenge," OpenCV's cv2.VideoCapture(0) will only see the 8-bit standard webcam stream. It cannot access the 14-bit radiometric data without significant, complex modification. **Do not use opencv to get temperature data.** +* **pylepton (The Direct-Sensor Library)** + * **Limitation:** This library is for connecting a Lepton sensor *directly* to a single-board computer's GPIO pins (using SPI and I2C). + * **This is not for your USB connection** via the PureThermal board. +* **v4l2 / Video4Linux2 (The OS Framework)** + * This is the kernel-level framework in Linux that handles all video devices. `libuvc` and opencv use this in the background, but you don't interact with it. + +## **Important Considerations** + +* **Image Dewarping (Lepton 3.1R):** + * The Lepton 3.1R has a wide-angle lens (71° HFOV), which causes **significant 'fisheye' distortion** in the raw image. + * If you need accurate spatial measurements (e.g., mapping temperatures to specific locations), you **must** apply a dewarping/undistortion algorithm to the image you get from `flirpy`. + * See the official FLIR application note: [Image Dewarping for 3.1R](https://oem.flir.com/en-ca/learn/thermal-integration-made-easy/lepton-3.1r-dewarping-application-note/). +* **FFC (Flat-Field Correction):** + * The camera will periodically perform a "Flat-Field Correction" (FFC) to recalibrate its sensor. + * When this happens, you will hear a "click" from the camera’s shutter, and the video stream will freeze for a moment. This is normal and essential for accurate temperature readings. `flirpy` and GetThermal handle this automatically. + +## **References and Useful Links** + +* **FLIR Lepton Page (Downloads):** https://oem.flir.com/en-hk/products/lepton/?vertical=microcam\&segment=oem\&docPage=2\#Downloads +* **FLIR Windows Integration:** https://oem.flir.com/en-ca/developer/lepton-family/lepton-integration-with-windows/ +* **3.1R Dewarping App Note:** https://oem.flir.com/en-ca/learn/thermal-integration-made-easy/lepton-3.1r-dewarping-application-note/ +* **flirpy Repository:** https://www.google.com/search?q=https://github.com/groupgets/flirpy +* **uvc-radiometry.py Example:** The [purethermal1-uvc-capture repo](https://github.com/groupgets/purethermal1-uvc-capture/tree/master/python) contains a uvc-radiometry.py script. This is a great example of how to do what `flirpy` does, but manually using pyuvc. \ No newline at end of file From 5325b33c6ace6e2ad34805b93bd96192fc2155a7 Mon Sep 17 00:00:00 2001 From: mrichards03 Date: Mon, 27 Oct 2025 21:01:18 -0700 Subject: [PATCH 02/15] temp thermal cam code --- Tools/ThermalCamTemp.py | 732 +++++++++++++++++++++++++++++++++++++++ ros2_ws/requirements.txt | 4 + 2 files changed, 736 insertions(+) create mode 100644 Tools/ThermalCamTemp.py diff --git a/Tools/ThermalCamTemp.py b/Tools/ThermalCamTemp.py new file mode 100644 index 0000000..c4775ad --- /dev/null +++ b/Tools/ThermalCamTemp.py @@ -0,0 +1,732 @@ +""" +Thermal Hotspot GPS Locator - ROS Integration Version +Works with FLIR Lepton 2.5 or 3.1R on PureThermal 3 + +Two modes: +- SIMULATION: Testing on Windows with fake data +- ROS: Production on RPi with GPS data from ROS topics +""" + +import cv2 +import numpy as np +import math +import time +from datetime import datetime +import platform +import os +from flirpy.camera.lepton import Lepton + +# Operating mode configuration +IS_WINDOWS = platform.system() == "Windows" +SIMULATION_MODE = False # Set to False when running in ROS on rover +CALIBRATION_MODE = True # Set to True for testing with real camera + fake GPS +USE_ROS = not SIMULATION_MODE and not CALIBRATION_MODE # Use ROS only in production + +# ROS imports (only when USE_ROS is True) +if USE_ROS: + try: + import rospy + from sensor_msgs.msg import NavSatFix, Imu + from std_msgs.msg import Float64 + from geometry_msgs.msg import PoseStamped + from tf.transformations import euler_from_quaternion + print("ROS imports successful") + except ImportError: + print("ERROR: ROS not found. Install ROS or set SIMULATION_MODE = True") + exit(1) + +# Always needed +try: + from geopy.distance import distance as geopy_distance + from geopy.point import Point +except ImportError: + print("ERROR: geopy required. Install with: pip install geopy") + exit(1) + + +class ThermalGPSLocator: + def __init__(self, config): + """ + Initialize thermal GPS locator + + Args: + config: Dictionary with configuration parameters + """ + self.config = config + self.altitude = config['altitude_m'] + self.pitch = math.radians(config['pitch_deg']) + + # FLIR Lepton specs - Different for 2.5 vs 3.1R + if config['lepton_model'] == '2.5': + self.img_width = 80 + self.img_height = 60 + self.hfov = math.radians(51) # Horizontal FOV + self.vfov = math.radians(38) # Vertical FOV + elif config['lepton_model'] == '3.1R': + # Lepton 3.1R has higher resolution and wider FOV + self.img_width = 160 + self.img_height = 120 + self.hfov = math.radians(57) + self.vfov = math.radians(44) + else: + raise ValueError("lepton_model must be '2.5' or '3.1R'") + + print(f"Initialized for Lepton {config['lepton_model']}: {self.img_width}x{self.img_height}") + + # State variables + self.current_gps = None + self.current_heading = 0 + self.current_ground_speed = 0 # m/s + + # ROS-specific state + self.ros_gps_received = False + self.ros_heading_received = False + self.last_gps_time = None + + # Initialize ROS if enabled + if USE_ROS: + self._init_ros() + + def _init_ros(self): + """Initialize ROS node and subscribers""" + print("Initializing ROS interface...") + + # Initialize node if not already initialized + try: + rospy.init_node('thermal_gps_locator', anonymous=True) + print("ROS node initialized") + except rospy.exceptions.ROSException: + print("ROS node already initialized") + + # Subscribe to GPS topic + # Expects: sensor_msgs/NavSatFix with latitude, longitude, altitude + # IMPORTANT: Change 'ros_gps_topic' in config to match your rover's topic + rospy.Subscriber( + self.config['ros_gps_topic'], + NavSatFix, + self._gps_callback, + queue_size=1 + ) + print(f"Subscribed to GPS topic: {self.config['ros_gps_topic']}") + + # Subscribe to IMU/heading topic + # Expects: sensor_msgs/Imu with orientation quaternion + # IMPORTANT: Change 'ros_imu_topic' in config to match your rover's topic + if self.config['ros_imu_topic']: + rospy.Subscriber( + self.config['ros_imu_topic'], + Imu, + self._imu_callback, + queue_size=1 + ) + print(f"Subscribed to IMU topic: {self.config['ros_imu_topic']}") + + # Publisher for hotspot results + # Other ROS nodes can subscribe to this for navigation/logging + self.hotspot_pub = rospy.Publisher( + '/thermal/hotspot_gps', + PoseStamped, + queue_size=10 + ) + print("Publisher created: /thermal/hotspot_gps") + + # Wait for initial GPS message + print("Waiting for GPS data...") + timeout = rospy.Time.now() + rospy.Duration(10.0) + while not self.ros_gps_received and rospy.Time.now() < timeout: + rospy.sleep(0.1) + + if self.ros_gps_received: + print("GPS data received!") + else: + print("Warning: No GPS data received yet") + + def _gps_callback(self, msg): + """ + ROS callback for GPS data from rover + + Args: + msg: sensor_msgs/NavSatFix + - latitude (float64): Latitude in degrees + - longitude (float64): Longitude in degrees + - altitude (float64): Altitude in meters + """ + self.current_gps = (msg.latitude, msg.longitude) + + # Optionally use altitude from GPS instead of fixed camera height + # Uncomment if you want dynamic altitude: + # self.altitude = msg.altitude + self.config['camera_height_above_base'] + + self.ros_gps_received = True + self.last_gps_time = rospy.Time.now() + + if self.config.get('verbose', False): + print(f"GPS update: {msg.latitude:.6f}, {msg.longitude:.6f}") + + def _imu_callback(self, msg): + """ + ROS callback for IMU data to get heading + + Args: + msg: sensor_msgs/Imu + - orientation: Quaternion (x, y, z, w) + """ + # Convert quaternion to Euler angles (roll, pitch, yaw) + orientation = [ + msg.orientation.x, + msg.orientation.y, + msg.orientation.z, + msg.orientation.w + ] + + _, _, yaw = euler_from_quaternion(orientation) + + # Convert yaw from radians to degrees (0-360) + # Yaw is typically 0 = East, 90 = North in ROS + # Convert to compass heading: 0 = North, 90 = East + self.current_heading = (90 - math.degrees(yaw)) % 360 + + self.ros_heading_received = True + + if self.config.get('verbose', False): + print(f"Heading update: {self.current_heading:.1f}°") + + def get_gps_position(self): + """Get current GPS position (mode-dependent)""" + if SIMULATION_MODE: + # Simulation: Return fake GPS coordinates + self.current_gps = self.config['sim_gps'] + return self.current_gps + + if CALIBRATION_MODE: + # Calibration: Use test GPS position (you'll set this to match your test location) + self.current_gps = self.config['test_gps'] + print(f"Using test GPS: {self.current_gps[0]:.6f}, {self.current_gps[1]:.6f}") + return self.current_gps + + # ROS mode: Data comes from callback + if not self.ros_gps_received: + print("Warning: No GPS data received from ROS topic yet") + return None + + # Check if data is recent (within last 2 seconds) + if self.last_gps_time: + age = (rospy.Time.now() - self.last_gps_time).to_sec() + if age > 2.0: + print(f"Warning: GPS data is {age:.1f}s old") + + return self.current_gps + + def get_heading(self): + """Get current compass heading in degrees (0-360)""" + if SIMULATION_MODE: + # Simulation: Return fake heading + self.current_heading = self.config['sim_heading'] + return self.current_heading + + if CALIBRATION_MODE: + # Calibration: Use test heading (direction camera is pointing) + self.current_heading = self.config['test_heading'] + print(f"Using test heading: {self.current_heading:.1f}°") + return self.current_heading + + # ROS mode: Data comes from IMU callback + if not self.ros_heading_received: + print("Warning: No heading data received from ROS IMU topic") + # Could implement fallback: calculate heading from consecutive GPS points + # if rover is moving fast enough + + return self.current_heading + + def capture_thermal_image(self): + """Capture thermal image from FLIR Lepton""" + if SIMULATION_MODE: + # Generate simulated thermal image with hotspots + # Units: centikelvin (Kelvin * 100) + # Room temp ~20°C = 29315 centikelvin + img = np.random.randint(29000, 30000, + (self.img_height, self.img_width), + dtype=np.uint16) + + # Add simulated hotspots at different temperatures + # Hotspot 1: 32°C (30515 centikelvin) - center-right + cv2.circle(img, (self.img_width//2 + 10, self.img_height//2 - 5), 8, 30515, -1) + + # Hotspot 2: 35°C (30815 centikelvin) - upper-left + cv2.circle(img, (self.img_width//3, self.img_height//3), 5, 30815, -1) + + # Hotspot 3: 38°C (31115 centikelvin) - lower-center + cv2.circle(img, (self.img_width//2, int(self.img_height*0.7)), 6, 31115, -1) + + print(f"Simulated thermal image: {img.shape}, range: {img.min()}-{img.max()} centikelvin") + return img + + + # Fallback: OpenCV capture + if IS_WINDOWS or platform.system() == "Linux": + try: + with Lepton() as camera: + # Get raw thermal data + # Print camera diagnostic fields (safe access) + thermal = camera.grab() + + if thermal is None: + raise RuntimeError("Failed to capture frame from Lepton") + + print(thermal.min(), thermal.max()) + + # Convert from raw values to Celsius + temp_celsius = (thermal.astype(np.float32) / 100.0) - 273.15 + + temp_celsius = cv2.flip(temp_celsius, 0) # Flip vertically + + print(f"Captured {thermal.shape} thermal frame") + print(f"Temperature range: {temp_celsius.min():.1f}°C to {temp_celsius.max():.1f}°C") + + # Save both raw and visualization images + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") + + # Create colored visualization + display_img = ((temp_celsius - temp_celsius.min()) / + (temp_celsius.max() - temp_celsius.min()) * 255).astype(np.uint8) + display_color = cv2.applyColorMap(display_img, cv2.COLORMAP_INFERNO) + + # Add temperature overlay + cv2.putText(display_color, f"Min: {temp_celsius.min():.1f}°C", (10, 20), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + cv2.putText(display_color, f"Max: {temp_celsius.max():.1f}°C", (10, 40), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + + # Save the visualization with temperature overlay + vis_filename = f"thermal_{timestamp}_viz.png" + cv2.imwrite(vis_filename, display_color) + + # Save the raw thermal data for analysis + raw_filename = f"thermal_{timestamp}_raw.npy" + np.save(raw_filename, thermal) + + print(f"Saved images:") + print(f"- Visualization: {vis_filename}") + print(f"- Raw data: {raw_filename}") + + camera.close() + + # Return the temperature data in Celsius + return temp_celsius + + except Exception as e: + print(f"Error capturing thermal image: {e}") + print("\nDiagnostic information:") + print("1. Check if the FLIR Lepton is properly connected") + print("2. Verify /dev/video1 permissions (try: ls -l /dev/video1)") + print("3. Make sure no other process is using the camera") + print("\nTrying to reset USB device...") + + try: + import subprocess + # Try to reset the USB device + subprocess.run(['sudo', 'usb_modeswitch', '-R', '-v', '0x1e4e', '-p', '0x0100']) + print("USB device reset attempted") + except Exception as reset_error: + print(f"Could not reset USB device: {reset_error}") + + import traceback + traceback.print_exc() + + return None + + def find_hotspots(self, thermal_img, temp_threshold): + """ + Find hotspot blobs (groups of hot pixels) above temperature threshold + + Uses connected component analysis to group adjacent hot pixels into blobs, + then returns the centroid and max temperature of each blob. + + Args: + thermal_img: Raw thermal image in celcius + temp_threshold: Temperature threshold in celcius + + Returns: + List of (centroid_x, centroid_y, max_temp, blob_size) tuples + sorted by max temperature (hottest first) + """ + # Create binary mask of pixels above threshold + hot_mask = (thermal_img > temp_threshold).astype(np.uint8) + + # Find connected components (blobs of hot pixels) + # connectivity=8 means diagonal pixels are considered connected + num_blobs, labels, stats, centroids = cv2.connectedComponentsWithStats( + hot_mask, connectivity=8 + ) + + hotspots = [] + + # Skip label 0 (background) + for i in range(1, num_blobs): + # Get blob statistics + centroid_x = centroids[i][0] + centroid_y = centroids[i][1] + blob_size = stats[i, cv2.CC_STAT_AREA] # Number of pixels in blob + + # Find maximum temperature within this blob + blob_mask = (labels == i) + max_temp = thermal_img[blob_mask].max() + avg_temp = thermal_img[blob_mask].mean() + + hotspots.append({ + 'centroid_x': float(centroid_x), + 'centroid_y': float(centroid_y), + 'max_temp': float(max_temp), + 'avg_temp': float(avg_temp), + 'size_pixels': int(blob_size) + }) + + # Sort by maximum temperature (hottest first) + hotspots.sort(key=lambda h: h['max_temp'], reverse=True) + + print(f"Found {len(hotspots)} hotspot blobs above {temp_threshold:.1f}°C") + if hotspots: + print(f" Largest blob: {hotspots[0]['size_pixels']} pixels, " + f"Max temp: {hotspots[0]['max_temp']:.1f}°C") + + return hotspots + + def pixel_to_angle(self, x, y): + """ + Convert pixel coordinates to angular offsets from camera center + + Args: + x, y: Pixel coordinates in image + + Returns: + (azimuth_offset, elevation_offset) in radians + - azimuth_offset: Horizontal angle (positive = right) + - elevation_offset: Vertical angle (positive = up) + """ + # Normalize to -0.5 to 0.5 range + norm_x = (x - self.img_width / 2) / self.img_width + norm_y = (y - self.img_height / 2) / self.img_height + + # Convert to angles using field of view + azimuth_offset = norm_x * self.hfov + elevation_offset = norm_y * self.vfov + + return azimuth_offset, elevation_offset + + def estimate_ground_distance(self, elevation_offset): + """ + Estimate horizontal ground distance to hotspot + + Uses camera altitude and pitch angle to estimate where the + pixel's line of sight intersects the ground plane. + + Args: + elevation_offset: Vertical angle offset from camera center (radians) + + Returns: + Distance in meters (horizontal ground distance) or None if invalid + """ + # Calculate total elevation angle to hotspot + # Positive elevation_offset means looking up in image + # We want positive to mean looking down at ground + elevation_angle = self.pitch - elevation_offset + + # Check if looking above horizontal or straight down + if elevation_angle <= 0 or elevation_angle >= math.pi / 2: + return None + + # Calculate ground distance using trigonometry + # distance = height / tan(angle) + ground_distance = self.altitude / math.tan(elevation_angle) + + # Sanity check: reject unreasonable distances + if ground_distance < 0 or ground_distance > 1000: + return None + + return ground_distance + + def calculate_hotspot_gps(self, pixel_x, pixel_y): + """ + Calculate GPS coordinates of a hotspot from its pixel location + + Args: + pixel_x, pixel_y: Pixel coordinates in thermal image + + Returns: + Tuple of (latitude, longitude, bearing, distance_m) or None + """ + if self.current_gps is None: + print("Warning: No GPS position available") + return None + + # Get angular offsets from image center + azimuth_offset, elevation_offset = self.pixel_to_angle(pixel_x, pixel_y) + + # Calculate absolute compass bearing to hotspot + bearing = (self.current_heading + math.degrees(azimuth_offset)) % 360 + + # Estimate ground distance + ground_dist = self.estimate_ground_distance(elevation_offset) + if ground_dist is None or ground_dist < 0: + return None + + # Calculate GPS coordinates using bearing and distance + start = Point(self.current_gps[0], self.current_gps[1]) + destination = geopy_distance(meters=ground_dist).destination(start, bearing) + + return (destination.latitude, destination.longitude, bearing, ground_dist) + + def publish_hotspot_ros(self, hotspot_data): + """ + Publish hotspot GPS coordinates to ROS topic + + Args: + hotspot_data: Dictionary with hotspot information + """ + if not USE_ROS: + return + + # Create PoseStamped message + msg = PoseStamped() + msg.header.stamp = rospy.Time.now() + msg.header.frame_id = "map" # Adjust if using different coordinate frame + + # Position: Use lat/lon as x/y (or convert to local coordinates) + msg.pose.position.x = hotspot_data['longitude'] + msg.pose.position.y = hotspot_data['latitude'] + msg.pose.position.z = 0 # Ground level + + # Orientation: Not meaningful for a point, but required + msg.pose.orientation.w = 1.0 + + self.hotspot_pub.publish(msg) + + if self.config.get('verbose', False): + print(f"Published hotspot to /thermal/hotspot_gps") + + def find_nearest_hotspot_gps(self, temp_threshold_celsius): + """ + Main function: Find GPS coordinates of nearest thermal hotspot + + Args: + temp_threshold_celsius: Temperature threshold in Celsius + + Returns: + Dictionary with hotspot information or None if no hotspots found + """ + print("\n" + "="*60) + print("THERMAL HOTSPOT GPS LOCATOR") + print(f"Mode: {'SIMULATION' if SIMULATION_MODE else 'ROS'}") + print("="*60) + + # Step 1: Get current GPS position + print("\n1. Getting GPS position...") + self.get_gps_position() + if self.current_gps is None: + print("ERROR: No GPS fix available") + return None + print(f" Current GPS: {self.current_gps[0]:.6f}, {self.current_gps[1]:.6f}") + + # Step 2: Get compass heading + print("\n2. Getting compass heading...") + self.get_heading() + print(f" Current heading: {self.current_heading:.1f}°") + + # Step 3: Capture thermal image + print("\n3. Capturing thermal image...") + thermal_img = self.capture_thermal_image() + if thermal_img is None: + print("ERROR: Failed to capture thermal image") + return None + + # Step 4: Find hotspots above threshold + print(f"\n4. Finding hotspots above {temp_threshold_celsius:.1f}°C...") + hotspots = self.find_hotspots(thermal_img, temp_threshold_celsius) + + if not hotspots: + print(" No hotspots found above threshold") + return None + + # Step 5: Calculate GPS coordinates for each hotspot blob + print("\n5. Calculating GPS coordinates for hotspots...") + nearest = None + min_distance = float('inf') + valid_hotspots = [] + + # Process top 10 hottest blobs + for i, blob in enumerate(hotspots[:10]): + x = blob['centroid_x'] + y = blob['centroid_y'] + max_temp = blob['max_temp'] + avg_temp = blob['avg_temp'] + size = blob['size_pixels'] + + result = self.calculate_hotspot_gps(x, y) + if result: + lat, lon, bearing, dist = result + temp_c = max_temp + avg_temp_c = avg_temp + + hotspot_info = { + 'centroid_x': x, + 'centroid_y': y, + 'max_temperature_c': temp_c, + 'avg_temperature_c': avg_temp_c, + 'size_pixels': size, + 'latitude': lat, + 'longitude': lon, + 'bearing': bearing, + 'distance_m': dist + } + valid_hotspots.append(hotspot_info) + + print(f" Blob {i+1}: {size} pixels, max {temp_c:.1f}°C, avg {avg_temp_c:.1f}°C") + print(f" -> {lat:.6f},{lon:.6f} (bearing: {bearing:.1f}°, dist: {dist:.1f}m)") + + # Track nearest hotspot + if dist < min_distance: + min_distance = dist + nearest = hotspot_info + + if nearest is None: + print(" Could not calculate GPS for any hotspots") + return None + + # Publish to ROS if enabled + if USE_ROS: + self.publish_hotspot_ros(nearest) + + # Print summary + print("\n" + "="*60) + print("NEAREST HOTSPOT FOUND:") + print("="*60) + print(f"Blob Size: {nearest['size_pixels']} pixels") + print(f"Max Temp: {nearest['max_temperature_c']:.1f}°C") + print(f"Avg Temp: {nearest['avg_temperature_c']:.1f}°C") + print(f"GPS: {nearest['latitude']:.6f}, {nearest['longitude']:.6f}") + print(f"Bearing: {nearest['bearing']:.1f}° from North") + print(f"Distance: {nearest['distance_m']:.1f} meters") + print(f"Google Maps: https://www.google.com/maps?q={nearest['latitude']},{nearest['longitude']}") + print("="*60 + "\n") + + return nearest + + +def main(): + """Main function with configuration""" + + # ==================== CONFIGURATION ==================== + config = { + # Hardware settings + 'lepton_model': '2.5', # Change to '3.1R' when you upgrade + 'camera_index': 0, # USB camera index (try 0, 1, 2 if not working) + + # ROS settings (ROS mode only) + # IMPORTANT: Update these to match your rover's actual topic names + 'ros_gps_topic': '/gps/fix', # Your rover's GPS topic + 'ros_imu_topic': '/imu/data', # Your rover's IMU topic (or None if no IMU) + + # Camera mounting geometry + # CALIBRATION MODE: Measure these values carefully! + 'altitude_m': 0, # Camera height above ground (meters) - MEASURE THIS + 'pitch_deg': 0, # Camera pitch angle (0=horizontal, 90=down) - MEASURE THIS + + # Detection parameters + 'temp_threshold_celsius': -90.0, # Only detect above this temperature + + # Simulation settings (SIMULATION_MODE only) + 'sim_gps': (49.8880, -119.4960), # Example: Kelowna, BC + 'sim_heading': 307.0, # Simulated compass heading (45° = NE) + + # Calibration/Testing settings (CALIBRATION_MODE only) + # Set these to match your test setup location + 'test_gps': (49.8880, -119.4960), # Your actual test location GPS + 'test_heading': 292.0, # Direction camera is pointing (0=North, 90=East, etc.) + 'save_images': True, # Save thermal images during testing + + # Debug options + 'verbose': False, # Set True for detailed GPS/IMU update messages + } + + print("="*60) + print("THERMAL HOTSPOT GPS LOCATOR - ROS INTEGRATION") + print("="*60) + + mode_str = "SIMULATION" if SIMULATION_MODE else ("CALIBRATION" if CALIBRATION_MODE else "ROS") + print(f"Mode: {mode_str}") + print(f"Platform: {platform.system()}") + print(f"Lepton Model: {config['lepton_model']}") + + if CALIBRATION_MODE: + print("\n" + "="*60) + print("CALIBRATION MODE - Testing Instructions") + print("="*60) + print("1. Set up your camera at a measured height and angle") + print("2. Update config values:") + print(f" - 'test_gps': Your camera's GPS position") + print(f" - 'test_heading': Direction camera points (0=N, 90=E, 180=S, 270=W)") + print(f" - 'altitude_m': Measured camera height = {config['altitude_m']:.2f}m") + print(f" - 'pitch_deg': Measured camera angle = {config['pitch_deg']:.1f}°") + print("3. Place a hot object (person, heater, etc.) at a known distance") + print("4. Measure actual distance with tape measure") + print("5. Run this script and compare predicted vs actual GPS/distance") + print("6. Thermal images will be saved for review") + print("="*60 + "\n") + + if USE_ROS: + print(f"GPS Topic: {config['ros_gps_topic']}") + print(f"IMU Topic: {config['ros_imu_topic']}") + + print("="*60 + "\n") + + # ==================== RUN ==================== + try: + locator = ThermalGPSLocator(config) + + if USE_ROS: + # ROS mode: Run continuously at set rate + rate = rospy.Rate(1) # 1 Hz - scan once per second + + print("Running in ROS mode. Press Ctrl+C to stop.\n") + + scan_count = 0 + while not rospy.is_shutdown(): + scan_count += 1 + print(f"\n[Scan #{scan_count}]") + + result = locator.find_nearest_hotspot_gps( + config['temp_threshold_celsius'] + ) + + if result: + print(f"✓ Hotspot detected and published") + else: + print("○ No hotspots detected") + + rate.sleep() + else: + # Simulation mode: Single scan for testing + result = locator.find_nearest_hotspot_gps( + config['temp_threshold_celsius'] + ) + + if result: + timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S") + print(f"\n✓ Success! Scan completed at {timestamp}") + else: + print("\n○ No hotspots found") + + except KeyboardInterrupt: + print("\n\n" + "="*60) + print("SHUTDOWN: Interrupted by user") + print("="*60) + if USE_ROS: + rospy.signal_shutdown("User interrupt") + except Exception as e: + print(f"\n\nERROR: {e}") + import traceback + traceback.print_exc() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/ros2_ws/requirements.txt b/ros2_ws/requirements.txt index ad29659..b03d6fb 100644 --- a/ros2_ws/requirements.txt +++ b/ros2_ws/requirements.txt @@ -2,3 +2,7 @@ pyserial pymavlink dronekit smbus +flirpy +numpy +opencv-python +geopy \ No newline at end of file From 8126853d86ec7e61188489edf370c2587cdcdf64 Mon Sep 17 00:00:00 2001 From: mrichards03 Date: Tue, 28 Oct 2025 13:05:12 -0700 Subject: [PATCH 03/15] untested, unfinished, thermal ros code --- Documentation/2025/Thermal Camera.md | 10 +- README.md | 6 +- ros2_ws/README.md | 20 +- ros2_ws/src/embr/embr/getCube.py | 22 +- ros2_ws/src/embr/embr/getThermal.py | 126 ------ ros2_ws/src/embr/embr/sendRf.py | 4 +- .../src/embr/embr/thermalHotspotLocator.py | 233 +++++++++++ ros2_ws/src/embr/embr/thermalStream.py | 373 ++++++++++++++++++ ros2_ws/src/embr/launch/embr_launch.py | 9 +- ros2_ws/src/embr/package.xml | 2 + ros2_ws/src/embr/setup.py | 2 + ros2_ws/src/msg_interface/CMakeLists.txt | 2 +- ros2_ws/src/msg_interface/msg/GPSAndIMU.msg | 7 + ros2_ws/src/msg_interface/msg/Gps.msg | 4 - 14 files changed, 668 insertions(+), 152 deletions(-) delete mode 100644 ros2_ws/src/embr/embr/getThermal.py create mode 100644 ros2_ws/src/embr/embr/thermalHotspotLocator.py create mode 100644 ros2_ws/src/embr/embr/thermalStream.py create mode 100644 ros2_ws/src/msg_interface/msg/GPSAndIMU.msg delete mode 100644 ros2_ws/src/msg_interface/msg/Gps.msg diff --git a/Documentation/2025/Thermal Camera.md b/Documentation/2025/Thermal Camera.md index b5ac7d4..1abbf07 100644 --- a/Documentation/2025/Thermal Camera.md +++ b/Documentation/2025/Thermal Camera.md @@ -19,21 +19,21 @@ This is why we need a software stack based on `libuvc`. For accessing temperature data in Python, `flirpy` is the most direct and recommended tool. It handles all the low-level complexity for you. -### **1\. `libuvc` (The C-Library / "Driver")** +### **1. `libuvc` (The C-Library / "Driver")** -* **What it is:** A low-level C library that enables direct communication with USB Video Class (UVC) devices like your PureThermal 3\. +* **What it is:** A low-level C library that enables direct communication with USB Video Class (UVC) devices like your PureThermal 3. * **Why you need it:** It's the foundation that allows `flirpy` (via `pyuvc`) to send the custom commands to enable radiometric mode. It is a required system-level dependency. * **Installation:** * **Ubuntu/Debian:** `sudo apt install libuvc-dev` * **macOS:** `brew install libuvc` * **Windows:** This is more complex; it's often easier to use the Windows-specific FLIR application for diagnostics. -### **2\. `pyuvc` (The Python Wrapper)** +### **2. `pyuvc` (The Python Wrapper)** * **What it is:** A Python library that "wraps" libuvc. It allows Python code to access the low-level controls of the UVC device. * **Why you need it:** `flirpy` uses this library to do the actual work. You don't interact with it directly, but `flirpy` will install it as a dependency. -### **3\. `flirpy` (The High-Level FLIR Library)** +### **3. `flirpy` (The High-Level FLIR Library)** * **What it is:** A high-level Python library specifically for FLIR Lepton cameras on PureThermal boards. * **Why you need it:** This is the tool for your job. It finds the camera, sends the command to enable radiometry, and grabs the 14-bit raw image. This image is a 2D NumPy array where values are in **centiKelvin**. You must convert this to Celsius manually. @@ -125,5 +125,5 @@ These are other tools you might encounter, but they are generally *not* what you * **FLIR Lepton Page (Downloads):** https://oem.flir.com/en-hk/products/lepton/?vertical=microcam\&segment=oem\&docPage=2\#Downloads * **FLIR Windows Integration:** https://oem.flir.com/en-ca/developer/lepton-family/lepton-integration-with-windows/ * **3.1R Dewarping App Note:** https://oem.flir.com/en-ca/learn/thermal-integration-made-easy/lepton-3.1r-dewarping-application-note/ -* **flirpy Repository:** https://www.google.com/search?q=https://github.com/groupgets/flirpy +* **flirpy Repository:** https://github.com/LJMUAstroecology/flirpy * **uvc-radiometry.py Example:** The [purethermal1-uvc-capture repo](https://github.com/groupgets/purethermal1-uvc-capture/tree/master/python) contains a uvc-radiometry.py script. This is a great example of how to do what `flirpy` does, but manually using pyuvc. \ No newline at end of file diff --git a/README.md b/README.md index 91e1554..2f57217 100644 --- a/README.md +++ b/README.md @@ -222,8 +222,8 @@ The EMBR-Bot system consists of three main ROS2 nodes: ### 1. getCube Node - **Purpose**: Reads telemetry from Cube Orange flight controller - **Device**: `/dev/ttyAMA0` (UART0) -- **Published Topic**: `gps` (GPS location, altitude, velocity) -- **Data**: Latitude, Longitude, Altitude, Ground Speed +- **Published Topic**: `gps_imu` (GPS location, altitude, velocity, yaw, pitch, roll) +- **Data**: Latitude, Longitude, Altitude, Ground Speed, Yaw, Pitch, Roll ### 2. getTemp Node - **Purpose**: Reads temperature data from Arduino sensor @@ -234,7 +234,7 @@ The EMBR-Bot system consists of three main ROS2 nodes: ### 3. sendRf Node - **Purpose**: Transmits data via RFD 900x radio using MAVLink protocol - **Device**: `/dev/ttyAMA1` (UART2) -- **Subscribed Topics**: `gps`, `temperature`, `/pointcloud` (LIDAR) +- **Subscribed Topics**: `gps_imu`, `temperature`, `/pointcloud` (LIDAR) - **Protocol**: MAVLink v2.0 - **Features**: - Transmits GPS and temperature data diff --git a/ros2_ws/README.md b/ros2_ws/README.md index ae0c3b7..da447c7 100644 --- a/ros2_ws/README.md +++ b/ros2_ws/README.md @@ -56,10 +56,28 @@ src/ │ ├── launch/ # embr_launch.py │ └── setup.py # entry points └── msg_interface/ - ├── msg/ # custom msgs (e.g., Gps.msg) + ├── msg/ # custom msgs (e.g., GPSAndIMU.msg) └── package.xml ``` +## Nodes + +### getCube +Interfaces with Cube Orange flight controller via MAVLink. + +### getTemp +Publishes temperature data from serial sensor. + +### sendRf +Handles RF communication for telemetry. + +### thermalStream +**New!** Thermal camera streaming node with HDMI output and intelligent frame publishing. +- Streams color-mapped thermal video over HDMI using FFmpeg +- Highlights and annotates hotspots above configurable temperature threshold +- Publishes raw radiometric frames when vehicle velocity is zero +- See [Thermal Stream Node Documentation](../Documentation/2025/Thermal-Stream-Node.md) for details + ## Also see - Top-level quick start: `../README.md` diff --git a/ros2_ws/src/embr/embr/getCube.py b/ros2_ws/src/embr/embr/getCube.py index b9ed7bf..e7b1c4b 100755 --- a/ros2_ws/src/embr/embr/getCube.py +++ b/ros2_ws/src/embr/embr/getCube.py @@ -2,25 +2,29 @@ from rclpy.node import Node import time from dronekit import connect -from msg_interface.msg import Gps +from msg_interface.msg import GPSAndIMU class AttitudePublisher(Node): def __init__(self): super().__init__('attitude_publisher') - self.publisher_ = self.create_publisher(Gps, 'gps', 10) + self.publisher_ = self.create_publisher(GPSAndIMU, 'gps_imu', 10) self.vehicle = connect("/dev/ttyAMA0", wait_ready=False, baud=57600) self.get_logger().info('Attitude Publisher node initialized') def publish_attitude(self): while True: location = self.vehicle.location.global_frame - gps_msg = Gps() - gps_msg.lat = int(location.lat * 1e7) - gps_msg.lon = int(location.lon * 1e7) - gps_msg.alt = int(location.alt * 1000) - gps_msg.vel = self.vehicle.groundspeed - self.get_logger().info(f'Published Telem Data: Lat: {gps_msg.lat} Lon: {gps_msg.lon} Alt: {gps_msg.alt} Velocity: {gps_msg.vel}') - self.publisher_.publish(gps_msg) + attitude = self.vehicle.attitude + gps_imu_msg = GPSAndIMU() + gps_imu_msg.lat = int(location.lat * 1e7) + gps_imu_msg.lon = int(location.lon * 1e7) + gps_imu_msg.alt = int(location.alt * 1000) + gps_imu_msg.vel = self.vehicle.groundspeed + gps_imu_msg.yaw = attitude.yaw + gps_imu_msg.pitch = attitude.pitch + gps_imu_msg.roll = attitude.roll + self.get_logger().info(f'Published Telem Data: Lat: {gps_imu_msg.lat} Lon: {gps_imu_msg.lon} Alt: {gps_imu_msg.alt} Velocity: {gps_imu_msg.vel} Yaw: {gps_imu_msg.yaw} Pitch: {gps_imu_msg.pitch} Roll: {gps_imu_msg.roll}') + self.publisher_.publish(gps_imu_msg) time.sleep(1) def main(args=None): diff --git a/ros2_ws/src/embr/embr/getThermal.py b/ros2_ws/src/embr/embr/getThermal.py deleted file mode 100644 index 5a8d3e5..0000000 --- a/ros2_ws/src/embr/embr/getThermal.py +++ /dev/null @@ -1,126 +0,0 @@ -import cv2 -import numpy as np -import math -#rclpy provides the canonical Python API for interacting with ROS 2. -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Image -from std_msgs.msg import String -from cv_bridge import CvBridge - -CAMERA_ID = 1 - -class WebcamProcessor(Node): - def __init__(self): - super().__init__('webcam_processor') - self.bridge = CvBridge() - self.image_pub = self.create_publisher(Image, 'webcam/image', 10) - self.bbox_pub = self.create_publisher(String, 'webcam/bounding_boxes', 10) - self.timer = self.create_timer(0.1, self.timer_callback) - - self.webcam_stream = cv2.VideoCapture(CAMERA_ID) - if not self.webcam_stream.isOpened(): - self.get_logger().error(f"Error: Could not open webcam with CAMERA_ID '{CAMERA_ID}'") - else: - self.get_logger().info("Webcam stream initialized.") - - def timer_callback(self): - ret, frame = self.webcam_stream.read() - if not ret: - self.get_logger().error("Error: Could not read frame from webcam.") - return - - processed_frame, all_boxes, largest_box = self.process_frame_with_bounding_box(frame) - - image_msg = self.bridge.cv2_to_imgmsg(processed_frame, encoding="bgr8") - self.image_pub.publish(image_msg) - - if largest_box: - self.bbox_pub.publish(String(data=str(largest_box))) - self.get_logger().info(f"Largest Box: {largest_box}") - - def process_frame_with_bounding_box(self, frame, nms_threshold=0.4): - gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - blur_frame = cv2.GaussianBlur(gray_frame, (1, 1), 0) - - _, maxVal, _, _ = cv2.minMaxLoc(blur_frame) - threshold_value = maxVal * 0.85 - _, thresh_frame = cv2.threshold(blur_frame, threshold_value, 255, cv2.THRESH_BINARY) - - contours, _ = cv2.findContours(thresh_frame, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - boxes = [] - for cnt in contours: - x, y, w, h = cv2.boundingRect(cnt) - boxes.append([x, y, x + w, y + h]) - - if len(boxes) == 0: - return frame, [], {} - - boxes = np.array(boxes) - - indices = cv2.dnn.NMSBoxes( - bboxes=boxes.tolist(), - scores=[1.0] * len(boxes), - score_threshold=0.0, - nms_threshold=nms_threshold - ) - - all_boxes = [] - largest_area = 0 - largest_box = {} - - for i in indices: - i = i[0] - x1, y1, x2, y2 = boxes[i] - cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 0, 0), 1) - box_dict = {"x1": x1, "y1": y1, "x2": x2, "y2": y2} - all_boxes.append(box_dict) - - width = x2 - x1 - height = y2 - y1 - area = width * height - if area > largest_area: - largest_area = area - x_center = x1 + width // 2 - y_center = y1 + height // 2 - largest_box = { - "x1": x1, - "y1": y1, - "x2": x2, - "y2": y2, - "x_center": x_center, - "y_center": y_center - } - - if largest_box: - frame_height, frame_width = frame.shape[:2] - bottom_center_x = frame_width / 2 - bottom_center_y = frame_height - dx = largest_box["x_center"] - bottom_center_x - dy = bottom_center_y - largest_box["y_center"] - angle_radians = math.atan2(dx, dy) - angle_degrees = angle_radians * 180 / math.pi - - largest_box["dx"] = dx - largest_box["dy"] = dy - largest_box["angle_degrees"] = angle_degrees - - return frame, all_boxes, largest_box - - def destroy_node(self): - if self.webcam_stream.isOpened(): - self.webcam_stream.release() - super().destroy_node() - -def main(args=None): - rclpy.init(args=args) - node = WebcamProcessor() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/ros2_ws/src/embr/embr/sendRf.py b/ros2_ws/src/embr/embr/sendRf.py index 69a4b90..9d320cc 100755 --- a/ros2_ws/src/embr/embr/sendRf.py +++ b/ros2_ws/src/embr/embr/sendRf.py @@ -3,7 +3,7 @@ import rclpy import time from rclpy.node import Node -from msg_interface.msg import Gps +from msg_interface.msg import GPSAndIMU from pymavlink.dialects.v20 import common as mavlink2 from pymavlink import mavutil from sensor_msgs.msg import Temperature @@ -15,7 +15,7 @@ class CommSubscriber(Node): def __init__(self): super().__init__('mavlink_subscriber') - self.subscription = self.create_subscription(Gps, 'gps', self.cube_callback, 10) + self.subscription = self.create_subscription(GPSAndIMU, 'gps_imu', self.cube_callback, 10) self.subscription_temperature = self.create_subscription(Temperature, 'temperature', self.temperature_callback, 10) self.subscription # prevent unused variable warning self.mavlink_connection = mavutil.mavserial(device='/dev/ttyAMA1', baud=57600) diff --git a/ros2_ws/src/embr/embr/thermalHotspotLocator.py b/ros2_ws/src/embr/embr/thermalHotspotLocator.py new file mode 100644 index 0000000..46f0692 --- /dev/null +++ b/ros2_ws/src/embr/embr/thermalHotspotLocator.py @@ -0,0 +1,233 @@ +#!/usr/bin/env python3 +""" +Thermal Hotspot GPS Locator (ROS2) + +Production ROS2 node that subscribes to the radiometric 2D array +published by the thermal stream node and computes the nearest hotspot's +GPS coordinates based on camera geometry, GPS, and IMU heading. + +Inputs: +- /thermal/radiometric_array (std_msgs/UInt16MultiArray) — raw radiometric data (Kelvin x 100) +- /gps_imu (msg_interface/GPSAndIMU) — GPS position and orientation + +Outputs: +- /thermal/hotspot_gps (geometry_msgs/PoseStamped) — nearest hotspot lat/lon encoded as x/y + +""" + +import math +from typing import Optional, Tuple, List, Dict + +import rclpy +from rclpy.node import Node +from std_msgs.msg import UInt16MultiArray +from geometry_msgs.msg import PoseStamped +from msg_interface.msg import GPSAndIMU + +import numpy as np +import cv2 +from geopy.distance import distance as geopy_distance +from geopy.point import Point + + +class ThermalHotspotLocator(Node): + def __init__(self): + super().__init__('thermal_hotspot_locator') + + # Configuration (can be adjusted here) + # Camera geometry + self.lepton_model = '2.5' # '2.5' or '3.1R' + self.altitude_m = 1.0 # Camera height above ground (meters) + self.pitch_deg = 45.0 # Camera pitch (0=horizontal, 90=down) + self.temp_threshold_c = 30.0 # Celsius threshold for hotspot detection + + # Topics (override here if needed) + self.array_topic = 'thermal/radiometric_array' + self.gps_imu_topic = 'gps_imu' + + # Set Lepton model parameters + self._set_camera_model(self.lepton_model) + + # State + self.current_gps: Optional[Tuple[float, float]] = None # (lat, lon) + self.current_heading_deg: float = 0.0 # compass heading 0-360, 0=N, 90=E + self.have_gps: bool = False + + # Subscribers + self.create_subscription(GPSAndIMU, self.gps_imu_topic, self._gps_imu_cb, 10) + self.create_subscription(UInt16MultiArray, self.array_topic, self._array_cb, 10) + + # Publisher + self.hotspot_pub = self.create_publisher(PoseStamped, '/thermal/hotspot_gps', 10) + + self.get_logger().info('ThermalHotspotLocator started') + + def _set_camera_model(self, model: str) -> None: + if model == '2.5': + # Lepton 2.5 + self.hfov = math.radians(51) + self.vfov = math.radians(38) + elif model == '3.1R': + self.hfov = math.radians(57) + self.vfov = math.radians(44) + else: + self.get_logger().warn("Unknown lepton_model, defaulting to 2.5 FOV") + self.hfov = math.radians(51) + self.vfov = math.radians(38) + + # -------------------- Subscribers -------------------- + def _gps_imu_cb(self, msg: GPSAndIMU) -> None: + # Extract GPS (lat/lon are int32 in 1e7 format, alt is int32 in mm) + self.current_gps = (float(msg.lat) / 1e7, float(msg.lon) / 1e7) + self.have_gps = True + + # Extract heading from yaw (yaw is in radians as int32, needs conversion) + # The yaw field appears to be in radians based on getCube.py using attitude.yaw + yaw_rad = float(msg.yaw) + + # Convert yaw to compass heading: 0 = North, 90 = East + # Assuming yaw is ENU convention: 0 = East, +pi/2 = North + heading = (90.0 - math.degrees(yaw_rad)) % 360.0 + self.current_heading_deg = heading + + def _array_cb(self, msg: UInt16MultiArray) -> None: + if not self.have_gps: + self.get_logger().warn_once('Waiting for GPS fix...') + return + + # Determine image shape from layout + if len(msg.layout.dim) >= 2: + h = msg.layout.dim[0].size + w = msg.layout.dim[1].size + else: + # Fallback: assume Lepton 2.5 default if no layout + h, w = 60, 80 + + rad = np.array(msg.data, dtype=np.uint16) + if rad.size != h * w: + self.get_logger().warn(f'Array size mismatch: {rad.size} vs {h}x{w}') + return + rad = rad.reshape((h, w)) + + # Convert to Celsius for analysis + temp_c = (rad.astype(np.float32) / 100.0) - 273.15 + + # Detect hotspots and publish nearest + hotspots = self._find_hotspots(temp_c, self.temp_threshold_c) + if not hotspots: + return + + nearest = self._compute_nearest_hotspot_gps(hotspots, w, h) + if nearest is None: + return + + self._publish_hotspot(nearest) + + # -------------------- Processing -------------------- + def _find_hotspots(self, temp_c: np.ndarray, threshold_c: float) -> List[Dict]: + hot_mask = (temp_c > threshold_c).astype(np.uint8) + num, labels, stats, centroids = cv2.connectedComponentsWithStats(hot_mask, connectivity=8) + + out: List[Dict] = [] + for i in range(1, num): # skip background + cx, cy = float(centroids[i][0]), float(centroids[i][1]) + size = int(stats[i, cv2.CC_STAT_AREA]) + blob_mask = (labels == i) + max_temp = float(temp_c[blob_mask].max()) + avg_temp = float(temp_c[blob_mask].mean()) + out.append({ + 'centroid_x': cx, + 'centroid_y': cy, + 'size_pixels': size, + 'max_temp': max_temp, + 'avg_temp': avg_temp, + }) + + out.sort(key=lambda h: h['max_temp'], reverse=True) + return out + + def _pixel_to_angle(self, x: float, y: float, width: int, height: int) -> Tuple[float, float]: + # Normalize coordinates to [-0.5, 0.5] + nx = (x - width / 2.0) / width + ny = (y - height / 2.0) / height + az = nx * self.hfov + el = ny * self.vfov + return az, el + + def _estimate_ground_distance(self, el_offset: float) -> Optional[float]: + pitch = math.radians(self.pitch_deg) + elevation_angle = pitch - el_offset + if elevation_angle <= 0.0 or elevation_angle >= math.pi / 2: + return None + dist = self.altitude_m / math.tan(elevation_angle) + if dist < 0 or dist > 10000: + return None + return dist + + def _compute_nearest_hotspot_gps(self, hotspots: List[Dict], width: int, height: int) -> Optional[Dict]: + if self.current_gps is None: + return None + + nearest = None + min_dist = float('inf') + for blob in hotspots[:10]: + az, el = self._pixel_to_angle(blob['centroid_x'], blob['centroid_y'], width, height) + bearing = (self.current_heading_deg + math.degrees(az)) % 360.0 + ground_dist = self._estimate_ground_distance(el) + if ground_dist is None: + continue + + start = Point(self.current_gps[0], self.current_gps[1]) + dest = geopy_distance(meters=ground_dist).destination(start, bearing) + dist_m = ground_dist + + info = { + 'centroid_x': blob['centroid_x'], + 'centroid_y': blob['centroid_y'], + 'size_pixels': blob['size_pixels'], + 'max_temperature_c': blob['max_temp'], + 'avg_temperature_c': blob['avg_temp'], + 'latitude': dest.latitude, + 'longitude': dest.longitude, + 'bearing': bearing, + 'distance_m': dist_m, + } + + if dist_m < min_dist: + min_dist = dist_m + nearest = info + + return nearest + + def _publish_hotspot(self, data: Dict) -> None: + msg = PoseStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'map' + # Store lon/lat in x/y for a quick transport (matches original script) + msg.pose.position.x = float(data['longitude']) + msg.pose.position.y = float(data['latitude']) + msg.pose.position.z = 0.0 + msg.pose.orientation.w = 1.0 + + self.hotspot_pub.publish(msg) + self.get_logger().info( + f"Hotspot -> lat={data['latitude']:.6f}, lon={data['longitude']:.6f}, " + f"bearing={data['bearing']:.1f}°, dist={data['distance_m']:.1f}m, " + f"max={data['max_temperature_c']:.1f}C, size={data['size_pixels']}px" + ) + + +def main(args=None): + rclpy.init(args=args) + node = ThermalHotspotLocator() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ros2_ws/src/embr/embr/thermalStream.py b/ros2_ws/src/embr/embr/thermalStream.py new file mode 100644 index 0000000..e3ffc2b --- /dev/null +++ b/ros2_ws/src/embr/embr/thermalStream.py @@ -0,0 +1,373 @@ +#!/usr/bin/env python3 +""" +Thermal Camera Streaming Node with Temperature Overlay and Frame Publishing +Uses flirpy to capture Lepton thermal data, streams via HDMI with ffmpeg, +and publishes radiometric frames when vehicle velocity is 0. +""" + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from std_msgs.msg import UInt16MultiArray, MultiArrayDimension, MultiArrayLayout +from msg_interface.msg import Gps +from cv_bridge import CvBridge +import cv2 +import numpy as np +import subprocess +import threading +import queue +from flirpy.camera.lepton import Lepton + + +class ThermalStreamNode(Node): + def __init__(self): + super().__init__('thermal_stream_node') + + # Configuration - can be modified here directly + self.temp_threshold = 40.0 # Celsius - temperature threshold for hotspot detection + self.video_fps = 9.0 # Lepton typical fps + self.display_width = 640 + self.display_height = 480 + self.colormap = cv2.COLORMAP_JET + self.min_temp = 20.0 # Celsius - minimum temperature for colormap scaling + self.max_temp = 80.0 # Celsius - maximum temperature for colormap scaling + + # State variables + self.current_velocity = None + self.velocity_lock = threading.Lock() + + # Publishers + self.frame_publisher = self.create_publisher( + Image, + 'thermal/radiometric_frame', + 10 + ) + # Optional: publish radiometric array (uint16, Kelvin x 100) for direct analysis + self.array_publisher = self.create_publisher( + UInt16MultiArray, + 'thermal/radiometric_array', + 10 + ) + + # Subscribers + self.gps_subscription = self.create_subscription( + Gps, + 'gps', + self.gps_callback, + 10 + ) + + # CV Bridge for ROS image messages + self.bridge = CvBridge() + + # Initialize camera + self.get_logger().info('Initializing Lepton camera...') + try: + self.camera = Lepton() + self.get_logger().info('Lepton camera initialized successfully') + except Exception as e: + self.get_logger().error(f'Failed to initialize camera: {e}') + raise + + # FFmpeg process for HDMI streaming + self.ffmpeg_process = None + self.frame_queue = queue.Queue(maxsize=2) + + # Start video streaming thread + self.streaming_active = True + self.stream_thread = threading.Thread(target=self._streaming_loop, daemon=True) + self.stream_thread.start() + + # Start FFmpeg process + self._start_ffmpeg() + + self.get_logger().info('Thermal stream node initialized') + + def gps_callback(self, msg): + """Handle GPS messages and update velocity""" + with self.velocity_lock: + self.current_velocity = msg.vel + + # Publish frame and temperature array if velocity is ~0 + if abs(msg.vel) < 0.01: # m/s threshold for stationary + self._capture_and_publish() + + def _start_ffmpeg(self): + """Start FFmpeg process for HDMI output""" + # FFmpeg command to output to HDMI (framebuffer) + # Adjust the output device based on your system configuration + ffmpeg_cmd = [ + 'ffmpeg', + '-f', 'rawvideo', + '-pix_fmt', 'bgr24', + '-s', f'{self.display_width}x{self.display_height}', + '-r', str(self.video_fps), + '-i', '-', # Read from stdin + '-f', 'fbdev', + '-vf', 'scale=1920:1080', # Scale to HDMI resolution + '/dev/fb0' # HDMI framebuffer device + ] + + try: + self.ffmpeg_process = subprocess.Popen( + ffmpeg_cmd, + stdin=subprocess.PIPE, + stderr=subprocess.PIPE, + bufsize=10**8 + ) + self.get_logger().info('FFmpeg HDMI streaming started') + except Exception as e: + self.get_logger().error(f'Failed to start FFmpeg: {e}') + # Fallback to OpenCV display if FFmpeg fails + self.ffmpeg_process = None + + def _streaming_loop(self): + """Main loop for capturing and processing thermal frames""" + self.get_logger().info('Starting thermal streaming loop') + + while self.streaming_active and rclpy.ok(): + try: + # Capture radiometric frame from camera + radiometric_frame = self.camera.grab() + + if radiometric_frame is None: + self.get_logger().warn('Failed to grab frame from camera') + continue + + # Convert to Celsius (Lepton outputs in Kelvin * 100) + temp_celsius = (radiometric_frame / 100.0) - 273.15 + + # Create display frame with colormap and overlays + display_frame = self._create_display_frame(temp_celsius) + + # Send to FFmpeg or display with OpenCV + if self.ffmpeg_process is not None and self.ffmpeg_process.poll() is None: + try: + self.ffmpeg_process.stdin.write(display_frame.tobytes()) + except BrokenPipeError: + self.get_logger().error('FFmpeg pipe broken') + self.ffmpeg_process = None + else: + # Fallback: display with OpenCV + cv2.imshow('Thermal Stream', display_frame) + cv2.waitKey(1) + + except Exception as e: + self.get_logger().error(f'Error in streaming loop: {e}') + import traceback + traceback.print_exc() + + def _create_display_frame(self, temp_celsius): + """ + Create a color-mapped display frame with temperature overlays + + Args: + temp_celsius: 2D numpy array of temperature values in Celsius + + Returns: + BGR color image ready for display/streaming + """ + # Normalize temperature to 0-255 range for colormap + temp_normalized = np.clip( + (temp_celsius - self.min_temp) / (self.max_temp - self.min_temp) * 255, + 0, + 255 + ).astype(np.uint8) + + # Apply colormap + colored_frame = cv2.applyColorMap(temp_normalized, self.colormap) + + # Resize for display + display_frame = cv2.resize( + colored_frame, + (self.display_width, self.display_height), + interpolation=cv2.INTER_LINEAR + ) + + # Find and annotate hot spots above threshold + hot_mask = temp_celsius > self.temp_threshold + + if np.any(hot_mask): + # Find contours of hot regions + hot_mask_uint8 = hot_mask.astype(np.uint8) * 255 + contours, _ = cv2.findContours( + hot_mask_uint8, + cv2.RETR_EXTERNAL, + cv2.CHAIN_APPROX_SIMPLE + ) + + # Scale factor for contours (from thermal resolution to display resolution) + scale_x = self.display_width / temp_celsius.shape[1] + scale_y = self.display_height / temp_celsius.shape[0] + + for contour in contours: + # Calculate temperature statistics for this region + mask_region = np.zeros_like(temp_celsius, dtype=np.uint8) + cv2.drawContours(mask_region, [contour], -1, 1, -1) + region_temps = temp_celsius[mask_region == 1] + + if len(region_temps) > 0: + max_temp = np.max(region_temps) + mean_temp = np.mean(region_temps) + + # Scale contour to display size + scaled_contour = contour.astype(np.float32) + scaled_contour[:, 0, 0] *= scale_x + scaled_contour[:, 0, 1] *= scale_y + scaled_contour = scaled_contour.astype(np.int32) + + # Draw contour + cv2.drawContours(display_frame, [scaled_contour], -1, (0, 255, 0), 2) + + # Get centroid for text placement + M = cv2.moments(scaled_contour) + if M["m00"] != 0: + cx = int(M["m10"] / M["m00"]) + cy = int(M["m01"] / M["m00"]) + else: + cx, cy = scaled_contour[0][0] + + # Draw temperature text + temp_text = f"{max_temp:.1f}C" + cv2.putText( + display_frame, + temp_text, + (cx - 30, cy), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (0, 255, 0), + 2 + ) + + # Add informational overlays + self._add_info_overlay(display_frame, temp_celsius) + + return display_frame + + def _add_info_overlay(self, frame, temp_celsius): + """Add informational text overlay to the frame""" + # Overall temperature statistics + min_temp = np.min(temp_celsius) + max_temp = np.max(temp_celsius) + mean_temp = np.mean(temp_celsius) + + # Timestamp + timestamp = self.get_clock().now().to_msg() + time_str = f"{timestamp.sec}.{timestamp.nanosec // 1000000:03d}" + + # Velocity info + with self.velocity_lock: + vel = self.current_velocity + + vel_str = f"Vel: {vel:.2f} m/s" if vel is not None else "Vel: N/A" + + # Create overlay text + info_lines = [ + f"Min: {min_temp:.1f}C Max: {max_temp:.1f}C Avg: {mean_temp:.1f}C", + f"Threshold: {self.temp_threshold:.1f}C", + vel_str, + f"Time: {time_str}" + ] + + # Draw semi-transparent background for text + overlay = frame.copy() + cv2.rectangle(overlay, (0, 0), (self.display_width, 100), (0, 0, 0), -1) + cv2.addWeighted(overlay, 0.5, frame, 0.5, 0, frame) + + # Draw text + y_offset = 20 + for line in info_lines: + cv2.putText( + frame, + line, + (10, y_offset), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + (255, 255, 255), + 1 + ) + y_offset += 22 + + def _capture_and_publish(self): + """Capture once and publish both radiometric image and temperature array""" + try: + # Capture fresh radiometric frame + radiometric_frame = self.camera.grab() + + if radiometric_frame is None: + self.get_logger().warn('Failed to capture radiometric frame') + return + + + # Also publish radiometric array (UInt16), units: Kelvin x 100 + rad_u16 = radiometric_frame.astype(np.uint16, copy=False) + h, w = rad_u16.shape + array_msg = UInt16MultiArray() + array_msg.layout = MultiArrayLayout( + dim=[ + MultiArrayDimension(label='height', size=h, stride=h * w), + MultiArrayDimension(label='width', size=w, stride=w), + ], + data_offset=0, + ) + array_msg.data = rad_u16.flatten().tolist() + self.array_publisher.publish(array_msg) + + self.get_logger().info('Published radiometric array only (uint16, Kx100) (velocity = 0)') + + except Exception as e: + self.get_logger().error(f'Failed to capture/publish radiometric data: {e}') + + def destroy_node(self): + """Clean shutdown""" + self.get_logger().info('Shutting down thermal stream node...') + + # Stop streaming loop + self.streaming_active = False + + # Wait for thread to finish + if self.stream_thread.is_alive(): + self.stream_thread.join(timeout=2.0) + + # Close camera + try: + if hasattr(self, 'camera'): + self.camera.close() + except Exception as e: + self.get_logger().error(f'Error closing camera: {e}') + + # Terminate FFmpeg + if self.ffmpeg_process is not None: + try: + self.ffmpeg_process.stdin.close() + self.ffmpeg_process.terminate() + self.ffmpeg_process.wait(timeout=2.0) + except Exception as e: + self.get_logger().error(f'Error closing FFmpeg: {e}') + + # Close OpenCV windows + cv2.destroyAllWindows() + + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + + try: + node = ThermalStreamNode() + rclpy.spin(node) + except KeyboardInterrupt: + pass + except Exception as e: + print(f'Error: {e}') + import traceback + traceback.print_exc() + finally: + if rclpy.ok(): + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ros2_ws/src/embr/launch/embr_launch.py b/ros2_ws/src/embr/launch/embr_launch.py index b38fa0e..9e34eb3 100644 --- a/ros2_ws/src/embr/launch/embr_launch.py +++ b/ros2_ws/src/embr/launch/embr_launch.py @@ -17,5 +17,12 @@ def generate_launch_description(): package='embr', executable='sendRf', name='sendRf' - ) + ), + Node( + package='embr', + executable='thermalStream', + name='thermal_stream', + output='screen', + emulate_tty=True + ), ]) \ No newline at end of file diff --git a/ros2_ws/src/embr/package.xml b/ros2_ws/src/embr/package.xml index c0b1857..9a48484 100644 --- a/ros2_ws/src/embr/package.xml +++ b/ros2_ws/src/embr/package.xml @@ -10,6 +10,8 @@ msg_interface sensor_msgs std_msgs + geometry_msgs + cv_bridge ament_copyright ament_flake8 diff --git a/ros2_ws/src/embr/setup.py b/ros2_ws/src/embr/setup.py index 78d0bf5..2aafdd2 100644 --- a/ros2_ws/src/embr/setup.py +++ b/ros2_ws/src/embr/setup.py @@ -26,6 +26,8 @@ 'getCube = embr.getCube:main', 'getTemp = embr.getTemp:main', 'sendRf = embr.sendRf:main', + 'thermalStream = embr.thermalStream:main', + 'thermalHotspot = embr.thermalHotspotLocator:main', ], }, ) diff --git a/ros2_ws/src/msg_interface/CMakeLists.txt b/ros2_ws/src/msg_interface/CMakeLists.txt index 7d22076..5799fa3 100644 --- a/ros2_ws/src/msg_interface/CMakeLists.txt +++ b/ros2_ws/src/msg_interface/CMakeLists.txt @@ -10,7 +10,7 @@ find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} - "msg/Gps.msg" + "msg/GPSAndIMU.msg" ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/ros2_ws/src/msg_interface/msg/GPSAndIMU.msg b/ros2_ws/src/msg_interface/msg/GPSAndIMU.msg new file mode 100644 index 0000000..8f93844 --- /dev/null +++ b/ros2_ws/src/msg_interface/msg/GPSAndIMU.msg @@ -0,0 +1,7 @@ +int32 lat +int32 lon +int32 alt +float32 vel +int32 pitch +int32 yaw +int32 roll \ No newline at end of file diff --git a/ros2_ws/src/msg_interface/msg/Gps.msg b/ros2_ws/src/msg_interface/msg/Gps.msg deleted file mode 100644 index 5266aaa..0000000 --- a/ros2_ws/src/msg_interface/msg/Gps.msg +++ /dev/null @@ -1,4 +0,0 @@ -int32 lat -int32 lon -int32 alt -float32 vel \ No newline at end of file From 6a3bab2199ef64232e31951ac4213ce8f8f6588d Mon Sep 17 00:00:00 2001 From: mrichards03 Date: Thu, 20 Nov 2025 15:48:00 -0800 Subject: [PATCH 04/15] change gps types and add dependencies to script --- Tools/Setup-Scripts/install-dependencies | 3 +++ Tools/ThermalCamTemp.py | 2 +- ros2_ws/src/embr/embr/thermalStream.py | 12 ++++++------ ros2_ws/src/msg_interface/msg/GPSAndIMU.msg | 12 ++++++------ 4 files changed, 16 insertions(+), 13 deletions(-) diff --git a/Tools/Setup-Scripts/install-dependencies b/Tools/Setup-Scripts/install-dependencies index 41c7813..f56677b 100644 --- a/Tools/Setup-Scripts/install-dependencies +++ b/Tools/Setup-Scripts/install-dependencies @@ -14,6 +14,9 @@ sudo apt install -y python3-pip echo "Installing libuvc-dev..." sudo apt install -y libuvc-dev +echo "Installing ffmpeg..." +sudo apt install -y ffmpeg + echo "=========================================" echo "Dependencies installed successfully!" echo "=========================================" diff --git a/Tools/ThermalCamTemp.py b/Tools/ThermalCamTemp.py index c4775ad..96111a7 100644 --- a/Tools/ThermalCamTemp.py +++ b/Tools/ThermalCamTemp.py @@ -632,7 +632,7 @@ def main(): 'pitch_deg': 0, # Camera pitch angle (0=horizontal, 90=down) - MEASURE THIS # Detection parameters - 'temp_threshold_celsius': -90.0, # Only detect above this temperature + 'temp_threshold_celsius': 25.0, # Only detect above this temperature # Simulation settings (SIMULATION_MODE only) 'sim_gps': (49.8880, -119.4960), # Example: Kelowna, BC diff --git a/ros2_ws/src/embr/embr/thermalStream.py b/ros2_ws/src/embr/embr/thermalStream.py index e3ffc2b..42e93f5 100644 --- a/ros2_ws/src/embr/embr/thermalStream.py +++ b/ros2_ws/src/embr/embr/thermalStream.py @@ -9,7 +9,7 @@ from rclpy.node import Node from sensor_msgs.msg import Image from std_msgs.msg import UInt16MultiArray, MultiArrayDimension, MultiArrayLayout -from msg_interface.msg import Gps +from msg_interface.msg import GPSAndIMU from cv_bridge import CvBridge import cv2 import numpy as np @@ -24,12 +24,12 @@ def __init__(self): super().__init__('thermal_stream_node') # Configuration - can be modified here directly - self.temp_threshold = 40.0 # Celsius - temperature threshold for hotspot detection + self.temp_threshold = 20.0 # Celsius - temperature threshold for hotspot detection self.video_fps = 9.0 # Lepton typical fps - self.display_width = 640 - self.display_height = 480 + self.display_width = 160 + self.display_height = 120 self.colormap = cv2.COLORMAP_JET - self.min_temp = 20.0 # Celsius - minimum temperature for colormap scaling + self.min_temp = 10.0 # Celsius - minimum temperature for colormap scaling self.max_temp = 80.0 # Celsius - maximum temperature for colormap scaling # State variables @@ -51,7 +51,7 @@ def __init__(self): # Subscribers self.gps_subscription = self.create_subscription( - Gps, + GPSAndIMU, 'gps', self.gps_callback, 10 diff --git a/ros2_ws/src/msg_interface/msg/GPSAndIMU.msg b/ros2_ws/src/msg_interface/msg/GPSAndIMU.msg index 8f93844..4695459 100644 --- a/ros2_ws/src/msg_interface/msg/GPSAndIMU.msg +++ b/ros2_ws/src/msg_interface/msg/GPSAndIMU.msg @@ -1,7 +1,7 @@ -int32 lat -int32 lon -int32 alt +float32 lat +float32 lon +float32 alt float32 vel -int32 pitch -int32 yaw -int32 roll \ No newline at end of file +float32 pitch +float32 yaw +float32 roll \ No newline at end of file From 799688b19a11ae102929c4256e18d50a783b5d0f Mon Sep 17 00:00:00 2001 From: mrichards03 Date: Thu, 20 Nov 2025 15:54:42 -0800 Subject: [PATCH 05/15] dependencies update --- ros2_ws/requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_ws/requirements.txt b/ros2_ws/requirements.txt index 54ef0e4..d85400f 100644 --- a/ros2_ws/requirements.txt +++ b/ros2_ws/requirements.txt @@ -3,6 +3,6 @@ pymavlink dronekit smbus flirpy -numpy +numpy<2 opencv-python geopy From 1c40a9fc8bbba629a8d9a9c83cbe1efcd9bdaa11 Mon Sep 17 00:00:00 2001 From: mrichards03 Date: Sun, 23 Nov 2025 06:34:21 +0000 Subject: [PATCH 06/15] streaming working but lags --- README.md | 27 ++ Tools/Setup-Scripts/setup-thermal-camera | 24 ++ ...malHotspotLocator.py => hotspotLocator.py} | 8 +- ros2_ws/src/embr/embr/thermalStream.py | 365 ++++++++++++------ ros2_ws/src/embr/launch/embr_launch.py | 5 + ros2_ws/src/embr/setup.py | 2 +- 6 files changed, 305 insertions(+), 126 deletions(-) rename ros2_ws/src/embr/embr/{thermalHotspotLocator.py => hotspotLocator.py} (97%) diff --git a/README.md b/README.md index 1361533..61fd047 100644 --- a/README.md +++ b/README.md @@ -293,6 +293,33 @@ Save and exit, then reboot. After rebooting, verify with `ls -l /dev/serial*` - you should see two devices. +### HDMI / Framebuffer Configuration + +To enable HDMI framebuffer output (used by the thermal streaming node to write directly to the display), the Pi must load the vc4 KMS driver and the user running ffmpeg must be in the `video` group. + +If you used `Tools/Setup-Scripts/setup-all` or `Tools/Setup-Scripts/setup-thermal-camera`, these steps are applied automatically. They do the following: + +- Add the following lines to `/boot/firmware/config.txt` (backed up to `/boot/firmware/config.txt.embr_bak`): + +``` +# EMBR-Bot: enable vc4 KMS and force HDMI output for framebuffer streaming +dtoverlay=vc4-kms-v3d +hdmi_force_hotplug=1 +hdmi_group=2 +hdmi_mode=16 +max_framebuffers=2 +``` + +- Add the current user to the `video` group so ffmpeg can write to `/dev/fb0`: + +``` +sudo usermod -aG video $USER + +# then log out / back in or reboot for the change to take effect +``` + +If you prefer to apply these manually, edit `/boot/firmware/config.txt` and append the block above, then run the `usermod` command and reboot. + ## Hardware Setup & Wiring ### UART Pin Configuration diff --git a/Tools/Setup-Scripts/setup-thermal-camera b/Tools/Setup-Scripts/setup-thermal-camera index 97a0033..fef9873 100644 --- a/Tools/Setup-Scripts/setup-thermal-camera +++ b/Tools/Setup-Scripts/setup-thermal-camera @@ -16,3 +16,27 @@ sudo udevadm trigger echo "=========================================" echo "Thermal camera setup complete!" echo "=========================================" + +echo "Configuring HDMI / framebuffer for display output..." +# Backup config.txt if present +if [ -f /boot/firmware/config.txt ]; then + sudo cp /boot/firmware/config.txt /boot/firmware/config.txt.embr_bak || true + echo "Backing up /boot/firmware/config.txt to /boot/firmware/config.txt.embr_bak" + # Append vc4 KMS overlay and HDMI force options if not already present + grep -q "dtoverlay=vc4-kms-v3d" /boot/firmware/config.txt 2>/dev/null || sudo bash -c 'cat >> /boot/firmware/config.txt <<"EOF" + +# EMBR-Bot: enable vc4 KMS and force HDMI output for framebuffer streaming +dtoverlay=vc4-kms-v3d +hdmi_force_hotplug=1 +hdmi_group=2 +hdmi_mode=16 +max_framebuffers=2 +EOF' + echo "Appended vc4 KMS and HDMI settings to /boot/firmware/config.txt" +else + echo "Warning: /boot/firmware/config.txt not found; skipping HDMI config append." +fi + +echo "Adding current user to 'video' group so ffmpeg can write to /dev/fb0" +sudo usermod -aG video "$USER" || true +echo "(You will need to log out and back in or reboot for group changes to take effect.)" diff --git a/ros2_ws/src/embr/embr/thermalHotspotLocator.py b/ros2_ws/src/embr/embr/hotspotLocator.py similarity index 97% rename from ros2_ws/src/embr/embr/thermalHotspotLocator.py rename to ros2_ws/src/embr/embr/hotspotLocator.py index 46f0692..c061c2c 100644 --- a/ros2_ws/src/embr/embr/thermalHotspotLocator.py +++ b/ros2_ws/src/embr/embr/hotspotLocator.py @@ -30,13 +30,13 @@ from geopy.point import Point -class ThermalHotspotLocator(Node): +class hotspotLocator(Node): def __init__(self): super().__init__('thermal_hotspot_locator') # Configuration (can be adjusted here) # Camera geometry - self.lepton_model = '2.5' # '2.5' or '3.1R' + self.lepton_model = '3.1R' # '2.5' or '3.1R' self.altitude_m = 1.0 # Camera height above ground (meters) self.pitch_deg = 45.0 # Camera pitch (0=horizontal, 90=down) self.temp_threshold_c = 30.0 # Celsius threshold for hotspot detection @@ -60,7 +60,7 @@ def __init__(self): # Publisher self.hotspot_pub = self.create_publisher(PoseStamped, '/thermal/hotspot_gps', 10) - self.get_logger().info('ThermalHotspotLocator started') + self.get_logger().info('hotspotLocator started') def _set_camera_model(self, model: str) -> None: if model == '2.5': @@ -219,7 +219,7 @@ def _publish_hotspot(self, data: Dict) -> None: def main(args=None): rclpy.init(args=args) - node = ThermalHotspotLocator() + node = hotspotLocator() try: rclpy.spin(node) except KeyboardInterrupt: diff --git a/ros2_ws/src/embr/embr/thermalStream.py b/ros2_ws/src/embr/embr/thermalStream.py index 42e93f5..c7c512b 100644 --- a/ros2_ws/src/embr/embr/thermalStream.py +++ b/ros2_ws/src/embr/embr/thermalStream.py @@ -24,11 +24,12 @@ def __init__(self): super().__init__('thermal_stream_node') # Configuration - can be modified here directly - self.temp_threshold = 20.0 # Celsius - temperature threshold for hotspot detection + self.temp_threshold = 30.0 # Celsius - temperature threshold for hotspot detection self.video_fps = 9.0 # Lepton typical fps - self.display_width = 160 - self.display_height = 120 - self.colormap = cv2.COLORMAP_JET + self.display_width = 640 + self.display_height = 480 + # use a reddish/inferno colormap for thermal-style colors + self.colormap = cv2.COLORMAP_INFERNO self.min_temp = 10.0 # Celsius - minimum temperature for colormap scaling self.max_temp = 80.0 # Celsius - maximum temperature for colormap scaling @@ -71,7 +72,10 @@ def __init__(self): # FFmpeg process for HDMI streaming self.ffmpeg_process = None - self.frame_queue = queue.Queue(maxsize=2) + # queue used to buffer frames for ffmpeg writer thread; small size to keep latency low + self.frame_queue = queue.Queue(maxsize=4) + # control how often expensive overlays (contours/stats) are computed + self.overlay_every_n_frames = 1 # set >1 to do overlays less frequently # Start video streaming thread self.streaming_active = True @@ -94,32 +98,141 @@ def gps_callback(self, msg): def _start_ffmpeg(self): """Start FFmpeg process for HDMI output""" - # FFmpeg command to output to HDMI (framebuffer) - # Adjust the output device based on your system configuration - ffmpeg_cmd = [ - 'ffmpeg', - '-f', 'rawvideo', - '-pix_fmt', 'bgr24', - '-s', f'{self.display_width}x{self.display_height}', - '-r', str(self.video_fps), - '-i', '-', # Read from stdin - '-f', 'fbdev', - '-vf', 'scale=1920:1080', # Scale to HDMI resolution - '/dev/fb0' # HDMI framebuffer device - ] - + # Initialize direct framebuffer writer (faster and lower latency than ffmpeg) try: - self.ffmpeg_process = subprocess.Popen( - ffmpeg_cmd, - stdin=subprocess.PIPE, - stderr=subprocess.PIPE, - bufsize=10**8 - ) - self.get_logger().info('FFmpeg HDMI streaming started') + self._init_fbdev() + self._fbdev_writer_thread = threading.Thread(target=self._fbdev_writer_loop, daemon=True) + self._fbdev_writer_thread.start() + self._fbdev_active = True + self.get_logger().info('FBDev writer thread started (direct framebuffer output)') + except Exception as e: + self.get_logger().error(f'Failed to initialize framebuffer writer: {e}') + self._fbdev_active = False + + def _init_fbdev(self): + """Read fb0 properties (resolution, bpp) and compute layout for direct writes.""" + try: + with open('/sys/class/graphics/fb0/virtual_size', 'r') as f: + vs = f.read().strip() + fb_w, fb_h = [int(x) for x in vs.split(',')] + except Exception: + fb_w, fb_h = 1920, 1080 + + try: + with open('/sys/class/graphics/fb0/bits_per_pixel', 'r') as f: + bpp = int(f.read().strip()) + except Exception: + bpp = 16 + + self._fb_width = fb_w + self._fb_height = fb_h + self._fb_bpp = bpp + self._fb_bpp_bytes = max(1, bpp // 8) + self.get_logger().debug(f'FB dev init: {fb_w}x{fb_h} {bpp}bpp') + + def _fbdev_writer_loop(self): + """Consume frames from the queue (BGR uint8 arrays) and write to /dev/fb0 in rgb565le.""" + try: + fd = open('/dev/fb0', 'r+b', buffering=0) + except Exception as e: + self.get_logger().error(f'Could not open /dev/fb0 for writing: {e}') + return + + while self.streaming_active: + try: + frame = self.frame_queue.get(timeout=0.5) + except queue.Empty: + continue + + if frame is None: + continue + + try: + img = frame + # Scale the image to fit within the framebuffer while preserving aspect + scale = min(self._fb_width / img.shape[1], self._fb_height / img.shape[0]) + inner_w = max(1, int(img.shape[1] * scale)) + inner_h = max(1, int(img.shape[0] * scale)) + img_scaled = cv2.resize(img, (inner_w, inner_h), interpolation=cv2.INTER_LINEAR) + + # Convert BGR -> RGB565 little endian + r = (img_scaled[:, :, 2] >> 3).astype(np.uint16) + g = (img_scaled[:, :, 1] >> 2).astype(np.uint16) + b = (img_scaled[:, :, 0] >> 3).astype(np.uint16) + rgb565 = (r << 11) | (g << 5) | b + + # Create full framebuffer buffer and place rgb565 into center + fb_buf = np.zeros((self._fb_height, self._fb_width), dtype=np.uint16) + x0 = max(0, (self._fb_width - inner_w) // 2) + y0 = max(0, (self._fb_height - inner_h) // 2) + fb_buf[y0:y0+inner_h, x0:x0+inner_w] = rgb565 + + out_bytes = fb_buf.astype(' 1: + # use a modulo counter stored on the node + if not hasattr(self, '_overlay_counter'): + self._overlay_counter = 0 + self._overlay_counter = (self._overlay_counter + 1) % self.overlay_every_n_frames + if self._overlay_counter != 0: + # regenerate a lightweight display frame (just colormap + resize) + # this avoids repeated contour/findContours work + temp_normalized = np.clip( + (temp_celsius - self.min_temp) / (self.max_temp - self.min_temp) * 255, + 0, + 255 + ).astype(np.uint8) + display_frame = cv2.applyColorMap(temp_normalized, self.colormap) + display_frame = cv2.resize(display_frame, (self.display_width, self.display_height), interpolation=cv2.INTER_LINEAR) + + # Send to framebuffer writer via queue or display with OpenCV + if getattr(self, '_fbdev_active', False): try: - self.ffmpeg_process.stdin.write(display_frame.tobytes()) - except BrokenPipeError: - self.get_logger().error('FFmpeg pipe broken') - self.ffmpeg_process = None + # Non-blocking enqueue; drop oldest frame if queue is full to preserve low latency + try: + self.frame_queue.put_nowait(display_frame) + except queue.Full: + try: + # drop oldest + _ = self.frame_queue.get_nowait() + self.frame_queue.put_nowait(display_frame) + except Exception: + # if we still can't enqueue, just drop this frame + pass + except Exception: + self.get_logger().error('Frame queue error for fbdev writer') else: - # Fallback: display with OpenCV + # Fallback: display with OpenCV (useful for debugging) cv2.imshow('Thermal Stream', display_frame) cv2.waitKey(1) @@ -167,12 +306,21 @@ def _create_display_frame(self, temp_celsius): Returns: BGR color image ready for display/streaming """ - # Normalize temperature to 0-255 range for colormap - temp_normalized = np.clip( - (temp_celsius - self.min_temp) / (self.max_temp - self.min_temp) * 255, - 0, - 255 - ).astype(np.uint8) + # Adaptive normalize temperature to 0-255 range for colormap using percentiles + try: + lo, hi = np.percentile(temp_celsius, (2, 98)) + lo = max(lo, self.min_temp) + hi = min(hi, self.max_temp) + if hi - lo < 0.1: + lo = self.min_temp + hi = self.max_temp + temp_normalized = np.clip((temp_celsius - lo) / (hi - lo) * 255, 0, 255).astype(np.uint8) + except Exception: + temp_normalized = np.clip( + (temp_celsius - self.min_temp) / (self.max_temp - self.min_temp) * 255, + 0, + 255 + ).astype(np.uint8) # Apply colormap colored_frame = cv2.applyColorMap(temp_normalized, self.colormap) @@ -184,63 +332,61 @@ def _create_display_frame(self, temp_celsius): interpolation=cv2.INTER_LINEAR ) - # Find and annotate hot spots above threshold + # Find and annotate hot spots above threshold (efficient, on radiometric data) hot_mask = temp_celsius > self.temp_threshold - + if np.any(hot_mask): - # Find contours of hot regions - hot_mask_uint8 = hot_mask.astype(np.uint8) * 255 + # Create uint8 mask for contours + hot_mask_uint8 = (hot_mask.astype(np.uint8) * 255) contours, _ = cv2.findContours( hot_mask_uint8, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE ) - + # Scale factor for contours (from thermal resolution to display resolution) scale_x = self.display_width / temp_celsius.shape[1] scale_y = self.display_height / temp_celsius.shape[0] - + for contour in contours: + # Skip tiny contours to avoid noise + if cv2.contourArea(contour) < 4: + continue + # Calculate temperature statistics for this region mask_region = np.zeros_like(temp_celsius, dtype=np.uint8) cv2.drawContours(mask_region, [contour], -1, 1, -1) region_temps = temp_celsius[mask_region == 1] - - if len(region_temps) > 0: - max_temp = np.max(region_temps) - mean_temp = np.mean(region_temps) - - # Scale contour to display size - scaled_contour = contour.astype(np.float32) - scaled_contour[:, 0, 0] *= scale_x - scaled_contour[:, 0, 1] *= scale_y - scaled_contour = scaled_contour.astype(np.int32) - - # Draw contour - cv2.drawContours(display_frame, [scaled_contour], -1, (0, 255, 0), 2) - - # Get centroid for text placement - M = cv2.moments(scaled_contour) - if M["m00"] != 0: - cx = int(M["m10"] / M["m00"]) - cy = int(M["m01"] / M["m00"]) - else: - cx, cy = scaled_contour[0][0] - - # Draw temperature text - temp_text = f"{max_temp:.1f}C" - cv2.putText( - display_frame, - temp_text, - (cx - 30, cy), - cv2.FONT_HERSHEY_SIMPLEX, - 0.6, - (0, 255, 0), - 2 - ) - - # Add informational overlays - self._add_info_overlay(display_frame, temp_celsius) + + if region_temps.size == 0: + continue + + max_temp = float(np.max(region_temps)) + + # Only annotate if above threshold (should be by mask, but double-check) + if max_temp <= self.temp_threshold: + continue + + # Scale contour to display size and draw a bounding box + scaled_contour = contour.astype(np.float32) + scaled_contour[:, 0, 0] *= scale_x + scaled_contour[:, 0, 1] *= scale_y + scaled_contour = scaled_contour.astype(np.int32) + + x, y, w_box, h_box = cv2.boundingRect(scaled_contour) + # Draw a thin rectangle around the hot region + cv2.rectangle(display_frame, (x, y), (x + w_box, y + h_box), (0, 255, 255), 2) + + # Prepare temperature label and draw above the box + temp_text = f"{max_temp:.1f}C" + text_size, _ = cv2.getTextSize(temp_text, cv2.FONT_HERSHEY_SIMPLEX, 0.6, 2) + text_w, text_h = text_size + text_x = x + text_y = max(10, y - 6) + + # Draw black outline for readability then white text + cv2.putText(display_frame, temp_text, (text_x, text_y), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 0), 3) + cv2.putText(display_frame, temp_text, (text_x, text_y), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1) return display_frame @@ -250,43 +396,20 @@ def _add_info_overlay(self, frame, temp_celsius): min_temp = np.min(temp_celsius) max_temp = np.max(temp_celsius) mean_temp = np.mean(temp_celsius) - - # Timestamp - timestamp = self.get_clock().now().to_msg() - time_str = f"{timestamp.sec}.{timestamp.nanosec // 1000000:03d}" - - # Velocity info - with self.velocity_lock: - vel = self.current_velocity - - vel_str = f"Vel: {vel:.2f} m/s" if vel is not None else "Vel: N/A" - - # Create overlay text - info_lines = [ - f"Min: {min_temp:.1f}C Max: {max_temp:.1f}C Avg: {mean_temp:.1f}C", - f"Threshold: {self.temp_threshold:.1f}C", - vel_str, - f"Time: {time_str}" - ] - - # Draw semi-transparent background for text - overlay = frame.copy() - cv2.rectangle(overlay, (0, 0), (self.display_width, 100), (0, 0, 0), -1) - cv2.addWeighted(overlay, 0.5, frame, 0.5, 0, frame) - - # Draw text - y_offset = 20 - for line in info_lines: - cv2.putText( - frame, - line, - (10, y_offset), - cv2.FONT_HERSHEY_SIMPLEX, - 0.5, - (255, 255, 255), - 1 - ) - y_offset += 22 + # Minimal overlay: show Min / Max / Avg to reduce per-frame work + info_text = f"Min: {min_temp:.1f}C Max: {max_temp:.1f}C Avg: {mean_temp:.1f}C" + + # Draw a solid background for readability (fast) + cv2.rectangle(frame, (0, 0), (self.display_width, 30), (0, 0, 0), -1) + cv2.putText( + frame, + info_text, + (8, 20), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (255, 255, 255), + 2 + ) def _capture_and_publish(self): """Capture once and publish both radiometric image and temperature array""" diff --git a/ros2_ws/src/embr/launch/embr_launch.py b/ros2_ws/src/embr/launch/embr_launch.py index 9e34eb3..cd503f4 100644 --- a/ros2_ws/src/embr/launch/embr_launch.py +++ b/ros2_ws/src/embr/launch/embr_launch.py @@ -25,4 +25,9 @@ def generate_launch_description(): output='screen', emulate_tty=True ), + Node( + package='embr', + executable='hotspotLocator', + name='hotspot_locator' + ) ]) \ No newline at end of file diff --git a/ros2_ws/src/embr/setup.py b/ros2_ws/src/embr/setup.py index fe41ae7..39c54b5 100644 --- a/ros2_ws/src/embr/setup.py +++ b/ros2_ws/src/embr/setup.py @@ -27,10 +27,10 @@ 'getTemp = embr.getTemp:main', 'sendRf = embr.sendRf:main', 'thermalStream = embr.thermalStream:main', - 'thermalHotspot = embr.thermalHotspotLocator:main', 'getCube_v2 = embr.getCube_v2:main', 'getTemp_v2 = embr.getTemp_v2:main', 'sendRf_v2 = embr.sendRf_v2:main', + 'hotspotLocator = embr.hotspotLocator:main', ], }, ) From a6160cec0f6d5482134189f1ef85eea403ec40ca Mon Sep 17 00:00:00 2001 From: mrichards03 Date: Sun, 23 Nov 2025 09:08:48 +0000 Subject: [PATCH 07/15] streaming working and images being received to hotspot locator --- .gitignore | 1 + ros2_ws/src/embr/embr/getCube_v2.py | 35 +++++++++-------- ros2_ws/src/embr/embr/hotspotLocator.py | 13 +++++-- ros2_ws/src/embr/embr/sendRf_v2.py | 26 ++++++++----- ros2_ws/src/embr/embr/sensors/cube.py | 51 +++++++++++++++++-------- 5 files changed, 81 insertions(+), 45 deletions(-) diff --git a/.gitignore b/.gitignore index 457d935..6211024 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ ros2_ws/log ros2_ws/src/embr/embr/__pycache__ ros2_ws/src/embr/embr/sensors/__pycache__ .vscode +__pycache__/ \ No newline at end of file diff --git a/ros2_ws/src/embr/embr/getCube_v2.py b/ros2_ws/src/embr/embr/getCube_v2.py index ec562e8..b686d19 100644 --- a/ros2_ws/src/embr/embr/getCube_v2.py +++ b/ros2_ws/src/embr/embr/getCube_v2.py @@ -6,7 +6,7 @@ import rclpy from rclpy.node import Node import time -from msg_interface.msg import Gps +from msg_interface.msg import GPSAndIMU from embr.sensors import create_sensor, SensorConfig, SensorFactory @@ -40,37 +40,42 @@ def __init__(self): try: self.sensor = create_sensor('cube', config) self.sensor.start() - + sensor_type = 'simulated' if 'Sim' in self.sensor.__class__.__name__ else 'real' self.get_logger().info(f'Cube sensor initialized in {config.mode} mode (using {sensor_type} sensor)') except Exception as e: self.get_logger().error(f'Failed to initialize sensor: {e}') raise - - # Create publisher and timer - self.publisher_ = self.create_publisher(Gps, 'gps', 10) + + # Create publisher and timer (GPS + IMU message) + self.publisher_ = self.create_publisher(GPSAndIMU, 'gps', 10) self.timer = self.create_timer(1.0, self.publish_attitude) - + def publish_attitude(self): """Read GPS data and publish.""" try: gps_data = self.sensor.read() - - gps_msg = Gps() - gps_msg.lat = gps_data.lat - gps_msg.lon = gps_data.lon - gps_msg.alt = gps_data.alt - gps_msg.vel = gps_data.vel - + + gps_msg = GPSAndIMU() + # Core GPS fields + gps_msg.lat = float(gps_data.lat) + gps_msg.lon = float(gps_data.lon) + gps_msg.alt = float(gps_data.alt) + gps_msg.vel = float(gps_data.vel) + # IMU fields + gps_msg.pitch = float(gps_data.pitch) + gps_msg.yaw = float(gps_data.yaw) + gps_msg.roll = float(gps_data.roll) + self.get_logger().info( f'GPS: Lat: {gps_msg.lat} Lon: {gps_msg.lon} ' f'Alt: {gps_msg.alt} Vel: {gps_msg.vel:.2f}' ) - + self.publisher_.publish(gps_msg) except Exception as e: self.get_logger().error(f'Error reading GPS: {e}') - + def destroy_node(self): """Cleanup sensor on shutdown.""" if hasattr(self, 'sensor'): diff --git a/ros2_ws/src/embr/embr/hotspotLocator.py b/ros2_ws/src/embr/embr/hotspotLocator.py index c061c2c..b7a8ec8 100644 --- a/ros2_ws/src/embr/embr/hotspotLocator.py +++ b/ros2_ws/src/embr/embr/hotspotLocator.py @@ -43,7 +43,7 @@ def __init__(self): # Topics (override here if needed) self.array_topic = 'thermal/radiometric_array' - self.gps_imu_topic = 'gps_imu' + self.gps_imu_topic = 'gps' # Set Lepton model parameters self._set_camera_model(self.lepton_model) @@ -52,6 +52,8 @@ def __init__(self): self.current_gps: Optional[Tuple[float, float]] = None # (lat, lon) self.current_heading_deg: float = 0.0 # compass heading 0-360, 0=N, 90=E self.have_gps: bool = False + # internal flag to avoid spamming the log while waiting for GPS + self._warned_waiting_gps: bool = False # Subscribers self.create_subscription(GPSAndIMU, self.gps_imu_topic, self._gps_imu_cb, 10) @@ -71,7 +73,7 @@ def _set_camera_model(self, model: str) -> None: self.hfov = math.radians(57) self.vfov = math.radians(44) else: - self.get_logger().warn("Unknown lepton_model, defaulting to 2.5 FOV") + self.get_logger().warning("Unknown lepton_model, defaulting to 2.5 FOV") self.hfov = math.radians(51) self.vfov = math.radians(38) @@ -92,7 +94,10 @@ def _gps_imu_cb(self, msg: GPSAndIMU) -> None: def _array_cb(self, msg: UInt16MultiArray) -> None: if not self.have_gps: - self.get_logger().warn_once('Waiting for GPS fix...') + # warn once to avoid log spam while waiting for GPS + if not getattr(self, '_warned_waiting_gps', False): + self.get_logger().warning('Waiting for GPS fix...') + self._warned_waiting_gps = True return # Determine image shape from layout @@ -105,7 +110,7 @@ def _array_cb(self, msg: UInt16MultiArray) -> None: rad = np.array(msg.data, dtype=np.uint16) if rad.size != h * w: - self.get_logger().warn(f'Array size mismatch: {rad.size} vs {h}x{w}') + self.get_logger().warning(f'Array size mismatch: {rad.size} vs {h}x{w}') return rad = rad.reshape((h, w)) diff --git a/ros2_ws/src/embr/embr/sendRf_v2.py b/ros2_ws/src/embr/embr/sendRf_v2.py index 49e942d..8ee0e44 100644 --- a/ros2_ws/src/embr/embr/sendRf_v2.py +++ b/ros2_ws/src/embr/embr/sendRf_v2.py @@ -8,7 +8,7 @@ import rclpy import time from rclpy.node import Node -from msg_interface.msg import Gps +from msg_interface.msg import GPSAndIMU from sensor_msgs.msg import Temperature from std_msgs.msg import Float32 from std_msgs.msg import String @@ -56,7 +56,8 @@ def __init__(self): raise # Create subscriptions - self.subscription = self.create_subscription(Gps, 'gps', self.cube_callback, 10) + # Subscribe to the new GPS+IMU message + self.subscription = self.create_subscription(GPSAndIMU, 'gps', self.cube_callback, 10) self.subscription_temperature = self.create_subscription( Temperature, 'temperature', self.temperature_callback, 10 ) @@ -78,15 +79,20 @@ def temperature_callback(self, msg): self.get_logger().error(f'Error sending temperature: {e}') def cube_callback(self, msg): - """Handle GPS messages.""" + """Handle GPS+IMU messages.""" try: - lat = msg.lat - lon = msg.lon - alt = msg.alt - vel = int(msg.vel * 100) - - self.get_logger().info(f"Sending GPS: Lat: {lat}, Lon: {lon}, Alt: {alt}, Vel: {vel}") - self.mavlink_connection.send_gps(lat, lon, alt, vel) + lat = float(msg.lat) + lon = float(msg.lon) + alt = float(msg.alt) + vel_i = int(msg.vel * 100) + + # Convert to MAVLink integer formats: lat/lon in 1e-7 degrees, alt in mm + lat_i = int(lat * 1e7) + lon_i = int(lon * 1e7) + alt_i = int(alt * 1000) + + self.get_logger().info(f"Sending GPS: Lat: {lat} Lon: {lon} Alt: {alt} Vel: {vel_i}") + self.mavlink_connection.send_gps(lat_i, lon_i, alt_i, vel_i) except Exception as e: self.get_logger().error(f'Error sending GPS: {e}') diff --git a/ros2_ws/src/embr/embr/sensors/cube.py b/ros2_ws/src/embr/embr/sensors/cube.py index c67d709..98e914d 100644 --- a/ros2_ws/src/embr/embr/sensors/cube.py +++ b/ros2_ws/src/embr/embr/sensors/cube.py @@ -4,16 +4,21 @@ import math from typing import Optional, Dict, Any from dataclasses import dataclass +from math import degrees from .base import Sensor, SensorConfig @dataclass class GpsData: - """GPS data structure.""" - lat: int # latitude * 1e7 - lon: int # longitude * 1e7 - alt: int # altitude in mm + """GPS data structure with optional IMU fields.""" + lat: float # latitude in degrees + lon: float # longitude in degrees + alt: float # altitude in meters vel: float # ground speed in m/s + # IMU fields in degrees + pitch: float = 0.0 + yaw: float = 0.0 + roll: float = 0.0 class CubeSensor(Sensor): @@ -49,11 +54,19 @@ def read(self) -> GpsData: try: location = self.vehicle.location.global_frame + attitude = self.vehicle.attitude + yaw = attitude.yaw + pitch = attitude.pitch + roll = attitude.roll + # Return floats: lat/lon in degrees, alt in meters, vel in m/s return GpsData( - lat=int(location.lat * 1e7), - lon=int(location.lon * 1e7), - alt=int(location.alt * 1000), - vel=self.vehicle.groundspeed + lat=float(location.lat), + lon=float(location.lon), + alt=float(location.alt), + vel=float(self.vehicle.groundspeed), + pitch=float(degrees(pitch)), + yaw=float(degrees(yaw)), + roll=float(degrees(roll)), ) except Exception as e: raise RuntimeError(f"Failed to read GPS data: {e}") @@ -109,10 +122,13 @@ def read(self) -> GpsData: lat, lon, alt = self._pause_position return GpsData( - lat=int(lat * 1e7), - lon=int(lon * 1e7), - alt=int(alt * 1000), - vel=0.0 # Zero velocity during pause + lat=float(lat), + lon=float(lon), + alt=float(alt), + vel=0.0, # Zero velocity during pause + pitch=0.0, + yaw=0.0, + roll=0.0, ) elif time_since_last_pause >= (self.pause_interval + self.pause_duration): # Pause ended - reset for next pause cycle @@ -123,10 +139,13 @@ def read(self) -> GpsData: lat, lon, alt = self._calculate_position(elapsed) return GpsData( - lat=int(lat * 1e7), - lon=int(lon * 1e7), - alt=int(alt * 1000), - vel=self.velocity + lat=float(lat), + lon=float(lon), + alt=float(alt), + vel=float(self.velocity), + pitch=5.0 * math.sin(elapsed / 5.0), + yaw=(elapsed * 10.0) % 360.0, + roll=2.0 * math.sin(elapsed / 3.0), ) def _calculate_position(self, elapsed: float) -> tuple: From 2fa95a030d9fa89d9f8c534220e7c603d7998ee6 Mon Sep 17 00:00:00 2001 From: mrichards03 Date: Sun, 23 Nov 2025 09:46:03 +0000 Subject: [PATCH 08/15] remove old nodes --- DOCKER_CHEATSHEET.md | 16 +-- Dockerfile | 8 +- README.md | 10 +- ros2_ws/README.md | 16 +-- ros2_ws/src/embr/config/CONFIG.md | 10 +- ros2_ws/src/embr/embr/getCube.py | 104 ++++++++++++++---- ros2_ws/src/embr/embr/getCube_v2.py | 99 ----------------- ros2_ws/src/embr/embr/getTemp.py | 85 +++++++++++--- ros2_ws/src/embr/embr/getTemp_v2.py | 88 --------------- .../src/embr/embr/{sendRf_v2.py => radio.py} | 0 ros2_ws/src/embr/embr/sendRf.py | 73 ------------ ros2_ws/src/embr/launch/embr_launch.py | 52 ++++++--- ros2_ws/src/embr/launch/embr_launch_v2.py | 51 --------- ros2_ws/src/embr/setup.py | 3 - 14 files changed, 214 insertions(+), 401 deletions(-) mode change 100755 => 100644 ros2_ws/src/embr/embr/getCube.py delete mode 100644 ros2_ws/src/embr/embr/getCube_v2.py delete mode 100644 ros2_ws/src/embr/embr/getTemp_v2.py rename ros2_ws/src/embr/embr/{sendRf_v2.py => radio.py} (100%) delete mode 100755 ros2_ws/src/embr/embr/sendRf.py delete mode 100644 ros2_ws/src/embr/launch/embr_launch_v2.py diff --git a/DOCKER_CHEATSHEET.md b/DOCKER_CHEATSHEET.md index a53c174..e89c03e 100644 --- a/DOCKER_CHEATSHEET.md +++ b/DOCKER_CHEATSHEET.md @@ -45,12 +45,12 @@ docker compose up -d ### Launch Nodes ```bash # All nodes -ros2 launch embr embr_launch_v2.py +ros2 launch embr embr_launch.py # Individual nodes -ros2 run embr getTemp_v2 -ros2 run embr getCube_v2 -ros2 run embr sendRf_v2 +ros2 run embr getTemp +ros2 run embr getCube +ros2 run embr sendRf ``` ### Monitor Topics @@ -101,7 +101,7 @@ docker logs embr-sim docker logs -f embr-sim # Follow # ROS logs (inside container) -ros2 run embr getTemp_v2 --ros-args --log-level debug +ros2 run embr getTemp --ros-args --log-level debug ``` ### Check Status @@ -160,7 +160,7 @@ docker cp my_config.json embr-sim:/workspace/ros2_ws/src/embr/config/ ### Terminal 1: Run Nodes ```bash docker compose exec embr-sim /bin/bash -ros2 launch embr embr_launch_v2.py +ros2 launch embr embr_launch.py ``` ### Terminal 2: Monitor Topics @@ -301,7 +301,7 @@ alias embr-logs='docker logs -f embr-sim' docker compose up -d && docker compose exec embr-sim bash -c " source /opt/ros/humble/setup.bash && source /workspace/ros2_ws/install/setup.bash && - ros2 launch embr embr_launch_v2.py + ros2 launch embr embr_launch.py " ``` @@ -311,7 +311,7 @@ docker compose up -d && docker compose exec embr-sim bash -c " docker exec -d embr-sim bash -c " source /opt/ros/humble/setup.bash && source /workspace/ros2_ws/install/setup.bash && - ros2 launch embr embr_launch_v2.py + ros2 launch embr embr_launch.py " # Then monitor from outside diff --git a/Dockerfile b/Dockerfile index 3efdc64..f14c8a3 100644 --- a/Dockerfile +++ b/Dockerfile @@ -82,12 +82,12 @@ echo "Sensor Mode: Uses config/sensors_sim.json by default"\n\ echo ""\n\ echo "Available commands:"\n\ echo " # Simulation mode (default):"\n\ -echo " ros2 launch embr embr_launch_v2.py config_file:=config/sensors_sim.json"\n\ +echo " ros2 launch embr embr_launch.py config_file:=config/sensors_sim.json"\n\ echo ""\n\ echo " # Individual nodes:"\n\ -echo " ros2 run embr getTemp_v2 --ros-args -p config_file:=config/sensors_sim.json"\n\ -echo " ros2 run embr getCube_v2 --ros-args -p config_file:=config/sensors_sim.json"\n\ -echo " ros2 run embr sendRf_v2 --ros-args -p config_file:=config/sensors_sim.json"\n\ +echo " ros2 run embr getTemp --ros-args -p config_file:=config/sensors_sim.json"\n\ +echo " ros2 run embr getCube --ros-args -p config_file:=config/sensors_sim.json"\n\ +echo " ros2 run embr sendRf --ros-args -p config_file:=config/sensors_sim.json"\n\ echo ""\n\ echo " # Monitoring:"\n\ echo " ros2 topic list - List topics"\n\ diff --git a/README.md b/README.md index 1595af6..8cdff0e 100644 --- a/README.md +++ b/README.md @@ -89,24 +89,24 @@ All sensor modes are configured through JSON configuration files in `ros2_ws/src **Option 1: Full Simulation (All Sensors)** ```bash -ros2 launch embr embr_launch_v2.py config_file:=src/embr/config/sensors_sim.json +ros2 launch embr embr_launch.py config_file:=src/embr/config/sensors_sim.json ``` **Option 2: Mixed Mode (Some Real, Some Simulated)** ```bash -ros2 launch embr embr_launch_v2.py config_file:=src/embr/config/sensors_mixed.json +ros2 launch embr embr_launch.py config_file:=src/embr/config/sensors_mixed.json ``` **Option 3: Real Hardware (Default)** ```bash -ros2 launch embr embr_launch_v2.py +ros2 launch embr embr_launch.py # Uses config/sensors.json by default ``` **Option 4: Custom Configuration** ```bash # Create your own config file based on examples -ros2 launch embr embr_launch_v2.py config_file:=/path/to/custom_config.json +ros2 launch embr embr_launch.py config_file:=/path/to/custom_config.json ``` For detailed configuration options, see [Configuration Guide](ros2_ws/src/embr/config/CONFIG.md). @@ -144,7 +144,7 @@ docker compose exec embr-sim /bin/bash source install/setup.bash # Launch with simulation config -ros2 launch embr embr_launch_v2.py config_file:=config/sensors_sim.json +ros2 launch embr embr_launch.py config_file:=config/sensors_sim.json ``` ### Docker Documentation diff --git a/ros2_ws/README.md b/ros2_ws/README.md index d5e74ad..0e32583 100644 --- a/ros2_ws/README.md +++ b/ros2_ws/README.md @@ -34,15 +34,15 @@ ROS 2 workspace containing: Launch all EMBR nodes with the default config (real hardware sensors): ``` -ros2 launch embr embr_launch_v2.py +ros2 launch embr embr_launch.py ``` Run a specific node: ``` -ros2 run embr getTemp_v2 -ros2 run embr getCube_v2 -ros2 run embr sendRf_v2 +ros2 run embr getTemp +ros2 run embr getCube +ros2 run embr sendRf ``` ### Using Custom Configuration @@ -51,19 +51,19 @@ For simulation or custom sensor configurations, pass a different config file: ```bash # Full simulation mode -ros2 launch embr embr_launch_v2.py config_file:=config/sensors_sim.json +ros2 launch embr embr_launch.py config_file:=config/sensors_sim.json # Mixed mode (some real, some sim) -ros2 launch embr embr_launch_v2.py config_file:=config/sensors_mixed.json +ros2 launch embr embr_launch.py config_file:=config/sensors_mixed.json # Your own custom config -ros2 launch embr embr_launch_v2.py config_file:=/path/to/custom_config.json +ros2 launch embr embr_launch.py config_file:=/path/to/custom_config.json ``` Run individual nodes with custom configs: ```bash -ros2 run embr getTemp_v2 --ros-args -p config_file:=config/sensors_sim.json +ros2 run embr getTemp --ros-args -p config_file:=config/sensors_sim.json ``` ### Sensor Configuration diff --git a/ros2_ws/src/embr/config/CONFIG.md b/ros2_ws/src/embr/config/CONFIG.md index e726819..2601897 100644 --- a/ros2_ws/src/embr/config/CONFIG.md +++ b/ros2_ws/src/embr/config/CONFIG.md @@ -43,10 +43,10 @@ Simply run nodes without any parameters to use `config/sensors.json`: ```bash # Single node -ros2 run embr getTemp_v2 +ros2 run embr getTemp # All nodes via launch file -ros2 launch embr embr_launch_v2.py +ros2 launch embr embr_launch.py ``` ### Using Custom Configuration @@ -55,13 +55,13 @@ Pass a custom config file to override the default: ```bash # Single node with simulation config -ros2 run embr getTemp_v2 --ros-args -p config_file:=src/embr/config/sensors_sim.json +ros2 run embr getTemp --ros-args -p config_file:=src/embr/config/sensors_sim.json # All nodes with simulation config via launch file -ros2 launch embr embr_launch_v2.py config_file:=config/sensors_sim.json +ros2 launch embr embr_launch.py config_file:=config/sensors_sim.json # Use an external config file -ros2 launch embr embr_launch_v2.py config_file:=/path/to/my/custom_sensors.json +ros2 launch embr embr_launch.py config_file:=/path/to/my/custom_sensors.json ``` ## Troubleshooting diff --git a/ros2_ws/src/embr/embr/getCube.py b/ros2_ws/src/embr/embr/getCube.py old mode 100755 new mode 100644 index e7b1c4b..b686d19 --- a/ros2_ws/src/embr/embr/getCube.py +++ b/ros2_ws/src/embr/embr/getCube.py @@ -1,39 +1,99 @@ +""" +Cube Orange GPS publisher node with sensor abstraction. +Supports both real and simulated sensors. +""" + import rclpy from rclpy.node import Node import time -from dronekit import connect from msg_interface.msg import GPSAndIMU +from embr.sensors import create_sensor, SensorConfig, SensorFactory + class AttitudePublisher(Node): def __init__(self): super().__init__('attitude_publisher') - self.publisher_ = self.create_publisher(GPSAndIMU, 'gps_imu', 10) - self.vehicle = connect("/dev/ttyAMA0", wait_ready=False, baud=57600) - self.get_logger().info('Attitude Publisher node initialized') + + # Declare parameter for config file path + self.declare_parameter('config_file', '') + config_file = self.get_parameter('config_file').value + + # Use default config path if not provided + if not config_file: + config_file = 'src/embr/config/sensors.json' + + # Try to load from config file + try: + configs = SensorFactory.load_config(config_file) + config = configs.get('cube') + + if config is None: + self.get_logger().error(f'Cube sensor not found in config file: {config_file}') + raise ValueError(f'Cube sensor configuration not found in {config_file}') + + self.get_logger().info(f'Loaded cube config from {config_file}') + except Exception as e: + self.get_logger().error(f'Failed to load config file: {e}') + raise + + # Create sensor + try: + self.sensor = create_sensor('cube', config) + self.sensor.start() + + sensor_type = 'simulated' if 'Sim' in self.sensor.__class__.__name__ else 'real' + self.get_logger().info(f'Cube sensor initialized in {config.mode} mode (using {sensor_type} sensor)') + except Exception as e: + self.get_logger().error(f'Failed to initialize sensor: {e}') + raise + + # Create publisher and timer (GPS + IMU message) + self.publisher_ = self.create_publisher(GPSAndIMU, 'gps', 10) + self.timer = self.create_timer(1.0, self.publish_attitude) def publish_attitude(self): - while True: - location = self.vehicle.location.global_frame - attitude = self.vehicle.attitude - gps_imu_msg = GPSAndIMU() - gps_imu_msg.lat = int(location.lat * 1e7) - gps_imu_msg.lon = int(location.lon * 1e7) - gps_imu_msg.alt = int(location.alt * 1000) - gps_imu_msg.vel = self.vehicle.groundspeed - gps_imu_msg.yaw = attitude.yaw - gps_imu_msg.pitch = attitude.pitch - gps_imu_msg.roll = attitude.roll - self.get_logger().info(f'Published Telem Data: Lat: {gps_imu_msg.lat} Lon: {gps_imu_msg.lon} Alt: {gps_imu_msg.alt} Velocity: {gps_imu_msg.vel} Yaw: {gps_imu_msg.yaw} Pitch: {gps_imu_msg.pitch} Roll: {gps_imu_msg.roll}') - self.publisher_.publish(gps_imu_msg) - time.sleep(1) + """Read GPS data and publish.""" + try: + gps_data = self.sensor.read() + + gps_msg = GPSAndIMU() + # Core GPS fields + gps_msg.lat = float(gps_data.lat) + gps_msg.lon = float(gps_data.lon) + gps_msg.alt = float(gps_data.alt) + gps_msg.vel = float(gps_data.vel) + # IMU fields + gps_msg.pitch = float(gps_data.pitch) + gps_msg.yaw = float(gps_data.yaw) + gps_msg.roll = float(gps_data.roll) + + self.get_logger().info( + f'GPS: Lat: {gps_msg.lat} Lon: {gps_msg.lon} ' + f'Alt: {gps_msg.alt} Vel: {gps_msg.vel:.2f}' + ) + + self.publisher_.publish(gps_msg) + except Exception as e: + self.get_logger().error(f'Error reading GPS: {e}') + + def destroy_node(self): + """Cleanup sensor on shutdown.""" + if hasattr(self, 'sensor'): + self.sensor.stop() + super().destroy_node() + def main(args=None): rclpy.init(args=args) attitude_publisher = AttitudePublisher() - attitude_publisher.publish_attitude() - rclpy.spin(attitude_publisher) - attitude_publisher.destroy_node() - rclpy.shutdown() + try: + rclpy.spin(attitude_publisher) + except KeyboardInterrupt: + pass + finally: + attitude_publisher.destroy_node() + rclpy.shutdown() + if __name__ == '__main__': main() diff --git a/ros2_ws/src/embr/embr/getCube_v2.py b/ros2_ws/src/embr/embr/getCube_v2.py deleted file mode 100644 index b686d19..0000000 --- a/ros2_ws/src/embr/embr/getCube_v2.py +++ /dev/null @@ -1,99 +0,0 @@ -""" -Cube Orange GPS publisher node with sensor abstraction. -Supports both real and simulated sensors. -""" - -import rclpy -from rclpy.node import Node -import time -from msg_interface.msg import GPSAndIMU -from embr.sensors import create_sensor, SensorConfig, SensorFactory - - -class AttitudePublisher(Node): - def __init__(self): - super().__init__('attitude_publisher') - - # Declare parameter for config file path - self.declare_parameter('config_file', '') - config_file = self.get_parameter('config_file').value - - # Use default config path if not provided - if not config_file: - config_file = 'src/embr/config/sensors.json' - - # Try to load from config file - try: - configs = SensorFactory.load_config(config_file) - config = configs.get('cube') - - if config is None: - self.get_logger().error(f'Cube sensor not found in config file: {config_file}') - raise ValueError(f'Cube sensor configuration not found in {config_file}') - - self.get_logger().info(f'Loaded cube config from {config_file}') - except Exception as e: - self.get_logger().error(f'Failed to load config file: {e}') - raise - - # Create sensor - try: - self.sensor = create_sensor('cube', config) - self.sensor.start() - - sensor_type = 'simulated' if 'Sim' in self.sensor.__class__.__name__ else 'real' - self.get_logger().info(f'Cube sensor initialized in {config.mode} mode (using {sensor_type} sensor)') - except Exception as e: - self.get_logger().error(f'Failed to initialize sensor: {e}') - raise - - # Create publisher and timer (GPS + IMU message) - self.publisher_ = self.create_publisher(GPSAndIMU, 'gps', 10) - self.timer = self.create_timer(1.0, self.publish_attitude) - - def publish_attitude(self): - """Read GPS data and publish.""" - try: - gps_data = self.sensor.read() - - gps_msg = GPSAndIMU() - # Core GPS fields - gps_msg.lat = float(gps_data.lat) - gps_msg.lon = float(gps_data.lon) - gps_msg.alt = float(gps_data.alt) - gps_msg.vel = float(gps_data.vel) - # IMU fields - gps_msg.pitch = float(gps_data.pitch) - gps_msg.yaw = float(gps_data.yaw) - gps_msg.roll = float(gps_data.roll) - - self.get_logger().info( - f'GPS: Lat: {gps_msg.lat} Lon: {gps_msg.lon} ' - f'Alt: {gps_msg.alt} Vel: {gps_msg.vel:.2f}' - ) - - self.publisher_.publish(gps_msg) - except Exception as e: - self.get_logger().error(f'Error reading GPS: {e}') - - def destroy_node(self): - """Cleanup sensor on shutdown.""" - if hasattr(self, 'sensor'): - self.sensor.stop() - super().destroy_node() - - -def main(args=None): - rclpy.init(args=args) - attitude_publisher = AttitudePublisher() - try: - rclpy.spin(attitude_publisher) - except KeyboardInterrupt: - pass - finally: - attitude_publisher.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/ros2_ws/src/embr/embr/getTemp.py b/ros2_ws/src/embr/embr/getTemp.py index d6ef582..1e5e878 100644 --- a/ros2_ws/src/embr/embr/getTemp.py +++ b/ros2_ws/src/embr/embr/getTemp.py @@ -1,39 +1,88 @@ +""" +Temperature publisher node with sensor abstraction. +Supports both real and simulated sensors. +""" + import rclpy from rclpy.node import Node from sensor_msgs.msg import Temperature -import time -import re -import serial +from embr.sensors import create_sensor, SensorConfig, SensorFactory + class TemperaturePublisher(Node): def __init__(self): super().__init__('temperature_publisher') + + # Declare parameter for config file path + self.declare_parameter('config_file', '') + config_file = self.get_parameter('config_file').value + + # Use default config path if not provided + if not config_file: + config_file = 'config/sensors.json' + + # Try to load from config file + try: + configs = SensorFactory.load_config(config_file) + config = configs.get('temperature') + + if config is None: + self.get_logger().error(f'Temperature sensor not found in config file: {config_file}') + raise ValueError(f'Temperature sensor configuration not found in {config_file}') + + self.get_logger().info(f'Loaded temperature config from {config_file}') + except Exception as e: + self.get_logger().error(f'Failed to load config file: {e}') + raise + + # Create sensor + try: + self.sensor = create_sensor('temperature', config) + self.sensor.start() + + sensor_type = 'simulated' if isinstance(self.sensor.__class__.__name__, str) and 'Sim' in self.sensor.__class__.__name__ else 'real' + self.get_logger().info(f'Temperature sensor initialized in {config.mode} mode (using {sensor_type} sensor)') + except Exception as e: + self.get_logger().error(f'Failed to initialize sensor: {e}') + raise + + # Create publisher and timer self.publisher_ = self.create_publisher(Temperature, 'temperature', 10) - self.timer = self.create_timer(1.0, self.read_serial_data) - self.ser = serial.Serial('/dev/ttyACM0', 9600) - time.sleep(2) - - def read_serial_data(self): - raw = self.ser.readline().decode('utf-8', errors='ignore').strip() - - match = re.search(r"[-+]?\d*\.\d+|\d+", raw) # Match a float or int - if match: - temperature = float(match.group()) - self.get_logger().info(f"Received: {temperature:.2f}") + self.timer = self.create_timer(1.0, self.read_and_publish) + + def read_and_publish(self): + """Read temperature and publish.""" + try: + temperature = self.sensor.read() + self.get_logger().info(f"Temperature: {temperature:.2f}°C") + temperature_msg = Temperature() temperature_msg.header.stamp = self.get_clock().now().to_msg() temperature_msg.temperature = temperature temperature_msg.variance = 0.0 self.publisher_.publish(temperature_msg) - + except Exception as e: + self.get_logger().error(f'Error reading temperature: {e}') + + def destroy_node(self): + """Cleanup sensor on shutdown.""" + if hasattr(self, 'sensor'): + self.sensor.stop() + super().destroy_node() + def main(args=None): rclpy.init(args=args) temperature_publisher = TemperaturePublisher() - rclpy.spin(temperature_publisher) - temperature_publisher.destroy_node() - rclpy.shutdown() + try: + rclpy.spin(temperature_publisher) + except KeyboardInterrupt: + pass + finally: + temperature_publisher.destroy_node() + rclpy.shutdown() + if __name__ == '__main__': main() diff --git a/ros2_ws/src/embr/embr/getTemp_v2.py b/ros2_ws/src/embr/embr/getTemp_v2.py deleted file mode 100644 index 1e5e878..0000000 --- a/ros2_ws/src/embr/embr/getTemp_v2.py +++ /dev/null @@ -1,88 +0,0 @@ -""" -Temperature publisher node with sensor abstraction. -Supports both real and simulated sensors. -""" - -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Temperature -from embr.sensors import create_sensor, SensorConfig, SensorFactory - - -class TemperaturePublisher(Node): - def __init__(self): - super().__init__('temperature_publisher') - - # Declare parameter for config file path - self.declare_parameter('config_file', '') - config_file = self.get_parameter('config_file').value - - # Use default config path if not provided - if not config_file: - config_file = 'config/sensors.json' - - # Try to load from config file - try: - configs = SensorFactory.load_config(config_file) - config = configs.get('temperature') - - if config is None: - self.get_logger().error(f'Temperature sensor not found in config file: {config_file}') - raise ValueError(f'Temperature sensor configuration not found in {config_file}') - - self.get_logger().info(f'Loaded temperature config from {config_file}') - except Exception as e: - self.get_logger().error(f'Failed to load config file: {e}') - raise - - # Create sensor - try: - self.sensor = create_sensor('temperature', config) - self.sensor.start() - - sensor_type = 'simulated' if isinstance(self.sensor.__class__.__name__, str) and 'Sim' in self.sensor.__class__.__name__ else 'real' - self.get_logger().info(f'Temperature sensor initialized in {config.mode} mode (using {sensor_type} sensor)') - except Exception as e: - self.get_logger().error(f'Failed to initialize sensor: {e}') - raise - - # Create publisher and timer - self.publisher_ = self.create_publisher(Temperature, 'temperature', 10) - self.timer = self.create_timer(1.0, self.read_and_publish) - - def read_and_publish(self): - """Read temperature and publish.""" - try: - temperature = self.sensor.read() - self.get_logger().info(f"Temperature: {temperature:.2f}°C") - - temperature_msg = Temperature() - temperature_msg.header.stamp = self.get_clock().now().to_msg() - temperature_msg.temperature = temperature - temperature_msg.variance = 0.0 - - self.publisher_.publish(temperature_msg) - except Exception as e: - self.get_logger().error(f'Error reading temperature: {e}') - - def destroy_node(self): - """Cleanup sensor on shutdown.""" - if hasattr(self, 'sensor'): - self.sensor.stop() - super().destroy_node() - - -def main(args=None): - rclpy.init(args=args) - temperature_publisher = TemperaturePublisher() - try: - rclpy.spin(temperature_publisher) - except KeyboardInterrupt: - pass - finally: - temperature_publisher.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/ros2_ws/src/embr/embr/sendRf_v2.py b/ros2_ws/src/embr/embr/radio.py similarity index 100% rename from ros2_ws/src/embr/embr/sendRf_v2.py rename to ros2_ws/src/embr/embr/radio.py diff --git a/ros2_ws/src/embr/embr/sendRf.py b/ros2_ws/src/embr/embr/sendRf.py deleted file mode 100755 index f6f165f..0000000 --- a/ros2_ws/src/embr/embr/sendRf.py +++ /dev/null @@ -1,73 +0,0 @@ -from math import atan2, degrees, sqrt, pi -import numpy as np -import rclpy -import time -from rclpy.node import Node -from msg_interface.msg import GPSAndIMU -from pymavlink.dialects.v20 import common as mavlink2 -from pymavlink import mavutil -from sensor_msgs.msg import Temperature -from std_msgs.msg import Float32 -from std_msgs.msg import String # Example for publishing received data -from sensor_msgs.msg import PointCloud2 -import sensor_msgs_py.point_cloud2 as pc2 - -class CommSubscriber(Node): - def __init__(self): - super().__init__('mavlink_subscriber') - self.subscription = self.create_subscription(GPSAndIMU, 'gps_imu', self.cube_callback, 10) - self.subscription_temperature = self.create_subscription(Temperature, 'temperature', self.temperature_callback, 10) - self.subscription # prevent unused variable warning - self.mavlink_connection = mavutil.mavserial(device='/dev/ttyAMA1', baud=57600) - - self.mavlink_connection.mav = mavlink2.MAVLink(self.mavlink_connection) - - self.get_logger().info('Mavlink Subscriber node initialized') - - def temperature_callback(self, msg): - timems = int((time.time() - time.mktime(time.gmtime(0))) * 1000) % 4294967296 - self.get_logger().info(f"Temperature: {msg.temperature}") - self.mavlink_connection.mav.named_value_float_send( - time_boot_ms = timems, - name = b'temp', - value = msg.temperature) - - def cube_callback(self, msg): - lat = msg.lat - lon = msg.lon - alt = msg.alt - vel = int(msg.vel * 100) - timems = int((time.time() - time.mktime(time.gmtime(0))) * 1000) % 4294967296 - self.get_logger().info(f"Received Gps: Lat: {lat}, Lon: {lon}, Alt: {alt}, Vel: {vel}") - self.mavlink_connection.mav.global_position_int_send(timems, lat, lon, alt, 0, vel, 0, 0, 0) - - def read_mavlink_messages(self): - msg = self.mavlink_connection.recv_match(blocking=False) - if msg: - self.get_logger().info(f"Received MAVLink message: {msg}") - # Example: If you expect to receive a STATUSTEXT message - if msg.get_type() == "STATUSTEXT": - text = msg.text.decode().strip('\x00') - received_msg = String() - received_msg.data = f"Received STATUSTEXT: {text}" - self.received_data_publisher.publish(received_msg) - elif msg.get_type() == "NAMED_VALUE_FLOAT": - print(f"Received {message.name}: {message.value}") - name = msg.name.decode().strip('\x00') - value = msg.value - received_msg = String() - received_msg.data = f"Received {name}: {value}" - self.received_data_publisher.publish(received_msg) - # Add more conditions here to handle other expected message types - # elif msg.get_type() == "YOUR_MESSAGE_TYPE": - # # Extract data from the message and publish it as a ROS2 message - -def main(args=None): - rclpy.init(args=args) - comm_subscriber = CommSubscriber() - rclpy.spin(comm_subscriber) - comm_subscriber.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/ros2_ws/src/embr/launch/embr_launch.py b/ros2_ws/src/embr/launch/embr_launch.py index cd503f4..b2c746f 100644 --- a/ros2_ws/src/embr/launch/embr_launch.py +++ b/ros2_ws/src/embr/launch/embr_launch.py @@ -1,33 +1,51 @@ +""" +Launch file with sensor abstraction support. +Sensors are configured via config file (default: config/sensors.json). +Users can override the config file by passing a different one via config_file parameter. +""" + from launch import LaunchDescription from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + def generate_launch_description(): + # Declare launch argument for config file + config_file_arg = DeclareLaunchArgument( + 'config_file', + default_value='', + description='Path to sensor configuration file (default: config/sensors.json)' + ) + + # Get launch configuration + config_file = LaunchConfiguration('config_file') + return LaunchDescription([ + config_file_arg, + Node( package='embr', executable='getTemp', - name='getTemp' + name='getTemp', + parameters=[{ + 'config_file': config_file, + }] ), - Node( + Node( package='embr', executable='getCube', - name='getCube' + name='getCube', + parameters=[{ + 'config_file': config_file, + }] ), Node( package='embr', executable='sendRf', - name='sendRf' - ), - Node( - package='embr', - executable='thermalStream', - name='thermal_stream', - output='screen', - emulate_tty=True - ), - Node( - package='embr', - executable='hotspotLocator', - name='hotspot_locator' + name='sendRf', + parameters=[{ + 'config_file': config_file, + }] ) - ]) \ No newline at end of file + ]) diff --git a/ros2_ws/src/embr/launch/embr_launch_v2.py b/ros2_ws/src/embr/launch/embr_launch_v2.py deleted file mode 100644 index 346490c..0000000 --- a/ros2_ws/src/embr/launch/embr_launch_v2.py +++ /dev/null @@ -1,51 +0,0 @@ -""" -Launch file with sensor abstraction support. -Sensors are configured via config file (default: config/sensors.json). -Users can override the config file by passing a different one via config_file parameter. -""" - -from launch import LaunchDescription -from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration - - -def generate_launch_description(): - # Declare launch argument for config file - config_file_arg = DeclareLaunchArgument( - 'config_file', - default_value='', - description='Path to sensor configuration file (default: config/sensors.json)' - ) - - # Get launch configuration - config_file = LaunchConfiguration('config_file') - - return LaunchDescription([ - config_file_arg, - - Node( - package='embr', - executable='getTemp_v2', - name='getTemp', - parameters=[{ - 'config_file': config_file, - }] - ), - Node( - package='embr', - executable='getCube_v2', - name='getCube', - parameters=[{ - 'config_file': config_file, - }] - ), - Node( - package='embr', - executable='sendRf_v2', - name='sendRf', - parameters=[{ - 'config_file': config_file, - }] - ) - ]) diff --git a/ros2_ws/src/embr/setup.py b/ros2_ws/src/embr/setup.py index 39c54b5..6a4384c 100644 --- a/ros2_ws/src/embr/setup.py +++ b/ros2_ws/src/embr/setup.py @@ -27,9 +27,6 @@ 'getTemp = embr.getTemp:main', 'sendRf = embr.sendRf:main', 'thermalStream = embr.thermalStream:main', - 'getCube_v2 = embr.getCube_v2:main', - 'getTemp_v2 = embr.getTemp_v2:main', - 'sendRf_v2 = embr.sendRf_v2:main', 'hotspotLocator = embr.hotspotLocator:main', ], }, From 5586327b372f004a7cd8e561f23801439fe54bc4 Mon Sep 17 00:00:00 2001 From: mrichards03 Date: Sun, 23 Nov 2025 01:54:42 -0800 Subject: [PATCH 09/15] add thermal nodes to launch --- ros2_ws/src/embr/launch/embr_launch.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/ros2_ws/src/embr/launch/embr_launch.py b/ros2_ws/src/embr/launch/embr_launch.py index 7e7da7e..d88e219 100644 --- a/ros2_ws/src/embr/launch/embr_launch.py +++ b/ros2_ws/src/embr/launch/embr_launch.py @@ -47,5 +47,17 @@ def generate_launch_description(): parameters=[{ 'config_file': config_file, }] + ), + Node( + package='embr', + executable='thermalStream', + name='thermal_stream', + output='screen', + emulate_tty=True + ), + Node( + package='embr', + executable='hotspotLocator', + name='hotspot_locator' ) ]) From 3d9f512d3e96878d880e1a0ea1811aa4d3b100cf Mon Sep 17 00:00:00 2001 From: mrichards03 Date: Sun, 23 Nov 2025 01:56:38 -0800 Subject: [PATCH 10/15] removing old reference to topic --- ros2_ws/src/embr/embr/radio.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_ws/src/embr/embr/radio.py b/ros2_ws/src/embr/embr/radio.py index 46459fe..67c4636 100644 --- a/ros2_ws/src/embr/embr/radio.py +++ b/ros2_ws/src/embr/embr/radio.py @@ -62,7 +62,7 @@ def __init__(self): # --- Publish lat/long points to new topic # --- Once initial message is received, create subscriptions and allow sending --- - self.subscription = self.create_subscription(Gps, 'gps', self.cube_callback, 10) + self.subscription = self.create_subscription(GPSAndIMU, 'gps', self.cube_callback, 10) self.subscription_temperature = self.create_subscription( Temperature, 'temperature', self.temperature_callback, 10 ) From 26f29ac286d594af5522fca346c62d4f12e3d205 Mon Sep 17 00:00:00 2001 From: mrichards03 Date: Sun, 23 Nov 2025 14:54:50 -0800 Subject: [PATCH 11/15] thermal cam sensor class --- Documentation/2025/Thermal Camera.md | 42 ++++++ ros2_ws/README.md | 20 ++- ros2_ws/src/embr/config/CONFIG.md | 9 +- ros2_ws/src/embr/config/sensors.json | 13 ++ ros2_ws/src/embr/config/sensors_mixed.json | 18 +++ ros2_ws/src/embr/config/sensors_sim.json | 14 ++ ros2_ws/src/embr/embr/hotspotLocator.py | 43 ++++-- ros2_ws/src/embr/embr/sensors/__init__.py | 4 + ros2_ws/src/embr/embr/sensors/factory.py | 5 + ros2_ws/src/embr/embr/sensors/thermal.py | 157 +++++++++++++++++++++ ros2_ws/src/embr/embr/thermalStream.py | 83 ++++++----- ros2_ws/src/embr/test/test_sensors.py | 127 +++++++++++++++++ 12 files changed, 488 insertions(+), 47 deletions(-) create mode 100644 ros2_ws/src/embr/embr/sensors/thermal.py diff --git a/Documentation/2025/Thermal Camera.md b/Documentation/2025/Thermal Camera.md index 1abbf07..53bbc47 100644 --- a/Documentation/2025/Thermal Camera.md +++ b/Documentation/2025/Thermal Camera.md @@ -4,6 +4,48 @@ This guide covers using the FLIR Lepton 3.1R camera with a PureThermal 3 (PT3) b This document focuses on the software stack required to access this 14-bit raw temperature data, primarily on Linux. +## **EMBR-Bot Sensor Abstraction** + +**Configuration Parameters:** +- `model`: Lepton model - "2.5" (60×80) or "3.1R" (120×160) +- `altitude_m`: Camera mounting height above ground (meters) +- `pitch_deg`: Camera pitch angle (0° = horizontal, 90° = straight down) +- `display_width/height`: HDMI output resolution (pixels) +- `colormap`: OpenCV colormap for thermal visualization (e.g., "INFERNO", "JET", "HOT") +- `min_temp_c/max_temp_c`: Temperature range for colormap scaling (Celsius) + +**Simulation-Only Parameters:** +- `base_temp`: Base temperature in Celsius for simulated frames (default: 22.0) +- `temp_variation`: Random temperature variation range (default: 5.0) +- `hotspot_temp`: Temperature of simulated hotspots (default: 40.0) +- `num_hotspots`: Number of simulated hotspots (default: 2) + + +### ROS2 Nodes + +**thermalStream** - Main thermal camera streaming node +- Streams color-mapped thermal video to HDMI/framebuffer +- Publishes raw radiometric arrays to `/thermal/radiometric_array` +- Annotates and highlights hotspots above threshold +- All parameters loaded from config file + +**hotspotLocator** - GPS locator for thermal hotspots +- Subscribes to thermal arrays and GPS/IMU data +- Computes GPS coordinates of hotspots using camera geometry +- Publishes locations to `/thermal/hotspot_gps` + +Run with default config (real hardware): +```bash +ros2 launch embr embr_launch.py +``` + +Run in simulation mode: +```bash +ros2 launch embr embr_launch.py config_file:=src/embr/config/sensors_sim.json +``` + +--- + ## **The Core Challenge: Radiometry (14-bit) vs. Webcam Video (8-bit)** The PureThermal 3 board can expose the Lepton camera to your computer in two different modes over USB: diff --git a/ros2_ws/README.md b/ros2_ws/README.md index 49fe58b..4164001 100644 --- a/ros2_ws/README.md +++ b/ros2_ws/README.md @@ -68,9 +68,11 @@ ros2 run embr getTemp --ros-args -p config_file:=config/sensors_sim.json ### Sensor Configuration +All sensors (temperature, GPS/IMU, radio, thermal camera) use a unified sensor abstraction that supports both real hardware and simulated modes. + Sensors are configured via JSON files in `src/embr/config/`: - `sensors.json` - Default config for real hardware -- `sensors_sim.json` - Full simulation mode +- `sensors_sim.json` - Full simulation mode (no hardware required) - `sensors_mixed.json` - Mixed real/sim example See [Configuration Guide](src/embr/config/CONFIG.md) for detailed documentation. @@ -115,11 +117,19 @@ Publishes temperature data from serial sensor. Handles RF communication for telemetry. ### thermalStream -**New!** Thermal camera streaming node with HDMI output and intelligent frame publishing. -- Streams color-mapped thermal video over HDMI using FFmpeg +Thermal camera streaming node with HDMI output and intelligent frame publishing. +- Streams color-mapped thermal video over HDMI with direct framebuffer access - Highlights and annotates hotspots above configurable temperature threshold -- Publishes raw radiometric frames when vehicle velocity is zero -- See [Thermal Stream Node Documentation](../Documentation/2025/Thermal-Stream-Node.md) for details +- Publishes raw radiometric frames (uint16, Kelvin×100) when vehicle velocity is zero +- All parameters (camera mounting, thresholds, display settings) configurable via JSON +- See [Thermal Camera Documentation](../Documentation/2025/Thermal Camera.md) for details + +### hotspotLocator +GPS locator for thermal hotspots using camera geometry and vehicle position. +- Subscribes to thermal radiometric arrays and GPS/IMU data +- Computes GPS coordinates of detected hotspots using camera FOV and mounting angle +- Publishes hotspot locations as PoseStamped messages +- Camera mounting parameters (altitude, pitch) configurable via JSON ## Also see diff --git a/ros2_ws/src/embr/config/CONFIG.md b/ros2_ws/src/embr/config/CONFIG.md index 4e8a6f4..a796153 100644 --- a/ros2_ws/src/embr/config/CONFIG.md +++ b/ros2_ws/src/embr/config/CONFIG.md @@ -10,9 +10,16 @@ The default configuration file used by all nodes. This is configured for **real - **Temperature Sensor**: `/dev/ttyACM0` @ 9600 baud - **Cube Orange (GPS)**: `/dev/ttyAMA0` @ 57600 baud - **MAVLink Radio**: `/dev/ttyAMA1` @ 57600 baud +- **Thermal Camera**: Lepton 3.1R with default mounting and display settings All sensors are set to `"mode": "real"` to use actual hardware connections. +### `sensors_sim.json` (Full Simulation) +Complete simulation configuration for testing without any hardware. All sensors generate synthetic data. + +### `sensors_mixed.json` (Mixed Mode Example) +Example showing how to mix real and simulated sensors (e.g., real temperature sensor with simulated GPS). + ## Configuration Structure Each sensor configuration has the following structure: @@ -79,7 +86,7 @@ If a specific sensor is missing from your config file, you'll see an error like: Temperature sensor not found in config file: config/my_config.json ``` -Make sure your config file includes all required sensors: `temperature`, `cube`, and `radio`. +Make sure your config file includes all required sensors: `temperature`, `cube`, `radio`, and `thermal`. ### Permission Denied on Serial Ports diff --git a/ros2_ws/src/embr/config/sensors.json b/ros2_ws/src/embr/config/sensors.json index d6b26d2..8a78b40 100644 --- a/ros2_ws/src/embr/config/sensors.json +++ b/ros2_ws/src/embr/config/sensors.json @@ -13,5 +13,18 @@ "mode": "real", "device": "/dev/ttyAMA1", "baud": 57600 + }, + "thermal": { + "mode": "real", + "params": { + "model": "3.1R", + "altitude_m": 1.0, + "pitch_deg": 45.0, + "display_width": 640, + "display_height": 480, + "colormap": "INFERNO", + "min_temp_c": 10.0, + "max_temp_c": 80.0 + } } } diff --git a/ros2_ws/src/embr/config/sensors_mixed.json b/ros2_ws/src/embr/config/sensors_mixed.json index 4e47b00..6f0db0f 100644 --- a/ros2_ws/src/embr/config/sensors_mixed.json +++ b/ros2_ws/src/embr/config/sensors_mixed.json @@ -20,5 +20,23 @@ "mavlink": { "mode": "sim" + }, + + "thermal": { + "mode": "sim", + "params": { + "model": "3.1R", + "base_temp": 22.0, + "temp_variation": 5.0, + "hotspot_temp": 40.0, + "num_hotspots": 2, + "altitude_m": 1.0, + "pitch_deg": 45.0, + "display_width": 640, + "display_height": 480, + "colormap": "INFERNO", + "min_temp_c": 10.0, + "max_temp_c": 80.0 + } } } diff --git a/ros2_ws/src/embr/config/sensors_sim.json b/ros2_ws/src/embr/config/sensors_sim.json index b1cf562..0ef0a04 100644 --- a/ros2_ws/src/embr/config/sensors_sim.json +++ b/ros2_ws/src/embr/config/sensors_sim.json @@ -23,5 +23,19 @@ "radio": { "mode": "sim" + }, + + "thermal": { + "mode": "real", + "params": { + "model": "3.1R", + "altitude_m": 1.0, + "pitch_deg": 45.0, + "display_width": 640, + "display_height": 480, + "colormap": "INFERNO", + "min_temp_c": 10.0, + "max_temp_c": 80.0 + } } } diff --git a/ros2_ws/src/embr/embr/hotspotLocator.py b/ros2_ws/src/embr/embr/hotspotLocator.py index b7a8ec8..3fa1641 100644 --- a/ros2_ws/src/embr/embr/hotspotLocator.py +++ b/ros2_ws/src/embr/embr/hotspotLocator.py @@ -2,7 +2,7 @@ """ Thermal Hotspot GPS Locator (ROS2) -Production ROS2 node that subscribes to the radiometric 2D array +ROS2 node that subscribes to the radiometric 2D array published by the thermal stream node and computes the nearest hotspot's GPS coordinates based on camera geometry, GPS, and IMU heading. @@ -23,6 +23,7 @@ from std_msgs.msg import UInt16MultiArray from geometry_msgs.msg import PoseStamped from msg_interface.msg import GPSAndIMU +from embr.sensors import SensorFactory, SensorConfig import numpy as np import cv2 @@ -34,18 +35,44 @@ class hotspotLocator(Node): def __init__(self): super().__init__('thermal_hotspot_locator') - # Configuration (can be adjusted here) + # Declare parameter for config file path + self.declare_parameter('config_file', '') + config_file = self.get_parameter('config_file').value + + # Use default config path if not provided + if not config_file: + config_file = 'src/embr/config/sensors.json' + + # Try to load thermal camera config to get model information + try: + configs = SensorFactory.load_config(config_file) + thermal_config = configs.get('thermal') + + if thermal_config is None: + self.get_logger().warning(f'Thermal sensor not found in config file: {config_file}. Using defaults.') + self.lepton_model = '3.1R' + params = {} + else: + params = thermal_config.params + self.lepton_model = params.get('model', '3.1R') + self.get_logger().info(f'Loaded thermal camera config from {config_file}: model={self.lepton_model}') + except Exception as e: + self.get_logger().warning(f'Failed to load config file: {e}. Using defaults.') + self.lepton_model = '3.1R' + params = {} + + # Configuration - load from config params with defaults # Camera geometry - self.lepton_model = '3.1R' # '2.5' or '3.1R' - self.altitude_m = 1.0 # Camera height above ground (meters) - self.pitch_deg = 45.0 # Camera pitch (0=horizontal, 90=down) - self.temp_threshold_c = 30.0 # Celsius threshold for hotspot detection + self.altitude_m = params.get('altitude_m', 1.0) # Camera height above ground (meters) + self.pitch_deg = params.get('pitch_deg', 45.0) # Camera pitch (0=horizontal, 90=down) + # Temp threshold will be set via radio commands, default here + self.temp_threshold_c = 30.0 # Celsius - will be updated via radio # Topics (override here if needed) self.array_topic = 'thermal/radiometric_array' self.gps_imu_topic = 'gps' - # Set Lepton model parameters + # Set Lepton model parameters based on config self._set_camera_model(self.lepton_model) # State @@ -62,7 +89,7 @@ def __init__(self): # Publisher self.hotspot_pub = self.create_publisher(PoseStamped, '/thermal/hotspot_gps', 10) - self.get_logger().info('hotspotLocator started') + self.get_logger().info(f'hotspotLocator started (camera model: {self.lepton_model})') def _set_camera_model(self, model: str) -> None: if model == '2.5': diff --git a/ros2_ws/src/embr/embr/sensors/__init__.py b/ros2_ws/src/embr/embr/sensors/__init__.py index 59f4d9f..1c3ba30 100644 --- a/ros2_ws/src/embr/embr/sensors/__init__.py +++ b/ros2_ws/src/embr/embr/sensors/__init__.py @@ -8,6 +8,7 @@ from .temperature import TemperatureSensor, RealTemperatureSensor, SimTemperatureSensor from .cube import CubeSensor, RealCubeSensor, SimCubeSensor from .radio import RadioConnection, RealRadioConnection, SimRadioConnection +from .thermal import ThermalCameraSensor, RealThermalCameraSensor, SimThermalCameraSensor __all__ = [ 'Sensor', @@ -23,4 +24,7 @@ 'RadioConnection', 'RealRadioConnection', 'SimRadioConnection', + 'ThermalCameraSensor', + 'RealThermalCameraSensor', + 'SimThermalCameraSensor', ] diff --git a/ros2_ws/src/embr/embr/sensors/factory.py b/ros2_ws/src/embr/embr/sensors/factory.py index b477cfb..9c2cdda 100644 --- a/ros2_ws/src/embr/embr/sensors/factory.py +++ b/ros2_ws/src/embr/embr/sensors/factory.py @@ -9,6 +9,7 @@ from .temperature import RealTemperatureSensor, SimTemperatureSensor from .cube import RealCubeSensor, SimCubeSensor from .radio import RealRadioConnection, SimRadioConnection +from .thermal import RealThermalCameraSensor, SimThermalCameraSensor class SensorFactory: @@ -28,6 +29,10 @@ class SensorFactory: 'real': RealRadioConnection, 'sim': SimRadioConnection, }, + 'thermal': { + 'real': RealThermalCameraSensor, + 'sim': SimThermalCameraSensor, + }, } @classmethod diff --git a/ros2_ws/src/embr/embr/sensors/thermal.py b/ros2_ws/src/embr/embr/sensors/thermal.py new file mode 100644 index 0000000..fd0a1f6 --- /dev/null +++ b/ros2_ws/src/embr/embr/sensors/thermal.py @@ -0,0 +1,157 @@ +"""Thermal camera sensor implementations.""" + +import time +import numpy as np +from typing import Optional, Dict, Any +from .base import Sensor, SensorConfig + + +class ThermalCameraSensor(Sensor): + """Abstract thermal camera sensor interface.""" + + def get_frame_shape(self) -> tuple: + """Get the shape of thermal frames (height, width).""" + raise NotImplementedError + + +class RealThermalCameraSensor(ThermalCameraSensor): + """Real thermal camera sensor using Lepton via flirpy.""" + + def __init__(self, config: Optional[SensorConfig] = None): + super().__init__(config) + self.camera = None + self.lepton_model = config.params.get('model', '3.1R') if config else '3.1R' + + def start(self) -> None: + """Initialize Lepton camera.""" + if self._running: + return + + try: + from flirpy.camera.lepton import Lepton + self.camera = Lepton() + self._running = True + except Exception as e: + raise RuntimeError(f"Failed to initialize Lepton camera: {e}") + + def read(self) -> np.ndarray: + """ + Read radiometric frame from camera. + + Returns: + 2D numpy array of uint16 values (Kelvin * 100) + """ + if not self._running: + raise RuntimeError("Sensor not started") + + try: + frame = self.camera.grab() + if frame is None: + raise RuntimeError("Failed to grab frame from camera") + return frame + except Exception as e: + raise RuntimeError(f"Failed to read thermal frame: {e}") + + def get_frame_shape(self) -> tuple: + """Get the shape of thermal frames based on Lepton model.""" + if self.lepton_model == '2.5': + return (60, 80) + elif self.lepton_model == '3.1R': + return (120, 160) + else: + # Default to 3.1R + return (120, 160) + + def stop(self) -> None: + """Close camera connection.""" + if self.camera: + try: + self.camera.close() + except Exception: + pass + self._running = False + + +class SimThermalCameraSensor(ThermalCameraSensor): + """Simulated thermal camera with realistic temperature patterns.""" + + def __init__(self, config: Optional[SensorConfig] = None): + super().__init__(config) + params = config.params if config else {} + # Resolution derived from Lepton model + model = params.get('model') + self.width, self.height = self._model_default_dims(model) + self.base_temp = params.get('base_temp', 22.0) # Celsius + self.temp_variation = params.get('temp_variation', 5.0) + self.hotspot_temp = params.get('hotspot_temp', 40.0) + self.num_hotspots = params.get('num_hotspots', 2) + self._start_time = 0.0 + self._hotspot_positions = [] + + @staticmethod + def _model_default_dims(model: Optional[str]) -> tuple: + """Return default (width, height) for given Lepton model name.""" + if model == '2.5': + return (80, 60) + # Treat anything else (including None, '3.1R') as 3.1R + return (160, 120) + + def start(self) -> None: + """Initialize simulated camera.""" + self._start_time = time.time() + self._running = True + # Generate random hotspot positions + import random + self._hotspot_positions = [ + (random.randint(20, self.width - 20), random.randint(20, self.height - 20)) + for _ in range(self.num_hotspots) + ] + + def read(self) -> np.ndarray: + """ + Generate simulated thermal frame with hotspots. + + Returns: + 2D numpy array of uint16 values (Kelvin * 100) + """ + if not self._running: + raise RuntimeError("Sensor not started") + + import random + + # Create base temperature field with some variation + frame = np.random.uniform( + self.base_temp - self.temp_variation, + self.base_temp + self.temp_variation, + (self.height, self.width) + ) + + # Add hotspots with Gaussian distribution + for hx, hy in self._hotspot_positions: + y, x = np.ogrid[:self.height, :self.width] + # Create Gaussian hotspot + sigma = 10.0 + hotspot = np.exp(-((x - hx)**2 + (y - hy)**2) / (2 * sigma**2)) + frame += hotspot * (self.hotspot_temp - self.base_temp) + + # Add temporal variation (simulated movement/flicker) + elapsed = time.time() - self._start_time + temporal_noise = np.sin(elapsed) * 2.0 + frame += temporal_noise + + # Convert to Kelvin * 100 format (same as Lepton output) + frame_kelvin = (frame + 273.15) * 100.0 + frame_uint16 = np.clip(frame_kelvin, 0, 65535).astype(np.uint16) + + # Simulate camera frame rate delay + time.sleep(1.0 / 9.0) # ~9 FPS like Lepton + + return frame_uint16 + + def get_frame_shape(self) -> tuple: + """Get the shape of simulated thermal frames.""" + return (self.height, self.width) + + def stop(self) -> None: + """Stop simulated camera.""" + self._running = False diff --git a/ros2_ws/src/embr/embr/thermalStream.py b/ros2_ws/src/embr/embr/thermalStream.py index c7c512b..eab04f9 100644 --- a/ros2_ws/src/embr/embr/thermalStream.py +++ b/ros2_ws/src/embr/embr/thermalStream.py @@ -1,49 +1,66 @@ #!/usr/bin/env python3 """ -Thermal Camera Streaming Node with Temperature Overlay and Frame Publishing -Uses flirpy to capture Lepton thermal data, streams via HDMI with ffmpeg, -and publishes radiometric frames when vehicle velocity is 0. +Thermal Camera Streaming Node with Temperature Overlay. +Uses sensor abstraction to support both real and simulated thermal cameras. +Streams via HDMI with direct framebuffer access and publishes the radiometric array (UInt16, Kelvin*100) when stationary. """ import rclpy from rclpy.node import Node -from sensor_msgs.msg import Image from std_msgs.msg import UInt16MultiArray, MultiArrayDimension, MultiArrayLayout from msg_interface.msg import GPSAndIMU -from cv_bridge import CvBridge +from embr.sensors import create_sensor, SensorConfig, SensorFactory import cv2 import numpy as np import subprocess import threading import queue -from flirpy.camera.lepton import Lepton class ThermalStreamNode(Node): def __init__(self): super().__init__('thermal_stream_node') - # Configuration - can be modified here directly - self.temp_threshold = 30.0 # Celsius - temperature threshold for hotspot detection + # Declare parameter for config file path + self.declare_parameter('config_file', '') + config_file = self.get_parameter('config_file').value + + # Use default config path if not provided + if not config_file: + config_file = 'src/embr/config/sensors.json' + + # Try to load from config file + try: + configs = SensorFactory.load_config(config_file) + config = configs.get('thermal') + + if config is None: + self.get_logger().warning(f'Thermal sensor not found in config file: {config_file}. Using default real mode.') + config = SensorConfig(mode='real', params={'model': '3.1R'}) + else: + self.get_logger().info(f'Loaded thermal camera config from {config_file}') + except Exception as e: + self.get_logger().warning(f'Failed to load config file: {e}. Using default real mode.') + config = SensorConfig(mode='real', params={'model': '3.1R'}) + + # Configuration - load from config params with defaults + params = config.params if config else {} + # Temp threshold will be set via radio commands, default here + self.temp_threshold = 30.0 # Celsius - will be updated via radio self.video_fps = 9.0 # Lepton typical fps - self.display_width = 640 - self.display_height = 480 + self.display_width = params.get('display_width', 640) + self.display_height = params.get('display_height', 480) # use a reddish/inferno colormap for thermal-style colors - self.colormap = cv2.COLORMAP_INFERNO - self.min_temp = 10.0 # Celsius - minimum temperature for colormap scaling - self.max_temp = 80.0 # Celsius - maximum temperature for colormap scaling + colormap_name = params.get('colormap', 'INFERNO') + self.colormap = getattr(cv2, f'COLORMAP_{colormap_name}', cv2.COLORMAP_INFERNO) + self.min_temp = params.get('min_temp_c', 10.0) # Celsius - minimum temperature for colormap scaling + self.max_temp = params.get('max_temp_c', 80.0) # Celsius - maximum temperature for colormap scaling # State variables self.current_velocity = None self.velocity_lock = threading.Lock() - # Publishers - self.frame_publisher = self.create_publisher( - Image, - 'thermal/radiometric_frame', - 10 - ) - # Optional: publish radiometric array (uint16, Kelvin x 100) for direct analysis + # Publisher: radiometric array (uint16, Kelvin x 100) for direct analysis self.array_publisher = self.create_publisher( UInt16MultiArray, 'thermal/radiometric_array', @@ -58,14 +75,14 @@ def __init__(self): 10 ) - # CV Bridge for ROS image messages - self.bridge = CvBridge() - - # Initialize camera - self.get_logger().info('Initializing Lepton camera...') + # Initialize camera using sensor abstraction + self.get_logger().info('Initializing thermal camera...') try: - self.camera = Lepton() - self.get_logger().info('Lepton camera initialized successfully') + self.camera = create_sensor('thermal', config) + self.camera.start() + + sensor_type = 'simulated' if 'Sim' in self.camera.__class__.__name__ else 'real' + self.get_logger().info(f'Thermal camera initialized in {config.mode} mode (using {sensor_type} sensor)') except Exception as e: self.get_logger().error(f'Failed to initialize camera: {e}') raise @@ -240,8 +257,8 @@ def _streaming_loop(self): while self.streaming_active and rclpy.ok(): try: - # Capture radiometric frame from camera - radiometric_frame = self.camera.grab() + # Capture radiometric frame from camera using sensor abstraction + radiometric_frame = self.camera.read() if radiometric_frame is None: self.get_logger().warn('Failed to grab frame from camera') @@ -414,8 +431,8 @@ def _add_info_overlay(self, frame, temp_celsius): def _capture_and_publish(self): """Capture once and publish both radiometric image and temperature array""" try: - # Capture fresh radiometric frame - radiometric_frame = self.camera.grab() + # Capture fresh radiometric frame using sensor abstraction + radiometric_frame = self.camera.read() if radiometric_frame is None: self.get_logger().warn('Failed to capture radiometric frame') @@ -452,10 +469,10 @@ def destroy_node(self): if self.stream_thread.is_alive(): self.stream_thread.join(timeout=2.0) - # Close camera + # Close camera using sensor abstraction try: if hasattr(self, 'camera'): - self.camera.close() + self.camera.stop() except Exception as e: self.get_logger().error(f'Error closing camera: {e}') diff --git a/ros2_ws/src/embr/test/test_sensors.py b/ros2_ws/src/embr/test/test_sensors.py index bb4b165..eb348ef 100644 --- a/ros2_ws/src/embr/test/test_sensors.py +++ b/ros2_ws/src/embr/test/test_sensors.py @@ -2,12 +2,14 @@ import pytest import time +import numpy as np from embr.sensors import ( SensorConfig, create_sensor, SimTemperatureSensor, SimCubeSensor, SimRadioConnection, + SimThermalCameraSensor, ) @@ -98,6 +100,118 @@ def test_simulated_sensor_movement(self): # Velocity should match config assert all(abs(p.vel - 10.0) < 0.1 for p in positions) + +class TestThermalCameraSensor: + """Tests for thermal camera sensor.""" + + def test_simulated_sensor_basic(self): + """Test basic simulated thermal camera operation.""" + config = SensorConfig( + mode='sim', + params={ + # Use 3.1R model (default) -> 160x120 resolution + 'model': '3.1R', + 'base_temp': 22.0, + 'hotspot_temp': 40.0, + 'num_hotspots': 2 + } + ) + sensor = create_sensor('thermal', config) + + assert not sensor.is_running + + sensor.start() + assert sensor.is_running + + # Read a frame + frame = sensor.read() + assert isinstance(frame, np.ndarray) + assert frame.dtype == np.uint16 + assert frame.shape == (120, 160) + + sensor.stop() + assert not sensor.is_running + + def test_simulated_sensor_temperature_range(self): + """Test thermal camera outputs correct temperature range.""" + config = SensorConfig( + mode='sim', + params={ + # Use 2.5 model -> 80x60 resolution + 'model': '2.5', + 'base_temp': 22.0, + 'temp_variation': 5.0, + 'hotspot_temp': 40.0, + 'num_hotspots': 1 + } + ) + sensor = create_sensor('thermal', config) + + with sensor: + frame = sensor.read() + + # Convert from Kelvin*100 to Celsius + temp_celsius = (frame / 100.0) - 273.15 + + # Should have base temperature around 22C + assert temp_celsius.min() > 10.0 + assert temp_celsius.max() < 50.0 + + # Should have at least some hot pixels + hot_pixels = np.sum(temp_celsius > 30.0) + assert hot_pixels > 0 + + def test_simulated_sensor_context_manager(self): + """Test using thermal sensor as context manager.""" + config = SensorConfig( + mode='sim', + params={'model': '2.5'} + ) + sensor = create_sensor('thermal', config) + + with sensor: + assert sensor.is_running + frame = sensor.read() + assert frame.shape == (60, 80) + + assert not sensor.is_running + + def test_simulated_sensor_frame_shape(self): + """Test get_frame_shape method.""" + # Test both model resolutions + config_big = SensorConfig(mode='sim', params={'model': '3.1R'}) + sensor_big = create_sensor('thermal', config_big) + assert sensor_big.get_frame_shape() == (120, 160) + with sensor_big: + assert sensor_big.get_frame_shape() == (120, 160) + frame_big = sensor_big.read() + assert frame_big.shape == sensor_big.get_frame_shape() + + config_small = SensorConfig(mode='sim', params={'model': '2.5'}) + sensor_small = create_sensor('thermal', config_small) + assert sensor_small.get_frame_shape() == (60, 80) + with sensor_small: + assert sensor_small.get_frame_shape() == (60, 80) + frame_small = sensor_small.read() + assert frame_small.shape == sensor_small.get_frame_shape() + + def test_simulated_sensor_multiple_frames(self): + """Test reading multiple frames shows temporal variation.""" + config = SensorConfig(mode='sim', params={'num_hotspots': 2}) + sensor = create_sensor('thermal', config) + + with sensor: + frames = [sensor.read() for _ in range(3)] + + # All frames should be valid + assert all(isinstance(f, np.ndarray) for f in frames) + assert all(f.dtype == np.uint16 for f in frames) + + # Frames should vary slightly due to temporal noise + # (though they may be very similar in sim mode) + means = [f.mean() for f in frames] + assert all(20000 < m < 35000 for m in means) # Reasonable range in Kelvin*100 + class TestRadioConnection: """Tests for Radio connection.""" @@ -164,6 +278,19 @@ def sim_radio_connection(): conn.stop() +@pytest.fixture +def sim_thermal_sensor(): + """Fixture providing a simulated thermal camera sensor.""" + config = SensorConfig( + mode='sim', + params={'model': '3.1R', 'num_hotspots': 2} + ) + sensor = create_sensor('thermal', config) + sensor.start() + yield sensor + sensor.stop() + + def test_integration_temperature_to_radio(sim_temperature_sensor, sim_radio_connection): """Test integration between temperature sensor and Radio.""" temp = sim_temperature_sensor.read() From bc1021cd468ca0200537e5d44a722f7273469dd8 Mon Sep 17 00:00:00 2001 From: mrichards03 Date: Mon, 24 Nov 2025 02:57:03 +0000 Subject: [PATCH 12/15] sensor configs changed --- ros2_ws/src/embr/config/sensors_mixed.json | 20 ++------------------ 1 file changed, 2 insertions(+), 18 deletions(-) diff --git a/ros2_ws/src/embr/config/sensors_mixed.json b/ros2_ws/src/embr/config/sensors_mixed.json index 6f0db0f..b3a2c5c 100644 --- a/ros2_ws/src/embr/config/sensors_mixed.json +++ b/ros2_ws/src/embr/config/sensors_mixed.json @@ -1,12 +1,5 @@ { - "comment": "Example configuration for mixed mode - temperature real, others simulated", - - "temperature": { - "mode": "real", - "device": "/dev/ttyACM0", - "baud": 9600 - }, - + "comment": "Example configuration for mixed mode - temperature real, others simulated", "cube": { "mode": "sim", "params": { @@ -17,19 +10,10 @@ "pattern": "hover" } }, - - "mavlink": { - "mode": "sim" - }, - "thermal": { - "mode": "sim", + "mode": "real", "params": { "model": "3.1R", - "base_temp": 22.0, - "temp_variation": 5.0, - "hotspot_temp": 40.0, - "num_hotspots": 2, "altitude_m": 1.0, "pitch_deg": 45.0, "display_width": 640, From 15bb1be516ae9301e0060d5a2bd220606e4090d5 Mon Sep 17 00:00:00 2001 From: mrichards03 Date: Fri, 20 Feb 2026 04:20:17 +0000 Subject: [PATCH 13/15] hotspotLocator much more accurate --- Documentation/2025/Cube Orange.md | 10 +- Documentation/2025/Thermal Camera.md | 4 + ros2_ws/requirements.txt | 1 + ros2_ws/src/embr/config/sensors.json | 15 ++- ros2_ws/src/embr/config/sensors_mixed.json | 19 ++- ros2_ws/src/embr/embr/hotspotLocator.py | 134 ++++++++++++++++----- ros2_ws/src/embr/embr/sensors/cube.py | 2 +- ros2_ws/src/embr/embr/sensors/thermal.py | 62 +++++++--- ros2_ws/src/embr/embr/thermalStream.py | 42 ++----- ros2_ws/src/embr/test/test_sensors.py | 21 +--- 10 files changed, 194 insertions(+), 116 deletions(-) diff --git a/Documentation/2025/Cube Orange.md b/Documentation/2025/Cube Orange.md index 577c056..4b2728b 100644 --- a/Documentation/2025/Cube Orange.md +++ b/Documentation/2025/Cube Orange.md @@ -15,7 +15,7 @@ Changes: Tip: To locate the file, you can run `python3 -c "import dronekit, inspect, os; print(os.path.dirname(inspect.getfile(dronekit)))"`. -## Radio receiver and RC input +## Radio receiver and RC input (Not required for Herelink) Connect your receiver to the Cube’s RC input: @@ -29,6 +29,7 @@ Transmitter configuration (example): 3. Choose output PPM and Serial i-BUS Calibrate radio in your flight stack (Mission Planner) so channels map as expected. +This is done under Servo output in mission planner setup tab. ## ESC and motor outputs @@ -39,6 +40,7 @@ Connect ESC signal leads to Cube Main Out pins: - Servo Output index correlates to Main Out N - Set the Function to match your control output from Radio Calibration -Safety -- Remove props during setup and calibration -- Ensure proper power distribution for ESCs and receiver \ No newline at end of file +**Note** You will need to "arm" the cube to be able to run the motors. This can be done by plugging in red led button that comes with the cube into the GPS port and holding it until it goes from flashing red to solid. + +## Lidar +https://www.youtube.com/watch?v=OCMjvF--N_E&t=112s \ No newline at end of file diff --git a/Documentation/2025/Thermal Camera.md b/Documentation/2025/Thermal Camera.md index 53bbc47..d7ed7cc 100644 --- a/Documentation/2025/Thermal Camera.md +++ b/Documentation/2025/Thermal Camera.md @@ -1,5 +1,9 @@ # **FLIR Lepton 3.1R Radiometric Camera Guide** +## Setup with Herelink +You must be running the `thermalStream` node with the FLIR connected over USB-C. The HDMI output must be connected to HDMI 1 (not 0). Then connect HDMI to herelink air unit. +Ensure cube is turned on and connected to herelink airunit. Open QGroundControl on Herelink ground unit, click drop down from box showing values and choose Video Stream 1 and enable. + This guide covers using the FLIR Lepton 3.1R camera with a PureThermal 3 (PT3) breakout board. The Lepton 3.1R is a **radiometric** camera, meaning it can output a 2D array of actual temperature values, not just a colorized image. This document focuses on the software stack required to access this 14-bit raw temperature data, primarily on Linux. diff --git a/ros2_ws/requirements.txt b/ros2_ws/requirements.txt index d85400f..ac32ac5 100644 --- a/ros2_ws/requirements.txt +++ b/ros2_ws/requirements.txt @@ -6,3 +6,4 @@ flirpy numpy<2 opencv-python geopy +future diff --git a/ros2_ws/src/embr/config/sensors.json b/ros2_ws/src/embr/config/sensors.json index 8a78b40..830c7f3 100644 --- a/ros2_ws/src/embr/config/sensors.json +++ b/ros2_ws/src/embr/config/sensors.json @@ -7,7 +7,14 @@ "cube": { "mode": "real", "device": "/dev/ttyAMA0", - "baud": 57600 + "baud": 57600, + "params": { + "start_lat": 37.7749, + "start_lon": -122.4194, + "start_alt": 100.0, + "velocity": 5.0, + "pattern": "hover" + } }, "radio": { "mode": "real", @@ -18,13 +25,13 @@ "mode": "real", "params": { "model": "3.1R", - "altitude_m": 1.0, - "pitch_deg": 45.0, + "altitude_m": 0.086, + "pitch_deg": 0, "display_width": 640, "display_height": 480, "colormap": "INFERNO", "min_temp_c": 10.0, - "max_temp_c": 80.0 + "max_temp_c": 200.0 } } } diff --git a/ros2_ws/src/embr/config/sensors_mixed.json b/ros2_ws/src/embr/config/sensors_mixed.json index b3a2c5c..424ee12 100644 --- a/ros2_ws/src/embr/config/sensors_mixed.json +++ b/ros2_ws/src/embr/config/sensors_mixed.json @@ -14,13 +14,24 @@ "mode": "real", "params": { "model": "3.1R", - "altitude_m": 1.0, - "pitch_deg": 45.0, - "display_width": 640, - "display_height": 480, + "altitude_m": 0.508, + "pitch_deg": 0, + "display_width": 1024, + "display_height": 768, "colormap": "INFERNO", "min_temp_c": 10.0, "max_temp_c": 80.0 } + }, + "temperature": { + "mode": "sim", + "params": { + "base_temp": 22.0, + "variation": 2.0, + "noise": 0.1 + } + }, + "radio": { + "mode": "sim" } } diff --git a/ros2_ws/src/embr/embr/hotspotLocator.py b/ros2_ws/src/embr/embr/hotspotLocator.py index 3fa1641..a40144b 100644 --- a/ros2_ws/src/embr/embr/hotspotLocator.py +++ b/ros2_ws/src/embr/embr/hotspotLocator.py @@ -23,7 +23,7 @@ from std_msgs.msg import UInt16MultiArray from geometry_msgs.msg import PoseStamped from msg_interface.msg import GPSAndIMU -from embr.sensors import SensorFactory, SensorConfig +from embr.sensors import SensorFactory, SensorConfig, create_sensor import numpy as np import cv2 @@ -61,6 +61,9 @@ def __init__(self): self.lepton_model = '3.1R' params = {} + # Create thermal sensor object from config so we can access camera intrinsics + self.thermal_sensor = create_sensor('thermal', thermal_config) + # Configuration - load from config params with defaults # Camera geometry self.altitude_m = params.get('altitude_m', 1.0) # Camera height above ground (meters) @@ -73,11 +76,14 @@ def __init__(self): self.gps_imu_topic = 'gps' # Set Lepton model parameters based on config - self._set_camera_model(self.lepton_model) + self._set_camera_model(self.lepton_model) # State self.current_gps: Optional[Tuple[float, float]] = None # (lat, lon) self.current_heading_deg: float = 0.0 # compass heading 0-360, 0=N, 90=E + self.current_roll_deg: float = 0.0 + self.current_pitch_deg: float = 0.0 + self.current_yaw_deg: float = 0.0 self.have_gps: bool = False # internal flag to avoid spamming the log while waiting for GPS self._warned_waiting_gps: bool = False @@ -106,20 +112,22 @@ def _set_camera_model(self, model: str) -> None: # -------------------- Subscribers -------------------- def _gps_imu_cb(self, msg: GPSAndIMU) -> None: - # Extract GPS (lat/lon are int32 in 1e7 format, alt is int32 in mm) - self.current_gps = (float(msg.lat) / 1e7, float(msg.lon) / 1e7) + # Extract GPS (lat/lon are floats in degrees) + self.current_gps = (float(msg.lat), float(msg.lon)) self.have_gps = True - - # Extract heading from yaw (yaw is in radians as int32, needs conversion) - # The yaw field appears to be in radians based on getCube.py using attitude.yaw - yaw_rad = float(msg.yaw) - + + # Store IMU attitude (assume msg pitch/yaw/roll are in degrees from cube sensor) + self.current_pitch_deg = float(msg.pitch) + self.current_yaw_deg = float(msg.yaw) + self.current_roll_deg = float(msg.roll) + # Convert yaw to compass heading: 0 = North, 90 = East # Assuming yaw is ENU convention: 0 = East, +pi/2 = North - heading = (90.0 - math.degrees(yaw_rad)) % 360.0 + heading = (90.0 - math.degrees(msg.yaw)) % 360.0 self.current_heading_deg = heading def _array_cb(self, msg: UInt16MultiArray) -> None: + self.get_logger().info("Thermal Array received") if not self.have_gps: # warn once to avoid log spam while waiting for GPS if not getattr(self, '_warned_waiting_gps', False): @@ -128,17 +136,10 @@ def _array_cb(self, msg: UInt16MultiArray) -> None: return # Determine image shape from layout - if len(msg.layout.dim) >= 2: - h = msg.layout.dim[0].size - w = msg.layout.dim[1].size - else: - # Fallback: assume Lepton 2.5 default if no layout - h, w = 60, 80 + h = msg.layout.dim[0].size + w = msg.layout.dim[1].size rad = np.array(msg.data, dtype=np.uint16) - if rad.size != h * w: - self.get_logger().warning(f'Array size mismatch: {rad.size} vs {h}x{w}') - return rad = rad.reshape((h, w)) # Convert to Celsius for analysis @@ -147,10 +148,12 @@ def _array_cb(self, msg: UInt16MultiArray) -> None: # Detect hotspots and publish nearest hotspots = self._find_hotspots(temp_c, self.temp_threshold_c) if not hotspots: + self.get_logger().info("No hotspots found") return nearest = self._compute_nearest_hotspot_gps(hotspots, w, h) if nearest is None: + self.get_logger().info("Nearest hotspot not found") return self._publish_hotspot(nearest) @@ -186,6 +189,42 @@ def _pixel_to_angle(self, x: float, y: float, width: int, height: int) -> Tuple[ el = ny * self.vfov return az, el + # New camera-based helpers + def _pixel_to_camera_ray(self, u: float, v: float, width: int, height: int) -> np.ndarray: + """Return unit ray in camera coords for pixel (u,v). Uses intrinsics""" + pts = np.array([[[u, v]]], dtype=np.float64) + und = cv2.undistortPoints(pts, self.thermal_sensor.camera_matrix, self.thermal_sensor.distortion_coeff, P=None) + x_norm = float(und[0, 0, 0]) + y_norm = float(und[0, 0, 1]) + ray = np.array([x_norm, y_norm, 1.0], dtype=np.float64) + ray /= np.linalg.norm(ray) + return ray + + def _camera_to_world_ray(self, ray_cam: np.ndarray, roll_deg: float, pitch_deg: float, yaw_deg: float) -> np.ndarray: + """Rotate ray from camera frame to world frame using roll/pitch/yaw (degrees).""" + r = math.radians(roll_deg) + p = math.radians(pitch_deg) + y = math.radians(yaw_deg) + Rx = np.array([[1, 0, 0], [0, math.cos(r), -math.sin(r)], [0, math.sin(r), math.cos(r)]], dtype=np.float64) + Ry = np.array([[math.cos(p), 0, math.sin(p)], [0, 1, 0], [-math.sin(p), 0, math.cos(p)]], dtype=np.float64) + Rz = np.array([[math.cos(y), -math.sin(y), 0], [math.sin(y), math.cos(y), 0], [0, 0, 1]], dtype=np.float64) + R_world_cam = Rz @ Ry @ Rx + ray_world = R_world_cam @ ray_cam + ray_world /= np.linalg.norm(ray_world) + return ray_world + + def _intersect_ray_ground(self, camera_alt_m: float, ray_world: np.ndarray): + """Intersect a world ray with z=0 ground plane. Returns (distance_m, ground_vec) or None.""" + dz = ray_world[2] + if dz >= 0: + return None + t = - camera_alt_m / dz + if t <= 0: + return None + horiz_vec = ray_world * t + horiz_distance = math.hypot(horiz_vec[0], horiz_vec[1]) + return horiz_distance, horiz_vec + def _estimate_ground_distance(self, el_offset: float) -> Optional[float]: pitch = math.radians(self.pitch_deg) elevation_angle = pitch - el_offset @@ -202,31 +241,62 @@ def _compute_nearest_hotspot_gps(self, hotspots: List[Dict], width: int, height: nearest = None min_dist = float('inf') + self.get_logger().info(f"hotspots found: {len(hotspots)}") + for blob in hotspots[:10]: - az, el = self._pixel_to_angle(blob['centroid_x'], blob['centroid_y'], width, height) - bearing = (self.current_heading_deg + math.degrees(az)) % 360.0 - ground_dist = self._estimate_ground_distance(el) - if ground_dist is None: + u = blob['centroid_x'] + v = blob['centroid_y'] + + ray_cam = self._pixel_to_camera_ray(u, v, width, height) + + # Camera attitude: roll/yaw follow the vehicle; pitch adds the fixed camera tilt + cam_roll = self.current_roll_deg + cam_pitch = self.current_pitch_deg + float(self.pitch_deg) + cam_yaw = self.current_yaw_deg + + # Convert Optical Frame (OpenCV) to Body Frame (ENU) + ray_body = np.array([ + ray_cam[2], # Body X (Forward) <- Optical Z + -ray_cam[0], # Body Y (Left) <- Optical -X + -ray_cam[1] # Body Z (Up) <- Optical -Y + ]) + + ray_world = self._camera_to_world_ray(ray_body, cam_roll, cam_pitch, cam_yaw) + dz = ray_world[2] + self.get_logger().info( + f"Hotspot centroid=({u:.2f},{v:.2f}), ray_world={ray_world}, dz={dz:.4f}, cam_pitch={cam_pitch:.2f}, cam_yaw={cam_yaw:.2f}, cam_roll={cam_roll:.2f}" + ) + + ground_res = self._intersect_ray_ground(self.altitude_m, ray_world) + if ground_res is None: + self.get_logger().info("Ray does not intersect ground (dz >= 0 or t <= 0)") continue + ground_dist, ground_vec = ground_res + + # compute bearing from north clockwise + east = ground_vec[0] + north = ground_vec[1] + bearing_rad = math.atan2(east, north) + bearing_deg = (math.degrees(bearing_rad) + 360.0) % 360.0 + start = Point(self.current_gps[0], self.current_gps[1]) - dest = geopy_distance(meters=ground_dist).destination(start, bearing) - dist_m = ground_dist + dest = geopy_distance(meters=ground_dist).destination(start, bearing_deg) info = { - 'centroid_x': blob['centroid_x'], - 'centroid_y': blob['centroid_y'], + 'centroid_x': u, + 'centroid_y': v, 'size_pixels': blob['size_pixels'], 'max_temperature_c': blob['max_temp'], 'avg_temperature_c': blob['avg_temp'], 'latitude': dest.latitude, 'longitude': dest.longitude, - 'bearing': bearing, - 'distance_m': dist_m, + 'bearing': bearing_deg, + 'distance_m': ground_dist, } - if dist_m < min_dist: - min_dist = dist_m + if ground_dist < min_dist: + min_dist = ground_dist nearest = info return nearest @@ -244,7 +314,7 @@ def _publish_hotspot(self, data: Dict) -> None: self.hotspot_pub.publish(msg) self.get_logger().info( f"Hotspot -> lat={data['latitude']:.6f}, lon={data['longitude']:.6f}, " - f"bearing={data['bearing']:.1f}°, dist={data['distance_m']:.1f}m, " + f"bearing={data['bearing']:.1f}°, dist={data['distance_m']:.5f}m, " f"max={data['max_temperature_c']:.1f}C, size={data['size_pixels']}px" ) diff --git a/ros2_ws/src/embr/embr/sensors/cube.py b/ros2_ws/src/embr/embr/sensors/cube.py index 98e914d..88873b4 100644 --- a/ros2_ws/src/embr/embr/sensors/cube.py +++ b/ros2_ws/src/embr/embr/sensors/cube.py @@ -90,7 +90,7 @@ def __init__(self, config: Optional[SensorConfig] = None): self.start_alt = params.get('start_alt', 100.0) # meters self.velocity = params.get('velocity', 5.0) # m/s self.pattern = params.get('pattern', 'circle') # circle, line, hover - self.pause_interval = params.get('pause_interval', 20.0) # seconds between pauses + self.pause_interval = params.get('pause_interval', 10.0) # seconds between pauses self.pause_duration = params.get('pause_duration', 5.0) # seconds to pause self._start_time = 0.0 self._last_pause_time = 0.0 diff --git a/ros2_ws/src/embr/embr/sensors/thermal.py b/ros2_ws/src/embr/embr/sensors/thermal.py index fd0a1f6..4de47cc 100644 --- a/ros2_ws/src/embr/embr/sensors/thermal.py +++ b/ros2_ws/src/embr/embr/sensors/thermal.py @@ -4,14 +4,11 @@ import numpy as np from typing import Optional, Dict, Any from .base import Sensor, SensorConfig +import cv2 class ThermalCameraSensor(Sensor): """Abstract thermal camera sensor interface.""" - - def get_frame_shape(self) -> tuple: - """Get the shape of thermal frames (height, width).""" - raise NotImplementedError class RealThermalCameraSensor(ThermalCameraSensor): @@ -21,6 +18,21 @@ def __init__(self, config: Optional[SensorConfig] = None): super().__init__(config) self.camera = None self.lepton_model = config.params.get('model', '3.1R') if config else '3.1R' + camera_parameters = { 'camera matrix': [[104.65403680863373, 0.0, 79.12313258957062], + [0.0, 104.48251047202757, 55.689070170705634], + [0.0, 0.0, 1.0]], + 'distortion coeff': [[-0.39758308581607127, + 0.18068641745671193, + 0.004626461618389028, + 0.004197358204037882, + -0.03381399499591463]], + 'new camera matrix':[[66.54581451416016, 0.0, 81.92717558174809], + [0.0, 64.58526611328125, 56.23740168870427], + [0.0, 0.0, 1.0]]} + self.camera_matrix = np.array(camera_parameters.get('camera matrix')) + self.distortion_coeff = np.array(camera_parameters.get('distortion coeff')) + self.new_camera_matrix = np.array(camera_parameters.get('new camera matrix')) + def start(self) -> None: """Initialize Lepton camera.""" @@ -34,6 +46,34 @@ def start(self) -> None: except Exception as e: raise RuntimeError(f"Failed to initialize Lepton camera: {e}") + def get_undistorted_img(self, img): + ''' + Undistort the image + + Args: + img = numpy array, + distorted image in uint16 + + Output: + undistorted_img = numpy array, + corrected image, cropped + ''' + + # Keep all pixels from input after dewarp + undistorted_img = cv2.undistort(img, self.camera_matrix, + self.distortion_coeff, + None, + self.new_camera_matrix) + # Get image dimension + img_dim = undistorted_img.shape + row = img_dim[0] + col = img_dim[1] + + # OpenCV generated cropping matrix still retains a few black pixels, + # return the corrected image with those pixels cropped out + undistorted_img = undistorted_img[14:row-18, 12:col-12] + return undistorted_img + def read(self) -> np.ndarray: """ Read radiometric frame from camera. @@ -52,16 +92,6 @@ def read(self) -> np.ndarray: except Exception as e: raise RuntimeError(f"Failed to read thermal frame: {e}") - def get_frame_shape(self) -> tuple: - """Get the shape of thermal frames based on Lepton model.""" - if self.lepton_model == '2.5': - return (60, 80) - elif self.lepton_model == '3.1R': - return (120, 160) - else: - # Default to 3.1R - return (120, 160) - def stop(self) -> None: """Close camera connection.""" if self.camera: @@ -148,10 +178,6 @@ def read(self) -> np.ndarray: return frame_uint16 - def get_frame_shape(self) -> tuple: - """Get the shape of simulated thermal frames.""" - return (self.height, self.width) - def stop(self) -> None: """Stop simulated camera.""" self._running = False diff --git a/ros2_ws/src/embr/embr/thermalStream.py b/ros2_ws/src/embr/embr/thermalStream.py index eab04f9..1e01fc6 100644 --- a/ros2_ws/src/embr/embr/thermalStream.py +++ b/ros2_ws/src/embr/embr/thermalStream.py @@ -263,9 +263,12 @@ def _streaming_loop(self): if radiometric_frame is None: self.get_logger().warn('Failed to grab frame from camera') continue + + + undistorted = self.camera.get_undistorted_img(radiometric_frame) # Convert to Celsius (Lepton outputs in Kelvin * 100) - temp_celsius = (radiometric_frame / 100.0) - 273.15 + temp_celsius = (undistorted / 100.0) - 273.15 # Create display frame with colormap and overlays display_frame = self._create_display_frame(temp_celsius) @@ -367,7 +370,7 @@ def _create_display_frame(self, temp_celsius): for contour in contours: # Skip tiny contours to avoid noise - if cv2.contourArea(contour) < 4: + if cv2.contourArea(contour) < 3: continue # Calculate temperature statistics for this region @@ -380,10 +383,6 @@ def _create_display_frame(self, temp_celsius): max_temp = float(np.max(region_temps)) - # Only annotate if above threshold (should be by mask, but double-check) - if max_temp <= self.temp_threshold: - continue - # Scale contour to display size and draw a bounding box scaled_contour = contour.astype(np.float32) scaled_contour[:, 0, 0] *= scale_x @@ -396,8 +395,6 @@ def _create_display_frame(self, temp_celsius): # Prepare temperature label and draw above the box temp_text = f"{max_temp:.1f}C" - text_size, _ = cv2.getTextSize(temp_text, cv2.FONT_HERSHEY_SIMPLEX, 0.6, 2) - text_w, text_h = text_size text_x = x text_y = max(10, y - 6) @@ -405,31 +402,10 @@ def _create_display_frame(self, temp_celsius): cv2.putText(display_frame, temp_text, (text_x, text_y), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 0), 3) cv2.putText(display_frame, temp_text, (text_x, text_y), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1) - return display_frame - - def _add_info_overlay(self, frame, temp_celsius): - """Add informational text overlay to the frame""" - # Overall temperature statistics - min_temp = np.min(temp_celsius) - max_temp = np.max(temp_celsius) - mean_temp = np.mean(temp_celsius) - # Minimal overlay: show Min / Max / Avg to reduce per-frame work - info_text = f"Min: {min_temp:.1f}C Max: {max_temp:.1f}C Avg: {mean_temp:.1f}C" - - # Draw a solid background for readability (fast) - cv2.rectangle(frame, (0, 0), (self.display_width, 30), (0, 0, 0), -1) - cv2.putText( - frame, - info_text, - (8, 20), - cv2.FONT_HERSHEY_SIMPLEX, - 0.7, - (255, 255, 255), - 2 - ) - + return display_frame + def _capture_and_publish(self): - """Capture once and publish both radiometric image and temperature array""" + """Capture once and publish temperature array""" try: # Capture fresh radiometric frame using sensor abstraction radiometric_frame = self.camera.read() @@ -453,7 +429,7 @@ def _capture_and_publish(self): array_msg.data = rad_u16.flatten().tolist() self.array_publisher.publish(array_msg) - self.get_logger().info('Published radiometric array only (uint16, Kx100) (velocity = 0)') + self.get_logger().info('Published radiometric array (uint16, Kx100) (velocity = 0)') except Exception as e: self.get_logger().error(f'Failed to capture/publish radiometric data: {e}') diff --git a/ros2_ws/src/embr/test/test_sensors.py b/ros2_ws/src/embr/test/test_sensors.py index eb348ef..6cdf892 100644 --- a/ros2_ws/src/embr/test/test_sensors.py +++ b/ros2_ws/src/embr/test/test_sensors.py @@ -175,26 +175,7 @@ def test_simulated_sensor_context_manager(self): assert frame.shape == (60, 80) assert not sensor.is_running - - def test_simulated_sensor_frame_shape(self): - """Test get_frame_shape method.""" - # Test both model resolutions - config_big = SensorConfig(mode='sim', params={'model': '3.1R'}) - sensor_big = create_sensor('thermal', config_big) - assert sensor_big.get_frame_shape() == (120, 160) - with sensor_big: - assert sensor_big.get_frame_shape() == (120, 160) - frame_big = sensor_big.read() - assert frame_big.shape == sensor_big.get_frame_shape() - - config_small = SensorConfig(mode='sim', params={'model': '2.5'}) - sensor_small = create_sensor('thermal', config_small) - assert sensor_small.get_frame_shape() == (60, 80) - with sensor_small: - assert sensor_small.get_frame_shape() == (60, 80) - frame_small = sensor_small.read() - assert frame_small.shape == sensor_small.get_frame_shape() - + def test_simulated_sensor_multiple_frames(self): """Test reading multiple frames shows temporal variation.""" config = SensorConfig(mode='sim', params={'num_hotspots': 2}) From 877a27a5bb880065d91736f04ba82b6494d63660 Mon Sep 17 00:00:00 2001 From: MacKenzie Richards <81762014+mrichards03@users.noreply.github.com> Date: Thu, 19 Feb 2026 20:25:58 -0800 Subject: [PATCH 14/15] small cleanup --- ros2_ws/src/embr/embr/getTemp.py | 2 +- ros2_ws/src/embr/launch/embr_launch.py | 14 ++++++++++---- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/ros2_ws/src/embr/embr/getTemp.py b/ros2_ws/src/embr/embr/getTemp.py index c7ca4d2..5be436f 100644 --- a/ros2_ws/src/embr/embr/getTemp.py +++ b/ros2_ws/src/embr/embr/getTemp.py @@ -41,7 +41,7 @@ def __init__(self): self.sensor.start() sensor_type = 'simulated' if isinstance(self.sensor.__class__.__name__, str) and 'Sim' in self.sensor.__class__.__name__ else 'real' - self.get_logger().info(f'Temperature sensor initialized in {config.mode} mode (using {sensor_type} sensor)') + self.get_logger().info(f'Temperature sensor initialized in {config.mode} mode') except Exception as e: self.get_logger().error(f'Failed to initialize sensor: {e}') raise diff --git a/ros2_ws/src/embr/launch/embr_launch.py b/ros2_ws/src/embr/launch/embr_launch.py index d88e219..eaf4f2e 100644 --- a/ros2_ws/src/embr/launch/embr_launch.py +++ b/ros2_ws/src/embr/launch/embr_launch.py @@ -14,8 +14,8 @@ def generate_launch_description(): # Declare launch argument for config file config_file_arg = DeclareLaunchArgument( 'config_file', - default_value='', - description='Path to sensor configuration file (default: config/sensors.json)' + default_value='src/embr/config/sensors.json', + description='Path to sensor configuration file (default: src/embr/config/sensors.json)' ) # Get launch configuration @@ -53,11 +53,17 @@ def generate_launch_description(): executable='thermalStream', name='thermal_stream', output='screen', - emulate_tty=True + emulate_tty=True, + parameters=[{ + 'config_file': config_file, + }] ), Node( package='embr', executable='hotspotLocator', - name='hotspot_locator' + name='hotspot_locator', + parameters=[{ + 'config_file': config_file, + }] ) ]) From ebd540319728d131022d26072298b605e59a95d6 Mon Sep 17 00:00:00 2001 From: maisonJar-dev Date: Wed, 13 May 2026 09:51:22 -0700 Subject: [PATCH 15/15] Fixed Base Py Optionality & minor path fixes in thermalStream.py --- ros2_ws/src/embr/embr/sensors/base.py | 6 +++--- ros2_ws/src/embr/embr/thermalStream.py | 5 ++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/ros2_ws/src/embr/embr/sensors/base.py b/ros2_ws/src/embr/embr/sensors/base.py index 15e3fd1..82c5af3 100644 --- a/ros2_ws/src/embr/embr/sensors/base.py +++ b/ros2_ws/src/embr/embr/sensors/base.py @@ -9,9 +9,9 @@ class SensorConfig: """Configuration for a sensor instance.""" mode: str # real, sim - device: Optional[str] - baud: int - params: Dict[str, Any] + device: Optional[str] = None + baud: Optional[int] = None + params: Dict[str, Any] = field(default_factory=dict) class Sensor(ABC): """Abstract base class for all sensors.""" diff --git a/ros2_ws/src/embr/embr/thermalStream.py b/ros2_ws/src/embr/embr/thermalStream.py index 1e01fc6..badc352 100644 --- a/ros2_ws/src/embr/embr/thermalStream.py +++ b/ros2_ws/src/embr/embr/thermalStream.py @@ -31,8 +31,7 @@ def __init__(self): # Try to load from config file try: - configs = SensorFactory.load_config(config_file) - config = configs.get('thermal') + config = SensorFactory.load_config('thermal', config_file) if config is None: self.get_logger().warning(f'Thermal sensor not found in config file: {config_file}. Using default real mode.') @@ -78,7 +77,7 @@ def __init__(self): # Initialize camera using sensor abstraction self.get_logger().info('Initializing thermal camera...') try: - self.camera = create_sensor('thermal', config) + self.camera = create_sensor('thermal', config_file) self.camera.start() sensor_type = 'simulated' if 'Sim' in self.camera.__class__.__name__ else 'real'