Thermal Camera code and documentation

.gitignore

@@ -16,3 +16,4 @@ ros2_ws/log
 ros2_ws/src/embr/embr/__pycache__
 ros2_ws/src/embr/embr/sensors/__pycache__
 .vscode
+__pycache__/

Dockerfile

@@ -85,9 +85,9 @@ echo "  # Simulation mode (default):"\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 --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 radio --ros-args -p config_file:=config/sensors_sim.json"\n\
+echo "  ros2 run embr getTemp --ros-args -p config_file:=src/embr/config/sensors_sim.json"\n\
+echo "  ros2 run embr getCube --ros-args -p config_file:=src/embr/config/sensors_sim.json"\n\
+echo "  ros2 run embr radio --ros-args -p config_file:=src/embr/config/sensors_sim.json"\n\
 echo ""\n\
 echo "  # Monitoring:"\n\
 echo "  ros2 topic list                       - List topics"\n\

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
+**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

Documentation/2025/Thermal Camera.md

@@ -0,0 +1,175 @@
+# **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.
+
+## **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:
+
+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://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.

README.md

@@ -252,6 +252,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
@@ -333,8 +360,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
@@ -345,7 +372,7 @@ The EMBR-Bot system consists of three main ROS2 nodes:
 ### 3. radio 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

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 "========================================="

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.)"